CN109990796B

CN109990796B - 基于双向扩展随机树的智能车路径规划方法

Info

Publication number: CN109990796B
Application number: CN201910328124.5A
Authority: CN
Inventors: 付克昌; 杨瑶; 蒋涛; 周楠; 郭德全; 张国良; 刘甲甲; 袁建英; 谢昱锐
Original assignee: Chengdu University of Information Technology
Current assignee: Chengdu University of Information Technology
Priority date: 2019-04-23
Filing date: 2019-04-23
Publication date: 2022-07-19
Anticipated expiration: 2039-04-23
Also published as: CN109990796A

Abstract

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

Description

基于双向扩展随机树的智能车路径规划方法

技术领域

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

背景技术

在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；

S7、选取以节点q_new1/q_new2所在区域内使得规划路径与障碍物之间的距离大于安全距离的节点更新节点q_new1/q_new2；

S8、根据智能车辆最大转向约束及节点q_new1/q_new2，选取节点q_new1/q_new2的父节点和子节点；

S9、判断扩展树tree1和扩展树tree2之间的距离是否小于设定阈值，若大于，则返回步骤S3，否则，进入步骤S10；

S10、根据扩展树tree1和扩展树tree2相遇的节点q_a和q_b，反向选取节点 q_a和q_b分别到起始点和目标点的路径构成智能车规划路径。

进一步地，在包含智能车起始点和目标点的地图上生成随机点q_rand1/q_rand2进一步包括：

S31、采用随机函数生成一个采样概率，并判断采样概率是否大于目标偏向概率，若大于，则进入步骤S32，否则，进入步骤S33；

S32、采用目标点/起始点作为扩展树tree1/扩展树tree2的随机点q_rand1/随机点q_rand2；

S33、在扩展树tree1/扩展树tree2中寻找与目标点/起始点最近的节点q_nr1/ 节点q_nr2，以节点q_nr1/节点q_nr2为中心，以扩展步长为边长的方形区域，并判断方形区域内是否有障碍物；

S34、若没有障碍物，则采用节点q_nr1/节点q_nr2建立滑动窗口，并在滑动窗口中生成q_rand1/随机点q_rand2；

S35、若有障碍物，则返回步骤S31。

进一步地，选取以节点q_new1/q_new2所在区域内使得规划路径与障碍物之间的距离大于安全距离的节点更新节点q_new1/q_new2进一步包括：

S71、形成一块以节点q_new1/q_new2为中心，扩展步长为1/2边长的方形区域 A1/A2，并同时初始化规划路径与障碍物之间的最小距离d_min为无穷大；

S72、选取方形区域A1/A2中未选取过的节点poses1/poses2，并判断节点poses1/poses2是否为障碍物；若是，执行步骤S72，否则进入步骤S73；

S73、以方形区域A1/A2中的每个节点poses1/poses2为中心，以扩展步长为边长，形成区域B1/B2；

S74、选取区域B1/B2中未选取过的节点poses11/poses21，并判断节点 poses11/poses21是否为障碍物；若是，返回步骤S72，否则进入步骤S75；

S75、计算节点poses11/poses21到目标点的距离d，并判断距离d是否小于最小距离d_min；若是，进入步骤S76，否则返回步骤S72；

S76、采用距离d更新最小距离d_min，采用距离d对应的节点poses1/poses2 更新节点q_new1/q_new2；

S77、判断方形区域A1/A2，是否存在未选取过得节点；若有，则执行S72；否则，输出节点q_new1/q_new2。

进一步地，根据智能车辆最大转向约束及节点q_new1/q_new2，选取节点q_new1/ q_new2的父节点进一步包括：

S811、选取以节点q_new1/q_new2为中心，预设半径R为半径所覆盖区域内的所有节点为待选父节点q_parent1/q_parent2；

S812、选取未选取的待选父节点q_parent1/q_parent2，计算待选父节点q_parent1/ q_parent2、待选父节点q_parent1/q_parent2的父节点和节点q_new1/q_new2所形成角度

S813、判断角度

是否小于车辆最大转角

若是，更新角度

对应待选父节点q_parent1/q_parent2的代价值为∞，并进入步骤S812；否则，进入步骤S814；

S814、计算角度

对应的待选父节点q_parent1/q_parent2的代价值：

C_parent＝A*e^d1

其中，C_parent为选取待选父节点的代价值；A为常数；d1为树根到q_new1/q_new2的距离，e为自然对数；

S815、判断所有的待选父节点q_parent1/q_parent2是否存在未被选取的待选父节点q_parent1/q_parent2；若存在，则返回步骤S812，否则进入步骤S816；

S816、选取代价值最小、且其所在区域内无障碍物的待选父节点q_parent1/ q_parent2作为节点q_new1/q_new2的父节点。

进一步地，根据智能车辆最大转向约束及节点q_new1/q_new2，选取节点q_new1/ q_new2的子节点进一步包括：

S821、选取以节点q_new1/q_new2为中心，预设半径R为半径所覆盖区域内的所有节点为待选子节点q_chlid1/q_chlid2；

S822、选取未选取的待选子节点q_chlid1/q_chlid2，计算节点q_new1/q_new2、待选父节点q_new1/q_new2的父节点和待选子节点q_chlid1/q_chlid2所形成角度

S823、判断角度

是否小于车辆最大转角

若是，更新角度

对应待选子节点q_chlid1/q_chlid2的代价值为∞，并进入步骤S822；否则，进入步骤S824；

S824、计算角度

对应的待选子节点q_chlid1/q_chlid2的代价值：

C_child＝B*e^d2

其中，C_child为待选子节点的代价值；B为常数；d2为树根到待选子节点q_chlid1/q_chlid2的距离，e为自然对数；

S825、判断所有的待选子节点q_chlid1/q_chlid2是否存在未被选取的待选子节点q_chlid1/q_chlid2；若存在，则返回步骤S822，否则进入步骤S826；

S826、选取代价值最小、且其所在区域内无障碍物的待选子节点q_chlid1/q_chlid2作为节点q_new1/q_new2的子节点。

进一步地，基于双向扩展随机树的智能车路径规划方法还包括对所述智能车规划路径进行优化处理：

A1、在智能车规划路径上取出两个未同时选取过的路径点，并计算两个路径点间的距离d₁；

A2、判断离d₁是否大于扩展步长，若是，则进入步骤A3；否则，返回步骤 A1；

A3、在两个路径点的连线上，取距离起点以扩展步长为长度的点作为路径点pose_new；

A4、选取以路径点pose_new所在区域内使得规划路径与障碍物之间的距离大于安全距离的节点更新路径点pose_new，之后返回步骤A1。

进一步地，基于双向扩展随机树的智能车路径规划方法还包括采用贝塞尔曲线对智能车路径进行拟合以得到智能车最终的规划路径。

进一步地，障碍物判断时，采用地图上像素点的灰度值进行确定。

本发明的有益效果为：本方案提供的智能车路径规划方法克服了现有技术中基本的RRT、RRT*、B-RRT*算法存在转折点较多、路线不平滑、规划路径与障碍物过近和曲率不连续等问题，使得所规划的路径能更加满足车辆的运动学模型。采用Reeds-Shepp曲线连接智能车的起始点和目标点，能够解决目标点朝向的问题。

附图说明

图1为基于双向扩展随机树的智能车路径规划方法的流程图。

图2为路径规划示意图，其中(a)为起始点朝向与目标点朝向相同且存在转折的情况，(b)为起始点朝向与目标点朝向垂直的情况。

图3为采样概率小于等于目标偏向概率时，随机点的选取示意图。

图4为节点周围/所在区域内节点的选取示意图。

图5为父节点的选择对比示意图，其中(a)为选择父节点初的示意图；(b) 为B-RRT*算法选择父节点后的示意图；(c)为本方案选择父节点后的示意图。

图6为子节点的选择对比示意图，其中(a)为选择子节点初的示意图；(b) 为B-RRT*算法选择子节点后的示意图；(c)为本方案选择子节点后的示意图。

图7为使用贝塞尔处理过规划路径的示意图。

图8为pose_new周围节点选取示意图。

图9为在某学校的校园地图。

图10为在图9内的区域一中生成的规划路径示意图，其中(a)为RRT算法在区域一中生成的规划路径示意图；(b)为RRT*算法在区域一中生成的规划路径示意图；(c)为B-RRT*算法在区域一中生成的规划路径示意图；(d)为采用本方案方法在区域一中生成的规划路径示意图。

图11为在图9内的区域二中生成的规划路径示意图，其中(a)为RRT算法在区域二中生成的规划路径示意图；(b)为RRT*算法在区域二中生成的规划路径示意图；(c)为B-RRT*算法在区域二中生成的规划路径示意图；(d)为采用本方案方法在区域二中生成的规划路径示意图。

具体实施方式

下面对本发明的具体实施方式进行描述，以便于本技术领域的技术人员理解本发明，但应该清楚，本发明不限于具体实施方式的范围，对本技术领域的普通技术人员来讲，只要各种变化在所附的权利要求限定和确定的本发明的精神和范围内，这些变化是显而易见的，一切利用本发明构思的发明创造均在保护之列。

参考图1，图1示出了基于双向扩展随机树的智能车路径规划方法的流程图；如图1所示，该方法S包括步骤S1至步骤S10。

在步骤S1中，采用Reeds-Shepp曲线连接智能车的起始点和目标点；具体参见图2，图中颜色较浅且较细的黑色路径为使用Reeds-Shepp曲线预处理生成的路径，颜色较深且较粗的黑色路径为B-RRT*算法规划的路径。从图2中可以看出使用Reeds-Shepp曲线能够有效解决车辆朝向的问题，并且所生成的轨迹也更加平滑。

在步骤S2中，当生成的曲线上存在障碍物时，从起始点向目标点构建扩展树tree1，从目标点向起始点构建扩展树tree2。当生成的曲线上不存在障碍物时，输出该曲线作为智能车的规划路径，并结束规划。

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

在本发明的一个实施例中，在包含智能车起始点和目标点的地图上生成随机点q_rand1/q_rand2进一步包括步骤S31至步骤S35。

在步骤S31中，采用随机函数生成一个采样概率，并判断采样概率是否大于目标偏向概率，若大于，则进入步骤S32，否则，进入步骤S33；其中，采样概率小于等于目标偏向概率时，随机点的选取可以参考图3。

在步骤S2中，采用目标点/起始点作为扩展树tree1/扩展树tree2的随机点 q_rand1/随机点q_rand2；

在步骤S33中，在扩展树tree1/扩展树tree2中寻找与目标点/起始点最近的节点q_nr1/节点q_nr2，以节点q_nr1/节点q_nr2为中心，以扩展步长为边长的方形区域，并判断方形区域内是否有障碍物；

在步骤S34中，若没有障碍物，则采用节点q_nr1/节点q_nr2建立滑动窗口，并在滑动窗口中生成q_rand1/随机点q_rand2；其中滑动窗口中的窗口长度优选为智能车的宽度。

在步骤S35中，若有障碍物，则返回步骤S31。

采用本方案的方式是对随机点q_rand1/q_rand2的选取进行约束，可以避免采样的随机性，从而避免了采样带来的不确定因素。

在步骤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；

在步骤S7中，选取以节点q_new1/q_new2所在区域内使得规划路径与障碍物之间的距离大于安全距离的节点更新节点q_new1/q_new2。

在本发明的一个实施例中，选取以节点q_new1/q_new2所在区域内使得规划路径与障碍物之间的距离大于安全距离的节点更新节点q_new1/q_new2进一步包括步骤 S71至步骤S76。

在步骤S71中，形成一块以节点q_new1/q_new2为中心，扩展步长为1/2边长(扩展步长的长度等于方形区域A1/A2的边长的一半)的方形区域A1/A2，并同时初始化规划路径与障碍物之间的最小距离d_min为无穷大；

在步骤S72中，选取方形区域A1/A2中未选取过的节点poses1/poses2，并判断节点poses1/poses2是否为障碍物；若是，执行步骤S72，否则进入步骤S73；

在步骤S73中，以方形区域A1/A2中的每个节点poses1/poses2为中心，以扩展步长为边长，形成区域B1/B2；

其中，节点poses1/poses2周围或所在区域(方形区域B1/B2)内节点的选取参考图4，poses1＝{A1,B1,C1,D1},poses11＝{A1¹,A1²,A1³,A1⁴,B1¹,B1²,B1³,B1⁴,C1¹,C1²,C1³,C1⁴, D1¹,D1²,D1³,D1⁴})，扩展步长为车辆宽度的一半，图4中的D为扩展步长。

在步骤S74中，选取区域B1/B2中未选取过的节点poses11/poses21，并判断节点poses11/poses21是否为障碍物；若是，返回步骤S72，否则进入步骤S75；

在步骤S75中，计算节点poses11/poses21到目标点的距离d，并判断距离d 是否小于最小距离d_min；若是，进入步骤S76，否则返回步骤S72；

在步骤S76中，采用距离d更新最小距离d_min，采用距离d对应的节点poses1/poses2更新节点q_new1/q_new2。

在步骤S77中，判断方形区域A1/A2，是否存在未选取过得节点。若有，则执行S72。否则，输出节点q_new1/q_new2。

在步骤S8中，根据智能车辆最大转向约束及节点q_new1/q_new2，选取节点q_new1/ q_new2的父节点和子节点。

在本发明的一个实施例中，根据智能车辆最大转向约束及节点q_new1/q_new2，选取节点q_new1/q_new2的父节点进一步包括：

S813、判断角度

是否小于车辆最大转角

若是，更新角度

S814、计算角度

对应的待选父节点q_parent1/q_parent2的代价值：

C_parent＝A*e^d1

本方案的父节点的选取可以参考图5中的(a)和(c)，图5中的(a)是以R 为半径⑨为圆心的重选选择父节点的过程，图5的(b)是使用基本B-RRT*算法执行重选父节点的结果(

→⑤→⑧→⑨其路径的长度为11)，图5的(c)是采用本方案选择的父节点的结果(

→④→⑨其路径的长度为14)。

通过图5的(b)和(c)中可知，虽然(b)的路径比(c)短，但是

(

为路径⑤→⑧→⑨的角度)不满足路径转角小于智能车最大转角的要求，所以放弃节点⑧作为节点⑨的父点。

实施时，本方案优选根据智能车辆最大转向约束及节点q_new1/q_new2，选取节点q_new1/q_new2的子节点进一步包括：

S823、判断角度

是否小于车辆最大转角

若是，更新角度

S824、计算角度

对应的待选子节点q_chlid1/q_chlid2的代价值：

C_child＝B*e^d2

其中，C_child为待选子节点的代价值；B为常数；d2为树根到待选子节点_qchlid1/q_chlid2的距离，e为自然对数；

本方案的子节点选取过程(重布随机树过程)可以参考图6，其中(a)为新生成的节点q_new⑨为圆心，R为半径并以④为父节点，圆内其他的节点为子节点(q_new的父节点除外)。图6的(a)表示基本B-RRT*算法重新选择子节点的结果， (b)表为基本B-RRT*算法重新布线的结果(

→④→⑨→⑥其路径为15)；(c)采用本方案的子节点选取方法重新布线的结果(

④→⑨→⑩其路径为16)。

通过图6的(b)和(c)中可知虽然(b)的路径比(c)短，但是

(

为路径④→⑨→⑥的角度)不满足路径转角小于智能车最大转角的要求，所以放弃节点⑥作为节点⑨的子节点。

在步骤S9中，判断扩展树tree1和扩展树tree2之间的距离是否小于设定阈值，若大于，则返回步骤S3，否则，进入步骤S10；

在本发明的一个实施例中，基于双向扩展随机树的智能车路径规划方法还包括对所述智能车规划路径进行优化处理：

实施时，本方案优选选取以路径点pose_new所在区域内使得规划路径与障碍物之间的距离大于安全距离的节点更新路径点pose_new进一步包括：

1)首先获得节点pose_new，初始化d_min(初始化定义d_min等于无穷大)。

2)获得pose_new四周的节点poses111，依次从中选取pose并判断是否为障碍物。如果是，则从poses111中从新选取pose。否则在pose节点四周添加poses22，如图8所示，poses111＝{A1₁,B1₁,C1₁,D1₁},poses22＝{A1₁ ¹,A1₁ ²,A1₁ ³,A1₁ ⁴,B1₁ ¹,B1₁ ²,B1₁ ³,B1₁ ⁴, C1₁ ¹,C1₁ ²,C1₁ ³,C1₁ ⁴,D1₁ ¹,D1₁ ²,D1₁ ³,D1₁ ⁴}；

3)在poses22中依次选择pose1，并判断poses1是否为障碍物。如果是，则执行b)。否则，计算pose到目标点距离d。

4)判断d和d_min大小，如果d_min>d，更新pose_new和d_min。否则执行b)。

5)输出pose_new。

实施时，本方案优选基于双向扩展随机树的智能车路径规划方法还包括采用贝塞尔曲线对智能车路径进行拟合以得到智能车最终的规划路径。采用贝塞尔曲线进行优化可以参考图7，优化过程的公式为：

B(t)＝(1-t)²*P₀+2*t(1-t)*P₁+t²*P₂t∈[0,1]

其中，P₀，P₁，P₂为二阶贝塞尔曲线的控制点。

本方案在对规划过程中的步骤S3至步骤S8中采用的符号“/”表示或的意思，且在对应过程中“/”前面部分对应所有“/”前面部分内容，“/”后面部分对应所有“/”后面部分内容。

下面以步骤S3至步骤S8为例对“/”前面和后面的内容进行分开说明：

对于扩展树tree1上节点实现步骤S3至S8的处理过程如下：

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

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

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

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

S7、选取以节点q_new1所在区域内使得规划路径与障碍物之间的距离大于安全距离的节点更新节点q_new1；

S8、根据智能车辆最大转向约束及节点q_new1，选取节点q_new1的父节点和子节点。

对于扩展树tree2上节点实现步骤S3至S8的处理过程如下：

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

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

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

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

S7、选取以节点q_new2所在区域内使得规划路径与障碍物之间的距离大于安全距离的节点更新节点q_new2；

S8、根据智能车辆最大转向约束及节点q_new2，选取节点q_new2的父节点和子节点。

本方案的扩展树tree1和扩展树tree2在节点是同步实现步骤S3至步骤S8 的，本方案每个步骤中“/”前面后面内容跟上面步骤S3至步骤S8是类似的，本方案就不在对每个步骤进行单独说明。

下面结合具体的实例，对本方案提供的智能车路径规划方法的效果进行说明：

分别将RRT、RRT*、B-RRT*和本方案算法应用于某大学学校园的栅格地图 (4846×2816)上(如图9所示)，栅格大小为0.298米/pixel，选择不同的地点(此处的不同地点指代图9中的区域一和区域二)进行试验。实验基于ROS(开源机器人操作***)平台，并使用Rviz对规划结果进行可视化显示，计算机配置为:ubuntu14.04LTS，处理器intel i5-6500，主频为3.2Ghz，运行内存为8G。

本次实验通过在栅格地图上选择起始点和终点，使用RRT、RRT*、B-RRT* 和本方案算法规划。通过路径的长度、到障碍物的距离和转折数量(

则认为是转折)等方式来评价路径的好坏。

对区域一和区域二进行路径规划，规划结果见图10和图11。其中，表1和表2分别为连续转折区域、直角转折曲线区域的指标对比。

图10和图11中的方形节点为起始点向目标点的采样节点，圆形节点为目标点向起始点的采样节点。

从图10和图11可以看出，本方案算法所规划出来的路径较为平滑，且具有连续的曲率，能够解决基本RRT、RRT*、B-RRT*算法在初始时刻路径不合理的问题，同时与障碍物形成一定的安全距离并且能够较好的满足车辆运动学特性。

表1区域一测试对比

表2区域二测试对比

通过表1和表2能够看到虽然基本RRT、RRT*、B-RRT*算法能够在较短的时间内规划出路径，但是所规划的路径转折角度过大、路径点贴合障碍物，导致路径不能被智能车使用。而本方案算法在牺牲较短时间的基础上获得了更加平滑、安全且更符合车辆运动学的路径。

Claims

1.基于双向扩展随机树的智能车路径规划方法，其特征在于，包括：

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

S10、根据扩展树tree1和扩展树tree2相遇的节点q_a和q_b，反向选取节点q_a和q_b分别到起始点和目标点的路径构成智能车规划路径；

选取以节点q_new1/q_new2所在区域内使得规划路径与障碍物之间的距离大于安全距离的节点更新节点q_new1/q_new2进一步包括：

S71、形成一块以节点q_new1/q_new2为中心，扩展步长为1/2边长的方形区域A1/A2，并同时初始化规划路径与障碍物之间的最小距离d_min为无穷大；

S74、选取区域B1/B2中未选取过的节点poses11/poses21，并判断节点poses11/poses21是否为障碍物；若是，返回步骤S72，否则进入步骤S75；

S76、采用距离d更新最小距离d_min，采用距离d对应的节点poses1/poses2更新节点q_new1/q_new2；

2.根据权利要求1所述的基于双向扩展随机树的智能车路径规划方法，其特征在于，在包含智能车起始点和目标点的地图上生成随机点q_rand1/q_rand2进一步包括：

S33、在扩展树tree1/扩展树tree2中寻找与目标点/起始点最近的节点q_nr1/节点q_nr2，以节点q_nr1/节点q_nr2为中心，以扩展步长为边长的方形区域，并判断方形区域内是否有障碍物；

S35、若有障碍物，则返回步骤S31。

3.根据权利要求1所述的基于双向扩展随机树的智能车路径规划方法，其特征在于，根据智能车辆最大转向约束及节点q_new1/q_new2，选取节点q_new1/q_new2的父节点进一步包括：

S812、选取未选取的待选父节点q_parent1/q_parent2，计算待选父节点q_parent1/q_parent2、待选父节点q_parent1/q_parent2的父节点和节点q_new1/q_new2所形成角度

S813、判断角度

是否小于车辆最大转角

若是，更新角度

S814、计算角度

对应的待选父节点q_parent1/q_parent2的代价值：

C_parent＝A*e^d1

S816、选取代价值最小、且其所在区域内无障碍物的待选父节点q_parent1/q_parent2作为节点q_new1/q_new2的父节点。

4.根据权利要求3所述的基于双向扩展随机树的智能车路径规划方法，其特征在于，根据智能车辆最大转向约束及节点q_new1/q_new2，选取节点q_new1/q_new2的子节点进一步包括：

S823、判断角度

是否小于车辆最大转角

若是，更新角度

S824、计算角度

对应的待选子节点q_chlid1/q_chlid2的代价值：

C_child＝B*e^d2

5.根据权利要求1所述的基于双向扩展随机树的智能车路径规划方法，其特征在于，还包括对所述智能车规划路径进行优化处理：

A2、判断离d₁是否大于扩展步长，若是，则进入步骤A3；否则，返回步骤A1；

6.根据权利要求1-5任一所述的基于双向扩展随机树的智能车路径规划方法，其特征在于，还包括采用贝塞尔曲线对智能车路径进行拟合以得到智能车最终的规划路径。

7.根据权利要求1-5任一所述的基于双向扩展随机树的智能车路径规划方法，其特征在于，障碍物判断时，采用地图上像素点的灰度值进行确定。