CN115454083A - 基于快速扩展随机树与动态窗口法的双层路径规划方法 - Google Patents

Abstract

本发明提出一种基于快速扩展随机树与动态窗口法的双层路径规划方法，先对地图内的障碍物进行处理，包括安全距离设定和障碍物填充，然后利用快速扩展随机树算法对已处理的地图进行初步规划，其中，提出模仿植物生长的趋光性的仿生设计。以无障碍物最远的规则取得局部目标点，进一步采用动态窗口法以局部目标点为指引向其运行，当智能车到达局部目标点一定范围内时，转而向下一个目标点运行，循环上述步骤，直到到达最终的全局目标点。应用本技术方案可实现将无人车的运动学约束融入到路径规划过程中，使算法规划出的路径符合无人车的行驶要求。

Description

基于快速扩展随机树与动态窗口法的双层路径规划方法

技术领域

本发明涉及智能驾驶路径规划和自主导航技术领域，特别是一种基于快速扩展随机树与动态窗口法的双层路径规划方法。

背景技术

当前，汽车产业正处于百年未有之大变局，汽车的“电动化”、“网联化”、“智能化”、“共享化”被认为是为来该产业的发展方向。

智能车辆通常是指在车辆上增加了先进的传感器(如激光雷达、毫米波雷达、摄像头等)、控制器、执行器等装置，通过车载环境感知系统和信息终端，实现与人、车、路等的信息交换，使车辆具备智能环境感知能力，能够自动分析车辆行驶的安全及危险状态，并使车辆按照人的意愿到达目的地，最终实现替代人来操作的目的的汽车。智能车的关键技术包括环境感知、导航定位、路径规划以及决策控制等，其中路径规划是智能车辆技术的关键部分，对智能车的研究具有重大意义。

路径规划可以分为静态环境下的全局路径规划和动态环境下的局部路径规划。静态环境下障碍物不会运动，因此全局路径规划算法的核心思想为在已知全局环境下各个障碍物的位置、大小等相关信息，以此进行最优路径的求取。而动态环境是在静态环境下，存在一些移动且运动轨迹未知的障碍物，传感器需要捕捉运动障碍物的信息，实时的进行路径规划。因此，局部路径规划算法则注重无人车的动态避障能力，如何在未知环境下通过传感器获取局部信息来进行路径的求取是目前的重点研究问题。

发明内容

有鉴于此，本发明的目的在于提供一种基于快速扩展随机树与动态窗口法的双层路径规划方法，实现将无人车的运动学约束融入到路径规划过程中，使算法规划出的路径符合无人车的行驶要求。

为实现上述目的，本发明采用如下技术方案：一种基于快速扩展随机树与动态窗口法的双层路径规划方法，包括以下步骤：

步骤S1:确定无人车起始点坐标和目标点坐标，读取障碍物坐标，并将不规则障碍物标定为矩形障碍物；

步骤S2:设置一定的障碍物间隙大小，若小于设定值，以若干正方形障碍物填充，同时在已有的障碍物附近设置“势能区”；

步骤S3:利用快速扩展随机树算法，对已处理的地图进行全局规划，得到一系列路径点；

步骤S4:对上述路径点进行选取，选取合适的路径点，作为局部路径规划的局部目标点，选取方法遵循最远无障碍物原则；

步骤S5:将无人车辆的运动学约束融入到动态窗口法中，依次对一系列局部目标点进行访问，直到最终的全局目标点。

在一较佳的实施例中，在步骤S1中，将不规则障碍物离散化，由此标定矩形障碍物的长L＝(x_max-x_min)，宽W＝(y_max-y_min)，其中，x_max， x_min，y_max，y_min分别为离散点中的横坐标的最大值和最小值以及纵坐标的最大值和最小值。

在一较佳的实施例中，在步骤S2中，将不规则障碍物标定为矩形障碍物后，若相邻两个障碍物之间的距离S₁＝K·a，其中a为无人车的宽，K 为安全系数，则填补该空缺；将障碍物表面距离

的区域设定为势能区的范围。

在一较佳的实施例中，在步骤S3中，将起始点作为快速扩展随机数的根节点，并且以该节点为基础，找到距离此节点距离最近的树节点，拓展出一个步长长度，以此方法不断进行迭代，最终生成一条可行的优化路径；快速扩展随机树算法以路径规划的起点作为随机树T的根节点q_start，在空间中随机采样，得到随机点q_rand，然后计算随机q_rand与随机树T中所有节点q_i的距离，找出离随机点q_rand最近的节点q_i作为邻近点q_near，随机树T 从邻近点q_near向随机点q_rand方向移动给定步长esp，扩展生成一个新的节点q_new；如果在邻近点q_near向新节点q_new方向扩展的过程中不与障碍物发生碰撞，且新节点不在势能区，则将新节点q_new加入到随机树T中；若不与障碍物发生碰撞，则以一定概率将新节点q_new加入到随机树T中；若发生碰撞，重新采样随机点q_rand；最后判断新节点q_new是否在目标点q_goal的区域内，若不在目标点区域内，重复上过程；若在目标点区域内，将新节点q_new作为目标点的父节点，并将目标点q_goal加入到随机树T中，从目标点q_goal回溯到根节点q_start，得到规划路径的路径点。

在一较佳的实施例中，在选取随机节点时，以一定概率选取为目标节点的一系列的同心圆弧上，选取公式如下：

式中r：

其中，q_rand(x，y)为待选取的随机点坐标；n₁，n₂，n₃，n₄是相互独立的0～1的随机数；X_max，Y_max分别为地图的横轴最大值和纵轴最大值；

分别为目标点的横坐标和纵坐标；R为起始点到目标点的距离， m为同心圆系数一般取在1～2之间；

在一较佳的实施例中，在步骤S4中，局部目标点选取遵循最远无障碍原则，具体的子目标点选取规则如下：

设提取的路径点为{P_k,k＝1,2,…,n}，连接P₁P₃，若P₁P₃不经过障碍物，即线段P₁P₃到障碍物的距离大于预设阈值，则继续连接P₁P₄，直到P₁P_k(k＝ 4,5,…,n)经过障碍物，则把P_k-1与P₁连接起来，同时删除中间冗余点，更新路径；从节点P_k-1重复上述操作，直到路径中没有冗余转折点，记录这些路径节点为局部目标点。

6.根据权利要求1所述的一种基于快速扩展随机树与动态窗口法的双层路径规划方法，其特征在于，在步骤S5中，利用动态窗口法在速度(v,w) 空间中采样多组速度，v、w分别是无人车的速度和角速度，并模拟无人车在这些速度下一定时间内的轨迹；在得到多组轨迹以后，对这些轨迹进行评价，选取最优轨迹所对应的速度来驱动无人车运动；评价函数：

G(v,ω)＝δ·(α·H_G(v,ω)+β·D(v,ω)+γ·V(v,ω))

其中，G(v,ω)为评价函数，H_G(v,ω)是航向角函数，D(v,ω)是与障碍物距离函数，V(v,ω)是速度函数，α，β，γ三个参数代表相应的权重，δ使得三部分的权重更加平滑。

在一较佳的实施例中，将两相邻点之间的运动轨迹看成直线，即沿机器人坐标系x轴移动了v_t·Δt；只需将该段距离分别投影在世界坐标系x轴和y轴上就能得到t+1时刻相对于t时刻智能车在世界坐标系中坐标移动的位移Δx和Δy；

Δx＝v_tΔtcos(θ_t)

Δy＝v_tΔtsin(θ_t)

其中，θ_t单位时间间隔的较大变化量，以此类推，推算一段时间内的轨迹，只需要将这段时间的位移增量累计求和:

x＝x+v_tΔtcos(θ_t)

y＝y+v_tΔtsin(θ_t)

θ_t＝θ_t+w_tΔt

依次对上述局部目标点遍历，直到运行到终点；在速度(v,w)的二维空间中，存在无穷多组速度；根据智能车本身的限制和环境限制将采样速度控制在一定范围内:

智能车受自身最大速度最小速度的限制：

V_s＝{v∈[v_min,v_max],w∈[w_min,w_max]}

智能车受自身加减速性能的限制：

其中v_c，w_c是智能车的当前速度，

分别为最大制动减速度和最大加速度，

分别为最大角减速度和最大角加速度；

在最大减速度下，速度范围：

其中，dist(v,w)是当前位置与障碍物的距离；

实际车辆转角有如下运动学约束；

其中，r为转弯半径，R为极限转弯半径，v，ω分别为车辆的速度和角速度，L是车辆的轴距，δ_max是前轮最大转角；

由此，V_s改写为：

修改后的速度空间：

V_r＝V′_s∩V_d∩V_a。

与现有技术相比，本发明具有以下有益效果：本发明的目的是提供一种基于快速扩展随机树算法(RRT)和动态窗口法(RRT)的双层路径规划方法，分别在全局层和局部层使用改进后的快速扩展随机树算法(RRT)和优化后的动态窗口法(DWA)，该方法解决了RRT算法无法对包含动态障碍物的情况进行规划和与车辆运动学约束的难题，以及DWA算法易陷入局部最小值等问题。对原始RRT进行相应改进以适应与DWA算法的融合。并将无人车的运动学约束融入到路径规划过程中，使算法规划出的路径符合无人车的行驶要求。

附图说明

图1为本发明优选实施例的方法流程图。

图2为本发明优选实施例的障碍物合并原理图。

图3为本发明优选实施例的障碍物势能区示意图。

图4为本发明优选实施例的仿生设计的RRT示意图。

图5为本发明优选实施例的未经改进的RRT示意图。

图6为本发明优选实施例的局部目标点选取示意图。

图7为本发明优选实施例的无人车运动学模型示意图。

图8为本发明优选实施例的RRT与DWA算法融合效果图示意图。

具体实施方式

下面结合附图及实施例对本发明做进一步说明。

应该指出，以下详细说明都是例示性的，旨在对本申请提供进一步的说明。除非另有指明，本文使用的所有技术和科学术语具有与本申请所属技术领域的普通技术人员通常理解的相同含义。

需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本申请的示例性实施方式；如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。

如图1-图7所示，本实施例提供的基于快速扩展随机树算法引导下的动态窗口法的双层路径规划方法包括以下实施过程：

步骤一：确定无人车起始点坐标和目标点坐标，读取障碍物坐标，并将不规则障碍物标定为矩形障碍物。将不规则障碍物离散化，由此标定矩形障碍物的长L＝(x_max-x_min)，宽W＝(y_max-y_min)，其中，x_max，x_min， y_max，y_min分别为离散点中的横坐标的最大值和最小值，纵坐标的最大值和最小值。

步骤二：设置一定的障碍物间隙大小，若小于设定值，以若干正方形障碍物填充，同时在已有的障碍物附近设置“势能区”，具有减少路径节点进入该区域的概率。将不规则障碍物标定为矩形障碍物后，若相邻两个障碍物之间的距离S₁＝K·a(其中a为无人车的宽，K为安全系数)，则填补该空缺。参考图1。将障碍物表面距离

的区域设定为势能区的范围，参考图2。具体应用见步骤S3中如何将新节点的加入到随机树中。

步骤三：利用快速扩展随机树算法，对已处理的地图进行全局规划，得到一系列路径点。将起始点作为快速扩展随机数的根节点，并且以该节点为基础，找到距离此节点距离最近的树节点，拓展出一个步长长度，以此方法不断进行迭代，最终生成一条可行的优化路径。具体来说，该算法以路径规划的起点作为随机树T的根节点q_start，在空间中随机采样，得到随机点q_rand，然后计算随机q_rand与随机树T中所有节点q_i的距离，找出离随机点q_rand最近的节点q_i作为邻近点q_near，随机树T从邻近点q_near向随机点q_rand方向移动给定步长esp，扩展生成一个新的节点q_new(图2)。如果在邻近点q_near向新节点q_new方向扩展的过程中不与障碍物发生碰撞，且新节点不在势能区，则将新节点q_new加入到随机树T中；若不与障碍物发生碰撞，则以一定概率将新节点q_new加入到随机树T中；若发生碰撞，重新采样随机点q_rand。最后判断新节点q_new是否在目标点q_goal的区域内，若不在目标点区域内，重复上过程；若在目标点区域内，将新节点q_new作为目标点的父节点，并将目标点q_goal加入到随机树T中，从目标点q_goal回溯到根节点q_start，得到规划路径的路径点。

为了加快到达目标点，提高算法效率，提出了一种仿生设计，主要模仿植物生长的趋光性，在目标点附近设置为“阳光”，由此向外生长的随机树具有了向目标点扩展的趋向性。具体来说，在选取随机节点时，以一定概率选取为目标节点(目标点)的一系列的同心圆弧上，选取公式如下：

式中r：

其中，q_rand(x，y)为待选取的随机点坐标；n₁，n₂，n₃，n₄是相互独立的0～1的随机数；X_max，Y_max分别为地图的横轴最大值和纵轴最大值；

分别为目标点的横坐标和纵坐标。R为起始点到目标点的距离， m为同心圆系数一般取在1～2之间。参考图3，其中黑色矩形是障碍物，绿色五角星是起点，红色五角星是目标点，蓝色的×是选取的随机点，呈现出目标点中心密集，边缘稀疏的情况，绿色线条为扩展出来的随机树，蓝色线条为规划出的路径。图4是未经改进的RRT规划的路径，随机树扩展较慢，没有方向。

步骤四：对上述路径点进行选取，选取合适的路径点，作为局部路径规划的局部目标点，选取方法遵循最远无障碍物原则。具体的子目标点选取规则如下：

设提取的路径点为{P_k,k＝1,2,…,n}，连接P₁P₃，若P₁P₃不经过障碍物，即线段P₁P₃到障碍物的距离大于预设阈值，则继续连接P₁P₄，直到P₁P_k(k＝ 4,5,…,n)经过障碍物，则把P_k-1与P₁连接起来，同时删除中间冗余点，更新路径；从节点P_k-1重复上述操作，直到路径中没有冗余转折点，记录这些路径节点为局部目标点。参考图5，该图隐去了随机点，其中，绿色线段是删除冗余点后的路径，红色*是选取的局部目标点，其他信息如上。

步骤五：将无人车辆的运动学约束融入到动态窗口法中，依次对一系列局部目标点进行访问，直到最终的全局目标点。利用动态窗口法在速度 (v,w)空间(v，w分别是无人车的速度和角速度)中采样多组速度，并模拟无人车在这些速度下一定时间内的轨迹。在得到多组轨迹以后，对这些轨迹进行评价，选取最优轨迹所对应的速度来驱动无人车运动。评价函数：

G(v,ω)＝δ·(α·H_G(v,ω)+β·D(v,ω)+γ·V(v,ω))

其中，G(v,ω)为评价函数，H_G(v,ω)是航向角函数，D(v,ω)是与障碍物距离函数，V(v,ω)是速度函数，α，β，γ三个参数代表相应的权重，δ使得三部分的权重更加平滑。

在动态窗口算法中，要模拟智能车的轨迹，需要知道智能车的运动模型。先将智能车简化为两轮模型，假设两轮移动智能车的轨迹是一段一段的圆弧或者直线(旋转速度为0时)，一对(v_t，w_t)就代表一个圆弧轨迹。为简单起见，由于智能车相邻时刻(一般ms计)内，运动距离短，因此可以将两相邻点之间的运动轨迹看成直线，即沿机器人坐标系x轴移动了v_t·Δt (间隔时间Δt)。只需将该段距离分别投影在世界坐标系x轴和y轴上就能得到t+1时刻相对于t时刻智能车在世界坐标系中坐标移动的位移Δx和Δy。

Δx＝v_tΔtcos(θ_t)

Δy＝v_tΔtsin(θ_t)

其中，θ_t单位时间间隔的较大变化量。

以此类推，推算一段时间内的轨迹，只需要将这段时间的位移增量累计求和:

x＝x+v_tΔtcos(θ_t)

y＝y+v_tΔtsin(θ_t)

θ_t＝θ_t+w_tΔt

依次对上述局部目标点遍历，直到运行到终点。在速度(v,w)的二维空间中，存在无穷多组速度。但是根据智能车本身的限制和环境限制可以将采样速度控制在一定范围内:

1.智能车受自身最大速度最小速度的限制：

V_s＝{v∈[v_min,v_max],w∈[w_min,w_max]}

2.智能车受自身加减速性能的限制：

其中v_c，w_c是智能车的当前速度，

分别为最大制动减速度和最大加速度，

分别为最大角减速度和最大角加速度。

3.基于智能车安全的考虑：

为了能够在碰到障碍物前停下来，因此在最大减速度下，速度有一个范围：

由于，实际车辆转角的约束，其中部分速度空间是不符合要求的，实际车辆转角有如下运动学约束；

其中，r为转弯半径，R为极限转弯半径，v，ω分别为车辆的速度和角速度，L是车辆的轴距，δ_max是前轮最大转角。参考图6，车辆的运动学约束。

由此，V_s可改写为：

修改后的速度空间：

V_r＝V′_s∩V_d∩V_a

最终，利用DWA算法依次遍历上述局部目标点，直到最终的目标点。参考图7。

本发明所进行的工作主要包括：第一，总结前人改进快速扩展随机树的可行性与不足，针对随机树扩展具有随机性导致的搜索效率低下的问题，提出了模仿植物生长的趋光性的设计，把目标点定义为“光源”，让随机点以一定概率取到目标点附近的一系列以目标点为圆心的同心圆上。第二：为了提高局部层规划，即动态窗口法的效果，提出了对障碍物进行预处理，通过减少障碍物一定距离内的通过性，进而更好地避开障碍物，进一步，针对快速扩展随机树算法和动态窗口法的衔接，提出最远无障碍策略，从起始点开始，选取当前节点连线最远的未与障碍物碰撞的节点，记录该节点，并把该节点更新为当前节点，循环该操作，直到遍历完所有节点。

Claims (8)

1.基于快速扩展随机树与动态窗口法的双层路径规划方法，其特征在于，包括以下步骤：

步骤S1：确定无人车起始点坐标和目标点坐标，读取障碍物坐标，并将不规则障碍物标定为矩形障碍物；

步骤S2：设置一定的障碍物间隙大小，若小于设定值，以若干正方形障碍物填充，同时在已有的障碍物附近设置“势能区”；

步骤S3：利用快速扩展随机树算法，对已处理的地图进行全局规划，得到一系列路径点；

步骤S4：对上述路径点进行选取，选取合适的路径点，作为局部路径规划的局部目标点，选取方法遵循最远无障碍物原则；

步骤S5：将无人车辆的运动学约束融入到动态窗口法中，依次对一系列局部目标点进行访问，直到最终的全局目标点。

2.根据权利要求1所述的基于快速扩展随机树与动态窗口法的双层路径规划方法，其特征在于，在步骤S1中，将不规则障碍物离散化，由此标定矩形障碍物的长L＝(x_max-x_min)，宽W＝(y_max-y_min)，其中，x_max，x_min，y_max，y_min分别为离散点中的横坐标的最大值和最小值以及纵坐标的最大值和最小值。

3.根据权利要求1所述的基于快速扩展随机树与动态窗口法的双层路径规划方法，其特征在于，在步骤S2中，将不规则障碍物标定为矩形障碍物后，若相邻两个障碍物之间的距离S₁＝K·a，其中a为无人车的宽，K为安全系数，则填补该空缺；将障碍物表面距离

的区域设定为势能区的范围。

4.根据权利要求1所述的基于快速扩展随机树与动态窗口法的双层路径规划方法，其特征在于，在步骤S3中，将起始点作为快速扩展随机数的根节点，并且以该节点为基础，找到距离此节点距离最近的树节点，拓展出一个步长长度，以此方法不断进行迭代，最终生成一条可行的优化路径；快速扩展随机树算法以路径规划的起点作为随机树T的根节点q_start，在空间中随机采样，得到随机点q_rand，然后计算随机q_rand与随机树T中所有节点q_i的距离，找出离随机点q_rand最近的节点q_i作为邻近点q_near，随机树T从邻近点q_near向随机点q_rand方向移动给定步长esp，扩展生成一个新的节点q_new；如果在邻近点q_near向新节点q_new方向扩展的过程中不与障碍物发生碰撞，且新节点不在势能区，则将新节点q_new加入到随机树T中；若不与障碍物发生碰撞，则以一定概率将新节点q_new加入到随机树T中；若发生碰撞，重新采样随机点q_rand；最后判断新节点q_new是否在目标点q_goal的区域内，若不在目标点区域内，重复上过程；若在目标点区域内，将新节点q_new作为目标点的父节点，并将目标点q_goal加入到随机树T中，从目标点q_goal回溯到根节点q_start，得到规划路径的路径点。

5.根据权利要求4所述的基于快速扩展随机树与动态窗口法的双层路径规划方法，其特征在于，在选取随机节点时，以一定概率选取为目标节点的一系列的同心圆弧上，选取公式如下：

式中r：

其中，q_rand(x，y)为待选取的随机点坐标；n₁，n₂，n₃，n₄是相互独立的0～1的随机数；X_max，Y_max分别为地图的横轴最大值和纵轴最大值；

分别为目标点的横坐标和纵坐标；R为起始点到目标点的距离，m为同心圆系数一般取在1～2之间。

6.根据权利要求1所述的基于快速扩展随机树与动态窗口法的双层路径规划方法，其特征在于，在步骤S4中，局部目标点选取遵循最远无障碍原则，具体的子目标点选取规则如下：

设提取的路径点为{P_k，k＝1，2，…，n}，连接P₁P₃，若P₁P₃不经过障碍物，即线段P₁P₃到障碍物的距离大于预设阈值，则继续连接P₁P₄，直到P₁P_k(k＝4，5，…，n)经过障碍物，则把P_k-1与P₁连接起来，同时删除中间冗余点，更新路径；从节点P_k-1重复上述操作，直到路径中没有冗余转折点，记录这些路径节点为局部目标点。

7.根据权利要求1所述的基于快速扩展随机树与动态窗口法的双层路径规划方法，其特征在于，在步骤S5中，利用动态窗口法在速度(v，w)空间中采样多组速度，v、w分别是无人车的速度和角速度，并模拟无人车在这些速度下一定时间内的轨迹；在得到多组轨迹以后，对这些轨迹进行评价，选取最优轨迹所对应的速度来驱动无人车运动；评价函数：

G(v，ω)＝δ·(α·H_G(v，ω)+β·D(v，ω)+γ·V(v，ω))

其中，G(v，ω)为评价函数，H_G(v，ω)是航向角函数，D(v，ω)是与障碍物距离函数，V(v，ω)是速度函数，α，β，γ三个参数代表相应的权重，δ使得三部分的权重更加平滑。

8.根据权利要求7所述的基于快速扩展随机树与动态窗口法的双层路径规划方法，其特征在于，将两相邻点之间的运动轨迹看成直线，即沿机器人坐标系x轴移动了v_t·Δt；只需将该段距离分别投影在世界坐标系x轴和y轴上就能得到t+1时刻相对于t时刻智能车在世界坐标系中坐标移动的位移Δx和Δy；

Δx＝v_tΔtcos(θ_t)

Δy＝v_tΔtsin(θ_t)

其中，θ_t单位时间间隔的较大变化量，以此类推，推算一段时间内的轨迹，只需要将这段时间的位移增量累计求和：

x＝x+v_tΔtcos(θ_t)

y＝y+v_tΔtsin(θ_t)

θ_t＝θ_t+w_tΔt

依次对上述局部目标点遍历，直到运行到终点；在速度(v，w)的二维空间中，存在无穷多组速度；根据智能车本身的限制和环境限制将采样速度控制在一定范围内：

智能车受自身最大速度最小速度的限制：

V_s＝{v∈[v_min，v_max]，w∈[w_min，w_max]}

智能车受自身加减速性能的限制：

其中v_c，w_c是智能车的当前速度，

分别为最大制动减速度和最大加速度，

分别为最大角减速度和最大角加速度；

在最大减速度下，速度范围：

其中，dist(v，w)是当前位置与障碍物的距离；

实际车辆转角有如下运动学约束；

其中，r为转弯半径，R为极限转弯半径，v，ω分别为车辆的速度和角速度，L是车辆的轴距，δ_max是前轮最大转角；

由此，V_s改写为：

修改后的速度空间：

V_r＝V′_s∩V_d∩V_a。

Images