[发明专利]基于多传感器紧耦合的移动机器人位姿估计方法和系统

说明书

本发明公开了一种基于多传感器紧耦合的移动机器人位姿估计方法和系统，属于机器人定位技术领域，其中方法包括：对相机采集的当前帧RGB‑D图像与上一帧RGB‑D图像进行特征匹配，计算匹配过程的视觉重投影误差；对IMU测量的数据进行积分，构建IMU积分残差；从激光雷达采集的点云中提取边缘特征点和平面特征点，计算边缘特征点到边缘线的距离以及平面特征点到平面的距离，构建激光点云几何残差；以视觉重投影误差、IMU积分残差和激光点云几何残差最小为目标，进行位姿估计，得到局部位姿；使用局部位姿更新激光点云地图后在激光点云地图上进行全局优化，得到全局位姿。本发明提高了移动机器人在复杂运动和复杂环境下的定位精度和鲁棒性。

技术领域

本发明属于机器人定位技术领域，更具体地，涉及一种基于多传感器紧耦合的移动机器人位姿估计方法和系统。

背景技术

在移动机器人领域，常使用视觉传感器、IMU、激光雷达中的一种或多种的融合作为实际SLAM应用算法。利用相机(单目、双目或深度)作为单一传感器的视觉SLAM方法受环境光照影响大，并且在纹理稀疏的环境下很难检测到有效的特征而造成定位缺失。利用激光雷达(2D或3D)作为单一传感器的激光SLAM方法测量频率低，并且激光雷达在运动速度变化大时存在难以矫正的运动失真。而IMU传感器是根据加速度和角速度积分来进行运动估计的，在短时间快速运动下可以提供较准确的运动估计。

将IMU和相机或激光雷达结合可以在一定程度上提高SLAM位姿估计的准确性和鲁棒性。但是视觉-IMU系统仍然无法在复杂、弱纹理的环境中长时间正常工作，激光-IMU系统在雾霾、灰尘环境下仍存在较大误差，而视觉-IMU-激光雷达系统现有尝试较少，且大都通过松耦合方式结合，无法同时发挥不同传感器的优势。

由此可见，现有机器人定位技术在快速运动和不同复杂环境下存在定位精度低、鲁棒性差的技术问题。

发明内容

针对现有技术的以上缺陷或改进需求，本发明提供了一种基于多传感器紧耦合的移动机器人位姿估计方法和系统，由此解决现有机器人定位技术在快速运动和不同复杂环境下存在定位精度低、鲁棒性差的技术问题。

为实现上述目的，按照本发明的一个方面，提供了一种基于多传感器紧耦合的移动机器人位姿估计方法，所述多传感器包括相机、IMU和激光雷达，所述方法包括：

对相机采集的当前帧RGB-D图像与上一帧RGB-D图像进行特征匹配，计算匹配过程的视觉重投影误差；

对IMU测量的数据进行积分，构建IMU积分残差；

从激光雷达采集的点云中提取边缘特征点和平面特征点，计算边缘特征点到边缘线的距离以及平面特征点到平面的距离，构建激光点云几何残差；

以视觉重投影误差、IMU积分残差和激光点云几何残差最小为目标，进行位姿估计，得到局部位姿；

使用局部位姿更新激光点云地图后在激光点云地图上进行全局优化，得到全局位姿。

进一步地，所述激光点云几何残差的具体构建方式为：

通过计算点云的局部曲率值，从激光雷达采集的点云中筛选边缘特征点和平面特征点，得到边缘特征点集合和平面特征点集合；

从当前次扫描得到的边缘特征点集合中选取一个边缘特征点，从上一次扫描得到的边缘特征点集合中选取两个边缘特征点组成边缘线；

从当前次扫描得到的平面特征点集合中选取一个平面特征点，从上一次扫描得到的平面特征点集合中选取三个平面特征点组成平面；

通过连续两次扫描的点云位姿变换关系计算边缘特征点和平面特征点的坐标，进而计算边缘特征点到边缘线的距离以及平面特征点到平面的距离，得到3D激光点云几何残差。

进一步地，所述边缘特征点集合与平面特征点集合的构建包括：