第4期 王文彬，等：基于滚动时域的无人机动态航迹规划 ·527· 3.1代价图 因此整个平面被划分成n个单元，具有以下性质： 根据第1节分析可知，从航迹规划全局看，滚 任意一点g位于点P所对应的单元中，当且仅当对 动时域优化是局部优化，因此容易陷入局部最 于任意的P,j≠都有dist(q,P)<dist(q,P)o 优。如图2所示，这种情况下无人机陷入了局部 根据定义1可知，VORONOI多边形的每条边 最优，目标不可达，因此需要估计预测航迹端点 上的点到相对应的两个点的距离相等，即VORO 到目标点的代价。代价估计阶段根据给出的障碍 NOI多边形上的点是到障碍物或威胁点的最远 物和目标预先计算，当探测到环境发生改变将会 点。因此可以得出，飞行器以VORONOI多边形 重新计算。航迹规划阶段使用预先计算的代价图 的顶点作为可视点，可以提高飞行器的安全系数。 来估计航迹端点到目标点的代价。 定理1 VORONOI图定理22：若n>3,则在 与平面上任意n个基点相对应的VORONOI图中， 顶点的数目不会超过2n-5,而且边的数目不会超 00 过3n-6。 0 由定理1可知，采用基于VORONOI图的可 视点的方法，相对于文献[8]，可视点呈线性增长， 并且小于2n。 3.2基于RHC-FPSO在线航迹规划 航迹规划是复杂约束条件下的最优化问题， -1 采用粒子群等智能优化算法进行航迹规划时，往 10-8-6-4-2024 往算法迭代初期，控制输入和状态可能存在违反 图2不含代价图的航迹规划示意图 约束的情况，算法需要迭代若干次才能产生满足 Fig.2 The diagram of trajectory planning without cost 要求的输入，并在此基础上进一步寻优。粒子群 map 算法中种群最优个体影响着粒子搜索方向，如何 文献[8]提出的代价图表示航迹端点到目标点 缩短搜索到可行输入的时间对提高算法的效率至 的时间估计值。基本思想为：在不考虑无人机自 关重要。因此，我们根据粒子违反约束条件的程 身的动力学约束条件下，对于多边形的障碍物， 度来更新粒子速度。 连接开始点、障碍物顶点和目标点，如果这些点 评价函数设计：评价函数用于给粒子群中每 的连线没有穿过障碍物，则添加一条边，这样形 个个体计算适应度。所有约束条件都满足的输入 成的图称为可视图。对于可视图使用Dijkstra2山 粒子称为可行个体，违反任何一项约束条件的个 单源最短路径算法，搜索从目标点到各个点的最 体称为不可行个体。在进行个体评价时遵循以下 短路径。由第3节问题描述可知，航迹端点为 准则： x(k+H。+1),因此基于代价图的终端惩罚项f(*)可 1)对于可行输入来说，根据式(3)，代价小的 以表示如下： 输入优于代价大的输人； f(x(++1).x))=d(x(k++1).xvis)+Cvis (10) 式中：x为基于代价图的可视点，dx(k+Hp+1),x) 2)对于不可行航迹来说，约束违背小的输入 为航迹端点到可视点的代价，C为可视点到目标 优于约束违背大的输入； 3)对于可行输人与不可行输人，可行输入总 点的代价。 是优于不可行输入： 然而，基于文献[8]提出的代价图，选择的可 视点为障碍物的顶点，这会导致规划出的航迹会 不可行输入意味着在实际中输入是不可行 沿着障碍物边缘，降低无人机安全性。因此，我 的，因此没有必要计算它的代价，可以减少计算 们引入基于VORONOI图的可视图，VORONOI图 量。基于以上分析，对任意输入，采用下面的评 的优点是使得可视点离障碍物的距离尽可能远： 价函数。 VORONOI图使得可视点的选择不再是障碍物的 C(u),可行解 F(u)= BigM+C,不可行解 (11) 顶点，而是障碍物之间的顶点连线的中点。 定义1 VORONOI图22。任意两点p和q之 式中：Cw为式(T)计算的航迹代价，BigM为一个 间的欧式距离，记作dist(p,q),假设平面P= 比较大的数。C为约束违背量，采用式(12)计算方式： {p1,P2,…,P为平面上的任意n个互异的点，P所对 C (wif()+w28(v:)+w3h(a)) (12) 应的VORONOI图是平面的一个子区域的划分，3.1 代价图 根据第 1 节分析可知，从航迹规划全局看，滚 动时域优化是局部优化，因此容易陷入局部最 优。如图 2 所示，这种情况下无人机陷入了局部 最优，目标不可达，因此需要估计预测航迹端点 到目标点的代价。代价估计阶段根据给出的障碍 物和目标预先计算，当探测到环境发生改变将会 重新计算。航迹规划阶段使用预先计算的代价图 来估计航迹端点到目标点的代价。 −10 −8 −6 −4 −2 0 2 4 −10 −5 0 5 x y 图 2 不含代价图的航迹规划示意图 Fig. 2 The diagram of trajectory planning without cost map x(k+ Hp +1) f(∗) 文献[8]提出的代价图表示航迹端点到目标点 的时间估计值。基本思想为：在不考虑无人机自 身的动力学约束条件下，对于多边形的障碍物， 连接开始点、障碍物顶点和目标点，如果这些点 的连线没有穿过障碍物，则添加一条边，这样形 成的图称为可视图。对于可视图使用 Dijkstra[21] 单源最短路径算法，搜索从目标点到各个点的最 短路径。由第 3 节问题描述可知，航迹端点为 ，因此基于代价图的终端惩罚项 可 以表示如下： f(x(k+ Hp +1), χF)) = d(x(k+ Hp +1), xvis)+Cvis (10) xvis d(x(k+ Hp +1), xvis) Cvis 式中： 为基于代价图的可视点， 为航迹端点到可视点的代价， 为可视点到目标 点的代价。 然而，基于文献[8]提出的代价图，选择的可 视点为障碍物的顶点，这会导致规划出的航迹会 沿着障碍物边缘，降低无人机安全性。因此，我 们引入基于 VORONOI 图的可视图，VORONOI 图 的优点是使得可视点离障碍物的距离尽可能远， VORONOI 图使得可视点的选择不再是障碍物的 顶点，而是障碍物之间的顶点连线的中点。 p q dist(p,q) {p1, p2,··· , pn} n P 定义 1 VORONOI 图 [22]。任意两点 和 之 间的欧式距离，记作 ，假设平 面 P = 为平面上的任意 个互异的点， 所对 应的 VORONOI 图是平面的一个子区域的划分， n q Pi Pj j , i dist(q,Pi) < dist(q,Pj) 因此整个平面被划分成 个单元，具有以下性质： 任意一点 位于点 所对应的单元中，当且仅当对 于任意的 ， 都有 。 根据定义 1 可知，VORONOI 多边形的每条边 上的点到相对应的两个点的距离相等，即 VORO￾NOI 多边形上的点是到障碍物或威胁点的最远 点。因此可以得出，飞行器以 VORONOI 多边形 的顶点作为可视点，可以提高飞行器的安全系数。 n > 3 n 2n−5 3n−6 定理 1 VORONOI 图定理[22] ：若 ，则在 与平面上任意 个基点相对应的 VORONOI 图中， 顶点的数目不会超过 ，而且边的数目不会超 过 。 2n 由定理 1 可知，采用基于 VORONOI 图的可 视点的方法，相对于文献[8]，可视点呈线性增长， 并且小于 。 3.2 基于 RHC-FPSO 在线航迹规划 航迹规划是复杂约束条件下的最优化问题， 采用粒子群等智能优化算法进行航迹规划时，往 往算法迭代初期，控制输入和状态可能存在违反 约束的情况，算法需要迭代若干次才能产生满足 要求的输入，并在此基础上进一步寻优。粒子群 算法中种群最优个体影响着粒子搜索方向，如何 缩短搜索到可行输入的时间对提高算法的效率至 关重要。因此，我们根据粒子违反约束条件的程 度来更新粒子速度。 评价函数设计：评价函数用于给粒子群中每 个个体计算适应度。所有约束条件都满足的输入 粒子称为可行个体，违反任何一项约束条件的个 体称为不可行个体。在进行个体评价时遵循以下 准则： 1) 对于可行输入来说，根据式 (3)，代价小的 输入优于代价大的输入； 2) 对于不可行航迹来说，约束违背小的输入 优于约束违背大的输入； 3) 对于可行输入与不可行输入，可行输入总 是优于不可行输入； 不可行输入意味着在实际中输入是不可行 的，因此没有必要计算它的代价，可以减少计算 量。基于以上分析，对任意输入，采用下面的评 价函数。 F(u) = { C(u), 可行解 BigM+C, 不可行解 (11) C(u) C 式中： 为式 (7) 计算的航迹代价，BigM 为一个 比较大的数。 为约束违背量，采用式 (12) 计算方式： C = ∑Hp i=1 (w1 f(li)+w2g(vi)+w3h(ai)) (12) 第 4 期 王文彬，等：基于滚动时域的无人机动态航迹规划 ·527·