CN114237302B - 一种基于滚动时域的三维实时rrt*航路规划方法 - Google Patents

Abstract

本发明公开了一种基于滚动时域的三维实时RRT*航路规划方法，属于飞行器任务规划技术领域。本发明针对RRT*算法随机采样优化效率低且无效随机点会成倍增加扫描时间，此外依据在线规划算法要求飞行器尽可能远离障碍物的特点，提出利用人工势场的方向作为影响因子引导RRT*节点生长，并采用有偏随机样本增加随机点的利用率，基于滚动时域实时优化且更新当前位置，最后用节点去除法减少非必要的扫描过程，以实现在线三维航路规划。

Description

一种基于滚动时域的三维实时RRT*航路规划方法

技术领域

本发明属于飞行器任务规划技术领域，尤其涉及一种基于滚动时域的三维实时RRT*方法。

背景技术

在线航路规划要在满足无人机飞行限制的前提下避开障碍危险，并考虑实时性、可行性等需求。传统实时规划A*和D*算法在需要的计算量较大，特别是在三维空间。人工势场算法简单易于实现，但容易陷入最小值和目标区域不可达等问题。群智能算法可以获得全局最优解，但计算量大收敛时间长不适用于要求规划时间短的问题。快速随机搜索树(RRT)是一种速度快、实时性好且具有良好的扩展性，可以在障碍物复杂的情况下快速寻找到航路的一种算法，其采用能逐步迅速缩短随机状态与期望状态点的特殊增量方式能很好的解决带障碍物和微分约束的问题。

但由于它随机点的随机性也造成了一定的优化效率低，以及重复计算的问题。RRT*算法优化航路的唯一办法是增加节点数，然而随着节点数的增加，每新增一个节点的扫描过程成比例增加。并且由于节点选择的随机性，增加节点只有极小一部分会对航路规划有帮助。此外动态环境下由于障碍物的不确定性，带来更加严重的挑战。首先在已规划好的路线上检测到障碍物时需要在极短的时间内规划好新的航路，传统RRT*无法实现实施规划的目的。考虑到无人机需要一定的避障空间，规划航路需要与障碍物保持一定的距离尽量远离障碍物。膨胀障碍物的方式虽然可以一定程度达到远离障碍物的效果，但不同的区域适应不同的膨胀参数。

发明内容

针对上述缺陷，本发明提供了一种基于滚动时域的三维实时RRT*方法，提出利用人工势场的方向作为影响因子引导RRT*节点生长，采用有偏随机样本增加随机点的利用率，基于滚动时域实时优化且更新当前位置，最后用节点去除法减少非必要的扫描过程，达到实时优化的目的以实现无人机航路在线规划。

一种基于滚动时域的三维实时RRT*方法，包括以下步骤，

步骤1，初始化起始点、目标点、三维环境空间和RRT*参数；

步骤2，选取起始点与目标点连线上固定距离的点作为偏置点，在偏置点附近进行采样，得到若干个随机点；

步骤3，确定距离随机点最近的节点，基于该节点利用人工势场法引导其生成新节点；

步骤4，连接新节点与该距离随机点最近的节点，对连线进行碰撞检测，判断各连线是否满足障碍物约束，若判断为是则重新布线更新树，否则返回步骤2；

步骤5，滚动时域内选择距离目标点最近点作为端点，基于该端点回溯寻找到当前轨迹并确定飞行器下一时刻位置节点，以更新飞行器当前位置；

步骤6，删除树结构中飞行器过去位置节点以及连接在过去位置节点的树枝；

步骤7，判断下一时刻位置节点与目标点是否可以直接连接，如若满足则航路规划结束，若不满足则返回步骤2。

作为优选，在步骤2中，由于只需要考虑局部区域的优化航路，随机样本的平均值、搜索半径都会对其结果产生影响。选择飞行器下一步位置点与目标点连线上固定距离的点作为偏置点，基于偏置点在局部区域内生成有偏差样本：

R_bx ²+R_by ²+γ×R_bz ²≤a²

其中，a为随机样本的半径，γ为y轴比例因子，σ为方差，x,y,z分别为随机数，[R_bxR_by R_bz]^T为从均值为零的高斯分布中获得随机值的变化范围，下标Nx，N,y，Nz表示随机点坐标，下标bia表示偏置点。

作为优选，在步骤3中为了加强对周围环境的感知以及增加随机点的指导性，用人工势场引导RRT*节点生长。用目标点与障碍物信息构建势场，得以有效避开障碍物向目标点靠近。具体方式为步骤3.1，基于有偏差样本生成随机搜索树及随机点Z_rand，扫描随机搜索树上所有点与随机点的距离，寻找与其距离最近的点Z_near；步骤3.2，判断节点Z_near指向随机点Z_rand方向与势场方向的角度大小θ，当θ＞π时舍弃此随机点，因为向势场低的方向走是不必要的，此时重新生成新随机点；否则将Z_near作为父节点生成新点Z_new；步骤3.3，进行碰撞检测，判断Z_new与Z_near的连线是否与障碍物碰撞，若出现碰撞则返回步骤3.1，否则加入树结构中，并进入步骤4重新布线。

作为优选，随机点除受随机方向和采样步长决定以外还受到引力与斥力的影响：表达式如下：

式中，F_orce是飞行器收到的合力，ε、φ分别表示随机分量因子和势场分量因子，τ＝θ/π表示新点受影响的程度与θ有关。显然，新的点是由随机分量和势场分量两部分共同决定的。为了节点距离障碍物较近时主要是受斥力影响使得向障碍物生长步长减小，而离障碍物较远时主要受引力影响，加快向目标点生长的速度。

作为优选，在步骤3.3中，碰撞检测为空间随机生成一个随机点Z_rand，扫描随机树上所有点与随机点的距离，寻找最近距离的点Z_near，在沿朝向随机点的方向，生成一个新的点Z_new距离Z_near固定步长。判断新点与Z_near的连线是否与障碍物碰撞，若有则重新生成随机点Z_rand。

作为优选，当满足障碍物约束时，判断新点Z_new作为父节点的代价是否低于现父节点Z_near，选择代价最小的父节点，之后依据代价再选择附近节点能否选择新节点作为父节点。

作为优选，代价为从初始点到达此节点的路径总长。

作为优选，步骤5中在滚动时域内循环判断随机点是否达到指定数量，若判断为是则认为达到一定时域，跳出循环，更新飞行器当前位置。在此步骤中考虑调整顶点集中的节点数量实现实时算法。影响RRT*算法时间的主要因素是顶点集中的节点数和寻优区域中的障碍物复杂度，但工作空间中的障碍物是算法运行过程中无法改变的元素。

作为优选，步骤6中删除节点是指消除连接到前一航路点节点的节点集。节点移除方法是指消除连接到前一航路点节点的节点集。接到前一个航路点的树不再影响航路，因此这些节点在算法过程中仍然存在只会增加计算时间。另外可能会有新生成的节点连接先前的航路点而导致随机树在无用的区域扩展，因此已走过的航路点也需被移除。树结构将两个节点之间的关系表示为子节点和父节点，树中每个节点最多有一个父节点。为了移除连接到前一个航路点的节点，首先在顶点集中找到父节点为此点的所有节点。接着将前一个航路点从顶点集中移除，父索引保存在临时存储器中。在下一个过程中，所有具有前一航路点索引的节点都在顶点集中找到并被移除。节点移除方法重复此过程，直至没有具有上一个父索引的节点。

作为优选，若下一时刻位置节点距离目标点在一定范围内且通过碰撞检测，则判断能直接连接。

本发明的有益效果是：

本发明引入利用人工势场的方向作为影响因子引导RRT*节点生长，采用有偏随机样本增加随机点的利用率，基于滚动时域实时优化且更新当前位置，最后用节点去除法减少非必要的扫描过程。加入人工势场的一方面可以更加快速的收敛，另一方面能够更好的远离障碍物。采用有偏随机样本增加随机点的引导性增加节点利用率，且优化过程在后退地平线区域中顺序执行，滚动时域内快速寻找下一时刻较优位置，由于考虑的节点更少可以减少计算负荷。

附图说明

为了更清楚地说明本发明实施例中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍。

图1是本发明一个实施例的基于滚动时域的三维实时RRT*算法的基本流程图；

图2是本发明一个实施例的节点移除示意图；

图3是本发明一个实施例的RRT、Goal-bias RRT和APF-RRT仿真对比图；

图4是本发明一个实施例的RRT*三维航路仿真结果；

图5是本发明一个实施例的基于滚动时域优化RRT*三维航路仿真结果。

具体实施方式

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

本发明实施例提供一种基于滚动时域的三维实时RRT*方法，如图1所示，主要目的是研究在线算法实时生成可行航路，因此算法时间是实现优化的前提条件。此外代价大小也是航路可行与否的重要保证，本文采用航路长度来计算航路成本。仿真主要从这两个方面以评估算法的优化性能，考虑到算法具备随机性进行重复实验取均值作为结果进行对比。不同方法的仿真数据对比如下表：

表一

如图1所示，为本发明的算法流程图，仿真实验按照下面的步骤进行。

步骤1，初始化起始点、目标点、RRT*参数设置以及环境信息，RRT*参数包括扩展步长：(节点与节点之间距离)、重新布线范围、采样点个数以及有偏采样点范围，环境信息为三维地形。

步骤2，以当前节点的树根节点为起始点，在起始点与目标连线固定距离d处建立偏置点，在偏置点附近采样，得到若干个随机点并建立有偏差样本。有偏差样本为随机点在偏置点附近范围内以正态分布波动的样本：

R_bx ²+R_by ²+γ×R_bz ²≤a²

其中，[R_bx R_by R_bz]^T为从均值为零的高斯分布中获得随机值，a为随机样本的半径，γ为y轴比例因子，可以降低高度变化，σ为方差，x,y,z分别为随机数，[R_bx R_by R_bz]^T为从均值为零的高斯分布中获得随机值的变化范围，下标Nx，N,y，Nz表示随机点坐标，下标bia表示偏置点。

步骤3，为了加强对周围环境的感知以及增加随机点的指导性，用目标点与障碍物信息构建势场，得以有效避开障碍物向目标点靠近。在有偏差样本范围内即以a为半径的球形范围内建立随机搜索树,具体包括：

步骤3.1，基于有偏差样本生成随机点Z_rand，扫描随机树上所有点与随机点Z_rand的距离，寻找最近距离的点Z_near。

步骤3.2，依据人工势场引导节点生长，具体方式为：判断节点Z_near指向随机点Z_rand方向与势场方向的角度大小θ，当θ＞π时舍弃此随机点，因为向势场低的方向走是不必要的，此时重新生成新随机点；否则将Z_near作为父节点生成新点Z_new。新点Z_new除受随机方向和采样步长决定以外还受到引力与斥力的影响，由下式表示：

式中，F_orce为飞行器收到的引力与斥力的合力，ε、φ分别表示随机分量因子和势场分量因子，τ＝θ/π表示新点受影响的程度与θ有关。显然，新点是由随机分量和势场分量两部分共同决定的。

F_orce为飞行器收到的合力，F_orce＝F_att+F_rep，其中F_att为新点受到的引力函数，F_rep为新点受到的斥力函数。当节点距离障碍物较近时主要受斥力影响，使得节点向障碍物生长的步长减小；而当节点距离障碍物较远时主要受引力影响，使得节点向目标点生长的速度加快。其中，引力函数和斥力函数分别表达为：

ρ≤ρ₀

其中，Z_near＝(x_near,y_near,z_near)、Z_goal＝(x_goal,y_goal,z_goal)分别为距离随机点最近的点和目标点的坐标，ρ是障碍物离Z_near的距离，η_att、η_rep分别表示引力和斥力常数，k是形状系数，ρ₀为障碍物的影响半径。

因为到达目标点附近可以直接与目标点相连不会发生过冲现象，所以不需要考虑距离目标点近时引力过大的问题，进而可以简化引力函数得到上述F_att。上述斥力表达式F_rep用更加清晰的方式将斥力分配更合理，当Z_near与障碍物距离靠近零时斥力接近η_rep，当Z_near与障碍物距离靠近ρ₀时，斥力接近零。

步骤4，判断新点Z_new与Z_near的连线是否与障碍物碰撞，若有则返回步骤2，否则进行布线。具体步骤为：首先在现有的随机树上寻找距随机点一定距离的点的集合，判断新节点Z_new到各集合内点的连线是否满足障碍物约束，即集合内的点与Z_near的连线不与障碍物碰撞。如果满足障碍物检测，再判断新节点选择其作为父节点的代价是否低于现父节点。选择代价最小的父节点，之后依据代价再选择附近节点能否选择新节点作为父节点。

增加父节点重选的步骤是为了避免不必要的节点导致的绕远路的现象。新节点的可以在指定范围内重新选择父节点，新节点的指定范围内节点也可以重新判断是否采用新节点作为其父节点，这样增加了航路优化的可能，以较少的计算量改变了树的结构，大大增加了节点的利用率。

步骤5，滚动时域内寻找离目标点最近的点作为端点，回溯当前航路，将当前航路下一时刻位置更新作为当前位置，将此时位置赋值给过去航路集。

步骤6，删除树中当前节点以及连接在当前节点的树枝。即使接到前一个航路点的树不再影响航路，依然可能会有新生成的节点连接先前的航路点而导致随机树在无用的区域扩展。如图2所示，为节点移除示意图。为了移除连接到前一个航路点的节点，首先在顶点集中找到父节点为此点的所有节点。接着将前一个航路点从顶点集中移除，父索引保存在临时存储器中。在下一个过程中，所有具有前一航路点索引的节点都在顶点集中找到并被移除。节点移除方法重复此过程，直至没有具有上一个父索引的节点。

步骤7，判断当前位置与目标点能否直接连接，具体包括距离目标点一定范围内且通过碰撞检测，若判定成功则规划结束否则返回步骤2。

如图3所示，相对于RRT和Goal-bias RRT，本文将人工势场算法(APF)与快速扩展随机树算法(RRT)融合的APF-RRT算法，采样点更少速度更快且离障碍物距离更远。

如图4至图5所示，基于滚动优化RRT*航路规划的平均时间为RRT*的44.96％，这是因为节点移除法使得每增加一个节点的扫描时间要比原本少，且在偏置点的偏差样本以及人工势场的引导也使得随机点的增加绕开障碍物的能力，减少了与障碍物发生碰撞的无效节点数。基于滚动优化RRT*航路规划的平均代价为RRT*的75.74％，这是由于在偏置点的引导下，一次采样过程更加注重局部的优化，大大增加了节点的利用率。此外，由于本文的算法是逐点优化，平均每步规划时间只需要0.0856s，在优化效果增加的基础上完全能满足实时性的要求，加上远离障碍物的特性算法在动态环境下也将表现出一定的优异性。

最后应说明的是：以上所述仅为本发明的优选实施例而已，并不用于限制本发明，尽管参照前述实施例对本发明进行了详细的说明，对于本领域的技术人员来说，其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (7)

1.一种基于滚动时域的三维实时RRT*航路规划方法，其特征在于，包括以下步骤：

步骤1，初始化当前起始点、目标点和RRT*参数；

步骤2，选取起始点与目标点连线上固定距离的点作为偏置点，在偏置点附近进行采样，得到若干个随机点，基于偏置点在局部区域内生成偏差样本表示为：

R_bx ²+R_by ²+γ×R_bz ²≤a²

其中，a为随机样本的半径，γ为y轴比例因子，σ为方差，x,y,z分别为随机数，[R_bx R_byR_bz]^T为从均值为零的高斯分布中获得随机值的变化范围，下标N x，Ny，Nz表示随机点坐标，下标bia表示偏置点；

步骤3，确定距离随机点最近的节点，基于该节点利用人工势场法引导其生成新节点，新点Znew除受随机方向和采样步长决定以外，还受到引力与斥力的影响，表达式如下：

式中，F_orce是飞行器收到的引力与斥力的合力，ε、φ分别表示随机分量因子和势场分量因子，τ＝θ/π表示新点受影响的程度与θ有关；

步骤4，连接新节点与该距离随机点最近的节点，对连线进行碰撞检测，判断各连线是否满足障碍物约束，若判断为是则重新布线更新树，否则返回步骤2；

步骤5，滚动时域内选择距离目标点最近点作为端点，基于该端点回溯寻找到当前轨迹并确定飞行器下一时刻位置节点，以更新飞行器当前位置；

步骤6，删除树结构中飞行器过去位置节点以及连接在过去位置节点的树枝，即消除连接到前一航路点节点的节点集；

步骤7，判断下一时刻位置节点与目标点是否可以直接连接，如若满足则航路规划结束，若不满足则返回步骤2。

2.根据权利要求1所述的一种基于滚动时域的三维实时RRT*航路规划方法，其特征在于，所述步骤3包括：

步骤3.1，基于有偏差样本生成随机搜索树及随机点Z_rand，扫描随机搜索树上所有点与随机点的距离，寻找与其距离最近的点Znear；

步骤3.2，判断节点Znear指向随机点Zrand方向与势场方向的角度大小θ，当θ＞π时重新生成新随机点；否则将Znear作为父节点生成新点Znew。

3.根据权利要求2所述的一种基于滚动时域的三维实时RRT*航路规划方法，其特征在于，所述步骤4中的碰撞检测方法为：在所述随机搜索树上寻找距离随机点Z_rand一定距离的点的集合，判断Z_near到集合内各点的连线是否满足障碍物约束。

4.根据权利要求3所述的一种基于滚动时域的三维实时RRT*航路规划方法，其特征在于，当满足障碍物约束时，判断新点Z_new作为父节点的代价是否低于现父节点Z_near，选择代价最小的父节点，之后依据代价再选择附近节点能否选择新节点作为父节点。

5.根据权利要求4所述的一种基于滚动时域的三维实时RRT*航路规划方法，其特征在于，所述代价为从初始点到达此节点的路径总长。

6.根据权利要求1或5所述的一种基于滚动时域的三维实时RRT*航路规划方法，其特征在于，步骤5中在滚动时域内循环判断随机点是否达到指定数量，若判断为是则认为达到一定时域，跳出循环，更新飞行器当前位置。

7.根据权利要求1所述的一种基于滚动时域的三维实时RRT*航路规划方法，其特征在于，若下一时刻位置节点距离目标点在一定范围内且通过碰撞检测，则判断能直接连接。