复杂环境下多UAV协同可飞行路径规划研究*

高晓冬，杨秀霞，张 毅

（1.海军装备部军械装备局，北京 100081；2.海军航空大学，山东 烟台 264001）

0 引言

目前，无人飞行器（UAV）无论在军事上还是民用上都得到了越来越广泛的应用。为提高UAV在复杂环境下执行任务的能力，通常采用多UAV协同，此时也对多UAV到达目的地的时间和飞行角度提出了要求。对UAV的路径规划主要有航路点规划及曲线规划两种。路径点规划只考虑路径点处UAV的性能约束，而没有在整个航迹上考虑，航迹的曲率变化会比较大，这样会导致实际飞行航迹与规划所得到的航迹之间存在较大误差。因此，要得到真正可飞行的航迹，必须考虑路径的曲率。

一些学者提出应用曲线来直接规划整个航迹，如文献[1]提出利用Dubins曲线来规划多UAV航迹。而从微分几何的原理来看，要得到平滑的飞行器运动轨迹，路径的前两阶导数至少是存在的，即曲线的曲率是连续的，而Dubin路径是C1类曲线，但不是C2类曲线。文献[2]提出将Bézier曲线应用于机器人的路径规划中，胡峰[3]将Bézier曲线应用于多UAV路径规划。Bézier曲线可基于起始终止点的位置和切向量使UAV即时生成路径，并且在路径的任意点处都可以进行路径重规划，并到达目标位置。

目前，Pythagorean Hodograph（PH）曲线[4-6]被广泛应用于飞行器的在线航迹规划中。PH曲线规划航迹为C2类曲率连续航迹，用于航迹规划相比其他曲线有好多优点：1）整体曲率比较小；2）曲线的长度、曲率、弯曲能量能以闭合形式求出；3）起始和终止点的位置和方向直接被用于边界条件；4）长度和曲率容易协调。PH曲线生成可基于Bézier样条曲线，因此，其还拥有Bézier曲线规划的优点。

文献[7-8]给出了多UAV同时到达的PH曲线生成方法，多UAV协同时，选最长的航迹作为参考航迹；考虑航迹的曲率等性能约束，若不满足要求，考虑将PH控制点的切向量增长，此时整体路径长度会增加，文中没有对二者的最优协调曲线参数选择进行研究，并且没有考虑复杂威胁环境下的多UAV协同。文献[9]提出一种复杂环境下的多UAV路径规划算法，该算法基于速度障碍圆弧法，采用PH曲线对UAV进行避障路径重规划，但没有考虑多UAV之间的协同。

1 基于PH曲线的可飞行路径生成及控制参数研究

1.1 PH路径的生成

采用五次PH曲线进行航迹规划，它具有弯曲点且能为飞行航迹提供足够灵活的最低阶次PH曲线[5]。UAV进行路径规划时，给出初始点P0和目标点P5处的位姿信息，，就可以规划出满足UAV性能约束的可飞行路径r（t）：

其中，（x0，y0）为出发点P0的坐标，（x5，y5）为目标点P5的坐标，θ0和θ5分别表示为出发点P0和目标点P5处的方向角，t为控制参数，表示UAV的性能约束条件。

文献[8]给出了二维五次 PH 路径r（t）基于控制点的表达形式

其中，u（t）和 v（t）可设置为

1.2 PH路径约束条件研究

首先，为了满足UAV的动态性能要求，所规划的PH路径需要满足UAV的最大曲率约束，即。

另外，为了能够实现多UAV路径规划在目标点P5处的角度协同，需要在PH路径规划时能够在目标点处考虑UAV的飞行角度约束。而五次PH路径规划的初始化条件中，目标点处UAV的位姿信息包含了UAV的飞行方向角θ5，因此，可将多UAV在目标点处协同的飞行角度作为规划UAV可飞行PH路径初始化条件中的位姿信息，从而实现多UAV在目标点处的方向角协同。

1.3 PH路径长度的控制参数研究

1.3.1 PH路径长度与控制参数关系的数学推导

PH曲线满足勾股条件，矢端曲线是曲线的切线也即曲线的一阶导数，因此，与速度矢量等价。以t为参数的r（t）的长度可写为如下形式：

PH航迹的长度、曲率和等距线均具有闭合的多项式表达式。选择合适的多项式σ（t），使其满足

则可通过对多项式σ（t）的积分算出航迹长度 s（t），而 σ（t）本身即为速度。曲线的路径长度为：

式（13）中，均匀变化的t可以得到沿曲线均匀变化的路径长度。

由式（2）、式（3）和式（12）、式（13）可得

再将式（4）和式（5）代入式（14），可确定 σ（t）的二次多项式形式为

即

将 u0，u1，u2，v0，v1和 v2看作变量，此时多项式σ（t）的阶次为

由文献[8]可知，变量 u0，u1，u2，v0，v1，v2都可以表达成为和一个函数的乘积。所以，多项式σ（q）可表示为

由于 θ0，θ5，θP0P5为设定的路径规划初始化条件，根据式（19）可得，PH路径的长度s与参数k和ε有关。而当ε的值较小时，ε对PH路径长度的影响较小，可认为PH路径长度s与出发点和目标点处切向量长度k具有‘类线性’关系。

1.3.2 PH路径长度与控制参数关系的仿真验证

将UAV路径规划条件进行初始化，对PH路径长度s与切向量长度k之间的‘类线性’关系进行仿真验证。仿真图1给出了其中一种路径规划初始化条件下的仿真结果，从图1中可以看出规划的PH路径长度s与切向量长度k之间为‘类线性’关系。

1.4 时间和角度约束下的多UAV协同路径规划

对于多UAV协同路径规划，可充分利用PH路径长度s与切向量长度k之间的‘类线性’关系，可快速规划出多UAV路径长度相差不大的PH路径，再通过简单的速度控制即可实现多UAV到达目标点处的时间协同。另外，可将目标点P5的飞行角度协同融入到PH路径规划的初始化条件中。

2 多UAV自主避障协同路径规划

2.1 复杂环境下UAV自主避障规划

UAV在复杂环境飞行过程中，可能会遭遇未知动态障碍物，但UAV可利用自身携带的传感器装置探测到周围环境中的障碍物，并获取它们的位置和速度信息，UAV探测到未知动态障碍物的情况示意图如下页图2所示。为简化UAV对未知动态障碍物的避碰，将UAV当作一个点，同时根据障碍物与UAV之间的相对大小，将障碍物‘膨化’为一个半径为R的圆。那么，过UAV位置坐标点作障碍圆的切线l1和l2，则UAV与障碍物之间形成了障碍锥CC（Collision Cone）。障碍物的位置坐标为，速度矢量为；UAV的位置坐标为，速度矢量为。另外，设定UAV携带的传感器探测距离为d0。

首先，需要根据相应的初始化条件，判断UAV探测到的障碍物威胁性。当α≥α0时，障碍物不具有威胁性；当α＜α0时，障碍物具有威胁性。其中，α为相对速度矢量vu0与位置矢量之间的夹角，α0为障碍锥的半顶角。α和α0可通过下列关系进行求解，

对威胁障碍物的避碰可通过速度障碍VO进行求解，速度障碍示意图如图3所示。图3中的速度障碍VO是将示意图2中的障碍锥CC平移障碍物的速度矢量vo得到。为实现UAV对威胁障碍物的避碰，需要将UAV的速度矢量vu旋转出速度障碍VO，图3给出了相应的旋转示意图。

假设UAV的速度大小不变，将速度矢量末端旋转至速度障碍VO的边界上，对应的UAV与障碍物之间相对速度矢量vuo位于速度障碍边界l1和l2上，从而可实现UAV对威胁障碍物的避碰。根据示意图3中的几何关系，可对UAV避碰的期望速度矢量vu进行求解，

又

2.2 复杂环境下多UAV避障协同路径规划

复杂环境下，UAV在飞行过程中可能探测到周围存在的威胁障碍物，需对其进行避碰，此时多UAV协同路径规划需考虑因避障路径重规划产生的影响。为实现复杂环境下的多UAV协同，设定以最长避障重规划路径作为多UAV协同的参考路径。

3 多UAV自主避障协同路径规划仿真

3.1 多UAV协同路径规划仿真

对时间和角度约束下基于PH曲线的多UAV协同路径规划方法进行仿真验证。各UAV的仿真初始化条件如表1所示。

表1 多UAV协同路径规划初始化条件及路径长度

下页图4给出了多UAV协同路径规划仿真图。通过计算，规划出的各UAV的PH路径长度分别为 559.3、560.0、559.6、559.8，则各 UAV 的 PH 路径长度相差不大，再通过简单的速度控制，即可实现各UAV到达目标点的时间协同。另外，各UAV到达目标点处的飞行角度为初始化条件的角度，到达目标点处的角度协同也可实现。

3.2 多UAV避障协同路径规划仿真

在3.1节仿真过程中，假设UAV2探测到在处存在一动态障碍O，‘膨化’半径为R=45，其速度大小为V0=40，速度方向与OX轴的夹角分别为θo=195°，各UAV速度Vu=55。

通过计算，UAV2需要对障碍物O进行避障，仿真结果如图5所示。从图5中可以看出，UAV2成功实现了避障，此时各PH路径的长度分别为559.3，560.7，559.6，559.8，各 PH 路径长度相差也不大，仍可以通过简单的速度控制实现时间协同。

4 结论

通过PH曲线路径规划实现多UAV到达目标点的时间和角度协同。将多UAV到达目标点处的飞行角度协同融入到规划PH路径的初始化条件中。推导并仿真验证了PH路径长度与控制参数之间的关系，得出了PH路径长度s与切向量长度k之间‘类线性’关系，据此可快速确定多UAV路径长度近似相等的控制参数k，避免了为实现多UAV时间协同而进行的迭代过程，大大地降低了算法的复杂度和协同规划时间。另外，对于复杂环境下的多UAV协同，首先采用速度障碍法进行避障，采用PH曲线进行路径重规划，通过UAV速度及PH曲线生成参数的控制，也可实现多UAV到达目标点处的时间和角度协同。