CN108621161B - 基于多传感器信息融合的足式机器人本体状态估计方法 - Google Patents

Abstract

本发明公开一种基于多传感器信息融合的足式机器人本体状态估计方法，步骤包括：S1.分别通过目标机器人内部的传感器采集各条腿的运动信息，根据采集到的信息对各腿进行单腿运动学解算，得到各腿的运动学信息；将各腿中支撑腿的运动学信息进行融合，得到本体初始状态估计结果；S2.通过IMU***获取IMU信息，并基于获取的IMU信息进行惯导建模解算，得到惯导解算结果；S3.将得到的本体初始状态估计结果和惯导解算结果进行融合，得到最终本体状态估计结果。本发明兼具IMU与运动学解算估计方式的优势，估计结果不会产生漂移，同时能滤除尖峰噪声，提高估计结果精度。

Description

基于多传感器信息融合的足式机器人本体状态估计方法

技术领域

本发明涉及足式机器人状态估计技术领域，尤其涉及一种基于多传感器信息融合的足式机器人本体状态估计方法。

背景技术

足式机器人是模仿人或动物等设计制造的一类机器人，腿部采用串联多关节结构。根据模仿对象与需求任务的不同，足式机器人又可以分成几大类：1、单足机器人，用于模仿哺乳动物单腿进行弹跳功能研究；2、双足步行机器人，用于模仿人类进行稳定行走、小跑、交互等功能研究；3、四足机器人，用于模仿狗、猫、马、猎豹等四足哺乳动物进行稳定性、柔顺性、负载能力、环境适应性研究；4、多足机器人，用于模仿蜘蛛、蜈蚣等爬行类昆虫用于危险狭小空间探索、救援等任务研发。

足式机器人技术研究的重要目标是使其能够像所模仿的动物一样拥有在现实世界行走运动的能力，其中感知能力是尤其重要的一项内容。一般来讲，机器人的感知可以分成两大类：一是对外部环境的感知，包括足端与地面的接触情况、是否有障碍物、障碍物的位置、外部的干扰等等，可以用来进行避障与越障等；二是对自身状态的感知，包括腿部的弯曲程度、关节的运动情况、本体的加速度、速度、位置、运动方向、姿态角等，用于控制中反馈可以提高控制器效果，提升机器人运动性能。

足式机器人本体速度与位置状态的准确估计，不仅能够更为精确描述机器人在现实世界中的运动状态，同时可以为相关的控制提供高质量的输入与反馈信号，提高控制效果与机器人性能。感知的信息来源一般是各种各样的传感器，主要包括惯性导航器件(IMU)、GPS定位***、雷达、摄像头、关节位移传感器、关节力传感器、足端力传感器等等，目前针对足式机器人的状态估计一般是基于IMU的惯导建模解算或基于内部传感器的运动学建模分析，其中惯导解算方法短时精准，但会随时间积累产生漂移，漂移主要来源于IMU的器件偏差、积分算法误差等；运动学分析不会产生漂移，但是存在大量噪声，尤其是机器人落地时的震荡、冲击等带来的尖峰噪声，无法通过普通的滤波算法消除。由于存在上述缺点，基于单一类传感器信息的惯导解算或运动学分析难以实现对机器人本体状态的准确估计，同时也无法满足机器人长时间运动的实时控制需求。

为了解决上述问题，有从业者提出使用多源信息融合方法实现足式机器人本体状态估计，但是通常都是直接由惯导信息与运动学信息融合得到估计机器人速度，并不考虑位置估计，且融合过程中并不考虑运动学内部信息的融合，该类融合方式仅仅是处于一级信息融合，仍然存在基于IMU的估计方式会产生漂移、基于运动学解算方式会产生大量噪声等问题，估计结果的精度仍然不高。

发明内容

本发明要解决的技术问题就在于：针对现有技术存在的技术问题，本发明提供一种实现方法简单、兼具IMU与运动学解算估计方式的优势，且估计结果不会产生漂移，同时又能滤除尖峰噪声，以及估计结果精度高的基于多传感器信息融合的足式机器人本体状态估计方法。

为解决上述技术问题，本发明提出的技术方案为：

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

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

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

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

作为本发明的进一步改进，所述步骤S1的具体步骤为：

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

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

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

作为本发明的进一步改进，所述步骤S11中单腿运动学解算的具体步骤为：

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

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

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

作为本发明的进一步改进：所述步骤S13中具体通过将各所述支撑腿的位置信息进行加权以实现融合，由加权结果得到所述本体初始位置。

作为本发明的进一步改进：所述本体初始位置具体取所有支撑腿的位置信息加权和的平均值。

作为本发明的进一步改进，所述步骤S2的具体步骤为：

S21.利用IMU***输出的姿态角，计算载体系与参考惯性系的姿态转换矩阵；

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

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

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

作为本发明的进一步改进：所述步骤S22中按照下式转换得到参考惯性系加速度；

其中，

为姿态转换矩阵，f^b为IMU直接输出的载体系加速度，gⁿ为重力加速度，

为转换后的参考惯性系加速度。

作为本发明的进一步改进，所述步骤S23中具体采用六位置法进行解算得到初步的真实加速度，具体步骤为：

S231.构建误差简化模型为：

其中，D_x，D_y，D_Z为IMU系中加速度计三个方向的真实值；M_x，M_y，M_Z为IMU系中加速度计三个方向的测量值；S_x，S_y，S_z为加速度计的标度因数；B_x，B_y，B_Z为加速度计三个方向的零偏；

S232.通过六位置翻滚标定法求解出所述误差简化模型中S和B，并将其补偿进参考惯性系加速度，得到参考惯性系的真实加速度。

作为本发明的进一步改进：所述步骤S24中具体结合所述本体初始状态估计结果得到的本体初始速度与位置，通过积分算法求得所述本体状态的惯导解算值。

作为本发明的进一步改进：所述步骤S3中具体使用卡尔曼滤波法将所述初始估计结果和所述惯导解算结果进行融合。

与现有技术相比，本发明的优点在于：

1)本发明基于多传感器信息融合的足式机器人本体状态估计方法，通过将运动学信息、IMU信息的两类传感器信息经过两级融合，实现对足式机器人本体状态的估计，能够充分发挥两类传感器信息的优势，一方面能够抑制IMU积分产生的漂移，另一方面能够减小运动学解算中震荡、冲击等带来的噪声影响，进而给出对本体速度和位置状态的最优估计，提高估计精度。

2)本发明基于多传感器信息融合的足式机器人本体状态估计方法，采用多种感知信息融合的方法实现本体状态估计，能够对动物的感知、行为、意识等实现更深入精细的模仿，从而有助于进一步提高机器人的智能化水平。

3)本发明基于多传感器信息融合的足式机器人本体状态估计方法，基于卡尔曼滤波方法将惯导信息与运动学信息进行二级信息融合，实现机器人速度与位置的估计，能够大大降低数据计算量且实时性高，可以进一步提高估计效率以及精度。

附图说明

图1是本实施例基于多传感器信息融合的足式机器人本体状态估计方法的实现流程示意图。

图2是本实施例实现足式机器人本体状态估计的原理示意图。

图3是本实施例构建的四足机器人简化模型的原理示意图。

图4是本实施例步骤S1的实现流程示意图。

图5是本实施例中构建的机器人单腿机械结构与简化模型示意图。

具体实施方式

以下结合说明书附图和具体优选的实施例对本发明作进一步描述，但并不因此而限制本发明的保护范围。

如图1、2所示，本实施例基于多传感器信息融合的足式机器人本体状态估计方法，步骤包括：

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

S2.惯导建模解算：通过IMU***获取目标机器人的IMU信息，并基于获取的IMU信息进行惯导建模解算，得到惯导解算结果；

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

本实施例通过将运动学信息、IMU信息的两类传感器信息经过两级融合，实现对足式机器人本体状态的估计，能够充分发挥两类传感器信息的优势，一方面能够抑制IMU积分产生的漂移，另一方面能够减小运动学解算中震荡、冲击等带来的噪声影响，进而给出对本体速度和位置状态的最优估计，提高估计精度；本实施例采用多种感知信息融合的方法实现本体状态估计，能够对动物的感知、行为、意识等实现更深入精细的模仿，从而有助于进一步提高机器人的智能化水平。

本实施例机器人具体为四足仿生机器人，四足仿生机器人简化模型如图3所示，机器人主要由本体和四条关节腿组成，仿造四足哺乳动物，每条腿共有四个关节，从下至上依次为：踝关节、膝关节、髋前关节和髋侧关节，其中，{O_n-X_nY_nZ_n}表示机器人运动的参考惯性系，{O_b-X_bY_bZ_b}表示机器人本体固连坐标系。IMU和四条支撑腿的运动学解算都可以作为本体状态估计的信息源，其中支撑腿之间属于同类数据，支撑腿与IMU为异类数据，本实施例先进行同类数据的融合，以完成一级信息融合，之后将初步融合结果再与IMU进行异类融合，以完成二级信息融合，最终实现机器人本体状态估计，如图2所示。

如图4所示，本实施例中步骤S1的具体步骤为：

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

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

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

本实施例首先基于内部位移传感器进行运动学建模解算足式机器人本体状态，通过对机器人单腿进行正运动学建模分析，求解本体相对于支撑腿足端的位姿，进而将内部位移传感器的运动学信息进行一级信息融合，求解出本体的速度与位置。

本实施例中，步骤S11中单腿运动学解算的具体步骤为：

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

S112.获取IMU输出的姿态角信息，根据获取的姿态角信息将步骤S11求解出的位置投影到参考惯性系三个轴向中；

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

本实施例单腿运动学解算时，具体定义机器人本体固连坐标系原点在本体质心，忽略IMU的安装误差，则IMU载体系与本体固连系三个轴向方向一致。首先基于内部关节位移传感器和机器人几何构型，通过单腿运动学求出足端在本体固连坐标系中的位置；然后利用IMU输出姿态角信息，将该位置投影到参考惯性系三个轴向中；最后反解出本体在参考惯性系中位置与速度，在具体应用实施例中的详细步骤为：

(1)求解足端在本体固连坐标系中位置。

如图5所示为机器人单腿机械结构图与简化模型，在每个关节处建立坐标系{O_i-X_iY_iZ_i}，i∈{1,2,3,4,5}，则从足端{O₅}到髋部{O₀}坐标系的转换关系为：

令足端点在足端坐标系中位置为P₅，足端点在髋部坐标系中位置为P₀，则足端点在髋部坐标系中位置可以表示为：

同样的原理可以求解髋部坐标系与本体固连坐标系的转换关系，则可求得足端点在本体系中的位置。

(2)利用IMU信息将步骤(1)求出的位置投影到机器人运动的参考惯性系。

通过IMU输出三个姿态角，利用该姿态角可以计算机器人本体当前时刻的坐标系与初始时刻的参考惯性系之间的转换矩阵，与步骤(1)计算的位置相乘即可求得该位置在参考惯性系中的投影。

(3)求解位置与速度。

根据步骤(1)、(2)的结果反解求出本体质心相对于支撑腿足端的位置变化，对位置进行微分后可以求得速度。当已知初始时刻质心的位置时，不断累加每一时间段的位移即可求得当前时刻机器人质心的位置。

本实施例中，步骤S13中具体通过将各支撑腿的位置信息进行加权以实现融合，由加权结果得到本体初始位置，本体初始位置具体是取所有支撑腿的位置信息加权和的平均值。

为了减小足端滑动、机构间隙等带来的误差影响，当有多条腿处于支撑状态时，取其位移计算的平均值作为机体质心最终的位移量，则本实施例针对四足机器人有：

即总共可能出现16中腿部着地情况，对多支撑腿运动解算信息采用加权法进行融合，详见表1。表中LF、RF、LH、RH分别代表四足机器人的左前腿、右前腿、左后腿、右后腿。相应的，机体质心的速度V_MEA可以通过位微分得到，其中支撑腿的判定具体根据足端与地面的接触力情况得到。

表1机体质心位移与支撑腿情况对应关系

本实施例通过表1求解从k-1时刻到k时刻机器人质心的位移后，在已知初始时刻位置的情况下，通过累加得到当前时刻的位置；利用该位移除以采样时间即可求得该采样时间段内的平均速度，当采样时间很短时，该平均速度可以看作为当前时刻的实时速度。

对于其他的双足或六足等多足机器人，腿部着地情况的种类不同，可适用与上述四足机器人原理相同的均值融合的方法求解位移。

本实施例中，步骤S2的具体步骤为：

S21.利用IMU***输出的姿态角，计算载体系与参考惯性系的姿态转换矩阵；

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

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

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

本实施例中，步骤S22中按照下式转换得到参考惯性系加速度；

其中，

为姿态转换矩阵，f^b为IMU直接输出的载体系加速度，gⁿ为重力加速度，

为转换后的参考惯性系加速度。

本实施例中，步骤S23中具体采用六位置法进行解算得到初步的真实加速度，具体步骤为：

S231.构建误差简化模型为：

其中，D_x，D_y，D_Z为IMU系中加速度计三个方向的真实值；M_x，M_y，M_Z为IMU系中加速度计三个方向的测量值；S_x，S_y，S_z为加速度计的标度因数；B_x，B_y，B_Z为加速度计三个方向的零偏；

S232.通过六位置翻滚标定法求解出误差简化模型中S和B，并将其补偿进参考惯性系加速度，得到参考惯性系的真实加速度。

本实施例中，步骤S24中具体结合本体初始状态估计结果得到的本体初始速度与位置，通过积分算法求得本体状态的惯导解算值。

将IMU载体坐标系转换到机器人运动的参考惯性系时，IMU固连系中X轴方向垂直于传感器接口面，向里为正，Z轴方向垂直于传感器上下平面，向下为正，Y轴根据右手法则确定。安装过程中，忽略安装及平台误差，使IMU的X轴朝向机器人运动的前进方向，Z轴朝向重力加速度方向。为实现惯导建模解算，对IMU进行初始化设置，使其输出当前时刻IMU固连系相对于初始时刻固连系(也即地面惯性参考惯性系)的角度，分别为滚转角

俯仰角θ、偏航角ψ，根据实际运动情况，其取值范围是：

ψ∈[-π，π]；而从地面惯性参考惯性系到IMU固连系的转换可以通过一个方向余弦矩阵表示，该矩阵主要由沿X轴转动产生的转换矩阵

沿Y轴转动产生的转换矩阵RM_θ以及沿Z轴转动产生的转换矩阵RM_ψ组合得到，则用欧拉角表示的方向余弦矩阵DCM可以表示为：

同时，对于任意的转换矩阵，其逆矩阵与转置矩阵等价，即

DCM^-1＝DCM^T (7)

则本体在参考惯性系中的加速度、速度向量可以通过投影在IMU坐标系的相应向量来表示

a_NED＝DCM·a_IMU (8)

V_NED＝DCM·V_IMU (9)

其中，三个姿态角

θ、ψ是经内置的扩展卡尔曼滤波进行信息融合后的输出。

本实施例进行惯导建模解算时，具体首先利用IMU输出姿态角，计算载体系与参考惯性系的姿态转换矩阵，通过坐标系变换将IMU输出的载体系加速度转换为参考惯性系加速度，地面惯性参考惯性系编排的速度更新微分方程如上述(4)所示；然后针对IMU漂移误差做静态标定，采用六位置法补偿确定性偏差，得到初步的真实加速度，其中确定性误差的简化模型如上述式(5)所示，通过六位置翻滚标定法求解S和B后，将其补偿进参考惯性系加速度，即可得到参考惯性系的真实加速度；最后结合本体初始状态估计结果得到的本体初始速度与位置，通过积分算法求得本体状态的惯导解算值。

本实施例中，步骤S3中具体使用卡尔曼滤波法将初始估计结果和惯导解算结果进行融合，基于卡尔曼滤波法建立估计对象的状态模型和量测模型，把需要用到的传感器信息纳入到这两个模型中来，进而通过一系列更新预测方程求得对状态的最优估计。卡尔曼滤波方法在对当前时刻的状态进行计算时只需用到前一时刻的状态与量测数据，本实施例基于卡尔曼滤波方法将惯导信息与运动学信息进行二级信息融合，实现机器人速度与位置的估计，能够大大降低数据计算量且实时性高，可以进一步提高估计效率以及精度。在具体应用实施例中，可以采用改进型Kalman滤波器，如EKF(Extended Kalman Filter)、UKF(Unscented Kalman Filter)和粒子滤波等，其中EKF可适用于非线性较弱的***，对于强非线性***，可考虑使用UKF(Unscented Kalman Filter)和粒子滤波方法。

上述只是本发明的较佳实施例，并非对本发明作任何形式上的限制。虽然本发明已以较佳实施例揭露如上，然而并非用以限定本发明。因此，凡是未脱离本发明技术方案的内容，依据本发明技术实质对以上实施例所做的任何简单修改、等同变化及修饰，均应落在本发明技术方案保护的范围内。

Claims (7)

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ⁿ为重力加速度，

为转换后的参考惯性系加速度。

5.根据权利要求4所述的基于多传感器信息融合的足式机器人本体状态估计方法，其特征在于，所述步骤S23中具体采用六位置法进行解算得到初步的真实加速度，具体步骤为：

S231.构建误差简化模型为：

其中，D_x，D_y，D_Z为IMU载体系中加速度计三个方向的真实值；M_x，M_y，M_Z为IMU载体系中加速度计三个方向的测量值；S_x，S_y，S_z为加速度计的标度因数；B_x，B_y，B_Z为加速度计三个方向的零偏；

S232.通过六位置翻滚标定法求解出所述误差简化模型中S_x、S_y、S_z和B_x、B_y、B_Z，并将其补偿进参考惯性系加速度，得到参考惯性系的真实加速度。

6.根据权利要求5所述的基于多传感器信息融合的足式机器人本体状态估计方法，其特征在于，所述步骤S24中具体结合所述本体状态初始估计结果得到的本体初始速度与初始位置，通过积分算法求得所述本体状态的惯导解算值。

7.根据权利要求1～4中任意一项所述的基于多传感器信息融合的足式机器人本体状态估计方法，其特征在于：所述步骤S3中具体使用卡尔曼滤波法将所述本体状态初始估计结果和所述惯导解算结果进行融合。

Images