1.一种考虑安全边界约束的自动驾驶车辆决策与规控方法,其特征在于,包括以下步骤:步骤S1:搭建行为决策模块,基于动态规划算法的行为决策模块对车辆前方道路进行采样,并将采样点映射到S‑L和S‑T图上,以五次多项式对采样点进行连接,构建相应的成本函数,以动态规划算法对成本函数进行求解,将得到的路径决策信息及速度决策信息作用于规划控制器;
步骤S2:由车辆三自由度耦合模型得到横纵耦合的非线性预测模型,选择前轮转向角与前后轮驱动力作为控制量,考虑车辆物理约束和安全边界约束,利用模型预测控制算法构造代价函数,并对其进行求解,得到控制信号,完成轨迹规划任务;
步骤S3:将获得的控制信号作用于车辆系统,实现车辆轨迹规划及路径跟踪的控制效果。
2.根据权利要求1所述的考虑安全边界约束的自动驾驶车辆决策与规控方法,其特征在于,所述步骤S1中,路径决策的具体方法如下:对自车前方道路横纵向分别进行等间隔路径点的采样,相邻列的采样点之间通过五次多项式曲线平滑连接;在Frenet坐标系下基于当前车道中心线对车辆行驶路径进行决策,以使车辆沿无碰撞路径行驶;
基于纵向路径采样的五次多项式有下列等式成立:
构造五次多项式曲线后,通过成本函数的求和来评估路径优劣,总成本函数是平滑度、避障和参考线成本函数的线性组合,总成本函数的公式如下:Ct(f)=Cs(f)+Co(f)+Cg(f)
其中,Ct(f)为总成本函数,其在总体上衡量路径的优劣程度;Cs(f)为平滑度成本函数,其作用为衡量路径的平滑程度;Co(f)为避障成本函数,其作用为衡量路径避障效果的优劣;Cg(f)为参考线成本函数,其作用为衡量车辆跟随车道参考线的能力。
3.根据权利要求2所述的考虑安全边界约束的自动驾驶车辆决策与规控方法,其特征在于,路径的所述平滑度成本函数通过下式衡量:
2 2
Cs(f)=w1∫(f′(s)) ds+w2∫(f″(s)) ds其中,w1,w2分别为路径五次多项式的一阶导和二阶导的代价权重系数;
路径的避障成本函数基于障碍物和汽车之间的距离进行设置,将其距离表示为d,具体表达式如下:其中,Cn为单调递减函数,Cc为碰撞成本,ds为安全距离,dc为危险距离;
路径的参考线成本函数设置如下:当路径周围没有障碍物时,参考线为路径的中心线,将其功能定义为g(s),参考线成本函数通过下式衡量:2
Cg(f)=∫(f(s)‑g(s)) ds。
4.根据权利要求1所述的考虑安全边界约束的自动驾驶车辆决策与规控方法,其特征在于,所述步骤S1中,利用动态规划算法将多阶段决策问题转化为一系列单阶段最优化问题,逐步求解以完成决策过程,求解待定系数,从而得到路径决策信息,具体方法如下:将车辆前方路径采样点作为研究对象;
因为从起点到第i列采样点的成本的计算结果,是基于从起点到i‑1列所有采样点的总成本计算之和,所以将从起点到每一列路径采样点的总成本作为一个阶段;
将每个阶段决策问题转化为单阶段最优化问题,得到成本最小的路径信息。
5.根据权利要求3所述的考虑安全边界约束的自动驾驶车辆决策与规控方法,其特征在于,所述步骤S1中,速度决策的具体方法如下:将障碍物信息离散为S‑T图上的矩形边框,将(t0,t1,…,tn)表示为时间轴上间隔为dt的等距点;分段线性速度分布函数表示为S=(s0,s1,…,sn);在Frenet坐标系下基于当前车道中心线对车辆的速度进行决策,利用动态规划算法逐步求解以完成决策过程,求出待定系数,得到速度决策信息;
基于横向速度采样的五次多项式有下列等式成立:
构造五次多项式曲线后,通过成本函数的求和来评估速度大小,总成本函数是平滑度、避障和参考速度成本函数的线性组合,总成本函数的公式如下:Ct(S)=Cs(S)+Co(S)+Cg(S)
其中,Ct(S)为总成本函数,其在总体上衡量当前速度的合理性;Cs(S)为速度平滑度成本函数,其作用为衡量速度变化的平滑程度;Co(S)为避障成本函数,其用来表征避障时速度的变化过程;Cg(S)为参考速度成本函数,其用来衡量车辆跟随参考速度的能力。
6.根据权利要求5所述的考虑安全边界约束的自动驾驶车辆决策与规控方法,其特征在于,速度的所述平滑度成本函数通过下式衡量:速度的避障成本函数是基于S‑T图中障碍物和车辆之间的距离,其表达式与路径的避障成本函数相同;
参考速度成本函数表达式如下:
参考速度成本函数表示当没有障碍物或红绿灯限制时,车辆应遵循指定速度,Vref描述了由道路速度限制、曲率和其他交通法规确定的参考速度。
7.根据权利要求1所述的考虑安全边界约束的自动驾驶车辆决策与规控方法,其特征在于,所述步骤S2中,将车辆平面运动动力学模型的两个前轮和两个后轮分别一个等效的前轮与后轮在车辆的轴线方向进行代替,得到车辆三自由度耦合模型;纵向车速、侧向车速和横摆运动方程如下:其中,vx为纵向速度,vy为横向速度,ωr为横摆角速度,δ为前轮转角,Fxr为后轮纵向力,Fxf为前轮纵向力,Fyr为后轮侧向力,Fyf为前轮侧向力,m为车身质量,lf为前轴距,lr为后轴距,Iz为转动惯量;
车辆在大地坐标系下的运动学方程如下:
其中,X、Y分别为纵向和横向位置,ψ为横摆角;
采用轮胎刷子模型计算轮胎侧向力,其计算公式如下所示:
其中,Fy为轮胎侧向力,Ca为侧偏刚度,α为轮胎侧偏角,μ为路面附着系数,Fz为车辆轮胎垂向载荷;
前轮侧偏角及后轮侧偏角计算公式如下所示:
其中,αf和αr分别为前轮侧偏角和后轮侧偏角。
8.根据权利要求7所述的考虑安全边界约束的自动驾驶车辆决策与规控方法,其特征在于,所述步骤S2中,考虑安全边界约束的非线性模型预测规划控制器设计步骤如下:建立车辆控制器预测模型,通过车辆三自由度自行车模型得到车辆的控制器预测模型如下:选取纵向速度vx、侧向速度vy、横摆角ψ、横摆角速度ωr、质心侧偏角β、大地坐标系下横向位移X和大地坐标系下纵向位移Y作为状态量,即:假设状态量全部可测,车辆前轮转角δ,以及车辆纵向力Fx为控制量,即:u=[δ,Fx]
采用欧拉方法对控制器预测模型进行离散,Ts为采样时间,在时刻k,离散后的预测模型为:记Np,Nc分别为预测时域和控制时域,并且满足Nc≤Np;则在k时刻,有如下序列:其中,U(k)为系统控制序列, 为状态序列;u(k|k)、u(k+1|k)、…、u(k+Nc‑1|k)分别为在k时刻,对k时刻、k+1时刻、…、k+Nc‑1时刻控制量u的预测;
分别为在k时刻,对k时刻、k+1时刻、…、k+Np‑1时刻状态量 的预测;
控制器的目标函数为:
其中,Y(k+i|k)为在k时刻,对k+i时刻纵向位置Y的预测;Yref(k+i)为k+i时刻的参考纵向位置;vx(k+i|k)为在k时刻,对k+i时刻纵向速度vx的预测;vxref(k+i)为k+i时刻的参考纵向速度;ψ(k+i|k)为在k时刻,对k+i时刻横摆角ψ的预测;ψref(k+i)为k+i时刻的参考横摆角;Δu(k+i|k)为在k时刻,对k+i时刻控制量u的预测增量;X(k+i|k)为在k时刻,对k+i时刻横向位置X的预测;Xobs(k+i)为k+i时刻的障碍物横向位置;Yobs(k+i)为k+i时刻的障碍物纵向位置;目标函数的前三项表征系统对于决策模块的给出的参考路径和速度信息的跟踪能力,第四项表征系统的平稳性,第五项为障碍物的碰撞函数。
9.根据权利要求8所述的考虑安全边界约束的自动驾驶车辆决策与规控方法,其特征在于,车辆行驶位置的所述安全边界约束为:其中,dc为车身宽度,Y(k+i|k)为在k时刻,对k+i时刻纵向位置Y的预测;Yref(k+i)为k+i时刻的参考纵向位置;Yl(k+i|k)为在k时刻,对k+i时刻的左侧安全位置边界线的预测;Yr(k+i|k)为在k时刻,对k+i时刻的右侧安全位置边界线的预测;在车辆行驶过程中,车辆位置相对变化的位置边界通过实时计算得到;
由车辆动力学可知,横摆角速度最大值和质心侧偏角的最大值由下式得到:其中,μ为路面附着系数,αrs为后轮侧偏角饱和值;ωrmax(k+i|k)为在k时刻,对k+i时刻横摆角速度的预测最大值;βmax(k+i|k)为在k时刻,对k+i时刻质心侧偏角的预测最大值;vx(k+i|k)为在k时刻,对k+i时刻纵向速度的预测;ωr(k+i|k)为在k时刻,对k+i时刻横摆角速度ωr的预测;
车辆运动状态的安全边界约束如下:
假设μ、ωr和vx可实时估计,则车辆运动状态安全边界的边界线可通过实时计算得到;
其中,ωr(k+i|k)为在k时刻,对k+i时刻横摆角速度ωr的预测;β(k+i|k)为在k时刻,对k+i时刻质心侧偏角β的预测;前轮转角δ、车辆驱动力Fx的变化值均在允许的最小值与最大值之间;
umin≤u(k+i|k)≤umax,i=0,1,...,Nc‑1其中,u(k+i|k)为在k时刻,对k+i时刻控制量u的预测;umin和umax的定义分别为:其中,δmin和δmax分别为前轮转角最小值和最大值,Fxmin和Fxmax分别为车辆驱动力的最小值和最大值。
10.根据权利要求1所述的考虑安全边界约束的自动驾驶车辆决策与规控方法,其特征在于,所述车辆轨迹规划和路径跟踪控制问题描述为如下形式的优化问题:在每个采样时刻在线进行数值优化求解,实现规划控制系统的控制目标。