惯导卫星组合导航辅助的视觉导盲仪定位建图

魏 彤，李 新

(北京航空航天大学仪器科学与光电工程学院，北京 100191)

0 引言

视觉的缺失对盲人的出行造成巨大困扰[1-2]。对于盲人出行困难的问题，穿戴式视觉导盲仪具有使用方便、成本较低等优势。而视觉即时定位与建图(visual simultaneous localization and mapping，VSLAM)算法被认为是实现移动机器人真正自主的关键[3-4]，能够提供位姿和环境地图，已成为研制视觉导盲仪的关键技术。目前，VSLAM已经能够实现小范围内的定位与建图[5-6]，但在大范围场景中，还存在鲁棒性差和误差累计的问题。

有研究者提出采用视觉与惯性器件组合来提高VSLAM的鲁棒性[7-9]，通过融合不包含累积误差的测量数据来消除视VSLAM的累积误差[10-12]。Daniel提出的IMU/GPS/SLAM紧耦合算法[13]，得到的定位结果精度较高，但GPS需要固定的参考基站，且算法结构复杂。Jonghyuk提出的SLAM辅助IMU/GPS组合导航系统有效降低了算法复杂度[14]，但该定位结果没有融合视觉数据，精度较低。

1 算法结构

针对目前IMU/GPS/VSLAM融合算法存在的问题以及视觉导盲仪的应用需求，本文提出了一种利用IMU/GPS组合导航位姿数据辅助的视觉导盲仪定位与建图算法，改进了ORB-SLAM的运动模型。另外，在定位和建图的优化过程中，都融合了IMU/GPS组合定位数据观测量，以提升定位精度和建图效果，在降低了算法复杂度的同时，保证了定位与建图精度。

双目ORB-SLAM主要包含三个线程：跟踪、局部建图和回环检测[6]。跟踪线程通过匹配当前帧的特征点和局部地图中的路标点，并最小化投影误差来定位当前帧的相机位姿；局部建图线程管理局部地图，并通过局部捆集调整来优化地图；回环检测线程检测是否形成回环，并通过位姿图优化来抑制累积误差。本文将IMU/GPS组合定位数据融合进跟踪线程和局部建图线程，去除了双目ORB-SLAM的回环检测线程。IMU/GPS组合导航辅助的视觉导盲仪定位与建图如图1所示。

图1 视觉导盲仪定位与建图

在跟踪线程中，IMU/GPS组合系统输出定位数据，根据定位数据预测当前帧的相机位姿并进行特征点匹配，提高跟踪鲁棒性；同时，将位姿数据加入跟踪局部地图时的非线性优化。通过最小化路标点投影误差和位姿误差，优化当前帧的相机位姿。在局部建图线程中，路标点和关键帧特征点构成视觉误差量，关键帧之间的位姿变化量构成位姿误差量，局部捆集调整通过最小化视觉误差量和位姿误差量对局部地图进行优化。

在跟踪线程和局部建图线程中，IMU/GPS组合定位数据都起到了抑制累积误差的作用，最终能够在室外大范围环境中稳定地进行定位与建图。

2 状态量与观测量

本质上，基于IMU/GPS组合系统辅助的VSLAM算法的核心是状态优化问题，即优化相机的状态量。为了求解该状态优化问题，需要明确其状态变量和观测量，进而构建优化模型来进行求解。在ORB-SLAM基础上加入IMU/GPS组合数据之后，对状态量和观测量也进行了重新定义。

(1)

观测量表示如下：

Zi=[Ci,Ii]=[{Cik},Ii]

(2)

3 跟踪线程运动模型与位姿优化

考虑到算法实时性与输出结果的连续性，本文提出的同时定位与建图算法采用了ORB-SLAM的多线程结构，主要包括跟踪线程和局部建图线程。

3.1 跟踪线程运动模型

跟踪线程同时接收图像数据和IMU/GPS组合数据，对当前帧的相机位姿进行估计。由于算法采用非线性优化方法估计位姿，因此需要给出当前帧位姿的初步估计值。

跟踪线程运动模型如图2所示。

图2 跟踪线程运动模型

图2中：上角标j为前一帧图像；上角标b为当前帧图像；下角标e为IMU/GPS组合器件固连坐标系；下角标c为地理坐标系；下角标c为相机坐标系；下角标w为初始相机坐标系。

(3)

式中：Rcb为相机与IMU/GPS组合器件的安装矩阵；Rew为初始矩阵。

3.2 跟踪线程位姿优化

一旦模型预测出当前帧相机位姿，就能将前一帧检测到的地图点投影到当前帧中，并在投影点附近根据描述子距离选取最优匹配。然后通过最小化相机观测误差及组合器件观测误差来优化当前帧位姿。

具体优化过程根据地图是否在局部建图线程中被更新而有所区别。

跟踪线程位姿优化如图3所示。

图3 跟踪线程位姿优化图

当地图更新时，即前一帧为关键帧，则认为前一帧的位姿数据是可靠的，因此只优化当前帧位姿。此时待优化的状态变量和优化变量为：

(4)

(5)

式中：EC为视觉观测误差(反投影误差)；E1为IMU/GPS组合观测误差。

对于某一特征点k，其对应的视觉观测误差EC定义为：

(6)

(7)

“玩阴术”表面隐晦莫测，其实不堪一击，乃典型的“见光死”。无论是政府机构还是各行各业乃至民间交往，只要一切事务均在法规之下公然运作，堵塞“暗箱操作”“私相授受”漏洞，履行各类监督机制，建立各类诚信档案，使违法违规者付出相应代价——“玩阴术”自然会失去滋生的土壤和疯长的空间。

IMU/GPS组合观测误差E1定义为：

(8)

∑1为组合观测数据的信息矩阵，从IMU/GPS组合模块中读取。

记IMU/GPS组合观测数据为ΔR和Δt，那么，观测数据的相应残差可表示为：

(9)

(10)

当局部地图未更新时，即前一帧为普通帧，则认为前一帧的位姿同样需要再进行优化。此时，需要同时对当前帧和前一帧进行优化，当前帧的组合观测误差和雅克比矩阵与局部地图更新时相同，误差方程对前一帧状态量的雅克比矩阵Ji(e1)。

最后，利用(levenberg marquardt,LM)算法对该非线性最小二乘问题进行求解，使得初始值不断向最优估计收敛。

4 局部地图优化

局部建图线程实现对局部地图内地图点坐标和关键帧位姿的优化。当插入关键帧时，利用局部捆集调整算法，对局部窗口内的N个关键帧的位姿以及关键帧所观测到的地图点坐标进行优化。N个关键帧由共视关系图确定。在局部窗口外但又能观测到局部地图点的关键帧也将参与到优化过程中，但这些关键帧的状态量固定，不再进行优化。

图4 局部建图线程优化图

在非线性优化时，状态变量为：

(11)

优化方程为：

(12)

式中：i为局部关键帧;j为固定关键帧;k为局部地图点;Ec(k,i)为局部关键帧与局部地图点的相机观测误差，定义与跟踪线程中的Ec相同，对关键帧位姿和地图点坐标同时进行优化;Ec(k,j)为固定关键帧与局部地图点的相机观测误差，定义与跟踪线程中的Ec相同，但不对关键帧位姿进行优化，只优化地图点坐标；EI(i,i+1)为相邻两个局部关键帧之间的IMU/GPS组合观测误差，定义与跟踪线程中未更新关键帧时的EI相同，同时对相邻两个关键帧的位姿进行优化。

5 试验验证

在盲人经常行走的人行道环境下，将本文算法的定位性能和参考导航系统的定位性能进行比较，同时对建图效果进行比较分析，验证本算法的可行性。

5.1 试验设备

利用IMU/GPS组合导航模块、便携式计算机和双目相机，搭建了一套视觉导盲仪。其中，USB摄像头为广角摄像头，最高帧率为30帧/s，采集图像像素为640×480；便携式计算机搭载Intel i7 6700HQ(2.6 GHz)处理器，8 GB内存。

5.2 定位精度分析

在校园中选取一条25 m×30 m的矩形封闭路线，选取矩形的一个角点作为起点，沿矩形边行走，最后回到起点。试验过程中，记录VSLAM算法、IMU/GPS组合导航算法和本文提出的IMU/GPS辅助VSLAM算法的实时定位数据，以及各算法输出的的建图数据。

试验结束后，用Matlab绘制各算法输出的定位数据，并进行分析。平移误差对比如表1所示。

表1 平移误差对比

表1中，X、Y方向误差为整个试验中定位结果与真实定位的平均误差，相对误差为定位误差与运动路程的比值。

各算法的轨迹对比如图5所示。

图5 各算法的轨迹对比图

从图5可以看出，VSLAM算法在每一个拐角处都会产生一定误差；另外，由于VSLAM算法逐帧估计位姿导致位姿误差不断累积，平移误差达到2.61%。IMU/GPS组合导航算法有GPS数据来抑制IMU数据的累积误差，定位结果相对VSLAM精度更高，但是，GPS同时也容易受到路边建筑、树木等高大物体的影响，使得相应环境下的定位结果产生偏移误差。试验中的平移误差为1.59%。

而本文提出的IMU/GPS辅助的VSLAM算法中，IMU/GPS组合导航的定位结果可以抑制VSLAM的累积误差，使得大范围环境下的定位误差不发散。同时，从轨迹图中IMU/GPS定位误差较大的区域可以看出，VSLAM利用视觉数据能够抑制GPS的偏移误差对定位结果的影响，提高IMU/GPS组合导航的定位精度。最终试验过程中的平移误差仅有0.43%。

以上试验结果证明，本文算法能够利用IMU/GPS组合导航的绝对定位信息抑制VSLAM的累积误差，同时利用VSLAM抑制GPS受环境影响产生的偏移误差，最终使得IMU/GPS辅助的VSLAM算法在整个试验过程中的定位精度优于0.5 m。

5.3 建图效果分析

鉴于实际场景中，各物体的位置等数据难以获取真值，建图效果只对ORB-SLAM算法和本文提出的IMU/GPS辅助VSLAM算法进行定性的分析比较。

通过对比两种算法的三维点云图中地面上地砖线的建图效果和相机轨迹，可以对两种算法的建图效果进行定性的比较分析。点云图效果对比如图6所示。

图6 点云图效果对比图

双目ORB-SLAM算法所建的点云图中，相机轨迹存在漂移和中断，且终点没能回到起点，只有一部分的地砖线比较明显。而IMU/GPS辅助的VSLAM算法所建的点云图中，相机轨迹更加平滑，终点与起点成功重合，地砖线清晰可见。以上试验结果证明，本文提出的IMU/GPS辅助的VSLAM算法建图效果显著优于双目ORB-SLAM算法。

6 结束语

本文针对穿戴式视觉导盲仪对定位与建图精度的要求，提出了一种基于IMU/GPS组合导航辅助的VSLAM算法。该算法在ORB-SLAM的基础上，融合了IMU/GPS组合导航系统的位置与姿态数据。IMU/GPS组合导航数据能够抑制VSLAM的累积误差，摆脱了对回环检测的依赖，并且提高了VSLAM的建图精度。同时，VSLAM能够有效抑制全球定位系统(global position system,GPS)的定位偏移误差。试验结果显示，本文提出的融合算法在人行道环境下定位精度达到0.47 m，基本实现了在室外大环境下能够实时稳定运行的定位与建图系统，并在穿戴式视觉导盲仪研制项目中成功得到应用。

在本文提出的IMU/GPS辅助VSLAM算法中，VSLAM算法对由环境引起的GPS定位结果偏移误差有抑制作用。但是，当GPS受环境影响偏移误差较大时，仍然会使得最终的定位结果产生较大误差，即该算法对于环境的鲁棒性还有待提高。