CN108454623B - 一种四轮独立驱动无人驾驶电动车辆轨迹跟踪控制方法 - Google Patents

Abstract

本发明公开了一种四轮独立驱动无人驾驶电动车辆轨迹跟踪控制工作方法，包括以下步骤：建立上层控制器，实现主动转向控制；建立中层控制器，实现车辆横摆稳定性控制；建立下层控制器，控制实际车速能稳定跟踪期望车速。本发明考虑车辆横向稳定性的四轮独立驱动无人驾驶车辆轨迹跟踪方法，通过上层控制器对期望轨迹进行跟踪，中层控制器利用上层控制器规划出的前轮转角对期望横摆角速度进行跟踪，实现了车辆在轨迹跟踪时的稳定性。本发明将车辆动力学约束加入上层控制器，能提高模型精确度和车辆行驶的安全性。上层控制器通过对车辆以及参考轨迹未来时刻的状态变化的考虑，提高了轨迹跟踪的精度。本发明算法简单有效，求解时间短、实时性好。

Description

一种四轮独立驱动无人驾驶电动车辆轨迹跟踪控制方法

技术领域

本发明涉及到无人驾驶车辆控制领域，特别是一种四轮独立驱动无人驾驶电动车辆轨迹跟踪控制工作方法。

背景技术

随着计算机、信息、认知、机械、化学等学科的发展，人们认识到从车辆驾驶的自动化、电动化来解决交通安全问题和能源问题将是目前最有效的方法之一。无人驾驶车辆是一种典型的四轮移动机器人，是智能车辆发展的高级阶段，涉及众多交叉学科知识，是当代计算机科学、模式识别、控制技术的高度结合和发展的产物，其利用功能不一的传感器来感知车辆周围环境，并根据感知所获得的道路、车辆位置和障碍物信息，规划一条安全无碰撞的路径，控制车辆的速度和转向，从而使车辆能够安全、可靠地在道路上自主驾驶。

轨迹跟踪控制系统是无人驾驶车辆实现智能化和实用化的必要条件。实现全自主无人驾驶车辆在高速和冰雪等低附路面安全稳定的轨迹跟踪控制具有十分重要的意义。目前，轨迹跟踪控制算法大都针对传统车辆，控制目的为缩小车辆实际轨迹与期望轨迹的误差，并且现有轨迹跟踪控制算法的局限为很少将车辆的速度控制以及横摆控制考虑进轨迹跟踪控制算法，导致轨迹跟踪的精度低，易使车辆在高速以及低附路面发生侧滑、失稳现象。所以设计适合四轮独立驱动车辆的轨迹跟踪算法、提高车辆在高速以及低附工况下的轨迹跟踪能力是亟待解决的问题。

发明内容

为解决车辆在高速以及低附工况下的轨迹跟踪能力的技术问题，本发明提供一种四轮独立驱动无人驾驶电动车辆轨迹跟踪算法。该算法采用分层控制，将轨迹跟踪控制问题转化为上层无人驾驶车辆的主动转向控制、中层横摆稳定性控制、下层力矩分配问题。最终使四轮独立驱动无人驾驶电动车辆能实现高速、低附工况下对期望轨迹的稳定性跟踪。

本发明的总体思路是：建立三层控制器，如图1所示，无人驾驶车辆的系统决策层根据车辆周围环境信息，自动规划出期望车速v_x和期望轨迹f(x,y)。上层控制器接受系统决策层的期望轨迹，根据车辆当前状态计算出当前时刻期望的车辆的前轮转角δ_f，当前时刻车辆转向系统以此转角实现转向。中层控制器根据期望的前轮转角δ_f计算出车辆为达到理想橫摆角速度所需要的橫摆控制力矩M_z，并将M_z输入到下层控制器中的驱动力分配控制器。下层控制器中的车速跟随控制器将系统决策层决策出的期望车速v_dx与实际车速v_x作为车速跟踪控制器的输入，输出总的驱动力矩。驱动力分配控制器将横摆控制力矩M_z与总的驱动控制力矩T作为控制输入，计算得到四个车轮的驱动力矩T_i驱动车辆以期望车速v_x行驶。

一种四轮独立驱动无人驾驶电动车辆轨迹跟踪控制工作方法，包括以下步骤：

A、建立上层控制器，实现主动转向控制

A1、根据车辆单轨模型，建立二自由度车辆动力学模型，并将其写成状态空间表达形式为：

式中：为状态空间的状态量的变化速度，ξ_dyn为状态空间的状态量，η_dyn为状态空间的输出量，u_dyn为状态空间的控制量，f_dyn为状态量和控制量之间的函数关系，h_dyn为输出量和状态量的函数关系；

考虑到轮胎所受的垂直力、纵向力、侧向力和回正力矩对车辆的平顺性、操纵稳定性和安全性起着重要作用；基于小角度假设和线性轮胎公式推导出车辆动力学非线性模型，其中状态量：

式中：代表车辆横向速度，代表车辆的纵向速度，代表横摆角，γ为横摆角速度，Y为车辆的横向位移，X为车辆的纵向位移；

控制量选取为：

u_dyn＝δ_f (2)

式中:u_dyn代表控制量，δ_f为前轮转角；

A2、将公式(1)-(2)所示的车辆动力学非线性模型进行线性处理，得到需要的线性时变系统为：

式中：A_dyn(t)为状态空间的状态量的系数矩阵，B_dyn(t)为状态空间的控制量的系数矩阵；

A3、将公式(3)采用一阶差商的方法进行离散化处理，得到离散的状态空间表达式：

ξ_dyn(k+1)＝A_dyn(k)ξ_dyn(k)+B_dyn(k)u_dyn(k) (4)

式中：ξ_dyn(k)为离散化后状态空间的状态量，u_dyn(k)为离散化后状态空间的控制量，A_dyn(t)为离散化后状态空间的状态量的系数矩阵，A_dyn(k)＝I+TA_dyn(t)，B_dyn(t)离散化后状态空间的控制量的系数矩阵，B_dyn(k)＝TB_dyn(t)，I为单位矩阵，T为采样周期；

A4、设车辆轨迹跟踪过程某一时刻为t时刻，为求t时刻的前轮转角，建立如下目标函数：

式中：N_p为预测时域，N_c为控制时域，ρ为权重系数，ε为松弛因子；

A5、因为在建立车辆动力学非线性模型时对轮胎模型进行了线性近似，故在求解步骤A4所建立目标函数时，需要对轮胎侧偏角、车辆横向加速度、质心侧偏角进行约束，即a_y,min-ε≤a_y≤a_y,max+ε，-2.5°＜α_f,t＜2.5°，-2°＜β＜2°，否则会导致求解精度降低；

A6、对目标函数(5)求解后得到控制时域内的一系列控制输入增量和松弛因子：

将该t时刻对应的控制量的第一个元素加上上一个时刻即(t-1)时刻对应的控制量作为实际的前轮转角的控制量δ_f；

A7、在t+1时刻，重复步骤A1-A6，完成对期望轨迹的跟踪；

B、建立中层控制器，实现车辆横摆稳定性控制

B1、根据当前纵向车速v_xr与上层控制器计算出的车辆前轮转角δ_f，计算得到理想横摆角速度为：

式中：γ_d为理想橫摆角速度，γ₀为目标横摆角速度；

式中：C_r为后轮侧偏刚度，C_f为前轮侧偏刚度，γ_max为目标横摆角速度的最大值，v_xr为理想纵向速度，μ为路面附着系数；

B2、由平面四轮车模型，在y方向上进行受力分析得到汽车绕质心的力矩方程为：

式中：I_z代表汽车绕z轴的转动惯量，γ为实际的横摆角速度，M_x为橫摆控制力矩，l_f为质心到前轴的距离，l_r为质心到后轴的距离，F_yi为第i个车轮的横向力，车辆的左前、右前、左后、右后四个车轮分别标定为1、2、3、4号车轮，l_w为轮距，M_d为考虑侧向风、路面凹凸不平形成的干扰力矩；

根据式(9)建立准滑膜橫摆力矩控制器，为了降低滑膜变结构的控制的高频抖动，利用双曲正切函数代替符号函数，准滑膜橫摆力矩控制器为：

式中：D为常数，D＞0；s为目标横摆角速度与理想橫摆角速度的差值，s＝γ₀-γ_d；

C、建立下层控制器，控制实际车速能稳定跟踪期望车速

C1、在进行驱动力矩分配之前，需要计算总的驱动力矩；下面将期望纵向车速和实际纵向车速的差值作为PID控制的输入，电子油门开度作为PID控制的输出；然后通过查油门工作特性表得到总的驱动力矩T；

C2、四个车轮的纵向力表示为：

F_x＝[F_x1 F_x2 F_x3 F_x4] (11)

式中：F_xi为第i个车轮的纵向力

构建驱动力矩性能指标为：

式中：车辆的左前、右前、左后、右后四个车轮分别标定为1、2、3、4号车轮；

σ_i＝μ_iF_zi，i＝1,...,4

μ_i为i号车轮的路面附着系数；F_zi为第i号车轮所受的垂直载荷；W_T为加权矩阵：

C3、在步骤C2基础上建立如下优化函数，求解驱动力矩；

s.t.SF_x＝F_T (15)

式中：S为系数矩阵，F_T为纵向力矩阵；

C4、针对步骤C3建立的优化函数，利用伪逆法进行求解：分配到四个车轮的力矩为：

式中：u₁,u₂分别代表左侧、右侧车轮总的驱动力矩；这里将左右两侧驱动力矩平均分配，即u₁＝0.5T，u₂＝0.5T；

C5、将中层控制器计算得到的橫摆控制力矩分配到四个车轮；分配规则如下：

式中：T_i为第i个车轮的驱动力矩；r为车轮的有效滚动半径；ΔM为左右侧力矩之差；

即车辆前后轴各产生所需附加橫摆力矩的1/2；车辆四个车轮最终的驱动力矩由步骤C4计算出的驱动力矩加上每个车轮应分配的橫摆控制力矩，最终车轮驱动力矩为：

与现有技术相比，本发明的有益效果如下：

1.本发明设计了一种考虑车辆横向稳定性的四轮独立驱动无人驾驶车辆轨迹跟踪方法，通过上层控制器对期望轨迹进行跟踪，中层控制器利用上层控制器规划出的前轮转角对期望横摆角速度进行跟踪，实现了车辆在轨迹跟踪时的稳定性。

2.本发明将车辆动力学约束加入上层控制器，能提高模型精确度和车辆行驶的安全性。上层控制器通过对车辆以及参考轨迹未来时刻的状态变化的考虑，提高了轨迹跟踪的精度。并且所设计的上层控制器对车速、路面附着条件、参考轨迹有很好的鲁棒性。

3.本发明基于准滑膜控制建立了橫摆力矩控制器，利用双曲正切函数代替符号函数，有效降低了准滑膜控制的抖振现象。

4.本发明的下层控制器利用伪逆法对所建立的力矩分配控制器进行求解，算法简单有效，求解时间短、实时性好。

附图说明

图1为一种四轮独立驱动无人驾驶电动车辆轨迹跟踪算法整体框架图

图2为二自由度车辆单轨动力学模型

图3为平面四轮车模型

图4为双曲正切光滑切换函数

图5为PID速度跟随控制器

具体实施方式

下面结合附图以及具体实施例对本发明作进一步的说明，但本发明的保护范围并不限于此。应当注意的是，下述实施例中描述的技术特征的组合不应当被认为是孤立的，它们可以被相互组合从而达到更好的技术效果。

如图1所示，一种基于一种四轮独立驱动无人驾驶电动车辆轨迹跟踪算法，包括从系统决策层得到期望轨迹的上层控制器、中层控制器以及从系统决策层获得期望车速的下层控制器。

一种基于一种四轮独立驱动无人驾驶电动车辆轨迹跟踪算法工作过程为：系统决策层根据车辆周围环境信息，自动规划出期望车速v_x和期望轨迹f(x,y)。上层控制器接受系统决策层的期望轨迹，根据车辆当前状态计算出期望的车辆的前轮转角δ_f，并将期望的车辆前轮转角δ_f直接输入到车辆以及中层控制器。中层控制器根据期望的前轮转角δ_f计算出车辆为达到理想橫摆角速度所需要的橫摆控制力矩M_z，并将M_z输入到下层控制器中的驱动力分配控制器。下层控制器中的车速跟随控制器将系统决策层决策出的期望车速v_dx与实际车速v_x作为车速跟踪控制器的输入，输出总的驱动力矩。驱动力分配控制器将横摆控制力矩M_z与总的驱动控制力矩T作为控制输入，计算得到四个车轮的驱动力矩T_i驱动车辆以期望车速v_x行驶。

1.上层控制器

下面结合图2对上层轨迹跟踪控制器的滚动时域优化算法进行详细说明：

1.1车辆动力学建模

对图2进行分别在x,y,z三个坐标轴上进行受力分析:

式中，m为车辆的整备质量，l_r、l_f分别为车辆质心到前、后轴的距离。I_z为车辆绕z轴的转动惯量。经过公式推导可以将其写成状态方程的形式：

η_dyn＝h_dyn(ξ_dyn) (23)

定义状态变量为控制量选取为u_dyn＝δ_f。 (24)

轮胎所受垂向力、侧向力和回正力矩对汽车的平顺性、操纵稳定性和安全性起着重要作用。大量学者对轮胎模型进行了大量的研究，但是如果将复杂的轮胎模型和车辆动力学魔性相结合建立的非线性状态空间表达式太过复杂，大大增加了控制系统计算量。在车辆前轮小角度偏转假设下，轮胎力可近似用线性函数表达。这个近似可以在车辆横向加速度a_y≤0.4g时保证很好的拟合精度。故轮胎的侧向力、纵向力可分别表示为：

F_c＝C_cα，F_l＝C_ls (25)

其中：C_c为轮胎侧向刚度，C_l为轮胎侧偏刚度。α为轮胎侧偏角，s为车轮纵向滑移率前轮小角度偏转时候:

cosθ≈1，sinθ≈θ，tanθ≈θ (26)

经过上述假设后，前后轮胎侧偏角的计算式可转化为：

由此可以推导出前后轮胎的纵向力与侧向力分别为：

F_lf＝C_lfs_f

F_lr＝C_lrs_r

将式(25)-(28)代入到式(24)中得到基于前轮较小偏角和线性轮胎模型假设后的车辆动力学非线性模型：

如同上述，状态量同样选为控制量为u_dyn＝δ_f。

1.2建立线性误差方程

为了提高上层控制算法的实时性，需要对建立的模型即式(9)进行线性化。将一个非线性系统近似为线性时变系统有很多方法，基本可以分为近似线性化和精确线性化。本发明采用近似线性化对上述建立的非线性车辆模型进行线性化，基本思想是对系统施加一常数控制量，得到一条状态轨迹，根据该轨迹和系统实际状态量的偏差设计线性模型预测控制算法。考虑系统的某个工作点为[ξ₀,u₀]，ξ₀(k)为始终施加控制量为u₀后得到的系统状态量，则存在以下关系：

经过推导后，可以得到：

式中：

对非线性方程进行线性化后还需对其进行离散化处理，本发明此处采用一阶差商的方法进行离散化处理。故针对上述式(9)，可以得到线性化后的离散时变方程为：

ξ_dyn(k+1)＝A_dyn(k)ξ_dyn(k)+B_dyn(k)u_dyn(k) (32)

式中：

式中：

1.3建立滚动时域优化控制器

针对上层控制器实现的轨迹跟踪问题建立滚动时域优化控制器，目标函数设计如下:

为了增加轨迹跟踪的平稳性，需要对控制量、控制增量进行约束和车辆的动力学。考虑到安全性还需要加入车辆动力学约束。

控制量与控制增量表达式为：

u_min(t+k)≤u(t+k)≤u_min(t+k) k＝0,1,...,N_c-1 (34)

Δu_min(t+k)≤Δu(t+k)≤Δu_min(t+k) k＝0,1,...,N_c-1 (35)

本发明这里将控制量极限设置为u_min(t+k)＝-25°，u_max(t+k)＝25°；控制增量极限设置为Δu_min(t+k)＝-0.47°，Δu_max(t+k)＝0.47°。

过大的侧向加速度会降低人的乘坐舒适性，另外如果确保二自由度车辆模型的精确度也需要将横向加速度限制在0.3g内。但是过小的约束条件限制也可能会导致目标函数无解，所以本发明将该约束设置为软约束，即加入松弛因子，约束条件会随着求解器在每个周期内的求解情况动态进行调整，即兼顾了求解的可行性又保证了模型的精确度和乘坐舒适性。横向加速度约束为：

a_y,min-ε≤a_y≤a_y,max+ε (36)

式中：a_y为横向加速度，a_y,min为横向加速度最小值，a_y,max为横向加速度最大值。

由轮胎的侧偏特性知，当轮胎侧偏角α不超过5o时，侧偏角与侧偏力为线性关系，本发明这里将其做更为严格的限制：

-2.5°＜α_f,t＜2.5° (37)

质心侧偏角β对车辆的稳定有很大影响，本发明也将其考虑在内：

-2°＜β＜2° (38)

结合式(33)-(38)，将上层控制器需要解决的轨迹跟踪问题转化为如下的优化问题：

式中：ΔU_dyn,t,ΔU_dyn,min,ΔU_dyn,max分别为控制时域内的控制增量、控制时域内的控制增量的极大值、控制时域内的控制增量的极小值。τ_h,τ_s分别为硬约束、软约束。

τ_hmin,τ_hmax为硬约束极大值、软约束极小值。τ_smin,τ_smax为软约束极大值、软约束极小值。

将上式转化为标准二次优化问题：

U_dyn，min≤AΔU_dyn，t+U_dyn，t≤U_dyn，max (40)

利用有效集解法对上式进行求解，得到控制时域内一系列控制输入的增量和松弛因子：

将上述序列第一个元素作为实际的控制输入增量作用于系统：

2.中层控制器

滑膜控制律可由等效控制u_sw和切换鲁棒控制u_sw构成。如果不考虑外界干扰和系统的不确定性，使可以得到滑膜控制律的等效控制项u_eq，再令u＝u_eq+u_sw，通过分析并且将u＝u_eq+u_sw代入，使得成立，就可以得到u_sw。

系统的在滑膜面上的状态由等效控制来保证，而切换控制的作用是确保系统的状态不离开滑模面。

2.1等效控制器

由图2可知，当车辆处于稳定状态时，期望横摆角速度γ_d可由下式求得：

式中：γ₀为理想橫摆角速度的目标值，如下式：

由于地面所能提供的附着力的限制，理想橫摆角速度还需加以限制，即

由图3可知，当考虑侧向风、路面凹凸不平等形成的干扰力矩M_d时，汽车绕质心得力矩方程为：

不考虑外界干扰，设计滑膜函数为：s＝γ_r-γ_d。使则可推导出：

则可以推出等效控制器为：

2.2切换控制器

在滑膜控制中，当控制结构的切换具有理想的开关特性,就可以在滑膜面上形成理想的华东模态，即是一种光滑的运动，渐进趋近于原点。但在实际工程中，由于存在时间上的延迟和空间上的滞后，使得滑动模态呈抖动形式，在光滑的滑动上叠加了抖动。所以为了减少这种抖动，有人采用饱和函数方法，让此代替符号函数，可以有效地克服滑膜抖振。其缺点是属于不连续函数。不适用于需要求导的场合。由于双曲正切函数是连续光滑的，采用双曲正切函数代替不连续的切换函数，可有效地降低滑膜控制中的抖振，如图4所示，对比符号函数和双曲正切函数。

双曲正切函数如下：

为保证切换控制项为：

其中：D＞0,ε＞0

结合式子(26)和(28)推导出最终的控制器为：

其中：δ_f为前轮转角，即为上层控制器的控制输入u_dyn(t)

上层控制器计算出的前轮转角传给中层控制器，中层控制器通过式(49)计算出橫摆控制力矩M_x。

3.下层控制器

下层控制器的主要作用是将橫摆控制力矩M_x与车轮的驱动力矩T_i分配到相应的车轮。首先要建立精确地轮毂电机模型，本发明只关注轮毂电机的外部性能，并且轮毂电机控制器技术已比较成熟，所以本发明这里采用采用转矩控制，即将轮毂电机模型简化为如下传递函数的形式：

车速的稳定性对轨迹跟踪的精度与平稳性起重要作用，将车速视为定值并不符合实际。本发明针对四轮独立驱动无人驾驶车辆设计了速度跟随控制器，如图5所示，将理想车速与实际车速作为PID控制器的输入，输出为电子节气门开度，经过查找电子节气门开度与力矩Map图输出车辆的总的驱动力矩T。

针对力矩分配问题众多学者进行了大量研究，但是复杂的求解算法实时性差，在实际中难以应用，尤其对于高速工况。本发明针对无人驾驶车辆在高速以及低附路面提出一种简化的力矩分配算法，即以路面附着系数利用率为优化目标，推导出求解速度快的力矩分配算法。首先设定左右两侧的驱动力矩分别为0.5T。

由图3平面四轮车模型知，车轮纵向力可表示为：

F_X＝[F_x1 F_x2 F_x3 F_x4]^T (51)

令：

则可以知道：

为了提高车辆在低附路面的安全行驶能力，本发明以车轮的附着利用率作为优化目标，对驱动力进行求解。性能指标为设计：

式中:μ为路面的附着系数，F_zi第i个车轮的垂直载荷，W_T为加权矩阵，公式如下：

建立如下优化问题：

为了求解该问题，首先构建汉密尔顿函数如下：

式中：ξ∈R⁴为拉格朗日乘子。

对式(55)中的F_x和ξ求偏导有：

由式(56)知：

W_TF_X＝-2(ξS)^T (J57)

将式(57)代入(58)可知:

由上式可求得ξ^T,并将其代入式(57)可得：

已知轮胎驱动力矩与轮胎纵向力之间的关系为：

式中：r_i为轮胎的有效滚动半径。

结合式(60)和式(61)可得力矩分配表达式为：

结合上述中层橫摆力矩控制器知，当橫摆力矩控制器工作时，同轴的左右侧车轮的力矩T_L,T_R相差:

则最终分配到车轮的力矩T_t1为：

本发明提供一种四轮独立驱动无人驾驶车辆的轨迹跟踪控制算法，该算法充分考虑了车辆车辆在高速以及低附路面行驶工况不稳定以及易出现失稳现象。上层控制器根据车辆信息以及轨迹跟踪误差规划前轮转角，中层控制器根据此转角计算橫摆控制力矩，使车辆在高速以及低附路面行驶时候既能跟踪期望轨迹，又不至于发生失稳等现象，本发明设计的下层控制器将车轮附着利用率为优化目标，合理的将车辆驱动力矩与橫摆力矩分配到车轮。本发明分层设计思想，可实施性强，计算量小，实时性好，充分提高了车辆在轨迹跟踪时的稳定性。

Claims (1)

1.一种四轮独立驱动无人驾驶电动车辆轨迹跟踪控制工作方法，其特征在于：包括以下步骤：

A、建立上层控制器，实现主动转向控制

A1、根据车辆单轨模型，建立二自由度车辆动力学模型，并将其写成状态空间表达形式为：

η_dyn＝h_dyn(ξ_dyn)

式中：为状态空间的状态量的变化速度，ξ_dyn为状态空间的状态量，η_dyn为状态空间的输出量，u_dyn为状态空间的控制量，f_dyn为状态量和控制量之间的函数关系，h_dyn为输出量和状态量的函数关系；

考虑到轮胎所受的垂直力、纵向力、侧向力和回正力矩对车辆的平顺性、操纵稳定性和安全性起着重要作用；基于小角度假设和线性轮胎公式推导出车辆动力学非线性模型，其中状态量：

式中：代表车辆横向速度，代表车辆的纵向速度，代表横摆角，γ为横摆角速度，Y为车辆的横向位移，X为车辆的纵向位移；

控制量选取为：

u_dyn＝δ_f (2)

式中:u_dyn代表控制量，δ_f为前轮转角；

A2、将公式(1)-(2)所示的车辆动力学非线性模型进行线性处理，得到需要的线性时变系统为：

式中：A_dyn(t)为状态空间的状态量的系数矩阵，B_dyn(t)为状态空间的控制量的系数矩阵；

A3、将公式(3)采用一阶差商的方法进行离散化处理，得到离散的状态空间表达式：

ξ_dyn(k+1)＝A_dyn(k)ξ_dyn(k)+B_dyn(k)u_dyn(k) (4)

式中：ξ_dyn(k)为离散化后状态空间的状态量，u_dyn(k)为离散化后状态空间的控制量，A_dyn(t)为离散化后状态空间的状态量的系数矩阵，A_dyn(k)＝I+TA_dyn(t)，B_dyn(t)离散化后状态空间的控制量的系数矩阵，B_dyn(k)＝TB_dyn(t)，I为单位矩阵，T为采样周期；

A4、设车辆轨迹跟踪过程某一时刻为t时刻，为求t时刻的前轮转角，建立如下目标函数：

式中：ΔU_dyn(t)为t时刻的控制增量，N_p为预测时域，N_c为控制时域，ρ为权重系数，ε为松弛因子；

A5、因为在建立车辆动力学非线性模型时对轮胎模型进行了线性近似，故在求解步骤A4所建立目标函数时，需要对轮胎侧偏角、车辆横向加速度、质心侧偏角进行约束，即a_y,min-ε≤a_y≤a_y,max+ε，-2.5°<α_f,t<2.5°，-2°<β<2°，否则会导致求解精度降低；

A6、对目标函数(5)求解后得到控制时域内的一系列控制输入增量和松弛因子：

将该t时刻对应的控制量的第一个元素加上上一个时刻即(t-1)时刻对应的控制量作为实际的前轮转角的控制量δ_f；

A7、在t+1时刻，重复步骤A1-A6，完成对期望轨迹的跟踪；

B、建立中层控制器，实现车辆横摆稳定性控制

B1、根据当前纵向车速v_xr与上层控制器计算出的车辆前轮转角δ_f，计算得到理想横摆角速度为：

式中：γ_d为理想橫摆角速度，γ₀为目标横摆角速度；

式中：C_r为后轮侧偏刚度，C_f为前轮侧偏刚度，γ_max为目标横摆角速度的最大值，v_xr为理想纵向速度，μ为路面附着系数；

B2、由平面四轮车模型，在y方向上进行受力分析得到汽车绕质心的力矩方程为：

式中：I_z代表汽车绕z轴的转动惯量，γ为实际的横摆角速度，M_x为橫摆控制力矩，l_f为质心到前轴的距离，l_r为质心到后轴的距离，F_yi为第i个车轮的横向力，车辆的左前、右前、左后、右后四个车轮分别标定为1、2、3、4号车轮，l_w为轮距，M_d为考虑侧向风、路面凹凸不平形成的干扰力矩；

根据式(9)建立准滑膜橫摆力矩控制器，为了降低滑膜变结构的控制的高频抖动，利用双曲正切函数代替符号函数，准滑膜橫摆力矩控制器为：

式中：D为常数，D>0；s为目标横摆角速度与理想橫摆角速度的差值，s＝γ₀-γ_d；

C、建立下层控制器，控制实际车速能稳定跟踪期望车速

C1、在进行驱动力矩分配之前，需要计算总的驱动力矩；下面将期望纵向车速和实际纵向车速的差值作为PID控制的输入，电子油门开度作为PID控制的输出；然后通过查油门工作特性表得到总的驱动力矩T；

C2、四个车轮的纵向力表示为：

F_x＝[F_x1 F_x2 F_x3 F_x4] (11)

式中：F_xi为第i个车轮的纵向力

构建驱动力矩性能指标为：

式中：车辆的左前、右前、左后、右后四个车轮分别标定为1、2、3、4号车轮；

σ_i＝μ_iF_zi，i＝1,...,4

μ_i为i号车轮的路面附着系数；F_zi为第i号车轮所受的垂直载荷；W_T为加权矩阵：

C3、在步骤C2基础上建立如下优化函数，求解驱动力矩；

s.t. SF_x＝F_T (15)

式中：S为系数矩阵，F_T为纵向力矩阵；

C4、针对步骤C3建立的优化函数，利用伪逆法进行求解：分配到四个车轮的力矩为：

式中：u₁,u₂分别代表左侧、右侧车轮总的驱动力矩；这里将左右两侧驱动力矩平均分配，即u₁＝0.5T，u₂＝0.5T；

C5、将中层控制器计算得到的橫摆控制力矩分配到四个车轮；分配规则如下：

式中：T_i为第i个车轮的驱动力矩；r为车轮的有效滚动半径；ΔM为左右侧力矩之差；

即车辆前后轴各产生所需附加橫摆力矩的1/2；车辆四个车轮最终的驱动力矩由步骤C4计算出的驱动力矩加上每个车轮应分配的橫摆控制力矩，最终车轮驱动力矩为：