[发明专利]一种考虑车头指向的两阶段多抽样的RRT路径规划方法

说明书

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

技术领域

本发明属于无人车路径规划技术领域，具体涉及一种考虑车头指向的两阶段多抽样的RRT路径规划方法。

背景技术

目前，在以大数据和云计算等新技术推动的新一轮技术革命的背景下，无人驾驶技术正在迅猛发展。无人车的路径规划是指已知车辆的起点、终点以及环境中的障碍物分布，规划出一条从起点至终点的与障碍物不相碰撞的路径，路径规划是无人车技术中的关键技术之一。

为了满足无人车路径规划的特点，相关研究者也进行了大量的工作。Dijkstra算法发明很早，用于最短路径的搜索，但搜索过程中过多无关节点的扩展会极大降低算法效率。A*是一种启发式最优搜索算法，其框架与Dijkstra基本一致，但增加了对当前节点到目标节点最低代价的估计函数，从而使靠近目标节点的节点获得优先扩展机会，但当搜索栅格较多时，需处理的节点数目过多，算法效率不高。基于仿生优化算法(诸如蜂群、蚁群等)的路径规划，通常需要对大量参数进行训练，而这些参数与场景紧密耦合，移植性不强。RRT(Rapid-exploration Random Tree，快速扩展随机树)的基本思想是以产生随机点的方式通过一个固定步长向目标点搜索前进，有效躲避障碍物，避免路径陷入局部极小值，收敛速度快。但是随着采样点数目的增加，树最终会稠密地充满整个状态空间自由区域，算法效率很低，尤其是未考虑车头的指向。对于车辆而言，通常会选择小角度转向而不会采用倒车往反方向运动，因此传统RRT算法往往规划出的路线与实际运动规律不符。

发明内容

本发明的目的在于提供解决已知车辆的起点、终点以及环境中的障碍物分布，在考虑车头指向的条件下，规划出一条从起点至终点的与障碍物不相碰撞的平滑路径的问题的一种考虑车头指向的两阶段多抽样的RRT路径规划方法。

本发明的目的通过如下技术方案来实现：包括以下步骤：

步骤1：初始化地图空间，输入无人车的起点P_init、终点P_goal以及环境中的障碍物分布信息；初始化随机树T、步长L、采样轮次k＝0及采样轮次上限M；设定安全距离SD；其中，以起点P_init作为随机树T的根节点；步长L为无人车每一步运动的长度；采样轮次k初始值为0，每确定一个随机采样点P_rand，采样轮次k的值都加1，当kM时，采样终止；

步骤2：确定随机采样点P_rand进行RRT树节点预扩展；

步骤2.1：设定在地图空间中任意位置随机取点作为一号临时采样点TP₁的概率为p₁₁；设定在地图空间中自起点至终点的线段上随机取点作为一号临时采样点TP₁的概率为p₁₂；执行第一阶段抽样，确定一号临时采样点TP₁；

步骤2.2：查找随机树T中离一号临时采样点TP₁最近的树节点P_near作为本次树节点扩展的原点；

步骤2.3：执行第二阶段抽样；以一号临时采样点TP₁为原点，以随机树T中离一号临时采样点TP₁最近的树节点P_near和一号临时采样点TP₁的连线为横轴，设定常数a，顺时针每隔a°确定一个候选临时采样点CTP_i，共确定360/a个候选临时采样点，构成集合CTP；