深度相机与微机电惯性测量单元松组合导航算法

张福斌， 林家昀

(1.西北工业大学 航海学院, 陕西 西安 710129; 2.湖南航天机电设备与特种材料研究所, 湖南 长沙 410200)

0 引言

随着机器人技术的不断发展，在室内反恐、排爆以及搜救等活动中机器人的使用量日益增加。对于一个自主运动的物体，无论其执行何种任务，准确获取自身位置和姿态(简称位姿)信息是一个必不可少的功能，只有知道自身位姿，才能更好地完成任务[1]。所以室内机器人高精度导航与定位一直是学术研究和工程实践上的一个重点问题。

全球定位系统(GPS)作为一种机器人常用定位方式，由于信号容易被墙壁等障碍物遮挡，在室内几乎不能正常使用[2]。基于里程计的航位推算是一种轮式机器人常用的定位方式[3]，但是由于建筑物内部经常有楼梯等，导致轮式机器人使用范围受限，需要使用双足等非轮式机器人，这种类型的机器人通常无法携带里程计，因此必须依靠其他手段进行导航定位。

微机电系统(MEMS)惯性测量单元(IMU)是一种机器人常用的惯性导航传感器，具有低功耗、体积小、抵抗外界干扰能力强等优点[4]，但单纯基于MEMS IMU的导航系统存在定位误差快速增加等不足[5]。视觉导航作为一种新兴的导航与定位技术，优点在于视觉获取的信息量大[6-7]，能够还原三维场景，并且可以通过回环消除累积的定位误差，但视觉受环境特征影响大，图像匹配存在随机误差[8-9]。因此，惯性导航与视觉导航能够实现传感器之间的优势互补，抑制定位误差发散速度，提升导航系统的鲁棒性和定位精度[10]。

视觉与MEMS IMU组合导航有两种组合方式，分别为松组合与紧组合[11]。

松组合算法优点在于两种传感器独立工作，计算速度快，但不足之处在于通常只使用视觉导航的位置信息作为量测，导航精度较低。目前主流的视觉与MEMS IMU松组合算法由孙彤等[12]提出，该算法将视觉输出的位姿作为扩展卡尔曼滤波(EKF)的量测值，对载体位姿等状态量进行估计，得到更加准确的位姿信息。

紧组合算法优点在于将两种传感器信息之间的耦合关系计算在内，得到更加精确的导航结果，但不足之处在于算法复杂，计算量大。目前主流的视觉与MEMS IMU紧组合导航中，有基于单目相机的视觉惯性组合导航系统(VINS)、基于双目相机的视觉惯性紧耦合系统(OKVIS)以及基于深度相机的深度图像与惯性器件组合导航系统(RGB-D-Inertial SLAM)[13]等多个视觉惯性组合导航系统。

VINS通过对相机及由IMU输出的惯性导航数据进行前端预处理、相机初始化、光束平差法(BA)联合优化、全局优化和回环检测，实现视觉惯性的紧耦合[14]。但由于单目相机本身缺乏尺度信息，需要将惯性导航信息预积分进行尺度初始化。这一操作会将惯性导航误差引入尺度计算，故降低了自身定位与三维重构的精度。

OKVIS通过惯性导航预积分的值作为预测量，依据预测提取和匹配特征点，使用双目相机计算得到具有尺度的图像信息，再对图像使用非线性优化方法进行重投影优化，预测和修正惯性导航系统的误差[15]。该方法能够得到较高精度的载体位姿信息，并且包含尺度；但计算量大，对硬件要求较高，且不能实时重构三维环境，且没有重定位的功能，无法消除累积误差。

在基于深度相机和由IMU构成的惯性导航系统融合算法的研究中， Brunetto等[16]提出的基于移动设备的彩色- 深度(RGB-D)视觉测量与惯性测量融合算法，通过一组卡尔曼滤波器，消除惯性导航位姿测量数据噪声，对视觉与惯性导航的测量数据进行融合。

Kim等[17]提出的高动态环境中基于RGB-D传感器和IMU的视觉里程计算法，针对高动态环境，基于加速稳健特征(SURF)描述子，将深度点云与彩色图像结合生成三维特征点，并使用IMU对两幅连续图像之间的旋转分量进行补偿，得到特征点的运动矢量。

Zhu等[18]开发了基于RGB-D和惯性输入的室内场景实时重建算法，将彩色图像、深度点云和惯性测量结果通过EKF相融合，并采用迭代的方法进行联合优化，从而提高传感器快速运动时的重建鲁棒性。

上述3种算法均为视觉与由IMU构成的惯性导航系统紧耦合的算法，计算精度高，系统鲁棒性强，但是系统工作时同时处理彩色图像、深度点云以及惯性导航系统测量数据，算法复杂，计算量大，运算时间长。

基于深度相机的工作原理，本文综合考虑室内环境的特点，在保证系统精度和鲁棒性的前提下，从减少计算量、降低对导航算法处理器运算能力的角度，提出了一种适用于室内的深度相机与MEMS IMU松组合导航算法。该算法使用MEMS IMU预积分得到的位姿变化量作为改进迭代最近点(ICP)的初值，而不是传统ICP中将初值设置为0，从而减少了迭代次数和计算时间；使用视觉导航得到的位置信息减去惯性导航得到的位置信息，得到二者的位置差，以此差值作为量测量，对姿态误差、速度误差、位置误差以及等效陀螺仪的常值漂移和加速度计的常值零偏进行估计，最后用估计值修正姿态误差等，从而得到更加准确的载体位姿信息，提升导航系统的定位精度和三维重构精度。

1 深度相机与MEMS IMU松组合导航算法设计

1.1 松组合导航算法工作流程

深度相机与MEMS IMU松组合导航算法工作流程如图1所示。工作时深度相机与MEMS IMU同时启动，深度相机对载体所处环境的彩色图像和深度点云进行采集，MEMS IMU对载体运动的角速度和加速度进行采集。

图1 深度相机与MEMS IMU松组合导航算法流程Fig.1 Flowchart of loose coupled navigation algorithm based on depth camera/MEMS IMU

深度相机采集到彩色图像和深度点云数据后，如果能够从前后两帧彩色图像之间检测并提取出大于等于12对特征点，则认为当前环境特征点丰富，仅用彩色图像进行位姿推算就能够保证导航系统的正常运行，所以为了减少计算量，这种情况下只用深度相机的彩色图像信息进行导航计算；对完成特征点提取的相邻两帧彩色图像使用随机一致性采样(RANSAC)算法剔除和优化特征点匹配结果，然后运用PnP算法计算出两帧之间载体在导航坐标系下的位姿变化量。

如果小于12对特征点，则认为载体当前处于特征点缺失的区域，此时单靠深度相机输出的彩色图像信息导航系统无法正常工作。为了保证系统的鲁棒性，使用深度相机输出的彩色图像和深度点云数据，采用基于MEMS IMU辅助的改进ICP算法计算载体位姿变化。首先对MEMS IMU采集到的角速度和加速度信息进行预积分，由于两帧之间的时间间隔只有0.1 s，所以MEMS IMU预积分得到的载体位姿变化量包含的误差较小。将MEMS IMU预积分的结果作为深度点云迭代中ICP算法的初值，从而在保证计算精度的情况下减少迭代次数和计算时间，得到载体的位姿变化量。

最后将视觉导航计算得到的载体运动信息与MEMS IMU预积分得到的载体运动信息作为量测量，送入深度相机与MEMS IMU组合算法，估计MEMS IMU解算的姿态误差、速度误差、位置误差、陀螺常值漂移和加速度计常值零偏。最后用估计得到的误差对计算结果进行修正，从而得到更加准确的载体位姿变化信息，进而完成对载体的导航与定位；同时彩色图像对深度点云中的点进行上色，然后运用两帧之间的位姿变化关系，对深度点云进行拼接，得到清晰平滑的三维重构模型。

1.2 基于MEMS IMU辅助的改进ICP算法

(1)

式中：R为旋转矢量；v为速度矢量；g为当地重力加速度；d为平移矢量。

MEMS IMU测量得到的角速度和加速度虽然本身含有常值漂移和零偏等误差，但通过积分得到的位姿信息在短时间内有较高的可信度，而Δt仅为0.1 s，所以可以通过积分计算在Δt内载体的旋转矩阵变化量ΔR与平移向量变化量Δd：

(2)

得到旋转矩阵变化量ΔR与平移向量变化量Δd后，将其代入ICP算法中待优化的目标函数：

(3)

式中：kbi为旋转后的点云集合；kri为旋转前的点云集合。改进后的ICP算法将IMU测量值作为初值代入ICP算法，使ICP算法的迭代初值不为0，而是从一个较为准确的旋转矩阵ΔR和平移向量Δd开始迭代，这样能够在不降低迭代精度的情况下，有效减少ICP算法的迭代次数。

1.3 深度相机与MEMS IMU组合算法

该算法中，视觉导航系统使用彩色图像或深度点云计算得到机器人的旋转矩阵和平移向量，使用MEMS IMU输出信息计算机器人的角增量和速度增量，然后通过预积分计算机器人的旋转矩阵和平移向量，将二者计算得到的位置作差运算，将差值作为EKF的量测量，进行误差估计，最后使用误差估计的结果反馈修正旋转矩阵和平移向量，从而得到更加精确的机器人位姿信息。

设置EKF滤波器的系统状态变量为

(4)

由常规捷联惯性导航系统的误差模型可以得到状态方程为

(5)

(6)

(7)

(8)

(9)

(10)

(11)

(12)

(13)

(14)

(15)

(16)

取量测值为捷联惯性导航系统计算得到的位置Pi与视觉计算得到的位置Pv之间的差值，所以量测方程为

Z=(Pi-Pv)=HX+V，

(17)

式中：

(18)

V=Vp，

(19)

Vp为零均值高斯白噪声[19-20]。

通过EKF估计系统的状态变量，从而得到姿态误差、速度误差、位置误差、等效陀螺的常值漂移和加速度计常值零偏，然后用估计得到的误差修正当前机器人的位姿信息，并用修正后的位姿信息完成航位推算与点云拼接。

2 算法实验验证

2.1 基于MEMS IMU辅助的改进ICP算法实验验证

实验在数学仿真软件MATLAB平台下进行，首先在网上下载10幅不同的深度点云图片，然后对它们进行旋转和平移，由于算法中时间间隔为0.1 s，为了保证实验的可靠性和真实性，对每一幅图片进行小幅度的旋转和平移，并记录每一幅图片的旋转量和平移量；然后参考MEMS IMU(如荷兰XSENS公司生产的的MTI 100-IMU)在0.1 s可能产生的位姿误差量，给上述记录的旋转量和平移量添加误差；最后实验组使用添加误差后的旋转和平移作为初值，对旋转前后的两幅深度点云进行计算，当迭代结果E(ΔR,Δd)，即重投影误差小于0.01时，结束并记录结束时的迭代次数，对照组不使用添加误差后的旋转和平移作为初值，直接将初值设置为0，对旋转前后的两幅深度点云进行计算，当迭代结果E(ΔR,Δd)小于0.01时，结束并记录结束时的迭代次数。实验结果如表1所示。

表1 两种算法各自的迭代次数

通过实验结果可以非常明显看出，当满足重投影误差小于0.01时，在对10幅深度点云的计算中，基于MEMS IMU辅助的改进ICP算法完成运算时的迭代次数，明显小于传统ICP算法完成运算时的迭代次数，说明改进的ICP算法是有效的。这是由于改进的ICP算法有了先验信息，即旋转和平移的加入，使得ICP算法在迭代初始时，更接近于真值，迭代次数才会明显下降。

2.2 松组合导航实验结果与分析

如图2所示，使用美国Microsoft公司产Kinect v1深度相机和 荷兰XSENS公司产MTI 100-IMU中的MEMS IMU作为传感器，在速度可以设定的遥控小车上搭建深度相机与MEMS IMU松组合导航实验平台。

图2 实验平台Fig.2 Experimental platform

载体以0.1 m/s的速度在学院大楼走廊运动，期望的载体运动轨迹为一个长4.7 m、宽1.0 m的矩形，如图3所示。

图3 期望的载体运动轨迹Fig.3 Expected trajectory of vehicle

实验分为两个部分：第1部分为室内导航实验，通过对比不同方法生成的载体轨迹，验证本文设计的图像选择逻辑，基于位置差值的深度相机与MEMS IMU松组合算法以及适用于室内小场景的回环检测方法的有效性；第2部分为室内三维重构实验，通过对比不同方法生成的走廊三维重构模型，验证本文设计的算法能够有效提高三维重构的精度和平滑度。

室内导航实验分为两组：第1组同时使用Kinect v1相机的彩色图像和MTI 100-IMU，基于目前认可度较高的VINS算法计算并绘制载体运动轨迹；第2组使用Kinect v1相机和MTI 100-IMU，基于本文设计的深度相机与MEMS IMU松组合导航算法计算并绘制载体运动轨迹。两种方法在使用相同的传感器前提下，计算载体的运动轨迹并比较定位误差的大小，验证本文设计的深度相机与MEMS IMU松组合导航算法的有效性。两组实验的载体运动轨迹结果，分别如图4和图5所示。

图4 基于VINS算法解算的载体运动轨迹Fig.4 Vehicle trajectory based on VINS

图5 基于本文松组合导航算法解算的载体运动轨迹Fig.5 Vehicle trajectory resolved by the proposed loose coupled integrated navigation algorithm

第1组基于VINS算法的实验中，使用Kinect v1相机输出的彩色图像和MTI 100-IMU组合，得到的航位推算轨迹没有发生混乱，但效果并不理想，最终得到的位置为(7.162 m,1.032 m)，与出发点(0 m,0 m)之间的距离误差为7.236 m，总里程为11.4 m，误差占总里程数的63.45%. 因回环检测未能正常工作，导航过程中的累积误差没有得到修正。可能是因为使用Kinect v1相机采集的彩色图像和MTI 100-IMU输出的角速度和加速度的精度达不到VINS算法的要求，所以最终计算得到的载体运动轨迹与真实轨迹相差较大，但由于VINS为视觉与MEMS IMU紧组合系统，所以绘制的轨迹比较平滑。

第2组本文设计的深度相机与MEMS IMU松组合导航系统较好地完成了导航任务。当载体运动到走廊墙壁上有展板部分时，特征点清晰，程序使用深度相机与MEMS IMU松组合导航算法计算小车的姿态和位移。当载体运动到白色墙壁部分时，视觉特征点丢失，程序切换为基于MEMS IMU辅助的改进ICP算法，保证了系统在视觉特征丢失时的鲁棒性，较好地解决了特征点缺失和重复场景导致的错误重定位问题，没有出现轨迹混乱，也没有出现较大的抖动与跳变。由于实验用的遥控车结构稳定性较弱，存在较大的抖动，所以轨迹存在轻微抖动。运动过程中使用航位推算法，因此存在累积误差。实验中遥控车回到出发点时，计算得到结束位置到出发点的距离误差为0.273 m，总里程为11.4 m，误差占总里程数的2.40%. 由轨迹图可以看出，回到出发点后，本文设计的基于导航先验信息的回环检测方法完成了重定位，修正了累积误差。

基于相同的实验场地和实验仪器，对两种算法分别进行了10次重复实验。载体回到初始位置时，记录航位推算的载体结束位置与初始位置之间的位置偏差，并计算位置偏差在总里程中的占比，结果如表2和表3所示。

从表2和表3中可以看出，VINS组合导航算法在轨迹平滑方面表现较好，但在计算载体旋转时很难正确估计姿态变化角度，且航位推算误差较大，位置误差大致占总里程的60.97%～81.07%之间。

表2 载体回到初始位置时的航位推算位置偏差

表3 位置偏差占总里程的百分比

本文设计的深度相机与MEMS IMU松组合导航算法有效地融合了深度相机和MEMS IMU的优点，位置误差大致占总里程的1.61%～3.14%之间。与VINS算法相比，在使用相同传感器的情况下，定位精度更高，但由于是松组合系统，所以轨迹平滑度不如VINS算法。

3 结论

本文从室内非轮式机器人对于非结构化环境感知与精确自身定位的需求出发，在分析视觉导航和惯性导航以及视觉与MEMS IMU组合导航不足的基础上，结合深度相机与MEMS IMU的优点，设计了一种深度相机与MEMS IMU松组合导航算法，具体如下：

1)针对传统ICP算法计算量大等问题，设计了一种基于MEMS IMU辅助的改进ICP算法。该算法中，通过MEMS IMU预积分为ICP算法提供迭代初值，从而减少迭代次数和计算时间，最后设计并进行了10组实验，验证了基于MEMS IMU辅助的改进ICP算法能够将迭代次数大约缩小至原来的一半。

2)针对单一传感器导航误差大，传统松组合只使用视觉信息作为量测，信息运用不充分的问题，提出了一种基于位置差值的深度相机与MEMS IMU松组合导航算法。该算法将视觉导航和惯性导航得到的载体位置差值作为量测量，对系统误差进行估计与修正，从而抑制误差增长速度，得到更加精确的导航信息，最后通过实验验证了基于位置差值的深度相机与MEMS IMU松组合导航算法的有效性。