果园机器人视觉导航行间位姿估计与果树目标定位方法

毕 松 王宇豪

(北方工业大学电气与控制工程学院， 北京 100041)

0 引言

自1990年至2019年，我国水果产量均位于世界首位[1]，果品及其相关产业已成为农民收入的主要来源，逐渐成为农村经济的重要支柱。目前，高度依赖人工的生产模式和快速增长的人工成本已成为制约水果生产的主要因素[2]，为进一步提高生产效率和水果品质，农业机械装备的智能化是未来发展的必然趋势。

自主导航是智能农业装备的关键技术之一，可有效降低劳动强度，提高作业效率，广泛应用于采摘、喷药、施肥等任务。视觉导航具有语义信息丰富、探测范围广、成本可控等优点，逐渐成为主流的导航方式，实现果园自主导航的关键在于位姿估计和果树定位，进而引导农业装备自主行进和自主作业。

针对自主行进问题，研究思路集中在两方面：基于道路或天空生成导航线和基于作物检测拟合导航路径。基于道路或天空的导航方法对植株种类、形状、高度具有较高鲁棒性，是国内外学者研究的热点。基于作物检测的导航方法需要准确识别作物主干，对复杂道路环境具有较高的适应性。文献[3-10]利用边界点拟合导航线，根据导航线定性提取航向信息，在特定环境中实现自主导航，但难以准确地定量分析位姿，导航准确性、位姿可控性可进一步提升。

为实现导航信息的定量分析，对基于视觉的位姿估计问题展开研究。文献[11-13]基于单目相机模型实现距离测量，但果园自主导航不仅需要测量道路宽度，也需要估计横向偏移和偏航角，这些方法应用于果园自主导航仍具有一定的局限性。

针对基于单目视觉的果树定位和自主作业方法，国内外学者展开深度研究。文献[14-17] 对果树识别方法展开探索，取得了较理想的结果。然而，上述研究停留在果树检测层面，并未深入研究果树定位方法。智能农业装备不仅需要自主行进、识别作物，更需要根据植株位置完成采摘、喷药、施肥等作业任务。

根据以上对果园自主导航研究成果的分析，目前视觉导航的局限性体现在以下两点：①导航信息不完整，以定性视觉自主导航为主，基于视觉的位姿估计方法难以有效提取偏航角和横向位移。②果树定位方法不完善，以果树检测研究为主，难以确定果树的相对位置。

针对现有方法的局限性，本文提出基于Mask R-CNN实例分割网络的位姿估计与果树定位方法。在改进并训练实例分割网络的基础上，完成道路掩码和果树掩码的提取，并进一步计算消失点[18-21]和边界方程。利用树行平行性，建立位姿-道路几何成像模型，确定不同位姿条件下，世界坐标与像素坐标的映射关系，定量分析位姿对图像关键点的影响，基于消失点和边界方程估计偏航信息和横向位移。

1 方法与模型

受光照条件和季节变化影响，种植园场景的色温、照度、路面环境、植株长势存在较大差异。与人工特征相比，深度神经网络具有更强大的特征提取与识别能力，可准确、稳定地提取复杂场景的语义信息。本文基于实例分割网络模型，提取道路与果树掩码，并进一步提出消失点与边界方程的计算方法。

为使智能农业装备成本可控，本文在果园条件下，研究单目相机成像过程。现代果园的可通行区域近似走廊，两侧树行平行且单侧树行共线。利用树行的平行性和共线性，建立位姿-道路几何成像模型，定量分析位姿、偏航角、边界方程三者的关系，推导果树相对位置的计算方法。

基于以上分析，本文主要工作由4部分组成：道路与果树掩码提取、消失点与边界方程计算、偏航角与横向偏移估计、果树相对位置估计。通过建立道路几何成像模型，仅用单目相机实现位姿估计与果树定位，对应的算法流程如图1所示。

1.1 基于Mask R-CNN网络的道路与果树掩码提取方法

深度卷积神经网络不仅可对纹理、形状、轮廓、颜色等低层次特征进行提取，而且可进一步提取高层次抽象特征，具有较强的分类、检测和分割能力。Mask R-CNN为基于深度卷积神经网络的实例分割模型，具有较高的计算效率和精确率，本文采用该网络完成道路和树干掩码提取，该模型主要由特征提取主干网络、区域建议网络、RoI Align、检测和分割分支组成，方法流程如图2所示。

基于该模型提取道路和果树掩码，但由于三维信息在单目相机中的成像过程为小孔成像，目标尺度在图像中表现为近大远小，其尺度存在较大差异性。为了获得多尺寸表示能力，要求特征提取能够以较大范围的感受野来描述不同尺寸的目标。基于以上原因，本文将Mask R-CNN模型的特征提取主干网络改进为Res2Net[23]和特征金字塔网络的组合，与ResNet[24]网络相比，该模型将特征图分成4组，除第1组特征X1外，每组特征图经3×3卷积后，以残差的方式与下一组进行连接，Res2Net模块结构如图3所示。

由图3可知，除X1特征组外，每个3×3的卷积核可以接收来自前层所有特征，其输出的特征图具有更大的感受野。基于该组网方式，Res2Net的输出包含不同尺寸及数量的感受野，同时，特征图输出前经过1×1卷积运算，可有效融合不同尺度信息。此外，为增强对不同尺寸物体的识别能力，本文在Res2Net网络的下一级增加特征金字塔网络，该模型通过提取多尺度的特征信息，并进行深度融合，从而提高检测的精确度。Res2Net网络和特征金字塔网络结构如图4所示。

由图4可知，主干网络提取特征的流程如下：首先，原始图像经卷积层和Res2Net模块提取不同层次的图像特征；其次，将C1～C4阶段的特征图进行1×1卷积运算得到特征图T1～T4，通过卷积上采样并与下一阶段的特征图叠加得到特征金字塔M1～M4；最后，将M1～M4的特征图进行3×3卷积运算后，输出特征图P1～P4至区域建议网络。

1.2 消失点与边界方程提取方法

针对Mask R-CNN分割的道路掩码，研究道路边界和消失点的提取方法。受到栽培精度、植株生长不确定性和行间平整度等因素的影响，可通行区域边界较不平滑，掩码区域的边界通常呈不规则锯齿线，如图5b所示。

由图5b可知，道路掩码属于非规则多边形，其左右边界呈波浪或锯齿曲线，增加了提取边界线方程和消失点的难度和复杂度。因此，本文首先寻找道路掩码的凸包，如图5c所示，掩码Q的凸包是一个最小的凸多边形P，满足Q中的每个点都在P的边界或者其内部。其次，基于霍夫变换和角度阈值计算左右边界方程，如图5d中绿色线所示，联合边界方程所得的交点坐标为道路消失点，如图5d中红色圆点所示。基于以上分析，所提方法由消失点提取、边界方程提取和果树坐标提取组成，具体算法流程如图6和图7所示。

边界方程、消失点和树干坐标的计算流程如下：首先，根据图6所示的边界方程和消失点计算方法，对道路掩码进行轮廓提取、寻找凸包和霍夫变换操作，获取描述边缘轮廓的直线簇，基于设定的斜率和长度阈值条件提取符合要求的边界方程，联立两侧边界线方程提取消失点坐标。其次，根据图7所示的树干像素坐标计算方法，提取果树掩码贴近道路边缘的坐标，位于消失点左侧的果树掩码，提取右下方坐标；位于消失右侧的果树掩码，提取左下方坐标；基于树干像素坐标计算距离边界线的距离，若该值大于阈值，则标记为邻行果树，不进行位置估计。

1.3 位姿-道路几何成像模型

偏航角、横向偏移和植株相对位置是实现自主导航的关键信息。本文基于单目相机成像原理，利用果园树行的共线性和平行性，间接测量以上信息。现就存在偏航角和横向位移的情况，对单目相机成像过程进行几何建模，其示意图如图8所示。

图8为一般果园场景示意图，左右树行边界S1、S2相互平行，由两树行所构成的空间为可通行区域，基于基本坐标系Oxyz建立成像模型，光心f距地面高度为h，与左右边界的距离分别为xl和xr，偏航角为φ，图像坐标系为Ocxcyc,像素坐标系为Oixiyi。基于果园道路边界的平行性和共线性，建立位姿-道路成像模型的方法如图9所示。

根据图9可知，建模过程可分为以下步骤：①由像平面关键点求解相机像平面方程。②基于针孔相机成像原理，计算边界线在像平面中的映射方程。③分析方程斜率和消失点表达式，逆解航向信息和横向位移。④基于地面点和像平面的映射关系，逆解果树的相对位置。其中，步骤①、②推导了道路边界在像平面中的映射方程，在本节展开分析；步骤③、④分析了根据边界方程计算位姿和果树位置。

1.3.1求解像平面方程

计算无偏航信息时的像平面方程：在相平面任意选取3个关键点pi(i=1,2,3)，文中坐标均表示为列向量或多维列向量，用于描述相机像平面在Oxyz坐标系下的平面方程。

i=1i=2i=3

(1)

式中CF——镜头焦距

CW——像平面宽度(相机感光元件宽度)

CH——像平面高度(相机感光元件高度)

H——相机安装高度(焦点与地面距离)

当存在偏航角φ时，关键点pi经偏航角旋转矩阵t变换为p′i

p′i=tpi=

i=1i=2i=3

(2)

其中

(3)

式中p′i——旋转变换的关键点

由关键点p′i可确定像平面法向量n

(4)

再与任一关键点(本文选取p′1点)构成平面点法式方程

n(1)(x-p′1(1))+n(2)(y-p′1(2))+n(3)(z-p′1(3))=0

xsinφ-ycosφ+CF=0

(5)

1.3.2计算树行边界在像平面中的映射方程

任取树行边界S1两点r1、r2(边界S2同理)，焦点f与r1、r2构成空间直线方程l1和l2。

(6)

i=1i=2

(7)

(8)

(9)

式中xl——关键点r的x轴坐标

y1、y2——关键点r的y轴坐标

β1、β2——直线参数方程的参数

联立像平面方程(式(5))与直线方程l1和l2，解得交点坐标为

i=1i=2

(10)

式中ai——直线参数方程与像平面的交点坐标(Oxyz坐标系)

将交点的世界坐标转换至图像坐标系，即计算ai在向量p′2p′1和向量p′3p′1上的投影a′i为

i=1i=2

i=1i=2

(11)

式中a′i——直线参数方程与像平面的交点坐标(Ocxcyc坐标系)

确定l1在Ocxcyc坐标系内的斜率k1、截距b1和S1在图像坐标系Ocxcyc的方程S′1、S′2的计算方法同理。其中，a′1(1)、a′1(2)为a′1向量的第1个元素和第2个元素，a′2(1)、a′2(2)同理。

(12)

b1=a′1(1)-kla′1(2)=-CFtanφ

(13)

(14)

(15)

式中kl——左边界直线方程斜率(Ocxcyc坐标系)

b1——左边界直线方程截距(Ocxcyc坐标系)

在存在偏航角和横向偏移的条件下，推导两侧树行边界在图像坐标系下的映射方程，进一步计算消失点坐标，估计偏航角和横向偏移。也可利用该映射关系，由像素坐标估计与两侧果树及相机的相对位置。

1.4 位姿估计与果树定位方法

基于建立的位姿-道路几何成像模型，首先，根据树行边界线方程计算消失点坐标，消失点是平行直线经过透视投影后的交点，基于该点的像素坐标，推导了偏航角的计算方法。其次，由像素坐标系下的边界方程，可解得与左右树行的距离，横向偏移是当前位置与道路中线的横向距离，基于与两侧边界的距离，推导了道路宽度和横向偏移的计算方法。最后，由树行边界点的三维坐标与像素坐标的映射关系，推导了果树相对位置的计算方法。

1.4.1偏航角与横向偏移计算方法

首先，计算消失点坐标。联立图像坐标系下的边界方程S′1和S′2，解得消失点(两直线交点)坐标，即联立式(14)、(15)求解交点坐标，可得消失点坐标Xc和Yc为

(16)

其次，推导偏航角计算方法。由式(16)可知，消失点xc轴坐标Xc为偏航角φ的一元函数，因此，可通过求解消失点坐标Xc方程可得偏航角φ。在图像坐标系中，消失点的坐标无法直接获取，可通过图像-像素坐标系的相互映射关系间接计算

(17)

式中Xi——消失点xi轴坐标(Oixiyi坐标系)

Ix——图像横轴分辨率

基于以上分析可知，通过消失点的像素坐标可计算偏航角估计值，计算方法为

(18)

再次，计算与边界S1和S2的距离。以左边界线为例，由S′1表达式可知，在图像坐标系和像素坐标系中，边界线斜率是关于偏航角φ和边界距离xl的二元函数，偏航角φ可由式(18)解出，因此，基于图像坐标系中边界线斜率可计算距离xl和xr。基于以上分析，与边界S1、S2的距离xl、xr估计值计算式为

(19)

(20)

最后，计算道路宽度和横向偏移。由于xl、xr为距离左右边界的距离，二者绝对值之和为道路宽度W，二者绝对值之差的一半为横向偏移λ，估计值计算式为

=|l|+|r|

(21)

(22)

1.4.2果树相对位置计算方法

由图8可知，在基本坐标系中，S1和S2为树行和可通行区域的交界，因此位于边界S1和S2的果树x轴坐标分别为xl和xr，只需求解y轴坐标即可估计果树的相对位置，因为果树的像素坐标可以从图像中获得，可利用式(11)所示的映射关系求解y轴坐标。因为安装高度h由测量得出，一般带有测量误差，为避免引入二次误差，采用yc轴坐标求解y，将a′i的yc轴坐标(式(11))一般化，由式(17)和

(23)

可得果树的yc轴与y轴坐标的映射关系，即y轴坐标的计算方法为

(24)

1.4.3算法流程

基于对偏航角、横向位移和位姿-道路几何成像模型的分析，可由图像中的消失点、树行边界方程估计当前位姿，计算偏航角和横向偏移。由树干的像素坐标计算果树与相机的相对位姿。若将单目相机刚性连接于农业机械装备，可通过图像中的关键点完成位姿估计和果树定位，计算流程如图10所示。

2 实验与结果分析

为了验证本文提出位姿估计和果树定位方法的准确性和鲁棒性，在训练、测试实例分割网络Mask R-CNN的基础上，进一步计算消失点与边界方程，完成了偏航角、横向偏移、果树相对位置计算实验，并验证本文所提方法的有效性。

2.1 实验条件

通过安装在三角架的单目相机抓取果园图像，偏航角由惯性测量单元的加速度计和陀螺仪融合滤波得出，通过卷尺测量得到横向偏移与果树相对位置。三脚架与InvenSense MPU6050型惯性测量单元刚性连接，图像采集设备为Nikon D90型相机，镜头焦距为35 mm，相机水平视场角为37.2°，感光元件尺寸为23.6 mm×15.8 mm，相机与三脚架螺纹连接，安装高度为0.95 m。

数据采集地位于山西省太原市尖草坪区向阳店苹果种植园，该种植园属于半结构化场景，平均行距为5 m，受光照、水肥、土壤等因素影响，果树生长具有较大随机性，其生长状况和株距也存在一定差异性。该苹果种植园中，果树树干与地面的颜色相似度较高，且可通行区域界线相对模糊，增加了道路与树干信息的提取难度，数据集包含多种拍摄角度、多种拍摄位置的图像数据，具有较强的代表性。

2.2 模型训练与验证

从相机中随机选取1 200幅图像制作数据集，其中720幅用于训练，240幅用于验证，240幅用于测试，采用Labelme工具标注树干和道路可通行区域。为提升训练和预测速度，图像预处理时，将图像长边的分辨率统一转换为1 280像素。为提高模型鲁棒性，抑制过拟合现象，本文训练时加入随机扰动扩充数据量，例如随机调整对比度、饱和度、亮度等。训练平台为Intel i7-6870K CPU，Nvidia Geforce GTX1080Ti×4 GPU，32 GB RAM。

为使区域建议网络生成的预测框更贴近目标最小外接矩，基于K-means方法聚类标注框的尺寸，为抑制不同尺寸的标注框对聚类误差的影响，本文选取交并比度量类别的相似性。聚类中心数量越多，与标注框的交并比越大，但计算量也相应增加；聚类中心数量较少时，数据处理量随之减少，与标注框的交并比也随之降低。聚类中心数量对应的平均交并比如图11所示。

由图11可知，聚类数量从2类增加为3类时，平均交并比提升显著。由于数据处理量随着预测框的增加而明显增加，聚类数量等于3时的综合性能较高，此时聚类中心的高宽比分别为0.7、3和8，并根据该比例调整区域建议网络生成的预测框。

在训练过程中，优化方法选择Adam自适应学习率优化器，其中GPU集群单次迭代样本数为4，初始学习率为0.005，学习率衰减系数为0.1，最小学习率为 1×10-6，总计训练100周期，训练时的平均损失值和验证集的平均精确度如图12所示。

由图12a可知，训练76周期时，边框回归平均精确度为0.564，分割平均精确度为0.559，且损失函数值较低，模型具有较强的分割和识别能力。此外，本文对比了以ResNet50为主干网络的Mask R-CNN模型，其损失函数曲线与平均精确度曲线如图12b所示，从图中可知其损失函数收敛速度、模型性能均弱于Res2Net，详细数据如表1所示。

由表1可知，Mask R-CNN(Res2Net50)的边框回归与分割具有更高精确度，分别提升4.5%和4%，推理速度提升20%；此外，基于该主干网络的Mask R-CNN对中小目标的检测和分割效果提升显著，受成像原理、拍摄角度、拍摄距离与单目成像原理等因素的影响，图像中的目标尺度具有较大差异，而Res2Net模型采用分组卷积方法，可有效提取多尺度目标的特征，在本文试验条件中表现出更高的适应性。

表1 2种主干网络的Mask R-CNN模型性能对比

基于神经网络分割的道路与树干掩码，提取消失点、边界方程、果树坐标等更深层次的信息：对于果树掩码，分类其所属位置：左行、右行和邻行，并对左右行的树干掩码提取贴近道路边界的坐标；对于道路可通行区域掩码，依次完成寻找凸包、霍夫变换、计算边界和消失点坐标操作，其中红色圆点表示消失点，绿色直线表示可通行区域的左右边界，实验结果如表2所示，其中，左上角为坐标原点，水平轴为u，竖直轴为v。

表2 消失点与边界提取结果

由表2可知，在不同拍摄位置和拍摄视角条件下，本文采用的网络模型可较稳定且准确地完成实例分割任务，采用的消失点和边界线提取方法具有较高的鲁棒性。存在偏航角和横向偏移时，本文所用方法仍能在不同位姿条件下，稳定地提取道路与果树掩码、消失点坐标、道路边界方程，为计算位姿与果树相对位置提供可靠基础。

2.3 偏航角与横向偏移估计

位姿估计是农业装备在半结构场景下自主导航的前提和基础，利用建立的道路成像几何模型，仅用单目相机即可实现偏航角与横向偏移的估计。基于消失点像素坐标、相机感光元件和镜头焦距可得当前航向信息，再由图像中的边界线方程和偏航角，估计与左右树行的距离，并估计行距与横向偏移。其中，偏航角、横向偏移与道路宽度的误差计算方法分别为

(25)

(26)

(27)

式中Eφ——偏航角估计误差

Eλ——横向偏移估计误差

EW——道路宽度估计误差

Ah——水平视场角，取37.2°

一般工况下，智能农业装备可能位于道路中间，也可能位于中线左侧或右侧。同理，偏航角也存在3种情况，既可能无偏航信息，也可能向左或向右偏航。为验证模型的鲁棒性与可靠性，本文分析了农业装备工作时的9种位姿，即3种横向位置和偏航角的交叉组合。实验结果如表3所示。

由表3可知，本文方法在存在偏航角和横向偏移时均能较准确地计算位姿，其中，偏航角平均误差为2.91%，横向偏移平均误差为4.82%，道路宽度平均误差为4.89%，其精度可满足农业装备导航需求。

表3 位姿估计及其误差

在此基础上，研究所处位姿对算法精度的影响，将偏航角与横向偏移取绝对值进行Min-Max归一化，对于偏航角误差，与当前偏航角、横向位移的协方差分别为0.069和0.035，对于横向偏移误差，相应的协方差分别为0.032和0.026，表明本文方法的精度受所处位姿的影响较小，具有较高的鲁棒性和适应性。

2.4 果树相对位置估计

农业自主导航任务不仅需要准确估计当前位姿，为轨迹规划和自主运行提供基础，更重要的是，在获取当前位姿的基础上，确定果树的相对位置，为智能化采摘、喷药和施肥等精细化操作提供保障。本文根据边界线像素-基本坐标系的相互映射关系，即根据式(19)～(24)计算左右2行各4棵果树的相对位置，其误差计算方法为

(27)

(28)

(29)

(30)

式中Exl——左侧果树横向位置(x轴)估计误差

Exr——右侧果树横向位置(x轴)估计误差

Ey——纵向位置(y轴)估计误差

Ed——株距估计误差

d——株距真实值

由于相机成像模型为射影变换，成像过程为三维信息向二维平面的映射，无法直接表示深度信息，当存在偏航角和横向位移时，加剧了果树的层叠现象，存在层叠现象的实验场景如图13所示。

由于图13的采集位置位于道路中线偏左，图像中左侧果树存在较明显的层叠现象。除距离较近的两棵果树层次结构较明显外，其余果树遮挡现象明显，增加了果树识别与定位难度。因此，本文在道路中线选取3处航向角为正前的实验点进行测试，分别测量每棵果树距离相机的横向相对位置、纵向相对位置与株距，并与估计值对比，计算其误差，实验场景如图14所示。

图14中的3处实验点位于同一行间的不同纵向位置，实验点1距离道路尽头最远，实验点2次之，实验点3距离道路尽头最近，3处实验点的位置示意图如图15所示。

本文在3处实验点测量20 m以内果树的相对位置以及与前株的株距，由图15所示，在实验点1分析编号为1～4的果树，在实验点2分析编号为2～5的果树，实验点3分析编号为4～7的果树，相应的实验数据如表4～6所示。

表4 实验点1果树位置估计结果

表5 实验点2果树位置估计结果

表6 实验点3果树位置估计结果

由表4～6可知，本文提出的果树相对位置估计方法，仅用单目相机即可较准确地估计果树位置，对最近探测距离到20 m以内的果树均能有效推测其位置，横向位置的平均误差为3.80%，纵向位置的平均误差为2.65%；此外，利用果树的相对位置可进一步估计株距，其平均误差为8.12%。

3 讨论

(1)提出基于改进Mask R-CNN网络的道路与树干识别方法，与Mask R-CNN(ResNet50)相比，边框回归精确率提升4.5%，分割精确率提升4.0%，推理速度提升20%。基于道路掩码可进一步计算消失点坐标和边界方程，对采集位姿、边界线的粗糙度具有较高的鲁棒性。基于提出的位姿-道路几何成像模型，利用消失点与边界方程，实现位姿估计与果树定位，偏航角、横向位移、果树位置的计算误差较低，且可提取不同位姿条件下的导航信息。

(2)误差主要来源于测量误差和相机系统误差。首先，用于测量偏航角的惯性导航单元存在累计误差，数据采集过程中的振动加剧了数据波动程度，因此，偏航角真值存在一定的误差。其次，本文采用相机并未经过标定，存在轻微畸变现象，因此，基于道路掩码提取的消失点坐标和边界方程也存在偏移，一定程度影响了位姿估计与果树定位精度。

(3)在路面平整度较低的种植园运行过程中，受道路不平度的影响，相机存在不同程度抖动问题，导致图像存在模糊的可能，可通过Richardson-Lucy算法抑制图像模糊。目前，果园作业仍对人工有一定的需求，可利用神经网络模型识别作业人员及其相对位置，基于数据帧差信息分析人员的运动趋势，进而实现更加智能化的行间自主导航。

4 结论

(1)提出基于改进Mask R-CNN网络的道路与树干识别方法。边框回归平均精确度为0.564，目标分割平均精确度为0.559，具有较强的抗干扰能力和更高的识别精度。

(2)提出基于位姿-道路几何成像模型的位姿估计与果树定位方法。偏航角、横向位移误差为2.91%和4.82%，果树位置的横、纵向误差为3.80%和2.65%，可提取不同位姿条件下的导航信息，具有较高的准确性和稳定性。