《一种基于稀疏ASUP/SUP搜索的三维多UAV协同航迹规划方法.pdf》由会员分享，可在线阅读，更多相关《一种基于稀疏ASUP/SUP搜索的三维多UAV协同航迹规划方法.pdf（17页珍藏版）》请在专利查询网上搜索。
1、(10)申请公布号 CN 103557867 A (43)申请公布日 2014.02.05 CN 103557867 A (21)申请号 201310467041.7 (22)申请日 2013.10.09 G01C 21/20(2006.01) G05D 1/10(2006.01) G06F 17/50(2006.01) (71)申请人 哈尔滨工程大学 地址 150001 黑龙江省哈尔滨市南岗区南通 大街 145 号哈尔滨工程大学科技处知 识产权办公室 (72)发明人 刘利强 顾海超 杨裕杰 戴运桃 李宁 齐昭 汪相国 张凯 赵明 孟欣冉 (54) 发明名称 一种基于稀疏 A*搜索的三维多 U。
2、AV 协同航迹 规划方法 (57) 摘要 本发明属于路径规划技术领域， 具体涉及一 种基于稀疏 A* 搜索的多 UAV 协同航迹规划方法。 本发明包括 ： 对路径规划的环境进行建模 ； 初始 化多目标 SAS 计算参数 ： 包括最小航迹段长度， 最 大拐弯角、 最大爬升/下滑角， UAV最小安全距离， UAV 最低飞行高度 ； 初始化 UAV 的位置， 每个 UAV 代表一条航迹 ； 更新 UAV 的位置 ； 扩展当前节点 ； 判断是否与其它航迹段发生碰撞 ； 更新航迹段的 节点表 ； 如果已经达到步骤 （2） 中设定的最小航 迹代价， 则执行步骤 （8） ， 否则， 执行步骤 （3） ； 确。
3、 定协同规划最优路径， 路径规划结束。 本发明能够 解决多目标优化问题， 具有通用性。 能够为决策者 提供合理的最优解， 更符合实际问题需要。 (51)Int.Cl. 权利要求书 2 页 说明书 11 页 附图 3 页 (19)中华人民共和国国家知识产权局 (12)发明专利申请 权利要求书2页 说明书11页 附图3页 (10)申请公布号 CN 103557867 A CN 103557867 A 1/2 页 2 1. 一种基于稀疏 A* 搜索的三维多 UAV 协同航迹规划方法， 其特征在于 ： （1） 对路径规划的环境进行建模 使用 500km*500km 范围的真实地形生成的 200*200。
4、 像素大小的数字高程地图， 相邻像 素之间的真实地形间距为 2.5km ； 在三维空间中进行路径规划， S 为 UAV 的出发点， G 为终 点， 在路径规划范围内建立全局坐标系 O-XYZ， 若 n 个路径点组成一个路径， 则路径表示为 L=S,L1,L2,.,Ln,G， 其中 (L1,L2,.,Ln) 为全局地图中的路径点的序列， 为路径规划的 目标 ； （2） 初始化多目标 SAS 计算参数 ： 包括最小航迹段长度， 最大拐弯角、 最大爬升 / 下滑 角， UAV 最小安全距离， UAV 最低飞行高度 ； 初始化 UAV 的位置， 每个 UAV 代表一条航迹 ； （3） 更新 UAV 的。
5、位置 ； （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,0T， D1为 b 系绕 z 轴逆时针旋转 g 度得到的矩阵， 根据坐标变换可得到矩阵 C1 ： 。
6、C 点坐标为 ： C=x,y,z+L*C1 同理， 可得到扩展点 D， E， F， G， H， I， J， K 在地球坐标系的坐标 ； 通过解算得到扩展点的坐标， 计算每个扩展节点的代价， 找到代价最小的节点， 以代价 最小的点为当前节点， 最终找到从起始点到目标点的协同最优航迹 其中每条航迹的代价函数为 ： 式中， xj代表第 j 条航迹， f(xj) 代表第 j 条航迹的代价， Ci(i=1,2,5) 分别代表 第 i 条航迹的最小航迹距离代价， 最大转弯角代价， 目标进入方向代价， 最大爬升 / 俯冲角 代价， 最长航迹距离代价， 飞行高度代价， 距离威胁区代价的约束条件， 即当满足约束。
7、条件 时 Ci取值为零， 不满足条件时， Ci取一个极大的正整数， i(i=1,2,5) 为其代价系数， 权 利 要 求 书 CN 103557867 A 2 2/2 页 3 L(xj) 为第 j 条航迹的协同航程代价， 为在航迹代价 f(xj) 中的代价系数， 为收缩因子， 式中， 固定常数 a 为 Mmax的 3 10 倍， Mmax= 航迹起始点到终点直线距离最大值 / 步长 L， 且 anmax， n 为扩展到当前节点的航迹段数， 收缩因子的取值范围为 0,1 ； 第 j 条航迹当前节点的航迹代价为 ： Lj=LG+LH， 其中， LG为已扩展航迹， LH为预估计达到目标点航迹， 扩展。
8、到当前节点的协同航程为 ： LX=maxL1,L2,Ln， 其中， L1,L2,.,Ln为 n 个 UAV 搜索到各自当前节点的航迹代价， 第 j 条航迹当前节点的协同航程代价为 ： （5） 判断是否与其它航迹段发生碰撞 如果航迹段与其它航迹段没有交点， 则执行步骤 （6） ； 否则， 执行步骤 （3） ； （6） 更新航迹段的节点表 把步骤 （4） 产生的合格扩展点增加到航迹段的节点表中， 形成新的航迹段 ； （7） 如果已经达到步骤 （2） 中设定的最小航迹代价， 则执行步骤 （8） ， 否则， 执行步骤 （3） ； （8） 确定协同规划最优路径， 路径规划结束 更新完的航迹段即为一组最优。
9、解的集合， 选择最优路径作为路径规划的结果。 2.根据权利要求1中所述的一种基于稀疏A*搜索的多UAV协同航迹规划方法， 其特征 在于 ： 所述收缩因子 的取值为 0,1。 3.根据权利要求1中所述的一种基于稀疏A*搜索的多UAV协同航迹规划方法， 其特征 在于 ： 权 利 要 求 书 CN 103557867 A 3 1/11 页 4 一种基于稀疏 A* 搜索的三维多 UAV 协同航迹规划方法 技术领域 0001 本发明属于路径规划技术领域， 具体涉及一种基于稀疏A*搜索的多UAV协同航迹 规划方法。 背景技术 0002 作为任务规划系统的核心之一， 航迹规划是一门伴随现代信息技术而发展起来。
10、的 高新技术， 它在特定约束条件下寻找从初始位置到目标位置且满足某种性能指标的飞行轨 迹。无人飞行器航迹规划是实现飞行器自主导航的一项关键技术， 无论在理论上还是实际 应用上都具有重大意义， 是人工智能领域及导航与制导领域中的重要研究方向之一。飞行 器安全突防技术经历了从地形跟随技术、 地形跟随 / 地形回避综合技术直到目前的航迹规 划技术。早期发展的地形跟随技术容易暴露飞行器的位置， 不具备自动回避威胁的飞行能 力。地形跟随 / 地形回避技术只能规划出局部航迹。 0003 与机器人二维路径规划相比， UAV 航迹规划在三维空间搜索， 其规划空间要大得 多。一些文献只考虑规则的包含多边形障碍物。
11、的规划环境， 而 UAV 航迹规划需要考虑实际 的战场环境， 不但地形特征复杂， 而且包括各种威胁、 禁飞等区域。同时， UAV 航迹规划还需 要考虑多种不同的约束条件， 因此简单采用机器人路径规划算法不能满足 UAV 航迹规划的 要求。准确地说， 无人飞行器航迹规划就是在综合考虑 UAV 到达时间、 燃料消耗、 威胁以及 飞行区域等因素的前提下， 为飞行器规划出一条最优， 或者是最满意的飞行航迹， 以保证圆 满完成飞行任务。 目前， 对于多目标路径规划问题已经有了一些研究成果， 但是大多数文献 为了简化问题， 通常采用加权法把多个性能指标函数组合成一个标量函数， 使之转化为单 目标优化问题进。
12、行解决， 如申请号为 200910113086.8 的专利采用加权法把即时子目标、 安 全性子目标和平稳性子目标组合成一个目标函数进行路径规划 ； 加权法简单直观， 但运行 一次只能得到一个解， 且存在权重选取的问题， 要求对问题的本身有很强的先验认识， 当所 考虑的环境发生改变时， 要相应改变权重。 0004 稀疏 A* 搜索 （Sparse A*Search， SAS） 是 Szczerba 等提出了一种改进的 A* 算法。 该算法通过把约束条件结合到搜索算法中去， 可以有效地修剪搜索空间中的无用节点， 从 而大大缩短了搜索时间， 同时它允许在规划过程中输入不同的航迹约束并在某一任务期间 。
13、改变这些参数的值。Szczerba 的方法虽然在一定条件下能够满足实时应用要求， 但它也是 在二维平面上进行航迹搜索， 因而有其不可克服的缺陷。由于 SAS 算法是一种全局规划方 法， 其规划时间随着规划区域的增大而增大， 当规划区域很大且多次遇到已知威胁时， 如每 一次都用 SAS 算法重新进行航迹规划， 其耗时将是非常巨大的。 发明内容 0005 本发明的目的在于提出一种有效地进行地形回避和威胁回避的基于 SAS 算法的 多 UAV 协同航迹规划方法。 0006 本发明的目的是这样实现的 ： 说 明 书 CN 103557867 A 4 2/11 页 5 0007 （1） 对路径规划的环境。
14、进行建模 0008 使用 500km*500km 范围的真实地形生成的 200*200 像素大小的数字高程地图， 相 邻像素之间的真实地形间距为 2.5km ； 在三维空间中进行路径规划， S 为 UAV 的出发点， G 为 终点， 在路径规划范围内建立全局坐标系 O-XYZ， 若 n 个路径点组成一个路径， 则路径表示 为 L S,L1,L2,.,Ln,G， 其中 (L1,L2,.,Ln) 为全局地图中的路径点的序列， 为路径规 划的目标 ； 0009 （2） 初始化多目标SAS计算参数 ： 包括最小航迹段长度， 最大拐弯角、 最大爬升/下 滑角， UAV 最小安全距离， UAV 最低飞行高。
15、度 ； 初始化 UAV 的位置， 每个 UAV 代表一条航迹 ； 0010 （3） 更新 UAV 的位置 ； 0011 （4） 扩展当前节点 0012 扩展步长 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 。
16、-c，N 1,0,0T， 0013 0014 D1为 b 系绕 z 轴逆时针旋转 g 度得到的矩阵， 0015 0016 根据坐标变换可得到矩阵 C1 ： 0017 0018 C 点坐标为 ： 0019 C x,y,z+L*C1 0020 同理， 可得到扩展点 D， E， F， G， H， I， J， K 在地球坐标系的坐标 ； 0021 通过解算得到扩展点的坐标， 计算每个扩展节点的代价， 找到代价最小的节点， 以 代价最小的点为当前节点， 最终找到从起始点到目标点的协同最优航迹 0022 其中每条航迹的代价函数为 ： 0023 0024 式中， xj代表第 j 条航迹， f(xj) 代表第。
17、 j 条航迹的代价， Ci(i 1,2,5) 分别 代表第 i 条航迹的最小航迹距离代价， 最大转弯角代价， 目标进入方向代价， 最大爬升 / 俯 冲角代价， 最长航迹距离代价， 飞行高度代价， 距离威胁区代价的约束条件， 即当满足约束 条件时 Ci取值为零， 不满足条件时， Ci取一个极大的正整数， i(i 1,2,5) 为其代价 说 明 书 CN 103557867 A 5 3/11 页 6 系数，为第j条航迹的协同航程代价， 为在航迹代价f(xj)中的代价系数， 为收缩因子， 0025 0026 式中， 固定常数 a 为 Mmax的 3 10 倍， Mmax= 航迹起始点到终点直线距离最。
18、大值 / 步长 L， 且 a nmax， n 为扩展到当前节点的航迹段数， 收缩因子的取值范围为 0,1 ； 0027 第 j 条航迹当前节点的航迹代价为 ： 0028 Lj LG+LH， 0029 其中， LG为已扩展航迹， LH为预估计达到目标点航迹， 0030 扩展到当前节点的协同航程为 ： 0031 LX maxL1,L2,Ln， 0032 其中， L1,L2,.,Ln为 n 个 UAV 搜索到各自当前节点的航迹代价， 0033 第 j 条航迹当前节点的协同航程代价为 ： 0034 0035 （5） 判断是否与其它航迹段发生碰撞 0036 如果航迹段与其它航迹段没有交点， 则执行步骤 。
19、（6） ； 否则， 执行步骤 （3） ； 0037 （6） 更新航迹段的节点表 0038 把步骤 （4） 产生的合格扩展点增加到航迹段的节点表中， 形成新的航迹段 ； 0039 （7） 如果已经达到步骤 （2） 中设定的最小航迹代价， 则执行步骤 （8） ， 否则， 执行步 骤 （3） ； 0040 （8） 确定协同规划最优路径， 路径规划结束 0041 更新完的航迹段即为一组最优解的集合， 选择最优路径作为路径规划的结果。 0042 收缩因子 的取值为 0,1。 0043 0044 本发明的有益效果在于 ： 0045 本发明提出的基于稀疏A*搜索的多UAV协同航迹规划方法， 能够解决多目标优。
20、化 问题， 比经典的多目标进化法、 多目标粒子群法更为简单易行， 具有通用性。本发明采用多 目标 SAS 算法解决同时考虑多个性能指标的路径规划问题， 能够为决策者提供合理的最优 解， 更符合实际问题需要。 附图说明 0046 图 1 是本发明提出的基于稀疏 A* 搜索的多 UAV 协同航迹规划方法的流程图。 0047 图 2 是本发明中进行路径规划带威胁区的数字高程侧视图。 0048 图 3 是本发明中改进 SAS 算法的三维扩展图。 说 明 书 CN 103557867 A 6 4/11 页 7 0049 图 4 是本发明中 UAV 姿态从 n 系到 b 系坐标转换。 具体实施方式 005。
21、0 下面结合附图和实施例对本发明进一步说明。 0051 实施例 1 0052 一种基于稀疏 A* 搜索的三维多 UAV 协同航迹规划方法， 具体包括以下几个步骤。 0053 步骤一 ： 对路径规划的环境进行数学建模 0054 威胁空间建模作为航迹规划的关键技术之一， 是地形、 地貌和地面威胁分布的数 据记录空间， 是进行航迹规划的信息来源和计算依据。数字地图利用数字化技术， 将地形、 地貌等信息以数据的形式存储起来， 以便飞机的各种电子设备调用。 0055 在本专利中使用的是500km*500km范围的真实地形生成的200*200像素大小的数 字高程地图， 相邻像素之间的真实地形间距为 2.5。
22、km。 （见图 2） 0056 在三维空间中进行路径规划， S为UAV的出发点， G为终点， 在路径规划范围内建立 全局坐标系 O-XYZ， 若 n 个路径点组成一个路径， 则路径表示为 L S,L1,L2,.,Ln,G， 其 中 (L1,L2,.,Ln) 为全局地图中的路径点的序列， 为路径规划的目标 ； 0057 步骤二 ： 初始化多目标 SAS 算法 0058 首先， 初始化多目标 SAS 算法的参数 ： 最小航迹段长度， 最大拐弯角和最大爬升 / 下滑角， 各 UAV 最小安全距离， 各 UAV 最低飞行高度 ； 初始化 UAV 的位置， 每个 UAV 代表一条 航迹。 0059 步骤。
23、三 ： 更新 UAV 的位置 0060 步骤五或步骤七不满足约束条件时， 重新寻找合适的节点进行规划。 0061 步骤四 ： 扩展当前节点 0062 扩展步长 L 为最小航迹段长度， 当前节点 B 包括以下信息， UAV 的经度、 纬度、 高度 (x,y,z)。UAV 的飞行航向角为 ， 与 x 轴， y 轴， z 轴的夹角分别为 a， b， c。UAV 的拐角 g， UAV 的爬升 / 俯冲角 l。对于当前节点 B 有 9 个扩展节点。 （见图 3） 0063 n 系为地球坐标系， b 系为载体坐标系。n 系绕 Z 轴逆时针旋转角得到 系，系绕 Y 轴逆时针旋转 角得到 b 系， N 为单位。
24、向量。其中， 90 -c， N 1,0,0T。 （见图 4） 0064 0065 D1为 b 系绕 z 轴逆时针旋转 g 度得到的矩阵， 0066 0067 根据坐标变换可得到矩阵 C1 ： 说 明 书 CN 103557867 A 7 5/11 页 8 0068 0069 C 点坐标为 ： 0070 C x,y,z+L*C1 (9) 0071 同理， 可得到扩展点 D， E， F， G， H， I， J， K 在地球坐标系的坐标。 0072 通过解算得到扩展点的坐标， 计算每个扩展节点的代价， 找到代价最小的节点， 以 代价最小的点为当前节点重复上面操作， 最终找到从起始点到目标点的协同最优。
25、航迹。结 合坐标变换的 SAS 算法三维扩展有以下优点 ： 0073 （a） 扩展的节点便于计算。 0074 （b） 规划出的航迹便于 UAV 飞行， 更贴近 UAV 真实航迹。 0075 （c） 与 UAV 导航信息一致， 更有利于操作人员控制 UAV。 0076 首先找到代价值最小的扩展点作为当前节点， 然后用改进 SAS 算法计算每条航迹 的代价函数。 0077 改进 SAS 算法中每条航迹的代价函数为 ： 0078 0079 式中， xj代表第 j 条航迹， f(xj) 代表第 j 条航迹的代价。Ci(i 1,2,5) 分别 代表第 i 条航迹的最小航迹距离代价， 最大转弯角代价， 目。
26、标进入方向代价， 最大爬升 / 俯 冲角代价， 最长航迹距离代价， 飞行高度代价， 距离威胁区代价等约束条件， 即当满足约束 条件时 Ci取值为零， 不满足条件时， Ci取一个极大的正整数， 使航迹代价 f(xj) 变大导致该 条航迹不容易被选取， i(i 1,2,5) 分别为其代价系数， 具体取值与每个 UAV 所处环 境有关，L(xj) 为第 j 条航迹的协同航程代价， 为其在航迹代价 f(xj) 中的代价 系数。 为收缩因子。 0080 （1） 收缩因子 0081 收缩因子的计算公式为 ： 0082 0083 式中， 固定常数 a 为一个经验值， 一般取 Mmax的 3 10 倍 （Mm。
27、ax= 航迹起始点到终点 直线距离最大值 / 步长 L） ， 且 a nmax， n 为扩展到当前节点的航迹段数。根据收缩因子的 计算公式可知收缩因子的取值范围为 0,1。 0084 通过引入收缩系数 ， 可以使得航迹代价随着航迹段数的增大而减小， 从而使得 算法收敛速度加快。从收缩因子的计算公式可知， a 的取值越大， 随着航迹段数的增大， 航 迹代价减小的越不明显， 即算法收敛速度增加的越不明显。a 的取值越小， 随着航迹段数的 增大， 航迹代价减小的越明显， 即算法收敛速度增加的越明显。 0085 （2） 协同航程 0086 在计算协同航程之前必须先计算单条航迹的航迹代价， 第 j 条航。
28、迹当前节点的航 迹代价为 ： 0087 Lj LG+LH (3) 说 明 书 CN 103557867 A 8 6/11 页 9 0088 其中， LG为已扩展航迹， LH为预估计达到目标点航迹。 0089 扩展到当前节点的协同航程的计算公式为 ： 0090 LX maxL1,L2,Ln (4) 0091 其中， L1,L2,.,Ln为 n 个 UAV 搜索到各自当前节点的航迹代价。 0092 第 j 条航迹当前节点的协同航程代价计算公式为 ： 0093 0094 由于协同航程是随着当前节点的变化而改变的， 所以协同航程总是在随时变化 的， 这样便可以使得算法能够跳出 SAS 算法中的一些局部。
29、无限循环搜索， 同时航迹的协同 率可以得到大大的提高。将协同航程代价设计为式 5 所示， 使得第 j 条航迹当前节点等于 协同航程， 该段航迹的多 UAV 航迹约束等于最短航迹规划约束， 可以使得协同航程最短。 0095 步骤五 ： 判断是否与其它航迹段发生碰撞 0096 如果此航迹段与其它航迹段没有交点， 则转到步骤六 ； 否则， 转到步骤三。 0097 步骤六 ： 更新该航迹段的节点表 0098 把步骤四产生的合格扩展点增加到航迹段的节点表中， 形成新的航迹段。 0099 步骤七 ： 是否达到最小航迹代价 0100 如果已经达到步骤二中设定的最小航迹代价， 则转到步骤八， 否则， 转到步骤。
30、三。 0101 步骤八 ： 确定协同规划最优路径， 路径规划结束 0102 最后更新完的航迹段即为一组最优解的集合， 根据实际问题需要， 从中选择一条 最优路径作为路径规划的结果。 0103 所述收缩因子 的取值为 0,1。 0104 改进协同航程代价计算公式可以使得算法能够跳出 SAS 算法中的一些局部无限循环搜索， 同时航迹的协同率可以得到大大提高。 0105 结合坐标系和 UAV 航姿， 通过解算得到扩展节点的坐标， 计算每个扩展节点的代 价， 找到代价最小的节点。 0106 实施例 2 0107 本发明提出的一种基于稀疏 A* 搜索的多 UAV 协同航迹规划方法， 如图 1 所示， 具。
31、 体包括以下几个步骤。 0108 步骤一 ： 对路径规划的环境进行数学建模 0109 威胁空间建模作为航迹规划的关键技术之一， 是地形、 地貌和地面威胁分布的数 据记录空间， 是进行航迹规划的信息来源和计算依据。数字地图利用数字化技术， 将地形、 地貌等信息以数据的形式存储起来， 以便飞机的各种电子设备调用。 0110 在本专利中使用的是500km*500km范围的真实地形生成的200*200像素大小的数 字高程地图， 相邻像素之间的真实地形间距为 2.5km。 （见图 2） 0111 在三维空间中进行路径规划， S为UAV的出发点， G为终点， 在路径规划范围内建立 全局坐标系 O-XYZ，。
32、 若 n 个路径点组成一个路径， 则路径表示为 L S,L1,L2,.,Ln,G， 其 中 (L1,L2,.,Ln) 为全局地图中的路径点的序列， 为路径规划的目标 ； 说 明 书 CN 103557867 A 9 7/11 页 10 0112 步骤二 ： 初始化多目标 SAS 算法 0113 首先， 初始化多目标 SAS 算法的参数 ： 最小航迹段长度， 最大拐弯角和最大爬升 / 下滑角， 各 UAV 最小安全距离， 各 UAV 最低飞行高度 ； 初始化 UAV 的位置， 每个 UAV 代表一条 航迹。 0114 步骤三 ： 更新 UAV 的位置 0115 步骤五或步骤七不满足约束条件时， 。
33、重新寻找合适的节点进行规划。 0116 步骤四 ： 扩展当前节点 0117 扩展步长 L 为最小航迹段长度， 当前节点 B 包括以下信息， UAV 的经度、 纬度、 高度 (x,y,z)。UAV 的飞行航向角为 ， 与 x 轴， y 轴， z 轴的夹角分别为 a， b， c。UAV 的拐角 g， UAV 的爬升 / 俯冲角 l。对于当前节点 B 有 9 个扩展节点。 （见图 3） 0118 n系为地球坐标系， b系为载体坐标系。 n系绕Z轴逆时针旋转角得到系，系 绕Y轴逆时针旋转角得到b系， N为单位向量。 其中， 90-c， N 1,0,0T。 （见图 4） 0119 0120 D1为 b 。
34、系绕 z 轴逆时针旋转 g 度得到的矩阵， 0121 0122 根据坐标变换可得到矩阵 C1 ： 0123 0124 C 点坐标为 ： 0125 C x,y,z+L*C1 (9) 0126 同理， 可得到扩展点 D， E， F， G， H， I， J， K 在地球坐标系的坐标。 0127 通过解算得到扩展点的坐标， 计算每个扩展节点的代价， 找到代价最小的节点， 以 代价最小的点为当前节点重复上面操作， 最终找到从起始点到目标点的协同最优航迹。结 合坐标变换的 SAS 算法三维扩展有以下优点 ： 0128 （a） 扩展的节点便于计算。 0129 （b） 规划出的航迹便于 UAV 飞行， 更贴近。
35、 UAV 真实航迹。 0130 （c） 与 UAV 导航信息一致， 更有利于操作人员控制 UAV。 0131 首先找到代价值最小的扩展点作为当前节点， 然后用改进 SAS 算法计算每条航迹 的代价函数。 0132 改进 SAS 算法中每条航迹的代价函数为 ： 0133 说 明 书 CN 103557867 A 10 8/11 页 11 0134 式中， xj代表第 j 条航迹， f(xj) 代表第 j 条航迹的代价。Ci(i 1,2,5) 分别 代表第 i 条航迹的最小航迹距离代价， 最大转弯角代价， 目标进入方向代价， 最大爬升 / 俯 冲角代价， 最长航迹距离代价， 飞行高度代价， 距离威。
36、胁区代价等约束条件， 即当满足约束 条件时 Ci取值为零， 不满足条件时， Ci取一个极大的正整数， 使航迹代价 f(xj) 变大导致该 条航迹不容易被选取， i(i 1,2,5) 分别为其代价系数， 具体取值与每个 UAV 所处环 境有关，L(xj) 为第 j 条航迹的协同航程代价， 为其在航迹代价 f(xj) 中的代价 系数。 为收缩因子。 0135 （1） 收缩因子 0136 收缩因子的计算公式为 ： 0137 0138 式中， 固定常数 a 为一个经验值， 一般取 Mmax的 3 10 倍 （Mmax= 航迹起始点到终点 直线距离最大值 / 步长 L） ， 且 a nmax， n 为扩。
37、展到当前节点的航迹段数。根据收缩因子的 计算公式可知收缩因子的取值范围为 0,1。 0139 通过引入收缩系数 ， 可以使得航迹代价随着航迹段数的增大而减小， 从而使得 算法收敛速度加快。从收缩因子的计算公式可知， a 的取值越大， 随着航迹段数的增大航迹 代价减小的越不明显， 即算法收敛速度增加的越不明显。a 的取值越小， 随着航迹段数的增 大航迹代价减小的越明显， 即算法收敛速度增加的越明显。 0140 （2） 协同航程 0141 在计算协同航程之前必须先计算单条航迹的航迹代价， 第 j 条航迹当前节点的航 迹代价为 ： 0142 Lj LG+LH (3) 0143 其中， LG为已扩展航。
38、迹， LH为预估计达到目标点航迹。 0144 扩展到当前节点的协同航程的计算公式为 ： 0145 LX maxL1,L2,Ln (4) 0146 其中， L1,L2,.,Ln为 n 个 UAV 搜索到各自当前节点的航迹代价。 0147 第 j 条航迹当前节点的协同航程代价计算公式为 ： 0148 0149 由于协同航程是随着当前节点的变化而改变的， 所以协同航程总是在随时变化 的， 这样便可以使得算法能够跳出 SAS 算法中的一些局部无限循环搜索， 同时航迹的协同 率可以得到大大的提高。将协同航程代价设计为式 5 所示， 使得第 j 条航迹当前节点等于 协同航程， 该段航迹的多 UAV 航迹约。
39、束等于最短航迹规划约束， 可以使得协同航程最短。 0150 步骤五 ： 判断是否与其它航迹段发生碰撞 0151 如果此航迹段与其它航迹段没有交点， 则转到步骤六 ； 否则， 转到步骤三。 0152 步骤六 ： 更新该航迹段的节点表 0153 把步骤四产生的合格扩展点增加到航迹段的节点表中， 形成新的航迹段。 说 明 书 CN 103557867 A 11 9/11 页 12 0154 步骤七 ： 是否达到最小航迹代价 0155 如果已经达到步骤二中设定的最小航迹代价， 则转到步骤八， 否则， 转到步骤三。 0156 步骤八 ： 确定协同规划最优路径， 路径规划结束 0157 最后更新完的航迹段。
40、即为一组最优解的集合， 根据实际问题需要， 从中选择一条 最优路径作为路径规划的结果。 0158 实施例 3 0159 针对现有技术中存在的问题， 本发明对基本 A* 算法进行改进， 提出一种基于 SAS 算法的多 UAV 协同航迹规划方法。本发明提供的方法区别于现有方法的显著特征在于 ： 其 一， 将 SAS 方法扩展到三维空间， 给出一种三维航迹规划算法。该算法充分地利用了规划环 境的三维信息， 可以有效地进行地形回避和威胁回避。实验证明该算法快速、 有效， 其规划 的航迹具有自动地进行地形回避和威胁回避的能力， 优于二维航迹。其二， 本发明针对多 目标路径规划问题， 在规划中同时考虑多个。
41、路径性能指标， 一次规划就能够得到一组最优 解集， 具有很大的灵活性。这种路径规划方法不同于传统的只针对单一目标的路径规划方 法和采用加权法把多目标转化为单目标的路径规划方法， 能更好地满足路径规划的实际需 要。 0160 一种基于稀疏 A* 搜索的多 UAV 协同航迹规划方法， 具体包括以下几个步骤。 0161 步骤一 ： 对路径规划的环境进行数学建模 0162 威胁空间建模作为航迹规划的关键技术之一， 是地形、 地貌和地面威胁分布的数 据记录空间， 是进行航迹规划的信息来源和计算依据。数字地图利用数字化技术， 将地形、 地貌等信息以数据的形式存储起来， 以便飞机的各种电子设备调用。 016。
42、3 在本专利中使用的是500km*500km范围的真实地形生成的200*200像素大小的数 字高程地图， 相邻像素之间的真实地形间距为 2.5km。 （见图 2） 0164 在三维空间中进行路径规划， S为UAV的出发点， G为终点， 在路径规划范围内建立 全局坐标系 O-XYZ， 若 n 个路径点组成一个路径， 则路径表示为 L S,L1,L2,.,Ln,G， 其 中 (L1,L2,.,Ln) 为全局地图中的路径点的序列， 为路径规划的目标 ； 0165 步骤二 ： 初始化多目标 SAS 算法 0166 首先， 初始化多目标 SAS 算法的参数 ： 最小航迹段长度， 最大拐弯角和最大爬升 /。
43、 下滑角， 各 UAV 最小安全距离， 各 UAV 最低飞行高度 ； 初始化 UAV 的位置， 每个 UAV 代表一条 航迹。 0167 步骤三 ： 更新 UAV 的位置 0168 步骤五或步骤七不满足约束条件时， 重新寻找合适的节点进行规划。 0169 步骤四 ： 扩展当前节点 0170 扩展步长 L 为最小航迹段长度， 当前节点 B 包括以下信息， UAV 的经度、 纬度高度 (x,y,z)。UAV 的飞行航向角为 ， 与 x 轴， y 轴， z 轴的夹角分别为 a， b， c。UAV 的拐角 g， UAV 的爬升 / 俯冲角 l。对于当前节点 B 有 9 个扩展节点。 （见图 3） 01。
44、71 n系为地球坐标系， b系为载体坐标系。 n系绕Z轴逆时针旋转角得到系，系 绕Y轴逆时针旋转角得到b系， N为单位向量。 其中， 90-c， N 1,0,0T。 （见图 4） 说 明 书 CN 103557867 A 12 10/11 页 13 0172 0173 D1为 b 系绕 z 轴逆时针旋转 g 度得到的矩阵， 0174 0175 根据坐标变换可得到矩阵 C1 ： 0176 0177 C 点坐标为 ： 0178 C x,y,z+L*C1 (9) 0179 同理， 可得到扩展点 D， E， F， G， H， I， J， K 在地球坐标系的坐标。 0180 通过解算得到扩展点的坐标， 。
45、计算每个扩展节点的代价， 找到代价最小的节点， 以 代价最小的点为当前节点重复上面操作， 最终找到从起始点到目标点的协同最优航迹。结 合坐标变换的 SAS 算法三维扩展有以下优点 ： 0181 （a） 扩展的节点更便于计算。 0182 （b） 规划出的航迹更便于 UAV 飞行， 更贴近 UAV 真实航迹。 0183 （c） 与 UAV 导航信息一致， 更有利于操作人员控制 UAV。 0184 首先找到代价值最小的扩展点作为当前节点， 然后用改进 SAS 算法计算每条航迹 的代价函数。 0185 改进 SAS 算法中每条航迹的代价函数为 ： 0186 0187 式中， xj代表第 j 条航迹， 。
46、f(xj) 代表第 j 条航迹的代价。Ci(i 1,2,5) 分别 代表第 i 条航迹的最小航迹距离代价， 最大转弯角代价， 目标进入方向代价， 最大爬升 / 俯 冲角代价， 最长航迹距离代价， 飞行高度代价， 距离威胁区代价等约束条件， 即当满足约束 条件时 Ci取值为零， 不满足条件时， Ci取一个极大的正整数， 使航迹代价 f(xj) 变大导致该 条航迹不容易被选取， i(i 1,2,5) 分别为其代价系数， 具体取值与每个 UAV 所处环 境有关，L(xj) 为第 j 条航迹的协同航程代价， 为其在航迹代价 f(xj) 中的代价 系数。 为收缩因子。 0188 （1） 收缩因子 018。
47、9 收缩因子的计算公式为 ： 0190 0191 式中， 固定常数 a 为一个经验值， 一般取 Mmax的 3 10 倍 （Mmax= 航迹起始点到终点 说 明 书 CN 103557867 A 13 11/11 页 14 直线距离最大值 / 步长 L） ， 且 a nmax， n 为扩展到当前节点的航迹段数。根据收缩因子的 计算公式可知收缩因子的取值范围为 0,1。 0192 通过引入收缩系数 ， 可以使得航迹代价随着航迹段数的增大而减小， 从而使得 算法收敛速度加快。从收缩因子的计算公式可知， a 的取值越大， 随着航迹段数的增大航迹 代价减小的越不明显， 即算法收敛速度增加的越不明显。a 的取值越小， 随着航迹段数的增 大航迹代价减小的越明显， 即算法收敛速度增加的越明显。 0193 （2） 协同航程 0194 在计算协同航程之前必须先计算单条航迹的航迹代价， 第 j 条航迹当前节点的航 迹代价为 ： 0195 Lj LG+LH (3) 0196 其中， LG为已扩展航迹， LH为预估计达到目标点航迹。 0197 扩展到当前节点的协同航程的计算公式为 ： 0198 LX maxL1,L2,Ln (4) 0199 其中， L1,L2,.,Ln为 n 个 UAV 搜索到各自当前节点的航迹代价。 0200 第 j 条。