CN109947129A - 基于Dijkstra与改进粒子群算法的旋翼无人机路径规划方法 - Google Patents

Abstract

本发明公开了一种基于Dijkstra与改进粒子群算法的旋翼无人机路径规划方法，包括步骤：1)通过对比超声波传感器及激光传感器，选定合适的测距传感器用于测量无人机周围障碍物的距离与方向，构建飞行环境的全局地图；2)根据全局地图，利用Dijkstra方法规划多条旋翼无人机飞行的可行路径；3)对传统的粒子群算法进行改进，采用改进后的粒子群优化算法搜索全局最优路径，并生成最优路径图；本发明通过激光传感器检测环境并构建全局地图，将Dijkstra和改进的粒子群优化算法结合起来，搜寻出利于旋翼无人机飞行的全局最优路径，有效实现无人机的路径规划功能，从而使得旋翼无人机精确避障，安全飞行。

Description

基于Dijkstra与改进粒子群算法的旋翼无人机路径规划方法

技术领域

本发明涉及消费级无人机安全飞行的技术领域，尤其是指一种基于Dijkstra与改进粒子群算法的旋翼无人机路径规划方法。

背景技术

随着科学技术和经济水平的提高，旋翼无人机已广泛应用于各个领域。除早期军事勘察外，无人机现已广泛应用于影视航拍，地质勘探，农业灌溉，线路检修，环境监测，森林防火等领域。这些应用对无人机的安全性和可靠性提出了更高的要求。无人机如何通过自主飞行实现避障是当今的研究热点。为了实现避障功能，可以将其分为两个部分，第一步是飞行环境检测，第二步是障碍物躲避。

当前市场用于检测障碍物的测距传感器主要有四类，分别是超声波传感器，红外传感器，激光传感器与视觉传感器。本文利用超声波传感器MB1043与激光传感器TFMini进行对比，找出最适于障碍物检测的测距传感器。

飞行环境检测实现之后，便开始进行路径规划。传统路径规划有多种方法，如栅格法，图搜索法，人工势场法等。本文利用Dijkstra方法，搜索出一条可行路径。但由于初始点的限制，它不一定是全局最优路径。因此，还需使用优化算法来实现全局最优路径搜索。现有的智能算法很多，如遗传算法，蚁群优化，蜂群算法，神经网络，粒子群优化算法等。

发明内容

本发明的目的在于克服现有技术的不足，提出了一种基于Dijkstra与改进粒子群算法的旋翼无人机路径规划方法，通过TFMini激光传感器检测环境并构建全局地图，将Dijkstra和改进的粒子群优化算法结合起来，搜寻出利于旋翼无人机飞行的全局最优路径，有效实现无人机的路径规划功能，从而使得旋翼无人机精确避障，安全飞行。仿真结果表明，所提出的旋翼无人机避障策略能够获得理想的全局最优路径，有效实现旋翼无人机的路径规划功能，从而使得旋翼无人机精确避障，安全飞行。

为实现上述目的，本发明所提供的技术方案为：基于Dijkstra与改进粒子群算法的旋翼无人机路径规划方法，包括以下步骤：

1)通过对比超声波传感器及激光传感器，选定合适的测距传感器用于测量无人机周围障碍物的距离与方向，构建飞行环境的全局地图；

2)根据全局地图，利用Dijkstra方法规划多条旋翼无人机飞行的可行路径；

3)对传统的粒子群算法进行改进，采用改进后的粒子群优化算法搜索全局最优路径，并生成最优路径图，具体如下：

考虑到传统粒子群算法中，惯性权值的w设定为常数，为了加速粒子群算法的优化速度，更好地收敛于全局最优解，在改进的粒子群算法中，将惯性权值w分别设定了线性衰减函数以及非线性衰减函数：

经典粒子群优化算法的惯性权值：

w＝C

式中，C为常数；

改进后粒子群优化算法的惯性权值：

w＝k*θⁱ

式中，k为比例系数，θ为粒子迭代基数，i为粒子迭代次数；

使用改进的粒子群优化算法在所得的路径树中搜寻全局最优路径为：

v_id(t+1)＝w*v_id(t)+c₁r₁(p_id(t)-x_id(t))+c₂r₂(p_gd(t)-x_id(t))

x_id(t+1)＝v_id(t+1)+x_id(t)

式中，v_id(t)是粒子i在第t次迭代时速度矢量的d维向量，x_id(t)是粒子i在第t次迭代时位置矢量的d维向量，p_id(t)是粒子i在第t次迭代时最优位置向量的d维向量，p_gd(t)是整个种群的最优位置，c₁和c₂是用于调整粒子学习步长的加速常数，r₁和r₂是在0到1之间均匀分布的随机数，以增加搜索的随机性，w是惯性权值，用来调整算法的搜索范围。

在步骤1)中，将超声波传感器MB1043与激光传感器TFMini作对比，选定使用激光传感器TFMini测量障碍物的方向和距离，对旋翼无人机所在的全局环境进行障碍物感知，建立旋翼无人机飞行环境的全局地图。

在步骤2)中，结合激光传感器感知所构建的飞行环境全局地图，将Dijkstra方法应用于可行路径的搜索，规划多条旋翼无人机飞行的可行路径，包括以下步骤：

2.1)考虑旋翼无人机的作用半径，在建立四旋翼无人机的动力学模型的同时，障碍物也被建模为圆形，设置无人机与障碍物之间的安全距离；

2.2)在全局地图上建立旋翼无人机的飞行起点及飞行终点；

2.3)计算所有点与起点之间的距离，采用Dijkstra方法求解单源最短路径问题，最后得到可行的路径树。

本发明与现有技术相比，具有如下优点与有益效果：

1、通过对比分析各种类型的测距传感器，最终选定激光传感器TFMini，测距精度高，价格适中，性价比高。

2、本方法将Dijkstra方法与粒子群算法相融合，在生成可行路径树之后，运用粒子群算法筛选出其最优路径。算法收敛速度快，实用性强。

3、本方法将粒子群算法进行改进，并通过实验分析，成功证明了改进粒子群算法能获得更优的全局解。

4、本方法在消费级无人机路径规划应用中具有广阔的前景，算法参数少，结构简单，路径搜索速度快，适应性强。

附图说明

图1为本发明逻辑流程示意图。

图2为激光传感器测距性能图。

图3为飞行环境的全局地图。

图4为Dijkstra方法运用时的边界条件示意图。

图5为改进粒子群算法流程图。

图6是粒子群优化算法改进前后最优路径生成图。

图7是算法适应度函数曲线图。

具体实施方式

下面结合具体实施例对本发明作进一步说明。

如图1至图7所示，本实施例所提供的基于Dijkstra与改进粒子群算法的旋翼无人机路径规划方法，使用了四旋翼无人机、超声波传感器和激光传感器等设备，其包括以下步骤：

1)通过对比超声波传感器及激光传感器，选定合适的测距传感器用于测量无人机周围障碍物的距离与方向，构建飞行环境的全局地图，其中，所述超声波传感器和激光传感器分别为MB1043和TFMini，对所述传感器的进行性能参数具体对比如表1所示：

表1两种传感器性能参数

其次，分别对两种传感器实际测距性能进行了测试。实验结果表明，超声传感器MB1043和激光传感器TFMini的有效测量距离分别为3.6m和6.5m以上。同时，超声波传感器在测距过程中不稳定，容易丢失信号。而基于飞行时间测距法(TOF)原理的激光传感器TFMini不会产生类似的问题。基于上述原因，选定TFMini传感器测量障碍物的方向和距离。

实验平台是一个四旋翼无人机，前侧装有三个TFMini传感器，并利用第三方软件Mission Planner地面站可对其飞行状态进行观察。如图2即获取自Mission Planner地面站的激光传感器实际性能图，它显示激光传感器正在探测障碍物。传感器测量的数据与实际数据相同，这意味着激光传感器TFMini能够准确地测量障碍物的距离和方向。根据激光传感器测量所得的障碍物距离与方向，对飞行环境的全局地图进行建模。

2)结合激光传感器感知所构建的飞行环境全局地图，将Dijkstra方法应用于可行路径的搜索，规划多条旋翼无人机飞行的可行路径，包括以下步骤：

2.1)考虑旋翼无人机的作用半径，在建立四旋翼无人机的动力学模型的同时，障碍物也被建模为圆形，设置无人机与障碍物之间的安全距离；

2.2)在全局地图上建立旋翼无人机的飞行起点S及飞行终点D；

2.3)计算所有点与起点之间的距离，采用Dijkstra方法求解单源最短路径问题，最后得到可行的路径树。所述Dijkstra方法应用于有效路径的搜索，具体方法解释如下：首先，除了起点S和终点D外，环境中还有一些随机分布的点，连接这些点的线贯穿整个环境。因此，有必要计算出所有点与起点S之间的距离。当连接点j和起点S经过圆时，这说明不能直接连接到起点S，距离是无穷大的。如果点j和起点S之间的线不接触圆，则需要更新最新的距离d_j。

在计算完每个点和起点S之间的距离后，最短距离d_k已知，因此点k成为一个新的起点。计算每个点与点k之间的距离是相同的方式。如果距离d_k+dist[k,l]＜d_l，则距离d_l需要更新至最新的距离，否则距离d_l保持不变。接着，将起点定义为第二短距离点，按上述方式重复计算，直到更新完所有点到起点的距离为止。

其中j,k是建模环境中随机分布的点，d_j，d_k是点j，k与起点之间的距离，d_l为弦距，dist[k,l]是点k与点l间的直线距离。

在应用Dijkstra方法搜索路径时，确定无人机是否遇到障碍物是非常关键的。当无人机撞上障碍物时，路径也是无用的，即使它是最短的路线。因此，定义碰撞条件是障碍检测的核心。如图4所示，存在无人机与障碍物碰撞的两种情况。如果任一个线段的端点在圆内，路径明显不可行。在这种情况下，需要测量圆心到各个线段的端点的距离。如果从圆的中心到端点的距离小于圆的半径，则路径是不合适的。这种情况可以用下列方程表示：

其中(x_k,y_k)是直线的端点坐标，(X_n,Y_n)是圆的中心坐标r_n是圆的半径。如果线段的端点在圆外，但线段已经经过圆，这与另一种情况相对应，此时两个方面需要讨论。一方面，需要计算弦距d_l和d_m，很明显，这两个弦距都小于相应的半径，但对于圆m，只有线段的延长线通过圆。另一方面，为了区分线段是通过圆圈还是线段的延长线通过圆，必须计算垂足。如果垂足是在线段的端点之间，它意味着路径是无用的，否则它就是一条可行的路径。在第二种情况下，若线段满足下列三个条件，则该路径是不合适的。

式中(x_i,y_i)，(x_j,y_j)分别为两端端点的坐标值，X_l和Y_l为圆l的中心坐标值，A＝y_j-y_i，B＝-(x_j-x_i)，C＝y_i*(x_j-x_i)-x_i*(y_j-y_i)。

3)通过分析各种最优化方法，最终选定使用粒子群优化算法在所得的路径树中搜寻全局最优路径：

v_id(t+1)＝w*v_id(t)+c₁r₁(p_id(t)-x_id(t))+c₂r₂(p_gd(t)-x_id(t))

x_id(t+1)＝v_id(t+1)+x_id(t)

式中，v_id(t)是粒子i在第t次迭代时速度矢量的d维向量，x_id(t)是粒子i在第t次迭代时位置矢量的d维向量，p_id(t)是粒子i在第t次迭代时最优位置向量的d维向量，p_gd(t)是整个种群的最优位置，c₁和c₂是用于调整粒子学习步长的加速常数，r₁和r₂是在0到1之间均匀分布的随机数，以增加搜索的随机性，w是惯性权值，用来调整算法的搜索范围。

考虑到传统粒子群算法中，惯性权值的w设定为常数。为了加速粒子群算法的优化速度，更好地收敛于全局最优解，在改进的粒子群算法中，将惯性权值w分别设定了线性衰减函数以及非线性衰减函数：

经典粒子群优化算法的惯性权值：

w＝C

式中，C为常数。

改进后粒子群优化算法的惯性权值：

w＝k*θⁱ

式中，k为比例系数，θ为粒子迭代基数，i为粒子迭代次数。

粒子群优化算法是一种进化算法。通过模拟鸟类觅食行为，得到最优解t。优化问题可以在搜索空间中找到。在改进的粒子群算法的初始化过程中，每个粒子都有一个初始状态来确定其速度与位置。在搜索过程中，每个粒子将根据粒子本身的历史最佳位置p_id和整个大众的历史最佳位置p_gd更新其速度。对于粒子，有速度范围限制和位置范围限制，以确保粒子总是在解空间内搜索。然而，在标准形式下，惯性权值w是算法收敛性较弱的常数。在收敛的早期阶段，往往期望惯性权值w更大，以加快收敛速度。在收敛的后期，特别是当粒子位于最优解附近时对，越小的w可以得到一个更好的最优解。总之，惯性权值w需要构造为一个衰减函数，以寻找更好的全局路径。

采用粒子群优化公式，分别代入改进前后的惯性权值，其中w₁设为常数，w₂设为线性衰减函数，w₃设为非线性衰减函数。通过仿真实验，发现经典的粒子群算法(w₁)与改进的粒子群算法(w₂,w₃)均能生成一条满足飞行条件的路径。其中，经典的粒子群算法生成的路径与改进的粒子群算法生成的路径差异明显。

从适应度曲线可以看出，当惯性权值w取非线性衰减函数时，适应度函数的值是最小的。这表明了改进的粒子群算法能生成一条优于传统粒子群算法的路径，这也证明了本发明的实用性与合理性。

综上所述，在采用以上方案后，本发明为无人机在复杂环境的飞行提供了了新的障碍规避方法，实现Dijkstra方法与粒子群优化算法相融合，在生成的多条可行路径中快速筛选出一条最优的路径，并对传统的粒子群优化算法进行了改进，遍寻出全局最优路径，具有实际推广价值，值得推广。

以上所述实施例只为本发明之较佳实施例，并非以此限制本发明的实施范围，故凡依本发明之形状、原理所作的变化，均应涵盖在本发明的保护范围。

Claims (3)

1.基于Dijkstra与改进粒子群算法的旋翼无人机路径规划方法，其特征在于，包括以下步骤：

1)通过对比超声波传感器及激光传感器，选定合适的测距传感器用于测量无人机周围障碍物的距离与方向，构建飞行环境的全局地图；

2)根据全局地图，利用Dijkstra方法规划多条旋翼无人机飞行的可行路径；

3)对传统的粒子群算法进行改进，采用改进后的粒子群优化算法搜索全局最优路径，并生成最优路径图，具体如下：

考虑到传统粒子群算法中，惯性权值的w设定为常数，为了加速粒子群算法的优化速度，更好地收敛于全局最优解，在改进的粒子群算法中，将惯性权值w分别设定了线性衰减函数以及非线性衰减函数：

经典粒子群优化算法的惯性权值：

w＝C

式中，C为常数；

改进后粒子群优化算法的惯性权值：

w＝k*θⁱ

式中，k为比例系数，θ为粒子迭代基数，i为粒子迭代次数；

使用改进的粒子群优化算法在所得的路径树中搜寻全局最优路径为：

v_id(t+1)＝w*v_id(t)+c₁r₁(p_id(t)-x_id(t))+c₂r₂(p_gd(t)-x_id(t))

x_id(t+1)＝v_id(t+1)+x_id(t)

式中，v_id(t)是粒子i在第t次迭代时速度矢量的d维向量，x_id(t)是粒子i在第t次迭代时位置矢量的d维向量，p_id(t)是粒子i在第t次迭代时最优位置向量的d维向量，p_gd(t)是整个种群的最优位置，c₁和c₂是用于调整粒子学习步长的加速常数，r₁和r₂是在0到1之间均匀分布的随机数，以增加搜索的随机性，w是惯性权值，用来调整算法的搜索范围。

2.根据权利要求1所述的基于Dijkstra与改进粒子群算法的旋翼无人机路径规划方法，其特征在于：在步骤1)中，将超声波传感器MB1043与激光传感器作TFMini对比，选定使用激光传感器TFMini测量障碍物的方向和距离，对旋翼无人机所在的全局环境进行障碍物感知，建立旋翼无人机飞行环境的全局地图。

3.根据权利要求1所述的基于Dijkstra与改进粒子群算法的旋翼无人机路径规划方法，其特征在于：在步骤2)中，结合激光传感器感知所构建的飞行环境全局地图，将Dijkstra方法应用于可行路径的搜索，规划多条旋翼无人机飞行的可行路径，包括以下步骤：

2.1)考虑旋翼无人机的作用半径，在建立四旋翼无人机的动力学模型的同时，障碍物也被建模为圆形，设置无人机与障碍物之间的安全距离；

2.2)在全局地图上建立旋翼无人机的飞行起点及飞行终点；

2.3)计算所有点与起点之间的距离，采用Dijkstra方法求解单源最短路径问题，最后得到可行的路径树。