视觉与惯导融合的煤矿移动机器人定位方法

张羽飞， 马宏伟， 毛清华， 华洪涛， 石金龙

(1.西安科技大学 电气与控制工程学院， 陕西 西安 710054；2.西安科技大学 机械工程学院， 陕西 西安 710054；3.陕西省矿山机电装备智能监测重点实验室， 陕西 西安 710054)

0 引言

随着煤矿智能化的不断发展，移动机器人在煤矿井下的应用逐步增多[1]。目前常用的井下机器人定位方法有超声定位、激光定位和超宽带(Ultra Wide Band,UWB)定位等[2-4]。其中，超声定位性价比高，但易受幻影干扰等因素影响；激光定位精度高，但成本很高，适用于无人环境；UWB定位精度高、速度快，但只在小范围内表现出色[5-7]。

近年来，基于视觉传感器的定位方法成为研究热点。单目视觉定位是指利用1台摄像机完成定位工作，具有结构简单、标定步骤少等优点。单目视觉定位算法主要包括特征点法和直接法。特征点法是指提取能够描述图像信息的特征点进行定位[8]，由于其稳定、对动态物体不敏感的优势，一直是目前主流的解算方法[9-10]。在计算机视觉中，图像信息一般都是以灰度值矩阵的方式储存[11]。特征点是图像的一个重要局部特征，也是图像信息的数字表现形式。很多特征点提取算法被提出。D. G. Lowe[12]提出了基于尺度不变特征变换(Scale-Invariant Feature Transform，SIFT)的算法，该算法精度高、鲁棒性好，但计算量极大。H. Bay等[13]提出了加速稳健特征(Speeded Up Robust Features，SURF)算法，该算法减少了提取时间，提高了鲁棒性，但实时性仍较差。E. Rublee等[14]提出了快速特征点提取和描述(Oriented FAST and Rotated BRIEF，ORB) 算法，该算法实时性、鲁棒性较好，但在光照变化和弱光照区域表现较差，无法应用于煤矿井下光照较暗的场景。

本文在ORB算法的基础上进行改进，采用随机抽样一致性(Random Sample Consensus，RANSAC)算法[15]进行特征点匹配，采用对极几何法进行视觉解算，提高了煤矿井下弱光照区域的特征点提取效率。但单目视觉定位仍存在尺度问题，即观测一个物体时，仅靠单目视觉定位无法确定机器人与物体的距离及物体的大小，因此，本文将单目视觉定位数据与惯导数据进行融合，以恢复视觉相机的尺度，提高定位精度。

1 特征点提取与匹配算法

1.1 ORB算法原理

ORB算法主要包括FAST关键点提取算法与BRIEF特征子描述2个方面。其中，BRIEF特征子主要负责对特征点进行描述，影响单目视觉定位精度的主要是FAST关键点提取算法。关键点是指图像中与周围像素点有明显灰度值差异的点。FAST关键点提取算法原理如图1所示，通过比较一定邻域内像素点的灰度值与中心点的灰度值大小，实现对特征点的判断。

图1 FAST关键点提取算法原理

以关键帧的灰度图像中的一个像素点p为圆心、固定半径画圆，从圆上选取16个像素点xn(n=1,2,…,16)，将各像素点的灰度值与中心点p的灰度值作差，并取绝对值，得

m=|I(xn)-I(p)|

(1)

式中I(xn)，I(p)分别为像素点xn，p的灰度值。

FAST-9是常用的FAST关键点提取算法，其原理如下：设定阈值ε作为判定关键点的标准，将像素点x1—x16代入式(1)进行计算。若存在连续9个以上的邻域点与中心点的灰度差满足m>ε，则说明像素点p为关键点，否则舍弃该点。

1.2 ORB算法改进

FAST-9算法需要大量的计算来确定关键点，且容易包含错误点。因此，本文将符合标准的邻域点数提高到12，并选取像素点x1,x5,x9,x13代入式(1)进行计算。如果上述4个点中存在2个以上的点不满足条件，则舍弃点p，反之则将点p确定为候选点。对候选点进行非极大值抑制处理：检查候选点的一个邻域内是否存在多个候选点，若邻域内无其他候选点，则该候选点为准确的关键点；若邻域内存在多个候选点，则计算每个候选点的强度响应值s(式(2))。若点p是邻域内所有候选点中s值最大的点，则选定点p为关键点，并舍弃其他候选点；否则舍弃点p，选择s值最大的点作为关键点。

(2)

对FAST-9算法进行改进后，不但减小了计算量，而且提高了特征点提取精度和效率。

FAST关键点提取算法中阈值是固定的，但是，光线条件不同，相邻像素点的灰度差值也不同。在光线较差的环境下，中心像素点与邻域像素点的灰度差值很小，若阈值过大，则提取出的关键点较少；若单纯地减小阈值，则可能导致在光照较好的环境下提取到多余或不稳定的关键点。因此，本文在FAST关键点提取算法中增加自适应阈值调节过程，根据前一帧检测到的关键点数确定当前帧的阈值，从而保证检测到的关键点的数量和质量在合理范围内。关键点数为300～600时定位结果较精确，因此，以该区间为基准，设定初始阈值εth为20。当前帧阈值与前一帧关键点数的关系见表1。

1.3 RANSAC算法原理

完成特征点提取后，需要对2个关键帧之间的特征点进行匹配，然后才能对图像数据进行解算。传统的特征点匹配方法在进行大量特征点匹配时往往会产生误匹配现象，导致视觉定位精度下降。本文选择RANSAC算法进行特征点匹配。

表1 当前帧阈值与前一帧关键点数的关系

RANSAC算法原理：在2个包含特征点的关键帧中随机选择4对点作为内点，其他特征点作为外点;根据内点求解出描述2个帧间位置关系的单应性矩阵，利用单应性矩阵对所有外点进行测试;将所有满足单应性矩阵的外点归为新的内点，再次利用新的内点更新单应性矩阵，对剩余的外点进行测试。

设W为本次测试中特征点都是内点的概率，则1-W为本次测试中特征点都是外点的概率，(1-W)y表示y次测试中特征点都是外点的概率，进行y次测试后,至少有1次选取的点数都是内点的概率z为

z=1-(1-W)y

(3)

(4)

进行y次测试后的剩余点对即为匹配好的点对。RANSAC算法能够根据大量特征点准确建立起2个关键帧之间的最佳匹配模型，减少误匹配的点，达到提高定位精度的目的。

2 视觉与惯导融合算法

多传感器耦合主要包括紧耦合和松耦合2种方法。松耦合是指各部分直接解算后再融合，因为惯导的误差不能完全消除，且解算过程中需要经过多次积分，会导致累计误差越来越大，所以本文采用紧耦合方法，即将惯导数据与单目视觉数据分别不完全解算后再进行联合优化。

紧耦合原理如图2所示。对惯导数据进行一次预积分，得到速度、旋转矩阵与平移向量；对单目视觉数据进行特征点提取与匹配，得到图像的特征数据；将图像特征与惯导预积分后的数据融合成一个待优化变量，构建优化函数；对函数进行求解与优化，从而估计出机器人位姿。

惯导与单目相机的频率不同，数据类型不匹配，因此需要分别对数据进行处理。惯导数据与视觉数据的关系如图3所示，其中r1—r3为地图中的路标点，c1—c3为相机的中心，d1—d3为关键帧。在2个关键帧之间存在许多惯导数据，因此需要将若干惯导数据积分成一点，才可进行后续数据融合。

图2 紧耦合原理

图3 惯导数据与视觉数据的关系

对惯导数据进行预积分[16]，得

(5)

(6)

(7)

通过对极几何法[17]将单目相机检测到的关键帧数据表示成如下形式：

(8)

G=K-TEK-1

(9)

(10)

将经过处理的惯导数据与单目相机数据融合成一个待优化的量：

χ=[Rj,RC,Pj,PC,vj,ba,bg]

(11)

式中ba，bg分别为IMU加速度计和陀螺仪的误差。

采用图优化[18]方法进行数据融合，将IMU的测量残差和视觉观测残差结合在一起构成代价函数，对代价函数进行优化，从而获得机器人的位姿信息。需要优化的代价函数为

(12)

式中：eB(χ)为IMU的测量误差方程;eC(χ)为单目相机的测量误差方程。

对代价函数进行展开、合并，可得

(13)

式中：q为阶数;ek(χk)为包含IMU测量误差和单目相机测量误差的误差方程；Ωk为信息矩阵，为协方差矩阵的逆。

对函数F(χ)进行一阶泰勒展开，可得

(ek+JkΔχ)TΩk(ek+JkΔχ)=

Mk+2NkΔχ+ΔχTHkΔχ

(14)

代价函数的改变量为

ΔFk=2NkΔχ+ΔχTHkΔχ

(15)

为了使增量为极小值，令ΔF对Δχ的导数为0，可得

(16)

代价函数优化问题被转换成线性方程求解问题，使用Schur消元法[19]求解线性方程，即可得到下一时刻机器人的位姿信息。

3 实验验证

3.1 同类算法对比

在弱光照条件下对本文算法进行实验验证。在光线很暗的楼道里，采用ZED双目相机对廊灯进行拍照采样，并使用ORB算法、SURF算法、SIFT算法、Harris算法分别对图像进行特征点提取。设置4种算法的最大特征点提取数均为400，特征点提取效果如图4所示，特征点提取点数、耗时及无用特征点数见表2。

(a) Harris算法

表2 4种算法的特征点提取结果对比

分析图4和表2可知，在弱光照环境下，特征点提取数量最多的为SIFT算法，但因为其存在无用特征点数，且实时性不好，所以不适合用于弱光照条件下的特征点提取。SURF算法与Harris算法虽然提取的特征点数较多且耗时较短，但其提取的特征点具有聚集性，不能完整描述空间物体的特征，也不能用于弱光照条件下的特征点提取。ORB算法虽然提取的特征点数较少，但耗时最短，且特征点分布均匀，可以准确描述物体特征。

3.2 ORB算法改进效果

由于ORB算法提取的特征点数较少，通过自适应阈值和极大值抑制等方法对其进行改进。改进ORB算法与原ORB算法的特征点提取结果对比见表3。由表3可知，改进ORB算法虽然提取时间有一定增加，但提取的可用特征点数也大大增加了。ORB算法改进前后特征点提取效率对比如图5所示。

表3 ORB算法改进前后特征点提取结果对比

(a) 提取时间对比

3.3 特征点匹配算法对比

设定特征点提取数为400，对同一图像进行特征点提取，并使用传统的暴力(Brute-Force，BF)匹配算法与RANSAC特征点匹配算法进行实验比对，特征点匹配效果如图6所示，通过对比发现，BF算法存在误匹配现象，而RANSAC算法剔除了误匹配点，提高了特征点匹配的准确性，从而可提高单目视觉定位精度。

3.4 视觉与惯导融合定位效果

使用煤矿移动机器人(图7)作为载体，ZED双目相机中的左摄像头作为视觉相机，其采样频率为100 Hz；使用SYD TransducerM型惯导，其采样频率为1 000 Hz，在Linux环境下运行视觉与惯导融合定位算法，进行实验验证。

(a) BF算法

图7 煤矿移动机器人

使移动机器人按照固定路线移动，用全站仪测量其移动轨迹，分别计算ORB算法改进前后与惯导融合定位的测量误差，结果如图8所示。从图8可看出，改进后融合定位方法精度有了很大提升，相对误差由原来的0.6 m降低到0.4 m以下，平均误差由0.20 m减小到0.15 m，均方根误差由0.24 m减小到0.18 m。实验结果证明，改进ORB算法与惯导融合定位方法能够有效提高定位精度。

4 结论

(1) 与同类算法相比，ORB算法虽然提取的特征点数较少，但耗时最短，且特征点分布均匀，可以准确描述物体特征。

(2) 改进ORB算法与原ORB算法相比，虽然提取时间有了一定的增加，但提取的可用特征点数也大大增加了。

(3) 特征点匹配算法对比结果表明，与BF算法相比，RANSAC算法剔除了误匹配点，增加了特征点匹配的准确性，从而提高了单目视觉定位精度。

(a) 原融合定位方法的误差

(4) 分别用改进前后的ORB算法与惯导进行融合定位，对比结果表明，改进后融合定位方法精度有了很大提升，相对误差由原来的0.6 m降低到0.4 m以下，平均误差由0.20 m减小到0.15 m，均方根误差由0.24 m减小到0.18 m。