CN112799420A - 一种基于多传感器无人机的实时航迹规划的方法 - Google Patents

Abstract

本发明所述的一种基于多传感器无人机的实时航迹规划的方法，首先，建立无人机航迹规划的数学模型和全局航迹；然后，根据收集实时信息与是否遇到障碍物进行改进快速随机扩展树将生成的随机采样点进行概率引导；最后，将新算法应用于无人机的航迹规划问题中。最终，降低了无人机航行的代价，提高了无人机在复制环境中能够实时的规划出最优的路径。

Description

一种基于多传感器无人机的实时航迹规划的方法

技术领域

本发明涉及无人机航迹规划技术领域，具体是涉及一种基于多传感器无人机的实时航迹规划的方法。

背景技术

近几年来无人机的应用越来越受到广大科研工作者的关注。在大的森林火灾中，消防人员无法靠近时，无人机在搭载多传感器进行一些有效的救援，在这方面的应用越来越受到大家的关注。因为无人机自身存在着续航时间短和搭载的无线传感器受到网络传输距离的限制，所以无人机的航行要求必须最优，因此，对于无人机航迹的最优性和实时性提出了要求。无人机的航迹可以分为局部航迹规划和全局航迹规划。全局的航迹规划是在已知环境地图的情况下进行的规划，这是要提前知道全局的环境信息的情况下。而局部航迹规划，主要是指无人机可以根据周围的环境进行实时的航迹规划，更加具有时效性。静态威胁的二维路径规划,比较容易陷入局部最优，航迹规划的时间长，实时性差等缺点，并且不能在动态的环境下，遇到障碍物时，及时有效的规划出新的航迹出来，随机快速扩展树算法可以解决这个问题。

传统的随机快速扩展树(RRT)方法的航迹规划以状态空间中规划起始点为根节点，通过随机采样逐渐增加叶节点的方式生成随机扩展树。当随机树的叶节点包含了目标点或者目标区域的点时，随机树的扩展停止，便可在随机树中找到一条以根节点组成的从起始点到目标点的路径。随机搜索产生扩展方向点q-rand，这虽然有利于算法搜索未知区域，但这种盲目的搜索方式会产生很大的冗余，浪费规划时间，降低算法的实时性。对于在基于多传感器的无人机的应用中，我们要有效的规划出飞行路径，避免一些资源的浪费，充分的利用好有限的资源。

发明内容

为解决上述技术问题，本发明提供了一种基于多传感器无人机的实时航迹规划的方法，其针对无线传感器网络飞行距离受到限制与无人机蓄电时间的有限性，提出运用改进的RRT算法，在节点扩展时，改进随机采样点最近的根节点的选取策略，可以有效地应用有限的资源规划出最优的飞行路径。

本发明所述的一种基于多传感器无人机的实时航迹规划的方法，，其步骤包括：

S1、建立无人机航迹规划的数学模型，输入起点，规划全局轨迹；

S2、判断无人机是否到达终点，若到达终点，则结束飞行任务；若未到达终点，则进行S3；

S3、无人机收集实时信息，检测前方航迹上是否有障碍物，如否，则进行S2，如有，则更新信息；

S4、采用改进RRT算法采样新节点，生成局部航迹，避让障碍物。

进一步的，采用改进RRT算法采样结点，方法为：

S4-1、搜索节点，初始化树及相应环境信息，导入周围环境的信息，整体规划，判断是否到达终点，能到达则结束飞行任务，否则进行S4-2；

S4-2、产生随机采样点q-rand，对随机采样点进行了多重随机采样策略，采用评价函数对于随机树扩展方向点进行最优选择；

S4-3、添加随机采样点q-rand周围环境的信息，在q-rand的free的环境中以概率θ选取最近节点q-near，其中0<θ<1；

S4-4、在随机采样点q-rand和最近节点q-near连线中选取步长step，根据步长扩展新节点q-new，若新节点q-new满足约束条件，则将新的节点加入到随机树中，进行S4-5，若不满足约束调节，则进行S4-2；

S4-5将满足约束条件的新节点q-new加入树中，进行判断是否到达终点，如果到达则结束飞行任务，否则进行S4-2。

进一步的，采用评价函数对最终采样节点进行选择的方法是：

从规划空间中产生一组随机采样点q-randi(i∈{1，2，…，n})，引入了距离和方向两个要素，即：

f(i)＝ω1*d+ω2*a

其中d表示采样点到起始点q-init和目标点q-goal连线的垂直距离，a表示起始点q-init到目标点q-goal与采样点到目标点q-goal的夹角，ω1、ω2分别是距离和角度的权重系数，f(i)表示评价函数；由于距离和方向的量纲不统一，对该评价函数中的d和a进行归一化处理，即计算i个采样点的距离和角度值，然后求出其平均值

和

即：

求出均值后，再对每一个采样点对应的距离和角度进行归一化处理，因此评价函数变为:f(i)′＝ω1*d′+ω2*a′

其中：

其中，a′表示采样点的角度，d′表示采样点的距离，f(i)′表示采样点的评价函数。根据采样点距离和角度的贡献，选择评价函数值最小的点作为最终采样节点。

进一步的，所述约束条件为最大拐弯角约束、最小直飞距离约束和最低飞行高度约束。

5.根据权利要求4所述的一种基于多传感器无人机的实时航迹规划的方法，其特征在于，最大拐弯角约束是，如果第i个航迹点q-near的水平拐角为θ_near，记最大拐弯角为θ_max，则θ_near≤θ_max；假设，在三维航迹中有3个连续的航迹点q_near-1,q_near,q_new(near＝2,3……),则

和

形成向量的水平投影分别为(x_near-x_near-1,y_near-y_near-1)和(x_new-x_near,y_new-y_near)，此时，θ_near可以表示为：

进一步的，最小直飞距离约束是，无人机的航迹段，由多个两个航迹点段组成，假设无人机有N个航迹段，假设无人机最小的航迹段直飞距离为l_min，设q-near，q-new两点之间的直飞距离为l_n，则||l_n||≥l_min。

进一步的，最低飞行高度约束是，假设在生成备用航迹点q-new时的高度是H_new，那么需要满足的条件是：H_min≤H_new≤H_max。

本发明所述的有益效果为：无人机动态航迹规划发现障碍物时，能快速有效的重新规划出最优的航迹，对快速随机扩展树算法中，随机采样点进行概率引导从而得出比较精确的采样点；改进RRT算法不需要建立空间信息模型，且具有算法简洁、运算量小，实时性强的优点，在一定的程度上提高的无人机航迹的实时性与最优性。

附图说明

为了使本发明的内容更容易被清楚地理解，下面根据具体实施例并结合附图，对本发明作进一步详细的说明。

图1无人机实时航迹规划流程图。

图2改进数据采样点的评价函数示意图。

图3改进后的快速随机扩展树(RRT)算法流程图。

具体实施方式

如图1所示，本发明所述的一种基于多传感器无人机的实时航迹规划的方法，其步骤包括：

S1、建立无人机航迹规划的数学模型，输入起点，规划全局轨迹；

S2、判断无人机是否到达终点，若到达终点，则结束飞行任务；若未到达终点，则进行S3；

S3、无人机收集实时信息，检测前方航迹上是否有障碍物，如否，则进行S2，如有，则更新信息；

S4、改进RRT算法采样新节点，生成局部航迹，避让障碍物。

如图3所示，采用改进RRT算法采样节点，步骤为：

S4-1、搜索节点，初始化树及相应环境信息，导入周围环境的信息，整体规划，判断是否到达终点，能到达则结束飞行任务，否则进行S4-2；

S4-2、产生随机采样点q-rand，对随机采样点进行了多重随机采样策略，采用评价函数对于随机树扩展方向点进行最优选择；

S4-3、添加随机采样点q-rand周围环境的信息，在q-rand的free的环境中以概率θ选取最近节点q-near，其中0<θ<1；

S4-4、在随机采样点q-rand和最近节点q-near连线中选取步长step，根据步长扩展新节点q-new，若新节点q-new满足约束条件，则将新的节点加入到随机树中，进行S4-5，若不满足约束调节，则进行S4-2；

S4-5将满足约束条件的新节点q-new加入树中，进行判断是否到达终点，如果到达则结束飞行任务，否则进行S4-2。

无人机航迹规划是生成一条由起始点q-init到目标点q-goal无碰撞的较优航迹。我们知道在航迹规划时扩展树中的节点均由q-rand扩展而来，q-rand节点选取是否合适将直接影响最终生成航迹的质量。因此，为了得到更高质量的q-rand节点，本发明引入多重随机采样策略，来对于q-rand节点进行选择。从规划空间中产生一组随机采样点q-randi(i∈{1，2，…，n}),然后对这一组点中选择最优的q-rand进行扩展。为了得到最优q-rand，则需要有相应的评价函数来对该组节点进行评价。如图2所示，由基础数学知识可得最短航迹一定是起始点q-init和目标点q-goal的连线，因此对q-rand1和q-rand2比较可知选择离最短航迹近的q-rand可缩短剩余航迹长度。因此，本发明在对采样点进行评价时，引入了距离和方向两个要素，即：

f(i)＝ω1*d+ω2*a

如图2所示，其中d表示采样点到q-init和q-goal连线的垂直距离，a表示起始点到目标点与采样点到目标点的夹角，ω1、ω2分别是距离和角度的权重系数。由于距离和方向的量纲不统一，因此还需对该评价函数中的d和a进行归一化处理，即计算i个采样点的距离和角度值，然后求出其平均值

和

即：

求出均值后，再对每一个采样点对应的距离和角度进行归一化处理，因此评价函数变为:f(i)′＝ω1*d′+ω2*a′

其中：

进行处理后，即可根据采样点距离和角度的贡献，选择评价函数值最小的点作为最终采样节点。采用该方法进行采样看似增加了计算量，但由于采样节点质量提升，使得扩展过程中的无效探索次数显著减少，从而提高了算法的整体搜索效率。

在规划无人机飞行航迹的时候，通常是要满足一些基本的条件，才能完整的规划出无人机的航迹。我们知道无人机飞行航迹在二维空间中已经比较完善了，但是在三维的航迹规划中还有很多地方可以改进的。无人机在三维空间中飞航需要满足最大拐弯角约束、航迹段直飞距离最小化约束、最低最高飞行高度的约束等等这些基本条件。其中，最大拐弯角约束是指无人机只能在水平面内小于或等于指定的最大转弯角内转弯，目的是在有限的方位上选取候选顶点，从而选出合适的候选节点；航迹段直飞距离最小化约束是指无人机在改变飞行的姿态之前，按目前的航迹方向飞行的最短航程；最低最高飞行高度约束是无人机的飞行高度不能无限制的过快变化，因此无人机要在指定的高度区域内飞行。

本发明在改进的RRT算法中，采样点的选择都是需要满足这些条件才能规划出无人机的航迹。

其中，最大拐弯角的约束1是，如果第i个航迹点q-near水平拐角为θ_near,记最大拐弯角为θ_max，则θ_near≤θ_max。假设，在三维航迹中有3个连续的航迹点q_near-1,q_near,q_new(near＝2,3……),则

和

形成向量的水平投影分别为(x_near-x_near-1,y_near-y_near-1)和(x_new-x_near,y_new-y_near)，此时，θ_near可以表示为：

无人机航迹段最小直飞距离约束2是，无人机的航迹段，由多个两个航迹点段组成，假设无人机有N个航迹段，假设无人机最小的航迹段直飞距离为l_min,设q-near，q-new两点之间的直飞距离为l_n，则||l_n||≥l_min。

转化最低和最高飞行高度约束3是，无人机在新生成备用航迹点q-new时受到自身性能的约束，存在最大飞行高度，同时飞行的高度也不能过低，以免与地面发生碰撞。因此，飞行高度是有约束的。假设在生成备用航迹点q-new时的高度是H_new，那么需要满足的条件是：H_min≤H_new≤H_max。

本发明在改进快速随机扩展树(RRT)算法中引入概率引导的随机采样一致性算法，二者相结合，增加算法搜索的指向性，提高搜索效率，减少航迹冗余，降低了无人机航行的代价，提高了无人机在复制环境中能够实时的规划出最优的路径。

以上所述仅为本发明的优选方案，并非作为对本发明的进一步限定，凡是利用本发明说明书及附图内容所作的各种等效变化均在本发明的保护范围之内。

Claims (7)

1.一种基于多传感器无人机的实时航迹规划的方法，其步骤包括：

S1、建立无人机航迹规划的数学模型，输入起点，规划全局轨迹；

S2、判断无人机是否到达终点，若到达终点，则结束飞行任务；若未到达终点，则进行S3；

S3、无人机收集实时信息，检测前方航迹上是否有障碍物，如否，则进行S2，如有，则更新信息；

S4、改进RRT算法采样新节点，生成局部航迹，避让障碍物。

2.根据权利要求1所述的一种基于多传感器无人机的实时航迹规划的方法，其特征在于，采用改进RRT算法采样节点，方法为：

S4-1、搜索节点，初始化树及相应环境信息，导入周围环境的信息，整体规划，判断是否到达终点，能到达则结束飞行任务，否则进行S4-2；

S4-2、产生随机采样点q-rand，对随机采样点进行了多重随机采样策略，采用评价函数对于随机树扩展方向点进行最优选择；

S4-3、添加随机采样点q-rand周围环境的信息，在q-rand的free的环境中以概率θ选取最近节点q-near，其中0<θ<1；

S4-4、在随机采样点q-rand和最近节点q-near连线中选取步长step，根据步长扩展新节点q-new，若新节点q-new满足约束条件，则将新的节点加入到随机树中，进行S4-5，若不满足约束调节，则进行S4-2；

S4-5将满足约束条件的新节点q-new加入树中，进行判断是否到达终点，如果到达则结束飞行任务，否则进行S4-2。

3.根据权利要求2所述的一种基于多传感器无人机的实时航迹规划的方法，其特征在于，采用评价函数对最终采样节点进行选择的方法是：

从规划空间中产生一组随机采样点q-randi(i∈{1，2，…，n})，引入了距离和方向两个要素，即：

f(i)＝ω1*d+ω2*a

其中d表示采样点到起始点q-init和目标点q-goal连线的垂直距离，a表示起始点q-init到目标点q-goal与采样点到目标点q-goal的夹角，ω1、ω2分别是距离和角度的权重系数，f(i)表示评价函数；由于距离和方向的量纲不统一，对该评价函数中的a和d进行归一化处理，即计算i个采样点的距离和角度值，然后求出其平均值

即：

求出均值后，再对每一个采样点对应的距离和角度进行归一化处理，因此评价函数变为:f(i)′＝ω1*d′+ω2*a′

其中：

其中，d′为采样点距离，a′为采样点角度，f(i)采样点评价函数。根据采样点距离和角度的贡献，选择评价函数值最小的点作为最终采样节点。

4.根据权利要求2所述的一种基于多传感器无人机的实时航迹规划的方法，其特征在于，所述约束条件为最大拐弯角约束、最小直飞距离约束和最低飞行高度约束。

5.根据权利要求4所述的一种基于多传感器无人机的实时航迹规划的方法，其特征在于，最大拐弯角约束是，如果第i个航迹点q-near，水平拐角为θ_near，记最大拐弯角为θ_max，则θ_near≤；假设，在三维航迹中有3个连续的航迹点q_near-1,q_near,q_new(near＝2,3……),则

和

形成向量的水平投影分别为(x_near-x_near-1,y_near-y_near-1)和(x_new-x_near,y_new-y_near)，此时，θ_near可以表示为：

。

6.根据权利要求4所述的一种基于多传感器无人机的实时航迹规划的方法，其特征在于，最小直飞距离约束是，无人机的航迹段，由多个两个航迹点段组成，假设无人机有N个航迹段，假设无人机最小的航迹段直飞距离为l_min，设q-near，q-new两点之间的直飞距离为l_n，则||l_n||≥l_min。

7.根据权利要求4所述的一种基于多传感器无人机的实时航迹规划的方法，其特征在于，最低飞行高度约束是，假设在生成备用航迹点q-new时的高度是H_new，那么需要满足的条件是：H_min≤H_new≤H_nax。

Images