果园环境下移动采摘机器人导航路径优化

胡广锐，孔微雨，齐 闯，张 硕，卜令昕，周建国，陈 军

（西北农林科技大学机械与电子工程学院，杨凌 712100）

0 引言

随着智慧农业的不断发展，智能化机器人技术被广泛应用于果园除草、剪枝、套袋、收获等作业环节，有效地减轻了果农的劳动强度，提高了作业效率与质量[1-4]。机器人在果园等非结构化的环境下行间行驶的安全性是研究重点，合理、可靠、安全的导航路径规划方法是保证机器人安全行驶的重要手段之一[5-6]。

导航路径规划根据获取环境信息的完整性分为局部路径规划与全局路径规划[1,7-8]。全局路径规划算法需要环境的整体信息，如田块形状、大小等[9]。局部路径规划只需要机器人附近的环境信息就能完成路径规划[10]。因此，完成局部路径规划可分为两步，一是环境感知，使用传感器获取机器人周围的环境信息；二是规划路径，根据传感器获得的信息，使用优化算法，规划出一条合理、安全的行驶路径。

局部路径规划注重路径的安全性与实时性，其典型算法包括A*算法[11-12]、人工势场法[13-14]、粒子群算法[15-16]等。近年来，随着传感器技术的进步，学者们基于机器视觉[17-20]或激光雷达[21-24]进行了农业机器人路径规划的相关研究。李秀智等[25]通过相机获取田间作物行信息，基于随机采样一致性（Random Sample Consensus, RANSAC）算法剔除离群点，使用最小二乘法（Least Squares Method, LSM）提取导航路径，路径检测率可达93.8%。陈子文等[26]基于自动霍夫（Hough）变换累加阈值法提高了作物行提取的鲁棒性与适用性。激光雷达传感器能精准地获取环境的距离信息，且具有很强的抗干扰能力[27]。赵腾等[28-29]结合视觉与激光雷达的优点，利用Hough变换法规划导航路径。刘沛等[30]使用激光扫描仪获取果树位置，采用LSM规划农机导航路径。艾长胜等[31]使用二维激光雷达获取葡萄园垄行点云，提出了基于支持向量机（Support Vector Machine, SVM）的葡萄园路径规划算法。刘路等[32]采用16线激光雷达获取玉米三维点云信息，引入置信区间结合K-means算法实现对玉米植株聚类，最后使用径向基函数（Radial Basis Function, RBF）路径规划法，使规划路径尽可能避开植株主干。现有研究多以检测单株或作物行为主，规划路径为直线，虽便于机器人控制，但不利于躲避行间果树冠层与行人等障碍物。陈日强等[33]虽通过无人机机载激光雷达成功提取了果树冠层轮廓，但未将冠层信息用于机器人导航路径规划中。

综上，机器人在果园作业时，果树不规则的较大冠层与行人等障碍物是阻碍机器人行驶的隐患之一。传统果园规划路径多为直线，不利于躲避果树冠层等障碍物。本研究以纺锤形苹果园为研究对象，提出改进人工势场法的机器人行间导航路径优化方法，主要步骤包括点云预处理、垄行线提取和初始路径优化。通过移动采摘机器人搭载的固态激光雷达获取果园三维点云数据，采用点云预处理去除果园地面点云，提取果园垄行与果树冠层点云；分别采用RANSAC、LSM和Hough变换3种方法提取垄行线与初始路径；将果树冠层与行人视为障碍物，以初始路径为基础，结合改进人工势场法，优化初始路径，最终规划出安全可靠的导航路径，为移动采摘机器人果园行间自主导航提供技术参考。

1 材料与方法

1.1 果园移动采摘机器人系统组成

移动采摘机器人由机器人底盘、激光雷达（RS-LiDAR-M1，速腾聚创，深圳市速腾聚创科技有限公司，中国）、主控制器（Jetson TX2，英伟达，英伟达有限公司，美国）、机械臂采摘系统组成（图1a）。为便于果园移动采摘机器人具有良好的通行能力，设计机器人底盘后轮由步进电机驱动，轮距为0.85 m，轴距为0.85 m，前轮采用阿克曼转向机构。激光雷达局部坐标系以激光雷达的几何中心为原点O，机器人前进方向为X轴正方向，垂直X轴向左为Y轴正方向，Z轴由右手定则确定（图1b）。激光雷达安装在机器人底盘前端居中位置，距地面高度约为0.5 m。激光雷达采用了微机电系统（Micro-Electro-Mechanical System, MEMS）固态激光雷达技术，MEMS振镜快速震动的同时发射高频率激光束对外界环境进行持续性的扫描，当激光束遇到障碍物，反射光束被雷达接收，经过时间飞行法[34]提供三维点云数据。激光雷达具体参数如表1所示。

表1 激光雷达参数 Table 1 Parameters of LiDAR

本研究使用主控制器Jetson TX2装载 Ubuntu16.04 LTS操作系统，并基于机器人操作系统（Robot Operating System, ROS）与点云库（Point Cloud Library, PCL）进行开发，CPU集群为双核denver2处理器和四核ARM Cortex-A57组成，最高频率为2.0 GHz，8 GB运行内存。

移动采摘机器人电控系统工作原理如图2所示。在果园环境中全球导航卫星系统（Global Navigation Satellite System, GNSS）信号被果树冠层遮挡严重，差分定位效果差，因此移动采摘机器人行间导航使用激光雷达采集环境三维点云信息，三维点云数据由以太网口传输至主控制器进行滤波、去除地面、果树冠层信息获取、提取垄行线、优化初始路径等处理。使用Arduino控制器作为下位机，通过串口接收主控制器控制指令，控制前轮转角与后轮转速，避免移动采摘机器人与果树冠层等障碍物碰撞，保障移动采摘机器人在果园行间导航安全行驶。

1.2 果园三维点云数据获取

本研究于2020年12月11日在陕西省杨凌汇承果业公司果园（34°18′N，108°1′E）进行了果园三维点云获取试验（图3）。苹果树树龄为10年，树形为纺锤形，行距约为4.2 m，株距约为1.6 m（图3a）。果园三维点云分布不均匀，近端点云密集，远端点云逐渐稀疏（图3b）。本研究提取Y轴上-4.0～4.0 m，X轴上1.0～6.0 m和Z轴上-0.5～4.0 m范围内的三维点云数据作为感兴趣区域（Region of Interest, ROI）。因果园地面不平整且杂草多，导致果园三维点云杂点较多，所以需滤除地面点云，减少地面点云对垄行线与初始路径提取的干扰。

1.3 果园三维点云预处理

激光雷达采集的原始果园三维点云每一帧约为7.8万个点，数据量大，为减少计算量、去除噪点与离群点需对原始果园三维点云进行预处理。使用直通滤波算法[32]提取ROI点云；体素下采样滤波算法是使用立方体的形心来表示立方体内的所有点，保留三维点云数据的结构特征，并大大降低三维点云数量，因此本研究采用边长为0.1 m的立方体对ROI点云进行了体素下采样；体素下采样后果园三维点云中仍会有许多噪点与离群点，故采用统计滤波去除噪点与离群点；为降低地面点云对果树冠层轮廓提取的干扰，采用文献[35]提出的地面平面拟合（Ground Plane Fitting, GPF）算法分割地面和非地面点云。

1.4 垄行线与初始路径提取

果园三维点云经预处理后，能较好分辨出果园垄行与果树冠层点云，使用高度阈值将果园三维点云分割成垄行点云和冠层点云，根据果园垄高与经验将高度阈值设置为0.1 m。垄行点云具有明显的行特征，将垄行点云投影至XY平面，使用LSM、Hough变换和RANSAC方法分别提取垄行线与初始路径，比较提取效果。将冠层点云投影至XY平面上，提取冠层点云边界轮廓特征，根据冠层边界轮廓优化导航路径，减低移动采摘机器人在作业中与果树冠层碰撞的几率。

1.4.1 最小二乘法（LSM）

最小二乘法（Least Squares Method, LSM）是一种被普遍使用的数学优化技术，通过最小化误差平方和来寻找点云数据的最佳函数参数[30]。采用LSM拟合移动采摘机器人行驶方向两侧垄行点云提取垄行线。当点云坐标满足F(W)取最小值时，W即为拟合的垄行线方程参数矩阵如式（1）所示：

式中W=[kd]T为垄行线的参数矩阵；k为垄行线的斜率；d为垄行线截距，m；X′n×2=[XI]为点云X轴坐标值矩阵Xn×1=[x1x2…xn]T与单位矩阵I组成的矩阵；Yn×1=[y1y2…yn]T为点云Y坐标值组成的矩阵。对式（1）求导，当X′TX′为正定矩阵时，垄行线方程参数矩阵W如式（2）所示：

1.4.2 霍夫（Hough）变换法

霍夫（Hough）变换是利用点与线的对偶特性，多用于图像中的导航线提取，即将直角坐标系下的直线转化到Hough空间中进行处理[26]。本研究将Hough变换应用于垄行点云提取垄行线中，Hough空间下的直线方程如式（3）所示：

式中ρ为直线到原点的距离，m；θ为直线经过的垂线与X轴之间的夹角，(°)；a、b为点云横、纵坐标值，m。

本研究将两侧的果树行点云数据的直线检测问题转换到Hough空间中，在Hough空间寻找交点值（ρ，θ）完成直线检测任务。在Hough空间中曲线交点很多，存在干扰点。为对干扰点进行过滤，将ρ与θ离散化，形成一个个小区域，点数最多的2个区域的平均值作为最优参数。

1.4.3 随机采样一致性（RANSAC）方法

本研究使用随机采样一致性（Random Sample Consensus, RANSAC）方法提取两侧垄行线，通过随机采样从两侧垄行点云中各选取一个子集建立直线模型，并计算该直线模型的内点数检验直线模型的正确性，不断迭代来获取最优的直线模型，该直线模型即为提取的垄行线。提取垄行线流程如图4所示。

迭代阈值KRANSAC为拟合的关键参数，若KRANSAC值设置过大则耗时过长，设置过小则拟合效果不佳，根据文献[25]中的选取方法，KRANSAC值的选取依据如式（4）所示：

式中α为迭代过程中选取的所有点均为内点的概率，%；ω为从数据中选择一个点为内点的概率，%；N为数据总点数。

初始路径为左右2条垄行线的平均值，初始路径方程如式（5）所示：

式中ko=(kl+kr)/2为初始路径的斜率；bo=(bl+br)/2为初始路径的截距，m；其中，kl与kr分别为左、右两侧垄行线的斜率；bl与br分别为左、右两侧垄行线的截距，m。

1.5 垄行点云噪声处理

本研究从实时性与抗噪能力两个方面，分别对利用LSM、Hough变换和RANSAC 3种方法所提取初始路径的效果进行了评价，得到实时性佳，抗噪能力强的初始路径提取方法。实时性是以提取初始路径的处理时间为评价指标，抗噪能力是初始路径提取方法对两种噪声的抵抗能力。本研究在垄行点云中分别加入了高斯噪声和人为噪声，其中高斯噪声是取消统计滤波处理，并对垄行点云进行均值为0，方差为0.1的高斯噪声处理；人为噪声则是在垄行点云的行间中加入干扰点，将干扰点视为一种人为噪声。

1.6 初始路径优化方法

本研究基于点云法向量方法从冠层点云中提取了边界轮廓数据，首先对投影至XY平面上的冠层点云及其邻近点做直线拟合，选取冠层点云中一点P，其邻近点云集Pk={P0,P1…Pk-1}，设定直线u·x+v·y=c（u2+v2=1），使用最小二乘法拟合直线，向量q(u,v)即为点P的法向量。其次，根据文献[36]可采用邻近点最大夹角大于设定阈值的方法提取冠层轮廓点云，将冠层轮廓点云记为点集U={d0,d1…dn-1}。最后，通过式（6）判断2条垄行线和冠层轮廓数据点的位置关系，去除垄行线外侧的冠层数据，将2条垄行线内侧冠层数据分为左侧点集UL与右侧点集UR。

式中dli与dri分别为冠层轮廓点di(xi,yi)距左、右侧垄行线的距离，m；i为点集索引号，i=0,1,2,…,n-1；n为冠层轮廓点云个数。

人工势场法通过构建虚拟斥力与引力势场，计算势场梯度下降方向规划机器人的行驶路径，但传统人工势场法易陷入局部最优解或震荡[13]。为避免初始路径的优化过程陷入局部最优或震荡，本研究仅建立斥力势场，两侧果树冠层轮廓点云势场相互作用，但方向相反，势能可相互抵消。斥力势场函数E如式（7）所示：

式中E为点云po(xo,yo)在点pj(xj,yj)处产生的势能，J；λ、μ均为势能系数，λ越大势能越大，μ越大势能变化越平缓，经前期预试验取λ=1.5，μ=4.0。

左侧冠层点集左侧点集UL与右侧点集UR在点pj(xj,yj)处的综合势能Ei(pi)如式（8）所示：

式中pr、pl分别为左侧点集UL与右侧点集UR中的点；nr、nl分别为左侧点集UL与右侧点集UR中的点的个数。

在初始路径长度一定的前提下，离散步长S越小，离散路径点就越多，优化路径就越平滑，但耗时也就越长。本研究假设机器人的初始位置是相对安全的，因此将机器人每次规划路径的初始位置的势能值作为势能阈值Ey。迭代阈值KOptimal与优化步长So需满足的约束条件如式（9）所示：

式中D2为果树行距，m；KOptimal为迭代阈值；So为优化步长，m。

在预试验中发现，优化步长So太大会导致路径规划错误，迭代阈值KOptimal过大会导致优化时间过长。因此本研究设置优化步长So为0.005 m，迭代次数KOptimal为420，离散步长S为0.25 m。

初始路径躲避果树较大冠层与行人等障碍物的能力较弱，因此本研究将冠层与行人视为障碍物，采用改进人工势场法对初始路径进行优化处理，以躲避行间果树冠层与行人等障碍物。以S为步长将初始路径离散化为离散点，根据改进人工势场依次优化每一个离散点，最后使用二次B样条曲线拟合优化后的离散点得到优化路径。初始路径的优化具体流程如图5所示。

2 结果与分析

2.1 果园三维点云预处理结果

为探究三维点云预处理方法对移动采摘机器人不同运动状态的适应性，移动采摘机器人在静止状态下和0.6 m/s前进速度下分别采集果园三维点云数据。从2种机器人状态采集的果园三维点云中分别选取了150帧点云进行处理，预处理后的点云个数与滤波算法的处理时间如表2所示。由表2可知，机器人在静、动2种状态下采集的三维点云经预处理后的平均点云数分别为3 308和3 202，预处理的总平均处理时间分别约为0.172 s和0.170 s，点云个数与总平均处理时间都没有明显变化，表明本研究选择的点云预处理方法对机器人不同运动状态具有一定的适用性。通过对比分析直通滤波、下采样滤波和统计滤波的处理时间，其中统计滤波的时间最长，约为132.774×10-3s，约占总预处理时间的76%，不利于算法的实时性。

表2 果园点云预处理结果 Table 2 Pretreatment results of orchard point cloud

果园三维点云经预处理后的可视化结果如图6所示。直通滤波后提取了ROI点云（图6a），点云个数约为5.8万个，下降了约25%；体素化下采样后点云个数约为5 987，保留了原始果园三维点云的结构特征，点云数下降了约90%的（图6b），极大的降低了点云数；统计滤波后点云数约为5 571个，去除约7%的离群点（图6c），地面平面拟合后点云个数约为3 308，去除了约41%的地面点云，成功保留了果园垄行与果树冠层点云信息（图6d）。

2.2 垄行线与初始路径提取结果

本研究对果园点云中分别加入高斯噪声和人为噪声，使用RANSAC、LSM和Hough变换3种方法提取垄行线和初始路径的可视化结果如图7所示。由图7可知，3种方法均可处理垄行点云数据。由图7a可知，RANSAC在未加入噪声、加入高斯噪声和加入人为噪声下提取垄行线结果（以左侧垄行线斜率为例）分别为-0.049，-0.040，-0.047，提取结果未发生明显变化，表明RANSAC对高斯噪声和人为噪声均具有一定的抵抗能力。同理，由图7c可知，Hough变换对高斯噪声和人为噪声也具有一定的抵抗能力。由图7b可知，加入人为噪声后，LSM提取的左侧垄行线斜率为-0.232，明显偏离垄行点云，提取效果下降。LSM考虑了整体点云到提取直线距离最短，当具有较多偏离垄行点云的数据时，会导致提取的垄行线出现偏差。

为量化分析3种方法提取初始路径的效果，将3种提取方法与3种点云噪声处理方式作为因素，提取初始路径的截距（bo，m）、斜率（ko）和处理时间作为响应，从移动采摘机器人在静态下采集果园三维点云数据中选取100帧果园三维点云提取初始路径，将响应值分为未加入噪声组、高斯噪声组和人为噪声组，导入数据统计与分析软件Minitab 19中进行组间方差分析，结果如表3所示。对表3中未加入噪声组分析可知，RANSAC提取初始路径截距（bo=-0.034 m）、斜率（ko=0.006）与LSM的提取结果（bo=-0.026 m，ko=-0.001）没有显著性差异，而与Hough变换提取的结果（bo=-0.212 m，ko=0.073）具有显著性差异。对高斯噪声组分析可知，3种方法对加入高斯噪声后的提取结果均没有显著性差异，表明3种方法均能很好的抵抗高斯噪声的影响。对比分析未加入噪声组、高斯噪声组和人为噪声组可知，RANSAC与Hough变换提取初始路径截距与斜率均无显著性差异，但加入人为噪声后LSM提取初始路径的斜率（ko= -0.046）与未加入噪声的结果（ko=-0.001）具有显著性差异，表明了RANSAC与Hough变换的抗噪能力要优于LSM。

对3种方法的处理时间分析可知，RANSAC在未加入噪声组、高斯噪声组合人为噪声组的处理时间均不具有显著性差异，LSM在未加入噪声组的处理时间（1.222×10-3s）与另外两组均具有显著性差异（P<0.05），Hough变换在未加入噪声组的处理时间（54.552×10-3s）与另外两组均具有显著性差异（P<0.05），表明RANSAC的处理时间相较于另2中方法受噪声影响小。3种方法在每组内的处理时间均具有显著性差异，其中RANSAC的运行时间要明显低于LSM与Hough变换，约为0.147×10-3s，标准差为0.014×10-3s。Hough变换法运行时间最长，约为54.552×10-3s，标准差为13.121×10-3s。RANSAC可通过选取合理的迭代次数获得良好的实时性。因此三种方法的实时性强弱依次为RANSAC、LSM和Hough变换。综合考虑到优化路径的实时性与抗噪能力，本研究采用改进的人工势场法优化的初始路径由RANSAC提取。

2.3 初始路径优化结果

本研究改进了人工势场法，并对RANSAC提取的初始路径进行优化，使优化后的路径尽可能避开果树冠层等障碍物。为探究本研究所提方法的适用性，选取了行人作为另一种障碍物，让试验人员站立果园行间模拟行人障碍物，运用改进人工势场法建立了冠层障碍物与行人障碍物点云的人工势场，并将三维人工势场图投影到XY平面内显示等势线（图8）。由图8可知，距冠层与行人障碍物越近势能绝对值越大，距冠层与行人障碍物越远势能绝对值越小。两侧冠层障碍物轮廓点云产生的势能范围为-30.17～30.01 J（图8a）。两侧冠层与行人障碍物轮廓点云产生的势能范围为-32.80～29.96 J（图8b）。在XY平面内行间均具有一条0等势线，0等势线具有远离两侧高势能区域的特点。

本研究采用冠层与行人障碍物点云距导航路径的最短距离（D，m）来评价路径的安全性。冠层与行人障碍物点云距导航路径的最短距离越大表明该路径越安全。运用改进人工势场对初始路径进行优化，优化结果如图9所示。由图9a可知，仅有冠层障碍物时，优化后的路径具有避开果树冠层的趋势。改进人工势场法优化后的路径将冠层障碍物点云距初始路径的最短距离由0.705 m提高至0.907 m，提高了28.65%。试验人员站立在机器人前方约2.5 m处模拟行人障碍物，导航路径优化结果如图9b所示。改进人工势场法优化后的路径将行人障碍物点云距初始路径的最短距离由0.156 m提高至0.863 m，表明优化后的路径具有绕开行人障碍物的能力，提高了导航路径的安全性。

从采集的果园点云中根据有无行人障碍物各选取3帧作为样本，使用RANSAC提取的初始路径、传统人工势场法优化后的路径和改进人工势场法优化后的路径作为导航路径，计算冠层与行人障碍物点云距导航路径的最短距离，结果如表4所示。由表4中仅有冠层障碍物的第3帧可知，障碍物点云距RANSAC提取的路径与传统人工势场法优化路径的最短距离分别为0.705、0.894 m，传统人工势场法将此最短距离提高了26.81%；由第二帧可知，障碍物点云距RANSAC提取的路径与改进人工势场法优化路径的最短距离分别为0.501、0.856 m，改进人工势场法将此最短距离提高了70.86%，而传统人工势场法陷入了震荡；有行人障碍物时，改进人工势场法优化后的路径更具优势，行人障碍物点云距初始路径的最短距离由0.324 m提高至0.778 m。优化后的路径比RANSAC提取的初始路径距点云的最短距离更大，表明了经优化后的路径更安全。传统人工势场法优化后的路径同样比RANSAC提取的初始路径更安全，但传统人工势场法易陷入震荡，无法继续求解。使用改进人工势场法的机器人行间导航路径优化方法对100帧果园点云数据进行优化，平均处理时间为0.059 s，标准差为0.007 s，表明该方法能满足实时性要求。

表4 障碍物点云距3种导航路径的最短距离 Table 4 Shortest distance between three navigation paths and obstacle point cloud

3 结 论

本研究在果园垄行线与果树冠层点云的提取方法研究基础上，提出了一种改进人工势场法的机器人行间导航路径优化方法，为避免机器人与果树冠层等障碍物碰撞提供了一种解决方法，提高了移动采摘机器人行间行驶的安全性。

1）从实时性与抗噪能力2个方面评价了最小二乘法（Least Squares Method, LSM）、霍夫（Hough）变换和随机采样一致性（Random Sample Consensus, RANSAC）3种方法提取垄行线与初始路径的性能。在本研究的试验条件下对处理时间、初始路径截距与斜率进行了组间方差分析，试验结果表明RANSAC运行时间最优，约为0.147×10-3s、标准差为0.014×10-3s。RANSAC与Hough变换的抗噪能力要优于LSM。

2）通过舍弃引力势场，改进了人工势场法，将果树冠层与行人均视为障碍物，建立了机器人行驶两侧果树冠层与行人障碍物轮廓点云势场，对初始路径进行了优化, 避免了传统人工势场法容易陷入震荡的问题。优化结果表明，改进后的导航路径不仅能躲避果树冠层，而且能够绕开行人障碍物。改进人工势场法优化后的路径可将障碍物点云距导航路径的最短距离由0.156 m提高至0.863 m，且优化处理平均耗时0.059 s，标准差为0.007 s，有效保障了规划路径的安全性与实时性。