[发明专利]一种多传感器融合的四足机器人运动里程计设计方法

说明书

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

技术领域

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

背景技术

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

发明内容

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

本发明的技术方案如下：

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

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

步骤2：初始化里程计XY位置数据，初始化里程计卡尔曼滤波器的系统状态X＝[p_xp_y v_x v_y]和协方差矩阵P＝I_4×4；

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

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

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

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

P_k＝AP_k-1A^T+Q

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

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

dX(i)＝X_leg(i)-X_{leg_st}(i)