基于视觉传感器的移动机器人定位算法*

张一博, 马 磊

(西南交通大学 电气工程学院，四川 成都 610031)

0 引 言

传统传感器定位导航过程中轮子打滑摩擦等现象的影响较为严重[1，2]，且全球定位系统(global positioning system,GPS)无法在室外使用，基于视觉传感器的里程计重要性就显得较为突出。根据所使用的传感器的不同，视觉里程计大致上又可以分为三类，其中双目立体视觉里程计由于精度较高，被广泛应用到移动机器人的定位中[3]，但其计算量太大，需要GPU或者现场可编程门阵列(field programmable gate array,FPGA)处理才能实时输出距离信息。单目传感器[4]由于不能直接计算出图像的距离，得到的只是一个相对的值。深度视觉传感器是2010年之后兴起的一种传感器，其可以通过红外结构光或者飞行时间(time-of-flight,TOF)原理，测出物体与传感器之间的距离。

2004年Nistér D等人[5,6]设计了一种实时的视觉里程计，同时还提出了2种基于特征点匹配的里程计实现流程，即单目视觉里程计和立体视觉里程计。另一种比较普遍的光流法最早由Silveira提出，随着一些LSD-SLAM[8]等开源项目的出现，光流法逐渐登上V-SLAM的主流舞台。

本文将惯性测量单元(inertial measurement unit,IMU)和GPS组合导航中的卡尔曼融合思想[9]运用到视觉里程计中，对光流法和特征点法进行融合，并提出局部地图快速匹配策略，经过开源数据集和实验室环境的测试，结果表明:该方法在保证鲁棒性同时，较一般的视觉里程计计算时间更短，定位精度更高，有效地降低了匹配跟踪失败率。

1 视觉里程计卡尔曼融合

1.1 基于光流法的运动估计

Lucas-Kanade算法是光流法的经典算法，该类算法的前提是同一个空间点对应的不同图像中像素点灰度值固定不变，在此前提的基础上，像素点的灰度可以写成I(x,y,t)。假设空间中有一固定点，t时刻对应在图像中的位置为(x,y)，由灰度不变前提可以得出

I(x+dx,y+dy,t+dt)=I(x,y,t)

(1)

对左边进行泰勒展开，保留一阶项，得

(2)

整理可得约束方程

(3)

图像在该点处灰度值梯度变化分别记为Ix和Iy，图像对时间的变化记为It，而图像的运动也可以记为u,v，则约束方程写成矩阵形式

(4)

若该点P在两帧图像中的非齐次坐标p1和p2，光流估计相机的主体思路是使得光度误差e=I1(p1)-I2(p2)尽可能最小，因此,BA优化的目标误差二范数函数为

(5)

为了求得优化变量也就是相机位姿ξ，使用李代数求解导数时的扰动模型思想，最终可以计算出误差项ei关于位姿ξ的雅可比矩阵

(6)

最后利用高斯牛顿法迭代计算出相机位姿ξ，进一步解算出移动机器人的旋转矩阵R和平移向量t，为后续的匹配法融合提供先验预测值。

1.2 基于特征点匹配的相机运动估计

特征点有关键点和描述子两部分组成，为了适当减少精确度和鲁棒性以提升计算速度，选择ORB特征[9]。ORB特征改进了FAST特征点检测中方向不确定问题，且具有快速描述子BRIEF[10]。

(7)

其中,误差项关于位姿的雅可比矩阵使用李代数扰动模型算出，最后在非线性优化中只要不断迭代，即可得出当目标函数(7)取得极小值时位姿ξ，从而解算出旋转矩阵R和平移向量t。

1.3 光流法与特征点法融合

光流法的特点是帧间计算速度快，定位频率高，但误差累积是不可避免的问题；特征点匹配法计算速度比光流法慢，但定位精度高。将特征点匹配位置估计对光流法的先验定位信息进行纠正，可以提升定位精度，同时在算法融合阶段建立局部地图，不仅能加强关键帧选取的严格性，还可以建立局部回环有效的降低误差累积，流程如图1所示。

图1 融合算法整体框架

根据卡尔曼滤波器预测方程

(8)

zk=Tkxk+vk

(9)

式中Tk为当前关键帧下视觉传感器的外参，zk为相机坐标系下点的坐标，vk为噪声

(10)

(11)

图2 关键帧选取与局部地图更新

2 实验结果和分析

采用现有的慕尼黑工业大学(TUM)提供的公开RBG-D数据集进行算法效果验证。TUM数据集可以提供精确运动轨迹当作移动机器人的真实轨迹位置信息，以便和传统视觉里程计算法以及融合算法进行比对，验证该算法的精确度。该数据集所使用的视觉传感器是深度相机Kinect 1，与本实验所使用的一致，因此得到的算法验证效果具有很高的参考价值。Kinect 1图像分辨率为640×480，深度图像和RGB图像采集频率都是20帧。将相邻帧之间通过光流直接法确定的特征点，根据这些特征点估计出移动机器人的运动，作为卡尔曼滤波的先验里程计信息，对关键帧之间的特征点进行匹配，为了避免特征点过多带来的观察不便，对特征点进行了剔除与筛选，将关键帧之间估计出的里程计信息与之前光流直接法解算出的先验信息进行融合，得到最终的移动机器人位置轨迹，如图3所示。

从图中可以看出:对于直线运动过程的估计，光流法可以得到相对不错的结果，但当移动机器人发生转弯时，光流法解算出的位置信息角度误差较大，机器人速度过快时也会造成一定的干扰，随着时间不断地推移误差累积也在增加，这时特征点匹配处理大尺度移动场景的优势就体现出来了，同时速度过快和转弯时建立的局部地图筛选关键帧机制减少了误差累积在定位精度上的干扰。

误差曲线是计算视觉里程计算法得到的移动机器人位置点和运动捕捉系统(真值)之间的欧氏距离，如图4所示。视觉里程计融合后算法误差要明显小于光流法的误差。

图4 不同视觉里程计算法的误差

真实环境下的实验器材为自主研发的移动机器人和Kinect 1视觉传感器，将文中改进的视觉里程计算法运用到实验室内场景中，观察实验效果。实验室环境下的结果如图5所示，上文中运用成熟的数据集已验证了视觉融合算法精确度，真实环境下主要测试该算法实时性能否得到保障。根据实验结果不难得出:在保证机器人定位精度的前提下融合算法可以满足实时性要求，融合算法得到最终的移动机器人轨迹平滑，比光流法定位在精度上有很大的提升，特别是在机器人转弯、速度过快等工作状况下具有良好的鲁棒性。

图5 室内场景下视觉里程计位置估计

3 结束语

基于视觉传感器的里程计算法研究在移动机器人定位中的地位逐步上升，该融合算法是将卡尔曼融合的思想运用在传统的光流法和特征点匹配算法的基础上，并提供了理论推导以及实现，提出了局部地图的建立以减少误差累积。在现有的数据集情况下经过测试满足了移动机器人定位精度要求，真实室内场景中实验则确保了实时性的要求，对于室外的情况，接下来会进行进一步的改进。