四足机器人足式里程计和IMU的融合方法、终端及存储介质与流程

本申请属于移动机器人，尤其涉及一种四足机器人足式里程计和imu的融合方法、终端及存储介质。

背景技术：

1、多传感器融合就是利用计算机技术将来自多传感器或多源的信息和数据，在一定的准则下加以自动分析和综合，以完成所需要的决策和估计而进行的信息处理过程。多传感器融合基本原理就像人脑综合处理信息的过程一样，将各种传感器进行多层次、多空间的信息互补和优化组合处理，最终产生对观测环境的一致性解释。在这个过程中要充分利用多源数据进行合理支配与使用，而信息融合的最终目标则是基于各传感器获得的分离观测信息，通过对信息多级别、多方面组合导出更多有用信息。这不仅是利用了多个传感器相互协同操作的优势，而且也综合处理了其它信息源的数据来提高整个传感器系统的智能化。

2、在机器人建图和定位过程中，为提高点云或特征匹配的稳定性和降低匹配所需时间，一般都需要利用里程计，imu(惯性传感器)等传感器信息融合后提供运动预测。对于轮式机器人来说，imu提供的旋转预测值比较准确，但是平移预测值误差较大，轮式里程计(一般基于编码器)可以提供较为准确的平移预测，但由于打滑等原因旋转信息可能会有误差，由于两种传感器在不同的方面体现出了绝对的优势，所以一般取轮式里程计的平移信息和imu的角度信息进行运动预测。

3、而对于没有轮式里程计的四足机器人，由于四足机器人的足式里程计提供的平移信息在不同的情况下可信度不同，不能直接作为融合后结果，需要与imu提供的平移信息进行融合，得到比单独使用imu或足式里程计更加有效和稳定的平移融合结果。如何对足式里程计及imu的平移信息进行融合，提升平移信息的可信度及四足机器人运动预测的准确性成了亟待解决的问题。

技术实现思路

1、针对上述技术问题，本申请提供一种四足机器人足式里程计和imu的融合方法、终端及存储介质，以提升平移信息的可信度及四足机器人运动预测的准确性。

2、本申请提供了一种四足机器人足式里程计和imu的融合方法，包括：获取足式里程计、imu惯性传感器的平移数据，及系统噪声，其中，所述平移数据包括平移向量及平移噪声；根据足式里程计及imu惯性传感器的平移噪声，对所述系统噪声进行修正；根据修正后的系统噪声、足式里程计及imu惯性传感器的平移向量，计算足式里程计与imu惯性传感器融合后的平移向量。

3、在一实施方式中，获取足式里程计、imu惯性传感器的平移数据，及系统噪声，包括：获取四足机器人与地面接触的足数；若所述四足机器人与地面接触的足数为4，则所述足式里程计的平移噪声值为a；若所述四足机器人与地面接触的足数小于4，则根据地面的平整度确定所述足式里程计的平移噪声值；其中，a为所述足式里程计的平移噪声值的最小值。

4、在一实施方式中，根据地面平整度确定所述足式里程计的平移噪声值，包括：通过以下公式计算所述足式里程计的平移噪声值：

5、

6、其中，hflat为所述地面平整度，nl为所述足式里程计的平移噪声值。

7、在一实施方式中，在根据地面平整度确定所述足式里程计的平移噪声值之前，包括：

8、通过以下公式计算所述地面平整度：

9、

10、haverage＝(ffl*hfl+ffr*hfr+fhl*hhl+fhr*hhr)/4

11、其中,hfl、hfr、hhl、hhr分别表示四足机器人的左前足、右前足、左后足、右后足接触地面时各足末端距离机体的垂直高度；ffl、ffr、fhl、fhr分别表示四足机器人的左前足、右前足、左后足、右后足与地面的接触情况，与地面接触时取值为1，未与地面接触时取值为0；haverage为平均高度；lleg是四足机器人的大腿与小腿的总长度。

12、在一实施方式中，所述根据足式里程计及imu惯性传感器的平移噪声，对所述系统噪声进行修正的步骤，包括：

13、通过以下公式计算修正后的系统噪声：

14、p′n+1＝(1-k)×pn+1

15、其中，p′n+1为第n+1个计算周期修正后的系统噪声，k为第n+1个计算周期的修正系数，pn+1为第n+1个计算周期修正前的系统噪声。

16、在一实施方式中，在计算修正后的系统噪声之前，包括：

17、通过以下公式计算所述第n+1个计算周期修正前的系统噪声：

18、pn+1＝p′n+ni

19、通过以下公式计算所述第n+1个计算周期的修正系数：

20、

21、其中，pn‘为第n个计算周期修正后的系统噪声，ni为imu惯性传感器的平移噪声，nl为第n+1个计算周期的足式里程计的平移噪声。

22、在一实施方式中，所述根据修正后的系统噪声、足式里程计及imu惯性传感器的平移向量，计算足式里程计与imu惯性传感器融合后的平移向量的步骤，包括：

23、通过以下公式计算所述融合后的平移向量：

24、t′n+1＝tn+1+k(tf-ti)

25、其中，t′n+1为第n+1个计算周期融合后的平移向量，tn+1为第n+1个计算周期融合前的平移向量，k为第n+1个计算周期的修正系数，tf为第n+1个计算周期足式里程计的平移向量，ti为第n+1个计算周期imu惯性传感器的平移向量。

26、在一实施方式中，在计算所述融合后的平移向量之前，包括：

27、通过以下公式计算所述第n+1个计算周期融合前的平移向量：

28、tn+1＝t′n+ti

29、其中，tn‘为第n个计算周期融合后的平移向量。

30、本申请还提供了一种终端，所述终端包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现上述融合方法的步骤。

31、本申请还提供了一种存储介质，所述存储介质存储有计算机程序，所述计算机程序被处理器执行时实现上述融合方法的步骤。

32、本申请提供的一种四足机器人足式里程计和imu的融合方法、终端及存储介质，能够根据足式里程计及imu的平移噪声，对系统噪声进行修正；并根据修正后的系统噪声、足式里程计及imu的平移向量，计算足式里程计与imu融合后的平移向量，提升平移向量的可信度及四足机器人运动预测的准确性。

技术特征：

1.一种四足机器人足式里程计和imu的融合方法，其特征在于，包括：

2.如权利要求1所述的融合方法，其特征在于，获取足式里程计、imu惯性传感器的平移数据，及系统噪声，包括：

3.如权利要求2所述的融合方法，其特征在于，根据地面平整度确定所述足式里程计的平移噪声值，包括：

4.如权利要求2-3任一项所述的融合方法，其特征在于，在根据地面平整度确定所述足式里程计的平移噪声值之前，包括：

5.如权利要求1所述的融合方法，其特征在于，所述根据足式里程计及imu惯性传感器的平移噪声，对所述系统噪声进行修正的步骤，包括：

6.如权利要求5所述的融合方法，其特征在于，在计算修正后的系统噪声之前，包括：

7.如权利要求1所述的融合方法，其特征在于，所述根据修正后的系统噪声、足式里程计及imu惯性传感器的平移向量，计算足式里程计与imu惯性传感器融合后的平移向量的步骤，包括：

8.如权利要求7所述的融合方法，其特征在于，在计算所述融合后的平移向量之前，包括：

9.一种终端，其特征在于，所述终端包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现如权利要求1至8任一项所述融合方法的步骤。

10.一种存储介质，所述存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至8任一项所述融合方法的步骤。

技术总结
本申请涉及一种四足机器人足式里程计和IMU的融合方法、终端及存储介质，其中，融合方法包括：获取足式里程计、IMU惯性传感器的平移数据，及系统噪声，其中，平移数据包括平移向量及平移噪声；根据足式里程计及IMU惯性传感器的平移噪声，对系统噪声进行修正；根据修正后的系统噪声、足式里程计及IMU惯性传感器的平移向量，计算足式里程计与IMU惯性传感器融合后的平移向量。本申请提供的四足机器人足式里程计和IMU的融合方法、终端及存储介质，能够根据足式里程计及IMU的平移噪声，对系统噪声进行修正；并根据修正后的系统噪声、足式里程计及IMU的平移向量，计算足式里程计与IMU融合后的平移向量，提升平移向量的可信度及四足机器人运动预测的准确性。

技术研发人员：沈岗,张旭
受保护的技术使用者：盈合（深圳）机器人与自动化科技有限公司
技术研发日：
技术公布日：2024/1/15