CN115930973B - 一种无人机航路规划方法及装置 - Google Patents

Abstract

本发明提供了一种无人机航路规划方法及装置，方法包括构建无人机航路规划模型，并根据无人机航路规划模型进行实际航路规划。本发明的航路规划方法是基于无人机机载导航传感器的性能，综合考虑机载导航传感器提供的导航参数的误差以及空域中障碍物覆盖情况，将现有的面向质点的航路规划算法转变为面向导航传感器误差覆盖区域的航路规划，实现无人机在一定安全区域内的动态航路规划。本发明在导航传感器性能的基础上同时考虑无人机的机动性能，针对无人机的机动性能限制，提供不同机动性能下无人机的个性化航路规划，使不同性能的无人机可以规划出最适合本机的航路，工程适用性及航路安全性大大提高。

Description

一种无人机航路规划方法及装置

技术领域

本发明属于路径规划技术领域，尤其是涉及一种无人机航路规划方法及装置。

背景技术

随着无人机的应用越来越广泛，有效的机载端实时航路规划成为无人机的自主飞行及运行安全的关键要素和核心技术。无人机航路规划算法是其中的关键。传统的航路规划算法主要有A*算法、Dijkstra算法、D*算法、人工势场法，基于采样的航路规划算法有PRM算法、RRT算法，智能仿生航路规划算法有神经网络算法、蚁群算法、遗传算法等。

传统的航路规划算法不考虑无人机导航***的性能，将无人机看作为一个质栅格进行航路规划，这会造成部分复杂低空空域，如城市峡谷、复杂山区等区域所规划的航路不可行。

例如，Dijkstra算法：该算法从起点遍历到其他各节栅格计算其距离直到目标节栅格的最短航路。该算法的主要特栅格是确保每一次的迭代航路均是最短的。该算法鲁棒性好但其搜索效率低。再例如A*算法：该算法从起点开始，检查其从起点开始，遍历起点周围邻近的栅格，然后再遍历已经遍历过的栅格邻近的栅格，逐步的向外扩散，直到找到终点。该算法准确性高、性能好，但可能出现目标不可达而浪费性能的现象。

这些航路规划大多都是静态航路规划，均假设不考虑导航***所提供的导航参数的精度，将无人机定义为质栅格，使无人机跟踪规划好的航路。但在现实条件下，无人机通常装配普通精度甚至低精度的导航模块，由于实际导航***所提供的导航参数的误差，会导致无人机与实际障碍物之间的安全区域降低，造成起点到终点的可行航路不存在。或者即使规划航路存在，也很有可能给无人机带来如碰撞等不可预知的风险。因此，在航路规划过程中，导航参数的误差所带来的影响是不可忽视的问题，对无人机的可行航路规划的工程应用有着十分重要的意义。

发明内容

有鉴于此，本发明一种无人机航路规划方法及装置，本发明的航路规划方法是基于无人机机载导航传感器的性能，综合考虑机载导航传感器提供的导航参数的误差以及空域中障碍物覆盖情况，将现有的面向质点（无人机作为一个点看待）的航路规划算法转变为面向导航传感器误差覆盖区域（无人机作为一个空间体看待）的航路规划，实现无人机在一定安全区域内的动态航路规划。本发明在导航传感器性能的基础上同时考虑无人机的机动性能，针对无人机的机动性能限制，提供不同机动性能下无人机的个性化航路规划，使不同性能的无人机可以规划出最适合本机的航路。

为达到上述目的，本发明的技术方案是这样实现的：

一种无人机航路规划方法，包括如下步骤：

步骤1：确定无人机飞行任务的起点和终点经纬度，根据飞行任务所在地区的地形数据库文件构建三维栅格地图；

步骤2：根据无人机的飞行高度进行分层航路规划，选择所有低于无人机飞行高度上限的高度作为无人机的可飞行高度，把高于无人机飞行高度上限的栅格设置为障碍物；

步骤3：确定无人机的导航***所提供的导航参数的误差大小以及无人机最大转弯角限制，将导航参数的误差大小转化为地形栅格值；

步骤4：进行A*算法搜索，不断搜索前、后、左、右、左斜前、左斜后、右斜前、右斜后的栅格，通过找出寻路消耗代价值最小的栅格作为新的起点再循环的找，直到找到终点，确定下一步可行最优航路；

步骤5：将无人机的导航参数的误差大小和无人机最大转弯角限制作为航路搜索的代价函数进行航路搜索；

步骤6：无人机根据规划出的航路飞行，导航***里的导航传感器提供无人机实时经纬度位置，不断更新起点，进行实时路线规划。

进一步的，所述步骤1中具体包括：

从无人机机载导航***输出起点，确定飞行任务终点，利用地形数据库文件，获取飞行任务所在的地区数字化地形数据库，导出高程图，利用编程语言构建三维栅格地图，确定横向边界和纵向边界的大小，再确定横向栅格的间距和纵向栅格的间距。

进一步的，所述步骤3中，将导航参数的误差大小转化为地形栅格值，误差大小为栅格数，并向上取整。

进一步的，所述步骤4中，A*算法的寻路消耗公式为

f_寻路消耗=g_{离起点距离}+h_{离终点距离}；

将用两栅格间的欧式距离作为估计代价h_{离终点距离}，欧氏距离衡量的是多维空间中两个栅格之间的绝对距离，将曼哈顿距离、无人机转弯角代价、航程代价、以及导航误差代价作为g_{离起点距离}代价值；

设计g_{离起点距离}函数的表达式为

为代价权重，且/>

，/>

为曼哈顿距离，/>

为无人机转弯角代价，/>

为航程代价，/>

为导航误差代价。

进一步的，所述步骤4中，A*算法还包括开启列表和关闭列表，开启列表用来存储可以考虑行进的栅格的列表，关闭列表用来存储不再考虑的栅格的列表，每次寻找的时候将周围非障碍物栅格且不在开启列表或者关闭列表的栅格放在开启列表中，将开启列表中寻路消耗最低的栅格放入关闭列表中，并判断该栅格是否是终点，如果是则寻路结束，反之则继续寻路。

进一步的，所述步骤5中具体包括：

搜索每一个要行走的下一栅格与所有障碍物之间的距离，形成一个矩阵变量，如果这个矩阵变量的最小值比无人机的导航***所提供的导航参数的误差小，则设置导航误差代价值为无穷大，规划航路不走该栅格，如果这个矩阵变量的最小值比无人机的导航***所提供的导航参数的误差大，则进行无人机转弯角度的筛选；

计算将要走的下一步栅格与前一步栅格形成的向量与前一步栅格和前两步的栅格形成的向量之间的夹角，将两个向量的点乘除以这两个向量的叉乘得出的值进行反余弦函数计算，得出两向量之间的夹角值，作为无人机转弯角度；

无人机最大转弯角度的大小恒大于等于0；

当无人机转弯角度的绝对值大于无人机最大允许转弯角度则无人机转弯角度代价值为无穷，过该栅格的航路被淘汰；否则，该栅格可以作为规划航路的下一个航路点。

本发明还提供了一种无人机航路规划装置，包括

构建底图单元，用于确定无人机飞行任务的起点和终点经纬度，根据飞行任务所在地区的地形数据库文件构建三维栅格地图；

飞行高度确定单元，用于根据无人机的飞行高度进行分层航路规划，选择所有低于无人机飞行高度上限的高度作为无人机的可飞行高度，把高于无人机飞行高度上限的栅格设置为障碍物；

数据确定单元，用于确定无人机的导航***所提供的导航参数的误差大小以及无人机最大转弯角限制，将导航参数的误差大小转化为地形栅格值；

最优航路搜索单元，用于进行A*算法搜索，不断搜索前、后、左、右、左斜前、左斜后、右斜前、右斜后的栅格，通过找出寻路消耗代价值最小的栅格作为新的起点再循环的找，直到找到终点，确定下一步可行最优航路；

数据处理单元，用于将无人机的导航参数的误差大小和无人机最大转弯角限制作为航路搜索的代价函数进行航路搜索；

实时航路规划单元，用于无人机根据规划出的航路飞行，导航***里的导航传感器提供无人机实时经纬度位置，不断更新起点，进行实时路线规划。

相对于现有技术，本发明所述的一种无人机航路规划方法及装置具有以下优势：

1、本发明利用低成本的导航传感器，创新并优化了传统的航路规划算法，使得低成本导航传感器的航路规划的安全性大大增高；

2、本发明提供基于无人机性能的个性化航路规划，不同的无人机有不同的飞行性能限制，不同的飞行性能限制就导致无人机不能使用同一个规划航路，提供个性化的航路规划，使不同的无人机都能规划出最适合自己的一条航路，工程适用性大大增高；

3、本发明可以进行动态分层航路规划，不同的飞行任务下，飞行高度不同，对应高度的障碍物也不同，以此，根据无人机的实时高度，在不同的高度进行最优航路规划，保证合理利用无人机的续航时间，提供最优航路，可有效提高航路规划的可行性及经济性；

4、本发明可根据导航***提供的实时经纬度更换起点，实时更新最优航路，提高了航路规划的灵活性。

附图说明

构成本发明的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：

图1为本发明的方法流程示意图；

图2为本发明实施例中的400m飞行航路规划图示意图；

图3为本发明实施例中的380m飞行航路规划图示意图；

图4为本发明实施例中的420m飞行航路规划图示意图；

图5为本发明实施例中的20m误差下规划的航路示意图；

图6为本发明实施例中的100m误差下规划的航路示意图；

图7为本发明实施例中的50度转弯角限制所规划的航路示意图；

图8为本发明实施例中的360度所限制的转弯角规划的航路示意图；

图9为本发明实施例中的误差大小为20m和0误差的航路规划对比图；

图10为本发明实施例中的误差大小为30m和0误差的航路规划对比图；

图11为本发明实施例中的实时更新起点航路规划示意图。

实施方式

需要说明的是，在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。

在本发明的描述中，需要理解的是，术语“中心”、“纵向”、“横向”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语“第一”、“第二”等仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”等的特征可以明示或者隐含地包括一个或者更多个该特征。在本发明的描述中，除非另有说明，“多个”的含义是两个或两个以上。

在本发明的描述中，需要说明的是，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以通过具体情况理解上述术语在本发明中的具体含义。

下面将参考附图并结合实施例来详细说明本发明。

如图1所示，本发明提供了一种无人机航路规划方法，本发明的航路规划方法是基于无人机机载导航传感器的性能，综合考虑机载导航传感器提供的导航参数的误差以及空域中障碍物覆盖情况，将现有的面向质点（无人机作为一个点看待）的航路规划算法转变为面向导航传感器误差覆盖区域（无人机作为一个空间体看待）的航路规划，实现无人机在一定安全区域内的动态航路规划。本发明在导航传感器性能的基础上同时考虑无人机的机动性能，针对无人机的机动性能限制，提供不同机动性能下无人机的个性化航路规划，使不同性能的无人机可以规划出最适合本机的航路。本发明的无人机航路规划方法包括构建无人机航路规划模型，并根据无人机航路规划模型进行实际航路规划，其中，构建无人机航路规划模型具体包括如下步骤：

第一步：从机载导航***输出起点，确定飞行任务终点，利用地形数据库文件，获取飞行任务所在的地区数字化地形数据库，导出高程图。

第二步：利用编程语言构建三维栅格地图，确定横向边界和纵向边界的大小，再由此确定横向栅格的间距和纵向栅格的间距。

第三步，根据无人机的飞行高度进行分层航路规划，选择所有低于无人机飞行高度上限的高度作为无人机的可飞行高度，把高于无人机飞行高度上限的栅格设置为障碍物。

第四步，确定无人机的导航***所提供的导航参数的误差大小以及无人机最大转弯角限制，将导航参数的误差大小转化为地形栅格值。

第五步，进行A*算法搜索最优航路，不断地搜索前、后、左、右、左斜前、左斜后、右斜前、右斜后的栅格，通过找出寻路消耗代价值最小的栅格作为新的起点再循环的找，直到找到终点。寻路消耗公式为

f_寻路消耗=g_{离起点距离}+h_{离终点距离}。

将用两栅格间的欧式距离作为估计代价h_{离终点距离}，欧氏距离衡量的是多维空间中两个栅格之间的绝对距离，将曼哈顿距离、无人机转弯角代价、航程代价、以及导航误差代价作为g_{离起点距离}代价值；

设计g_{离起点距离}函数的表达式为

为代价权重，且/>

，/>

为曼哈顿距离，/>

为无人机转弯角代价，/>

为航程代价，/>

为导航误差代价。

然后进行开启列表和关闭列表，开启列表用来存储可以考虑行进的栅格的列表，关闭列表用来存储不再考虑的栅格的列表，每次寻找的时候将周围非障碍物栅格（可行栅格）且不在开启列表或者关闭列表的栅格放在开启列表中，将开启列表中寻路消耗最低的栅格放入关闭列表中，并判断该栅格是否是终点，如果是则寻路结束，反之则继续寻路。

第六步，判断是否满足无人机导航性能约束条件，具体包括

搜索每一个要行走的下一栅格与所有障碍物之间的距离，形成一个矩阵变量，如果这个矩阵变量的最小值比无人机的导航***所提供的导航参数的误差小，则设置导航误差代价值为无穷大，规划航路不走该栅格，如果这个矩阵变量的最小值比无人机的导航***所提供的导航参数的误差大，则进行无人机转弯角度的筛选；

判断是否满足无人机飞行性能约束条件，具体包括

计算将要走的下一栅格与前一步栅格形成的向量与前一步栅格和前两步的栅格形成的向量之间的夹角，将两个向量的点乘除以这两个向量的叉乘得出的值进行反余弦函数计算，得出两向量之间的夹角值，即为无人机转弯角度；

无人机最大转弯角度的大小恒大于等于0；

当无人机转弯角度的绝对值大于无人机最大允许转弯角度则无人机转弯角度代价值为无穷，过该栅格的航路被淘汰，此栅格将被标记不再被搜索，重新规划航路；否则，该栅格可以作为规划航路的下一个航路点。

构建好无人机航路规划模型之后，进行实际航路规划。

第七步，将起点的高度设置为地形数据库中此栅格原有的高度，然后缓慢爬坡至设定的飞行高度，进行在此飞行高度的航路规划，同样，将终点的高度也设置为地形数据库中此栅格原有的高度，飞行任务结束后，缓慢降落至地面。

第八步，根据导航传感器提供的无人机实时经纬度位置，重新定义起点，更新航线，实现动态实时航路规划。

本发明还提供了一种无人机航路规划装置，包括

构建底图单元，用于确定无人机飞行任务的起点和终点经纬度，根据飞行任务所在地区的地形数据库文件构建三维栅格地图；

飞行高度确定单元，用于根据无人机的飞行高度进行分层航路规划，选择所有低于无人机飞行高度上限的高度作为无人机的可飞行高度，把高于无人机飞行高度上限的栅格设置为障碍物；

数据确定单元，用于确定无人机的导航***所提供的导航参数的误差大小以及无人机最大转弯角限制，将导航参数的误差大小转化为地形栅格值；

最优航路搜索单元，用于进行A*算法搜索，不断搜索前、后、左、右、左斜前、左斜后、右斜前、右斜后的栅格，通过找出寻路消耗代价值最小的栅格作为新的起点再循环的找，直到找到终点，确定下一步可行最优航路；

数据处理单元，用于将无人机的导航参数的误差大小和无人机最大转弯角限制作为航路搜索的代价函数进行航路搜索；

实时航路规划单元，用于无人机根据规划出的航路飞行，导航***里的导航传感器提供无人机实时经纬度位置，不断更新起点，进行实时路线规划。

下面通过具体的实施过程描述本发明的方法。

在全图的栅格中搜索高于无人机飞行任务高度的栅格，将它们设置为障碍物变量，所以飞行任务高度不同，障碍物变量也不同。然后将剩下的栅格的高度设置为无人机飞行任务高度，将二者合为一个变量，将此变量放入A*算法中，即基于飞行任务高度形成一个平面，航路就将在此平面上进行规划，实现了分层航路规划。图2-4是相同起点与终点在不同高度下的飞行路线图，图2为高度为400 m的飞行航路规划图，图3为高度为380m的飞行航路规划图，图4为高度为420m的飞行航路规划图。

已知无人机导航***提供的导航参数的误差大小的值，再基于A*算法，将下一搜索栅格与所有障碍物之间的距离作为代价，如果距离小于误差值，此栅格的导航误差代价值为无穷。图5-6为相同起点，相同的无人机转弯角度，不同的误差，得出的不同航路规划。图5为20m误差下规划的航路，图6为100m误差下规划的航路。

将无人机的转弯性能限制，加入到A*算法的代价值中，如果搜索栅格与当前无人机所在的栅格形成的向量与之前航线的夹角的绝对值超过无人机最大转弯角限制，此栅格的无人机转弯角度代价值即为无穷大。图7-8为相同起点，相同导航参数的误差下，不同无人机转弯角的限制，得出不同的航路规划。图7为50度转弯角限制所规划的航路，图8为360度转弯角限制所规划的航路。

有导航参数的误差安全区域限制条件下与无导航参数的误差下规划出的航路不同，图9和图10是基于不同的起点和终点规划出的航路，图9为误差大小为20m和0误差的航路规划对比图，图10为误差大小为30m和0误差的航路规划对比图。可以看出相较于传统A*算法，基于误差条件下的A*算法计算了障碍物之间的距离与误差的大小，规划出了更安全的最优航路，避免了无人机的碰撞风险，大大增加了航路的安全性。

在无人机的飞行过程中，随着机载导航***传回来的无人机实时的坐标的变化来实时规划路线，将机载导航***传输回来的坐标设置为起点，重新规划路线。图11为本发明实施例中的实时更新起点航路规划示意图，其中每个*代表重新规定的起点。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (7)

1.一种无人机航路规划方法，其特征在于：包括如下步骤：

步骤1：确定无人机飞行任务的起点和终点经纬度，根据飞行任务所在地区的地形数据库文件构建三维栅格地图；

步骤2：根据无人机的飞行高度进行分层航路规划，选择所有低于无人机飞行高度上限的高度作为无人机的可飞行高度，把高于无人机飞行高度上限的栅格设置为障碍物；

步骤3：确定无人机的导航***所提供的导航参数的误差大小以及无人机最大转弯角限制，将导航参数的误差大小转化为地形栅格值；

步骤4：进行A*算法搜索，不断搜索前、后、左、右、左斜前、左斜后、右斜前、右斜后的栅格，通过找出寻路消耗代价值最小的栅格作为新的起点再循环的找，直到找到终点，确定下一步可行最优航路；

步骤5：将无人机的导航参数的误差大小和无人机最大转弯角限制作为航路搜索的代价函数进行航路搜索；

步骤6：无人机根据规划出的航路飞行，导航***里的导航传感器提供无人机实时经纬度位置，不断更新起点，进行实时路线规划。

2.根据权利要求1所述的一种无人机航路规划方法，其特征在于：所述步骤1中具体包括：

从无人机机载导航***输出起点，确定飞行任务终点，利用地形数据库文件，获取飞行任务所在的地区数字化地形数据库，导出高程图，利用编程语言构建三维栅格地图，确定横向边界和纵向边界的大小，再确定横向栅格的间距和纵向栅格的间距。

3.根据权利要求1所述的一种无人机航路规划方法，其特征在于：所述步骤3中，将导航参数的误差大小转化为地形栅格值，误差大小为栅格数，并向上取整。

4.根据权利要求1所述的一种无人机航路规划方法，其特征在于：所述步骤4中，A*算法的寻路消耗公式为

f_寻路消耗=g_{离起点距离}+h_{离终点距离}；

将用两栅格间的欧式距离作为估计代价h_{离终点距离}，欧氏距离衡量的是多维空间中两个栅格之间的绝对距离，将曼哈顿距离、无人机转弯角代价、航程代价、以及导航误差代价作为g_{离起点距离}代价值；

设计g_{离起点距离}函数的表达式为

；

为代价权重，且/>

，/>

为曼哈顿距离，/>

为无人机转弯角代价，/>

为航程代价，/>

为导航误差代价。

5.根据权利要求1所述的一种无人机航路规划方法，其特征在于：所述步骤4中，A*算法还包括开启列表和关闭列表，开启列表用来存储考虑行进的栅格的列表，关闭列表用来存储不再考虑的栅格的列表，每次寻找的时候将周围非障碍物栅格且不在开启列表或者关闭列表的栅格放在开启列表中，将开启列表中寻路消耗最低的栅格放入关闭列表中，并判断该栅格是否是终点，如果是则寻路结束，反之则继续寻路。

6.根据权利要求1所述的一种无人机航路规划方法，其特征在于：所述步骤5中具体包括：

搜索每一个要行走的下一栅格与所有障碍物之间的距离，形成一个矩阵变量，如果这个矩阵变量的最小值比无人机的导航***所提供的导航参数的误差小，则设置导航误差代价值为无穷大，规划航路不走该栅格，如果这个矩阵变量的最小值比无人机的导航***所提供的导航参数的误差大，则进行无人机转弯角度的筛选；

计算将要走的下一步栅格与前一步栅格形成的向量与前一步栅格和前两步的栅格形成的向量之间的夹角，将两个向量的点乘除以这两个向量的叉乘得出的值进行反余弦函数计算，得出两向量之间的夹角值，作为无人机转弯角度；

无人机最大转弯角度的大小恒大于等于0；

当无人机转弯角度的绝对值大于无人机最大允许转弯角度则无人机转弯角度代价值为无穷，过该栅格的航路被淘汰；否则，该栅格作为规划航路的下一个航路点。

7.一种无人机航路规划装置，其特征在于：包括

构建底图单元，用于确定无人机飞行任务的起点和终点经纬度，根据飞行任务所在地区的地形数据库文件构建三维栅格地图；

飞行高度确定单元，用于根据无人机的飞行高度进行分层航路规划，选择所有低于无人机飞行高度上限的高度作为无人机的可飞行高度，把高于无人机飞行高度上限的栅格设置为障碍物；

数据确定单元，用于确定无人机的导航***所提供的导航参数的误差大小以及无人机最大转弯角限制，将导航参数的误差大小转化为地形栅格值；

最优航路搜索单元，用于进行A*算法搜索，不断搜索前、后、左、右、左斜前、左斜后、右斜前、右斜后的栅格，通过找出寻路消耗代价值最小的栅格作为新的起点再循环的找，直到找到终点，确定下一步可行最优航路；

数据处理单元，用于将无人机的导航参数的误差大小和无人机最大转弯角限制作为航路搜索的代价函数进行航路搜索；

实时航路规划单元，用于无人机根据规划出的航路飞行，导航***里的导航传感器提供无人机实时经纬度位置，不断更新起点，进行实时路线规划。

Images