CN113064426A - 一种改进双向快速搜索随机树算法的智能车路径规划方法 - Google Patents

Abstract

本发明公开了一种改进双向快速搜索随机树算法的智能车路径规划方法，包括以下步骤：1)起始点和目标点两棵树相向搜索；2)节点扩展时同时生成两个随机采样点，选择距离目标点较近的采样点为下一扩展节点提供方向；3)结合目标引力思想，根据概率来判断是以目标点还是随机采样点作为扩展节点方向；4)产生动态目标点，两棵随机树分别选择对方生成的最新节点作为目标；5)判断扩展过程中是否发生碰撞，若有则重新生成新的采样点；6)两棵树的最新节点间距小于一定阈值则生成路径；7)将路径结合车辆的非完整约束模型；8)采用B样条曲线对生成路径进行优化。与现有技术相比，本发明具有路径更短，实时性更高，考虑非完整约束等优点。

Description

一种改进双向快速搜索随机树算法的智能车路径规划方法

技术领域

本发明属于人工智能领域，涉及智能车路径规划，尤其是涉及一种改进双向快速搜索随机树算法的智能车路径规划方法。

背景技术

智能交通系统的兴起为汽车工业的发展带来了巨大的改变，智能交通系统的发展核心是无人驾驶车辆，而路径规划作为无人驾驶汽车发展研究的关键技术之一，一直以来受到广泛的关注和研究。

近年来基于快速搜索随机树的路径规划方法由于在高维空间中的卓越性能受到了广大学者的青睐。该算法具有概率完备性并且收敛速度较快，可以应用在具有障碍物的未知环境中，并且一定能够找到可行路径等优点。

但也存有一些缺点：1)为了找到可行路径，算法需要不停的进行反复迭代来探索整个未知空间，计算过程中会消耗大量的内存；2)由于算法在状态空间中采用随机采样的方式进行扩展，使得随机树节点的生成没有方向，会导致最终难以生成最优路径；3)由于算法的随机性，会很大程度上计算出较为粗糙弯折的路径，不适合智能车辆直接使用。

发明内容

本发明的目的在于提供一种改进双向快速搜索随机树算法的智能车路径规划方法，以解决现有技术中计算量较大，消耗大量内存，难以产生最优路径，生成的路径比较粗糙弯折不适合车辆直接使用的技术问题。

所述的一种改进双向快速搜索随机树算法的智能车路径规划方法，具体包括以下步骤：

9)起始点和目标点两棵树相向搜索；

10)开始生成新节点，在节点扩展时，在地图上同时生成两个随机采样点，并通过距离函数来选择距离目标点较近的采样点来为下一扩展节点提供方向；

11)结合目标引力思想，根据概率来判断生成下一节点时是以目标点还是上一步新生较近随机采样点作为扩展节点方向；

12)在规划过程中，产生动态目标点，加快规划速度，两棵随机树分别选择对方生成的最新节点作为目标；

13)判断扩展过程中是否与障碍物或者地图边界发生碰撞，若碰撞，则重新生成新的采样点；否则，产生新节点；

14)两棵树的最新节点小于一定阈值，则生成路径；

15)结合车辆的非完整约束模型，提高生成路径的可行性；

16)采用B样条曲线对生成路径进行优化，提高路径的平滑性。

优选的，所述的步骤3)包括：

在进行采样时，采用启发式采样，其随机树在进行扩展时，通过随机概率p₁(1>p₁≥0)来确定目标点作为随机采样点的可能性，设定参照概率p，当p<p₁，选择随机采样点作为最终采样点，当p₁>p，选择目标点作为最终采样点，当目标点作为采样点时，使得随机树的生长具有一定的方向性和收敛性，能够提高随机树的扩展能力。

优选的，所述的步骤3)还包括：计算扩展树新生节点q_new时，基本RRT中计算公式为：

q_new＝q_near+step(q_rand-q_near)/||q_rand-q_near||

其中，step为扩展时随机树的步长，(q_rand-q_near)表示两向量单位化，||q_rand-q_near||表示两点间欧氏距离，q_rand为随机点，q_near为距离随机点最近的一个邻近节点；

加入目标引力思想的新生节点q_new计算公式为：

q_new＝q_near+step₁(q_rand-q_near)/||q_rand-q_near||+step₂(q_goal-q_near)/||q_goal-q_near||

step₁为随机树向随机点方向扩展的步长，step₂为随机树向目标点方向扩展的步长，q_goal为目标点，||q_goal-q_near||为q_goal和q_near间的欧氏距离。

优选的，所述的步骤2)具体包括：

使用采样函数在规划地图上同时生成两个随机采样点，用距离函数来计算上一新生节点与两随机采样点之间的距离，选择距离目标近的随机采样点来为下一扩展节点提供方向，加快缩短路径规划距离。

优选的，所述的步骤4)具体包括：

产生动态目标点，在一棵随机树产生新的节点时，另一棵树的下一次扩展将以此新生节点作为目标点进行生长。

优选的，所述的步骤5)具体包括：

当原始节点向着最终采样点扩展时，判断是否会与障碍物或者地图边界发生碰撞，如果碰撞则舍弃此最终采样点，回到步骤2)；不碰撞则依照设定步长来扩展随机树长度到最终采样点。

优选的，所述的步骤6)具体包括：

当两棵随机树相向不断进行扩展，每生成新的节点，要计算出两棵树新生节点之间的距离，在给定的阈值下，若新生节点之间的距离小于等于阈值，则直接连接两棵随机树，生成初始路径，否则依照步骤2)至5)继续生成新节点。

优选的，所述的步骤7)具体包括：

设智能车在任务空间中状态变量为(x,y,θ,v,φ)，其中(x,y)为无人车后轮轴的中心在系统坐标系下的坐标，θ为移动机器人前进方向与x轴的夹角，v为移动机器人的速度，φ为无人车前进方向与前车轮之间的夹角，因为无人车受到非完整性约束，故车轮和地面是点接触，假设接触点只有纯滚动不产生相对的滑动，则智能车的运动方程为：

根据运动方程，智能车所受到的约束方程为：

其中，智能车的控制变量为加速度u₀和车轮的角速度u₁，智能车的最小转弯半径的计算式如下：

式中K_max为最大曲率，φ_max为车辆前轮最大转向角，L为轴距，R_max为最小转弯半径；

约束条件还包括：

优选的，所述的步骤8)具体包括：

B样条曲线具有灵活性、连续性和直线保持性等优点。准均匀B样条曲线会经过端点，用此法来拟合路径，以生成符合车辆运动学约束的可执行轨迹；B样条曲线的基函数如下：

端点位置的算式如下：

曲线函数如下：

优选的，所述的步骤1)具体包括：

从起始点生成一棵随机扩展树向着目标点生长，从目标点生成一棵随机扩展树向着起始点生长，两棵树相向生长，缩短路径规划时间。

本发明的技术效果：本发明采用双采样点的随机节点生成方式，通过选择距离目标点较近的节点作为采样点，加快缩短了规划距离。同时结合目标引力思想，根据概率来选择节点生成方向，一定概率采用目标点作为最终采样点，使得扩展沿着目标更具方向性，接着两棵树又分别将对方的新生节点做下次扩展的目标点，极大程度上提高了规划的效率。

另一方面，本发明考虑了智能车的非完整约束，通过B样条曲线拟合方法来满足该约束，从而获得连续平滑并且适应智能车需求的直接可跟随路径。从而让路径能直接用于引导智能车，克服了现有技术的缺陷。

附图说明

图1为本发明一种改进双向快速搜索随机树算法的智能车路径规划方法的流程图。

图2为本发明中智能车非完整约束的模型图。

图3为改进双向快速搜索随机树算法扩展生成路径的示意图

具体实施方式

下面对照附图，通过对实施例的描述，对本发明具体实施方式作进一步详细的说明，以帮助本领域的技术人员对本发明的发明构思、技术方案有更完整、准确和深入的理解。

如图1-3所示，本发明提供了一种改进双向快速搜索随机树算法的智能车路径规划方法，该方法具体包括以下步骤：

1)起始点和目标点两棵树相向搜索；

2)开始生成新节点，在节点扩展时，在地图上同时生成两个随机采样点，并通过距离函数来选择距离目标点较近的采样点来为下一扩展节点提供方向；

3)结合目标引力思想，根据概率来判断生成下一节点时是以目标点还是上一步新生较近随机采样点作为扩展节点方向；

4)在规划过程中，产生动态目标点，加快规划速度，两棵随机树分别选择对方生成的最新节点作为目标；

5)判断扩展过程中是否与障碍物或者地图边界发生碰撞，若碰撞，则重新生成新的采样点；否则，产生新节点；

6)两棵树的最新节点小于一定阈值，则生成路径；

7)结合车辆的非完整约束模型，提高生成路径的可行性；

8)采用B样条曲线对生成路径进行优化，提高路径的平滑性。

所述的步骤1)具体包括：

从起始点生成一棵随机扩展树向着目标点生长，从目标点生成一棵随机扩展树向着起始点生长，两棵树相向生长，缩短路径规划时间。

所述的步骤2)具体包括：

使用采样函数在规划地图上同时生成两个随机采样点，用距离函数来计算上一新生节点与两随机采样点之间的距离，选择距离目标近的随机采样点来为下一扩展节点提供方向，加快缩短路径规划距离。

所述的步骤3)具体包括：

为了防止新生节点过于偏离目标点，加入目标引力思想，使随机树的生长具有一定的方向性。在进行采样时，采用启发式采样，其随机树在进行扩展时，通过随机概率p₁(1>p₁≥0)来确定目标点作为随机采样点的可能性。若设定参照概率p，当p<p₁，选择随机采样点作为最终采样点，当p₁>p，选择目标点作为最终采样点，当目标点作为采样点时，会使得随机树的生长具有一定的方向性和收敛性，能够提高随机树的扩展能力。

计算扩展树新生节点q_new时，基本RRT中计算公式为：

q_new＝q_near+step(q_rand-q_near)/||q_rand-q_near||

其中，step为扩展时随机树的步长，(q_rand-q_near)表示两向量单位化，||q_rand-q_near||表示两点间欧氏距离，q_rand为随机点，q_near为距离随机点最近的一个邻近节点；

加入目标引力思想的新生节点q_new计算公式为：

q_new＝q_near+step₁(q_rand-q_near)/||q_rand-q_near||+step₂(q_goal-q_near)/||q_goal-q_near||

step₁为随机树向随机点方向扩展的步长，step₂为随机树向目标点方向扩展的步长，q_goal为目标点，||q_goal-q_near||为q_goal和q_near间的欧氏距离。

所述的步骤4)具体包括：

产生动态目标点，在一棵随机树产生新的节点时，另一棵树的下一次扩展将会以此新生节点作为目标点进行生长。

所述的步骤5)具体包括：

当原始节点向着最终采样点扩展时，判断是否会与障碍物发生碰撞，即判断随机采样点与节点的连线上是否有障碍物，如果碰撞则舍弃此最终采样点，回到步骤2)；不碰撞则依照设定步长来扩展随机树长度到最终采样点。

所述的步骤6)具体包括：

当两棵随机树相向不断进行扩展，每生成新的节点，要计算出两棵树新生节点之间的距离，在给定的阈值下，若新生节点之间的距离小于等于阈值，则直接连接两棵随机树，生成初始路径，否则依照步骤2)至5)继续生成新节点。

所述的步骤7)如图三具体包括：

设智能车在任务空间中状态变量为(x,y,θ,v,φ)，其中(x,y)为无人车后轮轴的中心在系统坐标系下的坐标，θ为移动机器人前进方向与x轴的夹角，v为移动机器人的速度，φ为无人车前进方向与前车轮之间的夹角。因为无人车受到非完整性约束，故车轮和地面是点接触，假设接触点只有纯滚动不产生相对的滑动，则可得智能车的运动方程为：

根据式运动方程可得智能车所受到的约束方程为：

其中，智能车的控制变量为加速度u₀和车轮的角速度u₁，智能车的最小转弯半径(即最大曲率)的计算式如下：

式中K_max为最大曲率，φ_max为车辆前轮最大转向角，L为轴距，R_max为最小转弯半径。

除了上面的约束条件之外，约束条件还有：

所述的步骤8)具体包括：

B样条曲线具有灵活性、连续性和直线保持性等优点。准均匀B样条曲线会经过端点，用此法来拟合路径，以生成符合车辆运动学约束的可执行轨迹。

B样条曲线的基函数如下：

端点位置：

曲线函数：

本发明考虑了智能车的非完整约束，通过B样条曲线拟合方法来满足该约束，从而获得连续平滑并且适应智能车需求的直接可跟随路径。

上面结合附图对本发明进行了示例性描述，显然本发明具体实现并不受上述方式的限制，只要采用了本发明的发明构思和技术方案进行的各种非实质性的改进，或未经改进将本发明构思和技术方案直接应用于其它场合的，均在本发明保护范围之内。

Claims (10)

1.一种改进双向快速搜索随机树算法的智能车路径规划方法，其特征在于，具体包括以下步骤：

1)起始点和目标点两棵树相向搜索；

2)开始生成新节点，在节点扩展时，在地图上同时生成两个随机采样点，并通过距离函数来选择距离目标点较近的采样点来为下一扩展节点提供方向；

3)结合目标引力思想，根据概率来判断生成下一节点时是以目标点还是上一步新生较近随机采样点作为扩展节点方向；

4)在规划过程中，产生动态目标点，加快规划速度，两棵随机树分别选择对方生成的最新节点作为目标；

5)判断扩展过程中是否与障碍物或者地图边界发生碰撞，若碰撞，则重新生成新的采样点；否则，产生新节点；

6)两棵树的最新节点小于一定阈值，则生成路径；

7)结合车辆的非完整约束模型，提高生成路径的可行性；

8)采用B样条曲线对生成路径进行优化，提高路径的平滑性。

2.根据权利要求1所述的一种改进双向快速搜索随机树算法的智能车路径规划方法，其特征在于，所述的步骤3)包括：

在进行采样时，采用启发式采样，其随机树在进行扩展时，通过随机概率p₁(1>p₁≥0)来确定目标点作为随机采样点的可能性，设定参照概率p，当p<p₁，选择随机采样点作为最终采样点，当p₁>p，选择目标点作为最终采样点，当目标点作为采样点时，使得随机树的生长具有一定的方向性和收敛性，能够提高随机树的扩展能力。

3.根据权利要求2所述的一种改进双向快速搜索随机树算法的智能车路径规划方法，其特征在于，所述的步骤3)还包括：计算扩展树新生节点q_new时，基本RRT中计算公式为：

q_new＝q_near+step(q_rand-q_near)/||q_rand-q_near||

其中，step为扩展时随机树的步长，(q_rand-q_near)表示两向量单位化，||q_rand-q_near||表示两点间欧氏距离，q_rand为随机点，q_near为距离随机点最近的一个邻近节点；

加入目标引力思想的新生节点q_new计算公式为：

q_new＝q_near+step₁(q_rand-q_near)/||q_rand-q_near||+step₂(q_goal-q_near)/||q_goal-q_near||

step₁为随机树向随机点方向扩展的步长，step₂为随机树向目标点方向扩展的步长，q_goal为目标点，||q_goal-q_near||为q_goal和q_near间的欧氏距离。

4.根据权利要求1-3中任一所述的一种改进双向快速搜索随机树算法的智能车路径规划方法，其特征在于，所述的步骤2)具体包括：

使用采样函数在规划地图上同时生成两个随机采样点，用距离函数来计算上一新生节点与两随机采样点之间的距离，选择距离目标近的随机采样点来为下一扩展节点提供方向，加快缩短路径规划距离。

5.根据权利要求1所述的一种改进双向快速搜索随机树算法的智能车路径规划方法，其特征在于，所述的步骤4)具体包括：

产生动态目标点，在一棵随机树产生新的节点时，另一棵树的下一次扩展将以此新生节点作为目标点进行生长。

6.根据权利要求1所述的一种改进双向快速搜索随机树算法的智能车路径规划方法，其特征在于，所述的步骤5)具体包括：

当原始节点向着最终采样点扩展时，判断是否会与障碍物或者地图边界发生碰撞，如果碰撞则舍弃此最终采样点，回到步骤2)；不碰撞则依照设定步长来扩展随机树长度到最终采样点。

7.根据权利要求1所述的一种改进双向快速搜索随机树算法的智能车路径规划方法，其特征在于，所述的步骤6)具体包括：

当两棵随机树相向不断进行扩展，每生成新的节点，要计算出两棵树新生节点之间的距离，在给定的阈值下，若新生节点之间的距离小于等于阈值，则直接连接两棵随机树，生成初始路径，否则依照步骤2)至5)继续生成新节点。

8.根据权利要求1所述的一种改进双向快速搜索随机树算法的智能车路径规划方法，其特征在于，所述的步骤7)具体包括：

设智能车在任务空间中状态变量为(x,y,θ,v,φ)，其中(x,y)为无人车后轮轴的中心在系统坐标系下的坐标，θ为移动机器人前进方向与x轴的夹角，v为移动机器人的速度，φ为无人车前进方向与前车轮之间的夹角，因为无人车受到非完整性约束，故车轮和地面是点接触，假设接触点只有纯滚动不产生相对的滑动，则智能车的运动方程为：

根据运动方程，智能车所受到的约束方程为：

其中，智能车的控制变量为加速度u₀和车轮的角速度u₁，智能车的最小转弯半径的计算式如下：

式中K_max为最大曲率，φ_max为车辆前轮最大转向角，L为轴距，R_max为最小转弯半径；

约束条件还包括：

9.根据权利要求1所述的一种改进双向快速搜索随机树算法的智能车路径规划方法，其特征在于，所述的步骤8)具体包括：

B样条曲线具有灵活性、连续性和直线保持性等优点。准均匀B样条曲线会经过端点，用此法来拟合路径，以生成符合车辆运动学约束的可执行轨迹；B样条曲线的基函数如下：

端点位置的算式如下：

曲线函数如下：

10.根据权利要求1所述的一种改进双向快速搜索随机树算法的智能车路径规划方法，其特征在于，所述的步骤1)具体包括：

从起始点生成一棵随机扩展树向着目标点生长，从目标点生成一棵随机扩展树向着起始点生长，两棵树相向生长，缩短路径规划时间。

Images