农业机器人是指能够进行自主路径规划和完成特定农业任务的智能化机器人，被广泛应用于采摘、运输、检测等方面^[1]。路径规划作为农业机器人自主导航必不可少的一部分，受到研究人员的广泛关注，其目标是找到一个从初始状态到目标状态始终满足特定性能指标的无碰撞的连续解^[2-4]。路径成本和解的质量是设计路径规划算法时需要考虑的重要因素。

在各种路径规划算法中，基于采样点的运动规划算法由于适用于大型环境和具有较好的搜索速度和避障碍能力而广受欢迎。快速探索随机树(Rapidly-exploring random trees, RRT)^[5]和概率路线图(Probabilistic road map, PRM)^[6]是2种常见的采样点运动规划算法。尽管它们在解决路径规划问题上具有代表性，但是试验证明它们不能保证获得最优解，即无法提供最优路径^[7]。研究人员在RRT和PRM的基础上提出了RRT*和PRM*算法保证渐进最优性，但它们收敛速度缓慢，都需要大量样本才能找到最优解^[7]。此外，Informed RRT*和探索利用树(Exploration and exploitation trees, EET)通过构建通道避免探索整个空间从而提高计算效率^[8-9]。虽然这些方法能提高计算效率，但并不适用于所有环境，例如复杂农业环境。其中，快速行进树(Fast marching trees, FMT*)^[10]方法借鉴Fast marching方法获得最优路径，兼具RRT*和PRM*的优点，更加快速高效。然而在探索空间效率方面，FMT*存在冗余探索的问题，为进一步提高FMT*探索效率，Starek等^[11]设计了双向FMT*方法(Bidirectional FMT*, BFMT*)从初始和目标同时搜索样本，以提高计算效率。Xu等^[12]在FMT*应用了Informed-RRT*思想，构建椭球区域IAFMT* (Informed anytime fast marching trees)约束探索空间，避免对整个空间探索；Wu等^[13]采用(Generalized voronoi graph, GVG)建立等距路线图，找到初始解并构建安全隧道(Secure tunnel FMT*, ST-FMT*)，避免整个空间探索。但是，不恰当的预处理和劣质的路径解均会造成IAFMT*和ST-FMT*算法区域划分不合理和探索低下的现象，在极端状态下甚至不能收敛于最优解。Ichter等^[14]利用GPU的并行计算能力来处理FMT*的扩展过程，但此方法对硬件性能要求高。上述方法大多从预处理和局部采样方面进行改进，启发式函数利用特定的环境信息引导规划算法进行探索。 Gao等^[15]利用A*算法指导算法探索，在Bidirectional FMT*的基础上形成双向启发式算法(Heuristic bidirectional FMT*, HBFMT*)，提高计算效率。但是传统的启发函数只利用了部分环境信息，通过改进启发函数来提升算法的探索性能成为人们的关注点。

由于FMT* 对规划空间的过度探索，有必要对FMT*进行探索引导，解决冗余探索问题。受自适应人工势场快速探索随机树(Adaptive artificial potential field RRT*, AAPF-RRT*)算法^[16]的启发，本文提出了基于改进人工势场的快速行进树方法(Artificial potential field FMT*, APF-FMT*)。人工势场法是由Khatib^[17]提出的一种虚拟力场的方法，该方法简单有效，但存在以下问题：当目标点不可达和当移动机器人距离目标点较远时，引力会变得过大，容易陷入局部最优。本文为解决引力过大的问题，引入了相对距离，根据不同距离，自适应调节引力大小，并融合FMT*算法，解决人工势场法目标不可达的问题。总体来说，本文对于给定的农业环境信息，通过改进人工势场法对环境建立势场，通过样本势场梯度的变化来引导样本向目标方向探索，减少对规划空间的过度探索，有助于保证在具有良好路径质量的同时提高计算效率。最后通过仿真对比，验证所提出的APF-FMT*算法的有效性。

1.   算法构建与分析

FMT*参考Fast marching算法对整个空间的所有样本进行动态递归扩展，具有良好的性能，但存在以下问题：1)当样本数量较少时，由图1a可以看出，FMT*难以获得高质量的解；2)FMT*在返回解之前几乎都是探索整个空间，造成资源的大量浪费，无法在嵌入式平台使用；3)如图1b所示，需要大量的样本才能确保获得高质量的解。因此，通过启发函数引导算法向有利规划的方向探索，将极大提升规划效率。

图  1  FMT*规划结果

红色圆心表示起始点，绿色圆心表示目标点，红色曲线为最终路径

Figure  1.  FMT* planning result

The red circle represents the starting point, the green circle represents the goal point, and the red curve represents the final path

下载: 全尺寸图片 幻灯片

为解决上述问题，本文提出了一种基于改进人工势场法的快速行进树(APF-FMT*)方法。

1.1   改进人工势场法

传统的人工势场法的引力势场函数为：

式中：${u}_{{\rm{att}}}\left(q\right)$为引力势场函数；$ \varepsilon $为引力增益系数，且$ \varepsilon $>0；q为当前移动机器人的位置；${q}_{{\rm{goal}}}$为目标点的位置；$\rho (q,{q}_{{\rm{goal}}})$为当前移动机器人到目标点的欧氏距离。由式(1)可知，当移动机器人与目标点距离过远时，即$\rho (q,{q}_{{\rm{goal}}})$过大时，${u}_{{\rm{att}}}\left(q\right)$过大，导致所受的引力过大，容易陷入局部最优。为解决引力过大的问题，本文引入相对距离将引力场划分为恒力场和变力场，改进的引力场函数如下所示：

式中：${U}_{{\rm{att}}}\left(q\right)$为改进的引力场函数；$ {d}_{1} $和$ {d}_{2} $为常量，当$\rho (q,{q}_{{\rm{goal}}})< {d}_{1}$时，$ {U}_{{\rm{att}}}\left(q\right) $与${u}_{{\rm{att}}}\left(q\right)$相同；当${d}_{1} \leqslant \rho (q,{q}_{{\rm{goal}}})< {d}_{2}$时，引入 $ \alpha $ 作为第2个引力增益系数，其中$ \alpha < \varepsilon $；当$\rho (q,{q}_{{\rm{goal}}}) \geqslant{ d}_{2}$时，${U}_{{\rm{att}}}\left(q\right)$为定值。通过设定$ {d}_{1} $和$ {d}_{2} $，实现引力场随着$\rho (q,{q}_{{\rm{goal}}})$的变化进行切换。从图2可以观察到，在传统引力场中，当起始点和目标点的距离过远时，引力过大，机器人难以脱离引力场的束缚并陷入局部最优，而改进后的引力场能够根据距离调节引力，从而避免这一问题的产生，机器人能够更有效地规划出路径。

图  2  人工势场法引力场

Figure  2.  Attractive field of artificial potential field method

下载: 全尺寸图片 幻灯片

改进的人工势场法的斥力场函数为：

式中：${U}_{{\rm{rep}}}\left(q\right)$为斥力场函数；$ \eta $为斥力系数；$ \rho \left(q\right) $为移动机器人距离障碍物的距离；$ {\rho }_{0} $为常量，代表障碍物影响的范围。由图3斥力场可知，当移动机器人与障碍物距离过近，即$ \rho \left(q\right) $变小时，${U}_{{\rm{rep}}}\left(q\right)$斥力变大，机器人有远离障碍物趋势。

图  3  人工势场法斥力场

Figure  3.  Repulsive field of artificial potential field method

下载: 全尺寸图片 幻灯片

1.2   APF-FMT*算法

本文基于FMT*算法的局限性，提出了改进的APF-FMT*算法。APF-FMT*整体算法流程如图4所示。首先，在给定的环境地图上建立整体人工势场，并随机生成采样点集V，将初始点${{X}}_{\text{init}}$添加进已遍历点集${{V}}_{\text{open}}$中，其他点添加进未遍历点集${{V}}_{\mathrm{u}\mathrm{n}\mathrm{v}\mathrm{i}\mathrm{s}\mathrm{i}\mathrm{t}\mathrm{e}\mathrm{d}}$中，初始化生成树${{X}}_{\text{tree}}$，然后从${{V}}_{\text{open}}$中选择成本F（X）最小的点作为节点Z，如图5a所示；接下来找出以节点Z为中心、半径为${{r}}_{{n}}$的圆内所有在${{V}}_{\mathrm{u}\mathrm{n}\mathrm{v}\mathrm{i}\mathrm{s}\mathrm{i}\mathrm{t}\mathrm{e}\mathrm{d}}$集合内的邻近点集X，如图5b所示；循环遍历邻近点集X每一个邻近点x，并重新计算在${{V}}_{\text{open}}$集合内连接成本最小的父节点，并进行碰撞检查，如图5c所示；当所有邻近点x都被探索，将节点Z加入到确认节点集${{V}}_{\mathrm{c}\mathrm{l}\mathrm{o}\mathrm{s}\mathrm{e}}$中，邻近点集X添加进${{V}}_{\text{open}}$中，如图5d所示；循环以上步骤，直至${{V}}_{\text{open}}$为空或节点Z为目标点。

图  4  APF-FMT*流程图

Figure  4.  APF-FMT* flow chart

下载: 全尺寸图片 幻灯片

图  5  APF-FMT*节点扩展过程

${{X}}_{\text{init}}$为初始点；${{X}}_{\mathrm{g}\mathrm{o}\mathrm{a}\mathrm{l}}$为目标点；${{V}}_{\text{open}}$为已遍历点集；${{V}}_{\mathrm{u}\mathrm{n}\mathrm{v}\mathrm{i}\mathrm{s}\mathrm{i}\mathrm{t}\mathrm{e}\mathrm{d}}$为未遍历点集；${{V}}_{\mathrm{c}\mathrm{l}\mathrm{o}\mathrm{s}\mathrm{e}}$为确认点集；${{r}}_{{n}}$为搜索半径

Figure  5.  APF-FMT* node expansion process

${{X}}_{\text{init}}$ is the initial point; ${{X}}_{\mathrm{g}\mathrm{o}\mathrm{a}\mathrm{l}}$ is the goal point; ${{V}}_{\text{open}}$ is the traversed point set; ${{V}}_{\mathrm{u}\mathrm{n}\mathrm{v}\mathrm{i}\mathrm{s}\mathrm{i}\mathrm{t}\mathrm{e}\mathrm{d}}$ is the untraversed point set; ${{V}}_{\mathrm{c}\mathrm{l}\mathrm{o}\mathrm{s}\mathrm{e}}$ is the confirmed point set; ${{r}}_{{n}}$ is the search radius

下载: 全尺寸图片 幻灯片

其中，

式中，$ \gamma $为超参数，n为样本点总数，d为维度，本文研究的是平面空间，因此取值为2；

式中，F(X)代表当前点X总体成本，C(X)代表由初始点到当前点X的欧氏距离，G(X)为启发函数，代表当前点X在改进人工势场法中的成本。

1.3   路径平滑处理

基于采样点的路径规划算法，得到连续线段的路径，曲率突变严重，影响移动机器人的平顺性和安全性，不符合移动机器人实际路径规划作业，采用三阶B样条曲线对得到的路径进行平滑处理，如图6所示。

图  6  三阶B样条曲线

Figure  6.  Third order B-spline curve

下载: 全尺寸图片 幻灯片

三阶B样条曲线具备三阶连续性，表达式为：

式中，$ {P}_{i} $为控制点，具体为APF-FMT*的路径节点，${{{\boldsymbol{u}}}}$为节点向量，${{{{\boldsymbol{u}}}}}\in$[0,1]，${N}_{i,k}\left({{{\boldsymbol{u}}}}\right)$是k次B样条曲线基函数，$P\left({{{\boldsymbol{u}}}}\right)$为最终平滑后的路径。

2.   理论分析

本节为APF-FMT*算法提供相应的理论依据。

2.1   定理1概率完备性^[18]

当路径规划问题存在解时，样本数$ n\to \infty $，APF-FMT*算法找到解的概率趋向于1，即：

式中，定义$\sigma :\left[\mathrm{0,1}\right]\to {X}_{{\rm{free}}}$为无碰撞的路径，如果$\sigma \left(0\right)={x}_{{\rm{init}}}，\sigma \left(1\right)={x}_{{\rm{goal}}}$，则为规划问题的可行解。

基于以下2种情况证明定理1：1)APF-FMT*每次从$ {V}_{{\rm{open}}} $取出代价最小的节点进行迭代；2)APF-FMT*在$ {V}_{{\rm{open}}} $为空时中止并返回空路径。因此只要证明所有样本都被加进$ {V}_{{\rm{open}}} $，而且没有重复加入。

对于每个迭代过程，只有${V}_{{\rm{unvisited}}}$集合中的点才能被加入到$ {V}_{{\rm{open}}} $，并在被加入之后从${V}_{{\rm{unvisited}}}$中删除，由于样本点只有在初始化阶段加入${V}_{{\rm{unvisited}}}$中，因此，其只能被加进一次${V}_{{\rm{open}}}$。

2.2   定理2 渐进最优

设(${X}_{{\rm{free}}},{X}_{{\rm{init}}},{X}_{{\rm{goal}}})$是$ d $维空间中的路径规划问题。令$ {c}_{n} $表示APF-FMT*算法在样本数为n、半径为$ {r}_{n} $下生成路径长度，$ {c}^{\mathrm{*}} $为最佳路径$ {\sigma }^{\mathrm{*}} $的长度，$ {r}_{n} $需要满足以下算式：

式中：$ \eta > 0 $，$\mu \left({x}_{{\rm{free}}}\right)$表示无障碍空间的勒贝格测度(二维空间为面积，三维空间为体积)，$ {\xi }_{d} $为$ d $维空间中单位球的体积。

当样本点趋于无穷大时，APF-FMT*算法获得的路径长度 $ {c}_{n} $ 趋于最佳路径长度 $ {c}^{*} $ 的概率为1，即：

用以下方法证明定理2：首先，APF-FMT*算法基于FMT*算法建立，FMT*算法已被证明在以$ {r}_{n} $为搜索半径时具有渐进最优性，由于启发函数的存在，APF-FMT*算法的总代价$ F\left(n\right) $始终比实际代价要小，即实际路径长度$ {c}_{n} $存在下界：

当APF-FMT*算法加入一个新节点x_new到路径时，APF-FMT*算法的代价$F\left({x}_{{\rm{new}}}\right)$必然不大于实际路径长度${c}_{{\rm{new}}}$。当样本点趋于无穷大时，${V}_{\rm{{open}}}$存在从初始节点${x}_{{\rm{init}}}$到目标节点${x}_{{\rm{goal}}}$最优路径的1个节点$v$，即初始节点${x}_{{\rm{init}}}$到节点$ {{v}} $的实际路径长$ {c}_{v} $等于到达节点v的最佳路径长度$ {c}_{v}^{*} $。

由于$v$是最优路径上的节点，APF-FMT*算法的启发函数$G\left(v\right)$满足$G\left(v\right)\leqslant {G}^{\mathrm{*}}\left(v\right)$，得到$F\left(v\right)\leqslant {F}^{\mathrm{*}}\left(v\right)\leqslant {c}_{v}^{\mathrm{*}}$，因此v必定能被APF-FMT*找到。

2.3   定理3算法复杂度

在考虑样本数为n，$\left({x}_{{\rm{free}}},{x}_{{\rm{init}}},{x}_{{\rm{goal}}}\right)$的路径规划问题时，路径规划算法复杂度会影响总体执行效率。APF-FMT*算法计算一个路径可行解花费$O\left(n{\rm{log}}n\right)$时间，同时占用$O\left(n{\rm{log}}n\right)$空间。

用以下方法证明定理3：APF-FMT*算法初始化人工势场的时间复杂度不大于$ O\left(n\right) $。由于APF-FMT*算法是基于FMT*算法结构建立的，需要$O\left(n{\rm{log}}n\right)$时间计算n个样本的解和$O\left(n{\rm{log}}n\right)$时间来进行$ O\left(n\right) $次碰撞检验，占用$ O\left(n\right) $空间。APF-FMT*算法改进了FMT*算法的代价函数，增加人工势场法启发函数，占用$ O\left(n\right) $空间，调用启发函数的次数是$O\left(n{\rm{log}}n\right)$，总时间复杂度为$O\left(n{\rm{log}}n\right)$，根据大$ O $原则，APF-FMT*的时间复杂度为$O\left(n{\rm{log}}n\right)$，空间复杂度为$O\left(n{\rm{log}}n\right)$。

3.   仿真与试验验证

本文设置仿真区域大小为600 m × 600 m，并使用二值图代表农业机器人工作环境，建立了3个具有代表性的农业环境地图进行测试。Map1为鱼塘环境，环境条件类似文献[19]； Map2为果园环境；Map3为有狭小果树行间的果园环境。通过将APF-FMT*算法和现阶段基于采样点的路径规划算法在上述地图中进行模拟仿真，比较APF-FMT*、FMT*、RRT*和Informed-RRT*这4种算法的优劣。为了确保仿真试验的有效性，所有仿真算法都在MATLAB R2020b编写和试验，采用的计算机配置为：Windows 10操作系统，AMD R5600X六核处理器，运行内存16 G。

仿真参数设置如下：地图分辨率为600 × 600，起始点坐标为(25，25)，目标点坐标为(550，550)。对于APF-FMT*算法和FMT*算法，使用最近邻搜索寻找邻近点；对于RRT*算法和Informed-RRT*算法，设定步长为20；并在节点选择过程中采用Lazy collision-checking减少碰撞检查次数。每个算法基于10组试验结果的平均值得出最终结果。图7为各算法在不同地图上的路径规划效果。

图  7  4种算法应用于不同地图的比较

红色圆心表示起始点，绿色圆心表示目标点，红色曲线为最终路径

Figure  7.  Comparison of four algorithms applied on different maps

The red circle represents the starting point, the green circle represents the goal point, and the red curve represents the final path

下载: 全尺寸图片 幻灯片

仿真试验结果如图8所示，在地图Map1和Map2中，4种路径规划算法获得的长度相差不大，表明APF-FMT*以超越其他算法的搜索速度获得质量良好的解，并且随着样本数量增加而得到改善。观察Map3可知，RRT*和Informed-RRT*在通过狭小区域时都无法快速正确地找到解，需要迭代更多次数，获得解的质量低于FMT*和APF-FMT*算法的。通过数据比较，APF-FMT*在不同样本数情况下搜索速度优于其他算法，同时可获得良好质量的解。

图  8  不同地图上4种算法样本数与时间和路径的关系

柱状图表示样本数和时间关系，折线图表示样本数和路径长度关系

Figure  8.  Relationship between sample size and time, path of four algorithms on different maps

The relationship between sample size and time is represented by a bar chart, while the relationship between sample size and path length is represented by a line chart

下载: 全尺寸图片 幻灯片

从柱状图还可以看出，APF-FMT*和FMT*的搜索时间与搜索时间增长幅度远低于RRT*和Informed-RRT*，表明APF-FMT*因为引导函数执行更少的碰撞检测使得获得解的速度优于其他算法。从折线图可以综合得出，APF-FMT*、FMT*、RRT*和Informed-RRT*都随着样本数的增加，获得的路径解得到改善，收敛趋势相对一致；在Map1和Map3中，由于FMT*搜索整个空间，当样本数较小时，其收敛趋势较其他算法相比变化缓慢；RRT*和Informed-RRT*难以迭代狭小通道里的路径解，导致获得路径解的质量比其他2种算法差；APF-FMT*则能够找到渐进最优解。

4.   结论

本文为农业机器人在复杂农业环境进行路径规划并针对FMT*在广度搜索过程中计算量大、冗余探索问题，提出一种基于改进人工势场法的快速行进树算法APF-FMT*。该算法通过人工势场法改变FMT*生成枝干的方式，利用目标点和障碍物信息来选择生长方向，明显降低计算复杂度，并将提出的路径规划算法与其他同类的3种算法进行MATLAB仿真对比，所提出算法获得较好的解，证明所改进算法在保证路径有良好质量的同时，提高了规划速度、降低了计算复杂度。

由于本文主要研究的是静态运动规划，而实际农业机器人工作环境是动态的，因此在进一步的工作中需要考虑到运动规划的实时性和农田环境的多变性，将APF-FMT*应用于高维空间。