最小二乘法与SVM组合的林果行间自主导航方法

刘星星，张 超，张 浩，杨圣慧，江世界，郑永军,2※，苏道毕力格，万 畅,3

（1. 中国农业大学工学院，北京 100083；2. 现代农业装备与设施教育部工程研究中心，北京100083； 3. 塔里木大学机械电气化工程学院，阿拉尔 843300）

0 引言

自动导航是实现农业装备智能化的重要部分，根据传感器种类可将导航方式分为激光雷达导航、GPS/GNSS导航、机器视觉导航以及多传感器融合导航等[1-2]。张雄楚等[3]提出一种枣树树行视觉导航路径检测算法，对于灰枣枣园与骏枣枣园，该算法的路经检测准确率平均值分别为94%和93%，单帧图像处理时间分别为0.042和0.046 s，但该算法受到抖动、人像干扰、逆光以及地膜干扰等情况影响时容易产生检测错误。杨洋等[4]提出了一种基于车轮正前方可行走动态感兴趣区域的玉米行导航线实时提取算法，该算法提取的导航线与人工提取结果平均误差为1.157°，准确率为96%，单帧图像处理时间为0.097 56 s。聂森等[5]提出一种基于机器视觉的果园环境行间导航算法，该算法能够提取两侧树行的中心线作为导航路径，但是图像平均处理时间为1.166 s，算法较为复杂，处理时间过长，且也会受到光照与落叶的影响。袁池等[6]针对果树复杂空间层次造成直线检测算法失效的问题，提出了一种果树行中心线检测算法，单帧图像处理时间长达0.6 s，图像处理算法较复杂。所以在实际生产应用中，机器视觉导航技术受光照强度、视角范围等条件限制较大[7-9]。魏爽等[10]提出一种基于GNSS的农机自动导航路径搜索方法与基于预瞄点搜索的纯追踪模型，导航均方根误差最大为25.23 cm。熊斌等[11]采用DBS技术设计了一套施药机自主导航系统，在果园自动化施药作业试验中，导航横向误差最大不超过13 cm，横向误差平均值不超过3 cm。罗锡文等[12]基于载波相位差分GPS技术设计了一套导航控制系统，并设计了地头跨行转向的控制算法，在直线导航速度为0.8 m/s时，平均误差不足3 cm，最大误差不足15 cm。但是尽管现代化果园环境相对标准，仍然存在果树枝叶丰茂冠层郁闭的情况，环境等同于半封闭环境，卫星信号受到树冠遮挡，容易产生较大误差，因此在果园作业环境中，基于GNSS定位的导航方法还受到一定的限制。激光雷达受环境影响较小，具有较强的环境适应性，能够在户外实时获取作物或果树轮廓以及环境、位置信息，近年来在导航、扫描、对靶等领域得到广泛应用[13-15]。

Oscar等[16]设计了一种基于二维激光雷达的果园自动导航系统，横向误差为110 mm，航向角误差为1.5°。Thanpattranon等[17]利用二维激光雷达，设计了一种适用于果园自动曲线导航行驶系统，与人工驾驶相比，路径跟踪平均偏差为275 mm，标准差为9 mm。Bayar等[18]利用廉价激光测距仪实现拖拉机在苹果园中直线行走和地头转弯，在保证系统稳定性情况下，降低了成本。Freitas等[19]利用激光雷达、编码器等多传感器感知果园垄行果树位置信息，并采用派生卡尔曼滤波器判别植保机器人相对垄行的位置以及植保机器人的导航路径。陈军等[20]采用SICK LMS291型激光雷达来模拟果园环境导航，平均横向偏差120 mm，最大横向偏差400 m。贾士伟等[21]提出一种基于激光雷达的温室导航定位方法，在走廊环境中平均横向偏差为12.7 mm，均方误差为26.8 mm，温室道路平坦地带可实现自主行走。艾长胜等[22]利用卡尔曼滤波器处理由激光雷达扫描获取的果园数据信息，并结合SVM技术实现植保机器人定位，平均横向偏差为4.22 mm，平均航向偏差为0.72°。毛鹏军等[23]设计了林下自主导航机器人，采用ROS操作系统，搭载激光雷达实现环境地图构建，在室内模拟环境中能够规划路线、躲避障碍物，实现一定程度的自主导航。周俊等[24]提出一种利用激光雷达前后多帧数据匹配的农业机器人定位方法，在密植果园的果树行间试验中定位误差约为80 mm，精度基本满足果园作业机器人应用需求。

综上，近年来国内外基于激光雷达的林间导航技术研究很多，但多针对单一的林间环境，单一扫描树冠或者扫描树干。本研究设计一种基于激光雷达的作业平台导航系统，针对冠层相连的桃园行间、冠层不相连的柑橘园行间以及扫描树干的松树林行间3种环境，提出最小二乘法与支持向量机（Support Vector Machine，SVM）融合的导航路径生成算法，研究作业平台在林间环境中的导航性能。

1 导航系统与导航路径计算

1.1 系统组成及原理

本研究基于中国农业大学工学院设计的履带式喷雾机（如图1所示），采用激光雷达扫描林间作业环境，获取导航基础数据。该履带式喷雾机主要技术参数如下：外形尺寸1 575 mm×1 190 mm×1 355 mm（长×宽×高），驱动系统由48 V、1.3 kW直流无刷电机与RV50型蜗轮蜗杆减速机组成，动力系统采用电压为48 V、容量为45 ah铅蓄电池，配置6 kW增程器提高续航能力。

系统总体方案如图2所示。激光雷达传感器获取两侧树行点云数据，通过串行接口将数据传输至上位机。姿态传感器获取作业平台的姿态信息并通过串行接口传输至上位机。上位机对点云数据及姿态位姿数据进行处理、分析并计算出导航路径后通过串口将控制信号发送至下位机；下位机根据控制信号输出脉冲宽度调制（PWM）信号至电机驱动器以驱动电动机，实现作业平台的移动。

所用激光雷达传感器为上海思岚科技有限公司的RPLIDAR S1，该激光雷达具有40 m测距半径，可在室外环境下工作，扫描频率为8～15 Hz可调。姿态传感器选用SC-AHRS-100D2型号姿态模块，可提供动静态环境下实时的、高精度的横滚角、俯仰角和航向角。姿态传感器性能指标如表1所示。

表1 100D2型姿态传感器性能参数 Table 1 Performance parameters of the attitude sensor of 100D2

1.2 导航路径计算

当作业平台在树林行间行走时，利用姿态传感器获取的作业平台位姿信息，对由激光雷达获取的行间数据信息做方向上的校正，然后用最小二乘法拟合识别树行，结合支持向量机，获得树林行间中心线，以此中心线作为作业平台的参考导航线。

1.2.1 激光雷达点云数据校正

作业平台在树行行间行走时，激光雷达初步扫描捕获的树行行间实况信息如图3a所示，由于作业平台行进时有航向偏差，激光雷达扫描的点云信息在行间行进中不能很好分辨出树林中的行与列，所以用姿态传感器的航向信息对激光雷达点云进行角度数据校正，排除作业平台行进时的航向偏差所带来的激光雷达点云数据起始位置的角度干扰，确保每一帧激光雷达数据起始位置的角度相同，以便于后续数据处理分辨出激光雷达点云数据中的树行信息。

姿态传感器读取的姿态数据使用四元数表示，分别为x、y、z、w，为了方便计算，需要将其转换为欧拉角。四元数到欧拉角的转换方程为

式中R、P、Y分别代表翻滚（Roll）角、俯仰（Pitch）角、航向（Yaw）角，（°）。作业平台在行间初始位置时姿态传感器获取的航向角数据记为yaw1，此后每一帧激光雷达数据对应的航向角数据记为yaw2，激光雷达点云数据需要校正的角度β为

激光雷达获取点云数据格式为Pi(ri,θi)，其中ri表示第i个点到激光雷达的距离，m；θi表示第i个点到初始位置的角度，(°)。分别对当前帧激光雷达每个点的角度减去需要校正的角度，如公式（3），校正后激光雷达点云数据为Pi(ri,φi)

为了方便后续数据处理，将经过姿态传感器校正后的激光雷达点云数据由极坐标Pi(ri,φi)转为笛卡尔坐标Pi(xi,yi)，其转换方程为

式中ir表示一帧激光雷达数据中第i个点到原点的距离，cm；iφ表示第i个点的与原点连线和第1个点与原点连线的夹角，（°）；ix、iy分别表示笛卡尔坐标系中第i个点坐标，cm。

激光雷达点云原始数据由极坐标转为笛卡尔坐标系如图3a，校正后如图3b。当作业平台在初始位置时，树行与车身接近平行，在作业平台行进过程中，航向角发生变化，如图3a所示，树行线有一定的倾斜，通过位姿传感器对激光雷达数据角度校正，如图3b中，树行线接近水平，此时激光雷达数据起始位置角度与第一帧数据起始位置角度相同，以便于后续数据处理分辨出激光雷达点云数据中的树行信息。

1.2.2 最小二乘法拟合树行

由于树林行间距约为2.5 m，初步滤掉iy>2.5 m与yi<-2.5 m的数据，大致保留当前树林行间树行数据信息，并对Pi(xi,yi)中的数据分为yi>0与yi<0两类，yi>0的数据集记为一侧树行激光雷达点云数据，记作L1(xi,yi)，yi<0的数据集记为另一侧树行激光雷达点云数据，记作L2(xi,yi)，设L1与L2数据集线性回归方程为

式中m1，m2分别为L1，L2的斜率；b1，b2分别为L1，L2的截距，cm。

最小二乘法拟合直线求解公式为

式中n为一帧激光雷达数据中点的数量。

将数据集L1(xi,yi)，L2(xi,yi)分别代入公式（6），求出对应的截距 1m，2m与截距b1，b2，将截距与斜率代入公式（5）分别求得最小二乘法拟合的左右两侧树行位置L1，L2，取L1与L2中心线L3，将L3作为分类线，将Pi(xi,yi)中的数据分为两类。中心线L3的方程如公式（7）所示，最小二乘法拟合树行数据如图4所示。

1.2.3 支持向量机生成导航线

树行趋向平行是树林种植的突出特征，而其平行性展现了两侧树行可以进行二分类的特点。利用支持向量机可确定一条与两侧树行间隔最大化的最优分类线，以此最优分类线作为作业平台参考导航线。

由图4中的L3将激光雷达点云数据分为两类，对于点Pi(xi,yi)有其对应的标签zi，对应训练数据为(Pi,zi)。

设分类面函数为wx+b= 0，即对两侧点云数据进行二分类的最优分类面，为使分类面对所有样本正确分类并具备分类间隔，其数据需要符合关系：（l为一帧激光雷达数据中点的数量）。由此可得分类间隔为2/||||w，将构造最优分类线的问题转化为具备约束条件的凸优化问题，且转化为拉格朗日乘子的目标函数[25]为。其中iα为拉格朗日系数，iα的值决定与其相对应各个激光雷达扫描点是否为支持向量点。同时在约束条件的限定下需要把目标函数的凸优化问题转变成拉格朗日的对偶求取最优解的问题。其中α= (α1,α2,… ,αi)[22]

通过对α的求解间接获取w*与b*最优估计

最终求得最优分类线函数，即参考导航线L

通过支持向量机生成理想导航线如图5所示。

1.2.4 最小二乘法与融合算法对比

为提升基于激光雷达作业平台的导航算法通用性，分别在北京市平谷区桃园、四川广安县柑橘园、北京市海淀区东升八家郊野公园北区松树林采集激光雷达点云数据。其中海淀区东升八家郊野公园北区松树林，树木较为高大，作业平台在林间行进时，激光雷达只能扫描到松树主干；北京市平谷区桃园与四川广安县柑橘园则由于矮化果树，作业平台在林间行进时，激光雷达只能扫描到果树冠层，其中柑橘园为柑橘树有间隔种植，桃园桃树为有行无列冠层相连种植。

图6为四川省广安县柑橘园以最小二乘法与支持向量机算法得到的导航线对比。

由于柑橘树冠层枝叶茂密，激光雷达往往只能扫描到外侧点云，扫描数据并不能真实反映出冠层横截面，由此采用最小二乘法拟合树行的结果并不理想，如图6浅色树行部分最小二乘法拟合出的树行拟合线不能作为实际树行位置参考，由此最小二乘法生成的参考导航线算法不适用于扫描冠层的激光雷达林间导航。

本研究融合算法基于支持向量机生成的分类边际线，在于找到分类点云的边界，对于激光雷达只能扫描冠层外侧的情况则不受影响，并且能够在一定程度上排除果树生长情况和冠层大小不一带来的干扰。本研究融合算法生成导航线能尽可能的远离左右两侧障碍物，避免在行间自主导航过程中产生的枝叶的碰撞，同时也能适用于多种林间作业环境。

1.3 横向偏差计算

如图5所示，激光雷达坐标原点，式（12）中参考导航线为L，所以横向偏差为激光雷达到L理想导航线的距离。由于点云数据经过姿态传感器校正，L趋向于水平，斜率较小，采用L距激光雷达的垂直距离作为横向偏差，横向偏差bias计算公式为

1.4 导航控制

作业平台的移动是通过上位机通过串口通讯给下位机发送速度控制指令，下位机输出PWM信号，通过电机驱动器，控制直流无刷电机转速而实现。本研究中用电机驱动器驱动左右两个电机，实现作业平台的差速转向。

本研究采用PD控制方式进行作业平台的导航控制。导航控制的输入量为作业平台的横向偏差kbias。输出kU为左右两侧直流电机速度差值，用于调整作业平台方向。

式中pK为比例系数；Kd为积分系数；Uk为k时刻输出量，m/s；biask为k时刻横向偏差，cm；biask-1为k-1时刻横向偏差，cm。

2 试验与结果分析

2.1 试验设计

为检验基于激光雷达作业平台的导航算法性能，分别在北京市平谷区桃园、四川广安县柑橘园、海淀区东升八家郊野公园北区松树林开展试验。试验场景及场地参数如表2所示。

表2 试验场景及场地参数 Table 2 Test scene and site parameters

激光雷达传感器安装在作业平台的正前方，距离地面垂直高度950 mm（高度可调），水平扫描，扫描角度为360°，扫描半径为15 m，角度分辨率为0.333°。履带作业平台的初始速度为0.5 m/s。

在树林中随机挑选3处行间（行1、行2、行3）作为导航试验环境，每一行重复导航3次，记录激光雷达数据、姿态传感器数据与横向偏差数据。

2.2 结果与分析

北京市平谷区桃园、四川广安县柑橘园、海淀区东升八家郊野公园北区松树林3处场景中激光雷达实时生成参考导航线如图7所示。

以北京市海淀区八家郊野公园松树林试验的场景为例，将作业平台导航过程中的激光雷达点云数据与姿态传感器数据用Cartographer建图并生成行走轨迹，分别在行1、2、3中试验导航效果如图8所示。

表3为试验时的横向偏差统计结果。由表3可知，在9次导航试验中最大横向偏差为107.7 mm，横向偏差绝对值最大为17.8 mm，横向偏差的标准差最大为18.7 mm，9次试验的导航行走轨迹作图如图9所示。其中行2第2次试验中最大横向偏差处，横向偏差为98.9 mm；行3第2次试验中最大横向偏差处，横向偏差为107.7 mm，这两次试验此处横向偏差跳变明显，此处横向偏差过大主要是路面不平，激光雷达随着车身而动，扫到地面或者松树树冠导致。9次试验轨迹沿中线均匀分布，并且能够快速响应并趋向平稳，基本满足行间自主作业要求。

表3 试验横向偏差统计 Table 3 Test lateral deviation statistics

图9中试验轨迹小幅震荡是由于作业平台行进过程中，底盘震动较大，导致架设于底盘之上的激光雷达小幅震荡，从而导致的测量误差。后续改进试验中，或可架设云台进行改进。9次试验轨迹图中大幅度震荡是由于PID控制导航时，微分系数较小，阻尼不够，导致在震荡中没有迅速趋于平稳，同时由于没有采用分段PID控制，导致导航控制不够精细。后续研究中采用分段PID控制，应该能够有所改进。9组试验中横向偏差绝对值的平均值最大为17.8 mm，标准偏差最大为18.7 mm，结合图9的运行轨迹，表明本研究的基于激光雷达的作业平台导航控制系统能够让作业平台横向偏差趋于0以获得稳定的导航行为。

3 结 论

本文针对传统导航方式在果园或林间的郁闭环境中GPS信号弱、光强影响大等问题，提出了基于激光雷达的林间作业平台导航方法，结论如下：

1）设计了最小二乘法与支持向量机组合的树林行间导航算法，在桃园、柑橘园和松树林3种不同环境进行了测试，结果表明该算法均能有效提取得到参考导航路径，可适应多种复杂环境。

2）在松树林开展了平台导航试验，将所设计算法的预测导航线与当前位置的横向偏差作为平台PID控制器的反馈输入，进行平台行进的方向调节，结果表明：在作业平台的速度为0.5 m/s条件下，最大横向偏差为107.7 mm，绝对偏差的平均值不超过17.8 mm，证明该导航方法能满足作业平台在林间自主导航的需求，可为林间地面装备的自主导航作业提供一种技术参考。