CN113219998B - 一种基于改进双向informed-RRT*的车辆路径规划方法 - Google Patents

Abstract

本发明公开了一种基于改进双向informed‑RRT*的车辆路径规划方法，包括：1建立栅格地图，确定起始点和目标点；2利用组合扩展策略并融合双向RRT*算法进行采样，获取初始路径；3利用改进双向informed‑RRT*算法获得椭圆状态子集空间，获取采样点；4利用目标引力融合双向RRT*算法对初始路径进行优化，得到优化路径，优化路径的长度更新椭圆区域，在更新后的椭圆区域中采样，获得采样点；5重复步骤4并得到最终路径；6对最终路径进行剪枝后，利用四阶贝塞尔曲线进行平滑处理。本发明降低采样随机性、提高收敛效率，生成的路径最优且满足车辆动力学约束，从而能使车辆成功避开障碍物快速到达目标点。

Description

一种基于改进双向informed-RRT*的车辆路径规划方法

技术领域

本发明涉及一种基于改进双向informed-RRT*的车辆路径规划方法，属于车辆路径规划领域。

背景技术

目前，随着无人驾驶技术的迅猛发展，路径规划作为无人驾驶车辆的关键技术之一越来越受到重视。无人驾驶车辆的路径规划是在一定的环境模型基础上，给定起始点和目标点，成功规划处一条起始点到目标点的自由空间中的无碰撞可行路径。

传统的路径规划算法包括基于构造势场的人工势场法、遗传算法、基于图搜索的Dijkstra和A*等，但是以上算法都需要在一个确定的空间内对障碍物建模，不适合在复杂环境中进行路径规划。快速探索随机树(RRT)使用随机采样的方式，从而避免对状态空间进行建模，能够有效地解决高维空间和复杂约束的路径规划问题。

传统的双向informed-RRT*从初始位置和目标位置同时生成两颗快速扩展随机树来搜索状态空间，生成一条能够有效避开障碍物的路径，是一种该算法在利用RRT*-connect找到初始解之后，立即生成由起点、目标点、当前路径长度决定的椭圆状态空间区域，之后进行将采样范围控制在椭圆状态空间子集内，加速收敛到最优解。但是该算法规划初始路径时，存在很多对空白区域的无用搜索，浪费搜索时间，规划效率低，且规划的路径无法满足车辆行驶要求，从而无法满足车辆快速有效的到达目标点。

发明内容

本发明是为了解决上述现有技术存在的不足之处，提出一种基于改进双向informed-RRT*的车辆路径规划方法，以期能降低采样随机性、提高收敛效率，规划的路径更短且满足车辆动力学约束，从而能使车辆成功避开障碍物并快速到达目标点。

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

本发明一种基于改进双向informed-RRT*的车辆路径规划方法的特点是按如下步骤进行：

步骤1、利用车辆自带的激光雷达传感器或深度相机传感器采集车辆周围的环境信息，并用于建立栅格地图，将所述栅格地图中的每一个栅格标记为障碍空间或自由空间，在标记后的栅格地图中选择一起始点x_start和目标点x_goal；

步骤2、采用目标约束和目标引力相结合的组合扩展策略并融合传统双向RRT*算法在栅格地图的自由空间中进行采样，以获取初始路径；

步骤3、获得初始路径后，利用改进双向informed-RRT*算法获得启发式子集，以确定标记后的栅格地图中的椭圆状态子集空间，从而在椭圆状态子集空间中进行采样，获取随机采样点x_rand；

步骤4、根据随机采样点x_rand，利用目标引力方法并融合传统双向RRT*算法对初始路径进行优化，从而得到渐近最优性路径，根据渐近最优性路径的长度更新椭圆区域，从而在更新后的椭圆区域中进行采样，获得更新后的随机采样点；

步骤5、重复步骤4的过程，直到达到迭代次数为止，从而得到最终规划的路径；

步骤6、对最终规划的路径进行剪枝处理后，再利用四阶贝塞尔曲线进行平滑处理，从而得到一条符合车辆约束的有效路径。

本发明所述的一种基于改进双向informed-RRT*的车辆路径规划方法的特点也在于，所述步骤2包括：

步骤2.1、定义两颗快速探索随机树Ta和Tb；

步骤2.2、获取快速探索随机树Ta的新节点x_{new_a}，以向目标点x_goal扩展；

步骤2.2.1、设置一个目标偏置概率ρ_bias；

步骤2.2.2、在当前次随机获取采样点时，按照均匀概率随机产生一个当前概率值ρ，若当前概率值ρ大于目标偏置概率ρ_bias，则在自由空间中随机产生一个当前采样点x_{rand_a}，否则将目标点x_goal作为当前采样点x_{rand_a}；

步骤2.2.3、遍历整个快速探索随机树Ta，并利用式(1)所示的对角线距离计算公式得到当前采样点x_{rand_a}在快速探索随机树Ta中的最近邻节点x_{near_a}：

式(1)中，x0为快速探索随机树Ta中当前子节点的x坐标值，y0为快速探索随机树Ta中当前子节点的y坐标值，x1为与快速探索随机树Ta中当前子节点相比较的当前采样点x_{rand_a}的x坐标值，y1与快速探索随机树Ta中当前子节点相比较的当前采样点x_{rand_a}的y坐标值；

步骤2.2.4、利用式(2)得到新节点x_{new_a}，如果新节点x_{new_a}和最近邻节点x_{near_a}之间的线段在自由空间中，则将新节点x_{new_a}插入到快速探索随机树Ta中，并执行步骤2.2.5；否则丢弃新节点x_{new_a}，并返回步骤2.2.2重复执行；

式(2)中：k_p为引力场系数，p为步长，x_{goal_ab}为目标位置向量，即目标点x_goal，||x_{goal_ab}-x_{near_a}||为目标位置向量与最近邻节点x_{near_a}距离的绝对值；

步骤2.2.5、以新节点x_{new_a}为中心，r为半径范围内搜索所述快速探索随机树Ta，以确定邻域集合X_{near_a}，将从根节点到邻域集合X_{near_a}的路径以及邻域集合X_{near_a}与新节点x_{new_a}之间无碰撞路径的代价之和，与从根节点经过最近邻节点x_{near_a}到达新节点x_{new_a}的路径的代价进行比较，将路径代价最小的节点作为新节点x_{new_a}的最佳父节点，并将最佳父节点与新节点x_{new_a}之间路径以及新节点x_{new_a}插入所述快速探索随机树Ta中；

步骤2.2.6、邻域集合X_{near_a}中每个近邻节点均尝试用新节点x_{new_a}代替邻域集合X_{near_a}中自身近邻节点原先的父节点，若代替后，从起始点x_start沿所述快速探索随机树Ta到对应近邻节点的累积成本比代替前更小，则对应近邻节点放弃原先的父节点，并将新节点x_{new_a}作为父节点后重新连接到近邻节点上；否则，不替换原先的父节点；

步骤2.3、通过新节点x_{new_a}将扩展后的快速探索随机树Ta与快速探索随机树Tb相连接，找到快速探索随机树Tb中与新节点x_{new_a}距离最近的邻节点x_{near_b}，如果x_{new_a}和x_{near_b}之间的距离达到两棵树的连接阈值，则表示两棵快速探索随机树Ta与Tb连接成功，并得到所述初始路径；否则；快速探索随机树Tb按照步骤2.2的过程向初始点x_start方向扩展，其中，在步骤2.2.2中是将起始点x_start作为采样点x_{rand_b}，在步骤2.2.3中的目标位置向量x_{goal_ab}为初始点x_start；若在步骤2.3的过程能得到初始路径；则执行步骤3，否则，快速探索随机树Ta再按照步骤2.2-步骤2.3的过程向目标点x_goal扩展和连接。

所述步骤3包括：

步骤3.1、根据所述初始路径，在当前次迭代中以初始点x_start和目标点x_goal为焦点，路径成本c_best为长轴，c_min为初始点x_start和目标点x_goal的距离，短轴为

构建当前次迭代中的椭圆状态子集空间；

步骤3.2、通过式(3)得到当前次迭代中的椭圆状态子集X_f，从而在当前次迭代中的椭圆状态子集中直接采样获得当前采样点x_rand：

X_f＝CLx_ball+x_centre (3)

式(3)中：C表示旋转变换矩阵，L表示横轴的对角线矩阵，x_ball表示单位圆中的均匀采样点，x_centre是椭圆的中心，并有：

C＝Udiag{1,...,1,det(U)det(V)}V^T (5)

式(5)中：det(.)表示行列式，

是通过对约束矩阵UΣV^T≡M奇异值分解得到的酉矩阵，且

其中，a₁表示世界坐标系的横轴，并通过式(7)计算，e₁表示单位矩阵第一列之外积；

所述步骤4包括：

步骤4.1、以先扩展快速探索随机树Ta；

步骤4.1.1、根据当前采样点x_rand，按照步骤2.2.3-步骤2.2.4的过程，最新节点x_new；如果最近邻节点x_near与最新节点x_new的线段在自由空间中，则将x_new作为备选节点，并执行步骤4.1.2；否则，丢弃最新节点x_new，并返回步骤3.1和步骤3.2；

步骤4.1.2、按照步骤2.2.5和步骤2.2.6的过程，更新快速探索随机树Ta；

步骤4.1.3、若成本之和c_new小于当前次迭代的路径成本c_best，则将最新节点x_new插入到快速探索随机树Ta中，从而得到当前次迭代的渐近最优性路径，并执行步骤4.2；否则，丢弃最新节点x_new后，返回步骤3.1、步骤3.2和步骤4.1；其中，c_new为起始点x_start到备选节点x_new的无障碍路径c(x_new,x_start)的成本与备选节点x_new到目标点x_goal的无障碍路径c(x_new,x_goal)的成本之和；

步骤4.2、扩展快速探索随机树Tb；

步骤4.2.1、用c_new代替c_best以更新当前次迭代的椭圆状态子集，以当前次迭代的渐近最优性路径代替初始路径后，重复步骤3.1、步骤3.2和步骤4.1执行。

与现有技术相比，本发明的有益效果在于：

1、本发明采用一种基于目标约束采样和目标引力思想相结合的组合扩展策略，降低了双向informed-RRT*算法采样的随机性，从而在车辆路径规划过程中能快速规划出最优路径；

2、本发明通过优化距离度量函数，提高了随机树的搜索效率，从而在车辆路径规划中规划路径时间短，路径规划效率高；

3、本发明将规划的路径进行剪枝处理，减少了时间的消耗，减少了规划的路径长度，针对阿克曼转角类型的车辆在行驶过程中受非完整约束，对该路径中采样节点进行贝塞尔曲线相拟合，使得规划的路径更加平稳与光滑，生成符合车辆动力学、起始状态、目标状态及曲率连续有界等约束的路径。

附图说明

图1为本发明基于改进双向informed-RRT*的车辆路径规划方法的流程图；

图2为本发明目标引力分量示意图；

图3为本发明的圆形障碍环境图；

图4为本发明的矩形障碍环境图；

图5为本发明双向informed-RRT*算法在圆形障碍环境下寻找最优路径结果示意图；

图6为本发明改进算法在圆形障碍环境下寻找最优路径结果示意图；

图7为本发明双向informed-RRT*算法在矩形障碍环境下寻找最优路径结果示意图；

图8为本发明改进算法在矩形障碍环境下寻找最优路径结果示意图；

图9为本发明改进算法在圆形障碍环境下平滑路径图；

图10为本发明改进算法在矩形障碍环境下平滑路径图；

图11为双向informed-RRT*和本发明改进算法规划时间对比图；

图12为双向informed-RRT*和本发明改进算法最终路径长度对比图。

具体实施方式

本实施例中，如图1所示，一种基于改进双向informed-RRT*的车辆路径规划方法包括以下步骤：

步骤1、利用车辆自带的激光雷达传感器或深度相机传感器采集车辆周围的环境信息，并用于建立栅格地图，将栅格地图中的每一个栅格标记为障碍空间或自由空间，在标记后的栅格地图中选择一起始点x_start和目标点x_goal；图3和图4中，黑色部分为障碍空间，白色部分为自由空间；

步骤2、采用目标约束和目标引力相结合的组合扩展策略并融合传统双向RRT*算法在栅格地图的自由空间中进行采样，以获取初始路径；

步骤2.1、定义两颗快速探索随机树Ta和Tb；

步骤2.2、获取快速探索随机树Ta的新节点x_{new_a}，以向目标点x_goal扩展；

步骤2.2.1、设置一个目标偏置概率ρ_bias；

步骤2.2.2、在当前次随机获取采样点时，按照均匀概率随机产生一个当前概率值ρ，若当前概率值ρ大于目标偏置概率ρ_bias，则在自由空间中随机产生一个当前采样点x_{rand_a}，否则将目标点x_goal作为当前采样点x_{rand_a}；

步骤2.2.3、遍历整个快速探索随机树Ta，并利用式(1)所示的对角线距离计算公式得到当前采样点x_{rand_a}在快速探索随机树Ta中的最近邻节点x_{near_a}：

式(1)中，x0为快速探索随机树Ta中当前子节点的x坐标值，y0为快速探索随机树Ta中当前子节点的y坐标值，x1为与快速探索随机树Ta中当前子节点相比较的当前采样点x_{rand_a}的x坐标值，y1与快速探索随机树Ta中当前子节点相比较的当前采样点x_{rand_a}的y坐标值；

步骤2.2.4、如图2所示，通过目标引力方法得到新节点更加偏向目标点，利用式(2)得到新节x_{new_a}，如果新节点x_{new_a}和最近邻节点x_{near_a}之间的线段在自由空间中，则将新节点x_{new_a}插入到快速探索随机树Ta中，并执行步骤2.2.5；否则丢弃新节点x_{new_a}，并返回步骤2.2.2重复执行；

式(2)中：k_p为引力场系数，p为步长，x_{goal_ab}为目标位置向量，即目标点x_goal，||x_{goal_ab}-x_{near_a}||为目标位置向量与最近邻节点x_{near_a}距离的绝对值；

步骤2.2.5、以新节点x_{new_a}为中心，r为半径范围内搜索快速探索随机树Ta，以确定邻域集合X_{near_a}，将从根节点到邻域集合X_{near_a}的路径以及邻域集合X_{near_a}与新节点x_{new_a}之间无碰撞路径的代价之和，与从根节点经过最近邻节点x_{near_a}到达新节点x_{new_a}的路径的代价进行比较，将路径代价最小的节点作为新节点x_{new_a}的最佳父节点，并将最佳父节点与新节点x_{new_a}之间路径以及新节点x_{new_a}插入快速探索随机树Ta中；

步骤2.2.6、邻域集合X_{near_a}中每个近邻节点均尝试用新节点x_{new_a}代替邻域集合X_{near_a}中自身近邻节点原先的父节点，若代替后，从起始点x_start沿快速探索随机树Ta到对应近邻节点的累积成本比代替前更小，则对应近邻节点放弃原先的父节点，并将新节点x_{new_a}作为父节点后重新连接到近邻节点上；否则，不替换原先的父节点；

步骤2.3、通过新节点x_{new_a}将扩展后的快速探索随机树Ta与快速探索随机树Tb相连接，找到快速探索随机树Tb中与新节点x_{new_a}距离最近的邻节点x_{near_b}，如果x_{new_a}和x_{near_b}之间的距离达到两棵树的连接阈值，则表示两棵快速探索随机树Ta与Tb连接成功，并得到初始路径；否则；快速探索随机树Tb按照步骤2.2的过程向初始点x_start方向扩展，其中，在步骤2.2.2中是将起始点x_start作为采样点x_{rand_b}，在步骤2.2.3中的目标位置向量x_{goal_ab}为初始点x_start；若在步骤2.3的过程能得到初始路径；则执行步骤3，否则，快速探索随机树Ta再按照步骤2.2-步骤2.3的过程向目标点x_goal扩展和连接。

步骤3、获得初始路径后，利用改进双向informed-RRT*算法获得启发式子集，以确定标记后的栅格地图中的椭圆状态子集空间，从而在椭圆状态子集空间中进行采样，获取随机采样点x_rand；

步骤3.1、根据初始路径，在当前次迭代中以初始点x_start和目标点x_goal为焦点，路径成本c_best为长轴，c_min为初始点x_start和目标点x_goal的距离，短轴为

构建当前次迭代中的椭圆状态子集空间；

步骤3.2、通过式(3)得到当前次迭代中的椭圆状态子集X_f，从而在当前次迭代中的椭圆状态子集中直接采样获得当前采样点x_rand：

X_f＝CLx_ball+x_centre (3)

式(3)中：C表示旋转变换矩阵，L表示横轴的对角线矩阵，x_ball表示单位圆中的均匀采样点，x_centre是椭圆的中心，并有：

C＝Udiag{1,...,1,det(U)det(V)}V^T (5)

式(5)中：det(.)表示行列式，

是通过对约束矩阵UΣV^T≡M奇异值分解得到的酉矩阵，且

其中a₁表示世界坐标系的横轴，通过式(7)计算，e₁表示单位矩阵第一列之外积。

步骤4、根据随机采样点x_rand，利用目标引力方法并融合传统双向RRT*算法对初始路径进行优化，从而得到渐近最优性路径；根据渐近最优性路径的长度更新椭圆区域，从而在更新后的椭圆区域中进行采样，获得更新后的随机采样点；

步骤4.1、以先扩展快速探索随机树Ta；

步骤4.1.1、根据当前采样点x_rand，按照步骤2.2.3-步骤2.2.4的过程，最新节点x_new；如果最近邻节点x_near与最新节点x_new的线段在自由空间中，则将x_new作为备选节点，并执行步骤4.1.2；否则，丢弃最新节点x_new，并返回步骤3.1和步骤3.2；

步骤4.1.2、按照步骤2.2.5和步骤2.2.6的过程，更新快速探索随机树Ta；

步骤4.1.3、若成本之和c_new小于当前次迭代的路径成本c_best，则将最新节点x_new插入到快速探索随机树Ta中，从而得到当前次迭代的渐近最优性路径，并执行步骤4.2；否则，丢弃最新节点x_new后，返回步骤3.1、步骤3.2和步骤4.1；其中，c_new为起始点x_start到备选节点x_new的无障碍路径c(x_new,x_start)的成本与备选节点x_new到目标点x_goal的无障碍路径c(x_new,x_goal)的成本之和；

步骤4.2、扩展快速探索随机树Tb；

步骤4.2.1、用c_new代替c_best以更新当前次迭代的椭圆状态子集，以当前次迭代的渐近最优性路径代替初始路径后，重复步骤3.1、步骤3.2和步骤4.1执行。

步骤5、重复步骤4的过程，直到达到迭代次数为止，从而得到最终规划的路径；

步骤6、对最终规划的路径进行剪枝处理后，再利用四阶贝塞尔曲线进行平滑处理，从而得到一条符合车辆约束的有效路径。

步骤6.1、对路径上的点进行简化，在保证简化路径不与障碍物发生碰撞的前提下，使这段路线上点的个数最少；

步骤6.1.1、将步骤6最终获得规划路径上的所有节点依次标记为x0,x1,x2,…,xn-1,xn,其中x0和x1分别为初始点和目标点，初始化路径点集合为X＝{x0}；

步骤6.1.2、从初始点x0开始，与第一个点直线相连，将相连与障碍物进行碰撞检测，如果不发生碰撞，则将起点与下一个点连接，并进行碰撞检测。当初始点x0与点xi+1相连时与障碍物发生了碰撞，则保留节点xi，且将节点xi加入到点集X中，同时以节点xi为起点，依次与后面的点进行连接，进行碰撞检测，将保留节点加入到点集X，直到与终点进行连接后停止，同时将起点、终点和判断过程中的保留节点相连，得到简化路径。

步骤6.2、对简化的路径利用式(8)进行平滑处理。

四阶贝塞尔曲线：

式(8),中P_i为经过经过剪枝后路径节点求解的待定系数。

为了验证本方法在求解路径规划问题的有效性，在64位Window10的主机(IntelCore处理器，主频3.6GHz，内存为32G)硬件环境下，利用Matlab2018b进行模拟仿真。实验环境的范围为500m*500m，起始点坐标为(20,20)，目标点为(490,490)。针对仿真环境，选取搜索步长为p＝30，邻域半径为R＝50，概率阈值为P_bias＝0.5。求解的目标是从起始点到目标点寻找一条无碰撞、距离最短、平滑的有效路径，图3是圆形障碍环境，图4是矩形障碍环境，在两个环境中进行路径规划。

为验证本发明改进双向informed-RRT*算法在不同环境下的适用性，在圆形障碍环境和矩形障碍环境下分别使用双向informed-RRT*和改进双向informed-RRT*进行路径规划，实验结果如图5、图6、图7和图8所示。

在圆形障碍环境和矩形障碍环境下分别对本发明改进双向informed-RRT*算法规划好的最优路径平滑处理，实验结果如图9和图10所示。

为验证本发明改进双向informed-RRT*的稳定性，多次运行双向informed-RRT*和本发明改进算法进行路径规划，记录每次运行结果，统计运行时间和路径长度得到的实验数据对比图如图11和图12所示。

如图11和图12所示，本发明改进算法与双向informed-RRT*算法相比较，所需的迭代时间和规划路径长度均有明显的减少。根据图11和图12数据计算可得表1。

表1两种算法实验结果对比分析图

注：T表示平均规划时间，L表示平均路径长度

从表1中可以看出，本发明改进双向informed-RRT*算法的有效性和可行性。在两种不同实验环境下，改进算法与双向informed-RRT*算法相比，平均规划时间降低了4％到10％，平均规划路径长度减少了10％到16％。因此验证了本发明改进双向informed-RRT*算法规划出来的路径更优，耗费的时间更短，收敛速度更快。

Claims (1)

1.一种基于改进双向informed-RRT*的车辆路径规划方法，其特征是按如下步骤进行：

步骤1、利用车辆自带的激光雷达传感器或深度相机传感器采集车辆周围的环境信息，并用于建立栅格地图，将所述栅格地图中的每一个栅格标记为障碍空间或自由空间，在标记后的栅格地图中选择一起始点x_start和目标点x_goal；

步骤2、采用目标约束和目标引力相结合的组合扩展策略并融合传统双向RRT*算法在栅格地图的自由空间中进行采样，以获取初始路径；

步骤2.1、定义两颗快速探索随机树Ta和Tb；

步骤2.2、获取快速探索随机树Ta的新节点x_{new_a}，以向目标点x_goal扩展；

步骤2.2.1、设置一个目标偏置概率ρ_bias；

步骤2.2.2、在当前次随机获取采样点时，按照均匀概率随机产生一个当前概率值ρ，若当前概率值ρ大于目标偏置概率ρ_bias，则在自由空间中随机产生一个当前采样点x_{rand_a}，否则将目标点x_goal作为当前采样点x_{rand_a}；

步骤2.2.3、遍历整个快速探索随机树Ta，并利用式(1)所示的对角线距离计算公式得到当前采样点x_{rand_a}在快速探索随机树Ta中的最近邻节点x_{near_a}：

式(1)中，x0为快速探索随机树Ta中当前子节点的x坐标值，y0为快速探索随机树Ta中当前子节点的y坐标值，x1为与快速探索随机树Ta中当前子节点相比较的当前采样点x_{rand_a}的x坐标值，y1与快速探索随机树Ta中当前子节点相比较的当前采样点x_{rand_a}的y坐标值；

步骤2.2.4、利用式(2)得到新节点x_{new_a}，如果新节点x_{new_a}和最近邻节点x_{near_a}之间的线段在自由空间中，则将新节点x_{new_a}插入到快速探索随机树Ta中，并执行步骤2.2.5；否则丢弃新节点x_{new_a}，并返回步骤2.2.2重复执行；

式(2)中：k_p为引力场系数，p为步长，x_{goal_ab}为目标位置向量，即目标点x_goal，||x_{goal_ab}-x_{near_a}||为目标位置向量与最近邻节点x_{near_a}距离的绝对值；

步骤2.2.5、以新节点x_{new_a}为中心，r为半径范围内搜索所述快速探索随机树Ta，以确定邻域集合X_{near_a}，将从根节点到邻域集合X_{near_a}的路径以及邻域集合X_{near_a}与新节点x_{new_a}之间无碰撞路径的代价之和，与从根节点经过最近邻节点x_{near_a}到达新节点x_{new_a}的路径的代价进行比较，将路径代价最小的节点作为新节点x_{new_a}的最佳父节点，并将最佳父节点与新节点x_{new_a}之间路径以及新节点x_{new_a}插入所述快速探索随机树Ta中；

步骤2.2.6、邻域集合X_{near_a}中每个近邻节点均尝试用新节点x_{new_a}代替邻域集合X_{near_a}中自身近邻节点原先的父节点，若代替后，从起始点x_start沿所述快速探索随机树Ta到对应近邻节点的累积成本比代替前更小，则对应近邻节点放弃原先的父节点，并将新节点x_{new_a}作为父节点后重新连接到近邻节点上；否则，不替换原先的父节点；

步骤2.3、通过新节点x_{new_a}将扩展后的快速探索随机树Ta与快速探索随机树Tb相连接，找到快速探索随机树Tb中与新节点x_{new_a}距离最近的邻节点x_{near_b}，如果x_{new_a}和x_{near_b}之间的距离达到两棵树的连接阈值，则表示两棵快速探索随机树Ta与Tb连接成功，并得到所述初始路径；否则；快速探索随机树Tb按照步骤2.2的过程向初始点x_start方向扩展，其中，在步骤2.2.2中是将起始点x_start作为采样点x_{rand_b}，在步骤2.2.3中的目标位置向量x_{goal_ab}为初始点x_start；若在步骤2.3的过程能得到初始路径；则执行步骤3，否则，快速探索随机树Ta再按照步骤2.2-步骤2.3的过程向目标点x_goal扩展和连接；

步骤3、获得初始路径后，利用改进双向informed-RRT*算法获得启发式子集，以确定标记后的栅格地图中的椭圆状态子集空间，从而在椭圆状态子集空间中进行采样，获取随机采样点x_rand；

步骤3.1、根据所述初始路径，在当前次迭代中以初始点x_start和目标点x_goal为焦点，路径成本c_best为长轴，c_min为初始点x_start和目标点x_goal的距离，短轴为

构建当前次迭代中的椭圆状态子集空间；

步骤3.2、通过式(3)得到当前次迭代中的椭圆状态子集X_f，从而在当前次迭代中的椭圆状态子集中直接采样获得当前采样点x_rand：

X_f＝CLx_ball+x_centre (3)

式(3)中：C表示旋转变换矩阵，L表示横轴的对角线矩阵，x_ball表示单位圆中的均匀采样点，x_centre是椭圆的中心，并有：

C＝Udiag{1,...,1,det(U)det(V)}V^T (5)

式(5)中：det(.)表示行列式，

是通过对约束矩阵UΣV^T≡M奇异值分解得到的酉矩阵，且

其中，a₁表示世界坐标系的横轴，并通过式(7)计算，e₁表示单位矩阵第一列之外积；

步骤4、根据随机采样点x_rand，利用目标引力方法并融合传统双向RRT*算法对初始路径进行优化，从而得到渐近最优性路径，根据渐近最优性路径的长度更新椭圆区域，从而在更新后的椭圆区域中进行采样，获得更新后的随机采样点；

步骤4.1、以先扩展快速探索随机树Ta；

步骤4.1.1、根据当前采样点x_rand，按照步骤2.2.3-步骤2.2.4的过程，最新节点x_new；如果最近邻节点x_near与最新节点x_new的线段在自由空间中，则将x_new作为备选节点，并执行步骤4.1.2；否则，丢弃最新节点x_new，并返回步骤3.1和步骤3.2；

步骤4.1.2、按照步骤2.2.5和步骤2.2.6的过程，更新快速探索随机树Ta；

步骤4.1.3、若成本之和c_new小于当前次迭代的路径成本c_best，则将最新节点x_new插入到快速探索随机树Ta中，从而得到当前次迭代的渐近最优性路径，并执行步骤4.2；否则，丢弃最新节点x_new后，返回步骤3.1、步骤3.2和步骤4.1；其中，c_new为起始点x_start到备选节点x_new的无障碍路径c(x_new,x_start)的成本与备选节点x_new到目标点x_goal的无障碍路径c(x_new,x_goal)的成本之和；

步骤4.2、扩展快速探索随机树Tb；

步骤4.2.1、用c_new代替c_best以更新当前次迭代的椭圆状态子集，以当前次迭代的渐近最优性路径代替初始路径后，重复步骤3.1、步骤3.2和步骤4.1执行；

步骤5、重复步骤4的过程，直到达到迭代次数为止，从而得到最终规划的路径；

步骤6、对最终规划的路径进行剪枝处理后，再利用四阶贝塞尔曲线进行平滑处理，从而得到一条符合车辆约束的有效路径。

Images