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

权利要求书

1.一种机器人多相机视觉惯性实时定位方法，其特征在于，包括以下步骤：

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

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

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

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

所述视觉点云地图具体为通过以下步骤构建：

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

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

相机的位姿更新具体为：

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

所述获取机器人的当前多目图像以及当前位姿的具体步骤如下：

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

根据所述当前多目图像、前一时刻的多目图像计算位姿变化量；

根据所述位姿变化量和前一时刻的位姿计算机器人的当前位姿；

所述的根据视觉点云地图、惯性传感器预积分对当前位姿进行优化时，也对历史关键位姿进行优化。

2.如权利要求1所述的机器人多相机视觉惯性实时定位方法，其特征在于，

所述的多目图像包括多个图像特征，所述的多目图像通过多目相机获取。

3.如权利要求1或2所述的机器人多相机视觉惯性实时定位方法，其特征在于，所述的定位方法通过以下模块执行：

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

第一计算模块，用于根据所述当前图像估计当前多目相机系统的位姿；

第二计算模块，用于根据所述当前惯性传感器数据得到当前机器人的角度、速度；

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

4.一种机器人多相机视觉惯性实时定位装置，其特征在于，所述的定位装置还包括处理器、存储器以及存储在存储器中的程序，所述程序被处理器执行时实现如权利要求1或2中任一项所述的机器人多相机视觉惯性实时定位方法的步骤。