[发明专利]一种足式机器人多模态感知的状态估计方法及系统

说明书

本发明公开了一种足式机器人多模态感知的状态估计方法及系统，所述方法包括：基于IMU传感器测量足式机器人的加速度和角速度，并预测所述足式机器人的状态均值和协方差；基于关节编码器获取所述足式机器人中所有关节的角度位置，并计算所述足式机器人中每一只脚的脚点位置所对应的计量值与预测值；基于所述计量值与所述预测值，计算所述足式机器人的测量残差；基于所述测量残差和测量雅可比，对所述状态均值和所述协方差进行校正，获取所述足式机器人的最终状态。所述方法将提高足式机器人状态估计的精确度和稳定性。

技术领域

本发明涉及机器人领域，尤其涉及一种足式机器人多模态感知的状态评估方法及系统。

背景技术

足式机器人的研发灵感源于哺乳动物，是仿生学与机器人技术的重要体现，其环境适应性好、运动范围广、负载能力强，具备一定的自主作业的能力，能够实现崎岖山地运输、危险灾难救援、军事侦察等任务，已受到广泛的关注。但是本体的传感器精度有限，测量数据存在不确定性，需要对其状态进行良好的估计，以执行闭环控制指令和适应非结构环境。状态估计包括对机器人运动和特征的了解，在已知的环境中充分描述机器人随时间的运动，为了用不完美的传感器获得最好的估计，估计状态时记录数据的不确定性来确定估计结果的自信程度。传统的状态评估方法是通过结合机器人的动力学模型来完成状态估计与测量模型，利用动态模型给出预期的状态，并使用机器人上的各种传感器，提供机器人的一个或多个状态量，该方法面临着几个问题：1)动力学高度非线性，以及数值解的动态模型计算不准确；2)足式机器人的状态空间很大，不仅包括身体的姿态，还有脚部关节的位置与姿态；3)足式机器人通过多个间歇性与环境进行交互地面接触和撞击，使传感器更加嘈杂。

由此可见，传统的算法通常依赖于多个本体及外部传感器的数据融合来进行状态估计，然而外部传感器受到光强度、距离测量、声音幅度等特征因素的影响，其测量可靠性无法得到保障，降低了状态估计的精确度和稳定性。另外，由于足式机器人通过断续的脚-地接触与周围环境交互，在测量中也会引入额外的噪声，如何最大程度地降低噪声的影响也是一大挑战性难题。

发明内容

本发明的目的在于克服现有技术的不足，本发明提供了一种足式机器人多模态感知的状态估计方法及系统，利用扩展卡尔曼滤波器(EKF)，通过融合IMU和运动学信息实现状态估计，降低建模的复杂性，提高状态估计的效率和可靠性。

为了解决上述技术问题，本发明实施例提供了一种足式机器人多模态感知的状态估计方法，所述方法包括：

基于IMU传感器测量足式机器人的加速度和角速度，并预测所述足式机器人的状态均值和协方差，其中所述状态均值包括本体位置、本体速度、本体姿态的四元数、各脚部位置、IMU加速度计偏置和IMU陀螺仪偏置；

基于关节编码器获取所述足式机器人中所有关节的角度位置，并计算所述足式机器人中每一只脚的脚点位置所对应的计量值与预测值；

基于所述计量值与所述预测值，获取所述足式机器人的测量残差；

基于所述测量残差和测量雅可比，对所述状态均值和所述协方差进行校正，获取所述足式机器人的最终状态；

其中，所述足式机器人的测量残差为：

式中：e为测量残差，s为所述足式机器人中每一只脚的脚点位置所对应的计量值，为所述足式机器人中每一只脚的脚点位置所对应的预测值，α为所述足式机器人的关节角度，为预测的旋转矩阵，为预测的第i只脚的脚点位置，为预测的所述足式机器人本体的质心位置。

可选的，所述基于IMU传感器测量足式机器人的加速度和角速度，并预测所述足式机器人的状态均值和协方差包括：

基于所述IMU传感器中的加速度计测量所述足式机器人的线性加速度；

基于所述IMU传感器中的陀螺仪测量所述足式机器人的角速度；