1.一种基于改进双向informed‑RRT*的车辆路径规划方法,其特征是按如下步骤进行:步骤1、利用车辆自带的激光雷达传感器或深度相机传感器采集车辆周围的环境信息,并用于建立栅格地图,将所述栅格地图中的每一个栅格标记为障碍空间或自由空间,在标记后的栅格地图中选择一起始点xstart和目标点xgoal;
步骤2、采用目标约束和目标引力相结合的组合扩展策略并融合传统双向RRT*算法在栅格地图的自由空间中进行采样,以获取初始路径;
步骤3、获得初始路径后,利用改进双向informed‑RRT*算法获得启发式子集,以确定标记后的栅格地图中的椭圆状态子集空间,从而在椭圆状态子集空间中进行采样,获取随机采样点xrand;
步骤4、根据随机采样点xrand,利用目标引力方法并融合传统双向RRT*算法对初始路径进行优化,从而得到渐近最优性路径,根据渐近最优性路径的长度更新椭圆区域,从而在更新后的椭圆区域中进行采样,获得更新后的随机采样点;
步骤5、重复步骤4的过程,直到达到迭代次数为止,从而得到最终规划的路径;
步骤6、对最终规划的路径进行剪枝处理后,再利用四阶贝塞尔曲线进行平滑处理,从而得到一条符合车辆约束的有效路径。
2.根据权利要求1所述的一种基于改进双向informed‑RRT*的车辆路径规划方法,其特征在于,所述步骤2包括:
步骤2.1、定义两颗快速探索随机树Ta和Tb;
步骤2.2、获取快速探索随机树Ta的新节点xnew_a,以向目标点xgoal扩展;
步骤2.2.1、设置一个目标偏置概率ρbias;
步骤2.2.2、在当前次随机获取采样点时,按照均匀概率随机产生一个当前概率值ρ,若当前概率值ρ大于目标偏置概率ρbias,则在自由空间中随机产生一个当前采样点xrand_a,否则将目标点xgoal作为当前采样点xrand_a;
步骤2.2.3、遍历整个快速探索随机树Ta,并利用式(1)所示的对角线距离计算公式得到当前采样点xrand_a在快速探索随机树Ta中的最近邻节点xnear_a:式(1)中,x0为快速探索随机树Ta中当前子节点的x坐标值,y0为快速探索随机树Ta中当前子节点的y坐标值,x1为与快速探索随机树Ta中当前子节点相比较的当前采样点xrand_a的x坐标值,y1与快速探索随机树Ta中当前子节点相比较的当前采样点xrand_a的y坐标值;
步骤2.2.4、利用式(2)得到新节点xnew_a,如果新节点xnew_a和最近邻节点xnear_a之间的线段在自由空间中,则将新节点xnew_a插入到快速探索随机树Ta中,并执行步骤2.2.5;否则丢弃新节点xnew_a,并返回步骤2.2.2重复执行;
式(2)中:kp为引力场系数,p为步长,xgoal_ab为目标位置向量,即目标点xgoal,||xgoal_ab‑xnear_a||为目标位置向量与最近邻节点xnear_a距离的绝对值;
步骤2.2.5、以新节点xnew_a为中心,r为半径范围内搜索所述快速探索随机树Ta,以确定邻域集合Xnear_a,将从根节点到邻域集合Xnear_a的路径以及邻域集合Xnear_a与新节点xnew_a之间无碰撞路径的代价之和,与从根节点经过最近邻节点xnear_a到达新节点xnew_a的路径的代价进行比较,将路径代价最小的节点作为新节点xnew_a的最佳父节点,并将最佳父节点与新节点xnew_a之间路径以及新节点xnew_a插入所述快速探索随机树Ta中;
步骤2.2.6、邻域集合Xnear_a中每个近邻节点均尝试用新节点xnew_a代替邻域集合Xnear_a中自身近邻节点原先的父节点,若代替后,从起始点xstart沿所述快速探索随机树Ta到对应近邻节点的累积成本比代替前更小,则对应近邻节点放弃原先的父节点,并将新节点xnew_a作为父节点后重新连接到近邻节点上;否则,不替换原先的父节点;
步骤2.3、通过新节点xnew_a将扩展后的快速探索随机树Ta与快速探索随机树Tb相连接,找到快速探索随机树Tb中与新节点xnew_a距离最近的邻节点xnear_b,如果xnew_a和xnear_b之间的距离达到两棵树的连接阈值,则表示两棵快速探索随机树Ta与Tb连接成功,并得到所述初始路径;否则;快速探索随机树Tb按照步骤2.2的过程向初始点xstart方向扩展,其中,在步骤2.2.2中是将起始点xstart作为采样点xrand_b,在步骤2.2.3中的目标位置向量xgoal_ab为初始点xstart;若在步骤2.3的过程能得到初始路径;则执行步骤3,否则,快速探索随机树Ta再按照步骤2.2‑步骤2.3的过程向目标点xgoal扩展和连接。
3.根据权利要求2所述的一种基于改进双向informed‑RRT*的车辆路径规划方法,其特征在于:所述步骤3包括:
步骤3.1、根据所述初始路径,在当前次迭代中以初始点xstart和目标点xgoal为焦点,路径成本cbest为长轴,cmin为初始点xstart和目标点xgoal的距离,短轴为 构建当前次迭代中的椭圆状态子集空间;
步骤3.2、通过式(3)得到当前次迭代中的椭圆状态子集Xf,从而在当前次迭代中的椭圆状态子集中直接采样获得当前采样点xrand:Xf=CLxball+xcentre (3)式(3)中:C表示旋转变换矩阵,L表示横轴的对角线矩阵,xball表示单位圆中的均匀采样点,xcentre是椭圆的中心,并有:T
C=Udiag{1,...,1,det(U)det(V)}V (5)T
式(5)中:det(.)表示行列式, 是通过对约束矩阵UΣV≡M奇异值分解得到的酉矩阵,且 其中,a1表示世界坐标系的横轴,并通过式(7)计算,e1表示单位矩阵第一列之外积;
4.根据权利要求3所述的一种基于改进双向informed‑RRT*的车辆路径规划方法,其特征在于:所述步骤4包括:
步骤4.1、以先扩展快速探索随机树Ta;
步骤4.1.1、根据当前采样点xrand,按照步骤2.2.3‑步骤2.2.4的过程,最新节点xnew;如果最近邻节点xnear与最新节点xnew的线段在自由空间中,则将xnew作为备选节点,并执行步骤4.1.2;否则,丢弃最新节点xnew,并返回步骤3.1和步骤3.2;
步骤4.1.2、按照步骤2.2.5和步骤2.2.6的过程,更新快速探索随机树Ta;
步骤4.1.3、若成本之和cnew小于当前次迭代的路径成本cbest,则将最新节点xnew插入到快速探索随机树Ta中,从而得到当前次迭代的渐近最优性路径,并执行步骤4.2;否则,丢弃最新节点xnew后,返回步骤3.1、步骤3.2和步骤4.1;其中,cnew为起始点xstart到备选节点xnew的无障碍路径c(xnew,xstart)的成本与备选节点xnew到目标点xgoal的无障碍路径c(xnew,xgoal)的成本之和;
步骤4.2、扩展快速探索随机树Tb;
步骤4.2.1、用cnew代替cbest以更新当前次迭代的椭圆状态子集,以当前次迭代的渐近最优性路径代替初始路径后,重复步骤3.1、步骤3.2和步骤4.1执行。