CN116301014A - 一种无人直升机贴地飞行轨迹优化方法 - Google Patents

Abstract

本发明公开了一种无人直升机贴地飞行轨迹优化方法。属于无人直升机轨迹规划领域，具体步骤：利用集成高度评估信息的启发式搜索算法在栅格地图上规划贴地飞行路径；利用冗余路径点消除算法去除所述规划路径中多余路径点，形成直线航段，以便后续对不同长度航段合理分配任务时间，更符合任务实际需求；采用平滑策略对所述航段添加约束，进行平滑处理，得到考虑性能的应飞轨迹；动态避障路径生成算法结合周围的实际环境，基于已有的路径，通过动态规划规避新障碍威胁。本发明能够解决现有路径规划方法中无人直升机动态规划、实时避障，性能约束考虑较少的问题，提高了实用性、可靠性。

Description

一种无人直升机贴地飞行轨迹优化方法

技术领域

本发明属于无人直升机轨迹规划领域，涉及了一种无人直升机贴地飞行轨迹优化方法。

背景技术

目前，无人直升机突防方式一直是任务安全的主要研究课题之一，由于电子技术的日趋完善，非隐身飞行器难以从中高空域突防，使得无人直升机常规突防方式可行性逐渐降低；因此需要寻求其他方法完成作战任务，如按预定最小离地高度在垂直平面内随地形起伏作机动飞行，可利用地形掩护，也可以较短航程完成任务；如在方位平面内作机动飞行，绕如山峰或其他高大障碍物飞行，充分利用地形掩护做机动飞行，不易被敌方发现，但与地形障碍碰撞的可能性较大，到达目标需要绕较远路程。

这就需要结合路径规划技术将这两种方式有机结合起来，合理为无人直升机规划航路；此外，在无人直升机低空突防任务中，对于无人直升机相对高度以及航段平滑度有一定的要求，地形的不确定性往往为贴地飞行提高了难度，不易生成理想航路，对基于无人直升机贴地飞行的轨迹规划方法的研究，达到提高飞行任务生存率的目的，能为后续的科研以及军事应用提供一定的技术支持。

发明内容

发明目的：本发明的目的在于提供了一种无人直升机贴地飞行轨迹优化方法，其目的在于解决现有路径规划方法中无人直升机动态规划、实时避障，性能约束考虑较少的问题。

技术方案：本发明所述的一种无人直升机贴地飞行轨迹优化方法，其具体制备步骤如下：

步骤(1)、利用集成高度评估信息的启发式搜索算法在栅格地图上规划贴地飞行路径，其中，贴地飞行路径规划算法的拥有异常保护措施以防止所述路径规划算法无解或跑入死循环；

步骤(2)、冗余航点消除算法去除所述规划路径中多余路径点，形成直线航段，以便后续对不同长度航段合理分配任务时间，更符合任务实际需求；

步骤(3)、采用平滑策略对所述优化后路径添加约束，进行平滑处理，得到考虑性能的应飞轨迹；

步骤(4)、动态避障路径生成算法实时规避新障碍威胁。

进一步的，所述集成高度评估信息的启发式搜索算法的表达式为：

f(n)＝g(n)+h₁(n)+ah₂(n)+bh₃(n)

公式中，f(n)表示从起始节点经由节点n到目标节点的估价值；g(n)为耗散函数，表示从起始节点到节点n的实际代价；h₁(n)表示从无人直升机当前位置n到目标点的估计代价值，用欧式距离来表示；h₂(n)表示扩展结点距离地面或障碍物安全高度平面的高度，这样父节点上方的扩展结点代价值更大，父节点下方的扩展结点代价值更小，使得在利用所述启发式搜索算法进行路径规划时，所述启发式搜索算法趋于采取贴近地面扩展结点作为路径点，从而实现无人直升机贴地飞行；h₃(n)表示路径规划中扩展结点相对于基准面的高度，使得在利用所述启发式搜索算法进行路径规划时，所述启发式搜索算法趋于采取和父节点同一高度的扩展结点作为路径点，从而使无人直升机避开山顶路径而采取山谷路线，绕地形或障碍飞行；a和b为系数，可根据无人直升机性能确定启发函数值的权重。

进一步的，所述利用集成高度评估信息的启发式搜索算法在栅格地图上实时规划贴地飞行路径，具体为：

步骤一、构造三个列表存储整个空间内可以通过的节点，分别是openset表、closeset表和可行路径距离dist表(起点到当前节点已搜索过的多条可行路径距离的最短距离g(n)，时刻更新比较获得)；openset表的功能是用来存放节点的代价值以及相对应的节点序号；closeset表主要用来存放已经被挑选出来的当前节点，即openset表中代价值最小的节点；算法开始的时候进行数据初始化，将openset表dist表置为无穷大、closeset表清零，判断起点以及终点是否被障碍物侵占，若被侵占，程序结束，搜索失败；

步骤二、将起点设置为当前节点，起始点的实际代价值设置为0，dist表存入起点到当前节点所走过的距离(即实际代价值)，openset表内放入起始点代价值(实际代价为0也就是估价值)以及起始点的节点序号，并将当前节点放入closeset表中；

步骤三、将当前节点在openset表中的代价值设置为无穷大，搜索当前节点的扩展节点；依次判断扩展节点是否在openset表中，如果不在则把它加入openset表中，并计算出此节点的代价值并存入openset表中，计算扩展节点的实际代价值，存入dist表中，并将当前节点设置为此扩展节点的父节点(一个节点的父节点表示为这个节点的上一个紧邻节点)；如果扩展节点在openset表中，以实际代价值大小为判断依据，来检查这条路径是否更好，比较此时的实际代价值与dist表中所存入的上一次实际代价值的大小，实际代价值小的路径表示为更好的路线，若此次实际代价值更小则更新dist中的数据以及openset表内代价值，父节点更新为此次的当前节点，若此次次实际代价值更大，则不做任何处理，保留原来的值；

步骤四、从openset表内找出代价值最小的节点m，判断节点m是否为终点，若是终点跳到步骤五，若不是则将节点m设置为当前节点，将节点m放入到closeset表内，然后重复步骤三；

步骤五、当前节点为目标点时，closeset表内存放的节点就是找到最优的路径；存储路径，从终点开始，将所有节点沿着父节点反向排序直至起点，即可得到可行路径。

进一步地，所述异常保护算法的具体步骤为：

一、判断起始点是否合理，要保证起始点在非障碍栅格内，若起始点在障碍栅格内，则直接跳出所述路径规划算法；

二、判断终点是否合理，要保证终点在非障碍栅格内，若终点在障碍栅格内，则直接跳出所述路径规划算法；

三、判断已搜索节点数量是否到达栅格数量上限，若达到栅格上限，则跳出所述路径规划算法，防止其进入搜索死循环。

进一步地，所述冗余航点消除算法，具体操作步骤为：

步骤一，把起点n₁作为第一个航点s₁，同时作为参考点，依次对s₁与后续路径点n₂,n₃,...,n₉作连线；

步骤二，当参考点与第n_i点、第n_i+1点的连线不在同一直线上，就将n_i作为下一个航路点s₂；

步骤三，将航点s₂作为新的参照点，依次和后续的路径点进行连线检测，重复上述步骤，直到终点；

步骤四，输出优化后航点和直线航段并保存航点的位置坐标。

进一步地，所述平滑策略为考虑无人直升机性能约束的最小化snap法。

进一步地，所述采用平滑策略对所述优化后路径进行平滑处理，得到平滑处理后的应飞轨迹，具体为：

步骤一、分割航段进行逐段分析，以与时间相关的n阶多项式表示轨迹；

轨迹多项式为：

其中，t表示时间，k表示轨迹的段数，p(i)＝[p_io,p_i1,p_i2,…,p_in]^T,表示第i段轨迹的参数向量；

步骤二，设定初始速度，给各航段适当分配飞行时间，确定速度剖面；

规划总时间计算方法为：

其中p_i表示路径点向量，p_i-1表示紧邻p_i-1的前一路径点,v表示预设的平均速度；

步骤三，构建一个最小化目标优化函数，使得相邻段之间的位置、速度、加速度、加加速度等连续；

函数为：

其中,p表示单个轨迹段参数，Q_i表示代价矩阵。

步骤四，构造与时间相关的等式约束；

相关公式为：

4个起始点约束：

x_s(t＝0)＝p_s；

4个目标点约束：

x_e(t＝T)＝p_e；

8*(m-1)个其余航点约束：

x_i(t_i)＝x_i-1(t_i)＝p_i；

x_i ⁽⁴⁾(t_i)＝x_i-1 ⁽⁴⁾(t_i)

x_i ⁽⁵⁾(t_i)＝x_i-1 ⁽⁵⁾(t_i)

x_i ⁽⁶⁾(t_i)＝x_i-1 ⁽⁶⁾(t_i)；

步骤五，考虑无人直升机性能限制，针对无人直升机过载过大的位置进行分段优化，若各航段加速度均不超出无人直升机性能限制，则输出应飞轨迹，否则降低初始速度，适当延长任务时间，并返回步骤二。

进一步地，所述动态避障路径生成算法，在飞行过程中，根据无人直升机探测到新障碍威胁的位置、大小信息，结合无人直升机当前位置，判断无人直升机到新障碍边缘点的最短距离，根据障碍物边缘点位置，在安全距离的范围内插入避障航路点，形成新航段，即当无人直升机沿航线飞行过程中检测到障碍物时，调用避障规划；没有检测到新障碍时，不调用避障规划；

结合飞行过程中周围的实际环境，基于已有的原规划路径，发明了两种动态路径生成策略，分别是自动快速避障规划和所述路径规划算法重规划；

第一种是自动快速避障规划：在飞行过程中，根据无人直升机探测到新障碍威胁的位置、大小信息，结合无人直升机当前位置，判断无人直升机到新障碍边缘点的最短距离，根据障碍物边缘点位置，在安全距离的范围内插入避障航路点，形成新航段；之后再结合原待飞航段，重新进行冗余航点消除和轨迹优化形成新的飞行轨迹；

第二种是所述路径规划算法重规划：在飞行过程中，根据无人直升机探测到新障碍威胁的位置、大小信息，更新规划空间数字地图数据后，结合无人直升机当前位置，直接重新调用所述路径规划算法规划从当前位置到着陆点的路径，对原待飞路径进行替换后再进行冗余航点消除和轨迹优化，形成新的应飞轨迹；

触发动态避障规划对应情况处理方式如下：

1)、没有障碍物

不进行避障规划；

2)、有障碍物，障碍物不在原路径上

更新地图数据，返回原路径；

3)、有障碍物，障碍物在原路径上，快速避障规划成功

更新地图数据，输出避障路径，进行冗余航点消除，轨迹优化；

4)、有障碍物，障碍物在原路径上，快速避障规划失败

更新地图数据，集成高度信息评估地路径规划算法重规划当前位置到着陆点的路径，进行冗余航点消除，轨迹优化；

具体为：

步骤一、根据障碍物数据，清除原有障碍物数据，将新的障碍物数据添加到原规划空间中，更新规划空间地图数据；

步骤二、若当前位置之后的待飞路径与新障碍发生碰撞，则进入步骤三；若所有航段均不发生碰撞则进入步骤六；

步骤三、自动配置碰撞航点的快速避障规划，沿路径前进方向按航段序列依次找出每个航段的碰撞障碍物次序和数量，按照先后次序在原路径中插入避障航点，与原待飞路径航点形成一个完整有次序的新路径序列；

步骤四、若新路径序列依然与新障碍发生碰撞，则进入步骤五；否则进入步骤六；

步骤五、调用所述路径规划算法重新规划当前位置到着陆点的路径；

步骤六、进行冗余航点消除，生成避障应飞轨迹。

进一步地，所述自动避障快速规划策略分别为爬升越障和水平绕飞避障，考虑到无人直升机性能和能量消耗，采用优先水平绕飞避障，其次爬升越障的整体策略。

有益效果：本发明与现有技术相比，本发明的特点是：本发明能够解决现有路径规划方法中无人直升机动态规划、实时避障，性能约束考虑较少的问题，提高了实用性、可靠性。

附图说明

图1是本发明中集成高度评估信息和异常保护的路径规划算法的流程示意图；

图2是本发明中冗余航点消除算法原理图；

图3是本发明中动态避障路径生成算法流程图；

图4是本发明中快速避障规划示意俯视图；

图5是本发明中障碍边界定义图；

图6是本发明中障碍块周围水平区域划分；

图7是本发明中避障规划仿真三维图；

图8是本发明中避障规划路径平面图；

图9是本发明中无人直升机贴地飞行轨迹优化方法流程图；

图10是本发明实施例中8km×3km×0.4km的栅格地图中集成高度评估信息和异常保护算法的路径规划仿真图。

具体实施方式

为了更清楚地说明本发明的技术方案，下面结合附图对本实用新型的技术方案做进一步的详细说明：

如图所述，本发明所述的一种无人直升机贴地飞行轨迹优化方法，其具体制备步骤如下：

步骤1、利用集成高度评估信息的启发式搜索算法在栅格地图上规划贴地飞行路径，所述路径规划算法的拥有异常保护措施以防止所述路径规划算法无解或跑入死循环；

其中，所述启发式搜索算法的表达式为：

f(n)＝g(n)+h₁(n)+ah₂(n)+bh₃(n)

公式中，f(n)表示从起始节点经由节点n到目标节点的估价值；g(n)为耗散函数，表示从起始节点到节点n的实际代价；h₁(n)表示从无人直升机当前位置n到目标点的估计代价值，估计代价值用欧几里得距离来表示；h₂(n)表示扩展结点距离地面或障碍物安全高度平面的高度，这样父节点上方的扩展结点代价值更大，父节点下方的扩展结点代价值更小，使得在利用所述启发式搜索算法进行路径规划时，所述启发式搜索算法趋于采取贴近地面扩展结点作为路径点，从而实现无人直升机贴地飞行；h₃(n)表示路径规划中扩展结点相对于基准面的高度，使得在利用所述启发式搜索算法进行路径规划时，所述启发式搜索算法趋于采取和父节点同一高度的扩展结点作为路径点，从而使无人直升机避开山顶路径而采取山谷路线，绕地形或障碍飞行；a和b为系数，可根据无人直升机性能确定启发函数值的权重；

所述启发式搜索算法，在三维空间中计算最优路径。节点的代价值采用成本估价函数计算，依据成本估价函数进行搜索，搜索到满足成本估价函数的节点作为下一次要搜索的节点，并重复此过程，直到找到目标点，形成最优路径；

所述启发式搜索算法在运算过程中，每次从优先队列中选取f(n)值最小(优先级最高)的节点作为下一个待遍历的节点；

本发明使用无人直升机，移动方位不受限制，并且欧几里得距离可以直接反映两点之间的距离，更加接近于实际距离；欧几里得距离表示为当前节点与目标节点之间的距离：

当无人直升机的移动方位不受限制时，使用欧几里得距离。

具体地说，利用改进后的全局路径规划A*算法在栅格地图上规划全局路径，具体流程图如图1所示，包括以下步骤：

步骤1.1、构造三个列表存储整个空间内可以通过的节点，分别是openset表、closeset表和可行路径距离dist表(起点到当前节点已搜索过的多条可行路径距离的最短距离g(n)，时刻更新比较获得)；openset表的功能是用来存放节点的代价值以及相对应的节点序号；closeset表主要用来存放已经被挑选出来的当前节点，即openset表中代价值最小的节点；算法开始的时候进行数据初始化，将openset表dist表置为无穷大、closeset表清零，判断起点以及终点是否被障碍物侵占，若被侵占，程序结束，搜索失败；

步骤1.2、首先从起点开始，起始点的实际代价值设置为0，dist表存入起点到起点的距离0(即实际代价值)，openset表内放入起始点代价值(实际代价为0也就是估价值)以及起始点的节点序号，然后将起点节点序号设置为当前节点，并将当前节点放入closeset表中；

步骤1.3、将当前节点openset表中代价值重置为无穷大，搜索当前节点的扩展节点；依次判断扩展节点是否在openset表中，如果不在则把它加入openset表中，并计算出此节点的代价值并存入openset表中，计算扩展节点的实际代价值，存入dist表中，并将当前节点设置为此扩展节点的父节点(一个节点的父节点表示为这个节点的上一个紧邻节点)；如果扩展节点在openset表中，以实际代价值大小为判断依据，检查这条路径是否更好，比较实际代价值与dist表中对应节点距离值的大小，实际代价值小的节点表示为更好的路线，若此次实际代价值更小则更新dist中的数据以及openset表内代价值，父节点更新为此次的当前节点，若此次次实际代价值更大，则不做任何处理，保留原来的值；

步骤1.4、从openset表内找出代价值最小的节点m，判断节点m是否为终点，若是终点跳到步骤1.5，若不是则将节点m设置为当前节点，将节点m放入到closeset表内，然后重复步骤1.3；

步骤1.5、当前节点为目标点时，closeset表内存放的节点就是找到最优的路径；存储路径，从终点开始，将所有节点沿着父节点反向排序直至起点，即可得到可行路径。

为了防止起始点和终点选取不合理导致所述路径规划算法无解或跑入死循环，本文对所述路径规划算法设置了3层保护，具体为：

一、判断起始点是否合理，要保证起始点在非障碍栅格内，若起始点在障碍栅格内，则直接跳出所述路径规划算法；

二、判断终点是否合理，要保证终点在非障碍栅格内，若终点在障碍栅格内，则直接跳出所述路径规划算法；

三、判断已搜索节点数量是否到达栅格数量上限，若达到栅格上限，则跳出所述路径规划算法，防止其进入搜索死循环；

其中，栅格数量计算方式为：

h₁(n)＝map.nx*map.ny*map.nz

其中，map.nx为设定栅格长度，map.ny为设定栅格宽度，map.nz为设定栅格高度。

增加保护措施后所述路径规划算法流程图如图1所示。

步骤2、采用冗余航点消除算法去除多余路径点，形成直线航段，以便后续对不同长度航段合理分配任务时间，更符合任务实际需求，具体包括以下步骤：

步骤2.1、把起点n₁作为第一个航点s₁，同时作为参考点，依次对s₁与后续路径点n₂,n₃,...,n₉作连线；

步骤2.2、当参考点与第n_i点、第n_i+1点的连线不在同一直线上，就将n_i作为下一个航路点s₂；

步骤2.3、将航点s₂作为新的参照点，依次和后续的路径点进行连线检测，重复上述步骤，直到终点；

步骤2.4、输出优化后航点，并保存航点的位置坐标；

生成的最优路径航点如表1所示：

表1最优路径航点序列

编号	x坐标(单位：米)	y坐标(单位：米)	z坐标(单位：米)
				1	7207.72758986668	1922.38010945170	400
2	2800	2180	214.586288416076
				3	800	1480	51.4141414141414
4	605.322438893337	1101.53611749894	20

步骤3、采用最小化snap法基于无人直升机性能对轨迹进行平滑处理，具体为：

步骤3.1、分割航段进行逐段分析，以与时间相关的n阶多项式表示轨迹；

轨迹多项式为：

其中，t表示时间，k表示轨迹的段数，p(i)＝[p_io,p_i1,p_i2,…,p_in]^T，表示第i段轨迹的参数向量；

通过建立起的数学方程式计算出任意节点的位置，速度，加速度等轨迹参数。以七阶多项式表示轨迹，将平滑问题通过数学建模转化为约束优化问题。由于轨迹通常比较复杂，所以将轨迹根据时间分成多段分别表示；

步骤3.2、设定初始速度，给各航段适当分配飞行时间，确定速度剖面；

规划总时间计算方法为：

其中，p_i表示路径点向量，p_i-1表示紧邻p_i-1的前一路径点,v表示预设的平均速度；

本实施例将平均速度常数设为20m/s，然后采用平方根分配法，根据各个航段的长度将总时间按比例分配给各航段；

步骤3.3、构建一个最小化目标优化函数；

函数为：

其中，p表示单个轨迹段参数，Q_i表示代价矩阵。

在执行任务时，只需要一条特定轨迹；所以需要使用最小化snap的轨迹优化方法在给定约束条件下，构建一个最小化目标优化函数，在众多优化轨迹中进一步寻优，选择最适合的轨迹；

步骤3.4、构造与时间相关的等式约束，使得相邻段之间的位置、速度、加速度，加加速度等连续；

相关公式为：

4个起始点约束：

x_s(t＝0)＝p_s；

4个目标点约束：

x_e(t＝T)＝p_e；

8*(m-1)个其余航点约束：

x_i(t_i)＝x_i-1(t_i)＝p_i；

x_i ⁽⁴⁾(t_i)＝x_i-1 ⁽⁴⁾(t_i)

x_i ⁽⁵⁾(t_i)＝x_i-1 ⁽⁵⁾(t_i)

x_i ⁽⁶⁾(t_i)＝x_i-1 ⁽⁶⁾(t_i)；

步骤3.5、考虑无人直升机性能限制，针对无人直升机过载过大的位置进行分段优化，若各航段加速度均不超出无人直升机性能限制，则输出应飞轨迹，否则降低初始速度，适当延长任务时间，并返回步骤3.2；

步骤4、利用动态避障路径生成算法，探测飞行过程中遇到新障碍威胁的位置、大小信息，结合无人直升机当前位置，判断无人直升机到新障碍边缘点的最短距离，根据障碍物边缘点位置，在安全距离的范围内插入避障航路点，形成新航段，具体为：

步骤4.1、根据障碍物数据，清除原有障碍物数据，将新的障碍物数据添加到原规划空间中，更新规划空间地图数据；

步骤4.2、若当前位置之后的待飞路径与新障碍发生碰撞，则进入步骤4.3；若所有航段均不发生碰撞则进入步骤4.6；

步骤4.3、自动配置碰撞航点的快速避障规划，沿路径前进方向按航段序列依次找出每个航段的碰撞障碍物次序和数量，按照先后次序在原路径中插入避障航点，与原待飞路径航点形成一个完整有次序的新路径序列；

步骤4.4、若新路径序列依然与新障碍发生碰撞，则进入步骤4.5；否则进入步骤4.6；

步骤4.5、调用所述路径规划算法重新规划当前位置到着陆点的路径；

步骤4.6、进行冗余航点消除，生成避障应飞轨迹。

所述自动避障快速规划策略分别为爬升越障和水平绕飞避障，考虑到无人直升机性能和能量消耗，采用优先水平绕飞避障，其次爬升越障的整体策略；

快速避障规划水平绕飞示意俯视图如图4所示，虚线为原路径，实线为避障绕飞路径，红点为插入的避障航路点。被检测航段每存在一个障碍就添加3个避障绕飞航路点，避障航路点的位置距障碍物边界100m(可更改)。

俯视图中障碍块左下角和右上角两个顶点的坐标是已知的，通过判断航段的起点到障碍块“前、后、左、右、上”5个边界哪一个距离最短，来决定无人直升机从哪个方向绕飞，如图5中障碍边界定义图所示；到上边界的距离乘以10倍(可更改)放大，即高度差乘以10倍放大，以此来决定水平绕飞避障的优先度；此外，对于垂直跨度大的障碍物，当障碍物的两个端点距离大于1000m(可更改)时，直接选择爬升越障。

避障绕飞选择策略如下，水平避障时把障碍块周围分成8个区域，如图6水平区域划分图所示。

下述策略描述中“航段起点到障碍块上边界距离”为已经乘以10倍放大的高度差：

1)、障碍块对角水平距离大于1000m：

无人直升机爬升越障，所有碰撞点都拉高到障碍块最高点上方20m处(可更改)，作为新添加的避障路径节点，形成新的着陆路径；

2)、障碍块对角水平距离小于1000m，航段起点到障碍块上边界距离最短

无人直升机爬升越障，所有碰撞点都拉高到障碍块最高点上方20m处，作为新添加的避障路径节点，形成新的着陆路径；

3)、障碍块对角水平距离小于1000m，航段起点到障碍块上边界距离不是最短，航段起点位于区域1；

无人直升机从障碍x1边左侧绕飞，在障碍块x1边x轴负方向200m处，沿y轴正方向，新增3个避障路径节点；

4)、障碍块对角水平距离小于1000m，航段起点到障碍块上边界距离不是最短，航段起点位于区域2；

无人直升机从障碍x1边左侧绕飞，在障碍块x1边x轴负方向200m处，沿y轴负方向，新增3个避障路径节点；

5)、障碍块对角水平距离小于1000m，航段起点到障碍块上边界距离不是最短，航段起点位于区域3；

无人直升机从障碍x2边右侧绕飞，在障碍块x2边x轴正方向200m处，沿y轴正方向，新增3个避障路径节点；

6)、障碍块对角水平距离小于1000m，航段起点到障碍块上边界距离不是最短，航段起点位于区域4；

无人直升机从障碍x2边右侧绕飞，在障碍块x2边x轴正方向200m处，沿y轴负方向，新增3个避障路径节点；

7)、障碍块对角水平距离小于1000m，航段起点到障碍块上边界距离不是最短，航段起点位于区域5；

无人直升机从障碍y1边下侧绕飞，在障碍块y1边y轴负方向200m处，沿x轴正方向，新增3个避障路径节点；

8)、障碍块对角水平距离小于1000m，航段起点到障碍块上边界距离不是最短，航段起点位于区域6；

无人直升机从障碍y1边下侧绕飞，在障碍块y1边y轴负方向200m处，沿x轴负方向，新增3个避障路径节点；

9)、障碍块对角水平距离小于1000m，航段起点到障碍块上边界距离不是最短，航段起点位于区域7；

无人直升机从障碍y2边上侧绕飞，在障碍块y2边y轴正方向200m处，沿x轴正方向，新增3个避障路径节点；

10)、障碍块对角水平距离小于1000m，航段起点到障碍块上边界距离不是最短，航段起点位于区域8；

无人直升机从障碍y2边上侧绕飞，在障碍块y2边y轴正方向200m处，沿x轴负方向，新增3个避障路径节点。

生成的避障路径点序列如表2所示：

表2避障路径点序列

编号	x坐标(单位：米)	y坐标(单位：米)	z坐标(单位：米)	备注
					1	7207.72758986668	1922.38010945170	400
2	3380	2180	279.030732860520	避障点
					3	3090	2180	246.808510638298	避障点
4	2800	2180	214.586288416076	避障点
					5	2600	1900	220
6	1380	1480	115.858585858586	避障点
					7	1090	1480	83.6363636363636	避障点
8	800	1480	51.4141414141414	避障点
					9	605.322438893337	1101.53611749894	20

以上仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，应视为本发明的保护范围。

Claims (7)

1.一种无人直升机贴地飞行轨迹优化方法，其特征在于，其具体制备步骤如下：

步骤(1)、利用集成高度评估信息的启发式搜索算法在栅格地图上规划贴地飞行路径，并通过拥有异常保护措施的异常保护算法来防止路径规划算法无解或跑入死循环；

步骤(2)、通过冗余路径点消除算法去除规划贴地飞行路径中多余的路径点，形成直线航段，用于后续对不同长度航段合理分配任务时间；

步骤(3)、采用平滑策略对形成的直线航段添加约束，进行平滑处理，从而得到考虑性能的应飞轨迹；

步骤(4)、通过动态避障路径生成算法结合周围的实际环境，基于已有的路径，通过动态规划规避新障碍威胁。

2.根据权利要求1所述的一种无人直升机贴地飞行轨迹优化方法，其特征在于，在步骤(1)中，所述启发式搜索算法的表达式如下式所示：

f(n)＝g(n)+h₁(n)+ah₂(n)+bh₃(n)

式中，f(n)表示从起始节点经由节点n到目标节点的估价值；g(n)为耗散函数，表示从起始节点到节点n的实际代价；h₁(n)表示节点n到目标点的估计代价值；h₂(n)表示节点n距离地面的高度；h₃(n)表示节点n相对父节点的高度；a和b为系数，其根据无人直升机性能确定启发函数值的权重。

3.根据权利要求1所述的一种无人直升机贴地飞行轨迹优化方法，其特征在于，在步骤(1)中，所述的异常保护算法对规划贴地飞行路径的算法共设置三层保护，其具体过程如下所述：

一、判断起始点是否合理，保证起始点在非障碍栅格内，若起始点在障碍栅格内，则跳出所述的路径规划算法；

二、判断终点是否合理，保证终点在非障碍栅格内，若终点在障碍栅格内，则跳出所述的路径规划算法；

三、判断已搜索节点数量是否到达栅格数量上限，若达到栅格上限，则跳出所述的路径规划算法，防止其进入搜索死循环。

4.根据权利要求1所述的一种无人直升机贴地飞行轨迹优化方法，其特征在于，

在步骤(2)中，所述冗余航点消除算法是指通过参考点与后续路径点连线并检测连线是否在同一直线上进而确定下一航点，从而去除中间多余航点，形成直线航段，以便后续合理分配任务时间，其具体的操作步骤为：

步骤一，把起点n₁作为第一个航点s₁，同时作为参考点，依次对s₁与后续路径点n₂,n₃,...,n₉作连线；

步骤二，当参考点与第n_i点、第n_i+1点的连线不在同一直线上，就将n_i作为下一个航路点s₂；

步骤三，将航点s₂作为新的参照点，依次和后续的路径点进行连线检测，重复步骤一至步骤二，直到终点；

步骤四，输出优化后航点和直线航段，并保存航点的位置坐标。

5.根据权利要求1所述的一种无人直升机贴地飞行轨迹优化方法，其特征在于，

在步骤(3)中，所述的平滑策略是对无人直升机的性能约束，针对无人直升机的加速度做出优化，在无人直升机性能限制内降低速度以满足过载限制，其具体的操作步骤如下：

步骤一、分割航段进行逐段分析，以与时间相关的n阶多项式表示轨迹；

步骤二、设定初始速度，给各航段适当分配飞行时间，确定速度剖面；

步骤三、构建一个最小化目标优化函数；

步骤四、构造与时间相关的等式约束，使相邻段之间的位置、速度及加速度连续；

步骤五、考虑无人直升机性能限制，针对无人直升机过载过大的位置进行分段优化，若各航段加速度均不超出无人直升机性能限制，则输出应飞轨迹，否则降低初始速度，延长任务时间，并返回步骤二。

6.根据权利要求1所述的一种无人直升机贴地飞行轨迹优化方法，其特征在于，

在步骤(4)中，所述动态避障路径生成算法是指在飞行过程中，根据无人直升机探测到新障碍威胁的位置及大小信息，结合无人直升机当前位置，判断无人直升机到新障碍边缘点的最短距离，根据障碍物边缘点位置，在安全距离的范围内插入避障航路点，形成新航段；其具体的操作步骤如下：

步骤一、根据障碍物数据，清除原有障碍物数据，将新的障碍物数据添加到原规划空间中，更新规划空间地图数据；

步骤二、若当前位置之后的待飞路径与新障碍发生碰撞，则进入步骤三；

步骤三、自动配置碰撞航点的快速避障规划，沿路径前进方向按航段序列依次找出每个航段的碰撞障碍物次序和数量，按照先后次序在原路径中插入避障航点，与原待飞路径航点形成一个完整有次序的新路径序列；

步骤四、若新路径序列依然与新障碍发生碰撞，则进入步骤五；否则输出新路径序列；

步骤五、缩小搜索空间，调用权利要求1中的启发式搜索算法重新规划当前位置到终点的路径；

步骤六、进行冗余航点消除，生成避障应飞轨迹。

7.根据权利要求6所述的一种无人直升机贴地飞行轨迹优化方法，其特征在于，

在步骤三中，所述插入避障航点的具体原理如下所述：

所述插入避障航点分为两种情况，即爬升越障和水平避障，不允许从障碍物下方绕飞；考虑到无人直升机机动能量消耗，优先水平绕飞避障，其次爬升越障；被检测航段每存在一个障碍就添加若干避障绕飞航点，避障航点距障碍物边界距离可更改，高度通过插值法对航段起点和航段终点的高度插值来获取；通过判断航段起点到障碍块距离最短，决定飞机从哪个方向绕飞；

此外，对于垂直大跨度障碍物，当跨度大于可更改阈值时，则选择爬升越障。

Images