[发明专利]基于双向扩展随机树的智能车路径规划方法

说明书

本发明公开了一种基于双向扩展随机树的智能车路径规划方法，其包括生成曲线，当曲线上有障碍物时构建起点相反的两棵扩展树；在地图上生成随机点，并查找扩展树距离随机点最近的节点；判断随机点与节点的连线上是否有障碍物；若有，则重新生成随机点，若无，在随机点与节点的连线上取一个新节点；判断最近的节点与新节点之间的连线上是否有障碍物，若有，则重新生成随机点，否则根据障碍物安全距离约束更新节点；之后根据智能车辆最大转向约束及节点，选取节点的父节点和子节点；判断两棵扩展树之间的距离是否小于设定阈值，若大于，则重新生成随机点，否则，根据两扩展树节点反向选取节点到起始点和目标点的路径构成智能车规划路径。

技术领域

本发明涉及物体运动路径的规划，具体涉及一种基于双向扩展随机树的智能车路径规划方法。

背景技术

在2001年Kuffner和LaValle提出并行生成随机树(RRT-Connect)的方法，以提高该算法的收敛速度。在2010年Karaman和Frazzoli提出RRT*算法，用于解决RRT算法产生的路径并非概率最优解的问题，但其运行效率较低。在2013 年，M.Jordan和A.Perez提出B-RRT*，用RRT*算法进行起始点和目标点的双向扩展，以减少RRT*算法所执行的时间，其应用于移动机器人中，B-RRT*虽然能够缩短一部分执行时间，但是其仍存在如下缺点：

(1)在运行过程中，使用全局均匀随机采样，使得算法运行时间增加，收敛的速度变慢；

(2)采用最近节点选择算法进行节点选择，容易导致其在复杂场景时所规划的路径失败；

(3)所规划的路径没有考虑车辆运动学约束，使得规划的路径不能运用于智能车的路径规划中。

发明内容

针对现有技术中的上述不足，本发明提供的基于双向扩展随机树的智能车路径规划方法，其规划的路径安全合理且更适用于车辆运动学特性。

为了达到上述发明目的，本发明采用的技术方案为：

提供一种基于双向扩展随机树的智能车路径规划方法，其包括：

S1、采用Reeds-Shepp曲线连接智能车的起始点和目标点；

S2、当生成的曲线上存在障碍物时，从起始点向目标点构建扩展树tree1，从目标点向起始点构建扩展树tree2；

S3、在包含智能车起始点和目标点的地图上生成随机点q_rand1/q_rand2，并查找扩展树tree1/tree2距离随机点q_rand1/q_rand2最近的节点q_nearrest1/q_nearrest2；

S4、判断随机点q_rand1/q_rand2与节点q_nearrest1/q_nearrest2的连线上是否有障碍物；若有，则返回步骤S3，否则进入步骤S5；

S5、在随机点q_rand1/q_rand2与节点q_nearrest1/q_nearrest2的连线上取位于一个扩展步长长度处的节点q_new1/q_new2；

S6、判断节点q_nearrest1/q_nearrest2与节点q_new1/q_new2的连线上是否有障碍物；若有，则返回步骤S3，否则，进入步骤S7；