基于BI-APF-RRT算法的无人机路径规划

UAV Path Planning Based on BI-APF-RRT Algorithm

摘要: 针对双向RRT算法中存在较大搜索范围随机性问题，论文提出一种基于目标偏置的BI-APF-RRT (Bidirectional Artificial Potential Field Method RRT)改进算法的无人机飞行路径规划策略。首先，将目标偏置策略用于随机采样点的产生和双树扩张方向的指导，并通过双向RRT算法建立两棵相互交替的随机搜索树完成搜索，实现了算法收敛速率的提升。其次，将改进的人工势场融入双向生长树，进一步较大程度减少了搜索迭代次数。同时，在平滑路径的过程中，通过采用3次B样条插值算法实现了轨迹路径的优化。最后，通过仿真实验表明：与一些已有算法相比，基于目标偏置的策略改进BI-APF-RRT算法能够有效减少迭代次数，提升收敛速度，同时改善了新节点生成的方向，有效降低了路径成本，较好地解决了双向RRT算法中存在较大搜索范围随机性问题。

Abstract: To address the issue of significant search space randomness inherent in the Bidirectional Rapidly-exploring Random Tree (RRT) algorithm, this paper proposes an improved Unmanned Aerial Vehicle (UAV) flight path planning strategy based on a goal-biased Bidirectional Artificial Potential Field method RRT (BI-APF-RRT) algorithm. Firstly, a goal-biasing strategy is employed for the generation of random samples and to guide the expansion direction of the bidirectional trees. The search is completed by establishing two alternately growing random search trees via the bidirectional RRT mechanism, thereby enhancing the algorithm’s convergence rate. Secondly, an improved Artificial Potential Field (APF) method is integrated into the bidirectional tree growth process, which further significantly reduces the number of search iterations. Concurrently, during the path smoothing phase, trajectory optimization is achieved through the application of a cubic B-spline interpolation algorithm. Finally, simulation experiments demonstrate that, compared to several existing algorithms, the proposed goal-biased BI-APF-RRT algorithm effectively reduces the number of iterations, enhances convergence speed, improves the directionality of new node generation, and effectively lowers the path cost. Consequently, it successfully mitigates the significant search space randomness problem associated with the conventional bidirectional RRT algorithm.

1. 引言

近些年来，随着无人驾驶技术的发展，无人机在农业[1]、军事[2]、物流运输[3]和工业[4]等领域得到广泛的应用。同时，凭借着高机动性、易于控制和价格便宜等优势，无人机深受国内外学者喜爱。随着无人机飞行任务越来越复杂、飞行环境越来越多变，路径规划在无人机控制系统中起着越来越重要的作用。目前，路径规划已成为无人机领域的重要研究热点之一。常见的路径规划算法有粒子群算法[5]、Dijkstra [6]、蚁群算法[7]等传统算法。基于网格的搜索算法典型代表是A*算法，应用图论来解决离散状态空间中的运动规划问题[8]。

值得指出的是，基于样本的路径规划算法是目前较受欢迎的路径规划算法之一。由于该算法不受空间维度的限制，因此它可以在更高维度空间中找到更好的路径，并且具有概率完整性。最典型的算法之一是快速探索的随机树算法(RRT) [9]。与其他路径规划算法相比，该算法被广泛使用，因为它简单的结构和强大的搜索能力可以解决复杂的约束，但是该算法无法找到最佳路径，而全局采样会导致存在大量冗余点。

为了应对RRT的缺点，研究学者加入人工势场法，提出了APF-RRT算法，该算法解决了RRT的随机性问题[10]。RRT*算法在2011年通过引入渐进最优化机制，在保持概率完整性的同时，显著提升了路径质量[11]。该算法通过重选父节点和重新布线等方式减少搜索时间，但随着采样点的增加，会大大增加算法的计算量。尽管RRT*可以找到更好的路径，但收敛速率仍然无法满足实时要求。有人对RRT*算法进行研究，提出RRT*-SMART-A*算法加速了收敛速度，并以两种智能采样的方式降低了RRT*算法的路径成本[12]。为了RRT*生成更好的采样空间，有人提出Informed-RRT*算法[13]。

为了加快搜索速度，有人提出了基于双采样点的BI-RRT算法[14]，该算法在起点和终点同时扩展两棵搜索树，减少了路径规划的时间，但规划的路径不是最优。通过改进双向树的连接方式，提出的RRT-Connect算法，能有效减少搜索时间[15]。

还有人提出改进的RRT-Connect算法[16]，在采样过程中引入转角约束，以减少搜索节点，但上述算法随机树的生长是随机的，在不必要的区域进行采样会耗费大量的时间，规划的路径也不是最优的。

在双向树的基础上，提出双向版的RRT*算法，称为BI-RRT*算法，搜索速度加快，能有效优化路径，但在复杂环境下计算节点数量较大[17]。在基于势场的双向规划算法中，BI-APF-RRT*代表了当前最先进的技术水平，它通过引入人工势场(APF)来引导双向RRT树的扩展，有效地提升了在复杂狭窄环境中的规划效率和解路径质量[18]。然而，我们发现，BI-APF-RRT*的算法框架在追求渐进最优性的同时，也引入了显著的在线计算负担。其核心在于，它在每次迭代中都需要执行复杂的父节点重连和树形优化操作，这在许多对实时性要求极高的应用场景(如机器人动态避障、自动驾驶紧急决策)中可能成为性能瓶颈。因此，本文旨在探索一种在规划效率与路径质量之间取得更佳平衡的替代方案。为了适应更复杂的环境，提出DBVSB-P-RRT*算法，设计自适应步长大小以增强环境适应性[19]。

本文主要提出了BI-APF-RRT算法的路径规划。它摒弃了BI-APF-RRT*中计算密集的渐进最优性保证机制，转而采用一种轻量级但高效的启发式优化策略。我们的核心思想是：通过精心设计的APF引导和双向收敛机制，在保持快速收敛到满意解的同时，极大地提升算法的收敛速度。首先，双向RRT算法同时创建两个搜索树，并根据目标偏置策略交替生长，可以获得更高质量的采样并减少收敛时间。接下来，通过引入改进的人工势场方法，能够有效减少迭代次数。然后，应用3次B样条插值优化方法以获得更光滑的路径，以便解决飞行中无人机频繁转弯的问题。最后，与RRT、APF-RRT、RRT*、APF-RRT*和BI-RRT算法相比，该算法生成的路径具有更高的性能。

2. 问题定义

数学符号用于描述无人机路径规划问题。路径规划算法的目的是规划无人机从起点到目标点的无障碍路径。本文研究了物流运输中无人机的路径规划算法。在本文中，F被定义为整个环境，即整个运输空间。F_ob是运输途中的障碍区域。F_s是环境中没有障碍的自由搜索区域。(F, F_start, F_goal)定义了一个路径规划问题，其中$F_{start}\in F_S$ 是随机生长树的根节点，并且F_goal是生长树的目标节点。

定义1：存在连续的函数σ：[0, 1]如果$v\in \left[0,1\right]$ ，$\sigma \left(v\right)\in F_S$ ，则该路径是从根节点到目标节点的无碰撞路径。对于路径规划问题(F, F_start, F_goal)，σ(0) = F_start，σ(1) = F_start，如果找不到解决方案，则报告失败。

定义2 (最佳轨迹)：给定轨迹规划问题(F, F_start, F_goal)，在发现了可行的路径σ(v)之后，经过优化后，σ(v)为最佳轨迹，其中c(σ)是路径的全长。如果找不到解决方案，则报告失败。

定义3 (最短时间路径)：在最短的时间内，找到了最低成本的轨迹，并获得了最佳解决方案。

3. BI-RRT算法

双向扩展随机生长树BI-RRT算法从目标点和起始点同时生长两棵树，直到两棵树相交，最后回溯路径[20]。伪代码显示在算法1中。

算法1. BI-RRT算法

1. $T_{tree1}\leftarrow \left\{q_{start}\right\},T_{tree2}\leftarrow \left\{q_{goal}\right\}$

2. $for\text{}\text{}\text{}i=1:N_{\max }\text{ do}$

3. $q_{rand}\leftarrow sample\left(i\right);$

4. $q_{near}\leftarrow Near\left(q_{rand},T_{tree1}\right);$

5. $q_{new}\leftarrow Steer\left(q_{rand},q_{near},T_{tree1},s_1\right);$

6. $if\text{CollisionFree}\left(q_{near},q{}_{rand\text{'}},F_{obs}\right)\text{ then}$

7. $T_{tree1}\leftarrow T_{tree1}\cup \left\{q_{new}\right\};$

8. $else;$

9. $Continue;$

10. $Endif;$

11. $if\text{Connect}\left(T_{\text{tree}1},T_{tree2}\right)\text{ then}$

12. $return{T}_{tree1},T_{tree2};$

13. $break;$

14. $else;$

15. $Swap\left(T_{tree1},T_{tree2}\right);$

16. $Continue;$

17. $Endif;$

18. $Endfor;$

19. $T_{tree}\leftarrow Combine\left(T_{tree1},T_{tree2}\right);$

在伪代码中，第一棵树T1开始生长，并且在每次迭代结束时确定与另一棵树之间的最小距离，如果距离小于固定值，则Connect函数将两棵树连接，否则，用Swap函数切换到另一棵树T2再生长一次，重复以上过程。

4. 改进的目标偏置BI-APF-RRT算法

4.1. 高质量抽样策略

随机采样点产生得到了改善。目标策略根据以下等式(1)选择随机采样点。

$q_{rand}=\{\begin{array}{l}random,\text{ if}rand\left(\right)>m\\ q_{goal},\text{ else}\end{array}$ (1)

其中，rand()是0到1之间的随机数，m是一个目标偏置阈值，随机树中新节点的生长方向由m确定。当rand() > m时，生长树在采样空间中随机生长，反之，生长树朝目标点位置生长。在每次迭代中同时扩展了两棵随机树，如图1所示。

Figure 1. Bidirectional RRT node expansion process

图1. 双向RRT节点扩展过程

4.2. 有效的搜索策略

人工势场法指导新的随机点的产生，通过目标的吸引力和障碍物的排斥力，使新的随机点往目标点方向产生，从而衍生出新的树枝。

人工势场方法包括引力场$U_{att}\left(p\right)$ ，如等式(2)所示。

$U_{att}\left(p\right)=\frac{1}{2}{k}_a{\rho }_g^2\left(p\right)$ (2)

式(2)中，P是无人机的随机点，k_a是重力场增益常数。排斥场$U_{rep}\left(p\right)$ ，如等式(3)所示。

$U_{rep}\left(p\right)=\{\begin{array}{l}\frac{1}{2}{k}_r\left(\frac{1}{\rho \left(p\right)}-\frac{1}{{\rho }_0}\right)\rho \left(p\right)\le {\rho }_0\\ 0\rho \left(p\right)\le {\rho }_0\end{array}$ (3)

式(3)中，k_r是排斥场增益常数。ρ₀代表每个障碍物的影响半径。总场就是斥力场和引力场的叠加，如等式(4)所示。

$U_{total}={\displaystyle \sum U_{rep}}+{\displaystyle \sum U_{att}}$ (4)

引力就是引力场对距离的导数，斥力就是斥力场对距离的导数。因此，引力函数定义如等式(5)所示。

$F_{att}\left(p\right)=k_a{\rho }_g\left(p\right)$ (5)

式(5)中：${\rho }_g\left(p\right)$ 表示该随机点与目标点的欧几里得距离。同样，斥力函数在等式(6)显示。

$F_{rep}=\{\begin{array}{l}{k}_r\left(\frac{1}{\rho \left(p\right)}-\frac{1}{{\rho }_0}\right)\rho \left(p\right)\le \rho \left(0\right)\\ \text{0 }\rho \left(p\right)\ge \rho \left(0\right)\end{array}$ (6)

式(6)中：$\rho \left(p\right)$ 代表随机点和障碍物之间的距离。总的力就是引力和斥力的叠加，如等式(7)所示。

$F_{total}={\displaystyle \sum F_{rep}}+{\displaystyle \sum F_{att}}$ (7)

双向RRT算法结合了改进的人工势场方法，其中以T1为例，F_att是q_goal上的引力，F_rep是F_obs的排斥力。根据平行四边形规则，发现所得的力方向是${q^{\prime }}_{rand}$ 的扩展方向。然后q_near朝着${q^{\prime }}_{rand}$ 的方向扩展一定步长得到q_new。双树RRT扩展方向如图2所示。

Figure 2. Force analysis diagram of the closest point of BI-RTT

图2. BI-RTT最近点的力分析图

但是，当无人机即将接近目标时，目标点附近存在障碍，而无人机的排斥力大于吸引力，因此无人机很难飞向目标，并且出现了局部最小值的问题。因此，本文提出了改进的人工势场函数(8)，如下所示：

$F_{total}=\{\begin{array}{l}{\displaystyle \sum F_{rep}}+{\displaystyle \sum F_{att}}\epsilon >\rho \left(q_{rand},q_{obs}\right)\\ {\displaystyle \sum F_{rep}}+{\displaystyle \sum F_{att}}+F_{random}\epsilon <\rho \left(q_{rand},q_{obs}\right)\end{array}$ (8)

F_random是一个随机方向上的扰动，目的是跳出局部最小值。$\epsilon$ 是设定的阈值，$\rho \left(q_{rnad},q_{obs}\right)$ 是随机点与障碍物的欧几里得距离。

4.3. 改进连接方式

改进连接方式。当两棵树最新节点之间没有与障碍物发生碰撞，则直接连接。如算法2所示。

算法2. Connect算法

$Input:T_{tree},target,F_{obs}$

$Output:T_{tree},connected$

1. $connected\leftarrow false;$

2. $\left(q_{near},q_{near\_id}\right)\leftarrow Near\left(T_{tree},target\right);$

3. $if\text{CollsionFree}\left(q_{near},target,F_{obs}\right)$

4. $T_{tree}\leftarrow Fuz\left(target,q_{near\_id}\right);$

5. $connected\leftarrow ture;$

6. $\text{End if};$

相关解释说明，q_near__id为最近节点的索引值，target用来判断是否相连的标签。Fuz函数用来更新树的节点和索引值。

4.4. 路径优化算法

三次样条插值是一种分段式插值方法，通过一系列基于三次多项式的插值点区间，形成一条光滑曲线。用三次样条插值方法拟合出的无人机移动路径曲线更加平滑，这样可以保证无人机在急停或急转时有较好的动力学特性。

设在区间[a, b]上取n + 1个节点，$a=x_0<x_1<x_2<\cdots <x_n=b$ ，给定这些节点的函数值$f\left(x_i\right)=f_i$ ，$i=0,1,2,\cdots ,n$ ，若s(x)满足以下条件：

1) $s\left(x\right)\in C^2\left[a,b\right]$ ；

2) $s\left(x_i\right)=f_i$ ，$i=0,1,2,\cdots ,n$ ；

3) 在每个小区间$\left[x_i,x_{i+1}\right]$ 上，s(x)是三次多项式。则称s(x)为三次样条插值函数。

本文中该算法产生的路径节点通过三次样条插值进行了优化，以获得适合无人机飞行的光滑飞行路径。无人机路径规划应满足两个条件：

1) 不能与障碍物发生碰撞；

2) 路径长度应尽可能短。

本文就以满足上述条件的无碰撞的路径长度最短作为适应度函数的评价标准。

本文构造的适应度函数为：

$z=L\left(1+\varpi +\eta \right)$ (9)

其中，$\varpi$ 是一个足够大的数字，可以排除穿过障碍区域的路径，在这里它的值为100。

$L\left(p\right)={\displaystyle \sum _{i=1}^{n-1}\sqrt{{\left(x_{i+1}-x_i\right)}^2+{\left(y_{i+1}-y_i\right)}^2}}$ (10)

其中，L(p)是路径长度，$\left(x_i,y_i\right)$ 是i路径点，n是一个总点。$\eta$ 是一个标志变量，其初始值设置为0。

4.5. 参数选取

为了使BI-APF-RRT算法在实际应用中达到最佳性能，对本算法中引入的关键参数进行深入理解与合理设置至关重要。本节将详细讨论主要参数的作用、选取方法及其对算法性能的影响。

本文提出的BI-APF-RRT算法主要涉及以下关键参数。M：目的控制随机采样时直接选择目标点作为采样点的概率。M值选取越接近1，树生长方向越朝向目标点。k_a (引力增益系数)与k_r (斥力增益系数)：目的控制人工势场中引力场与斥力场的相对强度。在简单或者开阔环境中可使用较小的k_r/k_a比值(如1.5~2.0)，让引力主导，加快收敛。反之在复杂或者狭窄环境，需使用较大的k_r/k_a比值(如3.0~5.0)，增强斥力以确保安全，避免碰撞。ε设定阈值的目的是防止算法在无解或极端复杂环境中无限循环，是保证算法实时性的硬性约束。ε通常设置为一个较小的值(如0.05到0.2)。这是因为过高的ε会使算法退化为贪婪搜索，在存在“陷阱区域”(如U型障碍)时极易失败。

5. 仿真结果

本文使用MATLAB作为仿真软件。比较了改进的BI-APF-RRT、APF-RRT、APF-RRT*的仿真结果以及现有的RRT、RRT*和BI-RRT*算法的仿真结果。事实证明，改进的BI-APF-RRT具有更快的收敛性和较短路径的优势。模拟环境是二维空间，其尺寸为100 km × 100 km。模拟环境如图3所示。

(a) 环境A (b) 环境B

Figure 3. Simulation environment

图3. 仿真环境

在模拟环境A中，无人机开始于(5, 5)，并以(95, 95)结束。在模拟环境B中，无人机开始于(5, 55)，并以(95, 55)结束。无人机无法穿越深灰色的障碍。q_near和q_new之间的连接由红线表示。在双向搜索树中，绿线表示另一个生长树q_near和q_new之间的连接。

5.1. 环境A

环境A的仿真结果如图4(a)所示，红线表示路径规划的无人机的飞行轨迹。在图4(a)中，由于RRT的抽样空间较大，该轨迹具有更多的拐点和更长的路径。图4(b)显示了RRT*算法的仿真结果。与原始的RRT算法相比，尽管搜索时间更长，但路径得到优化，去除多余冗余点。尽管RRT*算法改善了路径，但由于缺乏障碍排除，该算法在障碍物周围具有多余的点。因此，加入人工势场法后，图4(c) APF-RRT算法和图4(d) APF-RRT*算法，与RRT算法和RRT*算法相比，明显提高了算法的搜索效率；在图4(e)中，Bi-RRT双向随机生长树的生长速度更快，但路径不够平滑。图4(f)显示了本文中给出的算法，黑色轨迹为原始路径，红色轨迹为经过B样条曲线优化得来，能明显看出BI-APF-RRT算法路径更平滑且较短。

(a) RRT (b) RRT*

(c) APF-RRT (d) APF-RRT*

(e) BI-RRT (f) BI-APF-RRT

Figure 4. Simulation results in environment A

图4. 环境A仿真结果

每种算法在复杂的环境中进行了30次测试，以验证算法的稳定性，环境A中的仿真数据见图5。通过分析图5中的数据，能明显看到本文算法相比其他算法在路径长度、迭代次数、运行时间方面有明显的优势。

(a) 迭代次数 (b) 路径长度

(c) 运行时间

Figure 5. Data comparison in complex environments

图5. 复杂环境下数据对比

通过分析表1，能看到BI-APF-RRT算法相比APF-RRT算法产生轨迹的平均长度减少20.5%，平均运行时间减少54%，平均迭代次数降低了51%。BI-APF-RRT算法与BI-RRT算法相比产生轨迹的平均长度减少23.9%，平均运行时间减少35%，平均迭代次数降低33%，这表明该算法在复杂的环境中具有更高的搜索效率，并且该算法路径更平滑。

5.2. 环境B

在环境B中，图6中显示了6种算法计划的轨迹。本文中新算法的飞行路径如图6(f)所示。黑色轨迹为原始路径，红色轨迹为经过B样条曲线优化得来。与其他算法相比，生成的冗余点较少，飞行路径更符合实际需求。

每种算法在狭窄的环境中进行了30次测试，以验证算法的稳定性，环境B中的仿真数据见图7。通过分析图7中的数据，能明显看到本文算法相比其他算法在路径长度、迭代次数、运行时间方面有明显的优势。

通过分析表2，本文算法产生轨迹的平均长度比APF-RRT算法减少21.4%，平均运行时间减少52%，平均迭代时间降低52%，本文算法产生的轨迹平均长度比BI-RRT算法少28%，平均运行时间减少36%，平均迭代时间降低15%，这表明该算法在狭窄的环境中具有更高的搜索效率，并且该算法路径更平滑。

Table 1. Experimental results of finding feasible solutions in complex environments

表1. 复杂环境下找到可行解的实验结果

算法	平均路径长度/m	平均运行时间/s	平均迭代次数
RRT	166	1.32	1256
APF-RRT	156	0.48	669
RRT*	142	1.47	1334
APF-RRT*	138	0.51	706
BI-RRT	163	0.34	487
BI-APF-RRT	124	0.22	324

(a) RRT (b) RRT*

(c) APF-RRT (d) APF-RRT*

(e) BI-RRT (f) BI-APF-RRT

Figure 6. Simulation results in environment B

图6. 环境B仿真结果

(a) 迭代次数 (b) 路径长度

(c) 运行时间

Figure 7. Data comparison in confined environments

图7. 狭窄环境下数据对比

Table 2. Experimental results of finding feasible solutions in confined environments

表2. 狭窄环境下找到可行解的实验结果

算法	平均路径长度/m	平均运行时间/s	平均迭代次数
RRT	143	0.91	1982
APF-RRT	140	0.25	382
RRT*	148	1.41	2215
APF-RRT*	133	0.35	655
BI-RRT	153	0.19	214
BI-APF-RRT	110	0.12	180

6. 结束语

在双向RRT的基础上，本文引入了目标偏置策略以改善随机抽样点的产生。然后，将改进的人工势场引入BI-RRT算法，实现了选择质量更好的节点，并将其添加到生长树中。改进的算法减少了不必要的冗余点，降低了迭代次数，进而解决了RRT算法中路径太长的问题，算法的搜索效率得到有效提升。仿真结果表明：算法能够有效优化路径长度并提高搜索速率，较好地解决了双向RRT算法中存在较大搜索范围随机性问题。后期，将进一步研究3D环境中的多架无人机合作运输问题。