CN114296474A - 一种基于路径时间代价的无人机路径规划方法及系统 - Google Patents

Abstract

本发明涉及一种基于路径时间代价的无人机路径规划方法及系统，可根据无人机的运动约束条件、可飞行区域和障碍信息，以路径时间最短为目标，通过定制设计路径搜索过程中的节点扩展方式和节点代价计算机制，能够生成时间最短的无人机避障飞行路径。

Description

一种基于路径时间代价的无人机路径规划方法及系统

技术领域

本发明涉及无人机技术领域，特别是涉及一种基于路径时间代价的无人机路径规划方法及系统。

背景技术

随着人工智能的快速崛起，无人机已经在电力巡检领域中获得较为广泛的应用。相较于传统的人工巡检方式，无人机能够利用信息化技术实现自动巡检，并将数据进行实时传输，大幅降低人力劳动并能对传统地面人工巡检和机器人巡检的死角进行检查。无人机智能巡检的重要表现之一就是在不依靠人类控制的情况下自主地运动，这也是无人机执行智能巡检任务的基础环节。无人机所处的环境含有大量不确定的因素，要想在其中安全地执行巡检任务，需要具备以下两个能力：(1)无人机可以根据传感器感知外部环境及自身状态，需要实时定位与构图技术支持；(2)无人可以依据感知数据自我决策，在约束条件下以最小代价实现在含有障碍物的环境中移动，需要路径规划算法支持。

无人机的路径规划决定了无人机的巡检路线以及是否能够到达指定的巡检点进行检查，因此研究无人机的路径规划，对于巡检应用而言具有重要的现实意义。无人机路径规划的基本任务是在考虑无人机的物理性能约束、环境障碍约束等条件下，找到一条从起始点到目标点的无碰撞路径，并在此基础上使其实现如规划路径点最少、路径长度最短、无人机能量消耗最小等要求，最终为无人机确定一条最优的运动路径。

针对无人机巡检应用，完成时间是衡量任务效能的一项重要指标，而路径规划方法一般以路径长度最小为目标，难以准确反应实际需求。对此，在构建飞行时间代价的近似计算方法基础上，提出一种最小化任务时间的无人机避障路径规划方法，记为T-A*算法。

发明内容

本发明的目的是提供一种基于路径时间代价的无人机路径规划方法及系统，能够规划出一条路径时间最短的安全巡检路径。

为实现上述目的，本发明提供了如下方案：

一种基于路径时间代价的无人机路径规划方法，所述规划方法包括：

约束条件包括：无人机的可行区域、起点位置、目标点位置、可行区域内的障碍物。

无人机的可行区域：需要保证无人机在可行区域内运动；

可行区域内的障碍物：需要无人机在规划路径时避开区域内的障碍物。

1.在可行区域内且非障碍物的地方选择起点和终点；

2.将起点位置加入OPEN表；

3.取出OPEN表中代价值(代价值为：f＝g+h，其中g表示从起点到当前点的飞行时间，h表示当前点到目标点的估计时间)最小的节点，作为当前节点，并把当前节点加入CLOSE表中。若当前节点与目标点位置相等，则寻路结束；否则，扩展当前节点的邻居节点，并将选择邻居节点加入OPEN表中；

4.当OPEN表为空时，寻路结束；否则返回第3步继续执行。

根据所述约束条件及算法流程，利用T-A*算法找到起始点到目标点损耗时间最短的路径。

一种基于路径时间代价的无人机的路径规划系统，所述规划系统包括：

获取模块，用于获取无人机的运行约束条件；所述运行约束条件包括所述无人机的可行区域、起始点位置、目标点位置、步长、扩展节点和所述可行区域内的障碍物信息；

可行路径生成模块，用于根据所述运行约束条件，利用T-A*算法生成一条可行路径；

节点搜索模块，用于利用无人机转弯特性计算每一所述可扩展节点的路径时间代价，并选取所述路径时间代价最小的可扩展节点作为下一节点。

根据本发明提供的具体实施例，本发明公开了以下技术效果：

本发明所提供的一种基于路径时间代价的无人机路径规划方法及系统，在满足无人机的运行约束条件下，在节点扩展环节中，利用时间代价函数计算每一可行路径的路径时间代价，并选取路径时间代价最小的可行路径作为无人机的规划路径，进而能够为无人机规划出一条时间最短的规划路径，减少无人机实际任务的路径时间代价，从而提高了无人机执行任务的效率。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例1所提供的规划方法的方法流程图；

图2为本发明实施例1所提供的规划方法的详细流程图；

图3为本发明实施例1所提供的无人机避障路径规划示意图

图4为本发明实施例1所提供的无人机障碍规避约束示意图；

图5为本发明实施例1所提供的无人机飞行范围约束示意图；

图6为本发明实施例1所提供的无人机三维空间节点扩展示意图

图7为本发明实施例1所提供的无人机不同水平面的扩展节点示意图；

图8为本发明实施例1所提供的基于Gazebo的无人机路径跟踪试验示意图；

图9为本发明实施例1所提供的T-A*算法和A*算法100次测试试验的路径时间结果对比图；

图10为本发明实施例1所提供的T-A*算法和A*算法100次测试试验的路径长度结果对比图；

图11为本发明实施例1所提供的无人机路径规划结果的三维图；

图12为本发明实施例1所提供的无人机路径规划结果的俯视图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明的目的是提供一种基于路径时间代价的无人机路径规划方法及系统，能够规划出一条路径时间最短的规划路径。

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。

实施例1：

无人机路径规划算法可以分为基于节点的方法和基于采样的方法。基于采样的方法中，RRT(Rapidly-Exploring Random Tree，快速扩展随机树)算法原理简单，节点扩展不需要进行预处理，具有较强的搜索能力，能够适应任务和环境的动态变化。基于节点的方法的代表为A*算法，A*算法是对Dijkstra算法的改进，其引入了启发式函数和约束条件，并且在每轮寻优搜索中，需计算全局范围内每一个可行解对应的启发式函数的值，随后选出该轮搜索中具有最小启发式函数值的可行解作为当前轮次的最优解，然后继续下一轮的寻优搜索，最终保证全局最优路径能够被A*算法找到，其搜索能力较强，能够收敛到全局最优路径，A*算法通过增加启发信息，使得扩展节点规模大幅下降。因而，A*及其改进算法在工程实际中得到了广泛应用。

在以A*为代表的启发式搜索算法扩展过程中，通过代价函数来判断和选取扩展点中的最优点作为下一次搜索点，直至找到最终目标节点位置，从而得到最优路径。因此代价函数的选取对A*等启发式搜索算法的性能至关重要。A*算法的启发函数一般选为曼哈顿距离、欧式距离、对角线距离，这些距离准则隐含了无人机飞行时间仅与距离成比例的假设，然而对于不同转弯角下的等距离路径，无人机跟踪飞行的时间通常是不同的。

为了更加准确地考虑无人机沿规划路径的飞行时间，提出一种改进A*算法，采用基于飞行时间的代价函数进行路径搜索，并且在代价函数中加入转弯飞行引起的时间代价惩罚项，在确保路径规划结果可行的情况下，实现时间最小化的路径规划。

如图1和图2所示，本实施例所提供的一种基于路径时间代价的无人机路径规划方法包括：

S1：获取无人机的运动约束条件；所述运行约束条件包括所述无人机的可行区域、起始点位置、目标点位置和所述可行区域内的障碍物信息；

具体地，运行约束条件包括可行区域、障碍物信息、无人机路径规划的起始点x_start、目标点x_goal。

面向路径时间最优的无人机路径规划需要在满足避障约束和运动约束的条件下，生成从给定起点到终点的可行路径，并使得路径时间达到最优或次优。其示意图如图3所示。

本文考虑的无人机避障路径规划的目标函数和约束条件如下所述。

(1)路径规划目标函数

无人机的飞行路径L由一系列路径点表示，记为L＝{P_i,i＝1,2,..,N}，其中P_i为表示第i个路径点，N为路径点的总数量。对于本文考虑的时间最优路径规划问题，则路径规划的目标函数可表示为

其中，t_f表示无人机完成整条路径飞行的时间，t_i表示第i段路径的飞行时间，第i段路径即为第i个和第i+1个点间的路径。

(2)障碍规避约束

无人机执行任务过程中，需要对环境中的障碍进行规避，保障飞行安全。本文采用三维栅格地图的方式表示环境中的障碍信息，图4给出了导航栅格地图的二维投影示意图，其中存在障碍物的栅格被设置为不可飞行区域，不存在障碍的栅格则为可飞行区域，记为C^free。

为了满足障碍规避约束，需要路径点在可飞行区域C^free内，如下式所示。在路径搜索过程中，可以根据扩展点的位置和栅格地图信息，通过地图查询的方式确定搜索到的新节点是否满足障碍规避约束。

P_i∈C^free

(3)无人机飞行约束

无人机在飞行过程中，由于受到运动学和动力学的约束，无人机无法瞬时进行反向飞行。对于时间最优路径规划，应尽量避免无人机大幅减速进行方向调整，因此在本文研究中，假设无人机路径搜索过程中，其可行的扩展空间范围为当前速度方向确定的半空间，示意图如图4所示。图中，无人机当前路径点的速度方向向右，则下一路径点的可达范围为该无人机所在位置右侧的半空间，而左侧的半空间认为是下一路径点所不可达的，以减少无人机由于反方向飞行引起的减速，提高无人机的平均飞行速度。

S2：根据所述启发函数运行约束条件，利用A*算法生成可行路径；

S21：通过启发函数来搜寻每一步时间代价最小的节点从而找到时间代价最小的路径；S21具体包括：

1)定义名为OPEN和CLOSED的两个列表；OPEN表用于存储搜寻路径所需的有效节点，CLOSED表用于存储无用节点；

2)x_start为起始节点，x_goal为目标节点，将CLOSED表初始状态设为空，并将起始节点x_start放入OPEN表中；S21具体包括：

3)查看与起始点x_start相邻的点n(n称为x_start的子节点，x_start称为n的父节点)，可通过的点加入到OPEN表中，计算它们的f、g和h值。将点x_start移入CLOSED表中；节点代价f值包括累计代价g和估计代价h。其中，累计代价g表示从起点到当前点的飞行时间，估计代价h表示从当前点到目标点的估计时间，也称为启发函数。

4)判断OPEN表是否为空，如果是，表示搜索失败，如果不是，则执行下一步骤；

5)将点n从OPEN表移除加入到CLOSED表中，判断n是否为目标节点x_goal，如果是，表示搜索成功，算法运行结束；

6)如果不是，则扩展搜索n的子节点：

a.如果子节点是不可通过或在CLOSED表中，忽略它。

b.子节点如果不在OPEN表中，则加入OPEN表，并且把当前点设置为它的父节点，计算该点的f、g和h值。

7)跳转到步骤4)；

8)结束后保存节点。从终点开始沿着父节点方向寻径直至起点，即为最优路径。

在S2之前，本实施例的规划方法还包括：对无人机进行转弯运动特性试验，得到转弯特性数据的步骤。

本发明中的T-A*算法在路径搜索过程中，节点代价值f包括累计代价g和估计代价h，如下式所示。其中，累计代价g表示从起点到当前点的飞行时间，估计代价h表示从当前点到目标点的估计时间，也称为启发函数。

f＝g+h

由于T-A*算法中的节点代价期望基于飞行时间来表示，但由于实际飞行时间难以准确计算，因此本文采用基于路径跟踪测试结果来估算节点代价的方法。具体的计算方式如下式所示。

其中，g_f表示父节点的累计代价，l_g表示父节点到当前点的直线距离，l_h表示当前点到目标点的对角距离，

表示通过路径跟踪试验确定的单个步长直飞平均速度，p表示在当前节点需要转弯或变高飞向扩展节点所引起的时间惩罚值，其计算方法为

p＝t_m-t₀

其中，t₀表示跟踪试验测得的沿原方向飞向扩展节点1的时间，t_m表示跟踪试验测得的转弯或变高机动飞向其它扩展节点的时间。

本实施例中应用的T-A*方法，计算无人机路径时间代价的关键在于获取进行不同方向节点扩展时的代价计算参数，对此需要对不同的节点扩展路径进行路径跟踪试验，测试不同扩展方向下的飞行时间。本实施例的无人机路径跟踪试验在ROS和Gazebo环境下进行，如图8所示，无人机控制程序基于开源程序PX4实现。其中，无人机采用Gazebo模型库中的3DR-IRIS模型，无人机水平方向的最大速度设为2m/s，无人机上升和下降的最大速度设为1m/s。

由于四旋翼无人机运动的对称性，跟踪试验的测试路径包括图6所示的从节点0分别到节点1～8的路径。考虑到飞行控制误差，每个扩展路径分别进行10次跟踪试验，其飞行时间的平均值统计结果如表1所示。

表1

扩展节点	时间(s)	扩展节点	时间(s)
				1	2.40	6	4.70
2	4.50	8	4.52
				4	4.25	9	3.56
5	3.81	10	3.87

为了验证T-A*算法的有效性及其规划结果在路径时间上的优势，开展T-A*和A*算法的对比仿真试验。

仿真测试中，将起始点设置为原点，目标点则在非障碍区域内随机生成，随机100次获得100组测试场景。然后针对每组场景，分别采用T-A*和A*算法进行路径规划，记录两种算法所规划路径的长度和时间指标。100次试验的路径时间和长度数据曲线如图9和图10所示。由图可以看出，两种算法生成路径的长度基本相同，但T-A*算法所生成路径对应的飞行时间小于A*算法所生成的路径时间。根据100次试验的统计结果，T-A*和A*算法生成路径的飞行时间平均值分别为59.16s和88.99s，即T-A*算法生成的路径相比A*算法可减少约1/3的飞行时间。

图11和图12给出一个测试试验场景下的T-A*算法和A*算法的路径规划结果。图中两条路径的长度均为86.38m，但A*规划结果的路径时间约为101.92s，而T-A*规划的路径时间仅为49.61s。从图12中可以看出，无人机沿T-A*算法生成的路径飞行时，其转弯机动更少，有利于保持更大的平均速度，因此在路径长度相同时，T-A*算法获得路径结果用于无人机跟踪飞行时，无人机抵达目标点所需要的飞行时间更短。

Claims (10)

1.一种基于路径时间代价的无人机路径规划方法，其特征在于，所述规划方法包括：

获取无人机的运行约束条件；所述运行约束条件包括所述无人机的可行区域、障碍物信息、无人机路径规划的起始点x_start、目标点x_goal、搜索步长、初始方向角；根据所述运行约束条件，利用T-A*算法生成路径时间代价最小的可行路径；

利用路径时间代价函数确保算法每一步扩展的节点的时间代价最小，并最终得到路径时间代价最小的可行路径作为所述无人机的规划路径。

2.根据权利要求1所述的基于路径时间代价的无人机路径规划方法，其特征在于，所述根据所述运行约束条件，利用T-A*算法生成一条可行路径具体包括：

定义名为OPEN和CLOSED的两个列表；OPEN表用于存储搜寻路径所需的有效节点，CLOSED表用于存储无用节点。

3.根据权利要求2所述的基于路径时间代价的无人机路径规划方法，其特征在于：x_start为起始节点，x_goal为目标节点，将CLOSED表初始状态设为空，并将起始节点x_start放入OPEN表中。

4.根据权利要求2所述的基于路径时间代价的无人机路径规划方法，其特征在于，查看与起始点x_start相邻的点n(n称为x_start的子节点，x_start称为n的父节点)，可通过的点加入到OPEN表中，计算它们的f、g和h值；将点x_start移入CLOSED表中；节点代价f值包括累计代价g和估计代价h；其中，累计代价g表示从起点到当前点的飞行时间，估计代价h表示从当前点到目标点的估计时间，也称为启发函数。

5.根据权利要求4所述的基于路径时间代价的无人机路径规划方法，其特征在于，将点n从OPEN表移除加入到CLOSED表中，判断n是否为目标节点x_goal，如果是，表示搜索成功，算法运行结束。

6.根据权利要求5所述的基于路径时间代价的无人机路径规划方法，其特征在于，如果不是，则扩展搜索n的子节点：

a.如果子节点是不可通过或在CLOSED表中，忽略它；

b.子节点如果不在OPEN表中，则加入OPEN表，并且把当前点设置为它的父节点，计算所述子节点的f、g和h值。

7.根据权利要求6所述的基于路径时间代价的无人机路径规划方法，其特征在于，查看与起始点x_start相邻的点n(n称为x_start的子节点，x_start称为n的父节点)，可通过的点加入到OPEN表中，计算它们的f、g和h值；将点x_start移入CLOSED表中；节点代价f值包括累计代价g和估计代价h；其中，累计代价g表示从起点到当前点的飞行时间，估计代价h表示从当前点到目标点的估计时间，也称为启发函数。

8.根据权利要求7所述的基于路径时间代价的无人机路径规划方法，其特征在于，结束后保存节点，从终点开始沿着父节点方向寻径直至起点，即为最优路径。

9.根据权利要求1所述的基于路径时间代价的无人机路径规划方法，其特征在于，利用转弯特性函数计算每一待扩展节点的路径时间代价。

10.一种基于路径时间代价的无人机路径规划系统，其特征在于，所述规划系统包括：

获取模块，用于获取无人机的运行约束条件；所述运行约束条件包括无人机的可行区域、障碍物信息、无人机路径规划的起始点x_start、目标点x_goal，搜索步长、初始方向角和无人机的最大转弯角；根据所述运行约束条件，利用T-A*算法生成路径时间代价最小的可行路径；

可行路径生成模块，用于根据所述运行约束条件和基于路径时间代价的启发函数，利用T-A*算法生成一条路径时间代价最小的可行路径。

Images