# Adaptive Coordinated Path Tracking Control Strategy for Autonomous Vehicles with Direct Yaw Moment Control – Chinese Journal of Mechanical Engineering

Jan 4, 2022

### Vehicle Dynamics Model

Because the proposed controller requires the vehicle dynamics information under high-speed and large-curvature conditions, the vehicle lateral, yaw, and roll dynamics are established as shown in Figure 2 to capture the essential vehicle dynamics state parameters. The tires are the only components that connect the road to the vehicle. All movements, including the starting, acceleration, braking, and turning of the vehicle, are realized through the tires. During the path tracking process under high-speed and large-curvature conditions, vehicle yaw moment is provided by the lateral force of the front and rear tires. The lateral acceleration of the vehicle is mainly caused by the lateral force of the front and rear tires. At the same time, the coupling relationship between the yaw rate and longitudinal velocity can produce additional vehicle lateral acceleration. The vehicle roll dynamics are described using the characteristics of the suspension damping and stiffness considering the coupling relationship between the vehicle lateral velocity, longitudinal velocity, and roll dynamics.

In Figure 2, m is the vehicle total mass. h is the height of the sprung mass from the roll center, lf and lr are the distances from the center of gravity to the front and rear axles, respectively. ls is the half distance between the left and right tires. g is the acceleration due to gravity. vx and vy are the longitudinal and lateral speed, respectively. r and ϕ are the yaw rate and roll angle, respectively. Fx1 and Fx3 are the longitudinal tire force of the front and rear axles. Fy1 and Fy3 are the lateral tire force of the front and rear axles. δ and β are the front tire steering angle and vehicle sideslip angle. θ is the vehicle yaw angle. K and D are the suspension roll damping and stiffness.

The vehicle dynamics model is given by Eq. (1).

left{ begin{aligned} &m(dot{v}_{y} + v_{x} r) = 2F_{x1} sin delta + 2F_{y1} + 2F_{y3} , \ &I_{z} dot{r} = (2F_{x1} sin delta + 2F_{y1} )l_{f} – 2F_{y3} l_{r} + M, \& I_{x} ddot{phi } = m_{s} h(dot{v}_{y} + v_{x} r) + m_{s} ghphi – K_{phi } phi – D_{phi } dot{phi }, \& dot{Y} = v_{x} sin (theta ) + v_{y} cos (theta ), \ &dot{X} = v_{x} cos (theta ) + v_{y} sin (theta ), \ end{aligned} right.

(1)

where ms is the vehicle sprung mass. M is the external yaw moment. Iz is the moment of inertia about z-axis. Ix is the moment of inertia of sprung mass about the x-axis. X and Y are the coordinate positions of the vehicle in XOY coordinate system.

Under the small-angle approximation, the vehicle dynamics model can be approximated as

left{ begin{aligned} dot{v}_{y} & approx (2F_{y1} + 2F_{y3} )/m – v_{x} r, \ dot{r} & approx (2F_{y1} l_{f} – 2F_{y3} l_{r} + M)/I_{z} , \ ddot{phi } & approx [m_{s} h(dot{v}_{y} + v_{x} r) + m_{s} ghphi – Kphi – Ddot{phi }]/I_{x} . \ end{aligned} right.

(2)

### Tire Dynamics Model

The Fiala brush tire model is highly accurate and fully accounts for the nonlinear dynamic characteristics. The lateral force of the tire can be described as a function of the tire slip angle.

begin{aligned} F_{y[f,r]} & = f_{tire} (alpha ) \ & = left{ begin{aligned} & – C_{alpha } tan alpha + frac{{C_{alpha }^{2} }}{{3mu F_{z} }}left| {tan alpha } right|tan alpha \ & – frac{{C_{alpha }^{3} }}{{27mu^{2} F_{{_{z} }}^{2} }},left| alpha right| < arctan left( {frac{{3mu F_{z} }}{{C_{alpha } }}} right), \ & – mu F_{z} {text{sgn}} (alpha ),;text{otherwise}, \ end{aligned} right. \ end{aligned}

(3)

where α and Cα are the tire slip angle and cornering stiffness, respectively. μ is the adhesion coefficient between the road surface and the tire. Fz is the vertical force.

Because the tire slip angle does not directly participate in the control optimization solution process and the vehicle lateral speed is relatively small compared to the longitudinal speed, the tire slip angle of the front and rear tires can be approximated as

left{ begin{aligned} alpha_{f} & = arctan left( {frac{{v_{y} + l_{f} r}}{{v_{x} }}} right) – delta approx frac{{v_{y} + l_{f} r}}{{v_{x} }} – delta , \ alpha_{r} & = arctan left( {frac{{v_{y} – l_{r} r}}{{v_{x} }}} right) approx frac{{v_{y} – l_{r} r}}{{v_{x} }}. \ end{aligned} right.

(4)

To make full use of the tire nonlinear dynamics and reduce the nonlinear complexity of the controller, the front tire lateral force can be considered as the control input for the MPC optimization and mapped to the desired δ via

$$delta = frac{{v_{y} + l_{f} r}}{{v_{x} }} – alpha_{f} .$$

(5)

The nonlinear dynamics of the rear tire should be properly accounted for to approximate the MPC linear optimization problem accurately. The steady-state solution of the rear lateral force is expressed as

$$overline{F}_{r,ss} = frac{{ml_{f} }}{L}v_{x}^{2} kappa ,$$

(6)

where κ is the curvature of the reference path at the distance s.

The equivalent cornering stiffness can be written as

$$overline{C}_{r} = frac{{(overline{F}_{r,ss} – overline{F}_{r} )}}{{overline{alpha }_{r,ss} – overline{alpha }_{r} }},$$

(7)

where (overline{F}_{r}) and (overline{alpha}_{r}) are the tire slip angle and corresponding tire lateral force.

The predicted rear tire lateral force in the MPC frame can be written as

$$F_{yr} = overline{F}_{r} – overline{C}_{r} (alpha – overline{alpha }_{r} ).$$

(8)

### Path Tracking Model

It is desired that the path tracking should follow the reference path accurately by minimizing the lateral and heading deviations while maintaining rollover and lateral stability. The relationship between the vehicle and reference path is illustrated in Figure 3, where the CM point represents the center of vehicle mass.

The path tracking kinematic relationship can be expressed as

left{ begin{aligned} dot{e} & = v_{y} + v_{x} theta_{e} , \ dot{theta }_{e} & = r – v_{x} kappa (s), \ dot{s} & = v_{x} cos (theta_{e} ) – v_{y} sin (theta_{e} ), \ theta_{e} & = theta – theta_{d} , \ end{aligned} right.

(9)

where θ and θd are the actual yaw angle and reference yaw angle of vehicle, respectively. κ(s) is the curvature of the reference path at the position s.

### Design of Path Tracking Controller Based on MPC

#### Prediction Model

The controller state space model, in which the front tire lateral force and external yaw moment are taken as the control inputs, can be derived from Eqs. (2), (4), (8), and (9) as

left{ begin{aligned} dot{varvec{x}} & = {varvec{A}}_{1} {varvec{x}} + {varvec{B}}_{1} {varvec{u}} + {varvec{E}}_{1} {varvec{kappa}} + {varvec{D}}_{1} , \ {varvec{y}} & = {varvec{C}}_{1} {varvec{x}}. \ end{aligned} right.

(10)

The state vector, system output vector, control input, the state matrix and output matrix are respectively expressed as

$${varvec{x}} = left[ {begin{array}{*{20}ll} {v_{y} } & r & {dot{phi }} & {begin{array}{*{20}ll} phi & {theta_{e} } & e \ end{array} } \ end{array} } right]^{{text{T}}} ,quad {varvec{u}} = left[ {begin{array}{*{20}ll} {F_{y} } & M \ end{array} } right]^{{text{T}}} ,quad {varvec{y}} = left[ {begin{array}{*{20}ll} {theta_{e} } & e \ end{array} } right]^{{text{T}}}$$

$${varvec{A}}_{1} = left[ {begin{array}{*{20}c} {frac{{ – 2overline{C}_{r} }}{{mv_{x} }}} & {frac{{2l_{r} overline{C}_{r} }}{{mv_{x} }} – v_{x} } & 0 & 0 & 0 & 0 \ {frac{{2l_{r} overline{C}_{r} }}{{I_{Z} v_{x} }}} & { – frac{{2l_{r}^{2} overline{C}_{r} }}{{I_{z} v_{x} }}} & 0 & 0 & 0 & 0 \ {frac{{ – 2overline{C}_{r} Theta_{1} }}{{v_{x} }}} & {frac{{2l_{r} overline{C}_{r} Theta_{1} }}{{v_{x} }}} & { – DTheta_{2} } & {(m_{s} hg – K)Theta_{2} } & 0 & 0 \ 0 & 0 & 0 & 1 & 0 & 0 \ 0 & 1 & 0 & 0 & 0 & 0 \ 1 & 0 & 0 & 0 & {v_{x} } & 0 \ end{array} } right];{varvec{B}}_{1} = left[ {begin{array}{*{20}c} 0 & frac{2}{m} \ {frac{ – 1}{{I_{Z} }}} & {frac{{2l_{f} }}{{I_{Z} }}} \ 0 & {2Theta_{1} } \ 0 & 0 \ 0 & 0 \ 0 & 0 \ end{array} } right],;{varvec{E}}_{1} = left[ {begin{array}{*{20}c} 0 \ 0 \ 0 \ 0 \ { – v_{x} } \ 0 \ end{array} } right],;{varvec{D}}_{1} = left[ {begin{array}{*{20}c} {frac{{2(overline{F}_{r} + overline{C}_{r} overline{alpha }_{r} )}}{m}} \ {frac{{ – 2l_{r} (overline{F}_{r} + overline{C}_{r} overline{alpha }_{r} )}}{{I_{Z} }}} \ {2(overline{F}_{r} + overline{C}_{r} overline{alpha }_{r} )Theta_{1} } \ {0} \ {0} \ {0} \ end{array} } right]$$

$${varvec{C}}_{1} = left[ {begin{array}{*{20}c} 0 & 0 & 0 & 0 & 1 & 0 \ 0 & 0 & 0 & 0 & 0 & 1 \ end{array} } right],$$

$$Theta_{1} = frac{{m_{s} h}}{{mI_{x} + mm_{s} h^{2} }},;Theta_{2} = frac{1}{{I_{x} + m_{s} h^{2} }}.$$

To facilitate controller design and implementation, the continuous-time state-space model is converted into a discrete increment system using previous Euler method and expressed as Eq. (11).

left{ begin{aligned} {varvec{xi}}(k + 1) & = {varvec{A}varvec{xi} }(k) + {varvec{B}}Delta {varvec{u}} + {varvec{E}varvec{kappa} } + {varvec{D}}, \ {varvec{eta}}(k) & = {varvec{C}varvec{xi} }(k), \ end{aligned} right.

(11)

where T is the sampling time, and

$${varvec{xi}}(k) = left[ {begin{array}{*{20}c} {{varvec{x}}(k)} & {{varvec{u}}(k – 1)} \ end{array} } right]^{{text{T}}} ,;{varvec{eta}}(k) = left[ {begin{array}{*{20}c} {theta_{e} } & e \ end{array} } right]^{{text{T}}} ,;{varvec{A}} = left[ {begin{array}{*{20}c} {I + T{varvec{A}}_{1} } & {T{varvec{B}}_{1} } \ {mathbf{0}} & {varvec{I}} \ end{array} } right],;{varvec{B}} = left[ {begin{array}{*{20}c} {T{varvec{B}}_{1} } \ {varvec{I}} \ end{array} } right]^{{text{T}}} ,;{varvec{E}} = left[ {begin{array}{*{20}c} {T{varvec{E}}_{1} } \ {varvec{0}} \ end{array} } right]^{{text{T}}} ,;{varvec{D}} = left[ {begin{array}{*{20}c} {T{varvec{D}}_{1} } \ {varvec{0}} \ end{array} } right]^{{text{T}}} ,;{varvec{C}} = left[ {begin{array}{*{20}c} 0 & 0 & 0 & 0 & 1 & 0 & 0 \ 0 & 0 & 0 & 0 & 0 & 1 & 0 \ end{array} } right].$$

To guarantee a convex optimization solution and reduce the computational complexity of the MPC, the following assumptions are made

$${varvec{A}}_{i,k} = {varvec{A}}_{k,k} ,i = 1, ldots ,i + N_{p} – 1,;{varvec{B}}_{i,k} = {varvec{B}}_{k,k} ,i = 1, ldots ,i + N_{p} – 1,$$

where Np denotes the prediction horizon.

Therefore, the system prediction outputs in the prediction horizon can be expressed as

$${varvec{Y}}(k) = {varvec{Psi} xi }(k|k) +{varvec{varPhi}}Delta {varvec{u}}(k) +{varvec{varPi}},$$

(12)

where Nc denotes the control horizon, and

$${varvec{Y}}(k) = left[ {begin{array}{*{20}c} {{varvec{eta}}(k + 1|k)} \ {{varvec{eta}}(k + 2|k)} \ vdots \ {{varvec{eta}}(k + N_{p} |k)} \ end{array} } right],;{varvec{varPsi}}= left[ {begin{array}{*{20}c} {{varvec{CA}}} \ {{varvec{CA}}^{2} } \ vdots \ {{varvec{CA}}^{{N_{P} }} } \ end{array} } right],;{varvec{varPi}}= left[ {begin{array}{*{20}c} {{varvec{E}}kappa + {varvec{D}}} \ {{varvec{A}}({varvec{E}}kappa + {varvec{D}}) + {varvec{D}}} \ vdots \ {{varvec{A}}^{Np – 1} {varvec{E}}kappa + {varvec{A}}^{Np – 1} {varvec{D}} + cdots +{varvec{AD}} + {varvec{D}}} \ end{array} } right]$$

$${varvec{varPhi}}= left[ {begin{array}{*{20}c} {{varvec{CB}}} & 0 & 0 & 0 \ {{varvec{CAB}}} & {{varvec{CB}}} & 0 & 0 \ vdots & vdots & ddots & vdots \ {{varvec{CA}}^{Nc – 1} {varvec{B}}} & {{varvec{CA}}^{Nc – 2} {varvec{B}}} & ldots & {{varvec{CB}}} \ {{varvec{CA}}^{Nc} {varvec{B}}} & {{varvec{CA}}^{Nc – 1} {varvec{B}}} & cdots & {{varvec{CAB}}} \ vdots & vdots & ddots & vdots \ {{varvec{CA}}^{Np – 1} {varvec{B}}} & {{varvec{CA}}^{Np – 2} {varvec{B}}} & cdots & {{varvec{CA}}^{Np – Nc – 1} {varvec{B}}} \ end{array} } right]$$

#### Objective Cost Function and Optimization

The path tracking control objective of the MPC controller is the solution of a convex optimization problem at each sampling step. The first optimal value of the control sequence is used as the future input. The objective function and constraints can be expressed as

$$mathop {min }limits_{{{varvec{u}}(k)}} J_{Np} = sumlimits_{i = k}^{k + Np – 1} {{varvec{eta}}^{{text{T}}} {varvec{Qeta}}} + sumlimits_{i = k}^{k + Nc – 1} {Delta {varvec{u}}^{{text{T}}} {varvec{R}}Delta {varvec{u}}} + Wvarepsilon ,$$

(13)

$$s.t.left[ {{varvec{H}}_{text{v}} {varvec{x}}^{i} } right] le {varvec{G}}_{text{v}} ,i = 0, , 1, cdots ,N_{p} – 1,$$

(14)

$$- l_{s} le y_{ZMP} le l_{s} ,i = 0, , 1, cdots ,N_{p} – 1,$$

(15)

$$left| {Delta {varvec{u}}(k + i)} right| le Delta {varvec{u}}_{max } ,i = 0, , 1, cdots ,N_{c} – 1,$$

(16)

$$Delta {varvec{u}}(k + i){ = 0},i = N_{c} ,N_{c} + 1, cdots ,N_{p} – 1,$$

(17)

$$left| {{varvec{u}}(k + i)} right| le {varvec{u}}_{max } ,i = 0, , 1, cdots ,N_{p} – 1,$$

(18)

where ε is a non-negative slack variable used to ensure that the optimization problem is always feasible. The optimal front lateral force input Fyf and the external yaw moment M can be obtained using a look-up table and yaw moment allocation algorithm. To accurately capture the propagation of β, r and ϕ at high frequency, the sampling time T is set to 0.02 s considering the accuracy of the captured vehicle information. Note that the values of the control horizon Nc and prediction horizon Np are set to Nc = 20 and Np = 30 based on control accuracy and calculation efficiency considerations. The weighting matrices are obtained by iteratively tuning, R = diag{10, 1}, Q = diag{1000, 5}, and W = 10.

#### Stability Constraints

The vehicle lateral stability constraints are defined by the bounds of the two key variables of vehicle sideslip angle β and yaw rate r. The bounds of the β and r reflect the maximum capabilities of the tire under the assumptions of steady-state cornering [30]. The maximum steady-state yaw rate and vehicle sideslip angle can be expressed as

left{ begin{aligned} r_{max} & = frac{mu g}{{v_{x} }}, \ beta_{max} & { = }arctan (0.02mu g). \ end{aligned} right.

(19)

Considering the tire saturation and maximum tire lateral force, the maximum steady-state yaw rate and vehicle sideslip angle can also be expressed as

left{ begin{aligned} alpha_{r,sat} & = arctan (3l_{f} mu mg/LC_{{a_{r} }} ), \ r_{max} & = mu g/v_{x} , \ beta_{max} & = alpha_{r,sat} + l_{r} r/v_{x} . \ end{aligned} right.

(20)

The vehicle stability constraints defined by Eq. (20) can be expressed as the matrix inequality.

$$left| {{varvec{H}}_{V} {varvec{x}}} right| le {varvec{G}}_{V} ,$$

(21)

where ({varvec{H}}_{V} = left[ {begin{array}{*{20}c} {1/v_{x} } & { – l_{r} /v_{x} } & 0 & 0 & 0 & 0 \ 0 & {1} & 0 & 0 & 0 & 0 \ end{array} } right]), ({varvec{G}}_{V} = left[ {begin{array}{*{20}c} {alpha_{{r{,}sat}} } & {mu g/v_{x} } \ end{array} } right]^{{text{T}}}).

Vehicle rollover stability is a significant concern in transportation design. In particular, vehicle rollover stability is especially important under high-speed and large-curvature extreme conditions. To predict the vehicle rollover propensity, the zero-moment point (ZMP) metric is applied to the projected vehicle trajectories. The ZMP is given by Ref. [31].

$$y_{ZMP} = hphi + frac{h}{g}(dot{v}_{y} + v_{x} r) – frac{{I_{x} }}{mg}ddot{phi }.$$

(22)

According to the state space model, the ZMP can be written as

$$y_{ZMP} = {varvec{N}}_{1} dot{varvec{x}} + {varvec{N}}_{2} {varvec{x}},$$

(23)

where ({varvec{N}}_{1} = left[ {begin{array}{*{20}c} frac{h}{g} & 0 & { – frac{{I_{x} }}{mg}} & {begin{array}{*{20}c} 0 & 0 & 0 \ end{array} } \ end{array} } right]), ({varvec{N}}_{2} = left[ {begin{array}{*{20}c} 0 & {frac{{hv_{x} }}{g}} & 0 & {begin{array}{*{20}c} h & 0 & 0 \ end{array} } \ end{array} } right]).

The vehicle rollover stability constraints defined by Eq. (23) can be expressed as Eq. (24).

$$- l_{s} le y_{ZMP} le l_{s} .$$

(24)