沙质环境中基于视觉的月球车定位方法

李 琳 辉， 赵 一 兵， 郭 烈， 张 明 恒

（1.大连理工大学 工业装备结构分析国家重点实验室，辽宁 大连 116024；2.大连理工大学 运载工程与力学学部 汽车工程学院，辽宁 大连 116024）

0 引 言

视觉里程计是一种基于单目视觉或双目视觉传感器，通过跟踪图像中的特征点来进行车体运动估计的定位装置[1].目前，除了应用于无人地面车辆[2、3]（unmanned ground vehicle，UGV）外，视觉里程计最为成功的应用是在美国2004年着陆火星的勇气号和机遇号火星巡视器（Mars exploration rovers，MERs）[4]上.与传统的相对定位传感器（如车轮里程计、惯导传感器）相比，视觉里程计的定位精度仅依靠视觉输入信息，不受滑转、倾斜、下陷等因素的影响，在松软环境下具有较高的相对定位精度和巡视里程统计结果.此外，通过车轮里程计和惯导信息的结合，能够对滑转率进行估计，可以为导航控制提供状态反馈量，提高巡视器的行驶安全性和通过性[5].

实现视觉里程计的基本框架是特征点提取→特征点匹配与跟踪→运动估计，目前比较常用的特征是 Harris角点[3～6]和SIFT 算子[7、8].前者能够提高算法的实时性；后者能够适应较大的视角差异，但需要较长的算法时间，不适合在计算能力有限的月球车上使用.特征点的匹配和跟踪包括左、右图像特征点的匹配和前、后连续帧图像特征点的跟踪两个部分，一般引入多种约束条件从减少误匹配的角度来提高运动估计的精度；在运动估计部分，为了提高求解精度，常用的运动求解算法有最小二乘、随机抽样一致（RANSAC）[3]和最大似然估计[4、5]方法等.

虽然视觉里程计的理论已基本完善，但针对外星探测而言，仍存在许多问题需要解决.例如，在沙质环境中难以找到足够的特征点进行跟踪，特征点位置变化影响特征跟踪稳定性，需要提高里程计对不收敛问题的鲁棒性[4]等.

本文针对上述问题开展月球车用视觉里程计的开发，首先研究快速的图像增强算法，保证提取足量的特征点；其次通过预估搜索方向和搜索窗口的方法提高跟踪匹配的效率；同时，采用RANSAC方法求解基础矩阵并进行运动估计，提高里程计对不收敛问题的鲁棒性；最后在特征点缺乏的月面模拟试验环境中进行试验，以验证所提方法的有效性.

1 特征点提取与增强

视觉里程计作为一种依赖特征点跟踪进行运动估计的相对定位方法，特征点提取的结果影响运动估计的稳定性和精度.从月面的有关研究资料中可知，月壤的粒度与淤沙相似，如果摄像机视野中没有月岩、车辙等特征明显的目标，则容易产生特征点稀少并且不够强壮的问题，将难以实现特征点的跟踪，或者降低运动估计的精度.所以，需要选择合适的特征点提取方法和图像预处理方法以提高特征点提取的数量和质量，并且使特征点的分布尽量均匀，以提高运动估计的精度.

1.1 Harris角点检测

Harris角点具有快速、稳定，对光照条件、噪声等情况不敏感，能定量提取，且所得特征点分布合理的特点，在MERs视觉里程计应用中取得了良好的效果.Harris算子的卷积表达式如下：式中：gx为图像坐标系x方向的梯度，gy为y方向的梯度；G（槇s）为高斯模板，为卷积操作；E为每像素点的能量函数；det为矩阵的行列式；trace为矩阵的迹；k为常数，一般取0.04.

为了提高角度分布的均匀性，在计算出每个像素点的E后，首先按照非最大值抑制原则，取3×3窗口中各点E最大的像素点作为候选角点，并将候选角点按E由高到低进行排序.然后，设定一个候选角点间的最小距离Dmin，以其为约束条件对顺序排列的各候选角点进行筛选，便可以按需要的角点的数目进行定量提取.经过该种处理后，所保留的角点就是用于匹配跟踪的特征点.

1.2 Top－Hat变换

从Harris角点提取的原理可以看出，如果增大角点在两个正交方向上的梯度，将能够增大能量函数的值，得到更强壮的角点，这需要研究与Harris角点提取相匹配的局部图像增强算法.

局部图像增强算法以局部直方图均衡算法为代表，能够提高图像每个局部细节的对比度，但需要多次统计窗口内的直方图，计算量较大.为了达到快速局部图像增强的目的，本文采用形态学的Top－Hat变换来提高图像每个像素点的邻域梯度.该方法具有快速且易于并行实现等优点.

基于Top－Hat变换的对比度提高利用了形态学白Top－Hat和黑Top－Hat变换分别能够检测图像的峰值和谷值的性质.一幅图像f的白Top－Hat变换，用 WTH变换表示，定义为原始图像f与开运算γ（f）的差，即

开运算是一种非扩展变换，处理过程中在原始图像的下方，所以WTH变换用来检测图像的峰值.

一幅图像f的黑Top－Hat变换，用BTH变换表示，定义为原始图像f与闭运算 （f）的差，即

根据对偶性，BTH变换的结果是信号的波谷，因此BTH变换用来检测图像中的谷值.

根据WTH变换和BTH变换能分别检测出峰和谷的性质，如果将WTH变换的结果与原图像相加，就会增加图像中亮目标；而从得到的结果中减去BTH变换的结果，就会加强图像中的暗目标，这就提高了整幅图像的对比度，用kTH表示为

综上，本文图像特征提取部分的实现流程如下：设定能量函数的最小阈值Emin、角点间的最小距离Dmin和需提取的角度数目n进行角点提取，若能量值大于Emin的角点个数不足n，则进行Top－Hat变换后重新对角点进行提取，直至获取n个能量大于Emin的角点.

2 特征点匹配与跟踪

特征点的匹配与跟踪是里程计算的关键环节，需要首先对同时刻左、右图像上的特征点进行匹配，然后对不同时刻同视角图像上的特征点进行跟踪，最后建立同一物点在不同时刻不同视角图像上的对应关系.特征点匹配与跟踪的准确率直接影响运动估计的精度，同时也是影响算法实时性的关键环节.因此，本文从降维、合理选择搜索窗口和预估搜索方向等角度出发，提高特征匹配与跟踪的效率.

2.1 对极线约束匹配

对极线约束可描述为一幅图像上的点槇m在另一幅图像上的匹配点槇m′位于该点的对极线l′m上.数学上这一几何关系表示为一个3×3的基础矩阵F，如式（6）所示.通过基础矩阵的求解，可以将特征点匹配的搜索空间从二维降到一维的对极线上.

其中I表示匹配窗口的灰度值，fmw表示匹配窗口，上标A和B分别表示不同的图像.

（3）循环重复①～③m次，如果ε发生变化，使用式（8）作相应调整.其中Γ为至少取得一个良性取样子集的概率，应大于0.95；ε表示错误点占数据集的比例；p表示基本子集中数据点的数目.

① 在数据集中，取样基本子集p＝8；

② 由基本子集，采用最小二乘法计算基础矩阵；

③ 评估基础矩阵的相容性：对数据集中所有数据点，计算数据点像素与其对应极线的距离，大于某一阈值则被分类为错误匹配点，小于的为正确匹配点；

（4）通过与前一次循环的比较，保留具有最多正确匹配点数目的基础矩阵；

（5）所有正确匹配的点重新计算基础矩阵.

2.2 搜索窗口优化

设置合理的搜索窗口和搜索方向能够有效减少计算量，降低特征点位置变化对匹配跟踪的干扰，最终提高特征点匹配的效率.本文针对立体视觉传感器的特点，从以下几个方面对特征点匹配和跟踪的过程进行优化.

（1）限定感兴趣区域，从立体匹配的误差模型可知，物点的重建误差随其距摄像机距离的增大

为提高基础矩阵求解的精度以及对误匹配的鲁棒性，本文采用了基于RANSAC的方法.RANSAC的基本思想是：对数据进行多次随机取样，每次取出尽可能少但充分多个数据来确定模型参数，再根据已确定的模型对所有数据进行划分，在此模型误差范围内的点称为内点，反之为外点；经多次采样后，找出内点最多的点的集合进行进一步优化，最终确定模型参数.该方法理论上能在错误数据比例大于50%时获得稳定的估计结果，能够有效解决错误数据过多带来的不收敛问题.本文基于该思想的基础矩阵求解步骤如下：

（1）提取Harris角点作为待匹配的特征点.

（2）通过对Harris角点的初始匹配来确定匹配特征点数据集.初始匹配以式（7）所示的相似性度量函数为判断依据，引入唯一性约束来提高结果的可靠性.而增大.所以，在提取特征点进行匹配前，裁去原始图像中的远景部分，能够避免不精确的特征点跟踪对运动估计的干扰，同时也降低了计算量.

（2）对左、右图像特征点的匹配而言，由于摄像机安装姿态不变，具有固定的沿着极线的搜索方向.为了确定合适的搜索窗口，将图像分成若干区域，基于前一时刻立体匹配的结果为每个区域设置可参考的平均视差值，从而建立可更新的搜索窗口查找表，能够根据特征点的像素坐标和前一时刻匹配结果，分配合理的搜索区间.

（3）对前、后时刻同视角图像上的特征点跟踪而言，搜索方向和搜索窗口的确定依赖于车体的运动情况.所以，同样将图像分成若干区域，首先统计相邻时刻各区域内特征点的大致搜索方向和平均视差，然后采用Kalman滤波器预估下一时刻的搜索方向和平均视差，从而提高特征点跟踪的效率.

3 运动估计

根据立体视觉原理，确定左、右图像上特征点的对应关系后，依据摄像机标定结果，可以获取该特征点在车体坐标系下的三维坐标.定义前一时刻和当前时刻同一特征点i对应的三维坐标分别为Ppi＝ （xpiypizpi）T和Pci＝（xciycizci）T，运动估计的目标是希望找到坐标系间的旋转矩阵R和平移向量T，使得式（9）最小，从而获取车体位置和方向的变化.

其中N为跟踪上的特征点的个数.为避免错误的特征点跟踪影响运动估计的精度和收敛性，同样引入RANSAC思想，实现过程如下：

（1）在数据集中，取样基本子集p＝4；

（2）由基本子集，采用奇异值分解法计算旋转矩阵R和平移向量T；

（3）定义误差阈值，检测所有匹配点对R和T的支持程度，统计内点数；

（4）重复（1）～ （3）m次，找到内点数最多的R和T，并将这些内点作为新的匹配点集，采用奇异值分解法求解新的R和T.

用奇异值分解法求解坐标变换的过程为

（1）计算三维点集Pp和Pc的质心，并生成新的三维点集

4 试验分析

视觉里程计算利用安装在月球车上的一对立体相机加以实现，算法流程如图1所示.图2为立体相机在月球车原理型样车上的安装示意图，相机基线距离为10 cm，镜头焦距为12 mm，图像分辨率为640像素×480像素.立体视觉系统的标定采用了Tsai等的两步法[9]，该方法能够考虑镜头的径向畸变，其主要优点在于大部分求解过程是线性的，为非线性迭代部分提供了良好的初值，能同时计算出摄像机的内外参数.此外，为了计算月球车的滑转率，在6个车轮上还分别安装了轮速传感器.

图1 视觉里程算法流程图Fig.1 Flow chart of visual odometry algorithm

图2 视觉里程计和试验车Fig.2 Visual odometry and the test vehicle

为模拟月球车在月面的工作环境，需要对月壤进行模拟.依据参考资料[10]，月壤的粒度与淤沙相似，颗粒直径以小于1 mm为主，绝大部分颗粒直径在30μm～1 mm，中值直径在40～130 μm.为此，选择灰色的翻砂用细沙，搭建了小型试验场进行月球车漫游试验，图3给出了特征点稀少的模拟场地立体图像.从原始图像中可以看出，图像灰度均匀，没有明显的可提取特征用于匹配跟踪，可用于模拟恶劣的里程计工况.在该种工况下，如果不进行图像增强，则难以提取到足量的具有较大能量函数的角点；图4给出了进行Top－Hat变换后的立体图像，从直观上可以看出明显的对比度增强效果.角点提取试验表明，在能量函数阈值一定的前提下，增强后图像中提取的角点是原始图像中提取角点的2倍以上，可以有效提高所提取角点的质量，从而提高特征匹配的稳定性.

图3 立体相机拍摄的月面试验场图像Fig.3 Simulated lunar surface images taken by stereo camera

图4 Top－Hat增强后的图像Fig.4 The enhancement results of Top－Hat processing

图5给出了对增强图像中角点进行跟踪的结果，图中每个角点都带有一段直线，直线的另一端用于指示本特征点在另一幅图像中的对应点在本幅图像中的坐标投影位置.可以看出匹配跟踪算法能够适应特征点位置的变化，保证足量特征点的跟踪并且特征点分布比较均匀，便于进行运动估计.

图5 角点跟踪结果Fig.5 Corner tracking results

为了测试视觉里程算法在缺乏特征的环境中的稳定性和精度，本文进行了直行和弯道试验，月球车按最高速度200 m/h行驶.图6为直线行驶过程推算轨迹与实际轨迹的对比图，图7为弧线行驶过程推算轨迹与实际轨迹的对比图.从两组轨迹对比中可以看出，月球车实际行驶的轨迹与视觉里程计推算出的轨迹基本吻合，表明算法在松软的沙质环境中能够实现精确的定位.

图6 直行试验结果Fig.6 Straight experiment results

表1给出了上述两种工况的误差分析结果，从中可以看出，在近4 m的行驶过程中，里程计的定位误差整体不超过2%.试验车上没有安装更精密的姿态传感器，不能实时测量月球车行驶过程中车体俯仰角和侧倾角的变化，以及高度方向（Z方向）上的位移变化，所以在表1中无法给出这3个自由度上的误差统计，但从其较小的变化范围可以看出，算法能够输出稳定的结果，在运动估计过程中没有产生不收敛问题.

图7 弯道试验结果Fig.7 Bend experiment results

表1 试验数据分析Tab.1 Experiment data analysis

图8给出了弯道行驶过程中车体偏航角φ、俯仰角θ和侧倾角ψ的变化情况，通过与终点处车体的实际偏航角进行对比发现，偏航角的累计测量误差为1.28°，受软土路面的影响，车体的俯仰角和侧倾角在小范围内波动，整体变化范围不超过3°，说明本算法能够精确、稳定地反映出车体在6个自由度上的位姿变化.

图8 弯道试验车体的偏航、俯仰和侧倾角Fig.8 Yaw，pitch and roll of vehicle for bend experiment

为了计算月球车的滑转率s，对视觉里程计和车轮里程计的输出进行对比，图9给出了弯道行驶过程中车轮的滑转率变化曲线，实现了有效的滑转率估计，能够为基于滑转率的驱动控制奠定基础.

图9 滑转率检测Fig.9 Slip ratio measurement

5 结 语

本文针对沙质的月面模拟环境，开发了可以对月球车进行精确定位的视觉里程算法，能够推算车体在6个自由度上的姿态变化，并与车轮里程计结合实现了滑转率的实时估计，为月壤参数估计以及月球车驱动控制奠定了基础，具有重要的现实意义.作为一种相对定位装置，随着连续行驶里程的增加，视觉里程计会产生累计误差，所以，如果要实现月球车长距离的精确定位，下一步需要研究能够融合全局定位信息的视觉里程计定位误差修正方法.

[1]MATTHIES L，SHAFER S.Error modeling in stereo navigation[J].IEEE Journal of Robotics and Automation，1987，RA－3（3）：239－248

[2]NOURANI V N，ROBERTS J，SRINIVASAN M V.Practical visual odometry for car－like vehicles[C]//IEEE International Conference on Robotics and Automation. Kobe：Institute of Electrical and Electronics Engineers Inc.，2009：3551－3557

[3]NISTER O N，BERGEN J.Visual odometry[C]//Proceedings of the IEEE Computer Society Conference on Computer Vision and Pattern Recognition.Washington D C：Institute of Electrical and Electronics Engineers Computer Society，2004：I652－I659

[4]MARK M，YANG C，LARRY M.Two years of visual odometry on the Mars exploration rovers[J].Journal of Field Robotics，2007，24（3）：169－186

[5]DANIEL M H，YANG C.Path following using visual odometry for a Mars rover in high－slip environments [C]//IEEE Aerospace Conference Proceedings.Big Sky：Institute of Electrical and Electronics Engineers Computer Society，2004：772－788

[6]彭 勃，周文晖，刘济林.基于Harris角点检测的立体视觉里程计[J].兵工学报，2007，28（12）：1498－1502

[7]李 智，周文晖，刘济林.动态场景下基于视差空间的立体视觉里程计[J].浙江大学学报（工学版），2008，42（10）：1661－1665

[8]吴功伟，周文晖，顾伟康.基于视差空间的双目视觉里程计[J].传感技术学报，2007，20（6）：1432－1436

[9]TSAI R， LENZ R K. A technique for fully autonomous and efficient 3D robotics hand/eye calibration[J].IEEE Transactions on Robotics and Automation，1989，5（3）：345－358

[10]郑永春.模拟月壤研制与月壤的微波辐射特性研究[D].贵阳：中国科学院地球化学研究所，2005：21－22