第4期 王文彬，等：基于滚动时域的无人机动态航迹规划 ·529· 了实现。 实验二RHC-FPSO算法与传统航迹规划算 多旋翼无人机其动力学可以由具有速度和加 法对比 速度约束的质点动力学模型近似。 图5将传统经典的航迹规划方法SAS和RRT rk+1) r(k) +Ba(k) (18) 与本文提出的算法进行了对比。稀疏A*搜索 (k+1) v(k) (SAS)算法是启发式搜索算法A*的一种改进，首 式中：A= L2△tl2 (△)2/2I2 ；B= r、v、a分 先将规划环境表示为网格的形式，通过预先确定 02 △l, 别为位置、速度、加速度向量；12和O2表示二维单 的代价函数寻找最小代价的航迹。本文在扩展节 位矩阵和零矩阵。 点时，通过把约束条件结合到搜索算法中去，减 输出约束：一般的输出约束包括障碍物约束 少了搜索空间，进一步缩短了规划时间。RRT通 (19),速度约束(20)，加速度约束(21) 过对规划环境中的采样点进行碰撞检测来获取空 [L2,02]x(k+)EO (19) 间中的障碍物的信息，可以避免对规划环境进行 [02,I2]x(k+k)ll2Vmax (20) 建模。RRT首先选择开始点作为随机树的根节 Ia(k+jk)l2≤aax (21) 点，通过随机探索方向点来扩展随机树，直到扩 式中：OcR为不可飞区域，vm和amx分别为最大 展到目标点，然后从目标点回潮到根节点，完成 速度和最大加速度约束。 航迹规划。因此SAS算法在空间表示精度下是 仿真环境：处理器：PC(ntel(R)i3-3240CPU 全局最优解，而RRT是局部最优解。 3.40GHz:操作系统：Windows7:仿真软件：MAT- --RRT LAB2016。表1为仿真的实验参数，参数设置参 考文献[9,18]。本文实验部分为(-25m,25m) 到(-15m,12m)的一个二维规划环境。 实验一VORONOI代价图与传统代价图对比 图4为根据文献[8]的代价图和本文提出的基 于VORONOI图的代价图的对比。黑色方块表示 -25-20-15-10-5051015202: 障碍区域，按照VORONOI图的定义，所选取的可 视点都是距离障碍区最远的点，并且地图边框也 图53种航迹规划方法对比图 是不可飞区域，可视点如图4所示。文献[8]的可 Fig.5 Comparison of three trajectory planning methods 视点为多边形的顶点。 从图5和表2可以看出，在以距离为代价的 航迹规划算法中，稀疏A*搜索算法是3种算法里 50 航迹 5 面航迹规划最短的算法，RRT是随机算法，是算 40 法里面航迹最长的，并且不是最优路径，RHC- 35 FPSO算法不是距离最短的算法。从图5可知，本 -25 文算法规划出的航迹不是一些直线段的组合，主 20 要是因为需要满足无人机的各种约束，本实验主 15 文献101的代价图 基于Voronoi图的代价图 要是指速度和加速度约束。 表2航迹规划比较 0 5101520253035404550 Table 2 Comparison of trajectory planning 方法 代价 图4基于不同代价图航迹规划示意图 Fig.4 The diagram of trajectory planning based on differ- SAS 54.1176 ent cost maps RHC-PSO 55.5463 从实验结果可知，文献[8]的代价图会使得航 RRT 64.5409 线尽量靠近障碍物的顶点，在有扰动的情况下， 很可能导致飞行器与障碍物发生碰撞。本文提出 实验三RHC-FPSO与RHC-PSO算法对比 的代价图可以使得规划出的航迹远离障碍物，提 考虑多旋翼无人机，因其具有悬停功能，因此 高了无人机生存概率，但是规划出的航迹会比文 最小速度可以为零，设初始位置为(25,0)，目标 献[8]的航迹代价高，因此可以综合考虑各种因 为(-25，-3)。实验结果如图6所示，为将关于未 数，选择合适的代价图。 来H,步内的预测航迹的计算结果分解开，RHC-FPSO了实现。 多旋翼无人机其动力学可以由具有速度和加 速度约束的质点动力学模型近似[9]。 [ r(k+1) v(k+1) ] = A [ r(k) v(k) ] + Ba(k) (18) A = [ I2 ∆tI2 O2 I2 ] B = [ (∆t) 2 /2I2 ∆tI2 ] r v a I2 O2 式中： ； ； 、 、 分 别为位置、速度、加速度向量； 和 表示二维单 位矩阵和零矩阵。 输出约束：一般的输出约束包括障碍物约束 (19)，速度约束 (20)，加速度约束 (21) [I2,O2]x(k+ j) < O (19) ∥[O2,I2]x(k+ j|k)∥2 ⩽ vmax (20) ∥a(k+ j|k)∥2 ⩽ amax (21) O ⊂ R 2 式中： 为不可飞区域， vmax和amax分别为最大 速度和最大加速度约束。 仿真环境：处理器：PC (Intel (R)) i3-3240 CPU 3.40 GHz；操作系统：Windows 7；仿真软件：MAT￾LAB 2016。表 1 为仿真的实验参数，参数设置参 考文献[9，18]。本文实验部分为 (–25 m，25 m) 到 (–15 m，12 m) 的一个二维规划环境。 实验一 VORONOI 代价图与传统代价图对比 图 4 为根据文献[8]的代价图和本文提出的基 于 VORONOI 图的代价图的对比。黑色方块表示 障碍区域，按照 VORONOI 图的定义，所选取的可 视点都是距离障碍区最远的点，并且地图边框也 是不可飞区域，可视点如图 4 所示。文献[8]的可 视点为多边形的顶点。 0 5 10 15 20 25 30 35 40 45 50 5 10 15 20 25 30 35 40 45 50 航迹 文献 [10] 的代价图 基于 Voronoi 图的代价图 x y 图 4 基于不同代价图航迹规划示意图 Fig. 4 The diagram of trajectory planning based on differ￾ent cost maps 从实验结果可知，文献[8]的代价图会使得航 线尽量靠近障碍物的顶点，在有扰动的情况下， 很可能导致飞行器与障碍物发生碰撞。本文提出 的代价图可以使得规划出的航迹远离障碍物，提 高了无人机生存概率，但是规划出的航迹会比文 献[8]的航迹代价高，因此可以综合考虑各种因 数，选择合适的代价图。 实验二 RHC-FPSO 算法与传统航迹规划算 法对比 图 5 将传统经典的航迹规划方法 SAS 和 RRT 与本文提出的算法进行了对比。稀疏 A*搜索 (SAS) 算法是启发式搜索算法 A*的一种改进，首 先将规划环境表示为网格的形式，通过预先确定 的代价函数寻找最小代价的航迹。本文在扩展节 点时，通过把约束条件结合到搜索算法中去，减 少了搜索空间，进一步缩短了规划时间。RRT 通 过对规划环境中的采样点进行碰撞检测来获取空 间中的障碍物的信息,可以避免对规划环境进行 建模。RRT 首先选择开始点作为随机树的根节 点，通过随机探索方向点来扩展随机树，直到扩 展到目标点，然后从目标点回溯到根节点，完成 航迹规划。因此 SAS 算法在空间表示精度下是 全局最优解，而 RRT 是局部最优解。 −25 −20 −15 −10 −5 0 5 10 15 20 25 −15 −10 −5 0 5 10 RRT A* RHC-PSO x y 图 5 3 种航迹规划方法对比图 Fig. 5 Comparison of three trajectory planning methods 从图 5 和表 2 可以看出，在以距离为代价的 航迹规划算法中，稀疏 A*搜索算法是 3 种算法里 面航迹规划最短的算法，RRT 是随机算法，是算 法里面航迹最长的，并且不是最优路径，RHC￾FPSO 算法不是距离最短的算法。从图 5 可知，本 文算法规划出的航迹不是一些直线段的组合，主 要是因为需要满足无人机的各种约束，本实验主 要是指速度和加速度约束。 表 2 航迹规划比较 Table 2 Comparison of trajectory planning 方法 代价 SAS 54.117 6 RHC-PSO 55.546 3 RRT 64.540 9 实验三 RHC-FPSO 与 RHC-PSO 算法对比 Hp 考虑多旋翼无人机，因其具有悬停功能，因此 最小速度可以为零，设初始位置为 (25, 0)，目标 为 (–25, –3)。实验结果如图 6 所示，为将关于未 来 步内的预测航迹的计算结果分解开，RHC-FPSO 第 4 期 王文彬，等：基于滚动时域的无人机动态航迹规划 ·529·