基于卡尔曼滤波的机器人位姿预估方法技术

技术编号：40503198 阅读：4 留言：0更新日期：2024-03-01 13:16

本申请公开基于卡尔曼滤波的机器人位姿预估方法，包括：步骤A1、基于预先设置的相关性扫描匹配算法的运行结果，机器人获取当前位姿观测数据及其噪声协方差矩阵；再对当前位姿观测数据进行数据关联处理，得到历史位姿观测数据；步骤A2、基于运动学方程计算出当前位姿预测数据；并基于状态方程计算当前位姿预测数据的先验协方差矩阵；步骤A3、基于延迟乱序观测方程使用当前位姿观测数据的噪声协方差矩阵以及当前位姿预测数据的先验协方差矩阵计算卡尔曼增益矩阵；步骤A4、根据当前位姿观测数据与历史位姿观测数据之间的残差对当前位姿预测数据进行校正，再将校正后的当前位姿预测数据设置为当前预估出的位姿预测数据；提高机器人位姿预测的准确性。

全部详细技术资料下载

【技术实现步骤摘要】

本申请涉及机器人控制算法领域，涉及基于卡尔曼滤波的机器人位姿预估方法。

技术介绍

1、面向低成本轮式机器人同步定位与地图构建(simultaneous localization andmapping，slam)领域中，相关性扫描匹配(correlative scan matching，csm)算法是一种基于激光雷达的扫描匹配算法，具体是结合搜索窗口进行扫描匹配，获取栅格地图中被激光点云占用概率最大的位姿，作为最终位姿，然而，该算法精度严重依赖初始搜索位姿的置信度。

2、现有技术还可以选择以初始搜索位姿作为先验信息,通过扩展卡尔曼滤波(extended kalman filter，ekf)框架融合惯性测量单元(inertial measurement unit，imu)的感测数据与旋转编码器的感测数据来处理出机器人的最终优化位姿。

3、机器人基于相关性扫描匹配(correlative scan matching，csm)算法对激光数据处理不及时以及输出的位姿数据存在信息延迟等问题，例如，通过相关性扫描匹配算法执行窗口搜索和匹配操作会存在延时，导致接收到的量测数据经常存在延时、乱序等现象，使得来自同一目标的较早时刻的量测晚于当前时刻的量测到达处理中心的状况，称之为滞后无序量测问题，当延迟的量测数据到来时，直接利用延迟的量测数据会引起机器人的惯导传感器的感测误差快速增长。

技术实现思路

1、本申请公开基于卡尔曼滤波的机器人位姿预估方法，具体的技术方案如下：

2、机器人位姿预估方法包括：步骤a1、基于预先设置的相关性扫描匹配算法的运行结果，机器人获取当前位姿观测数据及其噪声协方差矩阵；再对当前位姿观测数据进行数据关联处理，得到历史位姿观测数据；其中，利用所述相关性扫描匹配算法获得历史位姿观测数据所需采集的原始数据与利用所述相关性扫描匹配算法获得当前位姿观测数据所需采集的原始数据在时间上相匹配；步骤a2、基于运动学方程，由当前位姿观测数据计算出当前位姿预测数据；并基于状态方程计算当前位姿预测数据的先验协方差矩阵；步骤a3、基于延迟乱序观测方程，使用当前位姿观测数据的噪声协方差矩阵以及当前位姿预测数据的先验协方差矩阵计算卡尔曼增益矩阵；步骤a4、根据当前位姿观测数据与历史位姿观测数据之间的残差，对当前位姿预测数据进行校正，再将校正后的当前位姿预测数据设置为当前预估出的位姿预测数据；并结合卡尔曼增益矩阵、当前位姿预测数据的先验协方差矩阵以及当前位姿观测数据的噪声协方差矩阵，对当前位姿预测数据的先验协方差矩阵进行校正。

3、综上可知，本申请将预先设置的相关性扫描匹配算法延迟输出的计算结果作为当前位姿观测数据(延迟的量测数据)，再基于前述步骤a1至步骤a4对机器人位姿数据进行校正，将校正后的位姿预测数据设置为当前预估出的位姿预测数据；从而在当前时刻充分利用延迟的量测数据(前述当前位姿观测数据)的位姿数据及协方差矩阵，约束机器人的惯导测量单元的传感器误差增长，克服相关性扫描匹配算法延迟输出位姿数据的影响，提高机器人位姿预测的准确性。

4、进一步地，通过重复执行步骤a3和步骤a4来更新所述当前位姿预测数据及其先验协方差矩阵，直至重复执行次数达到预设目标迭代次数，将当前校正后的当前位姿预测数据设置为目标位姿预测数据；或者，通过重复执行步骤a2至步骤a4来更新所述当前位姿预测数据及其先验协方差矩阵，直至重复执行次数达到预设目标迭代次数，将当前校正后的当前位姿预测数据设置为目标位姿预测数据。以满足预设的位姿校正精度要求。

5、进一步地，在所述步骤a1中，机器人获取当前位姿观测数据及其噪声协方差矩阵时，配置当前时刻存在当前位姿观测数据，并确定当前位姿观测数据的时间戳小于当前时刻，其中，当前位姿观测数据的时间戳是预先设置的相关性扫描匹配算法计算出当前位姿观测数据的时刻；当前位姿观测数据的噪声协方差矩阵的初始状态是由预先设置的相关性扫描匹配算法输出的位姿数据当中携带的测量误差确定；预先设置的相关性扫描匹配算法输出的位姿数据当中携带的测量误差包括机器人安装的惯性测量单元的测量误差，考虑所述相关性扫描匹配算法的计算延时，能够形成不同时刻状态的后验相关性，并保证基于卡尔曼滤波的有效性。

6、进一步地，在所述步骤a3中，计算所述卡尔曼增益矩阵的方法包括：其中，是当前位姿预测数据的先验协方差矩阵；vi是当前位姿观测数据的噪声协方差矩阵；h是所述延迟乱序观测方程相对于当前位姿预测数据的雅可比矩阵，以表示卡尔曼滤波算法中的观测矩阵；t是矩阵的转置；ki是卡尔曼增益矩阵；i是当前时刻。

7、进一步地，在所述步骤a4中对当前位姿预测数据进行校正的方法包括：其中，是所述当前位姿观测数据，且以向量表示；是所述历史位姿观测数据；j是所述历史位姿观测数据的时间戳，j小于i；是所述当前位姿预测数据；是当前校正后的当前位姿预测数据；是当前位姿观测数据与历史位姿观测数据之间的残差；在所述步骤a4中对当前位姿预测数据的先验协方差矩阵进行校正的方法包括：其中，是先验协方差矩阵经过校正后得到的矩阵，以形成当前时刻目标状态的协方差矩阵；i是单位矩阵。在机器人处于静止状态下并确定重复执行步骤a3至步骤a4时，可以在下一时刻中直接更新为是当前位姿观测数据与历史位姿观测数据之间的残差。

8、进一步地，在所述步骤a2中，所述基于运动学方程，由当前位姿观测数据计算出当前位姿预测数据的方法包括：计算当前姿态角：其中，ωi是机器人的陀螺仪测得的当前时刻角速度，ωb是预先设置的偏置角速度；q{·}用于将欧拉角转换为四元数，表示四元数乘法；是观测时刻i-d的姿态角，是当前姿态角，和均设置为四元数的形式；计算当前速度：其中，ai是机器人的加速度计测得的当前时刻加速度，ab是预先设置的偏置加速度；是观测时刻i-d的后验旋转矩阵；是观测时刻i-d的速度，是当前速度；计算当前预测先验位移：其中，是观测时刻i-d的预测先验位移，是当前预测先验位移；然后，将当前姿态角当前速度和当前预测先验位移组成所述当前位姿预测数据其中，观测时刻i-d的姿态角观测时刻i-d的速度和观测时刻i-d的时间戳预测先验位移组成所述当前位姿观测数据；其中，观测时刻i-d是所述当前位姿观测数据的时间戳，d是当前时刻i与观测时刻i-d之间的时间间隔。

9、因此，所述步骤a2基于机器人运动学模型，都是利用机器人在某一时刻的位移、速度以及加速度建立状态空间，并从牛顿运动力学定律出发描述机器人的位姿状态随时间的演变规律，以便于在卡尔曼滤波算法的框架中通过数值积分方法预测先验位移、速度和姿态角。

10、进一步地，在所述步骤a2中，所述基于状态方程计算当前位姿预测数据的先验协方差矩阵的方法包括：其中，是当前位姿预测数据的先验协方差矩阵；是观测时刻i-d的位姿预测数据的先验协方差矩阵；fε是状态方程相对于观测时刻i-d的位姿预测数据的雅可比矩阵；fi是状态方程相对于惯性测量单元测量噪声的雅可比矩阵；qi是当前位姿预测数据的本文档来自技高网...

【技术保护点】

1.基于卡尔曼滤波的机器人位姿预估方法，其特征在于，机器人位姿预估方法包括：

2.根据权利要求1所述机器人位姿预估方法，其特征在于，通过重复执行步骤A3和步骤A4来更新所述当前位姿预测数据及其先验协方差矩阵，直至重复执行次数达到预设目标迭代次数，将当前校正后的当前位姿预测数据设置为目标位姿预测数据；

3.根据权利要求1所述机器人位姿预估方法，其特征在于，在所述步骤A1中，机器人获取当前位姿观测数据及其噪声协方差矩阵时，配置当前时刻存在当前位姿观测数据，并确定当前位姿观测数据的时间戳小于当前时刻，其中，当前位姿观测数据的时间戳是预先设置的相关性扫描匹配算法计算出当前位姿观测数据的时刻；

4.根据权利要求1所述机器人位姿预估方法，其特征在于，在所述步骤A3中，计算所述卡尔曼增益矩阵的方法包括：

5.根据权利要求4所述机器人位姿预估方法，其特征在于，在所述步骤A4中对当前位姿预测数据进行校正的方法包括：其中，是所述当前位姿观测数据，且以向量表示；是所述历史位姿观测数据；j是所述历史位姿观测数据的时间戳，j小于i；是所述当前位姿预测数据；是

6.根据权利要求1所述机器人位姿预估方法，其特征在于，在所述步骤A2中，所述基于运动学方程，由当前位姿观测数据计算出当前位姿预测数据的方法包括：

7.根据权利要求6所述机器人位姿预估方法，其特征在于，在所述步骤A2中，所述基于状态方程计算当前位姿预测数据的先验协方差矩阵的方法包括：

8.根据权利要求6所述机器人位姿预估方法，其特征在于，所述机器人位姿预估方法还包括：

9.根据权利要求8所述机器人位姿预估方法，其特征在于，当确定机器人处于静止状态时，使用N组加速度测量矢量的平均值更新为所述偏置加速度，并使用N组角速度测量矢量的平均值更新为所述偏置角速度，以使机器人在更新所述偏置加速度和所述偏置角速度后通过执行步骤A2至步骤A4来更新所述当前位姿预测数据及其先验协方差矩阵。

10.根据权利要求8所述机器人位姿预估方法，其特征在于，当确定机器人不处于静止状态时，基于卡尔曼滤波算法，使用所述左轮转动测量值和所述右轮转动测量值分别计算出左轮预测状态数据和右轮预测状态数据，再分别校正左轮预测状态数据和右轮预测状态数据。

11.根据权利要求10所述机器人位姿预估方法，其特征在于，在确定机器人不处于静止状态后，在卡尔曼滤波算法所需的计算框架下，基于状态方程计算出左轮预测状态数据及其先验协方差矩阵，并基于状态方程计算出右轮预测状态数据及其先验协方差矩阵；然后通过引入机器人运动速度约束条件，对左轮预测状态数据和右轮预测状态数据分别进行卡尔曼滤波以形成非完整约束校正，得到校正后的左轮预测状态数据和校正后的右轮预测状态数据；然后将校正后的左轮预测状态数据更新为所述左轮转动测量值，并将校正后的右轮预测状态数据更新为所述右轮转动测量值；

12.根据权利要求8所述机器人位姿预估方法，其特征在于，所述对一个所述观测周期内所采集的N组测量值计算加权均方误差的方法包括：

...

【技术特征摘要】

1.基于卡尔曼滤波的机器人位姿预估方法，其特征在于，机器人位姿预估方法包括：

2.根据权利要求1所述机器人位姿预估方法，其特征在于，通过重复执行步骤a3和步骤a4来更新所述当前位姿预测数据及其先验协方差矩阵，直至重复执行次数达到预设目标迭代次数，将当前校正后的当前位姿预测数据设置为目标位姿预测数据；

3.根据权利要求1所述机器人位姿预估方法，其特征在于，在所述步骤a1中，机器人获取当前位姿观测数据及其噪声协方差矩阵时，配置当前时刻存在当前位姿观测数据，并确定当前位姿观测数据的时间戳小于当前时刻，其中，当前位姿观测数据的时间戳是预先设置的相关性扫描匹配算法计算出当前位姿观测数据的时刻；

4.根据权利要求1所述机器人位姿预估方法，其特征在于，在所述步骤a3中，计算所述卡尔曼增益矩阵的方法包括：

5.根据权利要求4所述机器人位姿预估方法，其特征在于，在所述步骤a4中对当前位姿预测数据进行校正的方法包括：其中，是所述当前位姿观测数据，且以向量表示；是所述历史位姿观测数据；j是所述历史位姿观测数据的时间戳，j小于i；是所述当前位姿预测数据；是当前校正后的当前位姿预测数据；是当前位姿观测数据与历史位姿观测数据之间的残差；

6.根据权利要求1所述机器人位姿预估方法，其特征在于，在所述步骤a2中，所述基于运动学方程，由当前位姿观测数据计算出当前位姿预测数据的方法包括：

7.根据权利要求6所述机器人位姿预估方法，其特征在于，在所述步骤a2中，所述基于状态方程计算当前位姿预测数据的先验协...

【专利技术属性】
技术研发人员：王珂，周和文，包敏杰，戴崑，许润泽，肖刚军，
申请(专利权)人：珠海一微半导体股份有限公司，
类型：发明
国别省市：

全部详细技术资料下载我是这个专利的主人