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

摘要

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

说明书

技术领域

本发明属于路径规划技术领域，具体涉及一种基于稀疏A*搜索的多UAV协同航迹规划 方法。

背景技术

作为任务规划系统的核心之一，航迹规划是一门伴随现代信息技术而发展起来的高新技 术，它在特定约束条件下寻找从初始位置到目标位置且满足某种性能指标的飞行轨迹。无人 飞行器航迹规划是实现飞行器自主导航的一项关键技术，无论在理论上还是实际应用上都具 有重大意义，是人工智能领域及导航与制导领域中的重要研究方向之一。飞行器安全突防技 术经历了从地形跟随技术、地形跟随/地形回避综合技术直到目前的航迹规划技术。早期发展 的地形跟随技术容易暴露飞行器的位置，不具备自动回避威胁的飞行能力。地形跟随/地形回 避技术只能规划出局部航迹。

与机器人二维路径规划相比，UAV航迹规划在三维空间搜索，其规划空间要大得多。一 些文献只考虑规则的包含多边形障碍物的规划环境，而UAV航迹规划需要考虑实际的战场 环境，不但地形特征复杂，而且包括各种威胁、禁飞等区域。同时，UAV航迹规划还需要考 虑多种不同的约束条件，因此简单采用机器人路径规划算法不能满足UAV航迹规划的要求。 准确地说，无人飞行器航迹规划就是在综合考虑UAV到达时间、燃料消耗、威胁以及飞行 区域等因素的前提下，为飞行器规划出一条最优，或者是最满意的飞行航迹，以保证圆满完 成飞行任务。目前，对于多目标路径规划问题已经有了一些研究成果，但是大多数文献为了 简化问题，通常采用加权法把多个性能指标函数组合成一个标量函数，使之转化为单目标优 化问题进行解决，如申请号为200910113086.8的专利采用加权法把即时子目标、安全性子目 标和平稳性子目标组合成一个目标函数进行路径规划；加权法简单直观，但运行一次只能得 到一个解，且存在权重选取的问题，要求对问题的本身有很强的先验认识，当所考虑的环境 发生改变时，要相应改变权重。

稀疏A*搜索（Sparse A*Search，SAS）是Szczerba等提出了一种改进的A*算法。该算 法通过把约束条件结合到搜索算法中去，可以有效地修剪搜索空间中的无用节点，从而大大 缩短了搜索时间，同时它允许在规划过程中输入不同的航迹约束并在某一任务期间改变这些 参数的值。Szczerba的方法虽然在一定条件下能够满足实时应用要求，但它也是在二维平面 上进行航迹搜索，因而有其不可克服的缺陷。由于SAS算法是一种全局规划方法，其规划时 间随着规划区域的增大而增大，当规划区域很大且多次遇到已知威胁时，如每一次都用SAS 算法重新进行航迹规划，其耗时将是非常巨大的。

发明内容

本发明的目的在于提出一种有效地进行地形回避和威胁回避的基于SAS算法的多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({\left(C_n^b\right)}^{-1}{\left(D_1\right)}^{-1}N\right)\right]}^T,$

C点坐标为：

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

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

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

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

$f\left(x_j\right)=\chi (\left(\underset{i=1}{\overset{5}{\Sigma }}{\lambda }_i{C}_i\right)+\mathrm{\alpha L}\left(x_j\right));$

式中，x_j代表第j条航迹，f(x_j)代表第j条航迹的代价，C_i(i＝1,2,…,5)分别代表第i条 航迹的最小航迹距离代价，最大转弯角代价，目标进入方向代价，最大爬升/俯冲角代价，最 长航迹距离代价，飞行高度代价，距离威胁区代价的约束条件，即当满足约束条件时C_i取值 为零，不满足条件时，C_i取一个极大的正整数，λ_i(i＝1,2,…,5)为其代价系数，为第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_i\ne L_x\\ L_j& L_j={L:}_x\end{array}\right);$

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

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

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

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

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

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

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

收缩因子χ的取值为χ∈[0,1]。

$L\left(x_j\right)=\left(\begin{array}{cc}|L_j-L_x|& L_i\ne L_x\\ L_j& L_j={L:}_x\end{array}\right).$

本发明的有益效果在于：

本发明提出的基于稀疏A*搜索的多UAV协同航迹规划方法，能够解决多目标优化问题， 比经典的多目标进化法、多目标粒子群法更为简单易行，具有通用性。本发明采用多目标SAS 算法解决同时考虑多个性能指标的路径规划问题，能够为决策者提供合理的最优解，更符合 实际问题需要。

附图说明

图1是本发明提出的基于稀疏A*搜索的多UAV协同航迹规划方法的流程图。

图2是本发明中进行路径规划带威胁区的数字高程侧视图。

图3是本发明中改进SAS算法的三维扩展图。

图4是本发明中UAV姿态从n系到b系坐标转换。

具体实施方式

下面结合附图和实施例对本发明进一步说明。

实施例1

一种基于稀疏A*搜索的三维多UAV协同航迹规划方法，具体包括以下几个步骤。

步骤一：对路径规划的环境进行数学建模

威胁空间建模作为航迹规划的关键技术之一，是地形、地貌和地面威胁分布的数据记录 空间，是进行航迹规划的信息来源和计算依据。数字地图利用数字化技术，将地形、地貌等 信息以数据的形式存储起来，以便飞机的各种电子设备调用。

在本专利中使用的是500km*500km范围的真实地形生成的200*200像素大小的数字高程 地图，相邻像素之间的真实地形间距为2.5km。（见图2）

在三维空间中进行路径规划，S为UAV的出发点，G为终点，在路径规划范围内建立全 局坐标系O-XYZ，若n个路径点组成一个路径，则路径表示为L＝{S,L₁,L₂,...,L_n,G}，其中 (L₁,L₂,...,L_n)为全局地图中的路径点的序列，为路径规划的目标；

步骤二：初始化多目标SAS算法

首先，初始化多目标SAS算法的参数：最小航迹段长度，最大拐弯角和最大爬升/下滑角， 各UAV最小安全距离，各UAV最低飞行高度；初始化UAV的位置，每个UAV代表一条 航迹。

步骤三：更新UAV的位置

步骤五或步骤七不满足约束条件时，重新寻找合适的节点进行规划。

步骤四：扩展当前节点

扩展步长L为最小航迹段长度，当前节点B包括以下信息，UAV的经度、纬度、高度 (x,y,z)。UAV的飞行航向角为θ，与x轴，y轴，z轴的夹角分别为a，b，c。UAV的拐角g， UAV的爬升/俯冲角l。对于当前节点B有9个扩展节点。（见图3）

n系为地球坐标系，b系为载体坐标系。n系绕Z轴逆时针旋转角得到系，系绕 Y轴逆时针旋转β角得到b系，N为单位向量。其中，β＝90°-c，N＝[1,0,0]^T。（见图4）

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)---\left(7\right)$

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

$C1={\left[\left({\left(C_n^b\right)}^{-1}{\left(D_1\right)}^{-1}N\right)\right]}^T,---\left(8\right)$

C点坐标为：

C＝[x,y,z]+L*C1    (9)

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

通过解算得到扩展点的坐标，计算每个扩展节点的代价，找到代价最小的节点，以代价 最小的点为当前节点重复上面操作，最终找到从起始点到目标点的协同最优航迹。结合坐标 变换的SAS算法三维扩展有以下优点：

（a）扩展的节点便于计算。

（b）规划出的航迹便于UAV飞行，更贴近UAV真实航迹。

（c）与UAV导航信息一致，更有利于操作人员控制UAV。

首先找到代价值最小的扩展点作为当前节点，然后用改进SAS算法计算每条航迹的代价 函数。

改进SAS算法中每条航迹的代价函数为：

$f\left(x_j\right)=\chi (\left(\underset{i=1}{\overset{5}{\Sigma }}{\lambda }_i{C}_i\right)+\mathrm{\alpha L}\left(x_j\right))---\left(1\right)$

式中，x_j代表第j条航迹，f(x_j)代表第j条航迹的代价。C_i(i＝1,2,…,5)分别代表第i条 航迹的最小航迹距离代价，最大转弯角代价，目标进入方向代价，最大爬升/俯冲角代价，最 长航迹距离代价，飞行高度代价，距离威胁区代价等约束条件，即当满足约束条件时C_i取值 为零，不满足条件时，C_i取一个极大的正整数，使航迹代价f(x_j)变大导致该条航迹不容易 被选取，λ_i(i＝1,2,…,5)分别为其代价系数，具体取值与每个UAV所处环境有关，L(x_j)为第j条航迹的协同航程代价，α为其在航迹代价f(x_j)中的代价系数。χ为收缩因子。

（1）收缩因子

收缩因子的计算公式为：

$\chi =\frac{a-n}{a}---\left(2\right)$

式中，固定常数a为一个经验值，一般取M_max的3～10倍（M_max=航迹起始点到终点直 线距离最大值/步长L），且a＞n_max，n为扩展到当前节点的航迹段数。根据收缩因子的计算 公式可知收缩因子的取值范围为[0,1]。

通过引入收缩系数χ，可以使得航迹代价随着航迹段数的增大而减小，从而使得算法收 敛速度加快。从收缩因子的计算公式可知，a的取值越大，随着航迹段数的增大，航迹代价 减小的越不明显，即算法收敛速度增加的越不明显。a的取值越小，随着航迹段数的增大， 航迹代价减小的越明显，即算法收敛速度增加的越明显。

（2）协同航程

在计算协同航程之前必须先计算单条航迹的航迹代价，第j条航迹当前节点的航迹代价 为：

L_j＝L_G+L_H   (3)

其中，L_G为已扩展航迹，L_H为预估计达到目标点航迹。

扩展到当前节点的协同航程的计算公式为：

L_X＝max{L₁,L₂,…,L_n}    (4)

其中，L₁,L₂,...,L_n为n个UAV搜索到各自当前节点的航迹代价。

第j条航迹当前节点的协同航程代价计算公式为：

$L\left(x_j\right)=\left(\begin{array}{cc}|L_j-L_x|& L_i\ne L_x\\ L_j& L_j={L:}_x\end{array}\right)---\left(5\right)$

由于协同航程是随着当前节点的变化而改变的，所以协同航程总是在随时变化的，这样 便可以使得算法能够跳出SAS算法中的一些局部无限循环搜索，同时航迹的协同率可以得到 大大的提高。将协同航程代价设计为式5所示，使得第j条航迹当前节点等于协同航程，该 段航迹的多UAV航迹约束等于最短航迹规划约束，可以使得协同航程最短。

步骤五：判断是否与其它航迹段发生碰撞

如果此航迹段与其它航迹段没有交点，则转到步骤六；否则，转到步骤三。

步骤六：更新该航迹段的节点表

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

步骤七：是否达到最小航迹代价

如果已经达到步骤二中设定的最小航迹代价，则转到步骤八，否则，转到步骤三。

步骤八：确定协同规划最优路径，路径规划结束

最后更新完的航迹段即为一组最优解的集合，根据实际问题需要，从中选择一条最优路 径作为路径规划的结果。

所述收缩因子χ的取值为χ∈[0,1]。

改进协同航程代价计算公式$L\left(x_j\right)=\left(\begin{array}{cc}|L_j-L_x|& L_i\ne L_x\\ L_j& L_j={L:}_x\end{array}\right).$可以使得算法能够跳出SAS算 法中的一些局部无限循环搜索，同时航迹的协同率可以得到大大提高。

结合坐标系和UAV航姿，通过解算得到扩展节点的坐标，计算每个扩展节点的代价， 找到代价最小的节点。

实施例2

本发明提出的一种基于稀疏A*搜索的多UAV协同航迹规划方法，如图1所示，具体包 括以下几个步骤。

步骤一：对路径规划的环境进行数学建模

威胁空间建模作为航迹规划的关键技术之一，是地形、地貌和地面威胁分布的数据记录 空间，是进行航迹规划的信息来源和计算依据。数字地图利用数字化技术，将地形、地貌等 信息以数据的形式存储起来，以便飞机的各种电子设备调用。

在本专利中使用的是500km*500km范围的真实地形生成的200*200像素大小的数字高程 地图，相邻像素之间的真实地形间距为2.5km。（见图2）

在三维空间中进行路径规划，S为UAV的出发点，G为终点，在路径规划范围内建立全 局坐标系O-XYZ，若n个路径点组成一个路径，则路径表示为L＝{S,L₁,L₂,...,L_n,G}，其中 (L₁,L₂,...,L_n)为全局地图中的路径点的序列，为路径规划的目标；

步骤二：初始化多目标SAS算法

首先，初始化多目标SAS算法的参数：最小航迹段长度，最大拐弯角和最大爬升/下滑角， 各UAV最小安全距离，各UAV最低飞行高度；初始化UAV的位置，每个UAV代表一条 航迹。

步骤三：更新UAV的位置

步骤五或步骤七不满足约束条件时，重新寻找合适的节点进行规划。

步骤四：扩展当前节点

扩展步长L为最小航迹段长度，当前节点B包括以下信息，UAV的经度、纬度、高度 (x,y,z)。UAV的飞行航向角为θ，与x轴，y轴，z轴的夹角分别为a，b，c。UAV的拐角g， UAV的爬升/俯冲角l。对于当前节点B有9个扩展节点。（见图3）

n系为地球坐标系，b系为载体坐标系。n系绕Z轴逆时针旋转角得到系，系绕 Y轴逆时针旋转β角得到b系，N为单位向量。其中，β＝90°-c，N＝[1,0,0]^T。（见图4）

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)---\left(7\right)$

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

$C1={\left[\left({\left(C_n^b\right)}^{-1}{\left(D_1\right)}^{-1}N\right)\right]}^T,---\left(8\right)$

C点坐标为：

C＝[x,y,z]+L*C1    (9)

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

通过解算得到扩展点的坐标，计算每个扩展节点的代价，找到代价最小的节点，以代价 最小的点为当前节点重复上面操作，最终找到从起始点到目标点的协同最优航迹。结合坐标 变换的SAS算法三维扩展有以下优点：

（a）扩展的节点便于计算。

（b）规划出的航迹便于UAV飞行，更贴近UAV真实航迹。

（c）与UAV导航信息一致，更有利于操作人员控制UAV。

首先找到代价值最小的扩展点作为当前节点，然后用改进SAS算法计算每条航迹的代价 函数。

改进SAS算法中每条航迹的代价函数为：

$f\left(x_j\right)=\chi (\left(\underset{i=1}{\overset{5}{\Sigma }}{\lambda }_i{C}_i\right)+\mathrm{\alpha L}\left(x_j\right))---\left(1\right)$

式中，x_j代表第j条航迹，f(x_j)代表第j条航迹的代价。C_i(i＝1,2,…,5)分别代表第i条 航迹的最小航迹距离代价，最大转弯角代价，目标进入方向代价，最大爬升/俯冲角代价，最 长航迹距离代价，飞行高度代价，距离威胁区代价等约束条件，即当满足约束条件时C_i取值 为零，不满足条件时，C_i取一个极大的正整数，使航迹代价f(x_j)变大导致该条航迹不容易 被选取，λ_i(i＝1,2,…,5)分别为其代价系数，具体取值与每个UAV所处环境有关，L(x_j)为第j条航迹的协同航程代价，α为其在航迹代价f(x_j)中的代价系数。χ为收缩因子。

（1）收缩因子

收缩因子的计算公式为：

$\chi =\frac{a-n}{a}---\left(2\right)$

式中，固定常数a为一个经验值，一般取M_max的3～10倍（M_max=航迹起始点到终点直 线距离最大值/步长L），且a＞n_max，n为扩展到当前节点的航迹段数。根据收缩因子的计算 公式可知收缩因子的取值范围为[0,1]。

通过引入收缩系数χ，可以使得航迹代价随着航迹段数的增大而减小，从而使得算法收 敛速度加快。从收缩因子的计算公式可知，a的取值越大，随着航迹段数的增大航迹代价减 小的越不明显，即算法收敛速度增加的越不明显。a的取值越小，随着航迹段数的增大航迹 代价减小的越明显，即算法收敛速度增加的越明显。

（2）协同航程

在计算协同航程之前必须先计算单条航迹的航迹代价，第j条航迹当前节点的航迹代价 为：

L_j＝L_G+L_H    (3)

其中，L_G为已扩展航迹，L_H为预估计达到目标点航迹。

扩展到当前节点的协同航程的计算公式为：

L_X＝max{L₁,L₂,…,L_n}    (4)

其中，L₁,L₂,...,L_n为n个UAV搜索到各自当前节点的航迹代价。

第j条航迹当前节点的协同航程代价计算公式为：

$L\left(x_j\right)=\left(\begin{array}{cc}|L_j-L_x|& L_i\ne L_x\\ L_j& L_j={L:}_x\end{array}\right)---\left(5\right)$

由于协同航程是随着当前节点的变化而改变的，所以协同航程总是在随时变化的，这样 便可以使得算法能够跳出SAS算法中的一些局部无限循环搜索，同时航迹的协同率可以得到 大大的提高。将协同航程代价设计为式5所示，使得第j条航迹当前节点等于协同航程，该 段航迹的多UAV航迹约束等于最短航迹规划约束，可以使得协同航程最短。

步骤五：判断是否与其它航迹段发生碰撞

如果此航迹段与其它航迹段没有交点，则转到步骤六；否则，转到步骤三。

步骤六：更新该航迹段的节点表

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

步骤七：是否达到最小航迹代价

如果已经达到步骤二中设定的最小航迹代价，则转到步骤八，否则，转到步骤三。

步骤八：确定协同规划最优路径，路径规划结束

最后更新完的航迹段即为一组最优解的集合，根据实际问题需要，从中选择一条最优路 径作为路径规划的结果。

实施例3

针对现有技术中存在的问题，本发明对基本A*算法进行改进，提出一种基于SAS算法 的多UAV协同航迹规划方法。本发明提供的方法区别于现有方法的显著特征在于：其一， 将SAS方法扩展到三维空间，给出一种三维航迹规划算法。该算法充分地利用了规划环境的 三维信息，可以有效地进行地形回避和威胁回避。实验证明该算法快速、有效，其规划的航 迹具有自动地进行地形回避和威胁回避的能力，优于二维航迹。其二，本发明针对多目标路 径规划问题，在规划中同时考虑多个路径性能指标，一次规划就能够得到一组最优解集，具 有很大的灵活性。这种路径规划方法不同于传统的只针对单一目标的路径规划方法和采用加 权法把多目标转化为单目标的路径规划方法，能更好地满足路径规划的实际需要。

一种基于稀疏A*搜索的多UAV协同航迹规划方法，具体包括以下几个步骤。

步骤一：对路径规划的环境进行数学建模

威胁空间建模作为航迹规划的关键技术之一，是地形、地貌和地面威胁分布的数据记录 空间，是进行航迹规划的信息来源和计算依据。数字地图利用数字化技术，将地形、地貌等 信息以数据的形式存储起来，以便飞机的各种电子设备调用。

在本专利中使用的是500km*500km范围的真实地形生成的200*200像素大小的数字高程 地图，相邻像素之间的真实地形间距为2.5km。（见图2）

在三维空间中进行路径规划，S为UAV的出发点，G为终点，在路径规划范围内建立全 局坐标系O-XYZ，若n个路径点组成一个路径，则路径表示为L＝{S,L₁,L₂,...,L_n,G}，其中 (L₁,L₂,...,L_n)为全局地图中的路径点的序列，为路径规划的目标；

步骤二：初始化多目标SAS算法

首先，初始化多目标SAS算法的参数：最小航迹段长度，最大拐弯角和最大爬升/下滑角， 各UAV最小安全距离，各UAV最低飞行高度；初始化UAV的位置，每个UAV代表一条 航迹。

步骤三：更新UAV的位置

步骤五或步骤七不满足约束条件时，重新寻找合适的节点进行规划。

步骤四：扩展当前节点

扩展步长L为最小航迹段长度，当前节点B包括以下信息，UAV的经度、纬度高度 (x,y,z)。UAV的飞行航向角为θ，与x轴，y轴，z轴的夹角分别为a，b，c。UAV的拐角g， UAV的爬升/俯冲角l。对于当前节点B有9个扩展节点。（见图3）

n系为地球坐标系，b系为载体坐标系。n系绕Z轴逆时针旋转角得到系，系绕 Y轴逆时针旋转β角得到b系，N为单位向量。其中，β＝90°-c，N＝[1,0,0]^T。（见图4）

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)---\left(7\right)$

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

$C1={\left[\left({\left(C_n^b\right)}^{-1}{\left(D_1\right)}^{-1}N\right)\right]}^T,---\left(8\right)$

C点坐标为：

C＝[x,y,z]+L*C1   (9)

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

通过解算得到扩展点的坐标，计算每个扩展节点的代价，找到代价最小的节点，以代价 最小的点为当前节点重复上面操作，最终找到从起始点到目标点的协同最优航迹。结合坐标 变换的SAS算法三维扩展有以下优点：

（a）扩展的节点更便于计算。

（b）规划出的航迹更便于UAV飞行，更贴近UAV真实航迹。

（c）与UAV导航信息一致，更有利于操作人员控制UAV。

首先找到代价值最小的扩展点作为当前节点，然后用改进SAS算法计算每条航迹的代价 函数。

改进SAS算法中每条航迹的代价函数为：

$f\left(x_j\right)=\chi (\left(\underset{i=1}{\overset{5}{\Sigma }}{\lambda }_i{C}_i\right)+\mathrm{\alpha L}\left(x_j\right))---\left(1\right)$

式中，x_j代表第j条航迹，f(x_j)代表第j条航迹的代价。C_i(i＝1,2,…,5)分别代表第i条 航迹的最小航迹距离代价，最大转弯角代价，目标进入方向代价，最大爬升/俯冲角代价，最 长航迹距离代价，飞行高度代价，距离威胁区代价等约束条件，即当满足约束条件时C_i取值 为零，不满足条件时，C_i取一个极大的正整数，使航迹代价f(x_j)变大导致该条航迹不容易 被选取，λ_i(i＝1,2,…,5)分别为其代价系数，具体取值与每个UAV所处环境有关，L(x_j)为第j条航迹的协同航程代价，α为其在航迹代价f(x_j)中的代价系数。χ为收缩因子。

（1）收缩因子

收缩因子的计算公式为：

$\chi =\frac{a-n}{a}---\left(2\right)$

式中，固定常数a为一个经验值，一般取M_max的3～10倍（M_max=航迹起始点到终点直 线距离最大值/步长L），且a＞n_max，n为扩展到当前节点的航迹段数。根据收缩因子的计算 公式可知收缩因子的取值范围为[0,1]。

通过引入收缩系数χ，可以使得航迹代价随着航迹段数的增大而减小，从而使得算法收 敛速度加快。从收缩因子的计算公式可知，a的取值越大，随着航迹段数的增大航迹代价减 小的越不明显，即算法收敛速度增加的越不明显。a的取值越小，随着航迹段数的增大航迹 代价减小的越明显，即算法收敛速度增加的越明显。

（2）协同航程

在计算协同航程之前必须先计算单条航迹的航迹代价，第j条航迹当前节点的航迹代价 为：

L_j＝L_G+L_H    (3)

其中，L_G为已扩展航迹，L_H为预估计达到目标点航迹。

扩展到当前节点的协同航程的计算公式为：

L_X＝max{L₁,L₂,…,L_n}    (4)

其中，L₁,L₂,...,L_n为n个UAV搜索到各自当前节点的航迹代价。

第j条航迹当前节点的协同航程代价计算公式为：

$L\left(x_j\right)=\left(\begin{array}{cc}|L_j-L_x|& L_i\ne L_x\\ L_j& L_j={L:}_x\end{array}\right)---\left(5\right)$

由于协同航程是随着当前节点的变化而改变的，所以协同航程总是在随时变化的，这样 便可以使得算法能够跳出SAS算法中的一些局部无限循环搜索，同时航迹的协同率可以得到 大大的提高。将协同航程代价设计为式5所示，使得第j条航迹当前节点等于协同航程，该 段航迹的多UAV航迹约束等于最短航迹规划约束，可以使得协同航程最短。

步骤五：判断是否与其它航迹段发生碰撞

如果此航迹段与其它航迹段没有交点，则转到步骤六；否则，转到步骤三。

步骤六：更新该航迹段的节点表

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

步骤七：是否达到最小航迹代价

如果已经达到步骤二中设定的最小航迹代价，则转到步骤八，否则，转到步骤三。

步骤八：确定协同规划最优路径，路径规划结束

最后更新完的航迹段即为一组最优解的集合，根据实际问题需要，从中选择一条最优路 径作为路径规划的结果。

本发明的优点在于：

第一，本发明提出的基于稀疏A*搜索的多UAV协同航迹规划方法，对A*算法进行改进， 提出一种多目标SAS算法。该算法能够解决多目标优化问题，比经典的多目标进化算法、多 目标粒子群算法更为简单易行，具有通用性。

第二，本发明提出的基于稀疏A*搜索的多UAV协同航迹规划方法，采用多目标SAS算 法解决同时考虑多个性能指标的路径规划问题，能够为决策者提供合理的最优解，符合实际 问题需要。