一种动态变采样区域RRT无人车路径规划方法

摘要

本发明涉及一种基于动态变采样区域的概率目标偏置快速扩展随机树(RRT)无人车路径规划方法。所述方法包括：首先，初始化地图信息，根据动态变采样区域公式判断所处区域；在此基础上进行预留安全距离的碰撞检测，并根据概率目标偏置公式和步长选择公式生成新生节点，重复上述步骤直到满足新生节点和目标节点之间的距离小于距离阈值，反向搜索，输出路径；最后，考虑最大转角约束对输出路径进行逆向寻优和3次B样条曲线拟合优化，仿真验证了所述方法的有效性。本发明能够降低节点搜索的盲目性和随机性，减少路径搜索的时间，且规划的路径平滑符合车辆运动动力学约束。

权利要求

1.本发明是一种动态变采样区域快速扩展随机树(RRT)无人车路径规划方法，包括如下步骤：

步骤1：

初始化地图信息，包括地图的边界以及障碍物的信息，并将地图信息二值化，设定起始节点Xstart存入有效节点集Xnodes中并设定目标节点Xend，设定新生节点Xnew的初始位置为起点坐标，设定车长Dcar、三级动态步长σ、二级动态安全距离Dsafe和最大转角θ，设定四级动态变采样区域Sarea，初始采样区域为全地图，设定区域采样阈值K和区域采样次数k；

步骤2：

判断区域采样次数k是否超过区域采样阈值K，若采样次数未超过区域采样阈值K则进入步骤4，否则进入步骤3；

步骤3：

判断新生节点Xnew是否位于一级采样区域，若位于一级采样区域则重新设定起始节点Xstart并返回步骤1，否则初始化区域采样次数k并返回上一级采样区域，进入步骤4；

步骤4：

采样区域范围内取随机采样点Xrand，寻找有效节点集Xnodes中距离随机采样点Xrand最近的点作为最近节点Xnear，进入步骤5；

步骤5：

构建随机点重构函数：

式中：flag为碰撞检测结果、ω为目标权重因子、p为随机数、ρ为概率因子，在最近节点Xnear的方向基于车长Dcar进行碰撞检测，当flag＝0时表示未检测到碰撞，flag＝1表示检测到碰撞，结合公式(1)重新选取随机采样点Xrand，此时区域采样次数k＝k+1；

构建步长选择函数：

式中：σ1为一级步长、σ2为二级步长、σ3为三级步长，flag2为结合二级安全距离Dsafe2的碰撞检测结果，flag1为结合一级安全距离Dsafe1的碰撞检测结果，σ为动态步长，根据公式(2)确定动态步长σ，构建新生节点生成函数：根据公式(3)生成新生节点Xnew，并将新生节点Xnew存入有效节点集Xnodes中，进入步骤6；

步骤6：

判断Xnew是否满足|Xend‑Xnew|≤radius，是则进入步骤8，否则进入步骤7，其中，|Xend‑Xnew|为计算目标节点Xend和新生节点Xnew的距离公式，radius为范围阈值；

步骤7：

构建动态区域选择函数：

式中：δ1为一级距离阈值、δ2为二级距离阈值、δ3为三级距离阈值、δ4为四级距离阈值，Sarea1为一级采样区域、Sarea2为二级采样区域、Sarea3为三级采样区域、Sarea4为四级采样区域，结合公式(4)确定采样区域Sarea，返回步骤2；

步骤8：

在有效节点集Xnodes中根据各个节点间的关系，反向搜索生成路径，进入步骤9；

步骤9：

利用考虑最大转角θ限制的逆向寻优的方法対生成路径进行初步优化，首先判断起始节点Xstart和目标节点Xend间是否存在障碍物，若没有障碍物，则起始节点Xstart和目标节点Xend间为最佳路径；若存在障碍物，则选取目标节点Xend的前一节点Xnodes1，判断该节点与起始节点Xstart是否存在障碍物并判断三点间形成的转角是否小于最大转角θ，若符合条件，则直接将起始节点Xstart作为该节点的父节点，若不符合条件则继续向前寻找节点Xnodes2，重复上述操作直到完成整条路径优化，生成路径并进入步骤10；

步骤10：

对步骤9生成的路径点进行3次B样条曲线拟合优化路径，得到可实际应用的平滑路径，结束规划。

说明书

一种动态变采样区域RRT无人车路径规划方法

技术领域

[0001] 本发明涉及一种动态变采样区域快速扩展随机树(RRT)算法的无人车路径规划方法，属于无人车路径规划领域。

背景技术

[0002] 随着科学技术的持续发展，无人车被广泛应用于军事领域，包括物资运输、危险作业、特种任务等，但在无人车路径规划方面仍存在不足，例如规划速度慢、全局规划难度大
等缺点。

[0003] 针对上述缺点，多种路径规划算法被应用于无人车路径规划，常用的路径规划算法有蚁群算法、遗传算法、A*算法、RRT算法等。RRT算法因运算速度快、搜索能力强、结构简
单等优点广泛应用于无人车的路径规划。但是RRT算法在无人车路径规划方面的应用存在
如下问题：

[0004] (1)路径的不连续问题，传统RRT算法由于节点搜索的盲目性和随机性，易造成无人车规划路径的不连续与曲折，不符合无人车自身的动力学约束；

[0005] (2)局部最优问题，常规的基于单一目标偏置的RRT算法容易导致算法在障碍物附近震荡，造成局部最优和区域停滞问题。

[0006] 论文《改进RRT‑Connect算法的移动机器人路径规划》提供的方法，有如下问题：

[0007] (1)动态步长的选择未考虑安全距离，规划的路径可能无法进行实际应用，为此，本发明结合安全距离进行步长选择；

[0008] (2)路径规划过程未进行路径优化，生成路径的质量较差，为此，本发明利用逆向寻优和3次B样条曲线优化，使路径更加的平滑。

[0009] 专利《路径规划系统及路径规划方法》提供的方法，有如下问题：

[0010] (1)当区域障碍物过多时，易导致路径局部震荡，为此，本发明引入了后退机制，避免节点搜索陷入局部震荡；

[0011] (2)未考虑动力学约束，生成的路径可行性差，为此，本发明考虑了车长和最大转角约束，满足车辆动力学约束。

[0012] 专利《基于高斯采样和目标偏向引导的快速扩展随机树算法、电子设备及存储介质》提供的方法，存在如下问题：

[0013] (1)新生节点Xnew生成过程未考虑安全距离，节点缺乏可行性，为此，本发明在节点生成中考虑了动态安全距离，保证了生成路径的可行性；

[0014] (2)对于目标点和随机点权重参数k1、k2实时性选择难度大，复杂性高，为此，本发明利用概率目标偏置策略，保证了节点生成的多样性和简单易行性。

发明内容

[0015] 本发明创造目的在于，提供一种改进快速扩展随机树算法。利用动态变采样区域的方法，优化快速扩展随机树算法节点选择的全局盲目性；运用动态概率目标偏向策略，避
免陷入局部最优解和障碍物区域震荡；结合安全距离的动态步长选择，保证了规划路径的
安全性和合理性；利用考虑最大转角的逆向寻优和3次B样条曲线拟合，保证了路径的连续
性和平滑度。

[0016] 为实现上述目的，本发明采用如下技术方案。

[0017] 本发明的一种基于动态变采样区域RRT路径规划方法，包括如下步骤：

[0018] 步骤1：

[0019] 初始化地图信息，载入地图的边界以及障碍物的信息，并将地图信息二值化，设定起始节点Xstart存入有效节点集Xnodes中并设定目标节点Xend，设定新生节点Xnew的初始值为
起点坐标，设定车长Dcar、三级动态步长σ、二级动态安全距离Dsafe和最大转角θ，设定四级动
态变采样区域Sarea，初始采样区域为全地图，设定区域采样阈值K和区域采样次数k；

[0020] 步骤2：

[0021] 判断区域采样次数k是否超过区域采样阈值K，若采样次数未超过区域采样阈值K则进入步骤4，否则进入步骤3；

[0022] 步骤3：

[0023] 判断新生节点Xnew是否位于一级采样区间，若位于一级采样区域则重新设定起始节点Xstart并返回步骤1，否则初始化区域采样次数k并返回上一级采样区域，进入步骤4；

[0024] 步骤4：

[0025] 采样区域范围内取随机采样点Xrand，寻找有效节点集Xnodes中距离随机采样点Xrand最近的点作为最近节点Xnear，进入步骤5；

[0026] 步骤5：

[0027] 构建随机点重构函数：

[0028]

[0029] 式中：flag为碰撞检测结果、ω为目标权重因子、p为随机数、ρ为概率因子，在最近节点Xnear的方向基于车长Dcar进行碰撞检测，当flag＝0时表示未检测到碰撞，flag＝1表示
检测到碰撞，结合公式(1)重新选取随机采样点Xrand，此时区域采样次数k＝k+1；

[0030] 构建步长选择函数：

[0031]

[0032] 式中：σ1为一级步长、σ2为二级步长、σ3为三级步长，flag2为结合二级安全距离Dsafe2的碰撞检测结果，flag1为结合一级安全距离Dsafe1的碰撞检测结果，σ为动态步长，根据
公式(2)确定动态步长σ，构建新生节点生成函数：

[0033]

[0034] 根据公式(3)生成新生节点Xnew，并将新生节点Xnew存入有效节点集Xnodes中，进入步骤6；

[0035] 步骤6：

[0036] 判断Xnew是否满足|Xend‑Xnew|≤radius，是则进入步骤8，否则进入步骤7，其中，|Xend‑Xnew|为计算目标节点Xend和新生节点Xnew的距离公式，radius为范围阈值；

[0037] 步骤7：

[0038] 构建动态区域选择函数：

[0039]

[0040] 式中：δ1为一级距离阈值、δ2为二级距离阈值、δ3为三级距离阈值、δ4为四级距离阈值，Sarea1为一级采样区域、Sarea2为二级采样区域、Sarea3为三级采样区域、Sarea4为四级采样区
域，结合公式(4)确定采样区域Sarea，返回步骤2；

[0041] 步骤8：

[0042] 在有效节点集Xnodes中根据各个节点间的关系，反向搜索生成路径，进入步骤9；

[0043] 步骤9：

[0044] 利用考虑最大转角θ限制的逆向寻优的方法对生成路径进行初步优化，首先判断起始节点Xstart和目标节点Xend间是否存在障碍物，若没有障碍物，则起始节点Xstart和目标
节点Xend间为最佳路径；若存在障碍物，则选取目标节点Xend的前一节点Xnodes1，判断该节点
与起始节点Xstart是否存在障碍物并判断三点间形成的转角是否小于最大转角θ，若符合条
件，则直接将起始节点Xstart作为该节点的父节点，若不符合条件则继续向前寻找节点
Xnodes2，重复上述操作直到完成整条路径优化，生成路径并进入步骤10；

[0045] 步骤10：

[0046] 对步骤9生成的路径点进行3次B样条曲线拟合优化路径，得到可实际应用的平滑路径，结束规划。

[0047] 本发明具有如下有益效果：

[0048] (1)本发明所述动态变采样区域RRT算法有效提升了节点搜索效率，简单易行，规划路径长度也有显著降低，且避免了陷入局部最优和区域震荡，考虑安全距离，保证了路径
的安全性与可行性；

[0049] (2)在动态变采样区域选择过程中，引入了后退机制，避免了因为区域障碍物过多造成的区域停滞；

[0050] (3)利用逆向寻优和3次B样条曲线拟合优化路径增加了路径的可行性，使路径更加平滑，符合无人车的动力学约束；

[0051] (4)仿真结果表明，本发明的改进RRT算法相比于传统RRT算法搜索时间减少了78.2％，平均搜索节点数减少了65.8％，经过逆向寻优和3次B样条曲线优化，相比优化前的
距离减少11.4％左右，且路径平滑并符合最大转角要求。

附图说明

[0052] 图1是本发明一种动态变采样区域RRT无人车路径规划方法的总体流程图；

[0053] 图2是动态变采样区域RRT算法的剪枝结果图；

[0054] 图3是原始RRT算法的剪枝结果图；

[0055] 图4是目标偏置RRT算法的剪枝结果图；

[0056] 图5是概率目标偏置RRT算法的剪枝结果图；

[0057] 图6是动态变采样区域RRT算法的逆向寻优结果图；

[0058] 图7是动态变采样区域RRT算法3次B样条曲线优化结果图；

[0059] 图8是未考虑安全距离的动态变采样区域RRT算法的剪枝结果图。

具体实施方式

[0060] 以下结合具体实施例和图1对本发明作详细说明：

[0061] 本发明涉及一种基于动态变采样区域的概率目标偏置RRT无人车路径规划方法。RRT算法有强大的搜索能力，但是由于其全局采样的盲目性和随机性，导致其搜索效率的不
足，且规划的路径曲折不连续，很难直接进行实际应用，为此本发明提供了基于动态变采样
区域的概率目标偏置RRT无人车路径规划方法，利用动态变采样区域的方法，控制采样区
间，降低快速扩展随机树算法节点选择的全局盲目性；运用动态概率目标偏向策略，实现随
机点的重新选择，避免陷入局部最优解和障碍物局部震荡；结合安全距离的动态步长选择，
保证了规划路径的安全性和合理性；利用考虑最大转角的逆向寻优和3次B样条曲线拟合，
保证了路径的连续性和平滑度。其具体包括如下步骤：

[0062] 步骤1：

[0063] 初始化地图信息，将地图按栅格数目均匀分割，载入地图的边界以及障碍物的信息，并将地图信息二值化，根据栅格化的地图结果，将其转化为一个二维数组，如果是障碍
物将该点对应数组设置为1，如果是可行路径将该点对应数组设置为0，设定起始节点Xstart
存入有效节点集Xnodes中并设定目标节点Xend，设定新生节点Xnew的初始值为起点坐标，设定
车长Dcar、三级动态步长σ、二级动态安全距离Dsafe和最大转角θ，设定四级动态变采样区域
Sarea，初始采样区域为全地图，设定区域采样阈值K和区域采样次数k；

[0064] 步骤2：

[0065] 判断区域采样次数k是否超过区域采样阈值K，若采样次数未超过区域采样阈值K则进入步骤4，否则进入步骤3；

[0066] 步骤3：

[0067] 判断新生节点Xnew是否位于一级采样区间，若位于一级采样区域则重新设定起始节点Xstart并返回步骤1，否则初始化区域采样次数k并返回上一级采样区域，进入步骤4；

[0068] 步骤4：

[0069] 采样区域范围内取随机采样点Xrand，寻找有效节点集Xnodes中距离随机采样点Xrand最近的点作为最近节点Xnear，进入步骤5；

[0070] 步骤5：

[0071] 构建随机点重构函数：

[0072]

[0073] 式中：flag为碰撞检测结果、ω为目标权重因子、p为随机数、ρ为概率因子，在最近节点Xnear的方向基于车长Dcar进行碰撞检测，当flag＝0时表示未检测到碰撞，flag＝1表示
检测到碰撞，结合公式(1)重新选取随机采样点Xrand，此时区域采样次数k＝k+1；

[0074] 构建步长选择函数：

[0075]

[0076] 式中：σ1为一级步长、σ2为二级步长、σ3为三级步长，flag2为结合二级安全距离Dsafe2的碰撞检测结果，flag1为结合一级安全距离Dsafe1的碰撞检测结果，σ为动态步长，根据
公式(2)确定动态步长σ，构建新生节点生成函数：

[0077]

[0078] 根据公式(3)生成新生节点Xnew，并将新生节点Xnew存入有效节点集Xnodes中，进入步骤6；

[0079] 步骤6：

[0080] 判断Xnew是否满足|Xend‑Xnew|≤radius，是则进入步骤8，否则进入步骤7，其中，|Xend‑Xnew|为计算目标节点Xend和新生节点Xnew的距离公式，radius为范围阈值；

[0081] 步骤7：

[0082] 构建动态区域选择函数：

[0083]

[0084] 式中：δ1为一级距离阈值、δ2为二级距离阈值、δ3为三级距离阈值、δ4为四级距离阈值，Sarea1为一级采样区域、Sarea2为二级采样区域、Sarea3为三级采样区域、Sarea4为四级采样区
域，结合公式(4)确定采样区域Sarea，返回步骤2；

[0085] 步骤8：

[0086] 在有效节点集Xnodes中根据各个节点间的关系，逆行连接有效节点集Xnodes中的各个节点，输出初始路径；

[0087] 步骤9：

[0088] 利用考虑最大转角θ限制的逆向寻优的方法対生成路径进行初步优化，首先判断起始节点Xstart和目标节点Xend间是否存在障碍物，若没有障碍物，则起始节点Xstart和目标
节点Xend间为最佳路径；若存在障碍物，则选取目标节点Xend的前一节点Xnodes1，判断该节点
与起始节点Xstart是否存在障碍物并判断三点间形成的转角是否小于最大转角θ，若符合条
件，则直接将起始节点Xstart作为该节点的父节点，若不符合条件则继续向前寻找节点
Xnodes2，重复上述操作直到完成整条路径优化，生成如图6所示路径并进入步骤10；

[0089] 步骤10：

[0090] 对步骤9生成的路径点进行3次B样条曲线拟合优化路径，得到可实际应用的如图7所示的平滑路径，结束规划。

[0091] 为了进一步验证前述方案的实际效果，下面以Matlab进行仿真实验，具体而言：

[0092] 设定地图规模为500×500，起始节点为(20,20)，终止节点为(480,480)，概率因子ρ＝0.2，目标权重因子ω＝0.8，一级动态步长σ1＝20、二级动态步长σ2＝10、三级动态步长
σ3＝5，车长Dcar＝10，一级安全距离Dsafe1＝20、二级安全距离Dsafe2＝40，区域采样阈值K＝
500，一级距离阈值δ1＝700、二级距离阈值δ2＝500、三级距离阈值δ3＝300、四级距离阈值δ4
＝100、终点距离阈值radius＝40，最大转角θ＝120°。

[0093] 为了验证改进RRT算法的可靠性，对比原始RRT算法、目标偏置RRT算法和概率目标偏置RRT算法，每种算法在地图中重复仿真50次，图2、图3、图4、图5为四种路径规划算法的
剪枝结果图，仿真数据如表1所示。

[0094] 表1四种算法实验数据图

[0095]

[0096] 由表可知，本文改进的RRT算法相较于原始RRT和目标偏置RRT算法有显著的提升，搜索时间减少了78.2％，平均搜索节点数减少了65.8％。相较于概率目标偏置RRT算法平均
搜索节点数减少了34.8％，平均搜索时间减少了35.2％。

[0097] 为了验证路径优化的效果，在地图中重复试验50次，图2、图3、图4分别为动态变采样区域RRT算法剪枝图、动态变采样区域RRT算法逆向寻优图、动态变采样区域RRT算法3次B
样条曲线优化图，经过逆向寻优后的平均路径长度为669.367m，3次B样条曲线拟合后的平
均路径长度为664.392m，经过逆向寻优和3次B样条曲线优化，相比优化前的距离减少
11.4％左右，且路径更加平滑并符合最大转角要求。

[0098] 当不考虑安全距离时，仿真结果如图8所示，规划路径紧靠障碍物，不符合实际应用要求，图2‑7均考虑了安全距离，所以考虑安全距离是有必要的。

[0099] 本发明的基于动态采样区间的概率目标偏置RRT算法有较高的可靠性，进一步扩展应用，结合动态障碍物，可以满足更多使用条件。

[0100] 以上所述具体实施方案，对本发明的发明目的、技术方案和有益效果进行了进一步说明，以上实施例仅用于说明本发明的技术方案，而非对本发明创造保护范围的限制，本
领域的普通技术人员应当理解，凡在本发明的技术方案进行修改、等同替换，均包含在本发
明的保护范围内。