CN115817509A - 一种基于ampc的多轴分布式驱动车辆转向辅助轨迹跟踪方法 - Google Patents

Abstract

本发明提供了一种基于AMPC的多轴分布式驱动车辆转向辅助轨迹跟踪方法，基于线性二自由度的单轨模型建立预测模型，能使车辆在轮胎力处于线性区时实现较高的预测精度；驾驶员模型采用了线性二次型调节器，能够尽可能模拟熟练驾驶员跟踪预想轨迹的条件；方法中考虑干预辅助量、参考轨迹跟踪误差、前轮转角及其变化率等多方面因素构造多成本目标函数，并且引入了驾驶权衰减系数和驾驶权重系数来衡量与表征预测窗口内对驾驶员意图的置信程度，最终求解得到的最优前轮转角输出值，能够使车辆迅速平稳的到达目标轨迹，且在过程中减少抖动并能保证平顺性。

Description

一种基于AMPC的多轴分布式驱动车辆转向辅助轨迹跟踪方法

技术领域

本发明属于智能电动汽车辅助驾驶技术领域，具体涉及一种基于AMPC(AdaptiveModel Predictive Control，自适应模型预测控制)的多轴分布式驱动车辆转向辅助轨迹跟踪方法。

背景技术

目前，适用于智能电动汽车的辅助驾驶技术借助不同类型传感器对环境信息的感知能力，利用高算力的车载计算平台对感知信息的处理和整合，能够在一定程度上辅助驾驶员决策并提高车辆行驶安全性。然而，本领域中大多现有技术主要是针对乘用车辆设计，但对于部分特殊的商用车辆并不能良好地适用，尤其是对于一些多轴的商用运输车辆，由于其重量大、车身体积大、驾驶舱较高等特点，导致驾驶员的驾驶视野普遍不佳，而且较差的车身灵活性与制动性能，也使得驾驶难度相对于乘用车高出很多，驾驶员对于车辆周围环境以及车身姿态都不容易把控。长途的驾驶运输任务中，疲劳、分神等问题也存在严重安全隐患。此类车辆的辅助驾驶中必须要兼顾作业任务，但由于现有任务型辅助驾驶系统多需要频繁的人机交替驾驶，导致驾驶员对其的接受和信任程度不高，并且在人机协同的辅助驾驶轨迹跟踪中，系统对驾驶员意图轨迹和权限的引入，仍缺乏较有效的解决手段。因此，本领域中迫切需要针对多轴分布式驱动车辆的特点，提供更加完善的辅助驾驶与转向轨迹跟踪策略。

发明内容

有鉴于此，针对上述本领域中存在的技术问题，本发明提供了一种基于AMPC的多轴分布式驱动车辆转向辅助轨迹跟踪方法，具体包括以下步骤：

S1、在上层控制中，针对n轴分布式驱动车辆，以车辆的横纵坐标xy、纵向速度v_x、航向角

侧向速度v_y和横摆角速度ω_r作为输入变量，建立线性二自由度单轨车辆模型对未来车辆的动力学状态进行预测，并输出预测的最优方向盘转角控制量、预测的轨迹与参考轨迹的侧向位置误差e_d和航向角误差

所述参考轨迹为所提方法外部输入信息，为辅助跟踪控制所需要的已知量；

S2、在上层控制中，以车辆的横纵坐标xy、纵向速度v_x、航向角

侧向速度v_y和横摆角速度ω_r作为输入变量，建立线性二次型调节驾驶员模型，以累计侧向误差、横向误差及方向盘转角加权和最小构建代价函数，计算驾驶员意图前轮转角；

S3、在下层控制中，定义由上层步骤S1预测的最优方向盘转角控制量与步骤S2得到的驾驶员意图前轮转角确定的前轮转角差为干预辅助量，并以所述干预辅助量、所述预测的轨迹与参考轨迹的侧向位置误差e_d和航向角误差

均达到最小作为优化目标，同时将方向盘转角及其变化率均达到最小也作为优化目标，综合以上优化目标构建多目标成本函数；

S4、基于所述多目标成本函数求解下一时刻最优前轮转角输出值。

进一步地，步骤S1中所述线性二自由度单轨车辆模型具体构建过程如下：

对五轴分布式驱动车辆进行动力学分析，并考虑控制量u(k)＝[δ_r(k)]，δ_r为前轮转角，扰动量v(k)＝[ρ(k)]，ρ为道路曲率，设车辆动力学状态量：

其中，e_d和

分别为侧向位置误差和航向角误差；建立以下连续状态空间方程：

Z＝C_cx+D_u，cu

式中，C_i为第i轴的侧偏刚度，L_i为第i轴距离车辆质心的长度，I为车辆沿横摆方向的转动惯量，Cc为连续状态方程的输出矩阵；

式中，

D_u，c＝O

进行离散化得到离散状态空间方程：

x(k+1)＝A_dx(k)+B_u，du(k)+B_v，dv(k)

Z(k)＝C_dx(k)

式中，k表示时间，A_d是离散状态空间方程的状态矩阵，B_u，d是离散状态空间方程的控制矩阵，B_v，d是离散状态空间方程的可测扰动矩阵，下标d表示对相应参数的离散处理，Cd为Cc相同形式的离散状态方程的输出矩阵；

则系统未来p个时刻动力学状态的迭代形式为：

X_p＝[x(k)，x(k+1)，…，x(k+p)]^T

U_p＝[u(k)，u(k+1)，…，u(k+p)]^T

V_p＝[v(k)，v(k+1)，...，v(k+p)]^T

其中，U_p为自适应模型预测控制求解的最优序列，p为预测时间窗口，上标T表示向量的转置；

在预测时域窗口内，模型预测输出表示为：

z(k+p|k)＝C_dx(k+p|k)

Z_p＝[y(k)，y(k+1)，...，y(k+p)]^T；

式中，Z_p为k时刻在预测时间窗口p内的模型预测输出序列，y为该预测时间窗口内的单个模型预测输出；

所述最优方向盘转角控制量、预测的轨迹与参考轨迹的侧向位置误差e_d和航向角误差

定义为：

式中，P_ego和P_i分别为车辆当前位置点和参考轨迹上各个离散点，β为质心侧偏角看，

为车辆所在道路映射点处切线在全局坐标系下的夹角，上标·表示对相应参数求导。

进一步地，步骤S2中所述线性二次型调节驾驶员模型具体建立过程包括：

假设在实际道路中道路曲率较小则：

式中，

k_r为可测干扰项，u_x为车辆纵向速度，前轮转角δ_f为控制量；

建立以下状态空间方程：

式中，

d的含义与可测干扰项k_r相同；

构建以下关于累计侧向误差、横向误差及方向盘转角加权和代价函数

式中，Q和R分别为状态量和控制量的加权矩阵，均为半正定矩阵，具体表示为：

矩阵中元素均表示具体参数的权重；

在求解该使代价函数最小的驾驶员意图前轮转角过程中，引入曲率前馈环节u_f，则控制量可表示为：

u(k)＝-Kx(k)+u_f

利用代数黎卡提方程

另e_d＝0来求解最优控制量u(k)得到相应驾驶员意图前轮转角；其中K为状态反馈增益，具体为

进一步地，步骤S3中定义步骤S1中预测的状态变量为：

分别针对预测的轨迹与参考轨迹的侧向位置误差e_d和航向角误差

定义道路跟踪误差成本函数：

J_z，k(k)＝[z(k)-z_ref(k)]²

式中，下标ref代表参考轨迹；

定义干预辅助量成本函数：

J_Δu，k(k)＝[u_Out(k)-u_d(k)]²

式中，u_out(k)为步骤S1中模型预测的控制量，u_d(k)为步骤S2得到的驾驶员意图控制量；

以及定义方向盘转角及其变化率成本函数J_u，k(k)和J_du，k(k)；

综合各成本函数构建以下多目标成本函数：

J_k＝J_zk+J_uk+J_duk+J_Δuk+J_εk

其中，J_εk＝ρ_ε·ε_k ²为松弛变量的代价函数，用于惩罚状态量、控制量等对优化问题中的软约束的违反程度，ε_k为松弛变量，ρ_ε为权重系数，且满足ρ_ε＞0。

道路跟踪误差成本函数J_zk的具体形式可表示为：

其中，s_z为归一化系数对角矩阵，具体表示为：

矩阵中各元素表示状态向量对应元素的归一化系数；

Z(k+h|k)与Z_ref(k+h|k)分别为k+h时刻的预测状态以及参考状态，Q(k+h|k)为用于惩罚跟踪误差的半正定对角代价权重矩阵，在滚动优化的过程中，将进行实时的调整更新；

干预辅助量成本函数、定义方向盘转角及其变化率成本函数的具体形式为：

其中，R(K)，E(k)，F(k)为t＝k时刻的权重矩阵，u(k+h|k)表示k时刻预测最优控制量，u_d(k+h|k)为k时刻驾驶员输入意图控制量；

上述各归一化系数分别由对应参数的最大值和最小值确定：

引入驾驶权衰减系数Γ和驾驶权重系数Θ，分别用于在预测时间窗口内衡量对驾驶员意图输入量的置信程度以及控制人机间驾驶权重，并构造成本函数以下J_k求解所述系数：

J_k＝(1-Γ(t))·Q_Track(k)·J_Track，k+ΘΓ(t)J_Δuk+n_ε·max{Γ(t)，(1-Γ(t))}·J_εk

系数

其中，n_Γ，min表示预测时间窗口t＝1，2，...，p内的短期预测部分，即预测步长的前几个步长，n_Γ，max表示预测时间窗口内的长期预测部分，即预测步长的后几个步长，ΔT为预测采样时间间隔，J_track，，k为反映车辆状态的部分代价函数记为：J_track，k＝[J_zk，J_uk，J_duk]^T；

Q(k)，R(k)，E(k)，F(k)分别表示为：

Q(k)＝Q_z，Track(k)·(1-Γ(t))

R(k)＝Q_u，Track(k)·(1-Γ(t))

E(k)＝Q_du，Track(k)·(1-Γ(t))

F(k)＝ΘΓ(t)

其中，Q_z，Track、Q_u，Track、Q_du，Track分别为道路跟踪误差成本函数J_zk的权重系数、干预辅助量成本函数J_uk的权重系数、方向盘转角成本函数J_duk的权重系数；

通过求解最终得到最优的前轮转角输出值。

上述本发明所提供的基于AMPC的多轴分布式驱动车辆转向辅助轨迹跟踪方法，基于线性二自由度的单轨模型建立预测模型，能使车辆在轮胎力处于线性区时实现较高的预测精度；驾驶员模型采用了线性二次型调节器，能够尽可能模拟熟练驾驶员跟踪预想轨迹的条件；方法中考虑干预辅助量、参考轨迹跟踪误差、前轮转角及其变化率等多方面因素构造多成本目标函数，并且引入了驾驶权衰减系数和驾驶权重系数来衡量与表征预测窗口内对驾驶员意图的置信程度，最终求解得到的最优前轮转角输出值，能够使车辆迅速平稳的到达目标轨迹，且在过程中减少抖动并能保证平顺性。

附图说明

图1为本发明所提供方法的原理框架图；

图2为针对五轴分布式驱动车辆平面结构和动力学原理框图；

图3为本发明所提供方法对驾驶意图转角控制与参考轨迹误差控制的原理图；

图4为本发明所提供方法引入的驾驶权衰减系数动态特性图。

具体实施方式

下面将结合附图对本发明的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明所提供的种基于AMPC的多轴分布式驱动车辆转向辅助轨迹跟踪方法，如图1所示，具体包括以下步骤：

S1、在上层控制中，针对n轴分布式驱动车辆，以车辆的横纵坐标xy、纵向速度v_x、航向角

侧向速度v_y和横摆角速度ω_r作为输入变量，建立线性二自由度单轨车辆模型对未来车辆的动力学状态进行预测，并输出预测的最优方向盘转角控制量、预测的轨迹与参考轨迹的侧向位置误差e_d和航向角误差

所述参考轨迹为所提方法外部输入信息，为辅助跟踪控制所需要的已知量；

S2、在上层控制中，以车辆的横纵坐标xy、纵向速度v_x、航向角

侧向速度v_y和横摆角速度ω_r作为输入变量，建立线性二次型调节驾驶员模型，以累计侧向误差、横向误差及方向盘转角加权和最小构建代价函数，计算驾驶员意图前轮转角；

S3、在下层控制中，定义由上层步骤S1预测的最优方向盘转角控制量与步骤S2得到的驾驶员意图前轮转角确定的前轮转角差为干预辅助量，并以所述干预辅助量、所述预测的轨迹与参考轨迹的侧向位置误差e_d和航向角误差

均达到最小作为优化目标，同时将方向盘转角及其变化率均达到最小也作为优化目标，综合以上优化目标构建多目标成本函数；

S4、基于所述多目标成本函数求解下一时刻最优前轮转角输出值。

在本发明的一个优选实施方式中，步骤S1中所述线性二自由度单轨车辆模型具体构建过程如下：

对五轴分布式驱动车辆进行如图2所示的动力学分析，并考虑控制量u(k)＝[δ_r(k)]，δ_r为前轮转角，扰动量v(k)＝[ρ(k)]，ρ为道路曲率，设车辆动力学状态量：

其中，e_d和

分别为侧向位置误差和航向角误差；建立以下连续状态空间方程：

z＝C_cx+D_u，cu

式中，

D_u，c＝O

式中，C_i为第i轴的侧偏刚度，L_i为第i轴距离车辆质心的长度，I为车辆沿横摆方向的转动惯量，Cc为连续状态方程的输出矩阵；

进行离散化得到离散状态空间方程：

x(k+1)＝A_dx(k)+B_u，du(k)+B_v，dv(k)

z(k)＝C_dx(k)

式中，k表示时间，A_d是离散状态空间方程的状态矩阵，B_u，d是离散状态空间方程的控制矩阵，B_v，d是离散状态空间方程的可测扰动矩阵，下标d表示对相应参数的离散处理，Cd为Cc相同形式的离散状态方程的输出矩阵；

则系统未来p个时刻动力学状态的迭代形式为：

X_p＝[x(k)，x(k+1)，...，x(k+p)]^T

U_p＝[u(k)，u(k+1)，...，u(k+p)]^T

V_p＝[v(k)，v(k+1)，...，v(k+p)]^T

其中，U_p为自适应模型预测控制求解的最优序列，p为预测时间窗口，上标T表示向量的转置；

在预测时域窗口内，模型预测输出表示为：

z(k+p|k)＝C_dx(k+p|k)

Z_p＝[y(k)，y(k+1)，...，y(k+p)]^T；

式中，Z_p为k时刻在预测时间窗口p内的模型预测输出序列，y为该预测时间窗口内的单个模型预测输出；

所述最优方向盘转角控制量、预测的轨迹与参考轨迹的侧向位置误差e_d和航向角误差

定义为：

式中，P_ego和P_i分别为车辆当前位置点和参考轨迹上各个离散点，β为质心侧偏角看，

为车辆所在道路映射点处切线在全局坐标系下的夹角，上标·表示对相应参数求导；

在本发明的一个优选实施方式中，步骤S2中所述线性二次型调节驾驶员模型具体建立过程包括：

假设在实际道路中道路曲率较小则：

式中，

k_r为可测干扰项，u_x为车辆纵向速度，前轮转角δ_f为控制量；

建立以下状态空间方程：

式中，

d的含义与可测干扰项k_r相同；

构建以下关于累计侧向误差、横向误差及方向盘转角加权和代价函数

式中，Q和R分别为状态量和控制量的加权矩阵，均为半正定矩阵，具体表示为：

矩阵中元素均表示具体参数的权重；

在求解该使代价函数最小的驾驶员意图前轮转角过程中，引入曲率前馈环节u_f，则控制量可表示为：

u(k)＝-Kx(k)+u_f

利用代数黎卡提方程

另e_d＝0来求解最优控制量u(k)得到相应驾驶员意图前轮转角；其中K为状态反馈增益，具体为

在本发明的一个优选实施方式中，如图3所示，步骤S3中定义步骤S1中预测的状态变量为：

分别针对预测的轨迹与参考轨迹的侧向位置误差e_d和航向角误差

定义道路跟踪误差成本函数：

J_z，k(k)＝[z(k)-z_ref(k)]²

式中，下标ref代表参考轨迹；

定义干预辅助量成本函数：

J_Δu，k(k)＝[u_out(k)-u_d(k)]²

式中，u_out(k)为步骤S1中模型预测的控制量，u_d(k)为步骤S2得到的驾驶员意图控制量；

以及定义方向盘转角及其变化率成本函数J_u，k(k)和J_du，k(k)；

综合各成本函数构建以下多目标成本函数：

J_k＝J_zk+J_uk+J_duk+J_Δuk+J_εk

其中，J_εk＝ρ_ε·ε_k ²为松弛变量的代价函数，用于惩罚状态量、控制量等对优化问题中的软约束的违反程度，ε_k为松弛变量，ρ_ε为权重系数，且满足ρ_ε＞0。

道路跟踪误差成本函数J_zk的具体形式可表示为：

其中，s_z为归一化系数对角矩阵，具体表示为：

矩阵中各元素表示状态向量对应元素的归一化系数；

Z(k+h|k)与Z_ref(k+h|k)分别为k+h时刻的预测状态以及参考状态，Q(k+h|k)为用于惩罚跟踪误差的半正定对角代价权重矩阵，在滚动优化的过程中，将进行实时的调整更新；

干预辅助量成本函数、定义方向盘转角及其变化率成本函数的具体形式为：

其中，R(k)，E(k)，F(k)为t＝k时刻的权重矩阵，u(k+h|k)表示k时刻预测最优控制量，u_d(k+h|k)为k时刻驾驶员输入意图控制量；

上述各归一化系数分别由对应参数的最大值和最小值确定：

引入驾驶权衰减系数Γ和驾驶权重系数Θ，分别用于在预测时间窗口内衡量对驾驶员意图输入量的置信程度以及控制人机间驾驶权重，并构造成本函数以下J_k求解所述系数：

J_k＝(1-Γ(t))·Q_Track(k)·J_Track，k+ΘΓ(t)J_Δuk+n_ε·max{Γ(t)，(1-Γ(t))}·J_εk

系数

其中，n_Γ，min表示预测时间窗口t＝1，2，...，p内的短期预测部分，即预测步长的前几个步长，n_Γ，max表示预测时间窗口内的长期预测部分，即预测步长的后几个步长，ΔT为预测采样时间间隔，J_track，k为反映车辆状态的部分代价函数记为：J_track，k＝[J_zk，J_uk，J_duk]^T；

Q(k)，R(k)，E(k)，F(k)分别表示为：

Q(k)＝Q_z，Track(k)·(1-Γ(t))

R(k)＝Q_u，Track(k)·(1-Γ(t))

E(k)＝Q_du，Track(k)·(1-Γ(t))

F(k)＝ΘΓ(t)

其中，Q_z，Track、Q_u，Track、Q_du，Track分别为道路跟踪误差成本函数J_zk的权重系数、干预辅助量成本函数J_uk的权重系数、方向盘转角成本函数J_duk的权重系数；

通过求解最终得到最优的前轮转角输出值。驾驶衰减系数动态变化如图4所示，随时时间的延长呈现出e指数的衰减效果，使得短期预测较多的考虑满足驾驶员的输入，长期的预测更加依赖于参考目标轨迹。

应理解，本发明实施例中各步骤的序号的大小并不意味着执行顺序的先后，各过程的执行顺序应以其功能和内在逻辑确定，而不应对本发明实施例的实施过程构成任何限定。

尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。

Claims (4)

1.一种基于AMPC的多轴分布式驱动车辆转向辅助轨迹跟踪方法，其特征在于：具体包括以下步骤：

S1、在上层控制中，针对n轴分布式驱动车辆，以车辆的横纵坐标x_y、纵向速度v_x、航向角

侧向速度v_y和横摆角速度ω_r作为输入变量，建立线性二自由度单轨车辆模型对未来车辆的动力学状态进行预测，并输出预测的最优方向盘转角控制量、预测的轨迹与外部输入的参考轨迹的侧向位置误差e_d和航向角误差

S2、在上层控制中，以车辆的横纵坐标xy、纵向速度v_x、航向角

侧向速度v_y和横摆角速度ω_r作为输入变量，建立线性二次型调节驾驶员模型，以累计侧向误差、横向误差及方向盘转角加权和最小构建代价函数，计算驾驶员意图前轮转角；

S3、在下层控制中，定义由上层步骤S1预测的最优方向盘转角控制量与步骤S2得到的驾驶员意图前轮转角确定的前轮转角差为干预辅助量，并以所述干预辅助量、所述预测的轨迹与参考轨迹的侧向位置误差e_d和航向角误差

均达到最小作为优化目标，同时将方向盘转角及其变化率均达到最小也作为优化目标，综合以上优化目标构建多目标成本函数；

S4、基于所述多目标成本函数求解下一时刻最优前轮转角输出值。

2.如权利要求1所述的方法，其特征在于：步骤S1中所述线性二自由度单轨车辆模型具体构建过程如下：

对五轴分布式驱动车辆进行动力学分析，并考虑控制量u(k)＝[δ_r(k)]，δ_r为前轮转角，扰动量v(k)＝[ρ(k)]，ρ为道路曲率，设车辆动力学状态量：

其中，e_d和

分别为侧向位置误差和航向角误差；建立以下连续状态空间方程：

z＝C_cx+D_u，cu

式中，

式中，C_i为第i轴的侧偏刚度，L_i为第i轴距离车辆质心的长度，I为车辆沿横摆方向的转动惯量，Cc为连续状态方程的输出矩阵；

进行离散化得到离散状态空间方程：

x(k+1)＝A_dx(k)+B_u，du((k)+B_v，dv(k)

z(k)＝C_dx(k)

式中，k表示时间，A_d是离散状态空间方程的状态矩阵，B_u，d是离散状态空间方程的控制矩阵，B_u，d是离散状态空间方程的可测扰动矩阵，下标d表示对相应参数的离散处理，Cd为Cc相同形式的离散状态方程的输出矩阵；

则系统未来p个时刻动力学状态的迭代形式为：

X_p＝[x(k)，x(k+1)，...，x(k+p)]^T

U_p＝[u(k)，u(k+1)，...，u(k+p)]^T

V_p＝[v(k)，v(k+1)，...，v(k+p)]^T

其中，U_p为自适应模型预测控制求解的最优序列，p为预测时间窗口，上标T表示向量的转置；

在预测时域窗口内，模型预测输出表示为：

z(k+p|k)＝C_dx(k+p|k)

Z_p＝[y(k)，y(k+1)，...，(k+p)]^T；

式中，Z_p为k时刻在预测时间窗口p内的模型预测输出序列，y为该预测时间窗口内的单个模型预测输出；

所述最优方向盘转角控制量、预测的轨迹与参考轨迹的侧向位置误差e_d和航向角误差

定义为：

式中，P_ego和P_i分别为车辆当前位置点和参考轨迹上各个离散点，β为质心侧偏角看，

为车辆所在道路映射点处切线在全局坐标系下的夹角，上标·表示对相应参数求导。

3.如权利要求2所述的方法，其特征在于：步骤S2中所述线性二次型调节驾驶员模型具体建立过程包括：

假设在实际道路中道路曲率较小则：

式中，

k_r为可测干扰项，u_x为车辆纵向速度，前轮转角δ_f为控制量；

建立以下状态空间方程：

式中，

d的含义与可测干扰项k_r相同；

构建以下关于累计侧向误差、横向误差及方向盘转角加权和代价函数

式中，Q和R分别为状态量和控制量的加权矩阵，均为半正定矩阵，具体表示为：

矩阵中元素均表示具体参数的权重；

在求解该使代价函数最小的驾驶员意图前轮转角过程中，引入曲率前馈环节u_f，则控制量可表示为：

u(k)＝-Kx(k)+u_f

利用代数黎卡提方程

另e_d＝o来求解最优控制量u(k)得到相应驾驶员意图前轮转角；其中K为状态反馈增益，具体为

4.如权利要求3所述的方法，其特征在于：步骤S3中定义步骤S1中预测的状态变量为：

分别针对预测的轨迹与参考轨迹的侧向位置误差e_d和航向角误差

定义道路跟踪误差成本函数：

J_zk(k)＝[z(k)-z_ref(k)]²

式中，下标ref代表参考轨迹；

定义干预辅助量成本函数：

J_Δu，k(k)＝[u_out(k)-u_d(k)]²

式中，u_out(k)为步骤S1中模型预测的控制量，u_d(k)为步骤S2得到的驾驶员意图控制量；

以及定义方向盘转角及其变化率成本函数J_u，k(k)和J_du，k(k)；

综合各成本函数构建以下多目标成本函数：

J_k＝J_zk+J_uk+J_duk+J_Δuk+J_εk

其中，J_εk＝ρ_ε·ε_k ²为松弛变量的代价函数，用于惩罚状态量、控制量等对优化问题中的软约束的违反程度，ε_k为松弛变量，ρ_ε为权重系数，且满足ρ_ε＞0。

道路跟踪误差成本函数J_zk的具体形式可表示为：

其中，s₂为归一化系数对角矩阵，具体表示为：

矩阵中各元素表示状态向量对应元素的归一化系数；

Z(k+h|k)与Z_ref(k+h|k)分别为k+h时刻的预测状态以及参考状态，Q(k+h|k)为用于惩罚跟踪误差的半正定对角代价权重矩阵，在滚动优化的过程中，将进行实时的调整更新；

干预辅助量成本函数、定义方向盘转角及其变化率成本函数的具体形式为：

其中，R(k)，E(k)，F(k)为t＝k时刻的权重矩阵，u{k+h|k)表示k时刻预测最优控制量，u_d(k+h|k)为k时刻驾驶员输入意图控制量；

上述各归一化系数分别由对应参数的最大值和最小值确定：

引入驾驶权衰减系数Γ和驾驶权重系数Θ，分别用于在预测时间窗口内衡量对驾驶员意图输入量的置信程度以及控制人机间驾驶权重，并构造成本函数以下J_k求解所述系数：

J_k＝(1-Γ(t))·Q_Tack(k)·J_Track，k+ΘΓ(t)J_Δuk+n_ε·max{Γ(t).(1-Γ(t))}·J_εk

系数

其中，n_Γ，min表示预测时间窗口t＝1，2...，p内的短期预测部分，即预测步长的前几个步长，n_Γ，max表示预测时间窗口内的长期预测部分，即预测步长的后几个步长，ΔT为预测采样时间间隔，J_track，k为反映车辆状态的部分代价函数记为：J_track，k＝[J_zk，J_uk，J_duk]^T；

Q(k)，R(k)，E(k)，F(k)分别表示为：

Q(k)＝Q_z，Track(k)·(1-Γ(t))

R(k)＝Q_u，Track(k)·(1-Γ(t))

E(k)＝Q_du，Track(k)·(1-Γ(t))F(k)＝ΘΓ(t)

其中，Q_z，Track、Q_u，Track、Q_du，Track分别为道路跟踪误差成本函数J_zk的权重系数、干预辅助量成本函数J_uk的权重系数、方向盘转角成本函数J_duk的权重系数；

通过求解最终得到最优的前轮转角输出值。

Images