[发明专利]用惯导、压力传感器修正下坡悬空后轮里程计误差的方法

权利要求书

1.用惯导、压力传感器修正下坡悬空后轮里程计误差的方法，其特征在于：采用陀螺仪和加速度传感器实时检测机器人的姿态，以判断机器人是否正在下坡，当由机器人的姿态判断出机器人处于下坡的过程中时，再利用压力传感器检测机器人的后轮是否悬空；

当由姿态判断出机器人处于平面行驶时，或者虽然处于下坡状态，但后轮均未悬空时，采用下式进行行走距离增量(△x,△y,△θ)的计算：

式中：(△x,△y,△θ)＝前一次采样间隔走过的路径；△x为机器人x方向位置增量，△y为机器人y方向位置增量，△s为机器人的位置增量，△s_r,△s_l分别为右轮、左轮行走的距离；b为差动驱动机器人两个轮子之间的距离；θ为机器人的朝向；

当检测出一个后轮已悬空时，忽略两个影响：

(1)后轮转速的强闭环控制使速度不受重力加速度的影响；

(2)忽略前轮的方向对于单后轮驱动直线行驶的影响；这样机器人只受一个后轮的驱动按直线行驶；

左轮悬空时，机器人行走距离增量(△x,△y,△θ)按照下面的修正公式进行计算：

右轮同理。

2.根据权利要求1所述的用惯导、压力传感器修正下坡悬空后轮里程计误差的方法，其特征在于：陀螺仪和加速度传感器均设于机器人底盘的中央位置，陀螺仪用于检测机器人的角速度变化；加速度传感器用于检测机器人的速度变化；压力传感器有两个，两个压力传感器分别用于检测两个后轮对于机器人支架的压力；机器人上还设有两个光电编码器、激光传感器和工控机；两个光电编码器分别安装在两个后轮上作为里程计使用；激光传感器用于检测周边环境的信息进行定位导航；工控机作为车载控制器，用于接收陀螺仪、加速度传感器、压力传感器、光电编码器和激光传感器的数据，进行定位导航的运算。

3.根据权利要求1所述的用惯导、压力传感器修正下坡悬空后轮里程计误差的方法，其特征在于：利用陀螺仪和加速度传感器数据计算机器人姿态角，判断机器人是否在下坡的步骤如下：

步骤(1)：系统建模，系统的状态方程为：

X(k+1)＝AX(k)+Bu(k)+v(k)； (1)

式中，k为时刻，X(k+1)为机器人在(k+1)时刻状态，A为状态转移矩阵，X(k)为机器人在k时刻状态，B为系统控制矩阵，u(k)为k时刻系统输入量，v(k)为系统过程噪声，

输出方程为：

Y(k)＝CX(k)+w(k)； (2)

式中，Y(k)为k时刻系统输出量，C为观测矩阵，w(k)为观测噪声，

步骤(2)：计算陀螺仪测量到的姿态角预估计值与加速度传感器之间的偏差：

error(k)＝angle_g(k)-angle_a(k)； (3)

式中，error(k)为在k时刻的姿态角偏差，angle_g(k)为k时刻陀螺仪计算到的姿态角，angle_a(k)为k时刻加速度传感器测得的姿态角，

步骤(3)：计算Kalman增益K(k)：

K(k)＝P(k-1)·C^T/[C·P(k-1)·C^T+R(k)]； (4)

式中，P(k-1)为状态估计，R(k)为旋转矩阵，

步骤(4)：计算k时刻机器人姿态角的最终输出值angle_final(k)：

angle_final(k)＝angle_g(k)+K(k)·error(k)； (5)

步骤(5)：计算k+1时刻的滤波协方差P(k+1)：

P(k|k)＝(I-K(k)·C)P(k|k-1)； (6)

式中，P(k|k)为k时刻状态估计，I为单位矩阵，P(k|k-1)为(k-1)时刻状态估计，至此就完成了k时刻对机器人姿态所有的Kalman滤波计算，当k+1时刻的陀螺仪角速度数据输入到机器人中，回到步骤(1)进行计算，这样不停的迭代运行以输出机器人的姿态信息，

在得到机器人的姿态信息后，判断angle_final(k)和根据斜坡斜率得出的可判断机器人正在下坡的临界角度critical_angle，下坡的临界角度critical_angle根据机器人结构设计由人工人为设定，当angle_final(k)>critical_angle时，判断机器人正在下坡，不然判断机器人在正常的行驶。

4.根据权利要求3所述的用惯导、压力传感器修正下坡悬空后轮里程计误差的方法，其特征在于：检测机器人的后轮是否悬空的方法如下：在k时刻判断机器人正在下坡时，此时不停的读取i、i≥k时刻两个后轮压力传感器的数据p_right(i)和p_left(i)来判断是否有后轮处于悬空的状态，记k-1时刻机器人仍在正常行驶时的两个后轮压力传感器的数据是p_right(k-1)与p_left(k-1)，通过压力传感器变化的临界值critical_p来判断后轮是否悬空，即当p_right(k-1)-p_right(i)>critical_p时，判断机器人的右后轮已处于悬空的状态中，当p_left(k-1)-p_left(i)>critical_p时，判断机器人的左后轮已处于悬空的状态中。