基于运动状态搜索与增强安全管道的直升机低空航迹规划方法与流程

1.本发明涉及的是一种飞行路径规划领域的技术，具体是一种基于运动状态搜索与增强安全管道的直升机低空航迹规划方法。


背景技术：

2.航迹规划是引导直升机低空飞行的核心技术，能为飞行员提供安全且便捷的参考航迹，辅助飞行任务的高效执行。该技术旨在自主规划一条从起始点到目标点的最优航迹，同时兼顾避障安全、能量损耗、规划对象机动特性、动力学约束等限制条件。
3.目前，直升机低空航迹规划的挑战之一是规划航迹机动可执行性弱。由于初始航迹的搜索空间高度离散，且对直升机动力学约束与机动特性考虑不足，容易引起规划航迹的平滑性较差、机动代价较大，进而造成可执行性弱的问题。此外，直升机执行低空突防时，常面临敌方雷达侦察与防空火力等新增战场威胁，容易造成规划航迹动态安全性弱的问题。


技术实现要素：

4.本发明针对现有技术对规划对象的运动建模不够精确，存在空间重叠与嵌套现象以及无法适用于复杂地形环境等不足，提出一种基于运动状态搜索与增强安全管道的直升机低空航迹规划方法，通过考虑直升机动力学约束与机动特性的状态空间，实时生成增强安全管道并作为后端航迹拟合参数的优化空间，显著改善规划航迹对直升机执行过程的适应性，有效提升直升机在低空动态威胁下的飞行安全性。
5.本发明是通过以下技术方案实现的：
6.本发明涉及一种基于运动状态搜索与增强安全管道的直升机低空航迹规划方法，首先基于栅格地图进行分段航迹规划后生成考虑动力学约束和机动特性的直升机运动状态空间，并通过扩展状态空间和建立代价函数后，采用基于改进混合a*算法在扩展后的状态空间中进行最优路径搜索；再在动态威胁实时更新的局部地图中，以初始航迹为基础，通过基于航迹安全性约束的膨胀、修剪与插补后生成增强安全管道；最后在增强安全管道内利用b样条曲线拟合航迹，并通过最小代价准则优化曲线参数，实现直升机低空航迹规划优化。
7.本发明涉及一种实现上述方法的系统，包括：全局地图与任务点输入模块、前端航迹搜索模块、增强安全管道模块和后端航迹优化模块，其中：全局地图与任务点输入模块根据原始地形数据，进行平滑与插值处理，生成航迹规划的任务空间；前端航迹搜索模块，通过建立考虑旋翼飞行器三轴控制机动差异的运动学模型，对航迹规划的任务空间进行扩展，并利用考虑安全裕度的改进混合a*算法搜索初始航迹；增强安全管道模块根据初始航迹的航迹点为中心膨胀出原始安全管道，通过修剪与插补得到具有空间连续和简洁表示的增强安全管道；后端航迹优化模块根据增强安全管道对航迹拟合控制点施加位置约束并生
成后端优化航迹用于与直升机执行，以保证优化航迹始终处于安全空间中的同时兼顾航迹与机动的平滑性准则。技术效果
8.本发明在前端航迹搜索的状态空间扩展阶段，对规划对象的三轴控制输入量施加尺度变换与运动学限制；在安全管道膨胀的基础上，引入子管道冗余合并与间断插补的检测与完善机制；与现有a*算法相比，本发明显著缩短航迹长度、提升规划成功率并进一步提升复杂场景下航迹规划的成功率、降低航迹执行代价。
附图说明
9.图1为本发明的整体流程图；
10.图2为实施例hybrid state a*算法流程图；
11.图3为本实施例采用的增强安全管道生成流程图；
12.图中：(a)为安全子管道膨胀流程图，(b)为安全子管道修剪流程图；
13.图4为本发明的实施效果与传统航迹规划方法的对比图；
14.图中：(a)为采用a*作为搜索前端的传统方法，(b)为本方法；
15.图5为规划航迹的运动学约束效果示意图；
16.图中：(a)、(b)分别为仿真实验输出的速度和加速度曲线；
17.图6为规划航迹对直升机的机动适应性效果示意图；
18.图中：(a)、(b)分别为不同尺度系数下的三轴运动特性曲线；
19.图7为实施例增强安全管道的效果示意图。
具体实施方式
20.如图1所示，为本实施例涉及一种基于运动状态搜索与增强安全管道的直升机低空航迹规划方法，包括：
21.步骤1)全局地图与任务点输入：将数字高程地图数据经过插值和滤波后，转化为栅格全局地图，并由用户输入任务航迹点，具体包括：原始地形平滑、栅格地图转换和任务点序列输入。
22.所述的原始地形平滑，通过三次样条曲线插值算法对地形数据补全后，采用二维高斯滤波对地形数据降噪得以实现。
23.所述的栅格地图的尺寸为40
×
40m，栅格分辨率为0.1m。
24.步骤2)前端航迹搜索：在全局地图中生成考虑动力学约束和机动特性的直升机运动状态空间，并通过扩展状态空间和建立代价函数后，采用基于改进混合a*算法在扩展后的状态空间中进行最优路径搜索。
25.所述的直升机运动状态空间是指：以加速度为作为三轴控制输入量的直升机状态转移方程其中：状态转移矩阵控制矩阵i3为3阶单位矩阵，0为相应维度填充的零矩阵；直升机的状态节点向量阶单位矩阵，0为相应维度填充的零矩阵；直升机的状态节点向量三轴加速度对应的三轴控制输入量直
升机的起始状态向量初始航迹的扩展规则为：
26.所述的扩展状态空间是指：对三轴控制输入量进行区间离散化，并进行尺度缩放，按离散形式进行树状轨迹扩展，并将其上的航迹点作为直升机状态空间，其中：三轴单位控制输入量u0的可行域ud＝[-u
max
，u
max
]，用刻度n对该可行域进行均匀划分以产生2n+1个离散的控制输入量，即对加速度的约束a
max
通过控制输入量的可行边界u
max
施加，而速度约束v
max
由加速度约束与分段轨迹扩展过程的时长共同施加。将三轴单位控制输入量u0(t)进行尺度变换，以近似单位控制输入量对直升机三轴机动响应的差异，得到三轴控制输入量为u(t)＝[u
x
(t)，uy(t)，uz(t)]
t
，u(t)＝λ
·
u0(t)，其中：变换系数矩阵λ＝diag(λ
x
，λy，λz)，t为采样周期。
[0027]
本实施例中n＝2。
[0028]
本实施例中对照组尺度变换参数的设置为λ
x
＝1，λy＝1，λz＝1，实验组为θ
x
＝1，λy＝1，λz＝0.5。
[0029]
本实施例中采样周期t＝0.001s。
[0030]
所述的建立代价函数，即扩展后的状态空间中的每个节点n的综合代价函数值f(n)＝g(n)+h(n)，其中：g(n)为度量当前状态节点距离起始点n
start
的机动代价，h(n)为当前状态节点距离目标点n
goal
的启发代价。
[0031]
如图2所示，所述的最优路径搜索包括：
[0032]
2.1)确定起始点node_start、目标点node_goal的位置和速度作为节点状态值，将起始节点加入待考察节点列表openlist中，并计算起始点的综合代价函数值f(n)；
[0033]
所述的待考察节点列表openlist在初始化阶段，包括起始点node_start、目标点node_goal，在运行阶段，不断加入由当前被检索节点扩展生成的相邻节点。
[0034]
2.2)当待考察节点列表openlist不为空集时，从中选取最小的综合代价函数值对应的节点node_current；
[0035]
2.3)当起始点node_start和目标点node_goal的综合代价函数值接近，则认为抵达目标，规划结束，否则指定步骤2.4；
[0036]
2.4)以当前节点node_current为起点进行运动微元扩展，并对扩展节点进行冗余删减，让每个栅格中最多保留一个节点，并将由前节点node_current扩展生成与冗余删减后的相邻节点集存储于由当前被遍历节点node_current根据运动微元法扩展出的相邻节点集合node_successor中；
[0037]
2.5)遍历相邻节点集合node_successor中的每个节点node，并对每个node执行步骤2.6；
[0038]
2.6)对尚未包含在closelist且能够通过碰撞检测的节点，计算当前扩展节点node到node_current的节点转移代价，叠加g(node_current)后作为node的临时代价g
temp
；
[0039]
所述的碰撞检测是指：计算被检测节点与地图中最近障碍物边界的距离，若距离非正，则说明被检测节点与障碍物碰撞。
[0040]
所述的节点转移代价是指：由节点间控制输入量的大小与运动时长度量，表征节点间转移所需的控制与时间成本。
[0041]
所述的临时代价g
temp
的公式为：g
temp
＝g(node_current)+(||u(t)||2+ρ)
·
τ，其中从节点node至node_current的移动过程中，u(t)表示对应的三轴控制输入量，τ表示移动时长，ρ为控制输入与时间成本的权衡系数。
[0042]
2.7)当扩展节点node尚未包含在openlist中时，将其加入，否则比较g(node)与临时代价g
temp
的大小，并将g(node)更新为两者中的较小值；
[0043]
2.8)计算并更新节点的综合代价值f(node)，并将node_current作为node的父节点。
[0044]
2.9)当openlist中的目标节点被访问后，得到最终返回的closelist即包含最优航迹中的点序列。
[0045]
步骤3)增强安全管道：在动态威胁实时更新的局部地图中，以包络初始航迹的管状空间为基础，通过基于航迹安全性约束的膨胀、修剪与插补生成增强安全管道；
[0046]
所述的增强安全管道是指：以长方体形状的子管道的集合对初始航迹进行包络的管状空间，表征了后端航迹拟合与优化时的安全范围。
[0047]
如图3(a)所示，所述的膨胀，具体包括：
[0048]
3.1)将以长方体的六个边界面所示的子管道初始化为初始航迹上离散点pi对应的x，y，z坐标p
ix
，p
ix
，p
iy
，p
iy
，p
iz
，p
iz
，同时初始化碰撞检测变量safe
←
true；
[0049]
3.2)以最大膨胀次数n为迭代上界，当膨胀边界与障碍物点集o通过碰撞检测，即safe＝true时，将子管道边界继续以ε为步长，沿空间坐标系的六个轴向膨胀，即c
μ
←cμ
+ε，μ∈{x
+
，x-，y
+
，y-，z
+
，z-}；
[0050]
3.3)当迭代次数达到上限或长方体边界与障碍物发生碰撞时，子管道膨胀终止，记以长方体6个边界描述的子管道为ci，则该算法对所有初始航迹点膨胀出的子管道集合为c＝{ci}，i∈{1，2，...，n}。
[0051]
所述的碰撞检测是指：计算增强安全管道边界与场景中最近障碍物的距离，若距离非正，则说明增强安全管道与障碍物发生碰撞。
[0052]
如图3(b)所示，所述的修剪与插补，具体包括：
[0053]
3.a)采用双循环方式遍历膨胀后得到的子管道集合：对外循环中当前被遍历的子管道集合{ci}中的子管道进行包含关系检测，当出现i≠j的情形，则从初始增强安全管道集合{ci}中删除被包含的冗余的内循环中被遍历的子管道cj；
[0054]
3.b)对遍历后的增强安全管道集合{ci}中的子管道进行连续性检测，当出现相邻子管道ci，cj不相交，即的情况，则在其对应的相邻航迹点pi，pj之间插入新航迹点p
ij
，并重新膨胀并生成新的子管道c
ij
；
[0055]
3.c)当遍历完初始增强安全管道中的全部子管道，且子管道全部通过冗余包含与空间连续性检测时，则得到修剪与插补后的增强安全管道。
[0056]
所述的包含关系检测是指：经双循环方式遍历膨胀后的子管道集合，对其中的子管道进行两两比较，检测其由长方体围成的空间是否具有包含关系。
[0057]
所述的连续性检测是指：经双循环方式遍历膨胀后的子管道集合，检测相邻子管道所占据的长方体之间，是否具有非空的空间交集。
[0058]
所述的修剪与插补是指：对双循环方式遍历膨胀后的子管道集合，通过包含关系检测和连续性检测，分别进行冗余子管道删减与间断空间补全的操作。
[0059]
步骤4)后端航迹优化：在步骤3得到的增强安全管道内，利用b样条曲线拟合航迹，并通过最小代价准则优化曲线参数，实现直升机低空航迹规划优化。
[0060]
所述的最小代价准则是指：增强安全管道对待优化的b样条控制点参数施加不等式约束，将保证后端优化出的航迹一定位于增强安全管道限定的空间内，具体为：后端航迹优化过程中的b样条曲线共用n+1个控制点拟合m段航迹，μ轴方向上的第i个控制点满足：其中：i∈{0，1，...，n}，j∈{0，1，...，m-1}，并对应第j个分段航迹，μ∈{x，y，z}，与分别为长方体所示的子管道cj在μ轴方向上两个边界面的位置。
[0061]
经过具体实际实验，在处理器为inter(r)core(tm)i5-7200u cpu@2.50ghz，内存为16g。仿真环境为ubuntu18.04系统中，ros melodic机器人仿真框架与rviz数据可视化工具。特别说明：为提高仿真效率，将真实飞行的物理尺度按10:1缩小(包括速度、加速度等)。
[0062]
在以a*算法为前端的实施例中，由于航迹搜索空间的离散度较高，且未考虑机动对象的运动特性，规划航迹的光滑性不高且机动代价较大，实验结果如图4(a)所示。本发明采用的前端基于改进混合a*算法，其搜索空间是由直升机基本动力学模型生成的连续性更强的状态空间，因而能有效提升规划航迹的安全与便捷性，在20和50次随机地形的仿真中，规划成功率分别提升10％和8％，实验结果如图4(b)和表1所示。
[0063]
表1：改进混合a*与a*前端搜索对比结果
[0064]
本发明在前端航迹搜索过程中考虑直升机的机动特性，并可根据不同直升机三轴耦合控制特性的差异，灵活调节尺度变换参数的相对大小，从而增强规划航迹对机动对象控制特性的适应性。实施例中，分别设置对照组的参数为λ
x
＝1,λy＝1,λz＝1，实验组为λ
x
＝1,λy＝1,λz＝0.5，其对应的三轴运动特性曲线如图5(a)和5(b)所示，数值结果见表2。
[0065]
表2：三轴加速度和速度约束效果
[0066]
此外，本发明在前端航迹搜索过程中还引入直升机的动力学约束，并可根据机动对象的实际情况灵活调节，从而使得规划航迹对机动对象的动力学特性具有较强的适应性。实施例中分别对仿真状态下的直升机施加v
max
＝2m/s和a
max
＝3m/s2的约束。从图6(a)和
6(b)所示的速度与加速度仿真曲线来看，整体规划航迹的运动状态被有效限制在阈值范围。
[0067]
最后，在基于运动状态搜索与增强安全管道的实施例中，设置增强安全管道的膨胀步长ε＝1m，最大膨胀次数n＝4，仿真效果如图7所示。在随机生成的20和50次仿真环境下，由于额外考虑动态环境下的安全约束，增强安全管道显著提升航迹规划的成功率，实验结果见表3。
[0068]
表3：增强安全管道对比实验结果
[0069]
与现有技术相比，本方法基于改进混合a*算法，在前端航迹搜索的状态空间扩展阶段，对直升机三轴控制输入施加尺度变换实现对机动特性的建模并施加最大机动限制，实现了规划航迹对规划对象的机动特性与约束的适应性，也提升航迹规划安全性并降低了飞行代价。此外，提出的增强安全管道对后端航迹拟合过程中控制点的位置约束，也有效提升了复杂动态环境下航迹规划的安全性。
[0070]
上述具体实施可由本领域技术人员在不背离本发明原理和宗旨的前提下以不同的方式对其进行局部调整，本发明的保护范围以权利要求书为准且不由上述具体实施所限，在其范围内的各个实现方案均受本发明之约束。