Simulation-Based Engineering LabUniversity of Wisconsin-Madison

Technical Report TR-2019-06

Autonomous Driving via Model Predictive Control for Path

Tracking and PID Speed Control

Shuping Chen and Dan Negrut

October 14, 2019

Abstract

Path tracking is an essential task for autonomous vehicles. In this report, we demon-strate a Model Predictive Control (MPC)-based approach to path tracking combinedwith a PID strategy for speed control to ensure the vehicle follows a desired path andspeed profile. To implement this controller, we first considered the situation in whichthe speed is constant and designed the model predictive path tracking controller tofollow an 8-shaped curve path. Then, we added the PID speed control in the modelpredictive control (MPC) scheme to make the vehicle track the desired longitudinalvelocity. We present MatlabR© simulation results for these two scenarios. Comparedwith the reference objectives, the tracking errors of the heading angle ‘ψ’, lateral dis-placement of vehicle center of mass (C.M.) ‘Y ’ and the speed from both controllers aresmall. This work uses the MPC fundamentals discussed in [1,2] to build on preliminaryresults discussed in [3] using vehicle models presented in [4].

Keywords: Model Predictive Control, Vehicle Dynamics, Path Tracking, SpeedControl

1

Contents

1 Introduction 3

2 Vehicle Modeling 42.1 Tire model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42.2 8-DOF vehicle model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42.3 14-DOF vehicle model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72.4 Wheel rotational dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13

3 Lateral and Longitudinal Control 143.1 Lateral control: model predictive control for path tracking . . . . . . . . . . 14

3.1.1 Linearization of the vehicle model . . . . . . . . . . . . . . . . . . . . 153.1.2 State prediction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 163.1.3 Cost function definition . . . . . . . . . . . . . . . . . . . . . . . . . 173.1.4 Constraint analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17

3.2 Longitudinal Control: PID speed control . . . . . . . . . . . . . . . . . . . . 193.2.1 Longitudinal dynamics equation . . . . . . . . . . . . . . . . . . . . . 193.2.2 PID controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19

4 Simulation and analysis 204.1 Model parameters . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 204.2 Tracking objective definition . . . . . . . . . . . . . . . . . . . . . . . . . . . 214.3 Path tracking and speed control simulation results . . . . . . . . . . . . . . . 24

4.3.1 Case A: Path tracking with constant speed (10m/s) . . . . . . . . . . 244.3.2 Case B: Path tracking with speed control . . . . . . . . . . . . . . . . 26

5 Conclusions and Future Work 30

2

1 Introduction

Model predictive control has its roots in optimal control. The basic concept of MPC isto use a dynamic model to forecast system behavior, and optimize the control move atthe current time to produce the best performance in the future [5]. MPC method, whichcombines predictive models, a receding horizon optimization and a feedback correction, hasa very good ability to solve the control problem with multiple constraints. The constraintsare used to build the objective function of an MPC controller to obtain an optimal controloutput [6]. Recent work has shown that MPC can be used to rigorously handling the multiplevehicle dynamics and safety constraints and the stability of this algorithm is also well studied[7]. The lateral dynamics control and the longitudinal dynamics control are essential forautonomous vehicles. In the literature, lateral and longitudinal control have been studiedin a decoupled way. Actually, numerous studies implemented the path tracking controllerbased on the assumption that the longitudinal velocity is constant for the lateral control.However, the longitudinal and lateral dynamics of an automotive vehicle are highly nonlinearand coupled and the vehicle speed may be varied along the path [8]. In actual situation, avehicle should drive or brake according to the road conditions, for instance, when a vehicleruns into a large curvature bend, slowing down in advance is necessary. After a vehicle gothrough a bend and enters a straight road, the necessary acceleration should be applied.Therefore, a time-varying velocity condition should be considered [9].

In this report, the model predictive path tracking and the speed controller are designedand implemented in Matlab R©. For the lateral control, we used model predictive controland took the 8-DOF model as the prediction model, the 14-DOF model as the plant. Thetracking path is a straight line connected with the top of an 8-shaped curve path shown inFigure 1. When implementing the path following controller, we first consider the desiredvehicle speed is constant (Case A) and then consider the vehicle speed is time-varied (CaseB). For the longitudinal control, we used PID controller to track the desired speed. The tiremodel here is simplified as a linear tire model with assumption of small slip ratio and slipangle. The lateral control and longitudinal control are combined in the MPC controller togenerate the optimal control output: the front wheel steering angle and the total driving orbraking wheel torque which were passed to the plant vehicle simultaneously so as to trackthe reference trajectory and speed.

The remainder of this paper is organized as follows. In Section 2 we described thetire model, 8-DOF vehicle model, 14-DOF vehicle model and wheel rotational dynamicsequations, which were used in the MPC controller and the plant. In Section 3 we specifiedthe design of model predictive control for path tracking and the PID controller for the speedtracking; In Section 4 we implemented the proposed controller with two cases and providedthe simulation results compared with the references. In Section 5 we presented the conclusionbased on the simulation results and the potential future work for this study.

3

Figure 1: reference path

2 Vehicle Modeling

2.1 Tire model

The tire model for calculating the lateral and longitudinal forces is complex and assumedto depend on normal force, slip angle, surface friction, and longitudinal slip ratio. However,when the slip ratio and slip angle are limited within small values, the tire model can besimplified and generate linearized lateral force and longitudinal force [10]. Herein, the tiremodel used in the controller and the plant is linear tire model assuming that the slip ratioand slip angle are small.

Under this assumption, the tire lateral force can be given as Fc = Cα(µ, Fz)α, where Cαis tire cornering stiffness related to tire normal force and road-tire friction coefficient; Thetire longitudinal force can be given as Fl = Cx(µ, Fz)sf,r, where sf,r is the slip ratio of fronttire or rear tire, Cx is the tire longitudinal stiffness which also related to the tire normalforce and road-tire friction coefficient.

2.2 8-DOF vehicle model

An 8-DOF full vehicle model is often used as a simplified lower order model for studyingvehicle handling in scenarios which do not involve significant longitudinal accelerations. Inthis section, a formulation for the 8-DOF model, adapted from various references [11] [12],that can match the 14-DOF model reasonably accurately is presented.

The schematic of the 8-DOF full vehicle model is shown in Figure 2. The model hasfour degrees of freedom for the chassis and one degree of freedom at each of the four wheels

4

representing the wheel spin dynamics. The chassis includes the longitudinal velocity, u,the lateral velocity, v, the roll angular velocity, ωx, and the yaw angular velocity, ωz. Thepitch and heave motions are not modeled and the front and rear suspensions are representedsimply by their respective equivalent roll stiffness (kψf/kψr) and roll damping coefficients(bψf/bψr) [11].

Figure 2: Schematic of 8-DOF full vehicle model [11]

mt

(u− ωzv

)=∑

Fxgij + (mufa−murb)ω2z − 2hrcmωzωx (1)

mt

(v + ωzu

)=∑

Fygij + (murb−mufa)ωz + hrcmωx (2)

Jzωz + Jxzωx = (Fyglf + Fygrf ) a− (Fyglr + Fygrr) b+

(Fxgrf − Fxglf

)cf

2

+

(Fxgrr − Fxglr

)cr

2+(murb−mufa

)(v + ωzu

)(3)(

Jx +mh2rc)ωx + Jxzωz =mghrcφ−

(kφf + kφr

)φ−

(bφf + bφr

)φ+ hrcm

(v + ωzu

), (4)

5

where

hrc =hrcfb+ hrcra

a+ b. (5)

In these equations, the forces Fxgij and Fygij are the longitudinal and lateral forces atthe four tire contact patches and the subscript ‘ij ’ denotes lf, rf, lr, and rr. As before,hrcf and hrcr are the vertical distances of the front and rear roll centers below the sprungmass C.M., and thus hrc is the vertical distance from the sprung mass C.M. to the vehicleroll center. It should be noted that as Eq.(4) for the roll degree of freedom is written byconsidering moments acting about the vehicle roll center rather than the sprung mass C.M.,the roll inertia of the sprung mass about the vehicle roll center

(Jx +mh2rc

)is considered in

Eq.(4) [11].The equations for the wheel dynamics and the longitudinal and lateral tire forces are the

same as those used in the 14-DOF vehicle model. The longitudinal and lateral velocities at,for example, the right front tire contact patch required in these equations are given as [11]

ugrf =u+ωzcf

2(6)

vgrf =v + ωza. (7)

The definition and calculation method of the lateral sideslip angle and the longitudinalslip ratio for each wheel are the same as those used in 14-DOF vehicle model.

The normal forces at the four tires are determined as [11]

Fzglf =mgb

2(a+ b)+mufg

2−

(mufhufcf

+mb(hcg − hrcf

)cf (a+ b)

)(v + ωzu

)−(kφf + bφf

)cf

−(mhcg +mufhuf +murhur

)(u− ωzv

)2(a+ b)

(8)

Fzgrf =mgb

2(a+ b)+mufg

2+

(mufhufcf

+mb(hcg − hrcf

)cf (a+ b)

)(v + ωzu

)+

(kφf + bφf

)cf

−(mhcg +mufhuf +murhur

)(u− ωzv

)2(a+ b)

(9)

Fzglr =mga

2(a+ b)+murg

2−

(murhurcr

+ma(hcg − hrcr

)cr(a+ b)

)(v + ωzu

)−(kφrφ+ bφrφ

)cr

+

(mhcg +mufhuf +murhur

)(u− ωzv

)2(a+ b)

(10)

Fzgrr =mga

2(a+ b)+murg

2+

(murhurcr

+ma(hcg − hrcr

)cr(a+ b)

)(v + ωzu

)+

(kφrφ+ bφrφ

)cr

+

(mhcg +mufhuf +murhur

)(u− ωzv

)2(a+ b)

. (11)

6

These equations are fairly simple and linearized. It is possible to include several additionalterms in the equations for the chassis velocities as well as the tire forces. However, in ourexperience, these terms have very little effect on the vehicle responses and can be ignored.The 8-DOF model cannot simulate vehicle behavior beyond wheel lift-off. Nevertheless, themodel is valid for applications which do not involve wheel lift-off such as active steering andactive throttle/brake systems for yaw control [11].

2.3 14-DOF vehicle model

In order to better represent the vehicle lateral and yaw dynamics as well as coupling ofyaw-roll motion due to the transient lateral load transfer during extreme maneuvers, higherorder model such as 8-DOF and 14-DOF are also used in rollover studies. A 14-DOF vehiclemodel, which considers the suspension at each corner, has the same benefits of an 8-DOFvehicle model, with the additional capabilities of predicting vehicle pitch and heave motions.It also offers the flexibility of modeling nonlinear springs and dampers and can simulatethe vehicle responses to normal force inputs in the case of an active suspension system.Moreover, the 14-DOF model, unlike the 8-DOF model, can predict vehicle behavior evenafter wheel lift-off and thus can be used in rollover prediction/prevention strategies [11].

The schematic shown in Figure 3 exhibits the two axle, 14-DOF, vehicle model used toinvestigate vehicle roll response to steering and torque inputs. This schematic includes 6DOF at the vehicle lumped mass C.M. and 2 DOF at each of the four wheels, includingvertical suspension travel and wheel spin. The body is modeled as being rigid, with body-fixed coordinates, (xyz), attached at the center of mass and aligned in principal directions(coordinate frame 1). u, v, w indicate forward, lateral, and vertical velocities, respectively,of the sprung mass [11].

The force and velocity components in the right front corner of a vehicle is depicted inFigure 4. The velocity usrf , vsrf , and wsrf are the velocities of the right front strut mountingpoint in the longitudinal, lateral, and vertical directions, respectively, in the body-fixedcoordinate frame, which is attached to the sprung mass C.M. (coordinate frame 1). Thesevelocities can be obtained by transforming the C.M. velocities as [11]

usrfvsrfwsrf

=

0 0

cf2

0 0 a−cf

2−a 0

ωxωyωz

+

uvw

. (12)

The forces Fxsrf ,Fysrf and Fzsrf are the forces transmitted to the sprung mass along thelongitudinal, lateral, and vertical directions, respectively, of coordinate frame 1. The forcesFxgsrf , Fygsrf , and Fzgsrf are the forces acting at the tire ground contact patch in the samecoordinate frame 1. These forces can be written in terms of the tire forces Fxgrf , Fygrf , and

7

Figure 3: Schematic of 14-DOF vehicle model with one-dimensional suspension and coordi-nate frames [11]

Fzgrf by projecting its components along coordinate frame 2 as [11]FxgsrfFygsrfFzgsrf

=

1 0 00 cosφ sinφ0 −sinφ cosφ

cosθ 0 −sinθ0 1 0

sinθ 0 cosθ

FxgrfFygrfFzgrf

. (13)

The forces Fxgrf and Fygrf are obtained by resolving the longitudinal(Fxtrf ) and cornering(Fytrf )forces at the tire contact patch as [11]

Fxgrf =Fxtrfcosδ − Fytrfsinδ (14)

Fygrf =Fytrfcosδ + Fxtrfsinδ, (15)

where the δ is the steering angle at the road wheels.Here, the linear tire model has been used in the development of the tire forces Fxtrf and

Fytrf . The lateral tire slip angle used in the tire model was calculated as follows [11]:

αrf =tan−1

(vgrfugrf

)− δ. (16)

So the lateral tire force in small angle region can be modeled as

Fytrf = Cαfαrf , (17)

where Cαf is the cornering stiffness of the front tire.

8

Figure 4: Description of forces and velocities at the right front corner of a vehicle [11]

The longitudinal slip is defined as the difference between the actual longitudinal velocityat the axle of wheel Vx and the equivalent rotational velocity reffω of the tire, i.e. reffω−Vx.And, the longitudinal slip ratio is defined as [13]

σx =reffωw − Vx

Vxduring braking (18)

σx =reffωw − Vxreffωw

during acceleration, (19)

where the Vx for front and rear wheel are given as

Vxrf = ugrfcosδ + vgrfsinδ (20)

Vxrr = ugrr. (21)

So, the longitudinal tire force in small-slip region can be modeled as

Fxtrf = Cxfσxf , (22)

where Cxf is the longitudinal tire stiffness of the front tire.The ugrf and vgrf at the right front can be determined as [11]

ugrf =cosθ(uurf − ωy · rrf

)+ sinθ

(wurfcosφ+ sinφ

(ωx · rrf + vurf

))(23)

vgrf =cosφ(vurf + ωx · rrf

)− wurfsinφ. (24)

9

The longitudinal (uurf ) and lateral (vurf ) velocities of the unsprung mass in the bodycoordinate frame used in the earlier equations are simply written as [11]

uurf =usrf − lsrfωy (25)

vurf =vsrf + lsrfωx, (26)

where lsrf is the instantaneous length of the strut.The derivative of uurf and vurf is given as

uurf =usrf −((ωsrf − ωurf )ωy + lsrf ωy

)(27)

vurf =vsrf +((ωsrf − ωurf )ωy + lsrf ωy

). (28)

The unsprung mass vertical velocity wurf represents the degree of freedom correspondingto the suspension deflection and can be expressed by applying Newton’s law for the verticalmotion of the unsprung mass as [11]

muwurf =cosφ(cosθ

(Fzgrf −mug

)+ sinθFxgrf

)− sinφFygrf − Fdzrf

− xsrf · ksf − xsrf · bsf −mu

(vurf · ωx − uurf · ωy

), (29)

where ksf is the suspension stiffness, bsf the suspension damping coefficient, and xsrf theinstantaneous compression of the right front suspension spring. The force, Fdzrf , representsthe additional load transfer that occurs at the wheels through the suspension links becauseof the reaction force to the force transmitted to the sprung mass through the roll center [11].

The instantaneous suspension spring deflection xsrf is given as [11]

xsrf = −wsrf + wurf . (30)

The vertical force Fzgrf acting at the tire ground contact patch in coordinate frame 2can be written in terms of the tire stiffness (ktf ) and the instantaneous tire deflection (xtrf )as [11]

Fzgrf = Fztrf = xtrfktrf . (31)

The instantaneous tire deflection xtrf in equation (31) is given as [11]

xtrf = wgrf − wuirf = wgrf −(cosθ

(wurfcosφ+ vurfsinφ

)− uurfsinθ

), (32)

where wuirf is the vertical velocity of the wheel center in the inertial coordinate frame.Forthe simulations in this article, it is assumed that the vertical velocity wgrf at the tire contactpatch is zero (smooth road) [11].

The instantaneous tire radius is then determined as [11]

rrf =r0 − xtrfcosθcosφ

. (33)

10

To account for the wheel lift-off, when the tire radial compression becomes less than zero,the tire normal force Fzgrf is set equal to zero. In addition, the instantaneous tire radius isconsidered equal to the nominal tire radius until the tire returns to the road surface [11].

If xtrf < 0 then Fzgrf = 0 and rrf = r0. (34)

As the tire normal force becomes zero, no lateral (Fygrf ) and longitudinal (Fxgrf ) tireforces are developed at that contact patch. Thus, the only forces acting at that suspensioncorner are the weight and inertia forces of the unsprung mass. As the cardan angles andappropriate coordinate transformations between the body-fixed coordinate frame 1 and thecoordinate frame 2 at the tire-ground contact patch are considered for all the forces andvelocities in the system, the model is able to simulate vehicle behavior after wheel lift-offand during the rollover event with just the modification mentioned in (34) [11].

The instantaneous length of the strut lsrf is given as [11]

lsrf = lsif −(xsrf − xsif

), (35)

where lsif is the initial length of strut and xsif is the initial suspension spring deflection.The initial length of the strut lsif is taken such that [11]

lsif = h− (r0 − xtif ), (36)

where xtif is the initial tire compression.The initial spring compression xsif and the initial tire compression xtif are determined

from the static conditions as [11]

xsif =mgb

2(a+ b)ksf(37)

xtif =(mgb/2(a+ b) +mufg)

ktf. (38)

The forces Fxsrf and Fysrf transmitted to the sprung mass along the u− and v−axes of thebody-fixed coordinate frame are obtained after subtracting the components of the unsprungmass weight and inertia forces from the corresponding forces Fxgsrf and Fygsrf acting at thetire contact patch as [11]

Fxsrf =Fxgsrf +mugsinθ −muuurf +muωzvurf −muωywurf (39)

Fysrf =Fygsrf −mugsinφcosθ −muvurf +muωxwurf −muωzuurf . (40)

The vertical force Fzsrf transmitted to the sprung mass through the strut is given as [11]

Fzsrf = xsrfksrf + xsrfbsrf . (41)

Figure 5 shows the forces and velocities in the roll plane of, for example, the frontsuspension. Generally, the roll center height is defined with reference to the ground. However,

11

for the development of this model, the front and rear roll centers are assumed to be fixed atdistances hrcf and hrcr, respectively, below the sprung mass C.M. along the negative w-axisof the body-fixed coordinate frame 1. Moreover, the roll center is simply considered to bea point of application of the forces transmitted to the sprung mass through the suspensionlinks and not as a kinematic constraint [11].

Figure 5: Forces and velocities in the front suspension roll plane [11]

In the figure 5, Fzslf and Fzsrf are the forces transmitted to the sprung mass through thestruts. Fyslf and Fysrf represet the lateral forces transmitted to the sprung mass through thesuspension links. In the absence of a roll center, i.e., when the roll center is assumed to bein the ground plane, the total roll moment transmitted to the sprung mass at, for example,the right front corner along the ωx direction is given as [11]

Mxrf =Fygsrf(lsrf + rrf

)−(mugsinφcosθ +muuurf −muωxwurf +muωzuurf

)· lsrf (42)

=Fygsrf · rrf + Fysrf · lsrf (43)

When a roll center is modeled as shown in Figure 5, the roll moment Mxrf transmittedto the sprung mass by the right front corner suspension is given as [11]

Mxrf = Fysrfhrcf . (44)

Thus, the inclusion of a roll center reduces the total roll moment transferred to the sprungmass by the front suspension. The difference between the roll moments in the absence of theroll center and when the roll center is considered acts directly on the unsprung mass and is

12

responsible for the link load transfer forces (jacking forces), Fdzlf and Fdzrf . These forcescan be estimated as [11]

Fdzrf = −Fdzlf =Fygsrfrrf + Fysrf lsrf + Fygslfrlf + Fyslf lslf − (Fysrf + Fyslf )hrcf

cf. (45)

The moments Myrf and Mzrf transmitted to the sprung mass at, for example, the rightfront corner by the suspension along the ωy and ωz directions can be given as [11]

Myrf =−(Fxsgrf (lsrf + rrf )− (−mugsinθ +muuurf −muωzvurf +muωywurf ) · lsrf

)(46)

=−(Fxsgrf · rrf + Fxsrf · lsrf

)(47)

Mzrf =0. (48)

The equation of motion for the 6 DOF of the sprung mass model can now be derivedfrom the direct application of Newton’s laws for the system as [11]

m(u+ ωyw − ωzv) =∑

(Fxsij) +mgsinθ (49)

m(v + ωzu− ωxw) =∑

(Fysij)−mgsinφcosθ (50)

m(w + ωxv − ωyu) =∑

(Fzsij + Fdzij)−mgcosφcosθ (51)

Jxωx =∑

(Mxij) +(Fzslf − Fzsrf )cf + (Fzslr − Fzsrr)cr

2(52)

Jyωy =∑

(Myij) + (Fzslr + Fzsrr)b− (Fzslf + Fzsrf )a (53)

Jzωz =∑

(Mzij) + (Fyslf + Fysrf )a− (Fyslr + Fysrr)b

+(−Fxslf + Fxsrf )cf + (−Fxslr + Fxsrr)cr

2, (54)

where m is the sprung mass and the subscript ‘ij ’ denotes left front (lf ), right front (rf ),left rear (lr), and right rear (rr).

The cardan angles θ, ψ, φ needed in the aforementioned equations are obtained by per-forming the integration of the following equations [11],

θ =ωycosφ− ωzsinφ (55)

ψ =ωysinφ

cosθ+ωzcosφ

cosθ(56)

φ =ωx + ωysinφtanθ + ωzcosφtanθ. (57)

2.4 Wheel rotational dynamics

In the case that the vehicle is front wheel driven, the rotational dynamics for the right frontand right rear wheels can be given as [12],

Jwωrf = Tarf − Tbrf − FxtrfR (58)

Jwωrr = −Tbrr − FxtrrR, (59)

13

where Tarf , Tbrf is the acceleration torque and braking torque applied to the front wheel.Tbrr is the braking torque applied to the rear wheel.

Then, we reconsider the longitudinal vehicle dynamics equation in the plant, 14-DOFvehicle model. The longitudinal model considered here is based on one wheel vehicle model.The sum of the longitudinal forces acting on the vehicle C.M. is given by [8] [9]:

m(u+ ωyw − ωzv) = Fp − Fr +mgsinθ, (60)

where v is the vehicle speed, Fp is the propelling force and Fr is the sum of resisting forces.The propelling force Fp is the controlled input resulting from brake and throttle actions.The sum of the resisting force Fr is given by [8] [9]:

Fr = Fa + Fg + Frr, (61)

where Fa is the aerodynamic force, Fg is the gravitational force and Frr is the rolling resis-tance force. The form of the wheel dynamics has been slightly modified to distinguish thetotal brake torque Tb and the total traction torque Ta as follows [8] [9]:

Iwω = −FlR + Ta − Tb. (62)

For longitudinal controller synthesis, the simplifying assumption of a non-slip rolling isconsidered, then the following relationship hold [8] [9]:

u = Rω (63)

Fp = Fl =1

R(Ta − Tb − Iwω) =

Ta − TbR

− IwR2u. (64)

For simplicity, the sum of the resisting force in the longitudinal dynamics is ignored,i.e., Fr = 0. The driving torque is divided equally and applied to the front left and rightwheels, the braking torque is divided equally and applied to the front and rear wheels, (i.e.,Talf = Tarf = 0.5Ta, Tblf = Tbrf = Tblr = Tbrr = 0.25Tb), so the vehicle longitudinal dynamicsequation (60) for the plant becomes:

u =1(

m+Jwr2lf

+Jwr2rf

+Jwr2lr

+Jwr2rr

)((0.5Ta − 0.25Tb)

rlf+

(0.5Ta − 0.25Tb)

rrf+

(−0.25Tb)

rlr

+(−0.25Tb)

rrr−mωyw +mωzv +mgsinθ

). (65)

3 Lateral and Longitudinal Control

3.1 Lateral control: model predictive control for path tracking

The basic concept of MPC is to use a dynamic model to forecast system behavior, andoptimize the control move at the current time to produce the best performance in the future.

14

Such a prediction is accomplished by employing an internal model over a fixed finite timehorizon, called the prediction horizon, from the current system state. At each sampling time,the controller generates an optimal control sequence, called control horizon, by solving anoptimization problem and the first element of this sequence is applied to the plant. Therepetition of this process over time by using the updated measurements creates a feedbackloop which continually controls the system, pushing it towards an optimal path [5] [14].

3.1.1 Linearization of the vehicle model

Here, we take the 8-DOF vehicle model as the internal prediction model and set the control

input u = δ and the state variable χ =[x, y, φ, ψ, φ, ψ, Y,X

]T, then the general form of the

system can be given as:

χ = f(χ, u). (66)

The general form around the operating point is

χo = f(χo, uo). (67)

Using the Taylor series expansion at the operating point and ignoring higher order terms,we can obtain [6]

χ = f (χo, uo) +∂f(χ, u)

∂χ

∣∣∣∣χ=χou=uo

(χ− χo) +∂f(χ, u)

∂u

∣∣∣∣χ=χou=uo

(u− uo) . (68)

subtracting Eq.(67) frome Eq.(68) results in

˙χ = Aχ+Bu, (69)

where A =∂f(χ, u)

∂χ

∣∣∣∣χ=χou=uo

, B =∂f(χ, u)

∂u

∣∣∣∣χ=χou=uo

, ˙χ = χ− χo, χ = χ− χo, and u = u− uo.

Eq.(69) is the linear error model [6].In order to apply the model to the design of the MPC controller, it is discretized in the

form of state-space representation [6]

χ(k + 1) = Adχ(k) +Bdu(k), (70)

where Ad = I + TA,Bd = TB and T is the sampling time.The Eq.(70) can be also given in the following form for the MPC controller design

χ(k + 1) = Adχ(k) +Bdu(k) + dk(k), (71)

where dk(k) = f(χo(k), uo(k))− (Adχo(k) +Bduo(k)).

15

3.1.2 State prediction

We define ξ(k) =

[χ(k)

u(k − 1)

]as the new state variable, η(k) as the output state variable,

and ∆u(k) = u(k)− u(k− 1) as the control input increment. Then, the discrete state-spacecontroller model can be translated into a new form as follows [6] [15]

ξ(k + 1) = Adξ(k) + Bd∆u(k) + dk(k) (72a)

η(k) = Cdξ(k), (72b)

where Ad =

[Ad Bd

0m×n Im

], Bd =

[Bd

Im

], dk(k) =

[dk(k)0m

], Cd =

[Cd 0p×m

](where m denotes

the dimension of control input, n denotes the dimension of state variable, and p denotes thedimension of output) [6].

We denote the sequence increment of future control input computed at time k as ∆Um,

that is ∆Um =[∆u(k), . . . ,∆u(k +m), . . . ,∆u(k +Nc − 1)

]T. The control input varies

for Nc time steps (i.e., the control horizon) and then is held constant up to the previewhorizon. We define the predicted output for the prediction state-space model as ηm(k) =[η(k + 1), . . . , η(k +Np)

]T. In this situation, it is straightforward to derive the prediction

model of performance outputs over the prediction horizon Np in a compact matrix form as [6]

ηm(k) = Θmξ(k) + Γm∆Um + ΨmDk, (73)

where

Θm =[CdAd CdA

2d . . . CdA

Ncd . . . CdA

Np

d

]T(74)

ηm(k) =[η(k + 1) . . . η(k +Np)

]T(75)

∆Um =[∆u(k) . . . ∆u(k +m) . . . ∆u(k +Nc − 1)

]T(76)

Dk =[dk(k) dk(k + 1) . . . dk(k +Np − 1)

]T(77)

Γm =

CdBd 0 · · · 0

CdAdBd CdBd · · · 0...

... · · · 0

CdANc−1d Bd CdA

Nc−2d Bd · · · CdBd

...... · · · ...

CdANp−1d Bd CdA

Np−2d Bd · · · CdA

Np−Nc

d Bd

(78)

Ψm =

Cd 0 0 · · · 0

CdAd Cd 0 · · · 0

CdA2d CdAd Cd · · · 0

......

... · · · ...

CdANp−1d CdA

Np−2d CdA

Np−3d · · · Cd

(79)

16

3.1.3 Cost function definition

The objective function of the path tracking controller can be given as [6]

J(k) =

Np∑j=1

χT (k + j)Qχ(k + j) + uT (k + j − 1)Ru(k + j − 1), (80)

where Q and R represent weight matrices, where the χ = χ− χo and u = u− uo. The firstterm in Eq.(80) reflects the capability of tracking performance, while the second reflects theconstraint on the change of the control output.

Considering the soft constraints concept, we can get an alternative form of the objectivefunction as follows [6]:

J(k) =

Np∑i=1

‖η(k + i|t)− ηref (k + i|t)‖2Q +Nc∑i=1

‖∆U(k + i|t)‖2R + ρε2. (81)

Considering Eq.(73), the objective function can be given as

J(k) =(

Θmξ(k) + Γm∆Um + Ψmdk − ηref)T

Q(Θmξ(k) + Γm∆Um + Ψmdk − ηref )

+ ∆UmTR∆Um + ρε2. (82)

To solve the following optimization problem, the objective function is converted into astandard quadratic form [6].

J(ξ(t), u(t− 1),∆U(t)

)= [∆U(t)T , ε]THt[∆U(t)T , ε] +Gt[∆U(t)T , ε], (83)

where Ht =

[ΓTmQΓm +R 0

0 ρ

], Gt =

[2eTt QΓm 0

], et = (Θmξ(k) + Ψmdk − ηref ) and et is

the tracking error in the prediction horizon [6].After obtaining the solution to Eq.(83) in each control cycle, a series of control input

increments in the control horizon can be calculated as [6]

∆U∗t =

[∆u∗t ,∆u

∗t+1, . . . ,∆u

∗k+Nc−1

]T. (84)

The first element of the control sequences is taken as the actual control input incrementof the controller [6].

u(t) = u(t− 1) + ∆u∗t . (85)

3.1.4 Constraint analysis

There are constrains imposed on the control increment and the outputs. Specifically, thevehicle dynamics model, the constraints imposed on steering angle for the path-trackingproblem are specified as [6]

17

1. The constraints for control can be given as

umin(t+ k) ≤ u(t+ k) ≤ umax(t+ k), k = 0, 1, · · · , Nc − 1 (86)

2. The constraints for control increments can be given as

∆umin(t+ k) ≤ ∆u(t+ k) ≤ ∆umax(t+ k), k = 0, 1, · · · , Nc − 1 (87)

3. The output constraints can be given as

ymin(t+ k) ≤ y(t+ k) ≤ ymax(t+ k), k = 0, 1, · · · , Nc − 1 (88)

According to the tire modeling, the relationship between the tire slip angle and corneringforce is linear when the tire slip angle is small. With small angle assumption for the lineartire models, the front wheel tire slip angle is limited in −2.5◦ ≤ α ≤ 2.5◦, and the controlincrement is limited in −0.85◦ ≤ ∆α ≤ 0.85◦.

In the cost function, the variables to be solved are control increments in the controlhorizon. The constraint conditions can only be expressed as a form of the control incrementor a form of the control increment multiplied by the transformation matrix. Thus, Eq.(86)through (88) need to be converted for obtaining the corresponding transformation matrix.The relationship is defined as [6]

u(t+ k) = u(t+ k − 1) + ∆u(t+ k), (89)

assuming that

Ut = 1Nc ⊗ u(k − 1) (90)

A =

1 0 · · · · · · 01 1 0 · · · 0

1 1 1. . . 0

......

. . . . . . 01 1 · · · 1 1

︸ ︷︷ ︸

Nc×Nc

⊗Im (91)

where 1Nc is the column vector of Nc ones, Im is the identity matrix with a dimension of m,⊗ is Kronecker product, and u(k − 1) is the previous control input [6].

Combining Eq.(89) through (91) can be converted into [6]

Umin ≤ A∆Ut + Ut ≤ Umax, (92)

where Umin and Umax are the lower and upper bound of the control input, respectively.This concludes the setting up of the constraint optimization problem induced by the modelprediction control algorithm [6].

18

3.2 Longitudinal Control: PID speed control

3.2.1 Longitudinal dynamics equation

The wheel rotational dynamics of front right and rear right wheel in the plant are given as,

Jwωrf = 0.5Ta − 0.25Tb − rrfFxtrf (93)

Jwωrr = −0.25Tb − rrrFxtrr, (94)

where the Ta is total driving torque applied to the vehicle, Tb is the total braking torqueapplied to the vehicle. With these wheel rotational equations and simplification in Section2.4, the longitudinal equation for the plant is given as

u =1(

m+Jwr2lf

+Jwr2rf

+Jwr2lr

+Jwr2rr

)((0.5Ta − 0.25Tb)

rlf+

(0.5Ta − 0.25Tb)

rrf+

(−0.25Tb)

rlr

+(−0.25Tb)

rrr−mωyw +mωzv +mgsinθ

). (95)

The longitudinal velocity and acceleration of the plant vehicle will be obtained from theabove equation, which will be used in the PID speed controller.

3.2.2 PID controller

More than half of the industrial controllers in use today are PID controllers or modified PIDcontrollers. The usefulness of PID controls lies in their general applicability to most controlsystems. Figure 6 shows a PID control of a plant [16].

Figure 6: PID control of a plant [16]

The speed tracking error is defined as

eu = ud − u (96)

eu = ud − u (97)

esx =

∫ t2

t1

eudt = euTs, (98)

where ud refers to the desired speed, u refers to the speed of the plant; ud refers to thedesired longitudinal acceleration, u refers to the longitudinal acceleration of the plant. Ts isthe sample time.

19

Here, when the speed of the plant is smaller than the desired speed, then Tb = 0, Ta =KP eu + KIesx + KDeu; when the speed of the plant is greater than the desired speed, thenTa = 0, Tb = KP eu +KIesx +KDeu; when the speed of the plant equals to the desired speed,then Ta = 0, Tb = 0.

4 Simulation and analysis

4.1 Model parameters

The parameters for these vehicle models are listed in the table 1:

Table 1: Vehicle modeling parameters

Symbol Parameter description Value

m Sprung mass 1400(kg)Jx Sprung mass roll moment of inertia 900(kgm2)Jy Sprung mass yaw moment of inertia 2000(kgm2)Jz Sprung mass pitch moment of inertia 2420(kgm2)a Distance of sprung mass C.M. from front axle 1.14(m)b Distance of sprung mass C.M. from rear axle 1.4(m)h Sprung mass C.M. height 0.75(m)cf Front track width 1.5 (m)cr Rear track width 1.5 (m)ksf Front suspension stiffness 35000 (N/m)ksr Rear suspension stiffness 30000 (N/m)bsf Front suspension damping coefficient 2500 (Ns/m)bsr Rear suspension damping coefficient 2000 (Ns/m)muf Front unsprung mass 80 (kg)mur Rear unsprung mass 80 (kg)ktf Front tire stiffness 200000 (N/m)ktr Rear tire stiffness 200000 (N/m)Cαf Front right tire cornering stiffness 44000 (N/rad)Cαr Rear right tire cornering stiffness 47000 (N/rad)Cxf Front right tire longitudinal stiffness 5000 (N)Cxr Rear right tire longitudinal stiffness 5000 (N)r0 Nominal tire radius 0.285 (m)Jw Tire/wheel roll inertia 1 (kgm2)hrcf Front roll center distance below sprung mass C.M. 0.65 (m)hrcr Rear roll center distance below sprung mass C.M. 0.6 (m)

20

4.2 Tracking objective definition

We implemented the controller for path tracking under the conditions of an 8-shaped curvepath. In Case A, we consider the speed of the vehicle is constant at 10m/s. In Case B, weconsider the speed is increased from 0.5m/s to 10.4m/s in the straight line and then keptconstant after entering the curve. In the path tracking controller, we chose the heading angle‘ψ’ and the lateral displacement of vehicle C.M. in global coordinates ‘Y ’ as the objectives;In the speed controller, we chose the speed of the vehicle C.M. as the objective.

In every simulation step, the optimal steering angle control input calculated from theMPC controller and the total driving or braking torque calculated from the PID controller aresimultaneously transmitted to the plant to implement the path tracking and speed control.

The reference path, the reference lateral displacement of vehicle C.M. ‘Y ’ in global coor-dinates, the reference heading angle ‘ψ’ and the speed reference are given as follows:

Figure 7: Tracking path: 8-shaped curve

21

Figure 8: Speed tracking objective (Case A)

Figure 9: Speed tracking objective (Case B)

22

Figure 10: path tracking objective: desired lateral displacement

Figure 11: path tracking objective: desired heading angle

23

4.3 Path tracking and speed control simulation results

4.3.1 Case A: Path tracking with constant speed (10m/s)

Figure 12: Comparison of Trajectory

Figure 13: Comparison of lateral displacement

24

Figure 14: Comparison of heading angle

Figure 15: Comparison of yaw rate

25

Figure 16: steering angle input

4.3.2 Case B: Path tracking with speed control

The speed reference in case B is firstly increased in the straight line and kept constant in the8-shaped curve, the tracking performance of the simulation results are presented as below:

Figure 17: Comparison of Trajectory

26

Figure 18: Comparison of speed

Figure 19: Comparison of lateral displacement

27

Figure 20: Comparison of heading angle

Figure 21: Comparison of longitudinal displacement

28

Figure 22: Comparison of yaw rate

Figure 23: Steering angle input

29

Figure 24: Wheel torque input

5 Conclusions and Future Work

In this study, the model predictive path tracking with PID speed control was proposedand implemented to ensure that the vehicle follows the given 8-shaped curve path and thedesired speed. With respect to the lateral control, we used the 8-DOF vehicle model as theprediction model in the MPC controller and used the 14-DOF vehicle model as the plant.With respect to the longitudinal control, we used the PID controller to track the desiredspeed. Here, two cases for the reference speed were considered, case A is path tracking withconstant speed, case B is path tracking and speed control with varied speed. Based on thesimulation results, both controllers tracked the desired path well and compared with thereference heading angle ‘ψ’, the yaw rate ‘ψ’, the lateral displacement of vehicle C.M. ‘Y ’and the desired speed, the tracking errors are small.

In the future work, designing a speed profile along the give path will be considered forthe vehicle in order to track the path as fast as possible.

Acknowledgments

The authors would like to thank Heran Shen of Columbia University for his comments onan early version of this document.

30

References

[1] S. Chen and D. Negrut, “The basics of Model Predictive Control in multibody dynam-ics,” Tech. Rep. TR-2019-02: https://sbel.wisc.edu/wp-content/uploads/sites/

569/2019/10/TR-2019-02.pdf, Simulation-Based Engineering Laboratory, Universityof Wisconsin-Madison, 2019.

[2] S. Chen and D. Negrut, “Model Predictive Control for multibody dynamics via cosimu-lation of Chrono and MatlabR©/SimulinkR©,” Tech. Rep. TR-2019-03: https://sbel.

wisc.edu/wp-content/uploads/sites/569/2019/10/TR-2019-03.pdf, Simulation-Based Engineering Laboratory, University of Wisconsin-Madison, 2019.

[3] S. Chen and D. Negrut, “Vehicle path tracking via Model Predictive Control,”Tech. Rep. TR-2019-05: https://sbel.wisc.edu/wp-content/uploads/sites/569/

2019/10/TR-2019-05.pdf, Simulation-Based Engineering Laboratory, University ofWisconsin-Madison, 2019.

[4] S. Chen and D. Negrut, “A MATLABR© implementation of a set of three vehicledynamics models,” Tech. Rep. TR-2019-04: https://sbel.wisc.edu/wp-content/

uploads/sites/569/2019/10/TR-2019-04.pdf, Simulation-Based Engineering Labo-ratory, University of Wisconsin-Madison, 2019.

[5] J. B. Rawlings, D. Q. Mayne, and M. Diehl, Model predictive control: theory, computa-tion, and design. Nob Hill Publishing, Madison, Wisconsin, 2nd ed., 2018.

[6] J. Gong, Y. Jiang, W. Xu, K. Liu, H. Guo, and Y. Sun, “Multi-constrained model pre-dictive control for autonomous ground vehicle trajectory tracking,” Journal of BeijingInstitute of Technology, vol. 24, no. 4, pp. 441–448, 2015.

[7] K. Liu, J. Gong, A. Kurt, H. Chen, and U. Ozguner, “Dynamic modeling and controlof high-speed automated vehicle for lane change maneuver,” IEEE Transactions onIntelligent Vehicles, vol. 3, no. 3, pp. 329–339, 2018.

[8] R. Attia, R. Orjuela, and M. Basset, “Combined longitudinal and lateral control forautomated vehicle guidance,” Vehicle System Dynamics, vol. 52, no. 2, pp. 261–279,2014.

[9] F. Lin, Y. Zhang, Y. Zhao, G. Yin, H. Zhang, and K. Wang, “Trajectory tracking ofautonomous vehicle with the fusion of dyc and longitudinallateral control,” ChineseJournal of Mechanical Engineering, vol. 32, no. 1, pp. 1–16, 2019.

[10] F. Borrelli, P. Falcone, T. Keviczky, J. Asgari, and D. Hrovat, “MPC-based approachto active steering for autonomous vehicle systems,” International Journal of VehicleAutonomous Systems, vol. 3, no. 2, pp. 1–25, 2005.

31

[11] T. Shim and C. Ghike, “Understanding the limitations of different vehicle models forroll dynamicsstudies,” Vehicle System Dynamics, vol. 45, no. 3, pp. 191–216, 2007.

[12] J. He, D. A. Crolla, M. C. Levesley, and W. J. Manning, “Integrated active steeringand variable torque distribution control for improving vehicle handling and stability,”SAE Technical Paper Series, pp. 2004–01–1071, 2004.

[13] R. Rajamni, D. Piyabongkarn, J. Lew, and J. Grogg, “Algorithms for real-time estima-tion of individual wheel tire-road friction coefficients,” in American Control Conference,2006.

[14] F. Kuhne, W. F. Lages, and J. M. G. da Silva, “Model predictive control of a mobilerobot using linearization,” Proceedings of mechatronics and robotics, pp. 525–530, 2004.

[15] L. Wang, Model predictive control system design and implementation using MATLAB R©.Springer Science & Business Media, 2009.

[16] K. Ogata, Modern Control Engineering. Pearson Education, Inc., 5nd ed., 2010.

32