基于多传感器融合的水稻行识别与跟踪导航研究

贺 静 何 杰 罗锡文 李伟聪 满忠贤 冯达文

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

0 引言

近年来，水稻生产机械现代化新技术发展快速，自动导航农业机械已经应用于水稻生产耕、种、管、收的各个环节中[1-2]，如无人驾驶拖拉机、无人驾驶插秧机和无人驾驶联合收获机等。然而，水稻生产工序多，劳动强度大，田间管理机械化水平不高。水稻追肥和病虫害防治的主要作业机械是高地隙喷雾机，作业时驾驶人员需集中精力避免轮胎碾压水稻行；水稻机械除草机作业时需避免轮胎碾压水稻，同时还需保证除草机具不损伤水稻[3-4]。目前，虽然全球卫星定位系统(Global navigation satellite system，GNSS)导航技术可实现高地隙喷雾机和插秧机等水田作业机械自动导航作业[5-6]，但其作业跟踪导航的路径规划并未考虑对水稻行的碾压问题。此外，采用无人驾驶直播机或插秧机作业时，由于侧滑和偏移等导致移栽的水稻行与规划路径有偏差。因此，识别水稻行并自动跟踪水稻行是实现水稻生产田间管理机械自动作业的关键。

机器视觉和激光雷达是常用的作物行识别技术，科研工作者开展了大量研究[7-14]。但是，传感器都有一定的局限性，仅搭载单一传感器的移动平台在未知环境中定位精度差、信息获取不完整。农业生产是非结构化环境[15]，采用多传感器融合的导航技术是实现复杂环境下作物行识别的最佳方案。BENET等[16]融合机器视觉、激光雷达和惯性测量单元(Inertial measurement unit，IMU)数据进行果树行识别与直线拟合，以实现自动跟踪果树行，试验结果表明，该融合方法具有较好的鲁棒性，可稳定实现果树行跟踪导航，但是通过识别拟合的路径和跟踪导航实际路径之间的偏差受车速和土壤结构的影响，作业平均横向误差在0.1～0.4 m之间。薛金林等[17]提出了一种基于机器视觉与激光雷达信息融合的农业车辆前方障碍物检测方法，试验结果表明，该方法能有效地剔除非障碍物的干扰，完整地检测出障碍物，但检测精度未见报道，且未考虑车体姿态的变化，算法难以在农田中应用。罗晨晖[18]以喷雾机为试验平台，提出机器视觉、2D激光雷达和GPS信息融合的作物行识别方法，基于改进的A*算法进行路径规划，并进行了离线仿真试验，但田间试验未见报道。

我国南方水田环境复杂，虽然研究人员采用不同的算法感知环境信息，但研究结果因环境、作物生长状态以及选用的传感器和算法不同而差异很大[19-20]。为此，本文在GNSS导航技术的基础上，提出机器视觉和激光雷达融合的水稻行识别方法，并进行水稻行跟踪导航控制，避免水稻生产田间管理机械作业时碾压水稻，达到提高水稻田间管理智能化水平、实现水稻全程无人化生产的目的。

1 机器视觉和激光雷达的坐标融合

坐标融合示意图如图1所示，涉及像素坐标系OuvUV、相机坐标系OCXCYCZC、激光极坐标系Oρθρθ、激光坐标系OlXlYlZl、车体坐标系OVXVYVZV和参考地面坐标系OWXWYWZW共6个坐标系。其中，相机坐标系和车体坐标系的原点在地面的投影重合，XW和YW通过WGS-84地心坐标系经高斯-克吕格投影转换后获取，ZW为高程信息(获取的高程信息均减去了安装高度)。

图1 多传感器空间融合Fig.1 Multi-sensors spatio information fusion

1.1 激光极坐标系转车体坐标系

(1)

式中dx23、dy23——O′l和OV在XV轴和YV轴方向的安装间距，mm

α——激光雷达与垂直线(ZV轴)的夹角，(°)

φ——激光雷达与左右方向水平线(YV轴)的夹角，(°)

Hl——激光雷达安装高度

1.2 像素坐标系转车体坐标系

(2)

式中fc1、fc2、cc1、cc2——相机内部参数

(3)

式中HC——相机安装高度，mm

γ——相机与前进方向水平线(XV轴)的夹角，(°)

ZV——图像中对象的高度，mm

对二维图像中某点的像素值，无法提供深度信息ZV，即不能准确地推算与它对应的三维空间坐标。但是，同一处理时刻，机器视觉和激光雷达识别的对象相同，因此图像二维像素转三维坐标时，缺少的高度信息可通过激光雷达获取。

1.3 车体坐标系转大地坐标系

航向姿态参考系统(Attitude and heading reference system，AHRS)安装在车体上，以准确获取车体的横滚角(αx)和俯仰角(αy)，双天线GNSS提供航向角(αz)，由全局翻滚-俯仰-航向矩阵

(4)

式中RX——绕X轴旋转矩阵

RY——绕Y轴旋转矩阵

RZ——绕Z轴旋转矩阵

可知从车体坐标系转至大地坐标的综合旋转矩阵wRV。设车体坐标系原点OV对应在大地坐标系下为点OW-V，OW-G为GNSS定位的当前车体位置，OG为车体坐标系下GNSS主天线对应的安装位置，可由车体的几何关系获得，则OW-V和OW-G的对应关系为

OW-V=OW-G+wRV(OV-OG)

(5)

(6)

2 水稻行识别

2.1 水稻行中心点提取

2.1.1目标区域选取

2.1.2目标区域内水稻行中心点提取

(1)水稻行图像中心点提取

图像中心点提取处理流程如图2所示，其中，目标区域的预处理操作包括超绿特征算法灰度化[21]、自动阈值二值化[22]和中值滤波去噪[23]。考虑一定区域内的水稻生长基本一致，若启动跟踪系统后无水稻生长时期分类信息，则利用最小错误率的贝叶斯决策进行生长时期判断[24]，并保存判断结果，同时根据生长时期判断结果进行行信息增强处理；若已知采集环境水稻行的分类信息，则直接进行行信息增强处理。

图2 视觉传感器水稻行中心点提取流程图Fig.2 Flow chart of rice row center point extraction by vision sensor

(2)目标区域范围内2D激光雷达中心点提取

激光雷达提取水稻行中心点的流程如图3所示。锐利的边缘和反射表面会导致激光雷达光束错位，并产生粗差点[25](图4a)。为了剔除粗差点干扰，选用拉依达准则(3σ准则)进行查找判断[26]，剔除粗差点干扰(图4b)。

图3 激光雷达水稻行中心点提取流程图Fig.3 Flow chart of extracting center point of rice row by LiDAR

图4 粗差点噪声剔除前后对比Fig.4 Comparison before and after outlier noise elimination

拟合平面法是常用的行地物分割方法[27-29]，但存在地面点需人工选择且高度差阈值的设定无依据，以及粗差点影响大等问题。剔除粗差点后的点云数据可反映真实的田面状况，但各数据点在高度方向的差异受地形等各种因素的影响，设定一个固定阈值难以准确实现地物分割。为此，提出基于AHRS横滚信息动态设定高度差阈值，对每帧数据独立进行地物分割。

图5 自动阈值设定示意图Fig.5 Schematics of automatic threshold setting

αx1=kaαx(i)+ka(1-ka)αx(i-1)+…+
ka(1-ka)9αx(i-9)

(7)

式中ka——平滑系数,0

考虑地面点云中可能存在低矮杂草的干扰点，设定阈值ZT(i)为

ZT(i)=mean(Zw(i,:))+30+Zcha(i)

(8)

式中Zw——激光扫描点P的垂直方向的高度

若Zw>ZT(i)，则该点为非地面点；若Zw≤ZT(i)，则该点为地面点。在低矮杂草的环境下进行地物分割的结果如图6所示。

同一激光帧下，不同对象对应大地坐标系的位置存在差异，根据这一特点对各帧激光的非地面点云数据进行分类，不同类别的阈值Tlidar为不同类别水稻行对应激光扫描线的最小间距差，其中0.352 9为激光雷达的角度分辨率，wl∈[0,1]为比例系数，设定wl=0.75，则

(9)

不同采集环境下，机器视觉和激光雷达在同一识别区域识别的中心点数据如图7所示，其中红色点为机器视觉识别的结果，蓝色点为激光雷达识别的结果。结果表明，机器视觉和激光雷达在不同环境下均能准确提取水稻行中心点。

2.2 中心点聚类拟合水稻行中心线

各中心点在两个坐标系下的XOY平面投影图如图7所示，结果显示大地坐标系和车体坐标系下均分类明显，但是大地坐标系下的数据相较于车体坐标系计算量大。因此，在车体坐标系下利用K-means方法聚类中心点[31]，依据大地坐标系和车体坐标系下数据存储时的关系，实现大地坐标系下的数据分类，最后利用稳健回归法拟合水稻行线[32]，结果如图8(图中红色点为机器视觉识别结果，绿色点为激光雷达识别结果)所示。

图7 不同坐标系下的水稻行中心点Fig.7 Rice row center point in different coordinate systems

图8 水田环境下的水稻行识别Fig.8 Rice row recognition in paddy field environment

3 水稻行跟踪导航信息提取与控制

3.1 水稻行跟踪导航信息提取

设大地坐标系下拟合的水稻行线的斜率和截距分别为kline和bline，a=mean(kline)，b=mean(bline),则导航基准线方程为：ynavi=ax+b。

为确保农机沿水稻行行走，需求解车体和导航基准线的横向偏差和航向偏差[33]，农机与导航基准线的横向偏差ε和航向偏差δ的关系如图9所示。

图9 航向偏差和横向偏差示意图Fig.9 Schematic of heading deviation and lateral deviation

航向偏差定义为：车体航向角βs和带有车头信息的导航基准线的方向角βlextend之间的夹角δ[34]，计算式为

(10)

横向偏差定义为点AGNSS=(Xwa，Ywa)到导航基准线的垂直距离ε，计算式为

(11)

横向偏差根据向量叉乘积的正负判定方向。若车头方向相对导航线偏左，ε=-ε；若车头方向相对导航线偏右，则ε=ε。

3.2 基于预瞄追踪的PID水稻行跟踪导航控制

以一定前视距离在路径上选取一点作为跟踪目标点进行导航控制具有良好的上线性能[35]，在大地坐标系OwXwYwZw中前视距离跟踪原理如图10所示。设路径跟踪前视距离为dp，试验平台融合系统的中心为O；P为前视距离为dp时在导航基准线上的目标点，相对预瞄追踪线的航向偏差为δ′，则基于航向偏差轮角决策量为

图10 前视距离跟踪示意图Fig.10 Schematic of tracking model

(12)

为满足系统的快速响应和稳定控制转向要求，在预瞄追踪的基础上，采用PID(Proportional integral differential)算法决策前轮转向轮角。

横向偏差轮角PID决策量θεpid为

(13)

式中Kεp、Kεi、Kεd——横向偏差轮角PID控制参数

ε(k)、ε(k-1)——第k和第k-1时刻所得到的横向偏差

Tε——横向偏差获取周期

预瞄追踪航向偏差轮角PID决策量θδ′pid为

(14)

式中Kδ′p、Kδ′i、Kδ′d——预瞄追踪航向偏差轮角PID控制参数

δ′(k)、δ′(k-1)——第k和第k-1时刻所得到的预瞄追踪航向偏差

Tδ′——预瞄追踪航向偏差获取周期

则转向角PID决策量θpid为

θpid=θεpid+θδ′pid

(15)

4 试验与分析

4.1 试验硬件平台

以乘坐式水稻插秧机的机头为平台设计了试验平台，如图11所示，主要包括：上海世达尔水稻精量穴直播机以及配套机头(洋马VP6G)、荷兰XSENS公司的MTI-300型AHRS、HOKUYO公司的URG-04LX 型2D激光雷达、BASLER公司的Aviator 1990-50gc面阵相机(分辨率为1 920像素×1 080像素)，BEI公司的DUNCAN9360型连杆式轮角传感器。其中相机的安装高度HC=1 115 mm，安装角γ=36.9°(利用Matlab R2019b自带工具Camera Calibrator进行相机标定获取)，fc1=1 508.7，fc2=1 554.4，cc1=944.1,cc2=531。激光雷达安装高度Hl=1 220 mm，安装角α=35.5°，φ=1.744°。预瞄追踪PID的主要控制参数为：Kδ′p=1,Kδ′i=0.2，Kδ′d=1，Kεp=1，Kεi=0.2，Kεd=1。

图11 硬件平台Fig.11 Hardware platform

采用LabVIEW设计水稻行跟踪导航系统，并通过CAN总线将横向偏差和航向偏差等信息发给GNSS导航系统，GNSS导航系统将导航控制指令发送到底盘线控系统，实现水田作业平台自动导航控制，如图12所示。GNSS导航系统为潍柴雷沃重工股份有限公司的AG1BD-2.5GD型，多传感器融合的导航系统采集AHRS、GNSS、面阵相机和激光雷达的频率均为10 Hz，通过CAN总线向GNSS导航系统发送横向偏差和航向偏差等信息的频率为10 Hz。

图12 试验平台系统框图Fig.12 System diagram of test platform

4.2 模拟水稻行曲线跟踪导航

为了验证多传感器融合的水稻行跟踪导航系统的性能，在华南农业大学增城试验基地的水田中插放仿真秧苗进行验证试验。如图 13a所示，仿真水稻插放在水田中，水稻株间距为150 mm，水稻最大弯曲度处的水稻偏移直线位置400 mm。试验过程中人工驾驶直播机至仿真水稻行起点位置，设置车体速度为1 m/s，转向控制的最大角速度约为0.448 rad/s，利用无线终端启动导航系统，导航结束后导出跟踪导航试验数据。

图13b为提取试验过程中机器视觉和激光雷达识别的水稻行中心点的大地坐标系数据在XOY平面的投影图，其中蓝色点为人工手持华测公司的I70型RTK-DGPS测量的水稻行位置，RTK平面精度为±(8+1×10-6D) mm，D为距离。结果表明，实时处理过程中提取的水稻行中心点数据真实地反映了水稻行信息。

图13 模拟水稻行跟踪导航试验Fig.13 Simulated rice row tracking and navigation experiment

试验过程中人工手持RTK测量记录的水稻行曲线即为跟踪导航的目标路径，水稻行跟踪试验结束后，将试验平台车体中轴线的GNSS定位数据作为试验平台实际行走轨迹，与目标路径上的位置点进行比较，分析水稻行曲线跟踪误差。跟踪误差计算方法为：查找行走轨迹中与目标路径位置点距离最近的点，采用欧氏距离计算两点的距离，该距离即为跟踪误差。

试验平台行走轨迹和目标路径的误差分析结果如图 14所示，结果表明，基于机器视觉和激光雷达的跟踪导航系统实现了水稻行跟踪导航，系统的标准偏差为27.51 mm，最小误差为1.73 mm，但是最大误差(92.45 mm)较大，可能是由于水田硬底层不平引起了车体侧滑。

图14 水稻行曲线跟踪误差Fig.14 Error of rice row curve tracking

4.3 机械移栽水稻行跟踪导航田间试验

田间试验环境为华南农业大学增城试验基地试验田，试验田采用插秧机移栽秧苗，2021年8月10日移栽，水稻行行距为Lspace=300 mm。田间试验环境和人机交互界面如图 15所示，进行水稻行跟踪导航试验。

图15 田间试验环境和交互界面Fig.15 Field experiment environment and human-computer interaction interface

试验过程：打开水稻行跟踪导航系统的人机交互界面点击开始导航按钮，同时开启录屏软件。发送导航指令，直播机由GNSS导航系统控制行驶至水稻行附近。启动水稻行跟踪导航系统获取水稻行信息后，系统从GNSS导航控制切换为水稻行跟踪导航系统提供横向偏差和航向偏差控制试验平台行走，到达行末时启用无线端发送指令停车,导航结束导出导航过程试验数据。

人工手持华测I70型RTK-DGPS测量记录水稻行位置作为目标路径的位置点，车体中轴线的GNSS定位数据作为车体路径，实际车体路径和目标路径的位置点如图16所示，结果表明，由于硬底层不平和水稻行种植不均，车体路径有小幅度的偏差，但基本在两行水稻中间。

图16 水稻行跟踪试验的试验平台轨迹Fig.16 Rice navigation driving track in field trials

水稻行跟踪导航系统的横向偏差和航向偏差如图17所示，其中最小横向偏差为0.03 mm，最大横向偏差为143.1 mm，横向偏差标准差为43.03 mm；最小航向偏差为0.002°，最大航向偏差为17.32°，航向偏差标准差为3.38°。结果表明：水稻行跟踪导航系统能准确控制试验平台跟踪水稻行行走。

图17 田间试验偏差Fig.17 Deviations of field trial

5 结论

(1)定义了机器视觉和激光雷达识别的坐标系统，并推导了各个坐标系的相互转换关系，以实现机器视觉和激光雷达识别的水稻行中心点的空间坐标统一。

(2)针对水田硬底层高低不平的特点，激光雷达提取水稻行中心时，提出了根据车体姿态信息动态设定分割阈值，试验结果表明在凹凸不平的路面环境下，该方法也能较好地实现地物分割。提出了在车体坐标系下利用K-means算法聚类中心点，聚类结果对应在大地坐标系下拟合直线，有效降低了运算量，提高了系统运行速度。

(3)水稻行跟踪导航试验结果表明，基于机器视觉和激光雷达的水稻行跟踪导航能快速识别水稻行，并实现水稻行跟踪导航，模拟水稻行曲线跟踪试验标准偏差为27.51 mm；在水田跟踪机械移载水稻行的横向偏差标准差为43.03 mm，航向偏差标准差为3.38°。但在硬底层高低不平的水田中会出现跟踪目标水稻行改变现象，主要是车体姿态变换引起的识别区域改变而引起导航路径发生改变。