基于卡尔曼滤波的机器人位姿预估方法与流程

文档序号:37161556发布日期:2024-03-01 11:56阅读:15来源:国知局
基于卡尔曼滤波的机器人位姿预估方法与流程

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


背景技术:

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是当前位姿预测数据的噪声协方差矩阵;qi是惯性测量单元测量噪声确定。从而利用当前位姿预测数据的噪声协方差矩阵、各个时刻的位姿预测数据以及状态方程相对于噪声协方差矩阵和位姿预测数据的雅可比矩阵,计算出当前位姿预测数据的先验协方差矩阵。

11、进一步地,所述机器人位姿预估方法还包括:从初始时刻开始,在一个采集周期内依次采集n组测量值,其中,一个采集周期的计时起点是时刻m,同一个采集周期的计时终点是时刻m+n-1;一组测量值包括加速度测量值、角速度测量值、左轮转动测量值和右轮转动测量值;对一个所述采集周期内所采集的n组测量值计算加权均方误差,再判断加权均方误差是否低于预先设定的统计阈值,是则确定机器人处于静止状态,否则确定机器人不处于静止状态。从而通过检测加权均方误差偏离阈值的程度来判断机器人是否处于静止状态或者启动运动。

12、进一步地,当确定机器人处于静止状态时,使用n组加速度测量矢量的平均值更新为所述偏置加速度,并使用n组角速度测量矢量的平均值更新为所述偏置角速度,以使机器人在更新所述偏置加速度和所述偏置角速度后通过执行步骤a2至步骤a4来更新所述当前位姿预测数据及其先验协方差矩阵。

13、进一步地,当确定机器人不处于静止状态时,基于卡尔曼滤波算法,使用所述左轮转动测量值和所述右轮转动测量值分别计算出左轮预测状态数据和右轮预测状态数据,再分别校正左轮预测状态数据和右轮预测状态数据。从而在卡尔曼滤波算法框架下根据机器人的实际运动状态实现约束值的自适应调整,滤波完成以后得到转动速率或转动距离的误差改正值,对机器人的速度误差进行修正,得出校正后三维方向的速度,即所述步骤a2计算的当前速度经过卡尔曼滤波算法的校正后在三种坐标轴方向上的速度分量;然后利用校正后的三维方向的速度对位置误差进行更新,得出机器人的修正速度值和修正速度方向,解决机器人车轮上的旋转编码器的感测数据处理出错的问题;则在复杂的动态环境中,机器人利用校正后的三维方向的速度预测向量对机器人位置误差进行修正,保证导航定位结果的准确性。

14、进一步地,在确定机器人不处于静止状态后,在卡尔曼滤波算法所需的计算框架下,基于状态方程计算出左轮预测状态数据及其先验协方差矩阵,并基于状态方程计算出右轮预测状态数据及其先验协方差矩阵;然后通过引入机器人运动速度约束条件,对左轮预测状态数据和右轮预测状态数据分别进行卡尔曼滤波以形成非完整约束校正,得到校正后的左轮预测状态数据和校正后的右轮预测状态数据;然后将校正后的左轮预测状态数据更新为所述左轮转动测量值,并将校正后的右轮预测状态数据更新为所述右轮转动测量值;其中,机器人运动速度约束条件包括机器人发生横向位移所需的速度约束条件;机器人发生横向位移所需的速度约束条件包括:机器人的当前速度在机器人的车轮转轴方向上的速度分量大小不等于数值0,且机器人的当前速度在垂直于机器人行走平面的速度分量大小等于数值0;其中,机器人的当前速度在机器人的车轮转轴方向上的速度分量是所述机器人发生横向位移所带来的横向速度。使得:在检测到机器人发生横向位移的状态下,利用校正后的左轮预测状态数据和校正后的右轮预测状态数据对机器人位置误差进行修正。

15、进一步地,所述对一个所述观测周期内所采集的n组测量值计算加权均方误差的方法包括:其中,γ是加权均方误差;ak是时刻k的加速度测量矢量,是时刻m至时刻m+n-1的时间间隔内的平均加速度,σa是加速度计测量噪声的标准差,σa是预先设置;时刻m至时刻m+n-1的时间间隔是当前观测周期;g是重力加速度;ωk是时刻k的角速度测量矢量,σω是陀螺仪测量噪声的标准差,σω是预先设置;是时刻k的左轮转动测量矢量,是时刻k的右轮转动测量值,σr是左旋转编码器或右旋转编码器测量噪声的标准差,σr是预先设置;机器人的左轮安装左旋转编码器,机器人的右轮安装右旋转编码器。从而在计算加权均方误差的过程中取各个测量值的测量误差分布标准差作为权重。

当前第1页1 2 
网友询问留言 已有0条留言
  • 还没有人留言评论。精彩留言会获得点赞!
1