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

权利要求书

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

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

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

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

步骤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)

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

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

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

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

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

V_leg(i)＝X_leg(i,k)-X_leg(i,k-1)

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

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

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

步骤16：计算卡尔曼增益

K＝PH^T(HPH^T+R)^-1

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

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

P＝(I-KH)P。