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关键点提取算法原理
Fig.1 FAST key point extraction algorithm principle
以关键帧的灰度图像中的一个像素点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 当前帧阈值与前一帧关键点数的关系
Table 1 Relationship between current frame threshold and key points of previous frame
前一帧关键点数当前帧阈值<100εth-5100～300εth-3300～600εth600～800εth+3>800εth+5>800εth+5
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 紧耦合原理
Fig.2 Tight coupling principle
图3 惯导数据与视觉数据的关系
Fig.3 The relationship between inertial navigation data and visual data
对惯导数据进行预积分[16]，得
(5)
(6)
(7)
式中：Rj，Ri，Rl为旋转矩阵，i,j表示2个不同的时刻，
分别为l时刻角速度和加速度的观测值；
分别为l时刻陀螺仪和加速度计的零位漂移误差；
分别为l时刻离散状态下陀螺仪和加速度计的高斯误差；Δt为惯性传感单元(Inertial Measurement Unit，IMU)的采样时间；Δtij为时刻i与时刻j之间的时间；vj，vi，vl为相应时刻的速度；g为重力加速度；Pj，Pi为平移向量。
通过对极几何法[17]将单目相机检测到的关键帧数据表示成如下形式：
(8)
G=K-TEK-1
(9)
(10)
式中：E为本质矩阵；
表示PC的反对称约束,PC为由单目相机数据解算出的平移向量;RC为由单目相机数据解算出的旋转矩阵；G为基础矩阵；K为相机内参矩阵；S1，S2为空间中某一物体在2个归一化平面上的坐标；h1，h2为空间中某一物体在2个关键帧中所对应的2个关键点在像素平面上的坐标。
将经过处理的惯导数据与单目相机数据融合成一个待优化的量：
χ=[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)
式中：
为初始点观测值；Δχ为增量；Jk为雅各比矩阵；Mk为机器人移动前的误差值；Nk为常数项；Hk为海塞矩阵。
代价函数的改变量为
ΔFk=2NkΔχ+ΔχTHkΔχ
(15)
为了使增量为极小值，令ΔF对Δχ的导数为0，可得
(16)
代价函数优化问题被转换成线性方程求解问题，使用Schur消元法[19]求解线性方程，即可得到下一时刻机器人的位姿信息。
3 实验验证
3.1 同类算法对比
在弱光照条件下对本文算法进行实验验证。在光线很暗的楼道里，采用ZED双目相机对廊灯进行拍照采样，并使用ORB算法、SURF算法、SIFT算法、Harris算法分别对图像进行特征点提取。设置4种算法的最大特征点提取数均为400，特征点提取效果如图4所示，特征点提取点数、耗时及无用特征点数见表2。
(a) Harris算法
(b) SURF算法
(c) ORB算法
(d) SIFT算法
图4 特征点提取算法效果对比
Fig.4 Comparison of the effect of feature point extraction algorithms
表2 4种算法的特征点提取结果对比
Table 2 Comparison of feature point extraction results of four algorithms
算法耗时/s提取点数无用点数Harris2.175 01880SURF3.249 62251ORB0.468 91940SIFT6.810 72875
分析图4和表2可知，在弱光照环境下，特征点提取数量最多的为SIFT算法，但因为其存在无用特征点数，且实时性不好，所以不适合用于弱光照条件下的特征点提取。SURF算法与Harris算法虽然提取的特征点数较多且耗时较短，但其提取的特征点具有聚集性，不能完整描述空间物体的特征，也不能用于弱光照条件下的特征点提取。ORB算法虽然提取的特征点数较少，但耗时最短，且特征点分布均匀，可以准确描述物体特征。
3.2 ORB算法改进效果
由于ORB算法提取的特征点数较少，通过自适应阈值和极大值抑制等方法对其进行改进。改进ORB算法与原ORB算法的特征点提取结果对比见表3。由表3可知，改进ORB算法虽然提取时间有一定增加，但提取的可用特征点数也大大增加了。ORB算法改进前后特征点提取效率对比如图5所示。
表3 ORB算法改进前后特征点提取结果对比
Table 3 Comparison of feature point extraction results before and after the improvement of ORB algorithm
设定特征点数原ORB算法改进ORB算法耗时/s特征点数耗时/s特征点数2000.204 7940.343 61424000.468 91940.561 42946000.621 82970.834 65248000.979 63431.183 46771 0001.104 74221.269 77861 2001.188 44531.313 4805
(a) 提取时间对比
(b) 提取点数对比
图5 ORB算法改进前后特征点提取效率对比
Fig.5 Comparison of feature point extraction efficiency before and after the improvement of ORB algorithm
3.3 特征点匹配算法对比
设定特征点提取数为400，对同一图像进行特征点提取，并使用传统的暴力(Brute-Force，BF)匹配算法与RANSAC特征点匹配算法进行实验比对，特征点匹配效果如图6所示，通过对比发现，BF算法存在误匹配现象，而RANSAC算法剔除了误匹配点，提高了特征点匹配的准确性，从而可提高单目视觉定位精度。
3.4 视觉与惯导融合定位效果
使用煤矿移动机器人(图7)作为载体，ZED双目相机中的左摄像头作为视觉相机，其采样频率为100 Hz；使用SYD TransducerM型惯导，其采样频率为1 000 Hz，在Linux环境下运行视觉与惯导融合定位算法，进行实验验证。
(a) BF算法
(b) RANSAC算法
图6 不同特征点匹配算法的效果对比
Fig.6 Comparison of the effects of different feature point matching algorithms
图7 煤矿移动机器人
Fig.7 Coal mine mobile robot
使移动机器人按照固定路线移动，用全站仪测量其移动轨迹，分别计算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) 原融合定位方法的误差
(b) 改进后融合定位方法的误差
图8 改进前后融合定位算法的误差对比
Fig.8 Error comparison of fusion positioning algorithm before and after improvement
(4) 分别用改进前后的ORB算法与惯导进行融合定位，对比结果表明，改进后融合定位方法精度有了很大提升，相对误差由原来的0.6 m降低到0.4 m以下，平均误差由0.20 m减小到0.15 m，均方根误差由0.24 m减小到0.18 m。
[1] 杨林,马宏伟,王岩,等.煤矿巡检机器人同步定位与地图构建方法研究[J].工矿自动化,2019,45(9):18-24.
YANG Lin,MA Hongwei,WANG Yan,et al.Research on method of simultaneous localization and mapping of coal mine inspection robot[J].Industry and Mine Automation,2019,45(9):18-24.
[2] 葛世荣.煤矿机器人现状及发展方向[J].中国煤炭,2019,45(7):18-27.
GE Shirong.Present situation and development direction of coal mine robots[J].China Coal,2019,45(7):18-27.
[3] 田原.悬臂式掘进机惯性定位技术研究与试验[J].煤矿机电,2020,41(1):9-12.
TIAN Yuan.Research and test on inertial positioning technology of boom-type roadheader[J].Colliery Mechanical & Electrical Technology,2020,41(1):9-12.
[4] 马宏伟,姚阳,赵昊,等.矿山钻孔救援探测机器人设计[J].工矿自动化,2019,45(9):1-6.
MA Hongwei,YAO Yang,ZHAO Hao,et al.Design of mine drilling rescue and detection robot[J]. Industry and Mine Automation,2019,45(9):1-6.
[5] 梁艳,马宏伟,崔亚仲,等.煤矿四旋翼飞行机器人环境信息数据压缩算法[J].工矿自动化,2020,46(6):31-34.
LIANG Yan,MA Hongwei,CUI Yazhong,et al.Environmental information data compression algorithm for coal mine four-rotor flying robot[J].Industry and Mine Automation,2020,46(6):31-34.
[6] 徐国保,尹怡欣,殷路,等.智能移动机器人技术研究进展[C]//第五届全国信息获取与处理学术会议,北戴河,2007:683-687.
XU Guobao, YIN Yixin, YIN Lu, et al. Research progress of intelligent mobile robot technology[C]//The Fifth National Conference on Information Acquisition and Processing,Beidaihe,2007:683-687.
[7] 田海波,马宏伟,魏娟.矿井搜救机器人移动系统的研究与发展[J].机床与液压,2012,40(20):147-151.
TIAN Haibo,MA Hongwei,WEI Juan.Research and development of search and rescue robot's locomotion system for mine disaster[J].Machine Tool & Hydraulics,2012,40(20):147-151.
[8] 周绍磊,吴修振,刘刚,等.一种单目视觉ORB-SLAM/INS组合导航方法[J].中国惯性技术学报,2016,24(5):633-637.
ZHOU Shaolei,WU Xiuzhen,LIU Gang,et al.Integrated navigation method of monocular ORB-SLAM/INS[J].Journal of Chinese Inertial Technology,2016,24(5):633-637.
[9] 陈常.基于视觉和惯导融合的巡检机器人定位与建图技术研究[D].徐州:中国矿业大学,2019.
CHEN Chang.Research on localization and mapping of inspection robot based on visual-inertial fusion[D].Xuzhou:China University of Mining and Technology,2019.
[10] 皮金柱.基于单目视觉融合惯导的定位技术研究[D].绵阳:西南科技大学,2019.
PI Jinzhu.Research on positioning technology based on monocular vision fusion inertial navigation[D].Mianyang:Southwest University of Science and Technology,2019.
[11] JIANG Ping, FENG Zuren, CHENG Yongqiang,et al.A mosaic of eyes[J].IEEE Robotics & Automation Magazine,2011,18(3):104-113.
[12] LOWE D G.Distinctive image features from scale-invariant keypoints[J]. International Journal of Computer Vision,2004,60(2):91-110.
[13] BAY H, TUYTELAARS T,VAN GOOL L. SURF:Speeded-up robust features[C]//LEONARDIS A, BISCHOF H, PINZ A.Computer Vision-ECCV 2006.Berlin, Heidelberg:Springer,2006,3951:404-417.
[14] RUBLEE E, RABAUD V, KONOLIGE K, et al. ORB: an efficient alternative to SIFT or SURF[C]//Proceedings of IEEE International Conference on Computer Vision. New York: IEEE Press, 2011:2564-2571.
[15] SCHNABEL R,WAHL R,KLEIN R.Efficient RANSAC for point-cloud shape detection[J].Computer Graphics Forum,2007,26(2):214-226.
[16] QIN T, LI P, SHEN S. VINS-Mono: a robust and versatile monocular visual-inertial state estimator[J]. IEEE Transactions on Robotics, 2018, 34(4): 1004-1020.
[17] HARTLEY R I.In defense of the eight-point algorithm[J].IEEE Transactions on Pattern Analysis and Machine Intelligence,1997,19(6):580-593.
[18] LU F,MILIOS E.Globally consistent range scan alignment for environment mapping[J]. Autonomous Robots,1997,4:333-349.
[19] ZHANG F.The schur complement and its applications[M].New York:Springer-Verlag, 2005.