线特征融合光流的单目SLAM算法*

贾 哲，冷建伟

(天津理工大学电气电子工程学院，天津300384)

1 引言

随着人工智能时代的来临，对机器人的智能化要求也越来越高，目前同步定位与地图构建SLAM(Simultaneous Location And Mapping)问题已成为机器人领域研究的热点。SLAM问题可以被描述为在未知环境里，机器人从某初始位置开始移动，并且在这个移动过程中根据自身所携带的传感器采集到的数据进行自主定位，同时构建增量式地图［1］。该技术是机器人能够完全实现自主规划路径和独立移动的重要技术之一。目前在SLAM系统中，视觉传感器因其体积较其他传感器小，成本低廉，安装便捷，同时又能采集到机器人运动环境中丰富的场景信息而被广泛采用［2，3］。主流的视觉SLAM算法在制图和定位时以提取到的图像中的点作为特征，点特征能够描述出图像里的明显特征区域，常见的特征点描述算法有尺度不变特征变换SIFT(Scale Invariant Feature Transform)、加速稳健特征SURF(Speeded Up Robust Features)、快速特征点提取和描述ORB(Oriented FAST and Rotated BRIEF)等成熟跟踪算法。但是，基于点特征构建出的地图为三维点云地图，点云地图较为稀疏，而面向机器人导航的地图要求能够反映场景中物体的结构信息，以便后续机器人的路径规划研究。与点特征相比，线特征同样广泛存在于各类环境场景中，线特征不易受视点和光照变化的影响，而且由于直线的空间维度比点高一维，对于一些结构化的场景，线特征更具优势，更能准确地表达环境的结构信息。其中线特征的参数化方法是研究基于线特征 SLAM 算法的基础［4］。文献［5］采用Plücker参数化的方式，将直线以 Plücker坐标来表示，其优点是可以很方便地对线特征使用线性投影模型，但Plücker参数化不利于后端对SLAM的非线性优化。而将空间直线进行Cayley参数化的优势主要是用其表示的空间直线具有4个自由度，没有一致性约束，也不存在冗余维度［6］，因此这类参数化方式能够更好地适用于非线性优化算法。本文重点阐述了直线的参数化问题。

SLAM的求解是一个状态估计问题，其解决方法可大致分为两类:一类是基于滤波器的方法［7-10］，另一类则是基于图优化的方法。基于滤波器的方法对于非线性系统很难得到精确的最优滤波解，为了解决该问题，文献［11］提出了扩展滤波器。而图优化在机器视觉和机器人领域都有大量研究，对SLAM问题具有鲁棒性，比较容易实现。

除此之外，机器人的应用场合往往要求机器人能够即时定位，当机器人快速移动时，由于线特征的提取和跟踪算法处理速度相对较慢，会造成移动和定位不同步，单次定位处理的时间越长，前后两帧图像之间的重叠越少，无疑会降低机器人定位的精度。而光流法经过长时间的发展，在利用图像关系估计运动对象的位姿时，不仅可以满足一定的精度要求，而且也有良好的稳定度。更为重要的是，光流法计算速度块，基于光流法的视觉里程计可以达到实际应用对实时性的要求［12］，但是光流法精度不高。本文提出将光流法与线特征匹配相融合，一方面提高光流法的定位精度，另一方面提高算法的实时性。

2 直线的表征

本文在对空间中的三维直线进行表征时，结合了Plücker和 Cayley两种参数化方式。采用Plücker坐标有利于线特征的线性投影计算，而采用Cayley参数化方式便于在后端对线特征的位置进行非线性优化。

2．1 直线的 Plücker坐标

Plücker直线坐标也被称作线坐标，在处理线几何等问题时有独特的优点。

设空间直线 L 经过 A(x1，y1，z1)、B(x2，y2，z2)两点，那么直线L的方向向量为:

要确定L的位置，还需求出直线L相对于原点的线矩向量:

那么该直线的Plücker坐标为L=(n，v)。

直线的Plücker坐标具有如下性质:

(1)满足 Plücker约束v⊥n，即v·n=0。

2．2 Plücker 直线位置的初始化

由于使用单目摄像头存在尺度局限性，某一特征的完整位置信息很难从单一图像中采集到，比如无法准确地从单幅图片中计算出线特征的方向以及深度。所以，在SLAM问题中，单目SLAM是部分可观测问题。那么，在描述和检测线特征时，就涉及到特征位置的初始化，具体来说，特征位置初始化指的是特征的3D位置信息通过图像中的特征观测信息来获得。文献［5］在以Plücker方式表征直线的基础上提出了非延迟的初始化方法，该方法与延迟类的初始化方法相比，其优点是能够利用更多的特征信息，而且在面临低视差的情况时仍旧有效。本文即采用该方法进行直线位置的初始化。

2．3 直线的 Cayley表征方式

Cayley表达式定义为一个4维向量W=(d，s)，其中，d表示直线到坐标系原点的距离，s=表示该直线在空间中的旋转信息。

以下为将Plücker直线 L转换为 Cayley直线的过程:

根据式(1)计算出Plücker直线L上距离坐标原点最近点P0:

按照式(3)构建正交矩阵Q:

根据式(4)计算出一个由向量s构成的反对称矩阵:

至此，Cayley直线的坐标表达式W=(d，s)已经求出。

将Cayley直线转换为 Plücker直线的过程亦需要构造矩阵Q，可通过式(5)求出:

根据Q的定义，可得Plücker直线坐标为:

2．4 线特征的优化

常见的易造成图片质量下降的因素如图像压缩、图像模糊或者存在噪声等问题容易使匹配上的线特征和预测的线特征之间存在误差，对SLAM问题的结果产生影响，所以有必要进行优化。本文的线特征优化方法是采用文献［4］中的优化方法，线特征在优化更新过程中采用Cayley参数化方式。与点特征不同的是，线特征的误差模型不再是简单的特征投影预测值与测量值之间的差，在本文中，线特征的误差计算模型为:

其中，l={l1，l2，l3}T表示图像中线特征的预测值。(p1，p2)表示图像中线特征测量值。上式结果为一个二维向量e=(e1，e2)，其模就是误差值，直观理解如图1所示。

3 基于线特征的位姿估计

性变换前的直线L=(u，m)与线性变换后的L'=(u'，m')之间存在约束关系为:

其中，R是变换时的旋转矩阵，T是平移矩阵，∧表示叉乘操作。由于空间直线段的方向相比其他量更加精确［13］，所以把运动估计的问题分为以下两个子问题:

(1)计算旋转矩阵R。

首先构造关于R的最小化目标函数。假设检测到的直线集为{Li=(ui，mi)，i=1，2，…，N}，那么最小化目标函数。如式(6)所示:

其中，|q|=1为单位四元数，×表示四元数乘法操作。将式(7)中的每一项都乘以

其中，表达式u'i×q－q×ui是关于变量q的线性函数，进一步分析可知，存在矩阵:，得到:

使得u'i×q－q×ui=Aiq，其中珘u是向量u的反对称矩阵，所以，式(8)进一步变化为:

同一直线在两个图像帧之间满足线性变换，线

(2)计算平移矩阵T。

构造关于T的最小化目标函数，如式(10)所示:

将式(10)对T求偏导数，并令其等于零，得到:

4 图优化

针对传统的利用滤波方法估计位姿存在线性化误差、更新效率低下等问题，本文依据SLAM研究中的图优化思想，设计了一种基于非线性优化的位姿优化方法，同时实现局部位姿优化和全局位姿优化。

4．1 模型的建立

在机器人视觉估计位姿领域，经典的SLAM数学模型由三部分组成，分别是状态变量、运动方程和观测方程［15］。本文的应用对象机器人在感知环境时，主要以单目视觉传感器，即单目摄像机作为输入，因此本文的算法构建出的目标函数只借助状态变量和观测方程，目标函数为:

其中，C代表样本最优一致集，xk代表第k时刻机器人的移动位姿，在图优化里被称作一个pose节点;yj代表第k时刻能被机器人视觉传感器观测到的路标，在图优化里被称作一个 point节点;zk，j则代表pose节点与 point节点的约束。需要指出的是，当优化过程是闭环检测时，zk，j代表 pose节点与 pose 节点的约束。e(xk，yj，zk，j)，则是向量误差函数，反映了两个节点满足约束条件的程度，在图优化里被表示为两个节点之间的连线，也被称作边。

最终所求的 e(xk，yj，zk，j) 的二次型(式(14))，实质上是在求解最大似然，即在假设噪声是高斯分布之后，将整个SLAM问题等效为一个最小二乘问题［16，17］。可借助通用图优化工具 g2o来解决上述非线性最小二乘问题。

4．2 图的建立

上述 SLAM问题全部抽象成节点与边，节点代表优化变量，包括 pose节点和 point节点，边代表误差项约束条件，包括 pose-point约束和posepose约束。pose-point约束依赖于相机观测产生的约束，如图2a所示。pose-pose约束则依赖于系统中闭环检测产生的约束，如图2b和图2c所示，可有效地抑制系统的累积误差，从而得到信息一致的地图。它的值越小，表示两个节点满足匹配约束的程度越高，值为0时，表示两个节点完全满足匹配约束。

目标函数的物理意义:这里假设了误差函数符合高斯分布(0，Ω)，则SLAM问题的贝叶斯概率描述为，求解适合的(x，y)，使系统最有可能产生当前的观测数据Z，即最大似然估计为:

5 融合算法

由上述分析可知，对于图像中线特征描述、匹配和优化需要一定的过程，这就导致移动机器人的定位难以获得实时性。为了不影响机器人的移动速度，引入光流法。光流法因其计算速度快，所以定位频率高，对帧间发生的小位移有较高的精度，十分适合机器人的实时定位［18］。但是，单纯使用光流法，容易造成累积误差随机器人的位移增大而增大。所以，本文提出了一种特征法与光流法的融合算法，利用光流法与本文所述的线特征法二者之间的互补性，兼顾系统的准确性和实时性。Lucas-Kanade算法是经典的光流求解算法，本文采用此算法求解光流。

5．1 基于光流计算的运动估计

基于光流的运动估计首先要通过光流法确定图像序列中前后两帧之间的对应关系，然后利用视觉理论中的对极几何约束来求出采集当前帧时机器人的位姿。值得说明的是，在利用光流求解时，为了突出光流法的鲁棒性和实时性，不同于利用线特征估计位姿的求解方法，而是根据光流法跟踪前后像素强度差值的均方差的大小自动切换求解位姿模型。均方差的值越大，说明跟踪前后的视差越大，则利用本质矩阵模型求解;均方差的值越小，说明跟踪前后的视差越小，则采用单应矩阵模型求解。

5．2 融合算法

具体融合算法流程图如图3所示。

令式(15)中xk－1取为光流法计算出来的前10帧累积的机器人位姿;观测数据z是通过线特征点匹配法计算出来的机器人的位姿。通过求解优化目标函数融合两种方法得到的数据。由于光流难免会受到光照变化的影响，所以计算出的结果会导致精度有所偏差。同时在摄像头拍摄地面的情况下，地面上线特征较少，误配的比例较高。将光流和特征结果按照上述流程图优化融合的好处就在于减小估计均方误差，提高精度和实时性。

6 实验

6．1 点特征和线特征性能比较实验

为验证采用线特征方式相比于点特征是否能够有更好的匹配跟踪的效果和反映场景的结构信息，采用普通RGB相机采集图4所示场景的一组图像序列，图片尺寸大小为744×992，分别用点特征和线特征构造三维地图，构建结果如图5所示，并在此基础上比较了点特征和线特征的校正特性。从图5中可以看出，采用点特征构建出的场景比较稀疏，而线特征则较好地反映了场景的结构信息。

点特征线特征校正性能比较实验:

首先随意选取图像序列里的任一图像帧作为实验对象，设定像素真实位置，然后利用点、线特征校正估计出像素位置，得出与真实位置之间的误差，实验结果如表1所示。

Table 1 Comparison experiment of feature alignment for point-line feature表1 点特征和线特征校正特性比较

由表1可知，在快速性方面，线特征比点特征用时少;在精准度方面，线特征的误差也比点特征的误差低。其原因主要是，采用线特征的方式可以利用线的梯度信息，这样不仅能提高极线搜索的速度，而且也使结果更加准确。而采用点特征在匹配过程中容易出现由于非线性相似度函数多极点而产生误差的现象。

6．2 快速性比较实验

为了验证光流法的引入能够提高机器人定位的实时性，特设立了该项实验。该实验以实验室搭建的移动机器人作为平台，分别运行未引入光流法和引入光流法的算法，并且在室外场景中行驶同一段路径。图6是所得到的实验结果对比图，其中，横轴DL代表机器人的移动距离，纵轴t是算法的处理时间。由图6可知，未引入光流法时，基于线特征的匹配算法平均处理时间为315．69 ms;而引入光流法后，融合算法的平均处理时间为69．38 ms。这是因为在融合光流法的SLAM算法中，耗时较多的线特征提取和匹配以及优化算法只处理每隔10帧的图像信息，每2帧之间用光流法计算，所以融合算法提高了机器人SLAM问题的实时性。

6．3 算法整体性能比较实验

为了验证本文提出的基于图优化的线特征融合光流SLAM算法的准确性，本实验采用Tum数据集中的多组数据来进行评估。图7分别表示“desk”“room”和“floor”数据经过本文算法得到的轨迹和Tum自带的真实轨迹对比效果图。

图7中实线代表摄像机在采集数据时的真实运动轨迹，虚线代表本算法得到的轨迹。同时，本文算法也与其他算法做对比，表2记录了本文算法和目前其他主流算法在不同数据集下，各自计算出的摄像机的三维运动轨迹和真实轨迹之间的均方根误差。通过和ORB-SLAM、RGBD-SLAM算法均方根误差的对比，可评估本文算法对机器人位姿跟踪的性能。从图7和表2中可以看出，本文算法能够有效实现机器人的运动轨迹跟踪。

Table 2 Comparison of algorithm performance表2 算法性能评估 m

7 结束语

本文针对机器人SLAM问题的准确性和实时性提出了线特征融合光流的单目SLAM算法。对于线特征的表征问题，本文算法将Plücker坐标和Cayley参数化方式相结合，在便于计算线性投影的同时，也有利于后端的非线性优化，使图优化能够运用到基于线特征的机器人位姿估计问题上。将光流法与线特征法相融合，有效避免了线特征处理时间长、光流法精度不高的缺点，突出了光流法的实时性和线特征的准确性。实验表明，本文算法的定位精度比较高，具有一定的准确性和鲁棒性。但是，如何避免生成的三维线段模型中存在过多的杂线是今后需要深入研究的内容。