CN107883962A - 一种多旋翼无人机在三维环境下的动态航路规划方法 - Google Patents

Abstract

本发明公开了一种多旋翼无人机在三维环境下的动态航路规划方法，属于无人机航路规划技术领域。该方法包括如下步骤：1、建立多旋翼无人机航路规划空间的赋权三维地图模型；步骤2：根据静态赋权三维地图模型，利用蚁群算法规划全局航路代价最小的三维离线航路；步骤3：在行进过程中实时监测滚动窗中障碍物信息，根据避障策略，利用改进RRT算法规划局部航路，直至到达目标点。本发明所提出的避障策略更适应多旋翼无人机飞行特性，实时性高，工程实现性强。实现了无人机绕过静态威胁源和突发障碍物安全迅速到达目的地。

Description

一种多旋翼无人机在三维环境下的动态航路规划方法

技术领域

本发明公开了一种多旋翼无人机在三维环境下的动态航路规划方法，属于无人机航路规划技术领域。

背景技术

多旋翼无人机可以实现定点悬停、垂直起降、倒飞等，飞行灵活、可控性强、抗干扰能力好，近年来受到研究人员的广泛关注，在军事、民用和科研等方面得到了越来越多的应用。

航路规划是无人机任务规划系统(Mission Planing System)的核心技术之一，它需要考虑导航精度、机动性能、环境约束、能量消耗、飞行时间和障碍物威胁等各种因素，为无人机规划出一条从起始点到目标点的无碰优化航路。在实际执行任务过程中，当无人机在预定航路上实际飞行时，可能会出现各种突发动态障碍物。此时若无人机按照预定路线飞行就会发生碰撞，所以无人机在飞行过程中需要实时获取动态障碍物信息进行局部动态路径规划。

现有的无人机航路规划算法大多是针对已有的二维环境信息进行离线规划，或仅针对突发不可移动障碍物进行避障，不能满足实际需求。因此，迫切需要更为有效、实用的三维动态航路规划方法。

发明内容

为了解决现有的无人机航路规划方法所存在的一些不足，本发明提出了一种多旋翼无人机在三维环境下的动态航路规划方法，结合多旋翼无人机自身飞行特性的局部避障策略，实现三维动态航路规划。

本发明为解决其技术问题采用如下技术方案：

一种多旋翼无人机在三维环境下的动态航路规划方法，包括如下步骤：

步骤1：建立多旋翼无人机航路规划空间的赋权三维地图模型；

步骤2：根据静态赋权三维地图模型，利用蚁群算法规划全局航路代价最小的三维离线航路；

步骤3：在行进过程中实时监测滚动窗中障碍物信息，根据避障策略，利用改进RRT算法规划局部航路，直至到达目标点。

步骤1的具体过程如下：

步骤1.1：用栅格法将曲面飞行区域S划分成由离散航点集合{(x,y,z)|(x,y,z)∈S}所构成的网格图，其中(x，y)表示该航点的水平位置，z表示为离地面高度，相邻航点的连线即备选飞行航路；

步骤1.2：利用静态威胁源信息，建立航路代价函数

H＝{h₁,h₂,h_n}为航点集合，E＝{e₁,e₂,...,e_n}为两两相邻航点组成的可飞航路集合，其中h₁,h₂,h_n表示离散航点，e₁,e₂,e_n表示两两相邻航点组成的可飞航路，用J_i表示第i段航路的总代价，J_i＝k₁J_threat,i+(1-k₁)J_fuel,i，k₁为雷达威胁代价的权重，J_threat,i表示第i段航路雷达威胁代价，J_fuel,i表示第i段航路能源代价。

步骤2的具体过程如下：

初始化蚁群算法各参数后利用蚁群算法规划全局航路代价最小的三维离线航路：设置蚁群算法最大循环次数，蚂蚁个数，信息素重要程度因子，启发函数重要程度因子，信息素的挥发系数；启发函数得到离线飞行序列S＝{s₁,s₂,...,s_k}，s₁,s₂,s_k表示选择的航点。

步骤3的具体过程如下：

步骤3.1：以传感器最大探测范围为半径设置滚动窗口，利用传感器检测在滚动窗口内是否有移动的障碍物，若没有检测到障碍物，则按照航点序列飞行一个步长并刷新滚动窗口，继续执行步骤3.1；若滚动窗内有障碍物，则利用传感器收集障碍物飞行速度和相对位置信息；

步骤3.2：利用障碍物飞行速度信息，依据避障策略，规划出局部新航路，更新航点序列，根据航点飞行一个步长或暂停一个步长，刷新滚动窗口，重复步骤3.1，直到无人机到达目标点。

本发明的有效益效果如下：

本发明提出一种蚁群算法和快速扩展随机树(RRT)算法相结合的改进混合算法，相比基本蚁群算法寻优精度更高，收敛速度更快，能适应三维环境下的优化问题。所提出的避障策略更适应多旋翼无人机飞行特性，实时性高，工程实现性强。实现了无人机绕过静态威胁源和突发障碍物安全迅速到达目的地。

附图说明

图1为本发明整体实现流程图。

图2为本发明蚁群算法的全局航路规划流程图。

图3(a)为本发明蚁群算法正向路径节点转移选择图；图3(b)为本发明蚁群算法斜上侧路径节点转移选择图；图3(c)本发明蚁群算法斜下侧路径节点转移选择图。

图4为本发明蚁群算法的全局航路规划航路示意图。

图5为本发明直线运动障碍物避障策略流程图。

图6为本发明基于RRT算法的动态避障策略流程图。

图7为本发明随机树起点终点选取示意图。

图8(a)为本发明局部优化前示意图；图8(b)为本发明局部优化后示意图。

具体实施方式

下面结合附图对本发明创造做进一步详细说明。

图1为本发明整体实现流程图

步骤1：建立多旋翼无人机航路规划空间的赋权三维地图模型。

步骤1.1：用栅格法将曲面飞行区域S划分成由离散航点集合{(x,y,z)|(x,y,z)∈S}所构成的网格图。其中(x，y)表示该航点的水平位置，z表示为离地面高度。相邻航点的连线即可飞行航路。

步骤1.2：利用静态威胁源信息，建立航路代价函数。航点集合H＝{h₁,h₂,h_n}，E＝{e₁,e₂,...,e_n}为两两相邻航点组成的可飞航路集合，h₁,h₂,h_n为栅格法划分曲面飞行区域后的离散航点。e₁,e₂,e_n为两两相邻航点组成的可飞航路。用J_i表示第i段航路的总代价，主要考虑无人机的所受静态威胁代价J_threat,i和飞行能源代价J_fuel,i。

(1)无人机受到的静态威胁代价J_threat,i。将每一条航路均分成六段，取 处三个点，将三个点上的威胁代价相加来代替整条边的威胁代价，则航路e_i的静态威胁代价为:

式中，L_i为该段航路的长度，d为威胁点到航路的距离，N为该条航路所受威胁点个数。

(2)飞行能源代价J_fuel,i。无人机的能源消耗只与航路长度有关，所以用L_i直接表示为能源消耗代价。

航路e_i上的航路代价可表示为：J_i＝k₁J_threat,i+(1-k₁)J_fuel,i，k₁为雷达威胁代价的权重。

步骤2：根据静态赋权三维地图模型，利用蚁群算法规划全局航路代价最小的三维离线航路。

以下是蚁群算法解决单机航路规划问题的实现步骤，图2表示其流程图。

(1)在起始航点上随机放置m只蚂蚁，并将蚂蚁的禁忌表清空；

(2)计算状态转移概率，航点处有三种选择方式，本发明图3(a)表示正向路径航点转移选择图，图3(b)表示斜上路径航点转移选择图，图3(c)表示斜下路径航点转移选择图，每只蚂蚁选择概率最大的航点作为下一步，直到m只蚂蚁都找到下一航点；

(3)修改每只路径存储表，即将每只蚂蚁移动到新的航点，将新航点添加进蚂蚁的禁忌表；

(4)若所有蚂蚁均到达目标点，执行(6)，否则继续执行(3)；

(5)计算每只蚂蚁的航路代价，记录本次循环最优航路，进行信息素更新，循环次数N_c＝N_c+1；

(6)当循环次数N_c达到最大值N_cmax时执行下一步，否则跳到第(3)步；

(7)输出本次规划得到的最优航路。

本发明将航程耗能、威胁作为影响航路性能的主要考虑因素，以此设计航路代价函数，代价函数越小的航路为最优航路。假设整个航路由W＝{w₁,w₂,...,w_n}条航路组成，那么航路代价为各航路代价之和，其中：w₁,w₂,...w_n表示选择的各航路。则公式为：

式中，W表示整个航路中航路段的个数。

初始化蚁群算法各参数后利用蚁群算法规划全局航路代价最小的三维离线航路：设置蚁群算法最大循环次数，蚂蚁个数，信息素重要程度因子，启发函数重要程度因子，信息素的挥发系数。注意对启发函数进行改进，使之关联航路代价：取n_ij(t)为时刻t的启发函数。得到离线飞行序列航点S＝{s₁,s₂,...,s_k}，s₁,s₂,s_k表示选择的航点。图4为本发明蚁群算法的全局航路规划航路示意图。

步骤3：在行进过程中利用传感器监测滚动窗中障碍物信息，根据避障策略，利用改进RRT算法规划局部航路，直至到达目标点。

本发明针对移动障碍物作直线运动和运动方向不确定两种情况，本文提出了两种避障策略，具体方法为：

(1)当障碍物作直线运动时，对可能发生的碰撞情况提出了基于多旋翼无人机可悬停性能的避障策略，称为暂停协调策略。图5本发明直线运动障碍物避障策略流程图。

障碍物的运行轨迹和无人机的既定航路无交叉点，无人机继续按照预定航点序列飞行一步长至s₁。

当障碍物的运动轨迹和无人机的既定航路有交叉点时，需要判断障碍物是否会与无人机在交叉点处发生碰撞。此时障碍物和无人机相对于交叉点的距离分别为L_UAV，L_b，计算二者到达交叉点时间间隔，若

则认为移动障碍物和无人机在交叉点处将发生碰撞，其中D为无人机直径，V_UAV为无人机速度大小，V_b为障碍物速度大小。

障碍物的运动轨迹和无人机的既定航路有交叉点，交叉点处不发生碰撞，无人机继续按照预定航点序列飞行一步长至s₁。

障碍物的运动轨迹和无人机的既定航路有交叉点，交叉点处发生碰撞，无人机航点序列更新为{s₁,s₁,s₂,...,s_k}，即让无人机暂停一个步长时间错过碰撞时间。

(2)当障碍物的运动方向不确定时，本发明将移动障碍物膨胀成静态障碍物，提出了基于快速扩展随机树(RRT)的动态避障策略。图6为基于RRT算法的动态避障策略流程图。

具体方法是：将被无人机探测到的障碍物膨胀，由于无人机每个步长时间T就刷新一次滚动窗口，该障碍物在时间T内可到达最远距离是，可到达的可能区域为以当前所处位置为圆心，为半径的圆形区域。判断该区域是否影响无人机的航路。当该区域会影响无人机之后航路，则无人机利用RRT局部航路规划算法规划出新的局部航路。

使用指导性RRT算法来实现局部航路规划。具体步骤如下：

Step1：算法初始化，设置随机树的出发起点q_init，任务终点q_goal，概率参数p_g；

Step2：此刻随机树生长到任务终点q_near位置，判断是否到达任务终点，若||q_goal-q_near||＜ε，ε为一个极小值，认为到达终点，进行step7，否则继续进行Step3；Step3：生成随机数p∈[0,1]，若p＜p_g，则跳到step4，否则跳到step5；

Step4：将最终目标点位置作为q_rand，在向量方向上生长一个步长ε，计算出候选新航点q_new，跳到step6；

Step5：在任务环境中随机生成航点q_rand，在向量方向上生长一步长ε，得到新航点q_new；

Step6：判断新航点q_new与当前航点q_near间是否存在雷达和障碍物威胁，若没有威胁表示新航点q_new可以加入随机树。找到现有随机树离新航点q_new最近的点连接新航点q_new，随机树成功行进一步，跳到step2。若威胁影响这一步长则重新转向step3；

Step7：在随机树中反向搜索寻找从起始q_init到目标q_goal的航路。

本发明在使用RRT算法生成随机树进行当前航点局部航路规划时，考虑以下几个方面：

①随机树起点与终点的选取

图7为本发明随机树起点终点选取示意图，当圆形膨胀区域影响无人机航点序列{s₁,s₂,...,s_k}，假设航点{s_i,s_i-1,...,s_j}受障碍物威胁，选取航点s_i-1为起点q_init，选取s_j+1为终点q_goal。利用指导性RRT算法规划出从起点到终点的路径后加入航点，更新航点序列为{s₁,...,s_i-1,s_new,1,s_new,2,...,s_new,m,s_j+1,s₃,...,s_k}，s_new,1，s_new,2，s_new,m表示生成的第1、2、m个新航点。

②局部死循环的优化

当无人机在飞行时遇到静态威胁和动态障碍物同时存在于航点序列周围，无人机往往无法规划出航路，且会大大增加算法循环次数，降低计算效率。针对这类情况，无人机需要暂停等待直至能在规定时间内规划出可行航路。

③航点序列的简单优化

指导性RRT算法具有较高实时性，但因为障碍物运动的不确定性和RRT算法的随机性，航点序列往往不是最优。基于此，本发明提出引入障碍物方向预判，调整RRT搜索方向并去除冗余航点，得到较优航路。图8(a)和图8(b)分别为局部优化前和局部优化后示意图。

由仿真结果可知，本文提出的算法在简单和复杂的任务环境下均表现良好，能在极短时间内为各架无人机规划出了满足各约束条件的最优路径，具有较强的鲁棒性。算法的效率随着问题规模的扩大略微降低，总体上仍满足安全性能要求。

Claims (4)

1.一种多旋翼无人机在三维环境下的动态航路规划方法，其特征在于，包括如下步骤：

步骤1：建立多旋翼无人机航路规划空间的赋权三维地图模型；

步骤2：根据静态赋权三维地图模型，利用蚁群算法规划全局航路代价最小的三维离线航路；

步骤3：在行进过程中实时监测滚动窗中障碍物信息，根据避障策略，利用改进RRT算法规划局部航路，直至到达目标点。

2.根据权利要求1所述的一种多旋翼无人机在三维环境下的动态航路规划方法，其特征在于，步骤1的具体过程如下：

步骤1.1：用栅格法将曲面飞行区域S划分成由离散航点集合{(x,y,z)|(x,y,z)∈S}所构成的网格图，其中(x，y)表示该航点的水平位置，z表示为离地面高度，相邻航点的连线即备选飞行航路；

步骤1.2：利用静态威胁源信息，建立航路代价函数

H＝{h₁,h₂,h_n}为航点集合，E＝{e₁,e₂,...,e_n}为两两相邻航点组成的飞航路集合，其中h₁,h₂,h_n表示离散航点，e₁,e₂,e_n表示两两相邻航点组成的飞航路，用J_i表示第i段航路的总代价，J_i＝k₁J_threat,i+(1-k₁)J_fuel,i，k₁为雷达威胁代价的权重，J_threat,i表示第i段航路雷达威胁代价，J_fuel,i表示第i段航路能源代价。

3.根据权利要求1所述的一种多旋翼无人机在三维环境下的动态航路规划方法，其特征在于，步骤2的具体过程如下：

初始化蚁群算法各参数后利用蚁群算法规划全局航路代价最小的三维离线航路：设置蚁群算法最大循环次数，蚂蚁个数，信息素重要程度因子，启发函数重要程度因子，信息素的挥发系数；启发函数得到离线飞行序列S＝{s₁,s₂,...,s_k}，s₁,s₂,s_k表示选择的航点。

4.根据权利要求1所述的一种多旋翼无人机在三维环境下的动态航路规划方法，其特征在于，步骤3的具体过程如下：

步骤3.1：以传感器最大探测范围为半径设置滚动窗口，利用传感器检测在滚动窗口内是否有移动的障碍物，若没有检测到障碍物，则按照航点序列飞行一个步长并刷新滚动窗口，继续执行步骤3.1；若滚动窗内有障碍物，则利用传感器收集障碍物飞行速度和相对位置信息；

步骤3.2：利用障碍物飞行速度信息，依据避障策略，规划出局部新航路，更新航点序列，根据航点飞行一个步长或暂停一个步长，刷新滚动窗口，重复步骤3.1，直到无人机到达目标点。