基于融合深度信息的自动喷雾车全局非线性轨迹优化方法

刘 慧 刘加林 沈 跃 朱嘉慧 李尚龙

(江苏大学电气信息工程学院， 镇江 212013)

0 引言

近年来，随着农业信息化、自动化的发展，林道间的植株施药从早期主要依靠人工完成逐渐向无人化、智能化发展。因此，基于高精度定位的自主行驶车技术[1-4]在农业施药领域将成为智能变量喷雾的发展趋势，有望解决人工喷雾过程中劳动强度大、危害施药人员安全、浪费农药、污染环境等问题[5-6]。

目前自主行驶车辆定位方法主要有：基于全球定位技术(GPS)[7]的方法；基于激光雷达的定位方法[8-12]；基于单目、双目、RGBD的视觉定位方法[13-19]。如文献[20]使用GPS提供精准的实时经纬度地理信息获取车辆位置，但室内和有枝叶遮蔽的果园林间GPS信号差，使得作业过程中经常需要重定位。文献[21]使用激光传感器对温室内道路边缘进行检测与深度数据提取，实现车辆自身定位和自主行驶，但其仅依赖道路边缘的深度信息检测，使得车辆在定位过程中易出现深度信息丢失，且激光传感器价格昂贵，成本较高。文献[22]基于Kinect深度摄像机使机器人从一个起始位置出发，利用单目视觉里程计进行自身定位，从而依据位置调整前进的方向，但由于依赖机器人前一位置状态，缺乏全局的非线性优化，定位误差大，行驶鲁棒性差。

本文在前人研究的基础上，提出一种融合深度信息的轨迹优化方法。应用RealSense[23-24]二代传感器获得连续彩色图像和深度图像，提取关键帧，并通过全局非线性优化的方法不断迭代误差，使喷雾车在田间行驶过程中具有高精度定位，并在优化过程中融合深度信息对喷雾车林道间行驶进行实时定位，增强喷雾车实时获取自身精确轨迹的鲁棒性，从而避免依赖单一深度信息轨迹定位时，深度信息帧易丢失、无法进行实时定位、自身轨迹与真实轨迹偏离较大的问题。

1 试验和方法

1.1 RealSense植株图像数据采集平台

试验地点选择室外林道，光照强度均为1 000 lx，将RealSense传感器安装在自主行驶喷雾车上，在林道间行驶并连续拍摄两侧植株。如图1所示，使用RealSense传感器获取彩色信息帧和深度信息帧，并通过RealSense自带的RealSense for Windows SDK对彩色信息帧和深度信息帧进行实时校准，获取相匹配的彩色帧和深度帧。

图1 试验车示意图Fig.1 Schematic of experimental vehicle1.主控制箱 2.可调支架 3.RealSense传感器 4.搭载的嵌入式计算机 5.车水箱 6.喷雾喷头

如图2所示，本文采用的RealSense传感器，可以同时获得彩色信息和深度信息，性价比高，且其体积小巧，无需搭载220 V外接电源，通过自带的USB接口与嵌入式计算机相连进行供电和数据传输，使用安全轻便。

图2 RealSense传感器示意图Fig.2 Schematic of RealSense sensor1.深度摄像头 2.彩色摄像头 3.定位孔

1.2 ORB特征提取及匹配

1.2.1Oriented FAST特征点

图3 FAST特征点Fig.3 FAST feature point

FAST[25-26]算法主要用于检测灰度图中像素灰度变化明显的地方，如图3所示，以像素点A为圆心，3为半径的圆上取16个像素点，若选取的圆上有连续9个点的亮度大于A点像素值的20%,则这个点就为FAST特征点。由于是直接比较像素亮度的差值，所以计算速度非常迅速。但同时由于FAST特征点数量巨大，且不具备尺度不变性和旋转不变性，所以本文对FAST特征点进行改进。

(1)为了提高检测效率，通过暴力检测，即直接检测邻域圆上1、5、9、13这4个像素的亮度，当这4个像素中有3个像素亮度值同时大于像素A亮度20%，则继续对该点进行FAST检测，否则直接排除。同时在第1遍检测出FAST特征点后，原始的FAST特征点经常会出现局部扎堆的现象，此时通过非极大值抑制的方法对FAST特征点进行进一步的筛选，即通过选取5×5的窗口，分别计算其中FAST特征点的Harris[27]响应值，并按照分值排序，选取其中排名靠前的6个特征点作为最终的特征点集合。

(2)由于FAST特征点不具备尺度不变性，所以通过构建多尺度空间进行改进。由于传统图像金字塔采用下采样的方式对每一层进行Harris响应值计算，提取FAST特征点。本文采用基于OpenCV库的高斯差分金字塔，通过利用高斯卷积构建非完整金字塔结构的图像尺度空间，大大减少了计算Harris响应值的金字塔层数，增强了提取FAST特征点的鲁棒性和实时性。其计算公式为

L(u,v,δ)=I(u,v)*G(δ)

(1)

式中I——图像中的像素值

δ——高斯方差

G(δ)——高斯核函数

L(u,v,δ)——图像尺度空间

(3)针对FAST特征点不具备旋转不变性，采用灰度质心法进行改进。通过选取计算FAST特征点所在邻域图像块B的质心，连接几何中心O到质心C构成一个方向向量OC。从而使得FAST特征点具有了良好的旋转描述性，大大提升了不同图像之间特征点表述的鲁棒性。其计算公式为

(2)

(3)

θ=arctan(m01/m10)

(4)

式中mpq——图像块B的矩

θ——特征点的方向角

x、y——图像块中像素点坐标

p、q——系数

1.2.2BRIEF描述子

BRIEF[28]是一种二进制的描述子，其描述向量由特征点附近两个像素的亮度关系决定，并使用二进制编码来加速存储。但原始的BRIEF描述子不具备旋转不变性和尺度不变性。通过结合Oriented FAST特征点计算的BRIEF描述子，可很好地解决这两个缺陷。其计算公式为

(5)

式中I(xp,yp)——图像中p点的像素值

I(xq,yq)——图像中q点的像素值

Vf——p到q的向量描述值

1.3 特征匹配算法

1.3.1粗匹配算法

在获取了BRIEF描述子后，通过FLANN算法[29]创建许多棵随机树，然后从具有最高距离方差的维度中随机选取若干维度，并使用这些维度对数据进行划分。本文通过BRIEF向量串之间的汉明距离作为查询点距离，将BRIEF描述子向量不断进行维度划分，建立树形结构，并通过由上而下的递归方法，将查询点和参考点按区域搜索，进行最终的匹配。FLANN算法相对于暴力匹配算法，在需要对大量特征点匹配时速度更快，鲁棒性更好。

为进一步加速近邻搜索，本文采用Nanoflann算法[30]加速，Nanoflann算法是从内存和编译的角度来对近邻搜索进行优化。此算法还可以在近邻搜索前直接定义目标点云的最小包围盒，避免了近邻搜索中的重复计算，对不同类型的数据兼容性更高，适用性更广泛。通过对多组图像进行特征点提取，按特征点数量进行排序，分别对FLANN搜索算法和经过Nanoflann加速过后的搜索算法进行快速性对比，如图4所示。在特征点数量很大时，Nanoflann算法比FLANN算法快速性得到了较大提高。

图4 Nanoflann算法和FLANN算法快速性对比Fig.4 Comparison of fastness between Nanoflann algorithm and FLANN algorithm

1.3.2剔除误匹配点

由于喷雾车行驶过程中存在运动误差和路标点观测误差，所以在特征匹配后，特征点之间存在着很多误匹配点对。针对这些误匹配点对，需要对匹配的特征点对进行筛选。

首先对匹配的特征点进行最小距离过滤的初始筛选。计算统计描述子之间的最小汉明距离和最大汉明距离，当描述子之间的距离大于两倍的最小距离时，即认为匹配有误。由于存在描述子之间的最小汉明距离非常小的情况，所以设定一个经验值作为下限，取汉明距离30作为经验值。计算公式为

(6)

式中dB——匹配特征点对之间的汉明距离

dmin——匹配特征点对之间的最小汉明距离

G——匹配特征点正确匹配标志

在进行初始筛选后，为了进一步消除图像背景纹理和相似物体干扰造成的误匹配，采用基于本质矩阵的RANSAC方法对匹配结果进行后验。首先，从初始筛选后的点集D中，随机采样10组匹配点，通过对极约束(Perspective-n-point, PnP)[31]方程计算本质矩阵E，由本质矩阵对喷雾车进行运动恢复，解算出喷雾车的位姿变换矩阵T。再通过本质矩阵[32]和位姿变换分别计算图像I1和I2中点到各自极线的距离方差。如果每个匹配点对到极线的距离平方和小于初始阈值χ，则将这些匹配点对加入到内点点集Din，并记录距离方差；否则认为是错误匹配点对，从初始筛选点集D中移除。重复上述过程，直到初始筛选点集D中的点对都被访问过，则将内点点集Din作为D的更新点集，计算距离方差的均值作为阈值更新量，重复筛选过程，直到阈值χ近似收敛于一个常值。内点中的点对集合即为正确的匹配点对集。

1.4 融合深度信息非线性全局轨迹优化

1.4.1对极几何

图5 对极约束几何图Fig.5 Polar constraint geometries

通过上文得到的有效匹配点对结合对极约束计算重投影误差

(7)

式中 (pi,pj)——图像中用于求解本质矩阵的观测点的坐标向量

K——运动相机的内部参数矩阵

1.4.2基于深度信息的车位置解算

图6 喷雾车深度信息位置解算图Fig.6 Position of depth information of spray vehicle

当喷雾车在行驶过程中，对已经匹配好的有效特征点对进行深度信息的采集，通过计算相邻两关键帧获取的匹配特征点到喷雾车的深度信息和位姿变换角即可计算出小车行驶过程中具体位置变化，如图6所示，图中C1、C2为车在相邻两关键帧中的位置，P1、P2、P3为相邻关键帧中的有效特征点，P1C1为第1帧特征点到车的深度信息，P1C2为第2帧特征点到车的深度信息，C1C2为相邻两帧车的位置变化。喷雾车从C1行驶到C2过程中，RealSense传感器采集到相邻两关键帧间喷雾车的位置变化，经过特征点匹配后，观测目标点P1场景位置不变，由对极约束获取的本质矩阵E解算出喷雾车旋转矩阵R，从而获得旋转角α。最后通过三角形余弦定理即可求出喷雾车在相邻关键帧间移动的距离lC1C2。其计算公式为

E=tR

(8)

(9)

式中t——运动相机的平移向量

lP1C1——第1帧特征点到喷雾车的距离

lP1C2——第2帧特征点到喷雾车的距离

将融合深度信息计算的位移lC1C2作为对极约束解算平移向量t的模长优化量，将优化后的平移向量t′和旋转矩阵R回代入公式，获得融合深度信息的本质矩阵E′，并将该本质矩阵作为特征点获取重投影点的本质矩阵，用于计算融合深度信息的重投影误差。其计算公式为

(10)

式中β——位移长度比例参数

t′——融合深度信息计算的平移向量

1.4.3融合深度信息非线性全局优化

在特征点观测和喷雾车运动过程中存在着大量的观测误差和运动误差，而传统基于滤波的线性优化方法对前一个时刻喷雾车状态的依赖性强，喷雾车在行驶过程中关键帧丢失对其正确计算重投影误差的影响极大，两帧间重投影误差迭代优化易陷入局部最优。因此本文算法通过获取喷雾车整个运动过程中一定时间间隔的所有连续关键帧，对全部匹配点累积重投影误差进行迭代优化，获得一个全局的最优解。

(11)

式中ri——车运动过程中采集每帧的时间间隔

i——车运动过程中采集帧序号

e——车当前运行的时间段序号

γ——车运动过程中采集的帧数

喷雾车从初始位置到当前运动位置时间段e内，按照式(11)计算相应时间间隔ri，并按此时间间隔采集全局轨迹的连续关键帧作为参考帧，进行特征点提取和特征匹配从而估计出各个连续关键帧间的本质矩阵E，并解算本质矩阵获得位移向量t和旋转矩阵R。然后通过RealSense传感器采集深度信息计算喷雾车实际位移，优化位移向量t，再结合旋转矩阵反求本质矩阵，获得融合深度信息的本质矩阵E′，再将特征点通过新的本质矩阵进行重投影，计算重投影误差。

(12)

式中ξ——车运动过程中重投影叠加误差

ηi——重投影点像素坐标

si——尺度因子

ξ^——相机运动过程中的扰动分量

最后综合全局各关键帧间的重投影叠加误差，构建最小二乘问题，并基于G2O库进行图优化，使总投影误差迭代收敛于一个全局最小误差值。其中在优化过程中，喷雾车位姿和观测路标点作为图顶点，重投影误差ξ作为图边，不断迭代更新喷雾车位姿和路标点观测值，获取喷雾车最优位姿和路标观测点最优位置。另外，针对迭代优化过程中的梯度下降策略，本文选取Dogleg算法进行更新值的求解，使其具有更高的效率和鲁棒性。

图7 融合深度信息和非线性位姿估计算法流程图Fig.7 Flow chart of fusion depth information and nonlinear pose estimation algorithm

融合深度信息非线性全局轨迹优化算法流程如图7所示。

2 结果与分析

2.1 FAST特征点获取

对RealSense传感器采集到的彩色信息关键帧提取FAST特征点。如图8所示，红色的点为FAST特征点。由图可知，图8a的FAST特征点密集冗余，在后期的特征点匹配过程中会占用大量计算时间，因此，图8b中在经过非极大值抑制后，减少了大量冗余点，消除了扎堆的现象，极大提高了匹配效率。

图8 FAST特征点提取图Fig.8 Extraction graphs of FAST feature points

2.2 特征匹配

在非极大值抑制后获得FAST特征点后，通过基于高斯差分金字塔分层提取特征点，灰度质心法计算特征点方向向量。获取具有尺度不变性和旋转不变性的特征点，并计算BRIEF描述子。使用FLANN快速最近邻算法进行粗匹配，并利用Nanoflann算法加速匹配时间，获取匹配结果后，对匹配点对进行筛选，移除误匹配点对。相对于传统ORB(Oriented FAST and rotated BRIEF)特征匹配过程中使用图像金字塔计算尺度不变性，暴力匹配算法对特征点进行特征匹配，并使用单一最小距离过滤误匹配点对进行筛选。本文算法在特征点提取上通过非极大值抑制减少冗余点，使用高斯差分金字塔计算尺度不变性，采用FLANN快速最近邻算法进行匹配，并增加了基于本质矩阵的匹配校验。使特征匹配在效率和精度上得到了很大的提升。匹配效果如图9所示。

图9 特征点匹配结果Fig.9 Matching graphs of feature points

图9a由于未对匹配点对进行筛选，其水平方向匹配线密集扎堆，匹配消耗大量时间，并出现许多倾斜的误匹配线。图9b为传统ORB匹配结果图，其误匹配线已经减少了一部分，但仍有较多误匹配对。图9c为本文算法匹配效果图，经过进一步筛选和校验后，可以明显看到匹配线稀疏得多，准确率也得到了很大提升。为验证试验结果的准确性，通过多次匹配试验，比较传统ORB匹配算法与本文算法，结果如表1所示。本文算法相对于传统ORB匹配算法，消除了63对误匹配对，匹配准确率提高了9个百分点，在时间上减少了3.33 s，使准确率和效率得到了较大的提高。

表1 传统ORB匹配算法和本文改进ORB匹配算法对比Tab.1 Comparison of traditional method based on ORB feature points matching algorithm and presented method

2.3 喷雾车行驶轨迹

图10 车行驶轨迹路线Fig.10 Track map of car

在获取正确的特征匹配点对后，融合RealSense传感器深度信息对车位姿进行全局非线性轨迹优化，其行驶轨迹曲线如图10所示。图中，散点为车行驶过程中提取的特征点。方框为经过本文算法优化后喷雾车的具体位姿。直线为喷雾车行驶轨迹。

2.4 行驶轨迹误差分析

由于室外林道光线强度大，RealSense深度信息传感器对光线敏感，单一使用RealSense深度传感器对喷雾车行驶轨迹进行优化，易出现深度信息丢失，喷雾车偏航误差较大的问题，如图11所示。喷雾车在林道间行驶过程中，单一基于深度信息帧定位的轨迹在第61帧开始出现大幅偏离真实轨迹的现象。而本文算法，通过加入基于全局的非线性轨迹优化后，补偿了喷雾车深度信息帧丢失情况下偏离的行驶轨迹，使喷雾车行驶轨迹能够继续波动在真实轨迹附近。

图11 深度信息帧丢失轨迹误差分析Fig.11 Analysis of tracking error of depth information frame loss

此外，在正常行驶过程中，本文算法通过融合深度信息帧对喷雾车行驶轨迹进行基于全局的非线性优化，相较于单一基于RealSense传感器深度信息，行驶轨迹估计具有更高的鲁棒性。其行驶轨迹如图12所示，其在第21帧到91帧和第171帧到231帧过程中，本文算法获得轨迹曲线更加贴近于喷雾车真实轨迹曲线。

为验证本文算法的稳定性，经过多次试验并与单一基于RealSense传感器深度信息行驶轨迹优化方法进行对比，结果如表2所示，在10组单次最大采集300帧关键帧试验中，单一依赖RealSense传感器深度信息对喷雾车进行轨迹定位出现了13帧关键帧的丢失，极易出现图11中大幅偏离真实轨迹的情况，不利于喷雾车对自身的精准定位。而本文算法由RealSense传感器获取彩色信息帧，通过特征匹配点对的观测数据在深度信息帧丢失时进行喷雾车的精准定位，并且融合深度信息进一步提高了喷雾车位姿估计的鲁棒性和准确性，使喷雾车获取的实时轨迹偏离真实轨迹误差均值下降了1.07 cm，方差下降了2.14 cm，超调量降低了2.13 cm。

图12 单一深度信息轨迹优化方法和融合深度信息的全局非线性轨迹优化方法优化轨迹误差对比Fig.12 Comparison of trajectory errors between single depth information trajectory optimization method and global nonlinear depth information fusion trajectory optimization method

表2 单一深度信息帧轨迹优化算法和本文轨迹优化算法对比Tab.2 Comparison of single depth frame trajectory optimization algorithm and presented algorithm

3 结论

(1)提出了改进的ORB特征提取筛选算法，使用高斯差分金字塔，减少了计算Harris响应值的金字塔层数，加快了FAST特征点的提取速度。

(2)在获取FAST特征点后，通过近邻算法进行特征匹配，并使用Nanoflann算法进行加速匹配过程。随后采用最小距离过滤和基于本质矩阵的RANSAC算法校验误匹配点对。相对于传统ORB特征匹配算法，匹配准确率提高了9个百分点，时间缩短了3.33 s。

(3)相对基于单一RealSense传感器深度信息定位喷雾车位置，本文算法通过RealSense传感器采集的彩色信息帧中特征匹配点对的观测数据进行定位补偿，并融合深度信息对喷雾车进行非线性全局轨迹优化，使喷雾车获取的实时轨迹始终波动于真实轨迹附近，相对于单一依赖深度信息帧的轨迹定位具有更高的精确性和鲁棒性，其喷雾车估计轨迹偏离真实轨迹误差均值下降了1.07 cm，方差下降了2.14 cm，超调量降低了2.13 cm。