一种多传感器融合的四足机器人运动里程计设计方法与流程

本发明涉及机器人运动控制领域，具体涉及一种四足机器人状态估计方法。

背景技术：

四足机器人运动里程计是借助机器人本体传感器实现对自身精确位置推算的关键技术，里程计推算结果除了为机器人落足提供全局定位信息外，同时也为机器人的位置控制提供反馈数据，四足机器人本体搭载了高精度imu，其能测量机器人的机体姿态与加速度数据、各关键的角度传感器能测量机器人关节角度并计算出各腿运动学参数即足端位置，传统里程计估计方法仅通过对末端位置微分得到速度，通过对支撑腿运动速度的反向积分得到本体里程计估计结果，但微分数据存在噪声经过二次积分后误差将进一步放大，同时由于采样和微分处理使得最终产生的里程计数据与真实机器人状态存在滞后，因此需要提出一种新的里程计估计方法。

技术实现要素：

本发明公开了一种多传感器融合的四足机器人运动里程计设计方法，其目的在于解决背景技术中提到的四足机器人产生的里程计数据与真实机器人状态存在滞后的问题。

本发明的技术方案如下：

一种多传感器融合的四足机器人运动里程计设计方法，其特征在于，包括如下步骤：

步骤1：沿机器人机头方向建立地形里程计坐标系{o}；

步骤2：初始化里程计xy位置数据，初始化里程计卡尔曼滤波器的系统状态x＝[pxpyvxvy]和协方差矩阵p＝i4×4；

步骤3：初始化滤波器对应的传感器测量噪声参数，加速度测量噪声an，足端速度测量噪声vn，位置里程测量噪声pn，得到对应的系统噪声矩阵与测量噪声矩阵：

步骤4：构建匀加速运动系统状态方程，采用姿态解算得到的四元数[q1q2q3q4]将机体测量得到的加速度值转换到{o}系下：

步骤5：基于匀加速模型预测下一时刻里程计的状态：

步骤6：有上一步骤得到系统的状态矩阵则可进一步计算先验协方差矩阵：

pk＝apk-1a^t+q

步骤7：计算基于足端位置累加产生的里程计位置测量数据，初始化里程计局部坐标系原点xodom_st与各腿自己的运动坐标系原点xleg_st(i)

步骤8：计算每个支撑腿相对各子运动坐标系原点的位置偏差：

dx(i)＝xleg(i)-xleg_st(i)

步骤9：计算支撑腿位移量的平均值：

步骤10：基于平均支撑平移量与机器人当前里程计运动坐标系原点得到位置测量值：

步骤11：判断单腿摆动切换，采用该触发信号复位里程计运动坐标系与单腿运动坐标系：

xleg_st(i)＝xleg(i)

步骤12：计算末端速度的反向运动速度，通过前向微分得到各支撑腿对应的速度为：

vleg(i)＝xleg(i,k)-xleg(i,k-1)

步骤13：计算支撑腿反向速度的平均值：

步骤14：将反向速度作为速度测量值并与位置测量值构建测量向量：

步骤15：构建系统的观测矩阵：

步骤16：计算卡尔曼增益

k＝ph^t(hph^t+r)^-1

步骤17：采用观测向量修正之前的里程计估计结果偿

步骤18：更新后验协方差矩阵

p＝(i-kh)p。

本发明的有益效果：通过构建在机体坐标系下的里程计估计坐标系解耦航向波动引起的估计扰动，同时构建了卡尔曼滤波器在使用末端速度微分估计里程数据的同时引入支撑相位置变化与本体imu加速度测量结果，从而在提高里程计精度的同时保证最终里程计数据的实时性。

附图说明

图1为采用本方法改进后的机器人的俯仰角和横滚角随时间变化的波动图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。

一种多传感器融合的四足机器人运动里程计设计方法，包括如下步骤：

步骤1：沿机器人机头方向建立地形里程计坐标系{o}；

步骤2：初始化里程计xy位置数据，初始化里程计卡尔曼滤波器的系统状态x＝[pxpyvxvy]和协方差矩阵p＝i4×4；

步骤3：初始化滤波器对应的传感器测量噪声参数，加速度测量噪声an，足端速度测量噪声vn，位置里程测量噪声pn，得到对应的系统噪声矩阵与测量噪声矩阵：

步骤4：构建匀加速运动系统状态方程，采用姿态解算得到的四元数[q1q2q3q4]将机体测量得到的加速度值转换到{o}系下：

步骤5：基于匀加速模型预测下一时刻里程计的状态：

步骤6：有上一步骤得到系统的状态矩阵则可进一步计算先验协方差矩阵：

pk＝apk-1a^t+q

步骤7：计算基于足端位置累加产生的里程计位置测量数据，初始化里程计局部坐标系原点xodom_st与各腿自己的运动坐标系原点xleg_st(i)；

步骤8：计算每个支撑腿相对各子运动坐标系原点的位置偏差：

dx(i)＝xleg(i)-xleg_st(i)

步骤9：计算支撑腿位移量的平均值：

步骤10：基于平均支撑平移量与机器人当前里程计运动坐标系原点得到位置测量值：

步骤11：判断单腿摆动切换，采用该触发信号复位里程计运动坐标系与单腿运动坐标系：

xleg_st(i)＝xleg(i)

步骤12：计算末端速度的反向运动速度，通过前向微分得到各支撑腿对应的速度为：

vleg(i)＝xleg(i,k)-xleg(i,k-1)

步骤13：计算支撑腿反向速度的平均值：

步骤14：将反向速度作为速度测量值并与位置测量值构建测量向量：

步骤15：构建系统的观测矩阵：

步骤16：计算卡尔曼增益

k＝ph^t(hph^t+r)^-1

步骤17：采用观测向量修正之前的里程计估计结果偿

步骤18：更新后验协方差矩阵

p＝(i-kh)p。

本发明公开的一种多传感器融合的四足机器人运动里程计设计方法，四足机器人运动里程计是借助机器人本体传感器实现对自身精确位置推算的关键技术，里程计推算结果除了为机器人落足提供全局定位信息外，同时也为机器人的位置控制提供反馈数据，四足机器人本体搭载了高精度imu，其能测量机器人的机体姿态与加速度数据、各关键的角度传感器能测量机器人关节角度并计算出各腿运动学参数即足端位置。本发明提出了一种融合多传感器数据的运动里程计设计方法，通过构建在机体坐标系下的里程计估计坐标系解耦航向波动引起的估计扰动，同时构建了卡尔曼滤波器在使用末端速度微分估计里程数据的同时引入支撑相位置变化与本体imu加速度测量结果，从而在提高里程计精度的同时保证最终里程计数据的实时性，具体情况见图1。

尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。