[发明专利]基于多传感器信息融合的足式机器人本体状态估计方法

权利要求书

1.一种基于多传感器信息融合的足式机器人本体状态估计方法，其特征在于，步骤包括：

S1.一级信息融合：分别通过目标足式机器人内部的传感器采集各条腿的运动信息，根据采集到的信息对各腿进行单腿运动学解算，得到各腿的运动学信息；将各腿中支撑腿的所述运动学信息进行融合，得到本体状态初始估计结果，所述本体状态包括位移和速度，所述足式机器人包括本体和各条腿；

S2.惯导建模解算：通过安装在目标足式机器人上的IMU系统获取IMU信息，并基于获取的所述IMU信息进行惯导建模解算，得到惯导解算结果；

S3.二级信息融合：将得到的所述本体状态初始估计结果和所述惯导解算结果进行融合，得到最终本体状态估计结果；

所述步骤S1的具体步骤为：

S11.单腿运动学解算：通过关节位移传感器采集目标足式机器人各单腿的关节位移信息并进行单腿运动学解算，得到各腿的位置与速度信息；

S12.支撑期判断：通过目标足式机器人中每条腿足端的力传感器检测足端与地面之间的接触力，并判断各腿是否处于支撑期，确定得到各腿中的支撑腿；

S13.信息融合：获取各所述支撑腿对应的位置信息，将各所述支撑腿对应的位置信息进行融合后得到本体初始位置，并根据所述本体初始位置得到本体初始速度；

所述步骤S11中单腿运动学解算的具体步骤为：

S111.定义机器人本体固连坐标系以及参考惯性系，根据目标足式机器人单腿的所述关节位移信息进行解算，求解出足端在所述本体固连坐标系中位置；

S112.获取IMU系统输出的姿态角信息，根据获取的所述姿态角信息将所述步骤S111求解出的位置投影到参考惯性系三个坐标系中；

S113.根据所述步骤S112的投影反解出本体在参考惯性系中位置与速度；

所述步骤S2的具体步骤为：

S21.利用IMU系统输出的姿态角，计算IMU载体系与参考惯性系的姿态转换矩阵，所述IMU载体系与所述本体固连坐标系三个坐标系向方向一致；

S22.根据所述姿态转换矩阵通过坐标系变换将IMU系统输出的载体系加速度转换为参考惯性系加速度；

S23.对IMU系统漂移误差进行静态标定以补偿确定性偏差，得到初步的真实加速度；

S24.根据所述本体状态初始估计结果得到的本体初始速度与初始位置，求得本体状态的惯导解算值，得到所述惯导解算结果。

2.根据权利要求1所述的基于多传感器信息融合的足式机器人本体状态估计方法，其特征在于：所述步骤S13中具体通过将各所述支撑腿的位置信息进行加权以实现融合，由加权结果得到所述本体初始位置。

3.根据权利要求2所述的基于多传感器信息融合的足式机器人本体状态估计方法，其特征在于：所述本体初始位置取所有支撑腿的位置信息加权和的平均值。

4.根据权利要求1所述的基于多传感器信息融合的足式机器人本体状态估计方法，其特征在于，所述步骤S22中按照下式转换得到参考惯性系加速度；

其中，为姿态转换矩阵，f^b为IMU系统输出的载体系加速度，gⁿ为重力加速度，为转换后的参考惯性系加速度。