一种鲁棒的多目视觉惯性即时定位与建图方法

傅 博，焦艳梅，丁夏清，吴 俊，熊 蓉

(1.浙江大学智能系统与控制研究所，杭州310027；2.浙江大学工业控制技术国家重点实验室，杭州310027)

1 引言

深空探测任务对空间机器人提出了全自主漫游的要求，其关键是机器人的精确自定位能力。深空探测作为一种机器人特殊应用场景，与地面环境存在很多相似，可以借鉴目前快速发展的地面即时定位与建图(SLAM)技术来满足上述新需求。

目前公开的美国火星探测机器人(MER)的定位主要通过视觉实现[1-2]，由双目相机通过匹配当前时刻与前一时刻的图像实现对自身前后两个时刻位姿变换的估计[3]，并利用惯性测量单元(IMU)对翻滚角和俯仰角能观的特性，提升了视觉里程计的性能[4]。双目相机利用两个相机之间的基线，相比于单目相机要更加稳定和精确[5]，但视野较小，难以形成全位置、丰富视角的回访定位；若遇到相机视野被障碍物遮挡、视觉特征不明显、特征纹理重复性高以致难以匹配等情况，往往会丢失定位[3]，也难以用于机器人运动速度过快以致成像模糊的情况[4]。

多目相机能够利用多个视角的信息提供一个更广阔的视野，可以解决视野问题。其中，Multi-Col-SLAM[6]将ORB-SLAM[7-8]扩展到了多目的情况，提出了多目SLAM优化方法，并且实现了多目情况下的回环检测。Yang等[9-10]将 PTAMSLAM[11]扩展到了多目的情况，给出了多目视觉SLAM中测量误差雅克比的求解方法，并且基于上述方法在SLAM的数值优化中引入了多个相机获取的局部视觉特征。相比于单目、双目视觉定位方法，多目视觉方法需要配置更多的硬件和运算资源。以MultiCol-SLAM为例，在i7处理器上采集多目相机数据，定位可以获得3 Hz的信息，而采集双目相机数据并运行ORB-SLAM定位，可以获得15 Hz信息。不过实际使用中，3 Hz足够完成实时定位，而多目视觉定位可以带来鲁棒性和定位精度的提升。

针对机器人快速运动带来的问题，Liu等[12]提出了一种适用于车体的多目视觉惯性算法，在计算相对位姿时，利用IMU获得的翻滚角和俯仰角的信息简化需要求解的每一次运动的旋转矩阵，进而简化了位姿估计过程。然而由于IMU的信息带有噪声，直接用于运动估计会带来一定的误差；另外，其方法是分别求出相机和IMU的位姿，再在此基础上进行联合优化，这是一种松耦合的优化方法，无法得出精确的优化结果。

为解决上述问题，本文提出一种新的多目视觉惯性定位方法，结合多目相机的广阔视野和IMU提供的高频运动信息来完成定位。与现有的松耦合优化方法[12-13]不同，所提方法基于紧耦合优化，将图像的特征点加入至状态向量，直接优化出相机和IMU的位姿，得到精确的机器人位姿。

2 系统流程

如图1所示，在多目VINS系统中，多相机提供视觉信息，IMU提供高频运动信息。算法流程分为3步：数据采集预处理、初始化以及优化定位。数据采集模块对采集到的图像进行特征提取，对IMU数据进行预积分输出给下一模块。在初始化模块，视觉前端进行不同时刻不同相机之间的位姿解算，得到机器人相邻两个时刻的位姿更新。IMU完成初始化，得到世界坐标系相对于惯性系的相对位姿，完成视觉惯性配准。在优化定位模块，融合视觉重投影误差和IMU误差进行紧耦合联合优化，输出机器人位姿。

图1 系统流程图Fig.1 System flowchart

3 系统建模与位姿更新

3.1 坐标系定义

世界坐标系定义为W，机器人坐标系定位为B，并定义旋转矩阵R和平移向量t，则k时刻机器人坐标系相对于世界坐标系的变换矩阵WBkT可以表示为式(1)：

考虑硬件通用性，定义机器人固定相机数为N，第i个相机的相机坐标系为Ci。 本文统一使用机器人坐标系来表示机器人的运动。因为相机之间都是刚性固定的，假定外参系数已知，那么各相机的位姿和观测到的特征都可以通过外参转到机器人坐标系下，从而统一了待优化量的坐标系。

3.2 多目系统投影

参考Yang等[9-10]的工作，多目的投影模型(第j个特征点在第i个相机坐标系下的图像投影)如式(2)：

式中：π(·)为任意第n个相机的相机坐标系相对于像素坐标系的投影变换；为机器人在世界坐标系的位姿；为第i个相机相对于机器人坐标系的位姿(也就是外参)，由于传感器刚性固定于机器人上，可以认为BCiT是不变的；pj为第j个世界坐标系下的空间3D点；uij为投影之后获得的第ij个像素点。投影原理如图2所示。图中五角星为空间中3D点，红色圆点为2D像素点。

图2 投影原理图Fig.2 Schematic diagram of projection

3.3 相机位姿更新

不失一般性，先计算机器人本体的位姿更新，再经由外参的转化更新为每一个相机的位姿更新。基于机器人从时刻k到时刻k+1的位姿变换，和机器人在时刻k的位姿，可以推导出机器人在时刻k+1的位姿，如式(3)：

同理，在已知相机与机器人之间外参的前提下，第i个相机从时刻k到时刻k+1的位姿更新可以表示为式(4)：

4 视觉惯性紧耦合与IMU初始化

4.1 紧耦合优化

在本文方法中，SLAM系统有2个并行的线程，位姿跟踪和地图优化。考虑空间环境的复杂与大尺度，为了能尽可能的保持算法的稳定性，位姿跟踪在前端实时完成，而地图优化部分则在后端运行。

IMU预积分得到两帧之间的约束，放入地图中完成联合优化。本方法所提SLAM系统的地图包含了N个相机在不同时刻的位姿、一组被这些相机观测到的地图3D点以及2个位姿之间的预积分约束。图优化框架如图3所示。可以看到，机器人上固定的多相机使得机器人在任意时刻都可以观测到更多的路标点，而不同时刻之间的观测也有了更密集的数据关联，从而提升定位的准确性和鲁棒性。优化变量的损失函数可以表示为式(5)：

式中：Eproj为重投影误差，由像素点的测量值与公式(2)得到的像素点的估计值计算得到；Eimu为IMU项的误差。两项误差同时优化并更新机器人的位姿和空间中路标点的位置。

图3 图优化框架Fig.3 Framework of graph optimization

4.2 自动静止初始化

多目VINS在使用之前需要完成IMU的初始化，初始化时要求一定的旋转和平移运动，而且对于运动的大小和方式有一定的要求，导致在初始化时容易因不恰当的运动而失败，进而影响多目VINS的跟踪定位效果。另外，多目VINS的初始化要求在一些情况下无法被满足，例如安装在无人车上时，初始化采集的数据缺少俯仰角和翻滚角的信息，从而容易导致初始化失败。本文提出一种稳定的静止初始化算法，在机器人运行前先静置几秒完成初始化，从而完成稳定的多目VINS算法。

对于多目VINS系统的自动IMU初始化的算法，IMU的测量值加速度 3×1向量 a满足式(6)[12]：

式中：einit为需要优化的误差向量，g为重力加速度3×1向量，矩阵为IMU坐标系相对于世界坐标系的旋转3×3矩阵，其偏航角设为零。在静止状态下采集多组IMU数据，然后联合优化求解得到初始的俯仰角和翻滚角，从而解出旋转矩阵完成静止初始化，得到世界坐标系相对于惯性系的相对位姿。

5 实验验证

5.1 平台搭建

在V-REP中搭建仿真环境，在移动机器人上安装多目相机与IMU，采集数据完成了实验，如图4所示。多目相机的固定方式为：一个朝前的双目相机和一个斜朝后的单目相机。

图4 移动机器人传感器配置Fig.4 Configuration of mobile robot

5.2 结果分析

由于移动机器人只能在平面上运动，无法进行各角度的充分运动来完成初始化。用静止初始化方法，先采集静置机器人的数据，再控制机器人绕圈运动采集多目图像和IMU数据。实验结果如图5所示。图中红色、黑色的点为地图路标点，蓝色的框为机器人位姿，绿色线为相机不同时刻之间的共视。从图中可以看到，即使机器人只运动了较短的一段距离，已经能建出全局的地图，并且不同相机之间产生了丰富的数据关联，所以重建出的路标点较为稠密，且轮廓分明。此重建结果可以说明，多目相机提供的更为广阔的视野对于定位是有益的。

图5 移动机器人实验结果Fig.5 Experiment results of mobile robot

保存所提定位算法的轨迹，而轨迹真值则由V-REP仿真平台提供，计算相对轨迹误差(RPE)如图6所示，上图为平移误差，单位是m，下图是旋转误差，取一个角度的值，单位是deg。

图6 相对轨迹误差Fig.6 Relative pose error

可以看到，本文所提方法的平移误差略好约ORB-SLAM方法，旋转误差远优于ORB-SLAM方法。主要原因是融合IMU的高频运动信息可以带来更好的旋转精度，而且多目相机提供了更广阔的视野和更丰富的数据关联，可以增加回环的概率从而修正旋转误差。

多目相机带来了新的观测的同时也带来了新的外点，这些外点的存在一定程度上降低了定位算法的精度。下一部分的工作是利用算法在引入观测的同时去除外点，从而使得新增的都是有用的观测，并且优化算法，提高后端优化的速度来提升定位精度。

6 结论

1)多目相机可以提供一个更广阔的视野，提供更多的观测和更丰富的数据关联，提升定位算法的准确性和鲁棒性。

2)多目视觉惯性紧耦合优化可以提高定位方法的鲁棒性，更好地应用于快速移动、急停急走的应用场景，并且可以在视觉丢失的情况下辅助机器人继续完成定位。

3)自动静止初始化算法，使得所提方法不受应用场景的限制，提高了所提定位方法的稳定性。