CN115525047B 一种具备多型避障方式的车辆局部轨迹规划方法及系统 （江苏集萃清联智控科技有限公司）

一种具备多型避障方式的车辆局部轨迹规本发明公开了一种具备多型避障方式的车出代价最低的候选局部轨迹,根据步骤S2生成的2S2，从所有障碍物中选择出当前车辆位置前方所S3，根据所述参考路径和当前自车位姿信息进行S5，对步骤S4获得的大地坐标系下代价最低的无碰撞候选局S22中，对于所述步骤S21判定的静态障碍物，a1，依据大地坐标系中静态障碍物三维边界框或三维包围盒中心点的高度参数zobs与b1，判断最高点的高度是否低于第一预设值、以及判则对该静态障碍物采取穿越避障决策；若最高点的高度高于第一预设值且低于第二预设c1，判断该静态障碍物三维边界框或三维包围盒宽度S22中，对于所述步骤S21判定的静态障碍物，a2，依据大地坐标系中静态障碍物三维边界框或三维包围盒中心点的高度参数zobs与3c2，判断该静态障碍物三维边界框或三维包围盒宽度obsS33，利用d(t)与s(t)为每一条横向候选局部轨迹与纵向候选局部轨迹分配相应的代横向候选局部轨迹的代价计算公式表示为式(11)：pjtp纵向候选局部轨迹的代价计算公式表示为式(12)：Cs=ky·"(t2dt+kr·T+ks·[s(T)-⃞a(T)2(12)jtd(T)表示在末端时刻期望达到的目标纵向位移，时刻的纵向位移，(T)表示当前纵向候选局部轨迹在末端时刻的纵向速度，$(t)表示车辆纵向加速度；latlon分别表示权重系数。4i+1yi+1并依据不同的避障决策，采用下面对应的碰撞检a.对于采用绕行避障的障碍物，使用车身二维矩形包络与障碍物二维矩形包络进行b.对于采用跨越避障的障碍物，使用车轮二维矩形包络与障碍物二维矩形包络进行进行碰撞检测时，若当前候选局部轨迹与障碍物发生碰撞，则4.如权利要求3所述的具备多型避障方式的车辆局部轨迹规划方法依据车身参数与定位模块获取的车辆位置信息与朝向信分别以障碍物二维矩形包络的中心点、车辆二维矩将障碍物二维矩形包络半长Lo，障碍物二维矩形包络半宽Wo，车辆二维矩形包络半长Lv，车辆二维矩形包络半宽Wv和障碍物二维矩形包络的中心点与车辆二维矩形包络的中心全局规划功能模块，其用于根据获取当前自车5分类单元，其用于依据障碍物速度vobs是否为零进行分类第二决策子单元，其用于对于静态障碍物，将其a1，依据大地坐标系中静态障碍物三维边界框或三维包围盒中心点的高度参数zobs与b1，判断最高点的高度是否低于第一预设值、以及判则对该静态障碍物采取穿越避障决策；若最高点的高度高于第一预设值且低于第二预设c1，判断该静态障碍物三维边界框或三维包围盒宽度a2，依据大地坐标系中静态障碍物三维边界框或三维包围盒中心点的高度参数zobs与c2，判断该静态障碍物三维边界框或三维包围盒宽度obs6轨迹生成单元，其基于参考路径构建曲线坐标，并使用多碰撞检测单元，其用于取出曲线坐标下代价最低的候选局a.对于采用绕行避障的障碍物，使用车身二维矩形包络与障碍物二维矩形包络进行b.对于采用跨越避障的障碍物，使用车轮二维矩形包络与障碍物二维矩形包络进行进行碰撞检测时，若当前候选局部轨迹与障碍物发生碰撞，则7行驶轨迹。目前常用的车辆局部规划算法包含动态窗口法(DWA，dynamicwindowapproach)、时间弹性带法(TEB，TimeElasticBand)、人工势场法(APF，Artificial[0004]为实现上述目的，本发明提供一种具备多型避障方式的车辆局步骤S2生成的所述避障决策选择碰撞检测方法，进行障碍物与候选局部轨迹的碰撞检测，8[0015]a1，依据大地坐标系中静态障碍物三维边界框或三维包围盒中心点的高度参数[0019]a2，依据大地坐标系中静态障碍物三维边界框或三维包围盒中心点的高度参数[0025]S33，利用d(t)与s(t)为每一条横向候选局部轨迹与纵向候选局部轨迹分配相应9pp轨迹的碰撞检测或不进行障碍物与该候选局[0046]去除障碍物三维边界框或三维包围盒的高度信息，得到障碍物二维矩形包络信长Lv，车辆二维矩形包络半宽Wv和障碍物二维矩形包络的中心点与车辆二维矩形包络的中[0057]局部规划功能模块，其用于根据所述参考路径和当前自车信息进行局部轨迹规轨迹并重新取出代价最低的候选局部轨迹，直至取出的代价最低的无碰撞候选局部轨迹，[0064]a1，依据大地坐标系中静态障碍物三维边界框或三维包围盒中心点的高度参数[0067]a2，依据大地坐标系中静态障碍物三维边界框或三维包围盒中心点的高度参数[0077]轨迹插值单元，其用于对大地坐标系下最低代价的无碰撞候选局部轨迹进行插[0083]图5是本发明实施例采用五次多项式方法生成的曲线坐标横向候选局部轨迹示意[0084]图6是本发明实施例采用五阶贝塞尔曲线方法生成的曲线坐标横向候选局部轨迹[0085]图7是本发明实施例采用四次多项式方法生成的曲线坐标纵向候选局部轨迹示意[0086]图8是本发明实施例采用五阶贝塞尔曲线方法生成的曲线坐标纵向候选局部轨迹等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有[0091]本发明实施例提供的具备多型避障方式的车辆局部轨迹规划方法主要包括图1中[0094]由环境感知模块收集环境中与障碍物相关的数据，得到每个障碍物信息obsobsobs)表示障[0098]S21，依据障碍物速度vobs是否为零进行分类，速度vobs为零的障碍物为静态障碍[0104]a1，依据大地坐标系中静态障碍物三维边界框或三维包围盒中心点的高度参数[0110]在另一个实施例中，作为依据对比结果获取避障决策的方法的[0111]a2，依据大地坐标系中静态障碍物三维边界框或三维包围盒中心点的高度参数骤c1为：判断该静态障碍物三维边界框或三维包围盒宽度wobs或长度lobs与Lb_d的大小关[0120]S32，在曲线坐标下可以采用多次多项式方法或多阶贝塞尔曲线方法生成横向候d[0123]车辆横向运动的初始状态包括曲线坐标下初始横向位移d(0)、初始横向速度和初始横向加速度io),这些量均可以直接从当前自车的车辆状态得到。分别为横向候选局部轨迹五阶贝塞尔曲线的控制点，到曲线坐标下的横向运动初始状态包括初始横向位移d(0)、初始横向速度dco)和初始横向加速度再对末端时刻T与末端横向位移d(T)进行采样，令末端横向速度d(r和末表示如下式(5)：横向候选局部轨迹示意图。图中使用的样例d(0)＝0，d(0)-0,d(0)-0,采样末端时s速度这些量可以直接从当前自车的车辆状态得到。在与横向采样同样的末端时刻Ti(0)=0,(0)=0,采样末端时刻T末端纵向速度s(r)进行采样，再令末端纵向位移s(T)=(s(0)+5(T))/2XT,末端纵s5如表示为下式(10)：td轨迹代价计算相同的kj与kt，若希望最优局部轨迹末速度更接近目标速度，则可适当提高步骤S2生成的所述避障决策选择碰撞检测方法，进行障碍物与候选局部轨迹的碰撞检测，)分别表示大地坐标系下当前候选局部轨迹同的碰撞检测方法或不进行障碍物与候选局部轨迹碰撞检测。对于采用绕行避障的障碍[0168]所述步骤S8所采用的OBB碰撞检测算法利用了分离轴定理。分离轴定理其几何原维矩形包络半长Lv，车辆二维矩形包络半宽Wv和障碍物二维矩形包络的中心点与车辆二维个车轮二维矩形包络分别与障碍物二维矩形包络进行OB[0185]其中，局部规划功能模块根据所述参考路径和当前自车信息进生成一系列候选局部轨迹，并依据评价函数为每一条候选局部轨迹分配代价,根据所[0193]a1，依据大地坐标系中静态障碍物三维边界框或三维包围盒中心点的高度参数[0195]c1，判断该静态障碍物三维边界框或三维包围盒宽度wobs和长度lobs与Lb_2d的大[0196]a2，依据大地坐标系中静态障碍物三维边界框或三维包围盒中心点的高度