[发明专利]一种机器人多相机视觉惯性实时定位方法及装置

说明书

本发明公开了一种机器人多相机视觉惯性实时定位方法及装置，包括获取机器人的当前多目图像以及惯性传感器数据，根据当前图像提取图像特征点，估计当前机器人位姿，根据当前机器人位姿重建出3D点云，存储历史及当前点云数据对视觉点云地图进行维护，根据惯性传感器数据完成初始化估计出传感器偏置值，预积分得到当前机器人的速度、角度，根据视觉点云地图、惯性传感器预积分对当前位姿进行优化等步骤，本发明中提及的多目相机利用多个视角的信息提供一个更广阔的视野，多个相机朝向不同，很难出现全部视野被遮挡的状况，并且多个相机提供的视觉特征更为丰富，几乎可以保证满足完成定位需要的特征要求。

技术领域

本发明涉及机器人导航技术，具体地说，涉及一种机器人多相机视觉惯性实时定位方法及装置。

背景技术

目前，越来越多不同类型的机器人出现在生产生活的方方面面，对于仓储物流、巡检监控等领域而言，其工作需要机器人能够在一个相对固定的环境内实现长期稳定的运行，且能实现精确的自定位。在相机视野被障碍物遮挡，视觉特征不明显，特征纹理重复性高难以匹配的情况下，往往会发生定位跟丢的事件。另外，当机器人运动速度过快以致成像模糊时，已有的纯视觉定位方法也难以胜任。多目相机利用多个视角(重叠或非重叠)的信息提供一个更广阔的视野，可以很大程度上解决以上问题。而IMU(惯性测量单元)的引入增强了系统的鲁棒性，使其在快速运动时定位不易丢失，从而达到较好的防抖动效果。多传感器的融合使用，可以获得更多的数据关联，在算法中充分发挥各个传感器的优势信息，从而到达提升定位精度、鲁棒性的效果。

发明内容

为了克服现有技术的不足，本发明的目的在于提供一种机器人多相机视觉惯性实时定位方法及装置，本发明是通过以下技术方案来实现的：

本发明公开了一种机器人多相机视觉惯性实时定位方法，包括以下步骤：

获取机器人的当前多目图像以及惯性传感器数据；

根据当前图像提取图像特征点，估计当前机器人位姿；根据当前机器人位姿重建出3D点云，存储历史及当前点云数据对视觉点云地图进行维护；

根据惯性传感器数据完成初始化估计出传感器偏置值，预积分得到当前机器人的速度、角度；

根据视觉点云地图、惯性传感器预积分对当前位姿进行优化。

作为进一步地改进，本发明所述的多目图像包括多个图像特征，所述的多目图像通过多目相机获取。

作为进一步地改进，本发明所述视觉点云地图具体为通过以下步骤构建：

获取工作环境的多目图像序列；计算各相机图像特征点的重投影误差来估计当前相机系统位姿；所述视觉点云地图由机器人位姿计算得到各相机位姿之后再进行点云重建，所述相机特征点与相机位姿的对应关系，具体为根据下式进行计算：

其中为第i个相机相对于世界坐标系的位姿变换，p_j为第j个世界坐标系下的空间3D点，为第i个相机的相机坐标系相对于像素坐标系的投影变换，u_ji为投影之后获得的第ji个像素点。

作为进一步地改进，本发明所述的相机的位姿更新具体为：

其中，e^μ为多目相机系统的位姿更新的se(3)李代数表示，为第i个相机相对于世界坐标系的位姿变换，E_i1为第i个相机相对于多目相机系统的位姿变换，为位姿更新后的第i个相机相对于世界坐标系的位姿变换。

作为进一步地改进，本发明所述获取机器人的当前多目图像以及当前位姿的具体步骤如下：

获取机器人的当前多目图像、以及机器人的前一时刻的多目图像和前一时刻的位姿；