一种基于深度特征的室外环境下激光地图辅助视觉定位方法

2020-06-13李海标田春月

科学技术与工程 2020年13期

李海标, 时君, 田春月

(桂林电子科技大学机电工程学院，桂林 541000)

自主定位是机器人领域中的一个重要技术，也是实现自主导航的重要前提。移动设备通过自身安装的传感器，获取相关的环境信息，然后通过对数据的处理，最后推算出自身的位置。一般情况下，室外的移动设备依靠GPS就可以进行定位，GPS可以提供无漂移的全局定位。然而，GPS定位经常会受到由多径效应引起的间歇性误差的影响，比如在城市、峡谷和室内环境下，无法很好发提供定位。

激光雷达可以提供准确的3D矢量信息，因此可以使用扫描匹配技术迭代最近点(iterated closest point,ICP)[1]在给定地图和当前扫描之间进行直接匹配，目前在室外环境下，自动驾驶车辆也多采用激光雷达进行地图的创建与定位。由于成本和硬件要求，考虑到经济效益，大范围的使用激光雷达并不是最好的选择。所以研究人员更倾向于利用廉价的相机来实现位姿的估计。

利用相机和激光数据融合进行定位存在一定困难，主要是因为相机和激光雷达数据是两种不同形式的数据，实现不同传感器数据的自动标定和融合也是解决同时定位与地图创建多传感器融合的关键。Frossard等[2]提出了通过端到端的方式，利用卷积神经网络将视觉和激光雷达数据直接生成3D轨迹。浙江大学团队利用基于激光地图的几何信息，提出一种混合BA(bundle adjustment)框架，该方法可以估计相机相对于激光地图的位姿，同时优化了视觉惯导里程计中的状态变量，为了更准确地进行交叉模式数据关联，使用了多会话激光和视觉数据来优化激光地图以提取用于视觉定位的显著且稳定的子集[3]。

与上述方法不同的是本文算法不关注全局的尺度定位，采用了一种在给定的激光地图中进行视觉定位的方法，系统结构如图1所示。定位系统由四个模块组成，预处理过程是对从地图和图像流中获取的原始数据进行处理，用于之后的跟踪和定位模块。深度图是利用立体视差产生。在局部地图提取中，将从全局激光地图中提取与视觉深度匹配的局部3D地图。系统开始时，需要在给定的激光地图上假定相机的初始位姿，为了确定初始假定相机位姿的合理性，在定位之前需要进行视觉跟踪，该模块对连续图像帧之间的相对姿态进行估计，然后利用跟踪过程中的局部图、深度图和假定位姿，对6自由度相机姿态进行估计。

图1 系统结构

1 基于激光地图的视觉定位

1.1 符号定义

首先定义二维点和三维点的归一化坐标如下：

(1)

相机姿态由SE(3)来表示：

(2)

式(2)中：R表示旋转；t表示平移；SE表示李群。

T与SE(3)上的指数映射有关：

(3)

(4)

式中，把平移记为ρ，把旋转记为φ，se表示李代数。位姿更新表示为

T←T(ξ)T

(5)

所有增量均采用左乘的方式[4]。

1.2 局部地图的提取

在全局的点云地图中，对相机可观察到的局部区域进行提取，将提取的局部区域定义为局部地图，使用基于八叉树[5]的半径内近邻搜索实现局部地图的提取。

八叉树是一种基于树的数据结构，通常用于处理三维点云数据。八叉树中的每个节点都有八个子节点，代表八个子多维数据集，所以利用八叉树可以快速实现空间划分和搜索。

1.3 生成深度地图

本系统实现跟踪和定位任务都是依据深度图来完成。首先，利用OpenCV[6]中的SGMB算法[7]获得视差图。SGMB是一种立体匹配算法，通过最小化基于互信息构成的能量函数来估计差异，利用该方法可以获得高密度的立体视差图。通过选取每个像素点的差值，组成一个差值图，设置一个和差值图相关的全局能量函数，使这个能量函数最小化，以达到求解每个像素最优差异值的目的，能量函数定义如下：

(6)

式(6)中：p、q为图像中的像素；Np是像素p的相邻像素点；C(p,Dp)是当像素点为Dp时，该像素点的损失函数；P1、P2为惩罚系数；I(·)是一个判断函数。

最后，采用文献[8-9]中的方法估计场景深度，通过重新统计三角测量特征分布，并且对级数进行展开，消除三角测量中与距离相关的统计偏差。

1.4 视觉跟踪

视觉跟踪可以为定位提供初始值。当系统执行定位任务时，由于基于深度的定位系统不依赖于特定的跟踪算法，可按照特定要求选择合适的跟踪算法。现采用一种基于高斯-牛顿光度误差最小化的视觉跟踪算法来实现定位[10]。首先利用相机位姿估计的初始值，根据像素的梯度，寻找到两个对应像素点的位置，通过优化点之间的光度误差，最终求解出相机位姿。

(7)

In[Xi]

(8)

式(8)中Xi=[ui,vi,1]T，它是一个三维向量，表示在齐次坐标系下的图像中第i个像素的坐标。第i个像素的深度表示为di。三维空间中的点的坐标P=[x,y,z,1]T，通过投影函数π(·)将三维空间中的点投影到图像平面上：

X=π(P)

(9)

π-1(·)表示图像投影函数的倒数。当前帧的图像强度为In，上一帧图像强度为In-1，I[X]表示在图像点X处的强度。

(10)

其中的增量ξ是来自方程:

JTΩJξ=-JTΩJr(0)

(11)

式(11)中：J由Ji组成，Ji是rI,j的雅可比矩阵;信息矩阵Ω的对角由误差项方差的倒数组成;此函数的求解是一个非线性优化问题，利用g2o[11]库求解上述问题，它是一种强大的图优化框架。

1.5 定位

视觉定位通过匹配从双目像机获得的地图点和当前激光深度地图来实现。地图点的深度与相应相机深度之间差定义为深度差rD，具体定义如下：

(12)

优化的执行方式与跟踪模块中的执行方式类似。深度残差的雅可比矩阵为

(13)

式(13)中相机投影函数的导数定义如下：

(14)

图2 场景中三平面

然而，在过于复杂的环境中，不同物体的交界会出现深度值的突变，三维边缘产生的深度突变，会影响位姿估计的精度。这是因为根据双目视觉深度估计出的边的位置有时会产生偏差，由于边缘引起的深度梯度的变化，使得位姿也会随之更新，因此会产生突变。为了使系统尽可能不受边缘影响，将深度残差的方差设置为与深度梯度的大小成正比，最终物体边缘对系统精度的影响将会减小。

2 实验与分析

为了验证算法的精度与可靠性，首先使用了KITTI[13]数据集验证本文算法，其次使用LG公司的自动驾驶仿真软件LGSVL测试本文定位算法，最后为了检测本算法在真实环境中的可行性，利用数据采集车进行验证。

实验分为两部分，测试单独的定位模块和测试完整的SLAM框架。首先通过使用KITTI数据集测试验证视觉定位器模块。将视觉定位精度与KITTI提供的数据进行比较，以进行定量评估。在仿真软件和真实场景中，将定位模块导入了ORB-SLAM[14]框架中，ORB-SLAM中的跟踪模块提供的值作为定位器的初始值。

2.1 KITTI数据集测试

在数据集序列00～10上评估系统的定位算法，由于01序列的场景是高速路上，此场景包含的特征有限，所以文中的算法在这个序列中失败了。在其他序列数据中均取得良好的效果。

数据集测试路程选择在100里内。图3(a)～图3(c)显示了序列中的平移和旋转误差。平移误差始终小于1.0 m，旋转误差小于5°，平均平移误差为0.13 m，平均波动幅度0.1 m。平均转动误差为0.4°，平均波动幅度0.4°。分析误差图时，发现在十字路口存在较大的误差，当汽车转弯时，相机旋转，而邻近的建筑物和停放的汽车忽然缺失，因此推断，所提出的算法不适用于快速旋转运动和无邻近结构的情况。然而，该方法的性能与其他方法并无明显差别。序列00的结果显示，平均平移和旋转误差低于文献[15]中提出的结果,轨迹图如图3(d)所示，本算法的估计值和数据集提供的真值基本吻合。

图3 数据集仿真

表1总结了其他序列的定位误差，每个序列中的定位误差以平均值±标准偏差格式给出。平均平移误差小于0.5 m，平均旋转误差小于1.0°。算法在序列00上的性能最好，而在序列04上的性能最差，场景中结构的缺失可能是导致算法性能下降的主要原因。

表1 定位误差(平均值±标准差)

2.2 LGSVL仿真

在利用KITTI数据集测试时，评估定位的精度达到了亚米级。然而，在KITTI数据集大多数场景都是从居民区或高速公路上拍摄的，场景比较单一。为了在更具挑战性的场景中进一步评估本算法，利用LGSVL仿真系统和SLAM框架，在结构变化大、道路宽、环境随时间变化以及动态物体等场景中进行评估。系统在线运行基于深度的定位模块，当深度残差较大时，会选择地进行从双目相机和地图的重建点云之间执行局部ICP。

利用仿真软件，使汽车行驶一定的距离，将相机的运动轨迹与地图提供的真值进行对比。在汽车行驶过程中，当系统出现问题时，如无法进行定位，将会重启系统，图4(a)中C、D、E处表示重启，而结构丰富的A、B处，系统运行良好。图5是行驶过程中的部分地图。

图4 系统仿真轨迹

图5 局部地图

2.3 真实环境测试

实验车的传感器系统配备了3D激光雷达，视觉传感器等，如图6所示。首先利用激光雷达获得点云地图，在此基础上重建三维激光雷达图像。为了进行视觉定位，先利用MATLAB相机校准应用程序对相机的内外参数进行了估计，其次，为了确定相机在传感器系统中的相对姿态，在相机和三维激光雷达之间也进行了标定。最后为了完成定位，先将相机强度图像与通过车辆运动重建的局部地图的强度图像进行比较。通过优化相机与激光雷达深度图像之间的误差，最终实现定位。图7为激光地图，图8为实验车的轨迹图，其中细绿色线为地图提供的真值，粗绿色线为相机的运动轨迹。图9为实验的定位误差。平均平移误差为5 m，波动幅度为6.8 m。平均旋转误差为5°，波动幅度3.5°。此次数据采集的行驶路程也控制在50 km内。

通过对实验结果分析发现，在无结构区域定位极易失效。在道路宽阔的地带，由于附近的结构信息稀缺，定位系统的精度将极大降低，每当检测到定位失效时，必须要重新启动系统。而在结构丰富区域，系统运行良好。经过多次采集数据并进一步进行分析，在结构丰富的区域，稠密或稀疏的激光点云地图对定位系统精度的影响并不大，而在无丰富结构的区域，稀疏的激光点云地图无法很好的辅助视觉定位，运行时，系统频繁的重启。