CN112179351B - 一种基于预规划路径优化rrt算法的三维避障航迹规划方法 - Google Patents

Abstract

本发明涉及一种基于预规划路径优化RRT算法的三维避障航迹规划方法，该方法包括以下步骤：步骤1：在障碍物膨胀规则和起点到终点连线与障碍物相交规则下生成预规划路径；步骤2：针对预规划路径从其中通过提取得到RRT搜索树的随机状态点；步骤3：RRT搜索树由起点沿着预规划路径上的随机状态点进行扩展，直至终点，生成避障轨迹。与现有技术相比，本发明提前生成的预规划路径可强化RRT扩展树搜索的连贯性，减少障碍物碰撞检测的时间；预规划路径上生成的连续质点作为搜索树在扩展中的随机状态点，可使搜索树的扩展具有方向性，进而减少避障搜索的时间，提高无人机避障规划的效率。

Description

一种基于预规划路径优化RRT算法的三维避障航迹规划方法

技术领域

本发明涉及一种航迹规划方法，尤其是涉及一种基于预规划路径优化RRT算法的三维避障航迹规划方法。

背景技术

目前很多基于改进RRT算法的无人机的避障航迹规划方法都是基于二维平面实现的，但是对于环境建模难度大、计算量大等问题，还未能在三维空间内利用RRT得到令人满意的避障规划路径。此外，随着维数的增加，如果仍采用传统的环境建模方法，RRT算法的计算量会大量增加，实时性会大幅度降低。

传统的RRT算法的随机树扩展的盲目性大，缺乏导向性，缺乏衡量航迹优劣的评价标准，在狭窄的通道寻路能力较差和生成的避障航迹不一定最优等。而现有的大部分改进方法一般是基于二维平面进行实现，而现实中无人机在飞行时面对的都是三维或N维的场景。而新型RRT算法在解决三维环境中无人机避障航迹规划问题，并且在与其他改进的RRT相比减少了无人机避障规划时间，并提高了避障路径搜索的效率。

发明内容

本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种基于预规划路径优化RRT算法的三维避障航迹规划方法，属于智能领域，是一种三维避障航迹优化方法，涉及到RRT算法改进方法、算法流程图以及算法仿真图和仿真相关数据表，本发明提出的基于预规划路径优化RRT算法的避障航迹规划算法可以运用在三维场景中

本发明的目的可以通过以下技术方案来实现：

一种基于预规划路径优化RRT算法的三维避障航迹规划方法，该方法包括以下步骤：

步骤1：在障碍物膨胀规则和起点到终点连线与障碍物相交规则下生成预规划路径；

步骤2：针对预规划路径从其中通过提取得到RRT搜索树的随机状态点；

步骤3：RRT搜索树由起点沿着预规划路径上的随机状态点进行扩展，直至终点，生成避障轨迹。

进一步地，所述的步骤1包括以下分步骤：

步骤101：运用障碍物膨胀法建立以便于计算机识别的三维环境中的障碍物模型；

步骤102：初始化膨胀处理后的障碍物模型的球心、球的半径、无人机的起点和终点；

步骤103：连接起点和终点组成连线，并计算球心到该连线的垂足坐标；

步骤104：基于球心到该连线的垂足坐标计算各球心到连线的距离；

步骤105：基于各球心到连线的距离和球的半径检查连线是否与障碍物相交；

步骤106：当检查结果为直线与球不相交或相切时，该连线即为预规划路径；

步骤107；当检查结果为直线与球相交时，确定预规划路径的新起点，并返回步骤103。

进一步地，所述的步骤2包括以下分步骤：

步骤201：在预规划路径为检查结果为直线与球不相交或相切的情况下所生成的情况下，将预规划路径视为由连续的质点组成，并从该预规划路径中通过每单位长度提取来得到RRT搜索树的随机状态点；

步骤202：在预规划路径为检查结果为直线与球相交的情况下所生成的情况下，将预规划路径视为由多个分段函数组成，并从每段分段函数中通过每单位长度提取来得到RRT搜索树的随机状态点。

进一步地，所述的步骤103中的球心到该连线的垂足坐标，其计算公式为：

式中，x_ni、y_ni和z_ni为垂足坐标的x、y和z轴坐标，x_g、y_g和z_g为无人机的终点的x、y和z轴坐标，x₀、y₀和z₀为无人机的起点的x、y和z轴坐标，x_oi、y_oi和z_oi为膨胀处理后的障碍物模型的球心的x、y和z轴坐标。

进一步地，所述的步骤104中的各球心到连线的距离，其计算公式为：

式中，L_i为各球心到连线的距离。

进一步地，所述的步骤107中的预规划路径的新起点，其计算公式为：

式中，X_i为预规划路径的新起点坐标，(x_i,y_i,z_i)为预规划路径的新起点坐标的具体x、y和z轴坐标值，X_oi为膨胀处理后的障碍物模型的球心坐标，R_oi为膨胀处理后的障碍物模型的球的半径，

为膨胀处理后的障碍物模型的球心到连线的垂足坐标的向量。

进一步地，其特征在于，所述的步骤201包括以下分步骤：

步骤2011：根据已知的起点和终点，由三维空间两点式求出预规划路径的直线方程；

步骤2012：通过控制x、y或z轴按扩展树步长的比例取值以确定随机动态点的选取及坐标。

进一步地，所述的步骤2011中的预规划路径的直线方程，其描述公式为：

式中，X、Y和Z分别表示x、y或z轴。

进一步地，所述的步骤2012中的随机动态点的坐标，其描述公式为：

式中，X_rand(i)为随机动态点的坐标，x_rand(i)为随机动态点的x轴坐标值且x_rand(i)＝x₀+i×AL，A和i均为常数，L为RRT搜索树的步长。

进一步地，所述的步骤101具体包括：利用最小的长方体完全包围障碍物，以长方体的对角线长度的设定倍数为直径做障碍物的外接球。

与现有技术相比，本发明具有以下优点：

(1)本发明方法基于预规划路径优化RRT算法的无人机三维避障规划算法，该算法首先在障碍物膨胀规则和起点到终点连线与障碍物相交规则下生成预规划路径，此预规划路径可强化RRT扩展树搜索的连贯性，减少障碍物碰撞检测的时间。

(2)本发明方法将预规划路径看作由连续的质点构成，连续质点可作为搜索树在扩展中的随机状态点，可使搜索树的扩展具有方向性，进而减少避障搜索的时间，提高无人机避障规划的效率，使得生成避障路径的时间更优。

(3)本发明方法针对传统的RRT算法的随机树扩展的盲目性大，缺乏导向性，现有RRT算法改进大部分不足以及限制在二维平面进行实现等缺点，提出一种预规划路径优化RRT算法的三维避障航迹规划的改进方法，以便于减少了无人机避障规划时间，并提高了避障路径搜索的效率，并且更加接近无人机真实的飞行环境。

附图说明

图1为本发明方法实施例中的障碍物的膨胀图；

图2为本发明方法实施例中的球体切割面示意图；

图3为本发明方法实施例中的预规划路径分段处理示意图；

图4为原本的RRT算法过程示意图；

图5为本发明改进的人工势场法示意图；

图6为本发明改进的RRT算法过程示意图。

具体实施方式

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

实施例

本发明一种基于预规划路径优化RRT算法的三维避障航迹规划方法，其具体步骤如下：

步骤一：运用障碍物膨胀法建立以便于计算机识别的三维环境中的障碍物模型，具体方法为：利用最小的长方体完全包围障碍物，以长方体的对角线长度的1.2倍为直径做障碍物的外接球，示意图如图1。

步骤二：初始化膨胀处理后的障碍物的球心及球的半径为，无人机的起点和重点。

步骤三：连接起点和终点组成连线。

步骤四：计算球心到连线的垂足坐标；

连线方向向量：

过球心做连线的垂线：

由

可知：

垂足在连线上，由

与

共线可得：

将(2)式代入(1)式，式中只有一个未知数k，可化解k为：

将(3)式代入(2)式即可求出垂足的坐标，

式中，x_ni、y_ni和z_ni为垂足坐标的x、y和z轴坐标，x_g、y_g和z_g为无人机的终点的x、y和z轴坐标，x₀、y₀和z₀为无人机的起点的x、y和z轴坐标，x_oi、y_oi和z_oi为膨胀处理后的障碍物模型的球心的x、y和z轴坐标。

步骤五：计算各球心到连线的距离，

根据垂足坐标可知向量

其模长即为各球心到连线的距离：

式中，L_i为各球心到连线的距离。

步骤六：检查连线是否与障碍物相交

只要判断球心到连线的距离与球的半径的关系即可：

1、当L_i>R_oi时，直线与球不相交，则可判断此连线为预规划路线；

2、当L_i＝R_oi时，直线与球相切，则可判断此连线为预规划路线，且切点为预规划路线与球面的交点；

3、当L_i<R_oi时，直线与球相交，继续下一步。

步骤七：确定预规划路径新起点

设垂线的单位方向向量为

则：

延长垂线交球面于点X_i，可求得交点坐标为：

以交点为新起点，跳转至步骤三。

步骤八：预规划路径生成

在障碍物膨胀规则和相交规则下，预规划路径的生成有以下俩种情况：

(1)当L_i>＝R_oi时，起点与终点的连线与障碍物的膨胀球体不相交或与球面相切，可判断此连线为求得的预规划路径。此预规划路径可看作是由连续的质点组成，而RRT算法的随机状态点可从连续的质点中通过每单位长度提取来得到，具体实现方法如下：

1)根据已知的起点和终点，由三维空间俩点式求出预规划路径的直线方程如下式：

式中，X、Y和Z分别表示x、y或z轴。

2)通过控制x、y或z轴按一定的扩展树步长的比例取值来确定随机状态点的选取及坐标，即：

式中，X_rand(i)为随机动态点的坐标，x_rand(i)为随机动态点的x轴坐标值且x_rand(i)＝x₀+i×AL，A和i均为常数，A一般大于等于2，L为RRT搜索树的步长，人为设置，i表示随机状态点数。

每个球面的交点内取最大数量的随机状态点数。这样在三维空间中，RRT算法在预规划路径上等量划分的随机状态点的指引下，使搜索树更具有方向性，减免了很多不必要的扩展节点，进而提高避障规划的效率。

(2)当L_i<R_oi时，起点与终点的连线与障碍物的膨胀球体相交，则此连线不符合预规划路径的要求，得重选。由一条直线与直线外的一点能构成且只能构成一个平面可知，连线和球心可确定一个平面，且平面切割球体形成有界切面，此平面示意如图2所示。

(3)以交点为新起点连接终点，通过相交规则判断新连线是否可以作为预规划路径，循环往复，直至最后连接终点时的连线与障碍物没有相交(L_i<R_oi)，则可判断预规划路径生成。

(4)此种情况生成的预规划路径可分成一段一段来处理，可根据每条线段的俩个端点坐标来确定分段函数，每个分段函数可根据(1)中的处理方式来确定RRT搜索树的随机状态点，其示意图如图3。

步骤九：RRT搜索树由起点沿着预规划路径上的随机状态点进行扩展，直至终点，生成避障航迹，其算法流程如图4所示。

具体仿真结果

本发明改进的RRT算法与原RRT算法和改进的人工势场法在同一仿真环境中做实验(设置步长L＝5，比例系数A＝2，无人机从起点(0,0,0)匀速飞行到终点(100,100,100)，障碍物的位置坐标以及半径如表1所示)，生成的仿真图如图5和6所示，并分别采样了各算法的运行时间、搜索树的扩展节点总数、生成路径节点数和生成路径的长度四种数据，如表1所示。通过仿真图和数据对比来验证本发明改进算法的有效性。

表1障碍物相关信息表

从表2可以很清楚的看出，在设定的相同静态障碍物的三维环境下，本文改进的算法较原RRT算法的平均节点个数生成总量减少将近一半，生成的避障路径节点占比明显增大，无效节点明显减少，避障路径趋于平缓，算法复杂度降低，效率提高；较原RRT算法和改进的人工势场法的避障路径搜索时间要短，生成的避障路径平均长度也要短；由此可见，基于预规划路径优化RRT算法有效的减少了无人机避障规划时间，并提高了避障路径搜索的效率。

表2三种路径规划法的特性数据对比

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到各种等效的修改或替换，这些修改或替换都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以权利要求的保护范围为准。

Claims (7)

1.一种基于预规划路径优化RRT算法的三维避障航迹规划方法，其特征在于，该方法包括以下步骤：

步骤1：在障碍物膨胀规则和起点到终点连线与障碍物相交规则下生成预规划路径；

步骤2：针对预规划路径从其中通过提取得到RRT搜索树的随机状态点；

步骤3：RRT搜索树由起点沿着预规划路径上的随机状态点进行扩展，直至终点，生成避障轨迹；

所述的步骤1包括以下分步骤：

步骤101：运用障碍物膨胀法建立以便于计算机识别的三维环境中的障碍物模型，具体方法为：利用最小的长方体完全包围障碍物，以长方体的对角线长度的1.2倍为直径做障碍物的外接球；

步骤102：初始化膨胀处理后的障碍物模型的球心、球的半径、无人机的起点和终点；

步骤103：连接起点和终点组成连线，并计算球心到该连线的垂足坐标；

步骤104：基于球心到该连线的垂足坐标计算各球心到连线的距离；

步骤105：基于各球心到连线的距离和球的半径检查连线是否与障碍物相交；

步骤106：当检查结果为直线与球不相交或相切时，该连线即为预规划路径；

步骤107；当检查结果为直线与球相交时，确定预规划路径的新起点，并返回步骤103；

所述的步骤107中的预规划路径的新起点，其计算公式为：

式中，X_i为预规划路径的新起点坐标，(x_i,y_i,z_i)为预规划路径的新起点坐标的具体x、y和z轴坐标值，X_oi为膨胀处理后的障碍物模型的球心坐标，R_oi为膨胀处理后的障碍物模型的球的半径，

为膨胀处理后的障碍物模型的球心到连线的垂足坐标的向量。

2.根据权利要求1所述的一种基于预规划路径优化RRT算法的三维避障航迹规划方法，其特征在于，所述的步骤2包括以下分步骤：

步骤201：在预规划路径为检查结果为直线与球不相交或相切的情况下所生成的情况下，将预规划路径视为由连续的质点组成，并从该预规划路径中通过每单位长度提取来得到RRT搜索树的随机状态点；

步骤202：在预规划路径为检查结果为直线与球相交的情况下所生成的情况下，将预规划路径视为由多个分段函数组成，并从每段分段函数中通过每单位长度提取来得到RRT搜索树的随机状态点。

3.根据权利要求1所述的一种基于预规划路径优化RRT算法的三维避障航迹规划方法，其特征在于，所述的步骤103中的球心到该连线的垂足坐标，其计算公式为：

式中，x_ni、y_ni和z_ni为垂足坐标的x、y和z轴坐标，x_g、y_g和z_g为无人机的终点的x、y和z轴坐标，x₀、y₀和z₀为无人机的起点的x、y和z轴坐标，x_oi、y_oi和z_oi为膨胀处理后的障碍物模型的球心的x、y和z轴坐标。

4.根据权利要求1所述的一种基于预规划路径优化RRT算法的三维避障航迹规划方法，其特征在于，所述的步骤104中的各球心到连线的距离，其计算公式为：

式中，L_i为各球心到连线的距离。

5.根据权利要求2所述的一种基于预规划路径优化RRT算法的三维避障航迹规划方法，其特征在于，所述的步骤201包括以下分步骤：

步骤2011：根据已知的起点和终点，由三维空间两点式求出预规划路径的直线方程；

步骤2012：通过控制x、y或z轴按扩展树步长的比例取值以确定随机动态点的选取及坐标。

6.根据权利要求5所述的一种基于预规划路径优化RRT算法的三维避障航迹规划方法，其特征在于，所述的步骤2011中的预规划路径的直线方程，其描述公式为：

式中，X、Y和Z分别表示x、y或z轴。

7.根据权利要求5所述的一种基于预规划路径优化RRT算法的三维避障航迹规划方法，其特征在于，所述的步骤2012中的随机动态点的坐标，其描述公式为：

式中，X_rand(i)为随机动态点的坐标，x_rand(i)为随机动态点的x轴坐标值且x_rand(i)＝x₀+i×AL，A和i均为常数，L为RRT搜索树的步长。

Images