CN112945254B - 一种基于快速拓展随机树的无人车曲率连续路径规划方法 - Google Patents

Abstract

本发明涉及一种基于快速拓展随机树的无人车曲率连续路径规划方法，属于路径规划技术领域。本发明采用节点列表记录那些可以无碰撞连接的节点，并用栈的思想从列表中逆序取出进行曲率约束检测，从而得到既符合曲率约束，又使路径长度更低的可行路径。本发明方法能极大提高随机树的扩展成功率，此外，由于平滑方案设计中考虑了节点的可扩展性，算法规划的路径在长度上将会随着规划进行逐渐降低。

Description

一种基于快速拓展随机树的无人车曲率连续路径规划方法

技术领域

本发明属于路径规划技术领域，具体涉及一种基于快速扩展随机树的路径规划方法，使得规划路径的曲率连续且满足最大曲率约束，在无人驾驶、机械臂控制和无人机航路规划等领域有广泛的应用。

背景技术

智能机器人为生活生产提供便利和效率使其成为当今社会的热点话题，而运动规划则是机器人运动的关键技术之一，特别是在对无人车技术需求越发激烈的今天，一条可行的无碰撞路径是机器人安全移动的关键。在现有规划技术中，采用图搜索算法可以获取最短路径，但此类算法的规划速度受环境大小影响较大。快速扩展随机树(RRT/rapidlyexploring random tree)算法摒弃了路径最优特性，通过随机采样的方案对环境进行探索，在环境适应性和求解速度上有较大优势，其随机树可被视为空间中已探索的安全区域，当随机树扩展到目标区域时，便可得到可行的安全路径。然而，传统的路径规划算法生成的路径由有序离散点组成，这样的路径很难被轮式无人车跟随，需要增加额外的路径平滑处理过程。

为了与应用相结合，使用曲线理论对直线段路径进行平滑的方式被广泛适用，Dubins曲线通过圆弧对路径的转角进行处理，但该路径在圆弧连接出曲率不连续，无法被car-like车辆匀速跟随；Clothoids曲线缺乏闭环表达形式难以被广泛推广；相对而言，样条曲线和贝塞尔曲线在路径平滑处理方面应用更多，Kwangjin Yang基于贝塞尔曲线理论提出了G²和G¹连续可行规划方案；Mohamed Elbanhawi使用B样条曲线得到C²连续平滑路径；然而，此类算法在规划过程中并未考虑曲率约束，其路径规划和平滑过程分步进行，这样的后处理平滑方案并未考虑平滑过程带来的路径偏移，从而导致平滑后的路径可能与环境中的障碍物发生碰撞，一旦平滑路径不可行，需要进行重规划，这将导致规划时间增加。

发明内容

要解决的技术问题

为了避免传统路径规划算法在平滑后处理过程中带来的碰撞导致重规划，在规划过程中明确考虑平滑问题是一个很好的解决方案，但由于曲率约束检测中的边长限制，传统基于快速扩展随机树的路径规划方法在规划过程中考虑曲率约束的规划效率十分低下。本发明提出一种对任意给定上限曲率的平滑路径规划方案，该方案能在规划过程中明确考虑路径的平滑问题，具备高效的规划性能且在时间充足情况下得到最短的可行路径。

技术方案

一种基于快速拓展随机树的无人车最大曲率约束路径规划方法，其特征在于步骤如下：

步骤1：在无障碍物区域X_free中进行随机采样，得到采样点x_new；

步骤2：在随机树包含的节点列表V中寻找与新节点x_new空间距离最近的节点，并将其记为x_nearest；

步骤3：如果x_nearest和x_new之间存在障碍物，即

则返回步骤1；反之，沿着x_nearest所在支线向随机树的根节点进行逐个检测，找到一个与x_new之间的局部路径安全且其父节点与x_new之间存在障碍物的节点，将其记为x_reachest，其父节点记为Parent(x_reachest)，即满足：{x_reachest-x_new}∈X_free且

同时使用节点列表X_reach记录从x_nearest到x_reachest之间所有检测过的可达节点；

步骤4：找到x_reachest后，利用二分法在x_reachest-Parent(x_reachest)上寻找可以与x_new之间无碰撞的点x_a；然后再次适用二分法在x_a-x_new线段上找到一个点x_b，使其满足{Parent(x_reachest)-x_b}∈X_free，同时使用节点列表X_create记录二分法在线段x_reachest-x_a和x_a-x_b之间创建的所有节点；

步骤5：从节点列表X_create中寻找节点x_parent，该节点能使得由x_new-x_parent-Parent(x_reachest)组成的局部路径满足给定上限曲率K_max的约束，这个判别过程称为曲率约束；如果存在这样的节点x_parent，进入步骤7，否则进入步骤6；

步骤6：从节点列表X_reach中寻找节点x_parent，该节点能使得由x_new-x_parent-Parent(x_parent)组成的局部路径满足给定上限曲率K_max的约束，这个判别过程称为曲率约束；如果存在这样的节点x_parent，进入步骤8，否则返回步骤1；

步骤7：对于随机树G＝(V,E)，增加节点x_parent和x_new，即V＝V∪{x_parent,x_new}；同时增加连接边(x_parent-Parent(x_reachest))和(x_new-x_parent)，即E＝E∪{(x_parent-Parent(x_reachest)),(x_new-x_parent)}；进入步骤9；

步骤8：对于随机树G＝(V,E)，增加节点x_new，即V＝V∪{x_new}；同时增加连接边(x_new-x_parent)，即E＝E∪{(x_new-x_parent)}；进入步骤9；

步骤9：如果新增节点x_new在目标区域，即x_new∈X_goal，当前随机树中存在可行路径，因此，如果需要输出路径，则进入步骤11；否则进入步骤10；

步骤10：在随机树中找到距x_new距离低于R_near的节点，记为x_near，如果局部路径x_near-x_new-x_parent和x_child-x_near-x_new满足曲率约束，则将x_near的父节点修改为x_new，即E＝E/{(x_near-Parent(x_near))}∪{(x_near-x_new)}，其中x_child表示x_near的子节点；返回步骤1；

步骤11：在随机树中，找到存在目标区域X_goal内的节点，按照连接关系，生成由离散节点组成的简单路径，该路径平滑后一定不会发生碰撞。

本发明技术方案更进一步的说：所述的曲率约束具体步骤如下：

步骤1：计算局部路径x₁-x₂-x₃的夹角∠x₁x₂x₃，记为θ；

步骤2：结合给定上限曲率K_max得到约束下的最短边长

步骤3：如果边长x₁-x₂或x₂-x₃小于L_min，则说明该段局部路径x₁-x₂-x₃不满足平滑条件，即不满足曲率K_max的约束，退出曲率约束检测程序，返回不可行；反之，进入步骤4；

步骤4：生成辅助节点

其中u₂₁和u₂₃分别为点x₂指向点x₁和x₃的单位向量；

步骤5：使用公式

和

得到局部路径的平滑路径

和

并判断该路径上是否存在障碍物，若存在障碍物，则说明局部路径x₁-x₂-x₃不满足曲率约束，反之，则说明局部路径x₁-x₂-x₃在平滑后不会与障碍物发生碰撞；其中s为曲线参数，取值范围为[0-1]。

有益效果

本发明提出的路径规划方案可以解决规划算法中随机树扩展效率低下问题，该问题主要由边长限制，因此，本发明采用节点列表记录那些可以无碰撞连接的节点，并用栈的思想从列表中逆序取出进行曲率约束检测，从而得到既符合曲率约束，又使路径长度更低的可行路径。这个方案能极大提高随机树的扩展成功率，此外，由于平滑方案设计中考虑了节点的可扩展性，算法规划的路径在长度上将会随着规划进行逐渐降低。

本发明将三次B样条曲线设计为路径的平滑方案，从而提出一种改进型快速随机树路径规划算法，该算法能一次性得到可行路径，避免了传统分层路径规划算法的后处理过程带来的重规划问题。此外，算法具备渐进最优性，能在充足时间下获得最短的可行路径。

本发明提出的路径规划方案可以一次得符合最大曲率约束的路径，不会因为后处理造成的平滑导致重规划问题；其平滑方案可以使得随机树结构进行局部修改，因此算法具备渐进最优性，即在不考虑规划时间情况下，可以得到最短的可行路径，具有十分重要的工程应用价值。

附图说明

图1是常见随机树结构；

图2是基于三次B样条曲线的局部路径平滑方案；

图3是本发明的规划算法中为新增节点创建父节点的示意图；

图4是本发明的规划算法中随机树局部结构重构方案；

图5是仿真地图环境信息；

图6是带曲率约束的路径规划方案流程图；

图7是带曲率约束的路径规划中创建新节点步骤的流程图；

图8是K_max＝0.36时的初始解；

图9是K_max＝0.36下的路径与时间关系图。

具体实施方式

现结合实施例、附图对本发明作进一步描述：

1、路径平滑方案

如图1所示为常见随机树的结构，可以将其划分为无数个如图2所示的由三个顺向连接的节点构成的局部路径，如其中x₁-x₂-x₃为一个局部路径示例，对于给定的上限曲率K_max，本发明提出以下的平滑方案，该方案也可作为规划过程中的曲率约束。

步骤1：计算局部路径x₁-x₂-x₃的夹角∠x₁x₂x₃，记为θ；

步骤2：结合给定上限曲率K_max得到约束下的最短边长

步骤3：如果边长x₁-x₂或x₂-x₃小于L_min，则说明该段局部路径x₁-x₂-x₃不满足平滑条件，即不满足曲率K_max的约束，退出曲率约束检测程序，返回不可行。反之，进入步骤4。

步骤4：生成辅助节点

其中u₂₁和u₂₃分别为点x₂指向点x₁和x₃的单位向量。

步骤5：使用公式

和

得到局部路径的平滑路径

和

并判断该路径上是否存在障碍物，若存在障碍物，则说明局部路径x₁-x₂-x₃不满足曲率约束，反之，则说明局部路径x₁-x₂-x₃在平滑后不会与障碍物发生碰撞。其中_s为曲线参数，取值范围为[0-1]。

2、路径规划方案

由于曲率约束中的边长限制问题，传统的RRT算法在规划过程中考虑路径的平滑问题可能会导致随机树扩展失败。为了增加随机树扩展的成功率，本发明提出一种名为Stack-RRT*的随机树扩展算法，算法的主要思想在于为新增节点提供一系列的候选节点，该方案可以大大增加随机树扩展的成功率。以下为算法的具体实施步骤：

步骤1：在无障碍物区域X_free中随机进行随机采样，得到采样点x_new；

步骤2：在随机树包含的节点列表V中寻找与新节点x_new空间距离最近的节点，并将其记为x_nearest；

步骤3：如果x_nearest和x_new之间存在障碍物，即

则返回步骤1；反之，沿着x_nearest所在支线向随机树的根节点进行逐个检测，找到一个与x_new之间的局部路径安全且其父节点与x_new之间存在障碍物的节点，如果将其记为x_reachest，其父节点记为Parent(x_reachest)，它们需要满足：{x_reachest-x_new}∈X_free且

同时使用节点列表X_reach记录从x_nearest到x_reachest之间所有检测过的可达节点；

步骤4：如图3所示，找到x_reachest后，利用二分法在x_reachest-Parent(x_reachest)上寻找可以与x_new之间无碰撞的点x_a；然后再次适用二分法在x_a-x_new线段上找到一个点x_b，使其满足{Parent(x_reachest)-x_b}∈X_free，同时使用节点列表X_create记录二分法在线段x_reachest-x_a和x_a-x_b之间创建的所有节点。此过程中二分法的终止条件为被划分区间长度低于给定阈值D_dichotomy。

步骤5：从节点列表X_create中寻找节点x_parent，该节点能使得由x_new-x_parent-Parent(x_reachest)组成的局部路径满足给定上限曲率K_max的约束，这个判别过程称为曲率约束。如果存在这样的节点x_parent，进入步骤7，否则进入步骤6。

步骤6：从节点列表X_reach中寻找节点x_parent，该节点能使得由x_new-x_parent-Parent(x_parent)组成的局部路径满足给定上限曲率K_max的约束，这个判别过程称为曲率约束。如果存在这样的节点x_parent，进入步骤8，否则返回步骤1。

步骤7：对于随机树G＝(V,E)，增加节点x_parent和x_new，即V＝V∪{x_parent,x_new}；同时增加连接边(x_parent-Parent(x_reachest))和(x_new-x_parent)，即E＝E∪{(x_parent-Parent(x_reachest)),(x_new-x_parent)}。进入步骤9。

步骤8：对于随机树G＝(V,E)，增加节点x_new，即V＝V∪{x_new}；同时增加连接边(x_new-x_parent)，即E＝E∪{(x_new-x_parent)}。进入步骤9。

步骤9：如果新增节点x_new在目标区域，即x_new∈X_goal，当前随机树中存在可行路径，因此，如果需要输出路径，则进入步骤11；否则进入步骤10；

步骤10：如图4所示，在随机树中找到距x_new距离低于半径R_near的节点，记为x_near，如果局部路径x_near-x_new-x_parent和x_child-x_near-x_new满足曲率约束，则将x_near的父节点修改为x_new，即E＝E/{(x_near-Parent(x_near))}∪{(x_near-x_new)}，其中x_child表示x_near的子节点。返回步骤1。

步骤11：在随机树中，找到存在目标区域X_goal内的节点，按照连接关系，生成由离散节点组成的简单路径，该路径平滑后一定不会发生碰撞。

实施例1：

采用matlab作为仿真平台，如图5所示地图被用于仿真，其中黑色方块为障碍物X_obs，白色区域为安全区域X_free；起始点x_start＝(30,80)；目标点x_goal＝(180,80)；X_goal是以点x_goal为圆心，半径为8的圆形区域。图6所示为仿真程序的执行逻辑，以下为具体执行步骤：

步骤1：获取算法所需的相关参数，包括：起始点x_start，目标区域X_goal，环境X＝X_obs∪X_free，其中X_obs为障碍物所在区域，X_free为无障碍物区域，因此有X_obs∩X_free＝φ。同时对算法中的参数进行初始化，包括：最长仿真时间T_max＝30s，初始化仿真时间time＝0s，二分法的终止条件阈值D_dichotomy＝1.5，给定上限曲率K_max＝0.36。随机树G＝(V,E)，其中V←{x_start}为随机树的节点列表，E←φ则表示节点之间的扩展关系。

步骤2：判断是否需要停止算法或者继续进行，即当time≥T_max时，进入步骤16；反之，当time<T_max时，进入步骤3。

步骤3：更新仿真时间time；

步骤4：在X_free区域中随机进行随机采样，得到采样点x_new，进入步骤5。

步骤5：在随机树包含的节点列表V中寻找与新节点x_new空间距离最近的节点，并将其记为x_nearest。然后进入步骤6。

步骤6：判别x_nearest到x_new之间的局部路径是否安全，即线段x_nearest-x_new满足{x_nearest-x_new}∈X_free时，则认为局部路径安全，进入步骤7；反之，若

则局部路径上存在障碍物，返回步骤2。在流程图中用CollisionFree函数表示该操作步骤的内容，函数CollisionFree的输入为两个空间坐标，返回值为是或否，分别对应了安全和不安全。

步骤7：如图3所示，沿着x_nearest所在支线向随机树的根节点进行逐个检测，找到一个与x_new之间的局部路径安全且其父节点与x_new之间存在障碍物的节点，如果将其记为x_reachest，其父节点记为Parent(x_reachest)，它们需要满足：{x_reachest-x_new}∈X_free且

同时使用节点列表X_reach记录从x_nearest到x_reachest之间所有检测过的可达节点。

步骤8：如图3所示，找到x_reachest后，利用二分法在x_reachest-Parent(x_reachest)上寻找可以与x_new之间无碰撞的点x_a；然后再次适用二分法在x_a-x_new线段上找到一个点x_b，使其满足{Parent(x_reachest)-x_b}∈X_free，同时使用节点列表X_create记录二分法在线段x_reachest-x_a和x_a-x_b之间创建的所有节点，该步骤的执行逻辑流程如图7所示，其实现步骤如下：

步骤8.1：初始化节点列表X_create←φ，变量x_allow←x_reachest，x_forbid←Parent(x_reachest)；

步骤8.2：如果二分区间x_allow-x_forbid的距离小于D_dichotomy，则进入步骤8.6；反之，进入步骤8.3；

步骤8.3：检测节点x_new与(x_allow+x_forbid)/2之间的路径是否安全，如果安全，则进入步骤8.4；反之进入步骤8.5；

步骤8.4：更新变量x_allow←(x_allow+x_forbid)/2，同时将更新后的节点x_allow加入列表X_create←X_create∪{x_allow}；返回步骤8.2；

步骤8.5：更新变量x_forbid←(x_allow+x_forbid)/2，返回步骤8.2；

步骤8.6：更新变量x_forbid←x_new，进入步骤8.7；

步骤8.7：如果二分区间x_allow-x_forbid的距离小于D_dichotomy，则进入步骤8.11；反之，进入步骤8.8；

步骤8.8：检测节点Parent(x_reachest)与(x_allow+x_forbid)/2之间的路径是否安全，如果安全，则进入步骤8.9；反之进入步骤8.10；

步骤8.9：更新变量x_allow←(x_allow+x_forbid)/2，同时将更新后的节点x_allow加入列表X_create←X_create∪{x_allow}；返回步骤8.8；

步骤8.10：更新变量x_forbid←(x_allow+x_forbid)/2，返回步骤8.8；

步骤8.11：创建节点完成，返回节点列表X_create。

步骤9：从节点列表X_create中寻找节点x_parent，该节点能使得由x_new-x_parent-Parent(x_reachest)组成的局部路径满足给定上限曲率K_max的约束，这个判别过程称为曲率约束。如果存在这样的节点x_parent，进入步骤10；否则，从节点列表X_reach中寻找节点x_parent，该节点能使得由x_new-x_parent-Parent(x_parent)组成的局部路径满足给定上限曲率K_max的约束。如果不存在这样的节点x_parent，则x_parent＝φ。

步骤10：如果x_parent≠φ，则进入步骤11；反之，则返回步骤2。

步骤11：如果x_parent∈X_reach，则进入步骤12；反之进入步骤13。

步骤12：将新增节点x_new添加到随机树G中，添加步骤为：V＝V∪{x_new}，E＝E∪{(x_new-x_parent)}；进入步骤14。

步骤13：将新增节点x_new和x_parent添加到随机树G中，添加步骤为：V＝V∪{x_parent,x_new}，E＝E∪{(x_parent-Parent(x_reachest)),(x_new-x_parent)}；进入步骤14。

步骤14：此步骤分为两种情况，一种为初始解获取，即当检测到新增节点x_new在目标区域，即x_new∈X_goal，进入步骤16；另一种为以规划时间为代价，获取成本更低的路径，在该模式的规划下，程序进入步骤15。

步骤15：如图4所示，在以x_new为圆心，R_near为半径的区域内，寻找已经存在于随机树G的节点x_near，并通过成本大小对比判断是否需要对其父节点进行修改。修改的附加条件为：局部路径x_near-x_new-x_parent和x_child-x_near-x_new满足曲率约束，其中x_child表示x_near的子节点。然后返回步骤2。

步骤16：在当前随机树中，找到存在目标区域X_goal内的节点，按照连接关系，生成由离散节点组成的简单路径，该路径平滑后一定不会发生碰撞。

步骤17：结束程序。

实验结果：

1、初始解

图8为本发明提出的算法在图5所示的仿真环境中的初始规划路径，路径的曲率连续且满足给定的最大曲率约束。

2、收敛性证明

本实施例进行了50次收敛性仿真，每次仿真30秒，并在整秒时记录当前的随机树结构，得到图9所示的规划路径于时间关系图，其中粗黑线为该时刻的平均路径，细黑线分别为该时刻的最长和最短的可行路径。由结果可知，随着规划进行，随机树中包含的可行路径长度越低，因此，在对路径长度要求较高的环境中，该算法可以提供一个较低的可行路径方案，但其路径优化是以时间为代价。

Claims (2)

1.一种基于快速拓展随机树的无人车最大曲率约束路径规划方法，其特征在于步骤如下：

步骤1：在无障碍物区域X_free中进行随机采样，得到采样点x_new；

步骤2：在随机树包含的节点列表V中寻找与新节点x_new空间距离最近的节点，并将其记为x_nearest；

步骤3：如果x_nearest和x_new之间存在障碍物，即

则返回步骤1；反之，沿着x_nearest所在支线向随机树的根节点进行逐个检测，找到一个与x_new之间的局部路径安全且其父节点与x_new之间存在障碍物的节点，将其记为x_reachest，其父节点记为Parent(x_reachest)，即满足：{x_reachest-x_new}∈X_free且

同时使用节点列表X_reach记录从x_nearest到x_reachest之间所有检测过的可达节点；

步骤4：找到x_reachest后，利用二分法在x_reachest-Parent(x_reachest)上寻找可以与x_new之间无碰撞的点x_a；然后再次适用二分法在x_a-x_new线段上找到一个点x_b，使其满足{Parent(x_reachest)-x_b}∈X_free，同时使用节点列表X_create记录二分法在线段x_reachest-x_a和x_a-x_b之间创建的所有节点；

步骤5：从节点列表X_create中寻找节点x_parent，该节点能使得由x_new-x_parent-Parent(x_reachest)组成的局部路径满足给定上限曲率K_max的约束，这个判别过程称为曲率约束；如果存在这样的节点x_parent，进入步骤7，否则进入步骤6；

步骤6：从节点列表X_reach中寻找节点x_parent，该节点能使得由x_new-x_parent-Parent(x_parent)组成的局部路径满足给定上限曲率K_max的约束，这个判别过程称为曲率约束；如果存在这样的节点x_parent，进入步骤8，否则返回步骤1；

步骤7：对于随机树G＝(V,E)，增加节点x_parent和x_new，即V＝V∪{x_parent,x_new}；同时增加连接边(x_parent-Parent(x_reachest))和(x_new-x_parent)，即E＝E∪{(x_parent-Parent(x_reachest)),(x_new-x_parent)}；进入步骤9；

步骤8：对于随机树G＝(V,E)，增加节点x_new，即V＝V∪{x_new}；同时增加连接边(x_new-x_parent)，即E＝E∪{(x_new-x_parent)}；进入步骤9；

步骤9：如果新增节点x_new在目标区域，即x_new∈X_goal，当前随机树中存在可行路径，因此，如果需要输出路径，则进入步骤11；否则进入步骤10；

步骤10：在随机树中找到距x_new距离低于R_near的节点，记为x_near，如果局部路径x_near-x_new-x_parent和x_child-x_near-x_new满足曲率约束，则将x_near的父节点修改为x_new，即E＝E/{(x_near-Parent(x_near))}∪{(x_near-x_new)}，其中x_child表示x_near的子节点；返回步骤1；

步骤11：在随机树中，找到存在目标区域X_goal内的节点，按照连接关系，生成由离散节点组成的简单路径，该路径平滑后一定不会发生碰撞。

2.根据权利要求1所述的一种基于快速拓展随机树的无人车最大曲率约束路径规划方法，其特征在于所述的曲率约束具体步骤如下：

步骤1：计算局部路径x₁-x₂-x₃的夹角∠x₁x₂x₃，记为θ；其中x₁、x₂、x₃代表三个顺向连接构成局部路径的节点；

步骤2：结合给定上限曲率K_max得到约束下的最短边长

步骤3：如果边长x₁-x₂或x₂-x₃小于L_min，则说明该段局部路径x₁-x₂-x₃不满足平滑条件，即不满足曲率K_max的约束，退出曲率约束检测程序，返回不可行；反之，进入步骤4；

步骤4：生成辅助节点

其中u₂₁和u₂₃分别为点x₂指向点x₁和x₃的单位向量；

步骤5：使用公式

和

得到局部路径的平滑路径

和

并判断该路径上是否存在障碍物，若存在障碍物，则说明局部路径x₁-x₂-x₃不满足曲率约束，反之，则说明局部路径x₁-x₂-x₃在平滑后不会与障碍物发生碰撞；其中s为曲线参数，取值范围为[0-1]。

Images