基于人工势场和RRT的四旋翼无人机航迹规划算法研究

杨登红 宋宏锐 文 渊

贵州理工学院航空航天工程学院 贵州 贵阳 550003

0 引言

随着科学技术的不断发展，四旋翼无人机的功能和性能得以不断提升。根据载荷不同，四旋翼无人机可以实现不同功能，现已在航拍、测绘、巡线、植保、播种等领域不断取得实质性应用。然而，因四旋翼无人机的低空飞行特性，四旋翼无人机在植保、播种等领域应用时易受现场环境影响，如山地、树木、房屋等障碍物，都直接威胁四旋翼无人机飞行安全，需要四旋翼无人机具备探测障碍物并进行实时航迹规划的能力，避免撞击障碍物出现坠机事故。目前，已有针对无人机航迹规划的研究，但对四旋翼无人机航迹规划算法研究较少。因此，本文对传统人工势场法和RRT快速拓展随机树算法应用于四旋翼无人机航迹规划进行研究，通过模拟实际校园环境作为航迹规划及避障的仿真环境，对两种算法进行仿真分析，得出两种算法应用于四旋翼无人机航迹规划和避障的特性及性能，对后续研究具有一定的参考价值。

1 人工势场法

1.1 人工势场法原理 人工势场法是由Khatib提出的一种虚拟力法并成功用这种方法实现了机械臂进行避障[1]，其原理如图1所示。

图1 人工势场法原理示意图[2]

由图1(a)可知，人工势场法原理是将四旋翼无人机的飞行环境视为一种虚拟的人工受力场[3]，然后将四旋翼无人机P的运动视为场内运动。由图1(b)可知，障碍物Po对四旋翼产生斥力Fc，目标点Pwp对四旋翼无人机产生引力Fy，引力和斥力的合力Fh作为四旋翼无人机的加速力，进而控制四旋翼无人机的运动方向从而实现避障。由于人工势场法有良好的实时性，所以很适合四旋翼无人机在动态或者静态的障碍物环境中进行航迹规划。

1.2 人工势场法航迹规划仿真 传统人工势场法的算法模型相对简单，在四旋翼无人机的飞行环境中无需过多地考虑环境因素或者相对复杂的碰撞算法，可直接在规划空间中进行航迹规划，具有算法运算量少、计算效率高，实时性强等优点[4]。但同时，传统人工势场法的缺点的产生正是因其算法设计简单，没有考虑到飞行环境中的复杂情况，因此在复杂的飞行环境中，算法很可能会陷入到其缺陷中，从而导致航迹规划失败。

根据传统人工势场法的数学模型[5]，利用Matlab进行仿真，得到传统人工势场法航迹规划结果如图2所示，其中蓝色的轨迹是算法输出的路径，白色区域代表障碍物，黑色区域为可行域，起点为点，目标航路点为Pwp。

图2 传统人工势场法局部最小值仿真

由图可知，当目标航路点与障碍物相隔太近时，四旋翼无人机受障碍物周围的斥力场影响而有很大可能导致无法达到目标航路点的情况。另外，当四旋翼无人机在复杂的飞行环境中执行任务时，遇到多个障碍物则也会导致局部最小值的情况等[6]。

2 RRT快速拓展随机树

2.1 RRT的算法模型 RRT快速扩展随机树算法是由La Valle教授在1998年提出[7]的，其思想是效仿树枝生长的随机性，快速拓展至复杂空间的所有区域，伺机找到可行的路径，具有概率的完备性。RRT算法主要以避障策略为主，其原理并不复杂，因此算法输出实时性高，适合四旋翼无人机避障应用。RRT随机树节点拓展过程如图3所示，其主要流程如下：

第一步：以起始点xroot作为随机树的初始根节点；

第二步：根节点xroot在状态空间中的随机方向中搜索采样点xrand，搜索并连接离xroot最近的随机采样点，以规定的步长Lengt h进行截取并生成子节点xnode；

第三步：将根节点与子节点相连就得到一条无碰撞路径段。在随机节点的生长方向中，若分布在障碍物内，就不会在此方向连接并删除该节点；

第四步：此时返回到第二步，直到将四旋翼无人机到目标航路点xgoal被随机树寻到。此时由xgoal回溯寻路，逐步寻到根节点后并标记即可获取一条无碰撞的航迹。

图3 RRT快速随机拓展树节点拓展

2.2 RRT航迹规划算法仿真分析 根据RRT快速拓展随机树的算法流程以及实现算法的数学模型[8]，利用Matlab模拟具有不同障碍物的四旋翼无人机飞行环境，通过Matlab进行仿真，得到RRT 航迹规划仿真结果如图4所示。

图4 RRT航迹规划仿真结果

图4(a)与图4(b)所示航迹的终点坐标均为(486，468，488)，前者随机生成的节点为N=987个，搜索时间为t=34s，后者随机生成N=46个节点，搜索时间为t=6s。图4(c)所示航迹的终点坐标为(50，581，310)，随机生成的节点为N=103个，搜索时间为t=10s。图4(d)所示航迹的终点坐标为(0，400，400)，即在Y和Z轴组成的平面内，随机生成的节点为N=1603个，搜索时间为t=54s。由此看出，本文所述RRT 算法能快速找到可行航迹，生成航迹的随机性较大，但从概率的角度看，RRT 算法具有概率完备性，不会陷入类似人工势场法等其他航迹规划算法的局部最小值问题。

2.3 主要参数对RRT算法的影响

2.3.1 搜索步长对算法的影响 搜索步长是RRT算法的重要参数之一，若搜索步长过大则导致航迹折线角度过小，会使航迹与障碍物的安全距离缩小。设定搜索步长Length=100 m 和Length=1 m，通过Matlab进行仿真，得到仿真结果如图5所示。由图可以看出，当Length=100 m 时，虽然搜索时间仅为t=0.8s，但航迹规划质量不佳，四旋翼无人机进行巡航会有较大的概率避障失败。反之，当Length=1 m 时，其搜索时间为t=323s。因此，若RRT搜索步长过小，则搜索时间增加，搜索效率大大降低，不适用于四旋翼无人机实时避障，即使航迹相对平滑，发生避障失败的概率也会增大。

综上所述，经多次试验，在(500 m×500 m×500 m)的三维环境中，RRT快速随机拓展树的最佳搜索步长设置范围为30-40m。

2.3.2 碰撞检测算法中的分段值 碰撞检测算法是实现RRT算法的重要避障数学模型，其主要采用分段法进行障碍物规避，分段数量Rp直接影响RRT检测障碍物和随机树的节点生成情况，分段数量Rp对随机树拓展的影响如图6所示。如果Rp增大，则计算量增大，算法效率会降低。反之，如果Rp减小，当遇到小型障碍物时，如果步长Length大于障碍物尺寸，则有一定概率会出现新节点与邻近点生成的树枝穿过障碍物，四旋翼无人机按照此航迹飞行避障失败的概率必然增大。

图5 搜索步长变化仿真结果

图6 分段数量R p 对随机树拓展的影响

3 对比分析

在航迹规划仿真过程中，因二维环境比三维环境可以更直观地验证算法的航迹规划效率，在仿真时可侧重考虑四旋翼无人机在水平方向的航迹规划效果。为了能够直观地比较两个算法的特性及性能，本文将人工势场法和RRT算法在相同二维飞行环境中进行对比仿真，便于观察不同算法的航迹规划效果。

本文主要通过算法的航迹规划效果、航迹规划时间和航迹规划效率来进行对比分析。先根据学校教学楼分布情况，通过Matlab模拟四旋翼无人机的飞行环境，在该模拟环境中分别基于人工势场法和RRT算法进行航迹规划仿真，得到航迹规划结果如图7所示。根据教学楼分布构造的飞行环境大小为(500 m×500 m)，四旋翼无人机起飞点坐标均为xroot(388，120)，飞行终点坐标均为xgoal(133，434)。

图7 同一实际环境中的航迹规划算法仿真

人工势场法航迹规划效果如图7(a)所示，由图可知，在四旋翼无人机接近目标航路点时，由于附近的障碍物比较多并且可行空间比较小，可能导致吸引和排斥势场力大小相等，从而使得算法规划的航迹陷入局部最小值状态，导致四旋翼无人机在飞行中无法找到可行航迹。RRT 快速拓展随机树航迹规划如图7(b)所示，绿色折线是RRT 算法标记的航迹，后经过贝塞尔曲线优化后所得到的红色曲线是可行航迹，该航迹平滑比较平滑，避障效果较好。

表1 人工势场法与RRT算法的性能比较

RRT算法/3 404 501 34 626 10 RRT算法/4 404 508 56 1086 10 RRT算法/5 404 499 43 991 10

人工势场法与RRT算法的航迹规划性能如表1所示。由表1可知，在同一飞行环境中，人工势场法陷入局部循环无法搜索到可行航迹，但RRT算法5次运行均能搜索到可行航迹，经过贝塞尔曲线优化后，能够与四旋翼无人机的飞行性能配合实时避开障碍物。因此，在相对复杂的环境中，如果使用人工势场法，则会导致飞行环境引力与斥力场相对紊乱，从而增加导致算法规划航迹失败的概率，但由于RRT 算法具有随机性，在复杂环境中体现出了全概率规划出可行航迹的特性，具有更好的航迹规划性能。

4结论

本文针对四旋翼无人机航迹规划与避障问题，对人工势场法和RRT快速拓展随机树算法进行研究，并根据学校教学楼分布模拟实际飞行环境对航迹规划效果进行仿真验证。结果表明，四旋翼无人机在障碍物较多、可行空间较小等情况下，人工势场法可能因吸引和排斥势场力大小相等，从而使得算法陷入局部最小值状态，导致四旋翼无人机在飞行中无法找到可行航迹，但由于RRT算法具有随机性，在复杂环境中体现出了全概率规划出可行航迹的特性，具有更好的航迹规划性能，在实际航迹规划中具有更大的应用前景。