[发明专利]一种基于稀疏A*搜索的三维多UAV协同航迹规划方法

权利要求书

1.一种基于稀疏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为单位向量，其中，β=90o-c，N=[1,0,0]^T，

D₁为b系绕z轴逆时针旋转g度得到的矩阵，

D1=cosgsing0-singcosg0001]]>

根据坐标变换可得到矩阵C1：

C1=[((Cnb)-1(D1)-1N)]T,]]>

C点坐标为：

C=[x,y,z]+L*C1

同理，可得到扩展点D，E，F，G，H，I，J，K在地球坐标系的坐标；

通过解算得到扩展点的坐标，计算每个扩展节点的代价，找到代价最小的节点，以代价最小的点为当前节点，最终找到从起始点到目标点的协同最优航迹

其中每条航迹的代价函数为：

f(xj)=χ((Σi=15λiCi)+αL(xj));]]>

式中，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)中的代价系数，χ为收缩因子，

χ=a-na,]]>

式中，固定常数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(xj)=|Lj-Lx|Li≠LxLjLj=L:x;]]>

（5）判断是否与其它航迹段发生碰撞

如果航迹段与其它航迹段没有交点，则执行步骤（6）；否则，执行步骤（3）；

（6）更新航迹段的节点表

把步骤（4）产生的合格扩展点增加到航迹段的节点表中，形成新的航迹段；

（7）如果已经达到步骤（2）中设定的最小航迹代价，则执行步骤（8），否则，执行步骤（3）；

（8）确定协同规划最优路径，路径规划结束

更新完的航迹段即为一组最优解的集合，选择最优路径作为路径规划的结果。