一种考虑车头指向的两阶段多抽样的RRT路径规划方法技术

本发明专利技术属于无人车路径规划技术领域，具体涉及一种考虑车头指向的两阶段多抽样的RRT路径规划方法。本发明专利技术以传统RRT路径规划方法作为主体流程，采用两阶段多抽样的随机采样点采集方法，并且考虑安全距离和路径平滑，尤其是考虑了车头的指向，使得本方法更加适合于无人车寻路。本发明专利技术用了两阶段多抽样的方法确定随机采样点，缩短了规划路径的总长度。本发明专利技术在路径规划过程中考虑了车头指向并对规划出的路径做了平滑处理，使输出的规划路径结果更加适合于无人车寻路并且可以缩短规划路径的总长度，得到更加接近实际的行驶路径。

【技术实现步骤摘要】
一种考虑车头指向的两阶段多抽样的RRT路径规划方法
本专利技术属于无人车路径规划
，具体涉及一种考虑车头指向的两阶段多抽样的RRT路径规划方法。
技术介绍
目前，在以大数据和云计算等新技术推动的新一轮技术革命的背景下，无人驾驶技术正在迅猛发展。无人车的路径规划是指已知车辆的起点、终点以及环境中的障碍物分布，规划出一条从起点至终点的与障碍物不相碰撞的路径，路径规划是无人车技术中的关键技术之一。为了满足无人车路径规划的特点，相关研究者也进行了大量的工作。Dijkstra算法专利技术很早，用于最短路径的搜索，但搜索过程中过多无关节点的扩展会极大降低算法效率。A*是一种启发式最优搜索算法，其框架与Dijkstra基本一致，但增加了对当前节点到目标节点最低代价的估计函数，从而使靠近目标节点的节点获得优先扩展机会，但当搜索栅格较多时，需处理的节点数目过多，算法效率不高。基于仿生优化算法(诸如蜂群、蚁群等)的路径规划，通常需要对大量参数进行训练，而这些参数与场景紧密耦合，移植性不强。RRT(Rapid-explorationRandomTree，快速扩展随机树)的基本思想是以产生随机点的方式通过一个固定步长向目标点搜索前进，有效躲避障碍物，避免路径陷入局部极小值，收敛速度快。但是随着采样点数目的增加，树最终会稠密地充满整个状态空间自由区域，算法效率很低，尤其是未考虑车头的指向。对于车辆而言，通常会选择小角度转向而不会采用倒车往反方向运动，因此传统RRT算法往往规划出的路线与实际运动规律不符。
技术实现思路
>本专利技术的目的在于提供解决已知车辆的起点、终点以及环境中的障碍物分布，在考虑车头指向的条件下，规划出一条从起点至终点的与障碍物不相碰撞的平滑路径的问题的一种考虑车头指向的两阶段多抽样的RRT路径规划方法。本专利技术的目的通过如下技术方案来实现：包括以下步骤：步骤1：初始化地图空间，输入无人车的起点Pinit、终点Pgoal以及环境中的障碍物分布信息；初始化随机树T、步长L、采样轮次k＝0及采样轮次上限M；设定安全距离SD；其中，以起点Pinit作为随机树T的根节点；步长L为无人车每一步运动的长度；采样轮次k初始值为0，每确定一个随机采样点Prand，采样轮次k的值都加1，当k>M时，采样终止；步骤2：确定随机采样点Prand进行RRT树节点预扩展；步骤2.1：设定在地图空间中任意位置随机取点作为一号临时采样点TP1的概率为p11；设定在地图空间中自起点至终点的线段上随机取点作为一号临时采样点TP1的概率为p12；执行第一阶段抽样，确定一号临时采样点TP1；步骤2.2：查找随机树T中离一号临时采样点TP1最近的树节点Pnear作为本次树节点扩展的原点；步骤2.3：执行第二阶段抽样；以一号临时采样点TP1为原点，以随机树T中离一号临时采样点TP1最近的树节点Pnear和一号临时采样点TP1的连线为横轴，设定常数a，顺时针每隔a°确定一个候选临时采样点CTPi，共确定360/a个候选临时采样点，构成集合CTP；步骤2.4：从候选临时采样点集合CTP中，剔除掉碰撞障碍物的候选临时采样点，剩余的n个候选临时采样点的集合为S；步骤2.5：从集合S中随机选择二号临时采样点TP2；步骤2.6：以随机树T中离一号临时采样点TP1最近的树节点Pnear为起点，向Pnear和二号临时采样点TP2的连线方向延伸步长L，此线段的终点为此两阶段所确定的随机采样点Prand；步骤2.7：随机树T向随机采样点Prand进行预扩展；步骤3：无人车从随机树T中离一号临时采样点TP1最近的树节点Pnear运行到随机采样点Prand的过程中，判断无人车与障碍物是否无碰撞、无人车与障碍物是否满足安全距离SD、无人车转向角α是否满足α<90°；若三个条件判断结果都为真，则执行步骤4；否则，执行步骤8；步骤4：随机树T向随机采样点Prand进行扩展，将树节点与对应的边添加到随机树T中，同时采样轮次k+1；步骤5：判断终点Pgoal是否包含在随机树T中；若终点Pgoal包含在随机树T中，则执行步骤6；否则，执行步骤8；步骤6：对步骤5输出的路径做平滑处理；步骤7：判断平滑后的路径是否满足安全距离SD；若平滑后的路径满足安全距离SD，则跳执行步骤9；否则，执行步骤8；步骤8：判断当前采样轮次k是否大于采样轮次上限M；若k>M，则判断无符合要求的路径；否则，返回步骤2；步骤9：输出满足要求的平滑路径，完成路径规划。本专利技术还可以包括：所述的步骤3中无人车的转向角α具体为：以随机树T中离一号临时采样点TP1最近的树节点Pnear为原点向其前驱节点Ppioneer方向引出的射线与以树节点Pnear为原点向随机采样点Prand方向引出的射线，两条射线的夹角即为无人车转向角α；若树节点Pnear为起点Pinit，则以树节点Pnear为原点向终点Pgoal方向引出的射线与以树节点Pnear为原点向随机采样点Prand方向引出的射线，两条射线的夹角即为无人车转向角α。本专利技术的有益效果在于：本专利技术以传统RRT路径规划方法作为主体流程，采用两阶段多抽样的随机采样点采集方法，并且考虑安全距离和路径平滑，尤其是考虑了车头的指向，使得本方法更加适合于无人车寻路。本专利技术用了两阶段多抽样的方法确定随机采样点，缩短了规划路径的总长度。本专利技术在路径规划过程中考虑了车头指向并对规划出的路径做了平滑处理，使输出的规划路径结果更加适合于无人车寻路并且可以缩短规划路径的总长度，得到更加接近实际的行驶路径。附图说明图1为本专利技术的总体流程图。图2为本专利技术中两阶段多抽样方法的流程图。具体实施方式下面结合附图对本专利技术做进一步描述。本专利技术属于路径规划领域，主要是一种考虑车头指向的两阶段多抽样的RRT路径规划方法，使输出的规划路径结果更加适合于无人车寻路并且可以缩短规划路径的总长度。该方法以传统RRT路径规划方法作为主体流程，采用两阶段多抽样的随机采样点采集方法，并且考虑安全距离和路径平滑，尤其是考虑了车头的指向，使得本方法更加适合于无人车寻路。本专利技术能降低路径总长度，得到更加接近实际的行驶路径。本专利技术旨在解决已知车辆的起点、终点以及环境中的障碍物分布，在考虑车头指向的条件下，规划出一条从起点至终点的与障碍物不相碰撞的平滑路径的问题。实施例1：一种考虑车头指向的两阶段多抽样的RRT路径规划方法，包括以下步骤：(1)沿用传统的RRT方法初始化地图空间，初始化随机树T、步长L、采样轮次k＝0及采样轮次上限M，将起点Pinit加入到随机树T中；初始化地图空间具体包括：绘制一张图片格式的图像，作为规划的构型空间，为了便于进行碰撞检测，将其二值化。选择起点Pinit和终点Pgoal。初始化随机树T，以起点Pinit作为随机树T的根节点，步长L为无人车每一步运动的长度，本文档来自技高网...

【技术保护点】
1.一种考虑车头指向的两阶段多抽样的RRT路径规划方法，其特征在于，包括以下步骤：/n步骤1：初始化地图空间，输入无人车的起点P

【技术特征摘要】
1.一种考虑车头指向的两阶段多抽样的RRT路径规划方法，其特征在于，包括以下步骤：
步骤1：初始化地图空间，输入无人车的起点Pinit、终点Pgoal以及环境中的障碍物分布信息；初始化随机树T、步长L、采样轮次k＝0及采样轮次上限M；设定安全距离SD；其中，以起点Pinit作为随机树T的根节点；步长L为无人车每一步运动的长度；采样轮次k初始值为0，每确定一个随机采样点Prand，采样轮次k的值都加1，当k>M时，采样终止；
步骤2：确定随机采样点Prand进行RRT树节点预扩展；
步骤2.1：设定在地图空间中任意位置随机取点作为一号临时采样点TP1的概率为p11；设定在地图空间中自起点至终点的线段上随机取点作为一号临时采样点TP1的概率为p12；执行第一阶段抽样，确定一号临时采样点TP1；
步骤2.2：查找随机树T中离一号临时采样点TP1最近的树节点Pnear作为本次树节点扩展的原点；
步骤2.3：执行第二阶段抽样；以一号临时采样点TP1为原点，以随机树T中离一号临时采样点TP1最近的树节点Pnear和一号临时采样点TP1的连线为横轴，设定常数a，顺时针每隔a°确定一个候选临时采样点CTPi，共确定360/a个候选临时采样点，构成集合CTP；
步骤2.4：从候选临时采样点集合CTP中，剔除掉碰撞障碍物的候选临时采样点，剩余的n个候选临时采样点的集合为S；
步骤2.5：从集合S中随机选择二号临时采样点TP2；
步骤2.6：以随机树T中离一号临时采样点TP1最近的树节点Pnear为起点，向Pnear和二号临时采样点TP2的连线方向延伸步长L，此线段的终点为此两阶段所确定的随机采样点Pr...

【专利技术属性】
技术研发人员：吕宏武，陆东浩，李宜杭，王慧强，郭方方，冯光升，李冰洋，
申请(专利权)人：哈尔滨工程大学，
类型：发明
国别省市：黑龙江;23