-
开放科学（资源服务）标识码（OSID）：
-
目前中国果园面积与水果产量均处于世界领先水平，水果产业成为推动中国农业经济发展的重要基石之一[1]。然而当前果园的自动化程度较低，劳动密集型作业模式仍然广泛存在。为满足现代高效农业的发展需求，引入果园移动机器人成为现代农业技术革新的一个关键方向[2]。在果园作业中，移动机器人依赖于高效的自主导航方案来有效覆盖相关作业区域。其中，路径规划算法作为机器人自主导航的核心技术之一，对提高机器人在果园内的导航性能具有重要影响[3-4]。
目前常见的路径规划算法主要有A*算法[5]、RRT(Rapidly-Exploring Random Trees)算法[6]、蚁群算法[7]等。在众多路径规划算法中，A*算法因其高效的全局搜索能力和生成最优路径的特性，在移动机器人领域得到广泛应用[8]。然而，该算法作为贪心算法力求探索最优路径，导致无法保证路径在实际应用中的安全性。此外，A*算法在路径规划中常常因生成过多的转弯节点，导致路径不够平滑，从而影响机器人的流畅移动。文献[9]通过引入指导线优化启发式函数，通过关键点辅助障碍物避让，以及变步长策略以提升A*算法的静态避障能力。文献[10]提出一种基于双向矩形扩展A*算法的全局路径规划方法，有效减少路径冗余节点，但生成的路径过于贴近障碍物。文献[11]通过引入安全距离因子和路径优化策略以增强A*算法的安全性和实时避障能力，但在剔除冗余节点时，未充分考虑处理后的路径是否会再次接近障碍物。相比于其他算法，A*算法的灵活性与可扩展性使其能够适应果园中的不规则障碍物，并且可以与其他算法结合解决更复杂的问题。为了能够适应果园场景，文献[12]提出一种针对果园环境的改进A*算法，有效降低算法的贪心程度并增强局部避障能力，但未考虑路径的平滑性，不足以规划出平滑全局路径。文献[13]通过引入施药区域代价函数和引导点优化启发式函数，辅助算法规划出行间中线路径，但未考虑果树外轮廓的不规则性，路径易发生偏移。
根据果园作业场景，本文提出一种基于RANSAC(Random Sample Consensus)算法与改进A*算法的移动机器人路径规划方案，以解决传统A*算法搜索时间较长、路径安全性低、冗余节点多、不够平滑以及果树行间作业精度不高等问题。该方法首先利用RANSAC算法拟合出树行直线从而提取果树行间中线，并作为引导路径为A*算法提供最优路径参考。之后，在A*算法中加入中线栅格缩减策略，辅助算法优先规划中线路径；引入动态距离权重，减少搜索时间；加入排斥力场函数，确保路径与障碍物保持安全距离；采用安全距离阈值剔除冗余节点方法，避免路径优化过程中安全性二次降低；使用三次均匀B样条曲线对路径进行平滑处理，提高路径平滑性。
HTML
-
移动机器人在果园内进行路径规划时，需综合考虑园区的实际布局和作业需求。当前需解决两类问题：一是必须避免与果树过于接近，以免发生碰撞；二是要提供高质量的行间作业路径，以满足两侧信息采集或农事作业需求[14]。这要求路径规划既要考虑各果树的位置和冠层枝叶空间分布情况，又要最大化作业效率，确保果园的每一部分都得到妥善管理。
图 1(a)为标准化的柑橘果园，机器人可在树行之间行驶。为便于后续对路径规划算法进行试验研究，本文根据部分果树的位置分布以及冠层大致形状，建立了如图 1(b)所示的70×70栅格果园地图。地图中，单个不规则绿色栅格代表一株果树冠层的俯视角投影，共有6列，每列9株果树，其余白色栅格为机器人可移动区域。本次设定栅格单位为0.5 m，果树冠层直径为2~3 m，株距为4 m，行距为5 m。每个树行之间均为作业区域，机器人需通过路径规划覆盖所有区域，并且要在保证路径安全性的同时满足作业需求。
-
本文算法的整体实现框架如图 2所示，主要方法包括果树特征点提取、RANSAC算法、改进A*算法路径规划3个部分。具体流程如下：首先，栅格化果园地图，提取单株果树冠层外轮廓点和中心点等特征点；然后，利用RANSAC算法拟合树行直线并提取行间中线，为路径规划提供最优路径参考。最后，为解决传统路径规划算法搜索效率低、安全性不足、冗余节点多、路径平滑度差等局限性，引入中线栅格节点代价缩减机制、预估函数优化策略、排斥力场函数、冗余节点剔除方法以及B样条曲线平滑技术等优化策略改进A*算法，进行全局路径规划。
1.1. 果园地图建立
1.2. 算法框架
-
为保证机器人在果树间安全行驶以及满足农事任务作业需求，需在行驶时确保机器人与两侧树行尽量保持等距。传统A*算法无法自生成这种精准路径，因此需要借助其他方法辅助规划。在果园路径规划研究中，提取行间中线能够有效提高机器人导航精度和作业覆盖度[15]。将该方法与A*算法结合，可为其提供最优路径参考，同时减少路径计算的复杂度以提高计算效率。为此，本文将采用RANSAC算法，以果树冠层的外轮廓及中心点作为特征点，拟合树行直线并提取中线作为引导路径。
-
行间中线提取的精准性依赖于拟合的树行直线，而直线拟合的质量则取决于所选取的特征点。如果选取不当，可能会导致采样点偏离真实分布，降低拟合精度。对于单株果树而言，其外轮廓点及中心点能够准确反映该果树在地图中的具体位置。若将这些特征点作为树行直线拟合的参考特征点，将有效提升直线拟合的精准性，进而提高提取出的行间中线精度[16]。为获取这些特征点，首先需要确定每棵果树对应的栅格坐标集合，对此，使用BFS (Breadth First Search)[17]算法遍历并记录所有果树占据的栅格点，获取每个果树的点集，并将所有坐标点集添加至合集O中。
在特征点的提取过程中，噪声点的存在可能会对后续直线拟合的精度产生不利影响，进而降低所提取中线的准确性[18]。因此，必须对噪声点进行过滤。首先，针对果树边缘点的提取，可以通过检查占据点的邻域内是否存在非占据点来判断其是否为边缘点。其次，为避免将离散的噪声点误判为边缘点，采用一种占据点邻域判别方法。具体而言，若某个占据点的8邻域内存在的占据点数量小于等于1，则将其判定为噪声点；反之，则认为该点为边缘点Pei(xei，yei)。至于果树中心点，可将单棵果树的边缘占据点合集的横纵坐标分别进行累加，将总和分别除以占据点个数m即得中心点坐标Pcj(xcj，ycj)，具体如式(3)所示。最后将所有特征点放入如式(4)所示合集Ps中，以便于进行后续的直线拟合分析。
-
对于直线拟合，常用的算法有RANSAC算法和最小二乘法(Least Squares Method，LSM)。LSM通过最小化误差的平方和来寻找数据的最佳函数匹配[19]，但易受噪声点干扰。RANSAC算法能通过随机选择最小样本集来拟合模型，并测试其他数据点的一致性以识别和排除异常值，最终得到最佳拟合模型[20]。因此本文采用RANSAC算法进行树行直线拟合并同时提取出行间中线。
RANSAC算法无法直接拟合得到各树行直线，因此需对特征点进行处理，将其划分成不同树行区域。然而，由于不同果园的果树定植行向不同，且果树的外形轮廓也存在差异，固定的区间划分方法难以适应。为解决该问题，本文提出一种基于中线坐标的区间分类方案。在该方案中，首先需对果园地图进行校正，以便于后续的坐标分析计算。采用主成分分析(Principal Components Analysis，PCA)计算出果园地图的主方向[21]，确定其与坐标轴的夹角，并对地图进行旋转校正，使所有树行与y轴方向保持一致。随后，获取所有果树中心点的x坐标值，统计与其相近的中心点总数，并计算平均值，从而得到每一个树行的拟定中心横坐标xmid。接着，以xmid为基准，分别向左右以1为单位步长进行扩展，并在每次扩展时检查该横坐标下是否存在特征点。若某一坐标位置上不再存在特征点，则将该位置作为当前树行的边界。最终，将边界范围内的特征点进行储存，从而获得每个树行的特征点集合。
在完成树行特征点划分后，即可进行树行直线拟合，具体流程如下：首先，将划分的树行按照顺序进行分组；此后，算法将进行多次迭代，每次迭代都会从每组中随机取出两点拟合出一条直线；再计算每组所有特征点到该直线的距离，当某个点到直线的距离小于距离阈值时，认定该点为内点，其余为外点；每次迭代完成后，记录取出的两个点以及内点数量；随着迭代的进行，如果当前迭代的内点数量超过之前的最佳记录，则更新最佳直线的端点和内点数；最终，经过最大迭代次数后，选择内点数量最多的迭代结果作为最佳树行直线Lcoli。树行直线方程如下[22]：
式中：kcoli为树行直线斜率；bcoli为树行直线截距；(x1，y1)和(x2，y2)为算法选择的最佳拟合点。得到各组的树行拟合直线后，将相邻两树行直线相加取平均值即得每个果树行间的中线方程Lmidi，如式(8)：
为测试RANSAC算法鲁棒性，在图 1(b)果园栅格地图中添加干扰点，以模拟果树冠层变化及偏移情况，并同时运行LSM与RANSAC算法进行对比实验，结果如图 3所示。由图 3可知，LSM算法因将干扰点也纳入直线拟合运算导致最终得到的树行直线发生偏斜，进而引起中线提取的偏移。而RANSAC算法能克服植株冠层不规则性与偏移带来的影响，筛选最佳特征点进行直线拟合，表现出更强的鲁棒性。
2.1. 果树拟合特征点获取
2.2. RANSAC算法拟合树行直线及中线提取
-
A*算法是基于Dijkstra算法改进而来的启发式算法，可以求解静态环境中的最优路径[23]。算法通过维护开放列表(open list)与关闭列表(close list)来管理待评估的节点以及已评估的节点。经过多次循环迭代不断寻找开放列表中的最小代价节点进行扩展直至终点，此时关闭列表将记录所有代价最小的节点，并最终将所有代价最小节点依次相连即得最优路径。节点评估采用8邻域搜索方案，以父节点为中心，搜索8个方向子节点代价，每个子节点代价采用如式(9)所示的评估函数进行评估。
式中：n为当前节点；g(n)为起始点到所在点的当前路径代价函数；h(n)为所在点到目标点的预估路径代价函数。
在算法搜索过程中，节点n若向4个斜方向移动一格，g(n)值将增加
$ \sqrt{2} $ ，若向其余4个正方向移动一格，g(n)值将增加1。h(n)采用曼哈顿距离进行代价计算，其公式如式(10)所示。式中：abs为绝对值函数；Ex和Ey分别为终点横、纵坐标；nx和ny分别为当前节点横、纵坐标。
目前传统A*算法存在搜索时间长、安全性低、冗余节点多、平滑性不足等诸多问题[24]，因此需要对评估函数进行针对性优化，使算法能够规划出满足果园机器人作业需求的高效路径。
-
为了引导A*算法优先将提取的中线作为最终路径节点，将采取一种代价缩减策略。A*算法的评估函数只将最小代价节点纳入最终路径节点中，对此，通过引入比例因子来调整中线覆盖的栅格节点总代价，赋予这些节点较低的代价值。该方法首先获取中线栅格节点坐标并储存于合集Mi中，在A*算法运行过程中将不断判断当前搜索节点坐标是否与中线节点坐标匹配，若成功匹配则通过如式(12)所示的改进评估函数进行搜索，其中α为比例因子，该因子能大幅度缩减节点的总代价。由于总代价受地图尺寸的影响较大，所以将α值设为栅格地图最大横纵坐标之和以动态适应不同的地图。改进后的评估函数在搜索到这类中线节点时，其总代价将显著低于周围其他节点，使得A*算法倾向于选择这些代价较低的中线节点，从而在路径规划时优先生成中线路径。
-
在A*算法中，代价函数g(n)和h(n)决定了算法的搜索效率[25]。增加h(n)权重可以加速搜索过程，使算法更快地向目标方向移动，但过大的权重会退化为广度优先算法，算法可能会陷入局部最优，导致搜索路径变长。反之，减少h(n)权重会搜索更多的节点，确保找到最短路径，但过小的权重会让其退化成深度优先算法，扩大搜索空间，从而消耗更多的时间[26]。因此，选择合适的权重系数可以在保证路径质量的同时减少搜索时间。对此，提出一种动态距离权重系数Wd以修改预估代价函数h(n)的权重。如式(13)所示，该系数通过计算当前节点(nx，ny)与目标点(Ex，Ey)的距离对权重系数进行动态调控。离目标点越远，Wd的值越大，h(n)权重随之增加，此时算法将更积极地探索可能的路径，从而快速缩小搜索范围，减少不必要的节点扩展。随着节点逐渐接近目标点，Wd的值逐渐减小，h(n)的权重降低，算法会更加注重路径的实际代价g(n)，以避免过度依赖不准确的预估方向，从而确保算法能够找到实际的最短路径。通过这种动态调整机制能够更好地平衡当前代价g(n)与预估代价h(n)之间的关系，在保证路径质量的同时，减少无意义的搜索过程，提高算法搜索效率。优化后的评估函数如式(14)所示。
-
A*算法作为一种贪心算法倾向于搜索最优路径，导致部分路径会过于靠近甚至斜穿障碍物(图 4)。在实际果园环境中，机器人沿此种路径行进会过于贴近果树，可能产生碰撞，存在安全风险，因而需对算法进行相关改进。
人工势场法因其具有实时响应和易于实现等优势，在机器人路径规划领域得到广泛应用[27]。该方法通过构建虚拟力场来模拟引力与斥力，指导机器人在避开障碍物的同时到达目标位置。本文提出一种由人工势场法启发而来的排斥力场函数R(n)，并将其引入A*算法的评估函数中来调控路径与障碍物的距离。函数的核心原理是当算法遍历子节点时，若该节点与附近障碍物的距离达到阈值dth，排斥力场函数会令其代价增大，距离越近或障碍物数量越多，代价值会成倍增加，算法将不再考虑这些高代价节点。
排斥力场函数与评估函数分别如式(15)和式(16)：
式中：o为阈值范围内的障碍物节点；dn，o为当前节点到各障碍物的欧几里得距离；dth为节点与障碍物之间的阈值距离；kr为排斥力系数。
-
改进算法优化后的路径存在拐点过多、易产生锯齿效应的问题，徒增机器人工作时间与能耗。目前，大量研究都采用节点间隔相连的方法来剔除冗余节点[28-29]。然而，这些方法只考虑到节点连线是否会接触障碍物而忽视了与障碍物的距离，这会导致剔除路径离障碍物过近，路径安全性二次降低。针对此问题，本文提出一种结合安全距离阈值的冗余节点剔除方案。
本方案在连线检测时除考虑是否会接触障碍物，还会考虑与周围障碍物的距离阈值是否满足设定条件，该阈值根据机器人尺寸及最小安全行驶距离设定。通过图 5中的改进冗余节点剔除过程进行说明。图中P0-P13为路径节点；圆圈ds为安全距离阈值的范围；虚线为原始路径；实线为使用当前方案优化后的路径；点划线为其中一条不满足安全距离条件的危险路径。
首先将所有路径节点都放入合集Pi中，从始发点P0开始，以P0、P1、P2 3个节点为一组，连接P0与P2，因连线间未接触障碍物，且该条线与最近障碍物的直线距离大于或等于设定的距离阈值，符合节点剔除条件，将P1从合集中剔除。接着将P0、P2、P3列为新的一组，重复上述判断。当P0与P4相连时，其连线因穿越障碍物而不满足条件，此时P0、P3连线达成最优路径。此后新一组的始发点将从P3节点开始，再次重复节点剔除程序。当所有路径节点都进行连线判断后，合集Pi中剩余的节点即为最优安全节点。
-
尽管经过剔除优化的路径在拐点数量上有所减少，但生成的路径仍由多段直线组成的折线构成，可能导致机器人在果园中移动时需要频繁进行转向或加减速操作，降低导航过程的平稳性。对此，可通过引入三次均匀B样条曲线来对路径进行平滑处理。
三次均匀B样条曲线公式与基函数公式[30]如式(17)与式(18)所示：
式中：t为参数变量；S(t)为B样条曲线函数；Ni，3(t)为B样条基函数；Pi为控制点坐标集。结合式(17)与式(18)可得到最终的曲线方程：
在应用过程中，首先将优化生成的路径点视为初始控制点，并将这些离散的路径控制点输入到三次均匀B样条曲线公式中，随后利用基函数来建立曲线模型，逐步进行路径平滑拟合。
3.1. A*算法原理
3.2. 中线栅格节点代价缩减策略
3.3. 预估函数优化
3.4. 排斥力场函数
3.5. 安全距离阈值冗余节点剔除方法
3.6. 路径平滑
-
为验证本文所提出的改进A*算法的高效性及安全性，在一张50×50的静态栅格地图中，与传统A*算法、Informed-RRT*算法[31]和文献[32]A*演化算法进行对比，设定起点(2，49)，终点(49，2)。仿真结果如图 6所示。传统A*算法与Informed-RRT*算法规划的全局路径在部分区域过于靠近障碍物，危险系数太高。文献[32]A*演化算法路径能与障碍物保持一定相对距离，但未剔除冗余节点，转折过多。本文改进的A*算法规划的全局路径相较于其他三者更安全、更平滑，能更好满足实际应用需求。
由上述试验得到表 1中的算法对比数据，从表中可知本文改进A*算法耗时相较于传统A*算法减少49.75%，比Informed-RRT*算法减少68.90%，比文献[32]A*演化算法减少14.29%。路径长度虽略大于其余算法，但安全性及平滑性更优。此外，搜索节点数比传统A*算法与文献[32]改进A*算法分别减少49.96%、7.62%，路径总转折角度相对传统A*算法、Informed-RRT*算法、文献[32]A*演化算法分别减少21.40%、23.15%、17.07%。综上，本文改进A*算法能规划出更高质量、更安全、更平滑的路径，并且在计算效率上也有一定提升。
-
为验证本文RANSAC与改进A*结合算法的优越性，对比传统A*算法、文献[32]A*演化算法及本文算法在图 1(b)地图中的表现，设定起点(4，66)，终点(64，4)，且为将路径覆盖至所有作业区域，设置6个导航点(G1至G6)进行多点导航，仿真结果如图 7所示。传统A*算法规划路径过于贴近果树，无法满足机器人的安全作业需求。文献[32]A*演化算法安全性与平滑性有所提升，但部分区域转折节点较多，可能导致机器人频繁转向，降低行驶效率。经RANSAC算法获取行间中线后，本文算法规划的路径质量更高，能有效提升机器人在果树行间作业时的安全性和作业精度。
由表 2试验数据可知，本文算法相比传统A*算法与文献[32]A*演化算法分别在计算时间上减少56.80%、27.84%。尽管本文算法路径长度相对其余算法有所增加，但路径更为安全合理，且始终能保持在最佳中线上。在搜索节点数对比中，比传统A*算法减少55.84%，比文献[32]A*演化算法减少31.74%。总转折角度比传统A*算法与文献[32]A*演化算法分别减少27.76%与7.20%。表 2中的横向偏移为同行两果树中心点距两者最近行间路径横坐标距离的差值，本文算法无论是最大横向偏移还是平均横向偏移均小于其余算法，可知本文算法所得到的中线路径精度更高，能有效提高机器人作业精度。综合来看，本文算法能够规划出高质量的中线路径，确保所有路径的安全性和平滑性，且算法耗时也相对较少。
-
为验证本文算法在实际应用中的可行性，在ROS(Robot Operating System)操作系统中，结合Gazebo虚拟仿真平台，并依据图 1中的柑橘果园局部场景，构建果园环境模型。该仿真果园共有5列果树，每列12棵。后续在RVIZ(ROS Visualization Tool)可视化平台中，使用搭载激光雷达的仿真差速机器人运行Gmapping算法建立二维仿真果园地图。在该地图中分别运行传统A*算法、文献[32]A*演化算法以及本文算法以规划全局路径，并同时使用纯轨迹跟踪算法进行跟踪试验。机器人长宽高为1.2×0.6×0.4 m，最大线速度设为0.7 m/s，最大角速度设为1.57 rad/s。
设定起点S与导航点G1至G6进行多点导航，结果如图 8所示，图中红线为各算法规划的全局路径，蓝线为机器人实际跟踪轨迹。从图 8中可看出，传统A*算法转折节点过多，安全性得不到有效保证。文献[32]A*演化算法因追求最短路径导致树行端头路径曲率过大，且行间路径容易斜向偏移。相比之下，本文算法路径更安全平滑，且以行间中线作为引导路径，有利于提高行间作业精度。
由图 9和表 3中各算法路径跟踪的横向偏差对比分析可知，本文算法在最大横向偏差、平均横向偏差和标准差3个指标上均优于其他算法，表明本文算法所规划路径更为连续平滑，机器人在行驶过程中更加平稳，路径跟踪效果最佳。综合来看，本文算法能够有效满足机器人在果园场景中的作业需求。
4.1. 改进A*算法对比试验
4.2. 果园仿真环境下算法对比试验
4.3. 模拟果园路径跟踪试验
-
本文针对果园移动机器人路径规划问题，提出一种基于RANSAC算法与改进A*算法的路径规划方案。该方案采用RANSAC算法拟合树行直线并提取出最优果树行间中线，并采用中线栅格节点代价缩减策略，为A*算法提供精准路径引导；针对A*算法预估代价函数，添加动态权重系数以减少搜索时间；引入排斥力场函数提升路径规划的安全性；提出距离阈值冗余节点剔除方法减少冗余节点；利用三次均匀B样条曲线对路径进行平滑化处理。不同路径规划算法对比试验结果表明，本文的改进A*算法在路径规划效率、平滑度以及安全性方面均具有较大优势。在栅格仿真果园对比试验中，本文算法可以规划更高质量的果树行间路径。在最后模拟果园路径跟踪试验中，本文算法各项横向偏差指标最小，能够满足机器人在果园中的作业需求。
DownLoad: