[发明专利]一种基于改进双向informed-RRT*的车辆路径规划方法

权利要求书

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