一种基于稀疏A*搜索的三维多UAV协同航迹规划方法

摘要

本发明属于路径规划技术领域，具体涉及一种基于稀疏A*搜索的多UAV协同航迹规划方法。本发明包括：对路径规划的环境进行建模；初始化多目标SAS计算参数：包括最小航迹段长度，最大拐弯角、最大爬升/下滑角，UAV最小安全距离，UAV最低飞行高度；初始化UAV的位置，每个UAV代表一条航迹；更新UAV的位置；扩展当前节点；判断是否与其它航迹段发生碰撞；更新航迹段的节点表；如果已经达到步骤（2）中设定的最小航迹代价，则执行步骤（8），否则，执行步骤（3）；确定协同规划最优路径，路径规划结束。本发明能够解决多目标优化问题，具有通用性。能够为决策者提供合理的最优解，更符合实际问题需要。

主权项

一种基于稀疏A*搜索的三维多UAV协同航迹规划方法，其特征在于：(1)对路径规划的环境进行建模使用500km*500km范围的真实地形生成的200*200像素大小的数字高程地图，相邻像素之间的真实地形间距为2.5km；在三维空间中进行路径规划，S为UAV的出发点，G为终点，在路径规划范围内建立全局坐标系O‑XYZ，若n个路径点组成一个路径，则路径表示为L＝{S,L₁,L₂,...,L_n,G}，其中(L₁,L₂,...,L_n)为全局地图中的路径点的序列，为路径规划的目标；(2)初始化多目标SAS计算参数：包括最小航迹段长度，最大拐弯角、最大爬升/下滑角，UAV最小安全距离，UAV最低飞行高度；初始化UAV的位置，每个UAV代表一条航迹；(3)更新UAV的位置；(4)扩展当前节点扩展步长L为最小航迹段长度，当前节点B包括UAV的经度、纬度、高度(x,y,z)，UAV的飞行航向角为θ，与x轴，y轴，z轴的夹角分别为a，b，c，UAV的拐角g，UAV的爬升/俯冲角l，对于当前节点B有9个扩展节点，n系为地球坐标系，b系为载体坐标系，n系绕Z轴逆时针旋转角得到系，系绕Y轴逆时针旋转β角得到b系，N为单位向量，其中，β＝90°‑c，N＝[1,0,0]^T，D₁为b系绕z轴逆时针旋转g度得到的矩阵，$D_1=\left[\begin{array}{ccc}\cos g& \sin g& 0\\ -\sin g& \cos g& 0\\ 0& 0& 1\end{array}\right]$根据坐标变换可得到矩阵C1：$C1={\left[{\left(C_n^b\right)}^{-1}{\left(D_1\right)}^{-1}N\right]}^T,$C点坐标为：C＝[x,y,z]+L*C1同理，可得到扩展点D，E，F，G，H，I，J，K在地球坐标系的坐标；通过解算得到扩展点的坐标，计算每个扩展节点的代价，找到代价最小的节点，以代价最小的点为当前节点，最终找到从起始点到目标点的协同最优航迹其中每条航迹的代价函数为：$f\left(x_j\right)=\chi \left(\right(\underset{i=1}{\overset{5}{\Sigma }}{\lambda }_i{C}_i)+\alpha L(x_j\left)\right);$式中，x_j代表第j条航迹，f(x_j)代表第j条航迹的代价，C_i(i＝1,2,…,5)分别代表第i条航迹的最小航迹距离代价，最大转弯角代价，目标进入方向代价，最大爬升/俯冲角代价，最长航迹距离代价，飞行高度代价，距离威胁区代价的约束条件，即当满足约束条件时C_i取值为零，不满足条件时，C_i取一个极大的正整数，λ_i(i＝1,2,…,5)为其代价系数，L(x_j)为第j条航迹的协同航程代价，α为在航迹代价f(x_j)中的代价系数，χ为收缩因子，$\chi =\frac{a-n}{a},$式中，固定常数a为M_max的3～10倍，M_max＝航迹起始点到终点直线距离最大值/步长L，且a＞n_max，n为扩展到当前节点的航迹段数，收缩因子的取值范围为[0,1]；第j条航迹当前节点的航迹代价为：L_j＝L_G+L_H，其中，L_G为已扩展航迹，L_H为预估计达到目标点航迹，扩展到当前节点的协同航程为：L_X＝max{L₁,L₂,…,L_n}，其中，L₁,L₂,...,L_n为n个UAV搜索到各自当前节点的航迹代价，第j条航迹当前节点的协同航程代价为：$L\left(x_j\right)=\left\{\begin{array}{cc}|L_j-L_X|& L_j\ne L_X\\ L_j& L_j=L_X\end{array}\right.;$(5)判断是否与其它航迹段发生碰撞如果航迹段与其它航迹段没有交点，则执行步骤(6)；否则，执行步骤(3)；(6)更新航迹段的节点表把步骤(4)产生的合格扩展点增加到航迹段的节点表中，形成新的航迹段；(7)如果已经达到步骤(2)中设定的最小航迹代价，则执行步骤(8)，否则，执行步骤(3)；(8)确定协同规划最优路径，路径规划结束更新完的航迹段即为一组最优解的集合，选择最优路径作为路径规划的结果。