一种基于横纵向协调的自动驾驶汽车动态轨迹规划及跟踪方法转让专利
申请号 : CN202010701188.8
文献号 : CN111845774B
文献日 : 2021-12-03
发明人 : 金贤建 , 严择圆 , 杨俊朋 , 王佳栋 , 杨杭 , 王启康
申请人 : 上海大学
摘要 :
权利要求 :
1.一种基于横纵向协调的自动驾驶汽车动态轨迹规划及跟踪方法,其特征在于,包括如下步骤:
S1、车辆行驶状态空间轨迹采样:将自动驾驶车辆轨迹规划问题从笛卡尔坐标系映射至Frenet坐标系下进行,在车辆行驶的状态空间中对规划时域内车辆末端运动状态进行密集采样,分别利用五次和四次多项式曲线对车辆的状态转移过程进行插值完成横、纵向的局部轨迹重规划;
S2、横纵向轨迹协调规划与车辆轨迹评估:在横纵向轨迹协调规划中,考虑复杂交通环境中车辆及环境中的静、动态障碍物都不能简单地等效为质点,分别采用移动的矩形及圆描述运行中的自动驾驶车辆、障碍物的形状、大小、朝向,引入超平面理论评估车辆轨迹是否与周围交通环境中的静、动态障碍物发生干涉及发生碰撞的风险,并设计轨迹的平顺性、舒适性、行使效率轨迹评价指标体系对轨迹质量进行评估;
S3、车辆操纵极限约束下的轨迹跟踪控制:在轨迹跟踪阶段,为保证自动驾驶车辆在自身物理条件限制之内轨迹跟踪行驶可达,采用Brush轮胎模型设计自动驾驶车辆动力学操纵极限约束规则,利用模型预测控制(MPC)完成自动驾驶车辆主动转向(AFS)与直接横摆力矩(DYC)集成轨迹跟踪控制。
2.根据权利要求1所述基于横纵向协调的自动驾驶汽车动态轨迹规划及跟踪方法,其特征在于,在所述步骤S1中,将车辆位置从笛卡尔坐标系映射至Frenet坐标系,全局地图信息包括车道路径点坐标(xi,yi),i∈N,自动驾驶主车的位置(x0,y0),车速v0,横摆角 加速度a0,横摆角速度将自动驾驶车辆轨迹位置从笛卡尔坐标系[nx,tx]映射至Frenet坐标系[nc,tc]转换关系表达式如下:
x(s(t),d(t))=r(s(t))+d(s)nc(s(t))其中,计算主车的横向运动速度vlat和纵向运动速度vlon,表达式为:计算主车的横向运动加速度alat和纵向运动加速度alon,表达式为:找出全局坐标点(xi,yi),i∈N中与主车的位置坐标(x0,y0)最近的点作为自动驾驶主车的全局定位点。将在笛卡尔坐标系中的信息映射至Frenet坐标系中,笛卡尔坐标系[nx,tx]到Frenet坐标系[nc,tc]的转换关系为:x(s(t),d(t))=r(s(t))+d(s)nc(s(t))。
3.根据权利要求1所述基于横纵向协调的自动驾驶汽车动态轨迹规划及跟踪方法,其特征在于,在所述步骤S1中,通过采样得横向规划轨迹簇轨迹集合为:dset=Ψ(t,d0,d1),t={t0,t1,t2,…,tN‑1}式中,Ψ(t,d0,d1)代表所有横向规划所得的轨迹集合,t表示N个时刻采样点,Ψ(ti,d0,d1),0≤i≤N‑1表示某一条状态确定的横向运动轨迹,车辆按该轨迹行驶,将使其从初始状态d0转移至末状态d1;
在横向轨迹规划中使用的五次多项式表达式为:
2 3 4 5
d(t)=cd1+cd2t+cd3t+cd4t+cd5t+cd6t横向规划轨迹求解中,利用获取车辆的当前横向运动状态 和根据配置的规划时域末端运动状态 求解,求解的表达式如下:
2 3 4 5
d(t0)=cd1+cd2t0+cd3t0+cd4t0+cd5t0+cd6t0
2 3 4 5
d(t1)=cd1+cd2t1+cd3t1+cd4t1+cd5t1+cd6t1求解矩阵条件及方式如下:
‑1
C=M O
T
其中C=[cd1,cd2,cd3,cd4,cd5,cd6]车辆在横向规划时间段内的规划轨迹求解结果表达式为:
4.根据权利要求2所述基于横纵向协调的自动驾驶汽车动态轨迹规划及跟踪方法,其特征在于,在所述步骤S1中,通过采样得纵向规划轨迹簇轨迹集合为:sset=Φ(t,s0,s1),t={t0,t1,t2,…,tN‑1}式中,Φ(t,s0,s1)代表所有纵向规划所得的轨迹集合,t表示N个时刻采样点,Φ(ti,s0,s1),0≤i≤N‑1表示某一条状态确定的纵向运动轨迹,车辆按该轨迹行驶,将使其从初始状态s0转移至末状态s1;
在纵向轨迹规划使用的是四次多项式轨迹表达式为:
2 3 4
s(t)=cs1+cs2t+cs3t+cs4t+cs5t纵向规划轨迹求解中,利用获取车辆的当前纵向运动状态 其配置的规划时域末端运动状态在高速环境采用速度跟随模式中使用 在低速环境中车距跟随模式下使用 对规划轨迹进行求解的表达式为:将具有相同规划时域和插值点的横、纵向轨迹曲线按时刻点ti∈[t0,t1],i=1,2,...,N一一对应组合,得到[s(ti),d(ti)],再根据转换关系表达式完成从Frenet坐标系到笛卡尔坐标系的映射及轨迹合成。
5.根据权利要求1所述基于横纵向协调的自动驾驶汽车动态轨迹规划及跟踪方法,其特征在于,在所述步骤S2中,横纵向轨迹协调规划设计过程为:评价横向规划轨迹的指标设置包括以下的两项:其中kj为权重系数,此项意在使横向规划轨迹jerk最小,保证舒适性;
2
Ce=ke(d(t))
其中ke为权重系数,此项意在使横向规划接近参考车道中心线,即让主车完成避障后能够回到初始参考车道行驶;
评价纵向规划轨迹的指标设置包括以下的三项:其中kj为权重系数,与横向轨迹的该指标一样,此项意在使纵向规划轨迹jerk最小,保证舒适性;
其中ke为权重系数,vtarget为目标车速,在车速跟随模式下需要使车速接近并保持目标速度时,纵向轨迹的末端速度参考采用上式所示的表达式;
其中starget为目标距离,在车距跟随模式下需要使主车与移动的交通车辆保持一定的车距时,纵向轨迹的末端距离采用如上所示的表达式;
在对横纵向轨迹规划进行单独初步评价后,进一步设计评价横、纵向合成协调轨迹的指标包括以下的四项:
考虑车辆及环境静、动态障碍物都不能简单地等效为质点,它们的形状、大小、朝向均可能给行驶带来风险,分别采用移动的矩形及圆描述运行中的自动驾驶车辆、障碍物,引入超平面理论进一步确定这些车辆轨迹是否与周围交通环境中的静态或者动态的障碍物发生干涉及是否有发生碰撞风险如下:定义车辆与障碍物不发生干涉及不发生碰撞风险的条件为:其中dist表示轨迹距离障碍物的最小距离,H(τ)为自动驾驶车辆的轨迹坐标,为静态或者动态障碍物的圆中心位置,μ为静态或动态的障碍物与车辆干涉的安全因子,m为障碍物数;
进一步引入轨迹避碰的超平面为:则自动驾驶车辆矩形顶点描述为:其中,T(τ)为坐标旋转矩阵,定义为:其中, θ(τ)为自动驾驶车辆朝向;
进一步推导超平面中的车辆与障碍物不发生干涉及不发生碰撞风险的条件得其中, 预测的为静态或者动态障碍物的圆中心位置, 为静态或者动态障碍物的圆半径;
定义横纵向协调轨迹的行驶效率指标为:Ct=ktTi
其中kt为权重系数,此项为轨迹覆盖的时间,与行驶效率有关;
定义横纵向协调轨迹的舒适性指标为其中
其中wa、wθ为权重系数,此项描述了规划轨迹的合加度,与舒适性有关;
定义横纵向协调轨迹的平顺性指标为:其中kk为权重系数,此项描述了转换至大地坐标系下的合成轨迹曲率,与行驶平顺性有关。
6.根据权利要求1所述基于横纵向协调的自动驾驶汽车动态轨迹规划及跟踪方法,其特征在于,在所述步骤S3中,在轨迹跟踪阶段,所设计的轨迹跟踪过程为:在设计车辆跟踪动力学操纵极限物理约束规则时,用于描述车辆物理特征的车辆动力学模型表达式如下:
式中,为车身坐标系下的横向加速度,为车身纵坐标下的纵向加速度,为横摆角,为横摆角速度, 为横摆角加速度,为车辆的纵向速度, 为车辆的横向速度,δf为前轮转角,Mz为直接横摆力矩,a,b分别为前、后轴到质心的距离,Iz为车辆的转动惯量, 为全局坐标系下的纵向速度,为全局坐标系下的横向速度,X为全局坐标系下的纵向位置,Y为全局坐标系下的横向位置,m为车辆的总质量;
采用的Brush轮胎模型刻画运动的自动驾驶车辆与地面交互作用及路面附着约束如下上式中
其中前后轮胎侧偏角为:
上述式,μ为地面附着系数,κi为滑移率,Cα,Cx分别为轮胎侧偏、纵向刚度,Fz,i为轮胎的垂直载荷;
采用稳态车辆质心侧偏角与横摆角速度来设计自动驾驶车辆动力学操纵极限,首先假设在转弯过程中纵向力作用很小,则稳态横摆角速度满足为改善驾驶员前轮转向角输入激励下的车辆横摆角速度的瞬态响应,理想的横摆角速度极限修正为
同时,自动驾驶车辆动力学操纵极限严重受制于地面条件约束,导致的轮胎‑路面交互作用的饱和效应,则后轮Brush轮胎模型产生临界侧向侧偏角进一步推导得推导车辆行使过程的横向速度极限为:其中,η为制动的依赖系数,表示为:
7.根据权利要求1所述基于横纵向协调的自动驾驶汽车动态轨迹规划及跟踪方法,其特征在于,在所述步骤S3中,在轨迹跟踪阶段,利用模型预测控制(MPC)原理完成自动驾驶车辆主动转向(AFS)与直接横摆力矩(DYC)集成轨迹跟踪控制如下:定义的自动驾驶AFS、DYC集成跟踪控制系统的状态向量定义输入向量分别为u=[δf,Mz]因此用于车辆路径跟踪的预测模型表达式写作:其中
定义模型预测控制的最优化问题的代价函数设计为:其中,J(ξ(t),u(t‑1),ΔU(t))为优化指标,Δu(t+i|t),t=0,1,...,Nc‑1为输入控制增量向量,Np为预测时域,Nc为控制时域,ηr(t+i|t),i=1,2,...,Np为参考输出量,Q,R,ρ分别为权重矩阵;
模型预测控制的约束条件设计为:ΔUmin≤ΔU(t)≤ΔUmaxUmin‑U(t‑1)≤MΔU(t)≤Umax‑U(t‑1)
0<ε<εmax
上述式中,Ycmin(t),Ycmax(t)分别为硬约束输出量的下限和上限,Yscmin(t),Yscmax(t)分别为软约束输出量的下限和上限,Ξ=ε1pn,其中1pn为pu维列向量;
在当前时刻,求解上述优化问题得到最优控制增量序列,将控制增量序列的第一个量与上一时刻的控制量累积,得到实际控制量,作为被控系统的输入;舍弃除第一个控制增量以外的序列中的其他控制增量;下一个优化时刻到来时,重复以上过程,实现滚动优化;在下一个规划周期更新车辆的初状态,重复步骤S1至步骤S3的操作,实现实时重规划与主动转向及直接横摆力矩集成跟踪控制。
说明书 :
一种基于横纵向协调的自动驾驶汽车动态轨迹规划及跟踪
方法
技术领域
背景技术
地取代人类驾驶者的操作,避免了人类驾驶者的操作失误,可有效提高交通系统的效率和
安全性,自动驾驶车辆的研发已经逐渐受到国内外各大汽车厂商和研究机构的重视。
法是自动驾驶技术的关键之一,与人类正常驾驶的汽车一样,自动驾驶汽车的轨迹规划主
要是拟规划出一条从起始位置抵达目标位置,能够避开障碍物的安全、可靠可行路径,其性
能的好坏将直接影响汽车的行驶安全性和无人驾驶汽车的智能化程度。目前存在的轨迹规
划算法大致可以分为图搜索、数值优化和采样等三类;大多数研究集中于前两类轨迹规划
方法。事实上,基于图搜索的方法将环境空间映射到加权图上从而将避障路径规划转换为
图搜索问题,其计算时间会随着地图规模的增大快速增长。基于数值优化的算法是把目标
函数和约束条件转化成凸优化形式,然后使用数值优化迭代器计算出其最优解,该方法最
大的优点在于最优解空间是连续的,然而要将周围环境抽象成凸优化的目标函数这一条件
难以满足,同时求解最优解往往依赖于计算开销较大的梯度迭代;这两类方法难以满足自
动驾驶的高实时性要求。
碍物场景的单一横向轨迹规划难以满足自动驾驶的高可靠性要求,如何在综合交通环境中
规划出能应对动、静态障碍物的横纵向协调的自动驾驶汽车动态轨迹,且满足无人驾驶车
辆行驶的安全性、舒适性和通行效率等条件。同时,规划所得的轨迹跟踪还需要考虑自动驾
驶车辆动力学操纵极限约束,保证其可执行性,也是应该对待的重要问题。
发明内容
对规划时域内车辆末端运动状态进行密集采样,分别利用五次和四次多项式曲线对车辆的
状态转移过程进行插值完成横、纵向的局部轨迹重规划,然后引入超平面理论进行障碍物
碰撞检测,综合考虑横纵向协调的轨迹并对轨迹的平顺性、舒适性、行使效率进行评估,在
设计轨迹操纵动力学极限约束规则下采用AFS+DYC集成系统对轨迹进行MPC跟踪控制,具有
高实时、安全、可靠的优点。
多项式曲线对车辆的状态转移过程进行插值完成横、纵向的局部轨迹重规划;
的形状、大小、朝向,引入超平面理论评估车辆轨迹是否与周围交通环境中的静、动态障碍
物发生干涉及发生碰撞的风险,并设计轨迹的平顺性、舒适性、行使效率轨迹评价指标体系
对轨迹质量进行评估;
(MPC)完成自动驾驶车辆主动转向(AFS)与直接横摆力矩(DYC)集成轨迹跟踪控制。
对车辆的状态转移过程进行插值完成横、纵向的局部轨迹重规划。
初始状态d0转移至末状态d1。
表达式如下:
初始状态s0转移至末状态s1。
环境中车距跟随模式下使用 对规划轨迹进行求解的表达式为:
笛卡尔坐标系的映射及轨迹合成。
静、动态障碍物发生干涉及发生碰撞的风险,选择最优规划轨迹用于底层的跟踪参考。
辆、障碍物,引入超平面理论进一步确定这些车辆轨迹是否与周围交通环境中的静态或者
动态的障碍物发生干涉及是否有发生碰撞风险如下:
因子,m为障碍物数。
成轨迹跟踪控制。
为前轮转角,Mz为直接横摆力矩,a,b分别为前、后轴到质心的距离,Iz为车辆的转动惯量,
为全局坐标系下的纵向速度, 为全局坐标系下的横向速度,X为全局坐标系下的纵向
位置,Y为全局坐标系下的横向位置,m为车辆的总质量。
R,ρ分别为权重矩阵。
增量以外的序列中的其他控制增量。下一个优化时刻到来时,重复以上过程,实现滚动优
化。在下一个规划周期更新车辆的初状态,重复步骤(1)至步骤(3)的操作,实现实时重规划
与主动转向及直接横摆力矩集成跟踪控制。
五次和四次多项式曲线对车辆的状态转移过程进行插值完成横、纵向的局部轨迹重规划,
能满足自动驾驶的高实时性要求;
辆、障碍物的形状、大小、朝向,引入超平面理论评估车辆轨迹是否与周围交通环境中的静、
动态障碍物发生干涉及发生碰撞的风险,并设计轨迹的平顺性、舒适性、行使效率等轨迹评
价指标体系对轨迹质量进行评估;
预测控制(MPC)完成自动驾驶车辆主动转向(AFS)与直接横摆力矩(DYC)集成轨迹跟踪控
制。
附图说明
具体实施方式
多项式曲线对车辆的状态转移过程进行插值完成横、纵向的局部轨迹重规划。
的形状、大小、朝向,引入超平面理论评估车辆轨迹是否与周围交通环境中的静、动态障碍
物发生干涉及发生碰撞的风险,并设计轨迹的平顺性、舒适性、行使效率轨迹评价指标体系
对轨迹质量进行评估。
(MPC)完成自动驾驶车辆主动转向(AFS)与直接横摆力矩(DYC)集成轨迹跟踪控制。
度vlon,表达式为:
卡尔坐标系[nx,tx]到Frenet坐标系[nc,tc]的转换关系为:
横、纵向的局部轨迹重规划。具体包括:
驶;在纵向规划时域中配置末端运动位置s(t1),运动速度 运动加速度
初始状态d0转移至末状态d1。
在车距跟随模式下使用 可以对规划轨迹进行求解,具体表达式为:
初始状态s0转移至末状态s1。
坐标系到笛卡尔坐标系的映射及轨迹合成。
包括:
测碰撞的方法,在本方案中,考虑车辆及环境静、动态障碍物都不能简单地等效为质点,它
们的形状、大小、朝向等均可能给行驶带来风险,分别采用移动的矩形及圆描述运行中的自
动驾驶车辆、障碍物,引入超平面理论进一步确定这些车辆轨迹是否与周围交通环境中的
静态或者动态的障碍物发生干涉及是否有发生碰撞风险如下:
因子,m为障碍物数。
测则该轨迹成为最终的参考轨迹,不通过则删除该轨迹后检测次优的轨迹,如此循环直至
通过。这样,就无需对每一条备选规划轨迹都进行碰撞检测,而仅检测其中的一部分,减小
了计算量。
为前轮转角,Mz为直接横摆力矩,a,b分别为前、后轴到质心的距离,Iz为车辆的转动惯量,
为全局坐标系下的纵向速度, 为全局坐标系下的横向速度,X为全局坐标系下的纵向位
置,Y为全局坐标系下的横向位置,m为车辆的总质量。
约束,采用Brush轮胎模型来建立轮胎模型如下
R,ρ分别为权重矩阵。
增量以外的序列中的其他控制增量。下一个优化时刻到来时,重复以上过程,实现滚动优
化。在下一个规划周期更新车辆的初状态,重复步骤1至步骤5的操作,实现实时重规划与主
动转向及直接横摆力矩集成跟踪控制。根据具体实施方式进行仿真验证,仿真结果如图3所
示:
下进行,在车辆行驶的状态空间中对规划时域内车辆末端运动状态进行密集采样,分别利
用五次和四次多项式曲线对车辆的状态转移过程进行插值完成横、纵向的局部轨迹重规
划。在横纵向轨迹协调规划中,考虑复杂交通环境中车辆及环境中的静、动态障碍物都不能
简单地等效为质点,分别采用移动的矩形及圆描述运行中的自动驾驶车辆及障碍物的形
状、大小、朝向,引入超平面理论评估车辆轨迹是否与周围交通环境中的静、动态障碍物发
生干涉及发生碰撞的风险,并设计轨迹的平顺性、舒适性、行使效率等轨迹评价指标体系对
轨迹质量进行评估。在轨迹跟踪阶段,为保证自动驾驶车辆在自身物理条件限制之内轨迹
跟踪行驶可达,采用Brush轮胎模型等设计自动驾驶车辆动力学操纵极限约束规则,利用模
型预测控制(MPC)完成自动驾驶车辆主动转向(AFS)与直接横摆力矩(DYC)集成轨迹跟踪控
制。本发明方法能满足自动驾驶的高实时性要求,具有高实时、安全、可靠的优点。
做的改变、修饰、替代、组合或简化,均应为等效的置换方式,只要符合本发明的发明目的,
只要不背离本发明的技术原理和发明构思,都属于本发明的保护范围。