[发明专利]基于多传感器信息融合的SLAM地图构建方法

说明书

本发明公开了基于多传感器信息融合的SLAM地图构建方法，使用二维激光雷达、深度相机、IMU和光电编码器采集数据，再将IMU和光电编码器采集到的位姿信息进行滤波处理，得到融合和后的位姿信息，同时将激光信息与深度图像信息进行融合，得到融合后的激光信息，最后将融合后的位姿信息与激光信息传入SLAM算法，并完成建图。本发明克服了单一传感器无法满足机器人在复杂环境中准确的位姿估计和构建与周围环境一致的地图的缺陷，使得二维激光雷达、深度相机、IMU和光电编码器信息融合的SLAM方法有益于提高机器人定位精度和建图效果，更有助于机器人在复杂环境中的自主路径规划、自主定位以及自主避障等应用需求的实现。

技术领域

本发明属于SLAM技术领域，具体涉及基于多传感器信息融合的SLAM地图构建方法。

背景技术

同时定位与建图(Simultaneous localization and mapping，SLAM)可有效解决自主移动机器人在未知环境中运动时定位、导航、建图等问题。机器人通过自身携带的传感器感知周围环境和自身信息，并根据感知到的信息进行机器人位姿估计和构建与周围环境一致的地图。地图构建完成后，机器人可以根据构建好的地图，实现自主路径规划、自主定位以及自主避障等应用需求，从而实现机器人自主移动的目的。

然而，随着机器人的广泛应用，使得机器人所处环境愈加复杂，单一传感器无法满足机器人在复杂环境中的位姿估计和构建与周围环境一致的地图，因此，限制了机器人在复杂环境中的使用。

发明内容

本发明的目的是提供基于多传感器信息融合的SLAM地图构建方法，克服了单一传感器无法满足机器人在复杂环境中的位姿估计和构建与周围环境一致的地图，限制了机器人在复杂环境中的使用的问题。

本发明所采用的技术方案是，

基于多传感器信息融合的SLAM地图构建方法，具体按照以下步骤实施：

步骤1、在机器人上安装二维激光雷达、深度相机、IMU和光电编码器，IMU包括陀螺仪和加速度计，机器人行走轮上连接光电编码器，采集机器人的速度、加速度以及角速度、激光信息、深度图像信息；

步骤2、根据采集到的加速度和角速度计算机器人的三维坐标系变化，并利于四元数解算机器人姿态角，对姿态角进行互补滤波，对采集到的速度进行滑动滤波；

将加速度、角速度、滤波后的姿态角以及滑动滤波后的速度利用扩展卡尔曼滤波进行融合，从而得机器人的位姿信息；

步骤3、将激光信息与深度图像信息进行信息融合，从而得到融合后的激光信息；

步骤4、将最新更新得到的位姿信息与融合后的激光信息传入SLAM算法，并构建地图。

对姿态角进行互补滤波具体过程为：

式中，ψ_a为加速度计姿态角，ψ_g为陀螺仪姿态角，ψ为滤波后的姿态角，dt/(τ+dt)为低通滤波，τ/(τ+dt)为高通滤波，τ为滤波器时间常数，1/τ为滤波器截止频率，dt为滤波器采样时间。

其中，第n次滤波时陀螺仪姿态角为：

则第n次滤波后的姿态角为：

步骤2中将加速度、角速度、滤波后的姿态角以及滑动滤波后的速度利用扩展卡尔曼滤波进行融合具体过程为：

步骤A、通过k-1时刻滤波后光电编码器测量的速度对k时刻的机器人位姿做先验估计，先验估计公式为：