一种大型无人机仿地飞行路径规划方法与流程

1.本发明涉及仿地飞行技术领域，特别是一种大型无人机仿地飞行路径规划方法。


背景技术：

2.大型无人机在执行仿地飞行任务时，优秀的规划路线使得飞行时间减短、飞行功耗减低。目前无人机仿地飞行路径规划大致有在避障规划路径基础上匹配dem或借助机载传感器计算相对高度并作出调整，这些方法在平原和丘陵地区适用，在高差较大的山区难以应用。一方面，航线高差变化剧烈时，无人机调整高度的时间长，耗能高；另一方面，规划航线穿过难以逾越的高山时，无人机将陷入困境。即使可以穿越，是采取穿越还是绕行需要定量的评价。


技术实现要素：

3.鉴于此，本发明提供一种大型无人机仿地飞行路径规划方法，充分考虑了飞机性能，提升了飞行的效率和安全性、飞行路径的光滑性。
4.本发明公开了一种大型无人机仿地飞行路径规划方法，包括以下步骤：步骤1：基于所述大型无人机仿地飞行的起点和终点，获取所述大型无人机仿地飞行的路径点集；步骤2：将所述路径点集映射到二维空间平面进行平滑处理，得到大型无人机仿地飞行的最短路径点集；步骤3：基于所述最短路径点集，得到大型无人机仿地飞行的路径规划。
5.进一步地，所述步骤1包括：步骤11：获取大型无人机仿地飞行的起点和终点，将起点添加到open表中，并初始化open表、close表；其中，open表和close表均用于记录点的位置、f值、方位角、父节点；步骤12：从open表中f值最小的点的所有邻域点中找出符合筛选条件的点，形成邻域点集；步骤13：计算邻域点集中所有点的f值；步骤14：分别判断邻域点集中的所有点是否在open表中，若不在，则将其添加至open表中，若在，则分别判断其f值是否小于open表中对应位置的点的f值；若是，则分别将较小的f值作为open表中对应位置的点的f值，且将open表中f值最小的点作为较小的f值对应的邻域点的父节点；步骤15：若open表中f值最小的点为终点，则算法结束，按照子父节点关系，从终点回溯至其先辈节点，即起点，以生成规划路径点集，记为路径点集m；否则，将open表中f值最小的点转移至close表，重复执行步骤12至步骤15，直至open表中f值最小的点为终点或open表为空，输出路径点集m或路径规划失败。
6.进一步地，所述筛选条件包括通行条件、转弯半径约束条件和旁侧安全条件；所述通行条件为不属于障碍点；
所述转弯半径约束条件为：其中，为open表中f值最小的点的方位角与邻域点方位角的夹角，为大型无人机的最小转弯半径，l为open表中f值最小的点到邻域点的空间距离；所述旁侧安全条件为：所述旁侧安全条件为：其中，为邻域点处的坡度，为邻域点处的坡向，为前进方向的方位角，为邻域点处坡向与垂直于前进方位角的平行线的夹角，为坡度阈值，坡度阈值由无人机仿地飞行高度和旁侧安全需求决定。
7.进一步地，所述计算邻域点集中所有点的f值的公式为：进一步地，所述计算邻域点集中所有点的f值的公式为：其中，f为代价，f值越小表示越接近终点，为累加求和，dis为邻域点集中任意一点n到终点的欧式距离，为起点到任意一点n的路径长度，为超出理想爬升、下滑角的额外代价，为open表中f值最小的点到任意一点n的爬升或下滑斜率，为大型无人机的经济速度，均为待定系数，和分别表示路径中第q个点的高程和该点的父节点，即第q-1个点的高程，q的取值范围为2至m，m为正整数，m指组成起点到任意一点n的路径的点数量。
8.进一步地，所述步骤2包括：步骤21：将路径点集m投影到xy平面得到二维点集，记为t；对点集t中所有点连成的路径进行平滑处理，并将其平滑后的坐标代入dem中插值求得每个点对应的z轴坐标，即可形成三维点集；步骤22：基于三维点集形成的路径，构造以该路径在xy平面的投影曲线为准线、z轴的平行线为直母线的柱面，将该柱面展开为平面；将三维点集映射到二维空间平面上得到二维点集，记为；对二维点集中所有点连成的路径进行平滑处理得到点集；在将点集逆变换到xyz坐标系中形成最短路径点集。
9.进一步地，所述步骤21包括：步骤211：去除点集中所有点的z轴坐标，得到点集，、、分别为点集中第个点的x轴坐标、y轴坐标、z轴坐标，n分别为路径点集m和点集中点的总数量；对以下目标函数进行最小化运算，以对点集t进行平滑处理并映射到xyz坐标系中得到三维点集，，、分别为三维点集中第个点的x轴坐标、y轴坐标；
其中，为累加求和，min表示最小值函数，为约束条件，为的余弦值，其计算方式为，a、b、c分别为三维点集中的第i、i-1、i+1个点，和分别为三维点集中的点相对于路径点集m中的点在x轴正方向和y轴正方向的最大偏离量，和分别为三维点集中的点相对于路径点集m中的点在x轴负方向和y轴负方向的最大偏离量；步骤212：取dem中三维点集中第i个点的邻域栅格值，基于所述邻域栅格值，采用二次线性插值法求得高程并将其作为三维点集中第i个点的z轴坐标；重复执行步骤212，直至求得三维点集中所有点的z轴坐标。
10.进一步地，所述步骤22包括：步骤221：将三维点集到映射至二维空间平面，即得到二维点集：：对以下目标函数进行最小化运算，以平滑处理二维点集中所有点连成的路径得到点集，记点集中第i个点的坐标为：：其中，为累加求和，min表示最小值函数，为约束条件，为三维点集投影在xy平面上的路径中的第i个点距起点的路径长度，、、分别为点集中第i-1个、i+1个、i个点在xyz坐标系中对应的z轴坐标，即为待求的参数，为在z轴负方向的最大偏离量，为在z轴正方向平滑的最大偏离量，和分别为下滑角和爬升角，为使点集中第i个点的z坐标有解的松弛因子；步骤222：将点集从二维空间平面逆变换到xyz坐标系中形成最短路径点集，、、分别为最短路径点集中第个点的x轴坐标、y轴坐标、z轴坐标。
11.进一步地，所述步骤3包括：步骤31：采用道格拉斯-普克算法，将最短路径点集简化为基本航点点集s；步骤32：分别从基本航点点集s中选取第j个基本航点以及与其相邻的两个基本航点，分别计算大型无人机在第j个基本航点的转弯半径，，其中，为基本航点点集s中的点数量；步骤33：依次基于第j个基本航点的转弯半径，分别计算第j个基本航点的修正航
点，并最终形成修正航点点集；步骤34：分别计算每个修正航点的转弯半径，并将每个修正航点的转弯半径与预设阈值进行比较，以规划大型无人机的飞行路径。
12.进一步地，所述步骤32包括：步骤321：假设在基本航点点集s中选取三个相邻的基本航点e、d、f，e、d、f分别为第、、个基本航点，dg为的角平分线，计算的补角：设两段折线的最短边边长为，则可求得基本航点d的转弯半径的初步估计值为：其中，为转弯半径的初步估计值，b为航点修正的最大位移量，min表示最小值函数；步骤322：按照步骤321类推，分别得到第2至第个基本航点的转弯半径。
13.进一步地，所述步骤33包括：步骤331：使基本航点d沿方向位移得到修正航点d
′
；步骤332：按照步骤331类推，分别得到第2至第个基本航点对应的修正航点，并最终形成修正航点点集；在所述步骤34中，分别使用步骤321中转弯半径的初步估计值公式计算所有修正航点的转弯半径；若修正航点的转弯半径大于预设阈值，则将大型无人机在该修正航点处的仿地飞行路线规划为直线；否则，将大型无人机在该修正航点处的仿地飞行路线规划为提前转弯。
14.由于采用了上述技术方案，本发明具有如下的优点：(1)本发明针对无人直升机仿地飞行特点，引入邻域方位角、爬升下滑代价、旁侧安全保证等设计改进a*算法，实现了仿地飞行最短路径点获取。该路径充分考虑了飞机性能，提升了飞行的效率和安全性。
15.(2)本发明针对格网路径的不规则的特点，分别在水平面和竖直方向采用有约束的平滑方法，提升了飞行路径的光滑性，使得执行飞行任务时爬升下滑更为合理。
16.(3)本发明考虑地面站和飞机平台的数据传输限制，采用路径简化和航点修正方法，实现了飞行航点提取，降低了路径的数据冗余。
附图说明
17.为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明实施例中记载的一些实施例，对于本领域普通技术人员来讲，还可以根据这些附图获得其他的附图。
18.图1为本发明实施例的一种大型无人机仿地飞行路径规划方法的流程示意图；图2为本发明实施例的open表中点p的邻域图；
图3为本发明实施例的某局部路径示意图；图4为本发明实施例的仿地飞行最短路径航点提取示意图。
具体实施方式
19.结合附图和实施例对本发明作进一步说明，显然，所描述的实施例仅是本发明实施例一部分实施例，而不是全部的实施例。本领域普通技术人员所获得的所有其他实施例，都应当属于本发明实施例保护的范围。
20.本方法针对仿地飞行避障、飞行性能约束下的飞行时间最少的无人机路径规划问题，利用仿地飞行最短路径点获取算法规划飞行路径并采用二次规划和路径简化算法求解任务区域内最少耗时飞行航点。同时本发明还提出了解决上述问题的仿地飞行最短路径点获取算法和简易的航点提取方法。
21.参见图1，本发明提供了一种大型无人机仿地飞行路径规划方法的实施例，该方法包括：s1：基于大型无人机仿地飞行的起点和终点，获取大型无人机仿地飞行的路径点集；s2：将路径点集映射到二维空间平面进行平滑处理，得到大型无人机仿地飞行的最短路径点集；s3：基于最短路径点集，得到大型无人机仿地飞行的路径规划。
22.本实施例中，s1包括：s11：获取大型无人机仿地飞行的起点和终点，将起点添加到open表中，并初始化open表、close表；其中，open表和close表均用于记录点的位置、f值、方位角、父节点；起点的f值为0，方位角为-1，-1表示起点不受转弯半径约束；s12：从open表中f值最小的点的所有邻域点中找出符合筛选条件的点，形成邻域点集；s13：计算邻域点集中所有点的f值；s14：分别判断邻域点集中的所有点是否在open表中，若不在，则将其添加至open表中，若在，则分别判断其f值是否小于open表中对应位置的点的f值；若是，则分别将较小的f值作为open表中对应位置的点的f值，且将open表中f值最小的点作为较小的f值对应的邻域点的父节点；s15：若open表中f值最小的点为终点，则算法结束，按照子父节点关系，从终点回溯至其先辈节点，即起点，以生成规划路径点集，记为路径点集m；否则，将open表中f值最小的点转移至close表，重复执行s12至s15，直至open表中f值最小的点为终点或open表为空，输出路径点集m或路径规划失败。
23.本实施例中，筛选条件包括通行条件、转弯半径约束条件和旁侧安全条件；通行条件为不属于障碍点；转弯半径约束条件为：其中，为open表中f值最小的点的方位角与邻域点方位角的夹角，为大型无
人机的最小转弯半径，l为open表中f值最小的点到邻域点的空间距离；假设open表中f值最小的点为点p，点p的邻域中每个方向的代号和分布见图2。在图2中，设1方向为正北，则邻域节点序号1-16对应的方位角（
°
）为0，26，45，63，90，116，135，153，180，206，225，243，270，296，315，333。当p为起点时，不受转弯半径约束。
24.旁侧安全条件为：旁侧安全条件为：其中，为邻域点处的坡度，为邻域点处的坡向，为前进方向的方位角，为邻域点处坡向与垂直于前进方位角的平行线的夹角，为坡度阈值，坡度阈值由无人机仿地飞行高度和旁侧安全需求决定。
25.本实施例中，计算邻域点集中所有点的f值的公式为：集中所有点的f值的公式为：其中，f为代价，f值越小表示越接近终点，为累加求和，dis为邻域点集中任意一点n到终点的欧式距离，为起点到任意一点n的路径长度，为超出理想爬升、下滑角的额外代价，为open表中f值最小的点到任意一点n的爬升或下滑斜率，为大型无人机的经济速度，均为待定系数，和分别表示路径中第q个点的高程和该点的父节点，即第q-1个点的高程，q的取值范围为2至m，m为正整数，m指组成起点到任意一点n的路径的点数量。其中，此路径指的是：假设open表中f值最小的点为邻域点集中任意一点n的父节点，根据子父节点关系，从任意一点n回溯至起点而形成的路径。
26.本实施例中，s2包括：s21：将路径点集m投影到xy平面得到二维点集，记为t；对点集t中所有点连成的路径进行平滑处理，并将其平滑后的坐标代入dem中插值求得每个点对应的z轴坐标，即可形成三维点集；s22：基于三维点集形成的路径，构造以该路径在xy平面的投影曲线为准线、z轴的平行线为直母线的柱面，将该柱面展开为平面；将三维点集映射到二维空间平面上得到二维点集，记为；对二维点集中所有点连成的路径进行平滑处理得到点集；在将点集逆变换到xyz坐标系中形成最短路径点集。
27.本实施例中，s21包括：s211：去除点集中所有点的z轴坐标，得到点集，、、分别为点集中第个点的x轴坐标、y轴坐标、z轴坐标，n分别为路径点集m和点集中点的总数量；对以下目标函数进行最小化运算，以对点集t进行平滑处理并映射到xyz坐标系中得到三维点集，，、分别为三维点集中第个
点的x轴坐标、y轴坐标；点的x轴坐标、y轴坐标；其中，为累加求和，min表示最小值函数，为约束条件，为的余弦值，其计算方式为，a、b、c分别为三维点集中的第i、i-1、i+1个点，和分别为三维点集中的点相对于路径点集m中的点在x轴正方向和y轴正方向的最大偏离量，和分别为三维点集中的点相对于路径点集m中的点在x轴负方向和y轴负方向的最大偏离量；某局部路径示意如图3所示，对于离散点坐标，其路径弯曲程度可由的余弦值表示。其余弦值越大，表示弯曲程度越大。前后余弦值的差可表征弯曲变化情况。因此，可以通过上述s211中的目标函数对路径点集m中每个点的x轴坐标和y轴坐标进行平滑处理。
28.s212：取dem中三维点集中第i个点的邻域栅格值，基于邻域栅格值，采用二次线性插值法求得高程并将其作为三维点集中第i个点的z轴坐标；重复执行s212，直至求得三维点集中所有点的z轴坐标。
29.本实施例中，s22包括：s221：将三维点集到映射至二维空间平面，即得到二维点集：：由于无人机爬升和下滑的性能约束，实际飞行中爬升、下滑的角度变化应是平滑的，且爬升和下滑率应在一定的区间内。将曲线点的xyz值转换为曲线在xy平面的投影线距起点的距离d和z值，以d、z两变量研究爬升和下滑的约束问题。将图4中的模作为弯曲程度指标，高程差作为高程变化指标，对以下目标函数进行最小化运算，以平滑处理二维点集中所有点连成的路径得到点集，记点集中第i个点的坐标为：：其中，为累加求和，min表示最小值函数，为约束条件，为三维点集投影在xy平面上的路径中的第i个点距起点的路径长度，、、分别为点集中第i-1个、i+1个、i个点在xyz坐标系中对应的z轴坐标，即为待求的参数，为在z轴负方向的最大偏离量，为在z轴正方向平滑的最大偏离量，和分别为下滑角和爬升角，为使点集中第i个点的z坐标有解的松弛因子；s222：将点集从二维空间平面逆变换到xyz坐标系中形成最短路径点集
，、、分别为最短路径点集中第个点的x轴坐标、y轴坐标、z轴坐标。
30.本实施例中，s3包括：s31：采用道格拉斯-普克算法，将最短路径点集简化为基本航点点集s；s32：分别从基本航点点集s中选取第j个基本航点以及与其相邻的两个基本航点，分别计算大型无人机在第j个基本航点的转弯半径，，其中，为基本航点点集s中的点数量；s33：依次基于第j个基本航点的转弯半径，分别计算第j个基本航点的修正航点，并最终形成修正航点点集；s34：分别计算每个修正航点的转弯半径，并将每个修正航点的转弯半径与预设阈值进行比较，以规划大型无人机的飞行路径。
31.本实施例中，s32包括：s321：假设在基本航点点集s中选取三个相邻的基本航点e、d、f，e、d、f分别为第、、个基本航点，dg为的角平分线，计算的补角：设两段折线的最短边边长为，则可求得基本航点d的转弯半径的初步估计值为：其中，为转弯半径的初步估计值，b为航点修正的最大位移量，min表示最小值函数；s322：按照s321类推，分别得到第2至第个基本航点的转弯半径。
32.本实施例中，s33包括：s331：使基本航点d沿方向位移得到修正航点d
′
；s332：按照s331类推，分别得到第2至第个基本航点对应的修正航点，并最终形成修正航点点集；在s34中，分别使用s321中转弯半径的初步估计值公式计算所有修正航点的转弯半径；若修正航点的转弯半径大于预设阈值，则将大型无人机在该修正航点处的仿地飞行路线规划为直线；否则，将大型无人机在该修正航点处的仿地飞行路线规划为提前转弯。
33.最后应当说明的是：以上实施例仅用以说明本发明的技术方案而非对其限制，尽管参照上述实施例对本发明进行了详细的说明，所属领域的普通技术人员应当理解：依然可以对本发明的具体实施方式进行修改或者等同替换，而未脱离本发明精神和范围的任何修改或者等同替换，其均应涵盖在本发明的权利要求保护范围之内。