[发明专利]一种基于激光与惯导融合的足式机器人本体状态估计方法

说明书

本发明公开了一种基于激光与惯导融合的足式机器人本体状态估计方法，所述方法包括：采集足式机器人本体的IMU数据以及激光数据；对所述IMU数据与所述激光数据进行时间戳同步与预处理；在所述时间戳同步与预处理之后，对所述IMU数据进行积分，得到积分数据，同时对所述激光数据进行激光扫描和帧间点线匹配，得到增量运动变化；将所述积分数据作为状态预测输入至递归贝叶斯滤波器，且将所述得到的增量运动变化作为状态校正与更新输入至所述递归贝叶斯滤波器；所述递归贝叶斯滤波器通过对比所述状态预测所输入的数据与所述状态校正与更新所输入的数据来进行误差测量，得到所述足式机器人本体运动状态。在本发明实施中，提高状态估计的可靠性。

技术领域

本发明涉及一种足式机器人本地运动状态的技术领域，尤其涉及一种基于激光与惯导融合的足式机器人本体状态估计方法。

背景技术

移动机器人需要对其状态进行良好的估计，才能在结构化和非结构化环境中进行闭环控制。许多现有的算法都依赖来自多个传感器的数据融合以计算这些估计值。该技术领域常用本体感应传感器的测量信息，如加速度、机器人内部的电机速度、关节角度，以及来自外部特征的测量信息，如光强度、距离测量、声音振幅等进行融合，然而这些数据来源并不总是可靠的。轮式机器人通常使用IMU以及电机编码器在与地面保持平稳可靠接触下能较好的进行里程和状态估计，而腿式机器人通过间歇性的脚底接触与环境接触交互，对IMU和编码器测量中引入了额外的噪声，足式机器人的里程状态通常采用卡尔曼滤波器，需要仔细调参，单靠IMU和关节传感器进行位置常发生飘逸。而传统足式机器人基于本体IMU容易受到噪声的影响，同时纯靠激光传感器信息容易受遮挡出现丢帧，无法匹配进行估计的现象。

发明内容

本发明的目的在于克服现有技术的不足，本发明提供了一种基于激光与惯导融合的足式机器人本体状态估计方法，融合内部感应传感器和外部测量传感器信息，避免依赖单一数据来源，采用点线匹配算法的二次收敛特性加速算法运行，利用扩展卡尔曼滤波对激光里程计和IMU里程计数据进行融合，最终得到本机器人本体状态的可靠估计。

为了解决上述技术问题，本发明实施例提供了一种基于激光与惯导融合的足式机器人本体状态估计方法，其特征在于，所述方法包括：

采集足式机器人本体的IMU数据以及激光数据；

对所述IMU数据与所述激光数据进行时间戳同步与预处理；

在所述时间戳同步与预处理之后，对所述IMU数据进行积分，得到积分数据，同时对所述激光数据进行帧间点线匹配，得到增量运动变化；

将所述积分数据作为状态预测输入至递归贝叶斯滤波器，且将所述得到的增量运动变化作为状态校正与更新输入至所述递归贝叶斯滤波器；

所述递归贝叶斯滤波器通过对比所述状态预测所输入的数据与所述状态校正与更新所输入的数据来进行误差测量，得到所述足式机器人本体运动状态。

可选的，其特征在于，所述采集足式机器人本体的IMU数据以及激光数据包括：

通过IMU传感器中的加速度计采集足式机器人本体的线性加速度；

通过IMU传感器中的陀螺仪采集足式机器人本体的角速度；

通过激光雷达采集足式机器人本体的激光数据。

可选的，所述对所述IMU数据与所述激光数据进行时间戳同步与预处理包括：

对所述IMU数据与所述激光数据进行时间戳同步，得到同步的IMU数据和激光数据；

基于所述IMU数据和所述激光数据，对所述IMU数据与所述激光数据进行预处理，其中，所述预处理包括将所述IMU数据和所述激光数据进行去噪，以及将所述IMU数据和所述激光数据进行坐标系转化。

可选的，所述将所述IMU数据进行坐标系转化包括：