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

说明书

技术领域

本发明属于路径规划技术领域，具体涉及一种基于稀疏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度得到的矩阵，