CN110108284A - 一种顾及复杂环境约束的无人机三维航迹快速规划方法 - Google Patents

Abstract

本发明公开了一种顾及复杂环境约束的无人机三维航迹快速规划方法，首先通过剖析地形、气象、禁飞区、危险物等典型复杂环境对无人机航迹的多种约束信息等，构建三维航迹规划环境模型，再结合无人机自身性能约束与飞行特点，设计多层扩展A*算法进行三维扩展，搜索获取无人机参考航迹，最后采用线简化和线平滑的方式对参考航迹进行简化与优化，得到无人机可飞最优航迹。本发明能够高效、准确地生成无人机航迹，能够实现安全绕开威胁，并加快了航迹规划的速度与准确度。

Description

一种顾及复杂环境约束的无人机三维航迹快速规划方法

技术领域

本发明属于无人机航迹规划技术领域，具体涉及一种顾及复杂环境约束的无人机三维航迹快速规划方法的设计。

背景技术

由于无人机(Unmanned Aerial Vehicle，UAV)具有应用方便、机动灵活、时效性强等特点，近些年得到了迅猛发展，广泛应用于航拍、农业、植保、灾难救援、监视、测绘、新闻报道、电力巡检、影视拍摄等领域。无人机航迹规划是指在考虑地形、气象、威胁等环境因素以及自身的飞行性能的基础上，为无人机制定出从初始位置到目标位置的最优飞行路径。无人机航迹规划是无人机任务规划系统中的关键，对于提高无人机作业效率和应用能力具有重要意义。

目前存在多种无人机航迹规划算法，其中二维或2.5维航迹规划方法较多，方法也较为成熟。由于三维空间更加贴近无人机的实际应用环境，较多专家学者逐渐将无人机航迹规划转向三维，提出了许多改进算法，但这些算法仍具有一定的局限性。例如神经网络算法结合可调步长策略能够快速生成三维航迹，但无法应对多种约束模型共存的环境要求；遗传算法与粒子群算法寻优精度低、容易陷入局部最优，并且在复杂约束的路径规划问题中航迹精度也不理想；蚁群算法改进启发函数和信息素更新能使三维路径长度与搜索效率得到提高，但并未考虑无人机的性能约束与航线特点，规划航迹适应性不足；引入混沌理论搜索的人工势场方法能够应对复杂环境，但航迹曲折多变，缺少最优性，且仅能适用于二维环境，无法应用于三维环境中。A*算法是一种启发式算法，在图形搜索领域应用成熟，能够很好地改进以利用于地理场景中逐格网搜索，但该算法一般仅用于二维环境下搜索，若直接应用进行三维搜索，则运算将随规划空间增大成指数倍上升，效率缓慢，需要进行改进。

综上可知，目前无人机三维航迹规划方法主要是现有各种算法的改进，能够在单一因素的约束下快速生成航迹，但在复杂环境约束下缺少对最优路径的考虑，且无法同时兼顾复杂环境约束与无人机自身性能约束的最优航迹规划。

发明内容

本发明的目的是为了解决目前无人机三维航迹规划方法在复杂环境约束下缺少对最优路径的考虑，且无法同时兼顾复杂环境约束与无人机自身性能约束的最优航迹规划的问题，提出了一种顾及复杂环境约束的无人机三维航迹快速规划方法。

本发明的技术方案为：一种顾及复杂环境约束的无人机三维航迹快速规划方法，包括以下步骤：

S1、构建无人机三维航迹规划的环境模型。

S2、将复杂环境约束信息融入环境模型。

S3、根据融入复杂环境约束信息的环境模型，采用多层扩展A*算法获取无人机的参考航迹。

S4、对参考航迹进行航迹简化与航迹平滑，得到无人机可飞最优航迹。

进一步地，步骤S1具体为：

将无人机在改变方向时的最小惯性距离作为格网单元的长度，将路径规划区域划分为一个m×n的格网，在每个格网单元中存储其对应的行列号、该格网单元中无人机的最大飞行高度值以及可飞行的高度范围信息，得到无人机三维航迹规划的环境模型，表示为：

其中Ω_m表示环境模型空间，(X_i,Y_j)表示格网单元的行列号，m,n分别为环境模型中格网单元的行数和列数，表示格网单元(X_i,Y_j)的无人机可飞行高度范围，Z₁,Z₂,Z₃,Z₄均为高度值，且Z₁<Z₂<Z₃<Z₄...。

进一步地，步骤S2中的复杂环境约束信息包括地形环境约束和大气环境约束。

进一步地，地形环境约束的融入方法具体为：

A1、通过仿射变换将真实三维地形数据每个栅格像元的行列号转换为环境模型中格网单元的行列号，转换公式为：

其中(X_m,Y_m)表示环境模型中格网单元的行列号，(X_p,Y_p)表示真实三维地形数据栅格像元的行列号，(X₀,Y₀)表示真实三维地形数据在路径规划区域左上角的经度和纬度，W_c表示真实三维地形数据的像元宽度，H_c表示真实三维地形数据的像元高度，R₁,R₂为旋转参数。

A2、把真实三维地形数据每个栅格像元的地形高程信息添加进对应的格网单元，完成地形环境约束的融入。

进一步地，大气环境约束的融入方法具体为：

B1、将大气环境约束区域竖直投影成一个多边形，并获取多边形每个顶点的经纬度坐标。

B2、根据经纬度坐标计算每两个相邻顶点的欧式距离，计算公式为：

其中(X₁,Y₁)和(X₂,Y₂)分别表示多边形两个相邻顶点的经纬度坐标，D₁₂表示这两个相邻顶点的欧氏距离。

B3、根据两个相邻顶点的欧式距离计算这两个相邻顶点连接成的直线的分段次数，计算公式为：

S₁₂＝ceil(D₁₂/W_c)

其中S₁₂表示顶点(X₁,Y₁)和(X₂,Y₂)连接成的直线的分段次数，ceil(·)为向上取整函数，W_c表示环境模型中每个格网对应真实三维地形数据的像元宽度。

B4、初始化计数值j＝0。

B5、判断j>S₁₂是否成立，若是则进入步骤B8，否则进入步骤B6。

B6、根据分段次数计算分段后各个点的坐标，计算公式为：

其中(X_12j,Y_12j)为分段后第j个点的坐标，j＝0,1,2,...,S₁₂。

B7、令计数值j加1，并返回步骤B5。

B8、获取分段后各个点的坐标在环境模型中对应格网单元的行列号，得到多边形对应的格网单元。

B9、判断相邻两个格网单元是否在同一行或同一列上，若是则进入步骤B11，否则进入步骤B10。

B10、在未处于同一行或同一列的两相邻格网单元之间补充一个格网单元，进入步骤B11。

B11、将多边形对应的格网单元和补充的格网单元作为大气环境约束区域，完成大气环境约束的融入。

进一步地，步骤S3包括以下分步骤：

S3-1、获取融入复杂环境约束信息的环境模型以及起始点与目标点的位置信息与高程信息。

S3-2、创建openlist线性表与closelist线性表，并初始化将其置为空。

S3-3、计算起始点的代价值，并将起始点添加至openlist线性表中。

S3-4、判断openlist线性表是否为空，若是则进入步骤S3-5，否则进入步骤S3-6。

S3-5、航迹搜索失败，进入步骤S3-21。

S3-6、获取openlist线性表中代价值最小的点作为当前点。

S3-7、判断当前点是否与目标点位于同一格网单元与高程，若是则进入步骤S3-8，否则进入步骤S3-9。

S3-8、航迹搜索成功，获取最优航迹节点，进入步骤S3-21。

S3-9、将当前点移除出openlist线性表，并添加进closelist线性表中。

S3-10、对当前点进行分层扩展，得到多个扩展点。

S3-11、判断是否遍历完所有扩展点，若是则返回步骤S3-4，否则选取下一个扩展点并进入步骤S3-12。

S3-12、判断closelist线性表中是否存在该扩展点，若是则进入步骤S3-13，否则进入步骤S3-14。

S3-13、舍去该扩展点，返回步骤S3-11。

S3-14、计算该扩展点的代价值。

S3-15、判断openlist线性表中是否存在该扩展点，若是则进入步骤S3-16，否则进入步骤S3-19。

S3-16、判断该扩展点的代价值是否小于openlist线性表中所存在与该扩展点位置相同的点的代价值，若是则进入步骤S3-17，否则进入步骤S3-18。

S3-17、在openlist线性表中用该扩展点替换扩展前的当前点，进入步骤S3-20。

S3-18、舍去该扩展点，返回步骤S3-11。

S3-19、将该扩展点添加进openlist线性表，进入步骤S3-20。

S3-20、将扩展前的当前点设置为该扩展点的父节点，返回步骤S3-11。

S3-21、按顺序连接最优航迹节点，得到无人机的参考航迹。

进一步地，代价值的计算公式为：

f(n)＝g(n)+ε·h(n)

其中f(n)表示当前节点n位置的代价值，g(n)表示从起始点到当前节点n位置的实际代价值，h(n)表示从当前节点n位置到目标点的估计代价值，ε为启发项权重系数且ε>1。

实际代价值g(n)的计算公式为：

g(n)＝ω₁L_g+ω₂Z_g+ω₃T₁+ω₄T₂

估计代价值h(n)的计算公式为：

h(n)＝ω₁L_h+ω₂Z_h+ω₃T₁+ω₄T₂

其中ω₁,ω₂,ω₃,ω₄均为权重值，L_g表示从起始点到当前节点n位置的航迹段长度，L_h表示从当前节点n位置到目标点的航迹段长度，Z_g表示从起始点到当前节点n位置的航迹平均高度，Z_h表示从当前节点n位置到目标点的航迹平均高度，T₁,T₂均为影响因素代价。

进一步地，步骤S3-10中对当前点进行分层扩展的具体方法为：

根据水平扩展距离与无人机最大俯仰角的大小计算本次扩展的竖直高度可达范围，计算公式为：

其中H_next表示竖直高度可达范围，H_cur为当前飞行高度，L_C为环境模型的格网单元宽度，α为无人机最大俯仰角，direct为方向值，其取值为从正北方向开始，顺时针分成八个方向。

在竖直高度可达范围内以固定高度间隔为基准进行分层扩展，得到多个扩展点。

进一步地，步骤S4中航迹简化的具体方法为：

C1、设从起始点到目标点，总共有n个具有顺序关系的三维坐标点。

C2、依次计算相邻两点的方向向量，得到n-1个方向向量d₁,d₂,...,d_n-1。

C3、初始化计数值i＝1。

C4、判断i>n-2是否成立，若是则进入步骤C5，否则进入步骤C6。

C5、按顺序连接所有不简化点，结束航迹简化。

C6、设置变量p＝i，q＝i+1，并初始化容忍计数值num＝0。

C7、计算方向向量d_p和d_q的夹角余弦值，计算公式为。

其中为方向向量d_p和d_q的夹角。

C8、判断该余弦值是否大于容忍值v，若是则进入步骤C9，否则进入步骤C10。

C9、令容忍计数值num加1，进入步骤C11。

C10、重置容忍计数值num＝0，进入步骤C13。

C11、判断容忍计数值num是否大于容忍数阈值t，若是则进入步骤C12，否则进入步骤C13。

C12、将第q+1-num个点设置为不简化点，并令i＝q-num，返回步骤C4。

C13、令变量q加1，返回步骤C7。

进一步地，步骤S4中航迹平滑的具体方法为：

采用三次Cardinal样条曲线对航迹简化后的参考航迹进行平滑，得到无人机可飞最优航迹。

本发明的有益效果是：本发明通过设计多层扩展A*算法，提出了一种顾及复杂环境约束的无人机三维航迹快速规划方法，通过剖析地形、气象、禁飞区、危险物等典型复杂环境对无人机航迹的约束方式，建立三维航迹规划环境模型，以应对典型复杂环境；并且本发明结合了无人机性能约束特点，设计多层扩展A*算法以代价函数为引导获取参考航迹，通过线简化与平滑的航迹优化方式，能够高效、准确地生成无人机航迹。

附图说明

图1所示为本发明实施例提供的一种顾及复杂环境约束的无人机三维航迹快速规划方法流程图。

图2所示为本发明实施例提供的环境模型示意图。

图3所示为本发明实施例提供的地形环境约束的融入方法流程图。

图4所示为本发明实施例提供的大气环境约束的融入方法流程图。

图5所示为本发明实施例提供的大气环境边界格网获取及补充示意图。

图6所示为本发明实施例提供的步骤S3的分步骤流程图。

图7所示为本发明实施例提供的方向值示意图。

图8所示为本发明实施例提供的航迹简化方法流程图。

图9所示为本发明实施例提供的实验区域环境示意图。

图10所示为本发明实施例提供的多层扩展A*算法各步骤结果对比图。

图11所示为本发明实施例提供的三种算法整体航迹结果比较图。

图12所示为本发明实施例提供的三种算法前部分局部航迹结果比较图。

具体实施方式

现在将参考附图来详细描述本发明的示例性实施方式。应当理解，附图中示出和描述的实施方式仅仅是示例性的，意在阐释本发明的原理和精神，而并非限制本发明的范围。

本发明实施例提供了一种顾及复杂环境约束的无人机三维航迹快速规划方法，如图1所示，包括以下步骤S1～S4：

S1、构建无人机三维航迹规划的环境模型。

为了拟合算法以格网空间进行搜索的方式，环境模型也需依此基于规则格网进行构建。主要方式如下：由于无人机在改变方向时存在最小惯性距离的约束，可将该最小惯性距离作为格网单元的长度，将路径规划区域分为一个m×n的格网，在每个格网单元中存储其对应的行列号、该格网单元中无人机的最大飞行高度值以及可飞行的高度范围信息，得到无人机三维航迹规划的环境模型。

某格网下可飞行高度范围为：

其中表示格网单元(X_i,Y_j)的无人机可飞行高度范围，Z₁,Z₂,Z₃,Z₄均为高度值，且Z₁<Z₂<Z₃<Z₄...，其表示可根据具体实际情况进行补充高度范围，通过这种方式能够将典型复杂环境的约束都整合进该环境模型，应对复杂环境的需求。

综上所述，所构建的环境模型表示为：

其中Ω_m表示环境模型空间，(X_i,Y_j)表示格网单元的行列号，m,n分别为环境模型中格网单元的行数和列数。对应的环境模型示意图如图2所示，空白处代表无障碍，即可飞行区域。

S2、将复杂环境约束信息融入环境模型。

本发明实施例中，复杂环境约束信息包括地形环境约束和大气环境约束。

其中，地形是无人机飞行环境的重要组成部分，也是无人机航迹规划的前提，无人机航迹必须大于地形高度。真实的三维地形数据，是以栅格的格网像元的形式存在，且每个像元都有对应的经度、纬度与高程值。添加地形要素信息，需要获取各栅格像元所对应的环境模型格网单元行列号，进而添加其对应高程信息。

如图3所示，地形环境约束的融入方法具体为：

A1、通过仿射变换将真实三维地形数据每个栅格像元的行列号转换为环境模型中格网单元的行列号，转换公式为：

其中(X_m,Y_m)表示环境模型中格网单元的行列号，(X_p,Y_p)表示真实三维地形数据栅格像元的行列号，(X₀,Y₀)表示真实三维地形数据在路径规划区域左上角的经度和纬度，W_c表示真实三维地形数据的像元宽度，H_c表示真实三维地形数据的像元高度，R₁,R₂为旋转参数(当地形数据没有进行过旋转处理，也就是数据指北针竖直向上时，旋转参数R₁,R₂均为0)。

A2、把真实三维地形数据每个栅格像元的地形高程信息添加进对应的格网单元，完成地形环境约束的融入。

此外，由于无人机在飞行时，一般要保持与地面高度具有一定的距离，以防止与山头或斜坡等突出石头或其它障碍物的碰撞。所以无人机飞行高度需要大于地形高度与最低离地高度之和。

无人机飞行经常要受到气象条件与禁飞区的影响，对于航迹规划而言，恶劣天气影响与禁飞区对航迹规划的影响是一样的，故统一考虑为大气环境约束。为保证飞行安全，要求无人机在任意时刻都位于大气环境约束区域外部。大气环境约束类似为一个下底面通向地面的柱形区域，该柱形区域内无法通行。

如图4所示，大气环境约束的融入方法具体为：

B1、将大气环境约束区域竖直投影成一个多边形，并获取多边形每个顶点的经纬度坐标。

B2、根据经纬度坐标计算每两个相邻顶点的欧式距离，计算公式为：

其中(X₁,Y₁)和(X₂,Y₂)分别表示多边形两个相邻顶点的经纬度坐标，D₁₂表示这两个相邻顶点的欧氏距离。

B3、根据两个相邻顶点的欧式距离计算这两个相邻顶点连接成的直线的分段次数，计算公式为：

S₁₂＝ceil(D₁₂/W_c) (5)

其中S₁₂表示顶点(X₁,Y₁)和(X₂,Y₂)连接成的直线的分段次数，ceil(·)为向上取整函数，W_c表示环境模型中每个格网对应真实三维地形数据的像元宽度。

B4、初始化计数值j＝0。

B5、判断j>S₁₂是否成立，若是则进入步骤B8，否则进入步骤B6。

B6、根据分段次数计算分段后各个点的坐标，计算公式为：

其中(X_12j,Y_12j)为分段后第j个点的坐标，j＝0,1,2,...,S₁₂。

B7、令计数值j加1，并返回步骤B5。

B8、获取分段后各个点的坐标在环境模型中对应格网单元的行列号，得到多边形对应的格网单元。

B9、判断相邻两个格网单元是否在同一行或同一列上，若是则进入步骤B11，否则进入步骤B10。

B10、在未处于同一行或同一列的两相邻格网单元之间补充一个格网单元，进入步骤B11。

B11、将多边形对应的格网单元和补充的格网单元作为大气环境约束区域(或者将地形高度设为一个特大的值使得后续搜索算法无法通过)，如图5所示，完成大气环境约束的融入。

由于危险物约束一般简化为球或规则长方体形式，找到球心与顶点，依次融入环境模型即可。

S3、根据融入复杂环境约束信息的环境模型，采用多层扩展A*算法获取无人机的参考航迹。

将复杂环境约束信息都融入环境模型后，通过设计多层扩展A*算法，搜索获取参考航迹。A*算法作为一种启发式搜索算法，能够获得最优路径。但搜索空间较大时，算法搜索与运算将需要花费大量的时间与内存。Szczerba将无人机最大水平转弯角约束融入算法节点扩展过程中，提出稀疏A*搜索(Sparse A*Search，SAS)算法，缩减了算法搜索空间，提高了算法规划效率，但该方式应用于三维环境中，会出现格网划分问题，当格网划分过大时，可能会错过最佳路径而绕行，从而导致规划失败。本发明实施例在以上基础上，设计出多层扩展A*算法，通过新增水平方向向量、竖直分层扩展节点以及改进代价函数应用于无人机航迹规划。

如图6所示，步骤S3包括以下分步骤S3-1～S3-21：

S3-1、获取融入复杂环境约束信息的环境模型以及起始点与目标点的位置信息与高程信息。

S3-2、创建openlist线性表与closelist线性表，并初始化将其置为空。

S3-3、计算起始点的代价值，并将起始点添加至openlist线性表中。

S3-4、判断openlist线性表是否为空，若是则进入步骤S3-5，否则进入步骤S3-6。

S3-5、航迹搜索失败，进入步骤S3-21。

S3-6、获取openlist线性表中代价值最小的点作为当前点。

S3-7、判断当前点是否与目标点位于同一格网单元与高程，若是则进入步骤S3-8，否则进入步骤S3-9。

S3-8、航迹搜索成功，获取最优航迹节点，进入步骤S3-21。

S3-9、将当前点移除出openlist线性表，并添加进closelist线性表中。

S3-10、对当前点进行分层扩展，得到多个扩展点。

无人机在飞行过程中受性能约束，在爬升与俯冲时有最大角度的限制。根据水平扩展距离与无人机最大俯仰角的大小计算本次扩展的竖直高度可达范围，计算公式为：

其中H_next表示竖直高度可达范围，H_cur为当前飞行高度，L_C为环境模型的格网单元宽度，α为无人机最大俯仰角，direct为方向值。

本发明实施例中，方向值direct的取值为从正北方向开始，顺时针分成八个方向，如图7所示。每次搜索时，仅能搜索与原先方向相同，或左右最邻近的两方向，例如原先1方向时，仅能搜索812，原先2方向时仅能搜索123等。起始点与目标点没有方向限制时，则设置为0，此时8个方向均可搜索，这样同样能解决对起止点有方向约束的问题。

最后在竖直高度可达范围内以固定高度间隔为基准进行分层扩展，得到多个扩展点。

S3-11、判断是否遍历完所有扩展点，若是则返回步骤S3-4，否则选取下一个扩展点并进入步骤S3-12。

S3-12、判断closelist线性表中是否存在该扩展点，若是则进入步骤S3-13，否则进入步骤S3-14。

S3-13、舍去该扩展点，返回步骤S3-11。

S3-14、计算该扩展点的代价值。

S3-15、判断openlist线性表中是否存在该扩展点，若是则进入步骤S3-16，否则进入步骤S3-19。

S3-16、判断该扩展点的代价值是否小于openlist线性表中所存在与该扩展点位置相同的点的代价值，若是则进入步骤S3-17，否则进入步骤S3-18。

S3-17、在openlist线性表中用该扩展点替换扩展前的当前点，进入步骤S3-20。

S3-18、舍去该扩展点，返回步骤S3-11。

S3-19、将该扩展点添加进openlist线性表，进入步骤S3-20。

S3-20、将扩展前的当前点设置为该扩展点的父节点，返回步骤S3-11。

S3-21、按顺序连接最优航迹节点，得到无人机的参考航迹。

本发明实施例中，代价值的计算公式为：

f(n)＝g(n)+ε·h(n) (8)

其中f(n)表示当前节点n位置的代价值，g(n)表示从起始点到当前节点n位置的实际代价值，h(n)表示从当前节点n位置到目标点的估计代价值，ε为启发项权重系数且ε>1。代价函数的优劣很大程度影响着算法搜索效率，对于无人机航迹而言，不同的应用目的其影响因素不同，但一般至少受距离与飞行高度的影响，因此：

实际代价值g(n)的计算公式为：

g(n)＝ω₁L_g+ω₂Z_g+ω₃T₁+ω₄T₂ (9)

估计代价值h(n)的计算公式为：

h(n)＝ω₁L_h+ω₂Z_h+ω₃T₁+ω₄T₂ (10)

其中ω₁,ω₂,ω₃,ω₄均为权重值，L_g表示从起始点到当前节点n位置的航迹段长度，L_h表示从当前节点n位置到目标点的航迹段长度，Z_g表示从起始点到当前节点n位置的航迹平均高度，Z_h表示从当前节点n位置到目标点的航迹平均高度，T₁,T₂均为影响因素代价。

S4、对参考航迹进行航迹简化与航迹平滑，得到无人机可飞最优航迹。

航迹简化用于判断将频繁的微小转向又回复的航迹直接简化为直线，主要思路是根据相邻点方向向量的差异程度判断是否简化，简化方向向量相近的点，此外，对于中途唯一不同的突变点，采用容忍数机制，具有一定容忍数量，超过容忍数量便标记为不简化点。

如图8所示，航迹简化的具体方法为：

C1、设从起始点到目标点，总共有n个具有顺序关系的三维坐标点。

C2、依次计算相邻两点的方向向量，得到n-1个方向向量d₁,d₂,...,d_n-1。

C3、初始化计数值i＝1。

C4、判断i>n-2是否成立，若是则进入步骤C5，否则进入步骤C6。

C5、按顺序连接所有不简化点，结束航迹简化。

C6、设置变量p＝i，q＝i+1，并初始化容忍计数值num＝0。

C7、计算方向向量d_p和d_q的夹角余弦值，计算公式为。

其中为方向向量d_p和d_q的夹角。

C8、判断该余弦值是否大于容忍值v，若是则进入步骤C9，否则进入步骤C10。

C9、令容忍计数值num加1，进入步骤C11。

C10、重置容忍计数值num＝0，进入步骤C13。

C11、判断容忍计数值num是否大于容忍数阈值t，若是则进入步骤C12，否则进入步骤C13。

C12、将第q+1-num个点设置为不简化点，并令i＝q-num，返回步骤C4。

C13、令变量q加1，返回步骤C7。

根据以上算法，即可去掉与周围点不在一条直线上的突变点和在同一直线上的中间点，达到航迹简化效果。

在进行航迹简化后，所生成的航线均是由一系列直线段构成，其规划结果具有引导作用，仍不符合无人机平滑转弯的实际飞行特点，不具有可飞性。因此，需要进行航线平滑优化，生成一条可飞航线。而三次Cardinal样条曲线符合无人机飞行特点，能使航线平滑得更具备可行性和可飞性。因此本发明实施例中采用三次Cardinal样条曲线对航迹简化后的参考航迹进行平滑，得到无人机可飞最优航迹。

下面以一个具体实验例对本发明提供的一种顾及复杂环境约束的无人机三维航迹快速规划方法的效果作详细说明。

本实验例中采用谷歌卫星影像与高程数据，其中DOM数据精度为2m，DEM(DigitalElevation Model，数字高程模型)数据精度为20m，实验区是四川省甘孜藏族自治州金沙江附近区域近400km²的范围。实验是在C^#环境下使用ArcEngine进行开发，并搭建了三维系统平台。实验硬件环境是戴尔Precision M2800笔记本，其处理器为英特尔四核2.80GHz，内存16GB，显卡为AMD Radeon HD 8790M(2GB)，操作系统为Windows 7。无人机航迹规划初始参数为：最大俯仰角为20°，最大水平角为45°，最小航迹段为20m，最低离地高度为10m，最大飞行高度为4500m，以航迹距离为代价，启发项权重系数ε为1.44，起止点均采用WGS84坐标分别为(98.815534，31.046619，3570)，(98.676002，31.095922，4300)，途中设置大气环境约束区域，如表1所示。将地形、大气环境约束区域、起止点添加至同一场景中，如图9所示。

表1威胁的参数信息

按照以上参数，按照本发明步骤S1进行环境建模后，进行算法搜索。首先对本发明提出的多层扩展A*算法在算法搜索后航迹简化前、算法搜索与航迹简化后航迹平滑前与算法搜索与航迹简化以及航迹平滑后的航迹比较，分别为图10中(a)，(b)，(c)所示。而采用经典A*算法与稀疏A*算法搜索后未进行航迹简化时结果如图10中(d)所示。

由图10对比可知，无论是经典A*算法、稀疏A*算法还是本发明提供的多层扩展A*算法，在若未进行航迹简化时，其航迹均有波浪形式；在平滑前的航迹在转弯时均是直边形式，而平滑后转弯是曲线平滑过渡，更符合无人机的实际飞行特点。

之后将经典A*算法、稀疏A*算法与本发明提供的多层扩展A*算法分别进行对比实验，其航迹局部效果如图11所示。三种算法生成的结果信息如表2所示。

表2算法结果信息对比表

由图11整体航迹结果图可知，三种算法航迹均能有效避开约束区域，且航迹主要差别在前半段部分，将其放大如图12局部图所示。由图12与表2可知，经典A*算法由于其扩展方向不受限制，规划时间长，其路径仅考虑最短的需求，还造成路径多曲折，频繁转向，也未考虑无人机性能约束。稀疏A*算法虽顾及了无人机自身性能特点，并利用在算法搜索中所规划时间较短，但由于其扩展节点过，导致航迹缺乏最优性。本发明提供的多层扩展A*算法在其基础上，采用分层扩展的方式，在规划效率与精度都有一定的提升，同时顾及了无人机自身性能特点生成的航路为可飞航路，少转向，更加适合无人机飞行。

本发明针对复杂环境约束下无人机三维航迹规划效率缓慢与精度低的问题，结合三维地理场景特点构建航迹规划模型以应对复杂环境要求，考虑无人机性能特点、无人机任务条件等特点提出多层扩展A*算法，对A*算法中扩展方式、代价函数作了改进；在该算法计算完成后进行航迹优化，根据无人机尽量保持自身的姿态，不频繁转弯或者升降，且转弯时平滑的特点进行简化与航迹平滑，最终生成一条适合无人机飞行的航迹。通过选择案例区域与经典A*算法、稀疏A*算法进行对比实验，实验结果证明，本发明方法能够实现安全绕开威胁，并加快了航迹规划的速度与准确度。

本领域的普通技术人员将会意识到，这里所述的实施例是为了帮助读者理解本发明的原理，应被理解为本发明的保护范围并不局限于这样的特别陈述和实施例。本领域的普通技术人员可以根据本发明公开的这些技术启示做出各种不脱离本发明实质的其它各种具体变形和组合，这些变形和组合仍然在本发明的保护范围内。

Claims (10)

1.一种顾及复杂环境约束的无人机三维航迹快速规划方法，其特征在于，包括以下步骤：

S1、构建无人机三维航迹规划的环境模型；

S2、将复杂环境约束信息融入环境模型；

S3、根据融入复杂环境约束信息的环境模型，采用多层扩展A*算法获取无人机的参考航迹；

S4、对参考航迹进行航迹简化与航迹平滑，得到无人机可飞最优航迹。

2.根据权利要求1所述的无人机三维航迹快速规划方法，其特征在于，所述步骤S1具体为：

将无人机在改变方向时的最小惯性距离作为格网单元的长度，将路径规划区域划分为一个m×n的格网，在每个格网单元中存储其对应的行列号、该格网单元中无人机的最大飞行高度值以及可飞行的高度范围信息，得到无人机三维航迹规划的环境模型，表示为：

其中Ω_m表示环境模型空间，(X_i,Y_j)表示格网单元的行列号，m,n分别为环境模型中格网单元的行数和列数，表示格网单元(X_i,Y_j)的无人机可飞行高度范围，Z₁,Z₂,Z₃,Z₄均为高度值，且Z₁<Z₂<Z₃<Z₄...。

3.根据权利要求1所述的无人机三维航迹快速规划方法，其特征在于，所述步骤S2中的复杂环境约束信息包括地形环境约束和大气环境约束。

4.根据权利要求3所述的无人机三维航迹快速规划方法，其特征在于，所述地形环境约束的融入方法具体为：

A1、通过仿射变换将真实三维地形数据每个栅格像元的行列号转换为环境模型中格网单元的行列号，转换公式为：

其中(X_m,Y_m)表示环境模型中格网单元的行列号，(X_p,Y_p)表示真实三维地形数据栅格像元的行列号，(X₀,Y₀)表示真实三维地形数据在路径规划区域左上角的经度和纬度，W_c表示真实三维地形数据的像元宽度，H_c表示真实三维地形数据的像元高度，R₁,R₂为旋转参数；

A2、把真实三维地形数据每个栅格像元的地形高程信息添加进对应的格网单元，完成地形环境约束的融入。

5.根据权利要求3所述的无人机三维航迹快速规划方法，其特征在于，所述大气环境约束的融入方法具体为：

B1、将大气环境约束区域竖直投影成一个多边形，并获取多边形每个顶点的经纬度坐标；

B2、根据经纬度坐标计算每两个相邻顶点的欧式距离，计算公式为：

其中(X₁,Y₁)和(X₂,Y₂)分别表示多边形两个相邻顶点的经纬度坐标，D₁₂表示这两个相邻顶点的欧氏距离；

B3、根据两个相邻顶点的欧式距离计算这两个相邻顶点连接成的直线的分段次数，计算公式为：

S₁₂＝ceil(D₁₂/W_c)

其中S₁₂表示顶点(X₁,Y₁)和(X₂,Y₂)连接成的直线的分段次数，ceil(·)为向上取整函数，W_c表示环境模型中每个格网对应真实三维地形数据的像元宽度；

B4、初始化计数值j＝0；

B5、判断j>S₁₂是否成立，若是则进入步骤B8，否则进入步骤B6；

B6、根据分段次数计算分段后各个点的坐标，计算公式为：

其中(X_12j,Y_12j)为分段后第j个点的坐标，j＝0,1,2,...,S₁₂；

B7、令计数值j加1，并返回步骤B5；

B8、获取分段后各个点的坐标在环境模型中对应格网单元的行列号，得到多边形对应的格网单元；

B9、判断相邻两个格网单元是否在同一行或同一列上，若是则进入步骤B11，否则进入步骤B10；

B10、在未处于同一行或同一列的两相邻格网单元之间补充一个格网单元，进入步骤B11；

B11、将多边形对应的格网单元和补充的格网单元作为大气环境约束区域，完成大气环境约束的融入。

6.根据权利要求1所述的无人机三维航迹快速规划方法，其特征在于，所述步骤S3包括以下分步骤：

S3-1、获取融入复杂环境约束信息的环境模型以及起始点与目标点的位置信息与高程信息；

S3-2、创建openlist线性表与closelist线性表，并初始化将其置为空；

S3-3、计算起始点的代价值，并将起始点添加至openlist线性表中；

S3-4、判断openlist线性表是否为空，若是则进入步骤S3-5，否则进入步骤S3-6；

S3-5、航迹搜索失败，进入步骤S3-21；

S3-6、获取openlist线性表中代价值最小的点作为当前点；

S3-7、判断当前点是否与目标点位于同一格网单元与高程，若是则进入步骤S3-8，否则进入步骤S3-9；

S3-8、航迹搜索成功，获取最优航迹节点，进入步骤S3-21；

S3-9、将当前点移除出openlist线性表，并添加进closelist线性表中；

S3-10、对当前点进行分层扩展，得到多个扩展点；

S3-11、判断是否遍历完所有扩展点，若是则返回步骤S3-4，否则选取下一个扩展点并进入步骤S3-12；

S3-12、判断closelist线性表中是否存在该扩展点，若是则进入步骤S3-13，否则进入步骤S3-14；

S3-13、舍去该扩展点，返回步骤S3-11；

S3-14、计算该扩展点的代价值；

S3-15、判断openlist线性表中是否存在该扩展点，若是则进入步骤S3-16，否则进入步骤S3-19；

S3-16、判断该扩展点的代价值是否小于openlist线性表中所存在与该扩展点位置相同的点的代价值，若是则进入步骤S3-17，否则进入步骤S3-18；

S3-17、在openlist线性表中用该扩展点替换扩展前的当前点，进入步骤S3-20；

S3-18、舍去该扩展点，返回步骤S3-11；

S3-19、将该扩展点添加进openlist线性表，进入步骤S3-20；

S3-20、将扩展前的当前点设置为该扩展点的父节点，返回步骤S3-11；

S3-21、按顺序连接最优航迹节点，得到无人机的参考航迹。

7.根据权利要求6所述的无人机三维航迹快速规划方法，其特征在于，所述代价值的计算公式为：

f(n)＝g(n)+ε·h(n)

其中f(n)表示当前节点n位置的代价值，g(n)表示从起始点到当前节点n位置的实际代价值，h(n)表示从当前节点n位置到目标点的估计代价值，ε为启发项权重系数且ε>1；

实际代价值g(n)的计算公式为：

g(n)＝ω₁L_g+ω₂Z_g+ω₃T₁+ω₄T₂

估计代价值h(n)的计算公式为：

h(n)＝ω₁L_h+ω₂Z_h+ω₃T₁+ω₄T₂

其中ω₁,ω₂,ω₃,ω₄均为权重值，L_g表示从起始点到当前节点n位置的航迹段长度，L_h表示从当前节点n位置到目标点的航迹段长度，Z_g表示从起始点到当前节点n位置的航迹平均高度，Z_h表示从当前节点n位置到目标点的航迹平均高度，T₁,T₂均为影响因素代价。

8.根据权利要求6所述的无人机三维航迹快速规划方法，其特征在于，所述步骤S3-10中对当前点进行分层扩展的具体方法为：

根据水平扩展距离与无人机最大俯仰角的大小计算本次扩展的竖直高度可达范围，计算公式为：

其中H_next表示竖直高度可达范围，H_cur为当前飞行高度，L_C为环境模型的格网单元宽度，α为无人机最大俯仰角，direct为方向值，其取值为从正北方向开始，顺时针分成八个方向；

在竖直高度可达范围内以固定高度间隔为基准进行分层扩展，得到多个扩展点。

9.根据权利要求1所述的无人机三维航迹快速规划方法，其特征在于，所述步骤S4中航迹简化的具体方法为：

C1、设从起始点到目标点，总共有n个具有顺序关系的三维坐标点；

C2、依次计算相邻两点的方向向量，得到n-1个方向向量d₁,d₂,...,d_n-1；

C3、初始化计数值i＝1；

C4、判断i>n-2是否成立，若是则进入步骤C5，否则进入步骤C6；

C5、按顺序连接所有不简化点，结束航迹简化；

C6、设置变量p＝i，q＝i+1，并初始化容忍计数值num＝0；

C7、计算方向向量d_p和d_q的夹角余弦值，计算公式为：

其中为方向向量d_p和d_q的夹角；

C8、判断该余弦值是否大于容忍值v，若是则进入步骤C9，否则进入步骤C10；

C9、令容忍计数值num加1，进入步骤C11；

C10、重置容忍计数值num＝0，进入步骤C13；

C11、判断容忍计数值num是否大于容忍数阈值t，若是则进入步骤C12，否则进入步骤C13；

C12、将第q+1-num个点设置为不简化点，并令i＝q-num，返回步骤C4；

C13、令变量q加1，返回步骤C7。

10.根据权利要求1所述的无人机三维航迹快速规划方法，其特征在于，所述步骤S4中航迹平滑的具体方法为：

采用三次Cardinal样条曲线对航迹简化后的参考航迹进行平滑，得到无人机可飞最优航迹。