【AI 无人机】基于RRT*算法的飞行器三维轨迹规划

标题：3-D Trajectory Planning of Aerial Vehicles Using RRT*

作者：P. Pharpatara, B. Hérissé and Y. Bestaoui

来源：IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY , VOL. 25, NO. 3, MAY 2017

编辑：张钊

审核：顾家旭

论文链接：

https://www./publication/305037318_3D_Trajectory_Planning_of_Aerial_Vehicles_Using_RRT

一、摘要

     本文简要介绍了一种飞行器在三维空间中避障时的轨迹规划算法。障碍物的性质可以是雷达探测区域、协同飞行器和不协同飞行器等。因此，这是一个复杂的轨迹规划问题。该规划器基于改进的速搜索随机树(RRT*)算法。将人工势场与RRT*算法相结合，通过偏置随机状态的产生，加快了收敛到次优解的速度。在异构环境下的复杂导弹应用中，验证了该框架的性能。事实上，由于空气密度随高度呈指数下降，飞行器的机动性依赖于气动力也随高度呈指数下降。为了解决这个问题，使用在异构环境中行驶的类Dubins飞行器的最短路径来构建度量。仿真结果表明，与RRT算法和RRT*算法相比，该框架能够以较少的迭代次数找到第一个解。此外，在给定的迭代次数内得到的最终解更接近于所考虑的准则的最优解。

二、问题描述

在本文中，飞行器被建模为质量为m的刚体。引入了三个框架(如图1所示)来描述飞行器的运动：以地球为中心的地球固定参照系，以点O为中心；飞行器机体坐标系，以机体质心Cg为中心；附着在机体上的速度坐标系,以机体质心Cg为中心。空中飞行器的动力学模型可以写成如下公式所示。

其中γ∈[−π/2、π/2]和χ∈[−π，π]。T₁，T₂，T₃是推进力的分量；F_a1，F_a2 , F_a3是气动力的分量，包括阻力f_D和升力f_L2和f_L3，表达式如下下式所示。

在公式中，C_D，C_L2，C_L3是空气动力系数，S是参考面，ρ(Z)是空气密度，v_a是空气速度V_a的绝对值，表示为V_a=V−V_w, V_W是风速。力F_p1, F_p2, F_p3包括作用在飞机上的其他扰动力。对于飞行器的控制，使用气动力f_L2和f_L3以及推进力(T₁，T₂，T₃)。

图1 飞行器模型

三、方法

RRT*算法是一种增量式方法，旨在通过向大的Voronoi区域生长搜索树来有效地探索非凸高维空间。该方法具有渐近最优性，即几乎必然收敛于最优解。算法1描述了RRT*作为路径规划器的原理。

APF是一种反应式方法，其中轨迹不是显式生成的。取而代之的是环境会产生一些力量，引导飞行器到达目的地。然而，局部极小值和振荡运动等问题可能会使目标无法实现。APF可用于将X_rand定向到目标点，通过与RRT*相结合，APF的缺点可以通过RRT*的随机性来解决，即飞行器通过试图进入X_rand来离开局部极小值。同时APF也能提高收敛到最优解的速度。这是一种用于定向的人工矢量融合算法。

人工势能和矢量可以用几种方式定义。在本文中，使用了以下人工势能和矢量。

（1）在障碍物周围使用排斥力势，以使飞行器避开障碍物。

（2）与排斥力势相反的吸引力势被用来将飞行器引导到期望的目的地。

（3）使用旋转矢量场而不是排斥势来引导飞行器绕过障碍物，以减少局部最小问题。在图2中，设ξ表示障碍物的重心，S_obs表示障碍物表面的集合，d_obs表示与障碍物的最大影响距离，r_obs表示图2中的最短距离。t、σ，λ是单位向量，定义为如下公式所示。

图2 障碍物周围旋转向量场的示意图

四、仿真结果与讨论

     本文使用MATLAB获得了仿真结果。图3所示的场景考虑如下配置：初始状态X_init=(1km，1km，1km，π/2，0)^T。在图4中，箭头表示初始状态X和会合状态X的方向。X_obs由3D着色曲面(渐变颜色)表示。障碍物的性质可以是禁飞区，例如城市上方和周围的区域，它们由圆柱体和雷达检测区表示，由半球体表示。

图3 场景的图解示意图

本节考虑了三种不同的算法：

1)RRT；

2)RRT*；

3)在给定裕度的情况下，由APF偏置RRT*以使随机产生的X_init的方向为10°(φAPF=10°)。

在以下图中，探索树以浅灰色表示，粗实心曲线是算法找到的最终解。图4显示了RRT算法经过1000次迭代后获得的结果。结果表明，RRT算法能够找到问题的解。然而，得到的轨迹显然不是最短的，因为可以观察到一些环路和转弯。这是因为RRT没有考虑最优标准。此外，该算法没有进行重新连接来提高路径质量。

图4 探索树和经过1000次迭代的RRT算法的结果：路径长度200公里。

另外两种算法适用于相同的场景。图5显示了200次迭代后，RRT*和APF偏向的RRT*获得的最佳结果之一。从结果可以看出，RRT*算法可以找到比RRT更好的解。在图5中，两个RRT*算法之间的区别在于，由APF偏向的RRT*的探索树朝向目的地[参见图5(b)]，而另一个算法向每个方向扩展[参见图5(a)]。

图5 200次迭代后的探索树结果。(a) RRT*：路径长度140.36公里。(b) RRT*由APF偏向：路径长度139.89公里。

为了进行深入的分析，针对每种算法，在200次迭代内进行了100次蒙特卡罗模拟以获得统计结果。这里使用迭代次数而不是计算时间来比较计算效率，因为代码没有完全优化来评估其实时能力。此外，计算工作量主要由度量函数来消耗，度量函数仍然可以优化。获得第一个解所需的平均迭代、第一个解的平均长度和最终解的平均长度见表I。

根据表I中的统计结果，对于RRT和RRT*算法，第一个解可以在几乎相同的迭代中得到。另请注意，RRT*的最终解的最佳性相对于迭代次数有所改善。APF偏向的RRT*是找到第一个解的最快方法。最重要的是，第一解和最终解都比其他解更接近最优解。原因在于，使用APF的启发式搜索有助于将搜索偏向于可行的最优解，而不是盲目地搜索探索空间，即增加了RRT*的重连概率。显然，如果给出更多的迭代，RRT*可以达到相同的最终性能。

本文没有显示的几种场景的蒙特卡罗模拟也已被模拟。对于复杂程度较低(即障碍较少)的场景，RRT*和偏置APF的RRT*的结果基本相同。由于障碍物体很少，算法收敛速度很快，因为重新连接的概率已经很高。因此，对于非完整问题，在不太复杂的环境中性能改善不大是合理的。

五、结论

     本文提出了一种有效的飞行器在三维空间中避障的轨迹规划框架。该算法以RRT*算法为基础，寻找接近最优的轨迹。此外，该算法加快了收敛到最优解的速度。该框架在导弹应用中得到了验证。该度量使用异构环境中的最短Dubins路径。结果表明，该方法在仿真结果中表现出良好的性能。与RRT算法和RRT*算法相比，由于集成了APF，它以更少的迭代次数找到了更好的解。虽然该方法的计算复杂度在本文中没有严格的分析，但如果算法在飞行器上实现，并以飞行器需要较少的迭代次数来求解，则实时约束将更容易验证。此外，值得注意的是，该算法是随机的，它可以相当快地获得第一个可接受的解，剩余的计算资源仅用于提高最优性。此属性对于实时应用程序非常有用。在未来的工作中，将考虑重新规划以提高算法在动态环境或任务变化情况下的性能。