基于姿态阈值滤波的单目视觉农业机械定位方法*

2018-01-26黄沛琛罗锡文张智刚刘兆朋

计算机工程与科学 2018年1期

黄沛琛，罗锡文，张智刚，刘兆朋

(1.华南农业大学工程学院，广东广州 510642； 2.华南农业大学南方农业机械与装备关键技术教育部重点实验室，广东广州 510642)

1 引言

自动导航是实现农业机械智能化的核心问题之一，定位是实现自动导航的基础性难题[1]。目前，GPS、激光雷达与机器视觉是农业机械自动导航定位研究的三大热门[2]。GPS定位精度高，但其会因障碍物遮挡而导致定位精度下降甚至丢失信号，如林业和果园环境[3 - 7]。激光检测技术测量精度高，能以较高频率提供准确的距离信息，但设备成本高，且探测范围有限[8-12]。机器视觉技术凭借其探测范围广、获取信息完整、成本低等优势，成为目前国内外应用较多的机器人定位方式[13 - 19]。

机器视觉定位是一个仅利用单个或多个相机的输入信息估计物体的运动信息的过程。根据所使用的相机数目可分为单目视觉定位和立体视觉定位。由于立体视觉可通过固定和已知的基线提供深度信息，容易确定位移绝对尺度因子，因此大部分的机器视觉定位研究集中在立体视觉定位[20]。Nister等人[21]利用随机抽样一致RANSAC(Random Sample Consensus)迭代提纯，实现了鲁棒的视觉运动估算。Olson等人[22]利用定位信息限制特征跟踪的搜索范围以提高跟踪精度，并利用极大似然方法求解运动估计，在20 m的道路上获得了1.2%的相对位置误差。Sünderhauf等人[23]将稀疏光束平差法SBA(Sparse Bundle Adjustment) 扩展运用于立体视觉定位中，在初始三维模型和相机运动未知的情况下，对左右立体图像对中获取的特征运用滑动窗口 ( Sliding Window)方法，结合简单的运动阈值滤波完成运动估计。Zhang等人[24]利用单目视觉与轮式里程计相结合，采用转向模型分别还原旋转姿态和位移姿态，实验结果显示，平均误差低于1%。许允喜等人[25]提出了立体视觉采用微粒群优化的初始运动估计和内点检测新方法对车体进行运动的估计。王亚龙等人[26]针对移动服务机器人在未知环境下三维路径估计的问题，设计了一种基于Kinect摄像机的实时估计机器人运动轨迹的方法，文献所提出的方法可将视觉定位误差降低至3.1%。夏凌楠等人[27]则采用惯性传感器和立体视觉信息融合实现机器人定位。对于立体视觉，当物体距离远大于基线时，立体视觉近似于单目视觉。因此，只有在合适范围内的物体特征，立体视觉才能提供比较精确的景深信息。同时，立体摄像机参数校正过程较单目摄像机复杂，而摄像机参数校正的精度在很大程度上影响机器视觉定位算法的精度。相比之下，单目摄像机具有成本低、参数校正过程容易和校正精度高的优点，但其最大的缺点是缺少位移绝对尺度。近年来研究指出，加入一定的约束和假设条件，可以恢复其绝对尺度，如利用轮式车体运动的非完整性约束[28]以及利用车体运动在平坦地形上的假设等[29]。

本文通过对捕获的每帧图像进行特征点检测与跟踪，基于特征点配对，还原农业车辆行驶过程中的姿态信息。同时，通过假设车辆前方区域近似平坦，摄像机离地高度近似固定不变，实时修正车辆位移的绝对尺度因子，配合姿态阈值滤波，对车辆姿态信息进行在线修正。实验以实时动态全球定位系统RTK-GPS(Real-Time Kinematic Global Positioning System)定位数据作为对比，选择在三种不同的场景进行检测。

2 单目视觉定位算法

2.1 特征点检测与跟踪

由于blob和corner算子具有特征点位置计算精确的优点，本文对采集的每帧图像分别采用5*5模板的blob滤波器和corner滤波器进行滤波。对滤波后的图像采用非最大值抑制算法产生后续配对的候选特征点[30]。为确保计算效率，每帧图像提取候选特征点不超过1 000个。

给定两个提取的特征点，对其的11*11区域范围内Sobel算子水平滤波和垂直滤波后的响应采用绝对差值总和SAD(Sum of Absolute Differences)公式进行区域匹配。一旦SAD计算出现最小值，则成功获得最优匹配特征点对。图1为连续两帧图像特征点匹配后在k-1图像帧中显示的结果，匹配特征点数目为858个，特征点检测时间为13 ms，特征点匹配时间为22 ms。

Figure 1 Feature matching results图1 特征点配对结果

2.2 车体姿态估算

本文计算农业车体姿态包括车辆的平面位置信息和航向角。算法分成两步，对于初始采集的前两帧图像采用2D-2D特征点配对方法计算车体姿态与重构特征点的三维坐标；对于第三帧开始的图像序列，则采用3D-2D特征点配对方法计算姿态与重构特征点的三维坐标。

(1)对于已标定内部参数的摄像机，可以通过本质矩阵E描述两帧图像Ik-1与Ik之间的几何关系：

(1)

(2)

车体姿态估算步骤总结如下：

①获取Ik-2,Ik-1图像帧；

②提取并匹配两帧图像的特征点；

③重构Ik-2,Ik-1图像帧的配对特征点三维坐标；

④获取图像帧Ik；

⑤提取特征点并与Ik-1进行匹配；

⑥对3D-2D匹配的特征点采用PnP(Perspective-n-Points)算法求解摄像机姿态；

⑦重构Ik-1,Ik图像帧的新配对特征点三维坐标；

⑧重复步骤④；

2.3 定位姿态修正

在实际应用中，由于图像噪声、图像模糊或车辆震荡等原因会造成特征点检测出现不精确，从而导致错误匹配的无效特征点，称为外点(Outlier)。一旦出现外点，即使数目很少，计算得到的姿态结果也会与真实值有很大的偏差，因此需要识别外点，确定有效的特征匹配点对，称为内点(Inlier)。本文采用RANSAC较常用的处理外点方法；同时，本文采用稀疏光束平差法SBA(Sparse Bundle Adjustment)软件包实现摄像机估算位姿与重构的三维坐标实时局部最优化[34]。

单目视觉的校正难度低于立体视觉，有利于降低相机校正过程引入的误差对最终的定位精度造成的误差影响。然而采用单目视觉设计定位算法的困难在于缺少绝对尺度，在模型求解过程中，会出现尺度因子的模糊性。因此，在设计单目视觉定位算法时，需加入一定约束条件和假设。本文采用Kitt等人[35]提出的方法求解单目视觉定位算法过程中的绝对尺度。该算法假设车体行进过程中前方一小块区域是近似平坦地形，同时摄像机在移动过程中离地垂直高度近似固定不变，从而对绝对尺度加以约束。算法首先在所有重构的三维坐标中筛选属于车辆前方小区域地面的三维坐标点，采用式(3)确定绝对尺度因子s：

(3)

采用式(4)和式(5)分别对滑动窗口里面的图像帧位移向量和重构的三维点进行修正：

(4)

(5)

其中,i∈{S,k}，S为滑动窗口中最早的关键帧序号。

Figure 2 Camera coordinate system图2 摄像机坐标系

|headingk-headingk-1|≤η

(6)

|distk-distk-1|≤δ

(7)

Figure 3 视觉定位轨迹与RTK-GPS轨迹对比图图3 Comparison between visual localization and RTK-GPS

Figure 4 Absolute value of heading change estimated by localization图4 视觉定位估算的相邻航向角变化绝对值

式(6)为姿态阈值滤波器的航向角滤波条件，式(7)为位移向量的滤波条件，η和δ为预设的阈值，需根据实际行走地形设定。一旦估算的姿态不符合以上两个滤波条件之一，则该估算姿态无效并忽略当前图像帧，重新捕获下一帧图像进行姿态估算。一旦连续5帧图像都无法满足滤波条件时，则认为视觉定位出现较大误差，算法停止。图5为采用本文方法修正后的视觉定位轨迹和RTK-GPS定位轨迹对比图，姿态阈值滤波器有效抑制了漂移现象。本文在后续实验中将此方法整合至系统，对视觉定位轨迹进行实时修正。

Figure 5 Comparison between refined visual localization and RTK-GPS图5 修正后视觉定位轨迹与RTK-GPS定位轨迹对比图

3 实验结果与分析

实验选择在华南农业大学校园内三种不同路面上进行(如图6场景1～场景3所示)。场景1为空旷平坦的人造草坪标准操场;场景2为地表覆盖杂草、部分土壤裸露的草地，带有较多坑洼，单边种植景观树，树高约3.5 m,株间距5 m;场景3主干道为沥青地面，道路平均宽度为4.1 m,主干道两旁种植景观树，树高约3.5 m,株间距5.1 m，道路总长100 m。车辆平台采用John Deere越野车，图像采集设备为德国ImagingSource工业摄像机DFK21AU04，采集图像大小为640 *480像素，存储格式为JPG。

Figure 6 Experimental environment and agricultural vehicle图6 实验场景图及实验车辆

摄像机安置于车顶前端(图6实验车辆所示)，其中心距离地面垂直高度为2.15 m，向下倾斜9.45°。图像处理所用计算机为SONY VGN-SR16，CPU主频为2.26 GHz，内存为2 GB，程序开发环境采用Visual Studio 2010，使用C++语言开发。实验前，使用Camera Calibration Toolbox for Matlab工具箱对摄像机进行校正，获取其内部参数。在实验过程中，对所采集的图像先使用Damien Teney提出的算法在线进行图像畸形校正[36]。定位对比设备采用John Deere RTK-GPS，GPS实时位置原始数据采用10 Hz方式通过RS232串口通信传送至图像处理计算机，经坐标转换后在软件界面实时显示视觉定位与GPS定位跟踪效果。本文采用相对位置误差和航向角误差作为衡量视觉定位算法跟踪效果的主要指标：

(8)

Table 1 Data of experimental environment 1

Table 2 Data of Experimental environment 2

Table 3 Data of experimental environment 3

估算的车辆位置坐标，xgi,ygi为对应时刻GPS原始定位数据转后的位置坐标。

H=headingvi-headinggi

(9)

其中，headingvi为第i帧图像估算的车辆航向角，headinggi为对应时刻GPS原始定位数据估算的航向角。

为了检测算法的鲁棒性和精度，车辆保持接近1 m/s的速度分别在三种不同的场景进行测试。η和δ为多次实验后的经验值，η调整范围为40°～50°，δ调整范围为1.0 m～1.5 m。依据不同行走地形，在此经验值范围内反复进行微调，直到系统在该地形上有较好的定位效果，并以此η和δ值作为该地形的阈值。不同场景采用的阈值滤波参数如下：场景1，η=45,δ=1.1；场景2，η=45,δ=1.2；场景3，η=45,δ=1.0。不同场景的位置偏差如表1～表3所示。由实验统计结果可知，算法在场景1中有较好的跟踪效果，相对位置偏差平均值为5.459 9%，标准差为4.184 2%，航向角平均偏差为7.717 70，标准差平均值为6.788 90。通过分析可知，场景1地表纹理特征丰富，因此视觉定位算法能捕获足够多的特征点进行匹配。同时，场景1为平坦的操场地面，可很好地满足摄像机移动过程中离地面垂直高度固定不变的约束条件，故算法在场景1中有较好的跟踪效果。尽管场景2地面纹理特征比场景1更为丰富，但场景2的地面出现较多凹凸不平的坑洼，致使约束条件不能很好地满足，同时，凹凸的地面也导致人工驾驶难以保持匀速行驶，加剧车辆的颠簸，场景2的相对位置偏差平均值为8.373 1%，标准差为5.110 7%，航向角平均偏差为5.738 9°，标准差平均值为4.834 9°。场景3的地表纹理特征不太丰富，但其地表相对平坦，出现的凹凸地形较场景2少，配合本文的姿态阈值滤波器，跟踪效果较为理想，相对位置偏差平均值为6.443 94%，标准差为3.811 84%，航向角平均偏差为3.438 3°，标准差平均值为3.134 0°。图7为场景3其中一次实验图像序列离线处理后的视觉定位与GPS定位效果对比图，由图6可知，视觉定位轨迹曲线在车辆发生幅度较大的转向时，算法处理时间滞后于车辆的变化，此时产生位置偏差，并且这个偏差累积在后续的定位跟踪过程中。但是，只要车辆不发生大幅度转向，视觉定位算法对于位置和航向角都能有较好的跟踪精度。对三种场景数据统计分析可知，本文所提出的视觉定位算法在地表纹理特征丰富、地面较为平坦的道路有较好的定位效果。在凹凸不平的地面，加入本文的姿态阈值滤波器能够在一定程度降低定位误差。同时，本文在实验过程中发现提升摄像机高度有利于近似满足约束条件。本文在实验过程中发现，只采用单目摄像机难以消除航向角偏差、位置偏差的误差累积，配合绝对姿态传感器如惯性测量装置IMU(Intertial Measurement Unit)或RTK-GPS会是一个解决视觉定位算法误差累加的方法。

Figure 7 Localization result comparison between the visual localization algorithm and GPS图7 视觉定位算法与GPS定位轨迹对比图

4 结束语

本文针对农业机械在果园作业时视觉定位算法进行了研究，设计了基于单目摄像机进行特征点捕获、特征点跟踪，采用特定约束条件求解车体姿态的定位算法，并采用RTK-GPS数据作为参照进行了实验。

(1)针对果园环境复杂，有利于特征点检测，提出了采用blob和corner算子配合非最大值抑制算法产生特征点，并采用SAD进行匹配，实验表明每帧图像检测约600～900个特征点，所耗平均时间为38 ms。

(2)基于特征点匹配方法求解车体位置与航向角信息。同时，基于车体行进过程中前方小块区域近似平坦，摄像机在移动过程中离地垂直高度近似固定不变的假设求解车体位移向量的绝对尺度，配合车辆姿态阈值滤波器实时修正所估算的位姿。

(3)在模拟果园环境的三种不同场景进行了实车实验。三种不同场景的平均位置偏差分别为5.459 9%、8.373 1%、6.443 94%；三种不同场景的航向角变化平均值分别为7.717 7°、5.738 9°、3.438 3°。实验结果表明，加入车辆姿态阈值滤波器的视觉定位算法，能够有效降低因地形起伏不平所引致的误差，在一定程度上提高了果园车辆自动导航的可靠性和适用性。

从现有的实验结果来看,研究的算法是可行的，但定位精度还达不到RTK-GPS的厘米等级，在后期研究工作中，可与其他传感器技术如轮系里程计、惯性导航设备或GPS 等相结合，能满足如果园喷雾、施药等作业的精确定位要求。同时，车辆的颠簸引起摄像头的俯仰也会对定位精度产生影响。因此，多传感器信息融合、将机器学习和模式识别新方法引入至视觉定位系统，采用GPU进行大规模的并行计算使算法从根本上得到加速将是下一步的主要研究工作。同时，在其他传感器信号受干扰的地区，如温室、南方山区或果园环境，GPS信号会丢失，本文方法可作为一种低成本的辅助定位方式,起到补偿 GPS 误差的作用。

[1] Luo Xi-wen,Ou Ying-gang,Zhao Zuo-xi,et al.Research and development of intelligent flexible chassis for precision farming[J].Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE),2005,21(2):83-85.(in Chinese)

[2] Li M,Imou K,Wakabayashi K,et al.Review of research on agricultural vehicle autonomous guidance[J].Int J Agric & Biol Eng,2009,2(3):1-16.

[3] Subramanian V, Burks T F, Arroyo A A.Development of machine vision and laser radar based autonomous vehicle guidance systems for citrus grove navigation[J].Computers and Electronics in Agriculture,2006,53(2):130-143.

[4] Luo Xi-wen, Zhang Zhi-gang,Zhao Zuo-xi,et al.Design of DGPS navigation control system for Dongfanghong X-804 tractor[J].Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE),2009,25(11):139-145.(in Chinese)

[5] Chen Yan, Zhang Man,Ma Wen-qiang,et al.Positioning method of integrated navigation based on GPS and machine vision[J].Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE),2011,27(3):126-130.(in Chinese)

[6] Zhang Hui-chun,Zheng Jia-qiang,Zhou Hong-ping.Positioning accuracy analysis of RBN DGPS applied in precision forestry[J].Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE),2011,27(7):210-213.(in Chinese)

[7] Wan Li-qin,Qiu Bai-jing.Experiment and study of the positioning accuracy of DGPS in precision agriculture[J].Journal of Agricultural Mechanization Research,2003 (1):115-117.(in Chinese)

[8] Liu Pei,Chen Jun,Zhang Ming-ying.Automatic control system of orchard tractor based on laser navigation[J].Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE),2011,27(3):196-199.(in Chinese)

[9] Wang Ling,Liu Si-yao,Lu Wei,et al.Laser detection method for cotton orientation in robotic cotton picking[J].Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE),2014,30(14):42-47.(in Chinese)

[10] Feng Juan,Liu Gang,Si Yong-sheng,et al.Construction of laser vision system for apple harvesting robot[J].Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE),2013,29(1):32-36.(in Chinese)

[11] Xue Jin-lin,Zhang Shun-shun.Navigation of an agricultural robot based on laser radar[J].Transactions of the Chinese Society for Agricultural Machinery,2014,45(9):55-60.(in Chinese)

[12] Li Yong-jian. Robot Monto Carlo self-localization method based on combination of vision sensors and laser range finder[J].Transactions of the Chinese Society for Agricultural Machinery,2012,43(1):170-174.(in Chinese)

[13] Feng Juan,Liu Gang,Si Yong-sheng,et al.Algorithm based on image processing technology to generate navigation directrix in orchard[J].Transactions of the Chinese Society for Agricultural Machinery,2012,43(7):185-189.(in Chinese)

[14] Wu Jia-yi,Yang Qing-hua,Bao Guan-jun,et al.Algorithm of path navigation line for robot in forestry environment based on machine vision[J].Transactions of the Chinese Society for Agricultural Machinery,2009,40(7):176-179.(in Chinese)

[15] Xiong Jun-tao,Zou Xiang-jun,Chen Li-juan,et al.Visual position of picking manipulator for disturbed litchi[J].Transactions of the Chinese Society of Agricultural Engineering(Transactions of the CSAE),2012,28(14):36-41.(in Chinese)

[16] Jiang Huan-yu,Peng Yong-shi,Shen Chuan,et al.Recognizing and locating ripe tomatoes based on binocular stereo vision technology[J].Transactions of the Chinese Society of Agricultural Engineering(Transactions of the CSAE),2008,24(8):279-283.(in Chinese)

[17] Zhou Jun,Chen Qin,Liang Quan.Vision navigation of agricultural mobile robot based on reinforcement learning[J].Transactions of the Chinese Society for Agricultural Machinery,2014,45(2):53-58.(in Chinese)

[18] Zhang Hao,Chen Yong,Wang Wei,et al.Positioning method for tea picking using active computer vision[J].Transactions of the Chinese Society for Agricultural Machinery,2014,45(9):61-65.(in Chinese)

[19] Meng Qing-kuan,Zhang Man,Qiu Rui-cheng,et al.Navigation line detection for farm machinery based on improved genetic algorithm[J].Transactions of the Chinese Society for Agricultural Machinery,2014,45(10):39-46.(in Chinese)

[20] Li Yu-bo,Zhu Xiao-zhou,Lu Hui-min,et al.Review on visual odometry technology[J].Application Research of Computers,2012,29(8):2801-2804.(in Chinese)

[21] Nister D, Naroditsky O,Bergen J. Visual odometry[C]∥Proc of IEEE International on Conference on Computer Vision and Pattern Recognition,2004:652-659.

[22] Olson C,Matthies L,Schoppers M,et al.Robust stereo ego-motion for long distance navigation[C]∥Proc of IEEE International Conference on Computer Vision and Pattern Recognition,2000:453-458.

[23] Sünderhauf N,Konolige K,Lacroix S,et al.Visual odometry using sparse bundle adjustment on an autonomous outdoor vehicle[C]∥Proc of Autonome Mobile System,2005:157-163.

[24] Zhang J,Singh S,Kantor G.Robust monocular visual odometry for a ground vehicle in undulating terrain[C]∥Proc of the 8th International Conference on Field and Service Robots (FSR),2014:311-326.

[25] Xu Yun-xi，Xiang Zhi-yu，Liu Ji-lin.Initial motion estimation and inliers detection based on particle swarm optimization for stereo odometry[J].Control Theory & Applications,2015,32(1):93-100.(in Chinese)

[26] Wang Ya-long,Zhang Qi-zhi,Zhou Ya-li.Design of 3D visual odometry based on Kinect[J].Journal of Computer Applications,2014,34(8):2371-2374.(in Chinese)

[27] Xia Ling-nan,Zhang Bo,Wang Ying-guan,et al.Robot localization algorithm based on inertial sensor and video odometry[J].Chinese Journal of Scientific Instrument, 2013,34(1)：166-172.(in Chinese)

[28] Scaramuzza D,Fraundorfer F,Siegwart R.Real-time monocular visual odometry for on-road vehicles with 1-point RANSAC[C]∥Proc of IEEE International Conference on Robotics and Automation,2009:4293-4299.

[29] Scaramuzza D, Siegwart R.Appearance-guided monocular omnidirectional visual odometry for outdoor ground vehicles[J].IEEE Transactions on Robotics,2008,24(5):1015-1026.

[30] Neubeck A,Gool L V.Efficient non-maximum suppression[C]∥Proc of ICPR,2006:850-855.

[31] Longuet-Higgins H. A computer algorithm for reconstructing a scene from two projections[J].Nature,1981,293(10):133-135.

[32] Shewchuk J R.Triangle:Engineering a 2D quality mesh generator and delaunay triangualtor[M]∥Applied Computational Geometry Towards Geometric Engineering. Berlin:Springer, 2012:203-222.

[33] Moreno-Noguer F,Lepetit V,Fua P.Accurate non-iterativeO(n) solution to the PnP problem[C]∥Proc of IEEE International Conference on Computer Vision,2007:1-8.

[34] SBA: A generic sparse bundle adjustment C/C++ package based on the Levenberg-Marquardt algorithm[CP/OL].[2015-03-17].http://users.ics.forth.gr/～lourakis/sba/.

[35] Kitt B,Rehder J,Chambers A,et al.Monocular visual odometry using a planar road model to solve scale ambiguity[C]∥Proc of European Conference on Mobile Robots,2011:43-48.

[36] Damien’s Matlab library[CP/OL].[2015-03-17].http://www.montefiore.ulg.ac.be/～dteney/dml.htm.

附中文参考文献：

[1] 罗锡文，区颖刚，赵祚喜，等.农业智能作业平台的模型的研

制[J].农业工程学报，2005，21(2):83-85.

[4] 罗锡文，张智刚，赵祚喜，等.东方红X-804拖拉机的DGPS自动导航控制系统[J].农业工程学报,2009,25(11):139-145.

[5] 陈艳，张漫，马文强，等.基于GPS和机器视觉的组合导航定位方法[J].农业工程学报,2011,27(3):126-130.

[6] 张慧春，郑加强，周宏平.精确林业 GPS 信标差分定位精度分析[J].农业工程学报,2011,27(7):210-213.

[7] 万丽芹，邱白晶.GPS 在精确农业应用中定位精度的实验研究[J].农机化研究,2003 (1):115-117.

[8] 刘沛，陈军，张明颖.基于激光导航的果园拖拉机自动控制系统[J].农业工程学报,2011,27(3):196-199.

[9] 王玲，刘思瑶，卢伟，等.面向采摘机器人的棉花激光定位算法[J].农业工程学报,2014,30(14):42-47.

[10] 冯娟，刘刚，司永胜，等.苹果采摘机器人激光视觉系统的构建[J].农业工程学报,2013,29(1):32-36.

[11] 薛金林，张顺顺.基于激光雷达的农业机器人导航控制研究[J].农业机械学报,2014,45(9):55-60.