1.一种考虑周车随机性的无人驾驶车辆滚动优化决策方法,无人驾驶车辆决策需要避免本车与周车的未来轨迹过近而增加发生交通事故的风险,其中本车是本方法中的无人驾驶车辆,周车是所有本车通过传感器感知到的车辆的集合;周车的未来轨迹是无法准确预测的随机变量,本方法基于周车轨迹预测模块输出的周车未来位置实现在周车的未来轨迹具有随机性的情况下的无人驾驶车辆决策;当周车的未来轨迹的随机性高时,相比于周车的未来轨迹的随机性低时,本车采取更保守的行为,增大本车与周车的距离以提高无人驾驶车辆的安全性;本方法基于无人驾驶车辆上加装的惯导、激光雷达和摄像头,获取无人驾驶车辆的车辆纵向速度、车辆纵向位移、车辆侧向位移、车辆横摆角、车辆侧向速度、横摆角速度、当前车道的中心线方程、目标车道l的中心线方程以及周车轨迹预测模块输出的周车未来位置这些无人驾驶车辆滚动优化决策系统状态,建立无人驾驶车辆决策模型,基于周车轨迹预测模块输出的周围车辆未来位置的二维高斯分布,进一步建立本车与周车的碰撞概率模型,设计考虑周车随机性的无人驾驶车辆滚动优化决策优化问题,并输出驾驶决策,实现考虑周车随机性的无人驾驶车辆横纵向滚动优化决策,其特征在于,本方法具体步骤如下:步骤一、建立无人驾驶车辆决策模型
建立无人驾驶车辆决策坐标系,坐标系X轴方向为在水平面上从无人驾驶车辆质心向无人驾驶车辆车头的方向,坐标系Y轴方向为在水平面上从无人驾驶车辆质心向无人驾驶车辆左侧的与坐标系X轴垂直的方向,坐标系原点为无人驾驶车辆质心位置,坐标系X轴方向为车辆纵向,坐标系Y轴方向为车辆侧向;
建立无人驾驶车辆纵向决策模型,其中包括车辆纵向系统模型和纵向控制模型两部分,车辆纵向系统模型如下:
其中,xx为车辆纵向系统状态,xx=[xo vx]′,′表示矩阵转置;xo为车辆纵向位移,单位m;vx为车辆纵向速度,单位m/s;ux为车辆纵向控制量,ux=ax;ax为车辆纵向加速度,单位m/s2,ax是由纵向控制模型得出的:ux=fx(v,vx) (2)
其中,ux为车辆纵向控制量;v为车辆速度决策,单位m/s;vx为车辆纵向速度,单位m/s;fx(v,vx)为纵向控制律,fx(v,vx)=P(v-vx),其中P为纵向控制律的比例系数,基于车辆纵向系统模型采用PID控制器的工程整定法确定纵向控制律的比例系数P;
建立无人驾驶车辆侧向决策模型,其中包括车辆侧向系统模型、侧向控制模型、侧向规划模型三部分,车辆侧向系统模型如下:其中yo为车辆侧向位移,单位m;vx为车辆纵向速度,单位m/s;ψ为车辆横摆角,单位rad;vy为车辆侧向速度,单位m/s;r为车辆横摆角速度,单位rad/s;wf为车辆前轴中心到质心的距离,单位m;wr为车辆后轴中心到质心的距离,单位m;Iz为车辆绕垂直于坐标轴X轴方向和坐标轴Y轴方向所构成平面的转动惯量,单位kg·m2;Cf为前轮侧偏刚度,单位N/rad;Cr为后轮侧偏刚度,单位N/rad;δ为车辆前轮转角,单位rad;m为车辆质量,单位kg;整理可得车辆侧向系统状态空间模型:其中xy为车辆侧向系统状态,xy=[yo ψ r vy]′;uy为车辆侧向控制量,uy=δ;δ为车辆的前轮转角,单位rad,是由侧向控制模型得出的:δ=fy(R(l,T,xo),xy)=R(l,T,xo)-Kxy (5)
其中,δ为车辆的前轮转角,单位rad;fy(R(l,T,xo),xy)为侧向控制律;xy为车辆侧向系统状态;K为侧向控制增益向量,向量维度是1行4列,根据车辆侧向系统状态空间模型采用线性系统控制器设计的极点配置方法获得;R(l,T,xo)是参考侧向位移,单位m,是由侧向规划模型得出的:R(l,T,xo)=(1-w(t,T))y0(xo)+w(t,T)yl(xo) (6)
其中,y0(xo)为纵向位移xo处的当前车道的中心线侧向位移,当前车道的中心线方程a0为当前车道的中心线方程三次系数,b0为当前车道的中心线方程二次系数,c0为当前车道的中心线方程一次系数,d0为当前车道的中心线方程零次系数;yl(xo)为纵向位移xo处的目标车道l的中心线侧向位移,目标车道l的中心线方程al为目标车道l的中心线方程三次系数,bl为目标车道l的中心线方程二次系数,cl为目标车道l的中心线方程一次系数,dl为目标车道l的中心线方程零次系数,l为目标车道编号决策,1≤l≤Ln且为整数,Ln为可行驶车道数量,为大于1的整数;w(t,T)为换道权重,由以下公式得出:其中,ts为侧向规划模型的执行周期,单位s;T为换道时间决策,单位s;t为当前换道时间,由以下公式得出:其中,yo为车辆侧向位移,单位m;d1为当前车道的中心线方程零次系数;d2为目标车道l的中心线方程零次系数;
结合车辆纵向系统模型和纵向控制模型,得到无人驾驶车辆决策模型:
其中,x为无人驾驶车辆决策模型的状态,x=[xx xy]′;y为无人驾驶车辆决策模型的输出;ψ为车辆横摆角,单位rad;fx(v,vx)为纵向控制律;fy(R(l,T,xo),xy)为侧向控制律;
对无人驾驶车辆决策模型离散化,得到离散化的无人驾驶车辆决策模型:
其中,x(k)为k时刻的无人驾驶车辆决策模型的状态x;x(k+1)为k+1时刻的无人驾驶车辆决策模型的状态x;为k时刻的无人驾驶车辆决策模型的状态x对时间的导数;Ts为模型离散的采样时间;y(k+1)为k+1时刻的无人驾驶车辆决策模型的输出;
步骤二、建立本车与周车的碰撞概率模型
周车轨迹预测模块输出的周车未来位置服从二维高斯分布,概率密度如下:
其中,xi(k)为第i辆周车在第k时刻的纵向预测位置,是一个随机变量,单位m;yi(k)为第i辆周车在第k时刻的侧向预测位置,是一个随机变量,单位m;μxi(k)为第i辆周车在第k时刻的纵向预测位置均值;μyi(k)为第i辆周车在第k时刻的侧向预测位置均值;σxi(k)为第i辆周车在第k时刻的纵向预测位置方差;σyi(k)为第i辆周车在第k时刻的侧向预测位置方差;ρi(k)为第i辆周车在第k时刻的纵向预测位置相关系数;
将周车简化考虑为矩形,计算周车的四角的坐标如下:
其中,为k时刻周车i的第一角的X轴坐标,单位m;为k时刻周车i的第一角的Y轴坐标,单位m;为k时刻周车i的第二角的X轴坐标,单位m;为k时刻周车i的第二角的Y轴坐标,单位m;为k时刻周车i的第三角的X轴坐标,单位m;为k时刻周车i的第三角的Y轴坐标,单位m;为k时刻周车i的第四角的X轴坐标,单位m;为k时刻周车i的第四角的Y轴坐标,单位m;μxi(k)为第i辆周车在第k时刻的纵向预测位置均值;μyi(k)为第i辆周车在第k时刻的侧向预测位置均值;vleni为周车i对角线的半长,单位m,并且ζi为周车i的形心与周车i的第一角的连线与周车i的左侧边的夹角,单位rad,leni为周车i的车辆长度,单位m,widi为周车i的车辆宽度,单位m;θi(k)为周车i的航向角,单位rad,计算本车的四角的坐标如下:
其中,x1(k)为k时刻本车的第一角的X轴坐标,单位m;y1(k)为k时刻本车的第一角的Y轴坐标,单位m;x2(k)为k时刻本车的第二角的X轴坐标,单位m;y2(k)为k时刻本车的第二角的Y轴坐标,单位m;x3(k)为k时刻本车的第三角的X轴坐标,单位m;y3(k)为k时刻本车的第三角的Y轴坐标,单位m;x4(k)为k时刻本车的第四角的X轴坐标,单位m;y4(k)为k时刻本车的第四角的Y轴坐标,单位m;xo为车辆纵向位移,单位m;yo为车辆侧向位移,单位m;vlen为本车对角线的半长,单位m,并且ζ为本车的形心与本车的第一角的连线与本车的左侧边的夹角,单位rad,len为本车的车辆长度,单位m,wid为本车的车辆宽度,单位m;
本车与周车的碰撞概率模型如下:
其中,pi(k)为本车与周车i的碰撞概率;xi(k)为第i辆周车在第k时刻的纵向预测位置,是一个随机变量,单位m;yi(k)为第i辆周车在第k时刻的侧向预测位置,是一个随机变量,单位m;为本车与周车i的碰撞概率在坐标系X轴的积分下限,单位m;为本车与周车i的碰撞概率在坐标系X轴的积分上限,单位m;为本车与周车i的碰撞概率在坐标系Y轴的积分下限,单位m;为本车与周车i的碰撞概率在坐标系Y轴的积分上限,单位m:其中,j1=1,2,3,4分别为k时刻本车第一角的X轴坐标、第二角的X轴坐标、第三角的X轴坐标以及第四角的X轴坐标,分别为k时刻本车第一角的Y轴坐标、第二角的Y轴坐标、第三角的Y轴坐标以及第四角的Y轴坐标,分别为k时刻周车i第一角的X轴坐标、第二角的X轴坐标、第三角的X轴坐标以及第四角的X轴坐标,分别为k时刻周车i第一角的Y轴坐标、第二角的Y轴坐标、第三角的Y轴坐标以及第四角的Y轴坐标;
其中,符号{}表示集合;
步骤三、设计考虑周车随机性的无人驾驶车辆滚动优化决策优化问题
建立避撞目标Jr1:
其中,Jr1表示避撞目标;pi(k)为本车与周车i的碰撞概率;P表示预测步长,为大于1的整数;Σ表示连加符号;Π表示连乘符号;n表示周围车辆数量;
建立快速目标Js1:
其中,Js1表示快速目标,v(k)为k时刻车辆速度决策,单位m/s;N为控制步长,1≤N≤P且为整数,P表示预测步长;vref为期望车速,单位m/s,vref=τvroadmax+(1-τ)vroadmin;vroadmax为道路最高限速,单位m/s;vroadmin为道路最低限速,单位m/s;τ为车速因子,0≤τ≤1;
建立车道保持目标Js2:
其中,Js2表示车道保持目标;N为控制步长;Δl(k)为车道变化量,Δl(k)=l(k)-l(k-1),l(k)为k时刻的目标车道编号决策;
建立舒适换道目标Js3:
其中,Js3表示舒适换道目标;N为控制步长;T(k)为k时刻的换道时间决策;
建立车道约束:
1≤l(k)≤Ln,l∈Z (20)
l(k)为k时刻的目标车道编号决策;Ln为可行驶车道数量;Z表示整数集;
建立换道时间约束:
Tmin≤T(k)≤Tmax (21)
其中T(k)为k时刻的换道时间决策;Tmin为换道时间下限;Tmax为换道时间上限;
建立速度变化量约束:
Δvmin≤Δv(k)≤Δvmax (22)
Δv(k)为速度变化量,单位m/s,Δv(k)=v(k)-Δv(k-1),v(k)为k时刻车辆速度决策;Δvmin为速度变化量下限,单位m/s,Δvmax为速度变化量上限,单位m/s;
建立速度约束:
0≤v(k)≤vroadmax (23)其中v(k)为k时刻车辆速度决策;vroadmax为道路最高限速;
采用模型预测控制方法,基于离散化的无人驾驶车辆决策模型预测系统未来状态,定义车辆速度决策控制序列vV=[v(1) v(1) … v(N)]、目标车道编号控制序列lV=[l(1) …l(N)]、换道时间控制序列TV=[T(1) … T(N)],其中,N为模型预测控制方法的控制时域,v(1)为1时刻车辆速度决策,v(2)为2时刻车辆速度决策,v(N)为N时刻车辆速度决策,l(1)为1时刻的目标车道编号决策,l(2)为2时刻的目标车道编号决策,l(N)为N时刻的目标车道编号决策,T(1)为1时刻的换道时间决策,T(2)为2时刻的换道时间决策,T(N)为N时刻的换道时间决策;将避撞目标、快速目标、车道保持目标、舒适换道目标加权,并结合车道约束、换道时间约束速度变化量约束、速度约束,得到考虑周车随机性的无人驾驶车辆滚动优化决策优化问题:其中,vV=[v(1) v(1) … v(N)]为车辆速度决策控制序列、lV=[l(1) … l(N)]为目标车道编号控制序列、TV=[T(1) … T(N)]为换道时间控制序列,J为考虑周车随机性的无人驾驶车辆滚动优化决策优化目标函数;Jr1为避撞目标;C为危险偏移常数,C>1;pi(k)为本车与周车i的碰撞概率;ε为碰撞风险容忍因子,0<ε<1;Js1为快速目标;Js2为车道保持目标;Js3为舒适换道目标;Γs1为快速目标权重;Γs2为车道保持目标权重;Γs3为舒适换道目标权重;Γr1为避撞目标权重;Jsmin为安全目标Js的最小值,Jsmax为安全目标Js的最大值,Js为安全目标:Js=Γs1Js1+Γs2Js2+Γs3Js3+Γr1Jr1 (24)其中,Jr1为避撞目标;Js1为快速目标;Js2为车道保持目标;Js3为舒适换道目标;Γs1为快速目标权重;Γs2为车道保持目标权重;Γs3为舒适换道目标权重;Γr1为避撞目标权重;
将车辆速度决策控制序列vV=[v(1) v(1) … v(N)]、目标车道编号控制序列lV=[l(1) … l(N)]和换道时间控制序列TV=[T(1) … T(N)]共同作为待优化变量,采用遗传算法求解考虑周车随机性的无人驾驶车辆滚动优化决策优化问题,将使无人驾驶车辆滚动优化决策优化目标函数J最小化的车辆速度决策控制序列vV=[v(1) v(1) … v(N)]、目标车道编号控制序列lV=[l(1) … l(N)]和换道时间控制序列TV=[T(1) … T(N)]定义为最优控制序列其中为最优车辆速度控制序列;为最优目标车道编号控制序列;为最优换道时间控制序列,*表示变量为最优的;
当无人驾驶车辆滚动优化决策系统状态被实时采集并更新时,就求解考虑周车随机性的无人驾驶车辆滚动优化决策优化问题得到最优控制序列并将最优车辆速度控制序列的第一个元素通过纵向控制律fx(v,vx)转换为车辆纵向加速度ax,最优目标车道编号决策向量的第一个元素最优换道时间决策向量的第一个元素通过侧向控制律fy(R(l,T,xo),xy)转换为车辆的前轮转角δ,以实现对车辆的纵向和侧向的控制。