INTEGRATED VEHICLE DYNAMICS CONTROL IN AUTONOMOUS VEHICLES:

A REAL-TIME MPC APPROACH

Jahan Asgari, H. Eric Tseng, Davor HrovatFord Research Laboratory, Dearborn, USA

Paolo Falcone, Francesco Borrelli, Luigi GlielmoUniversità degli Studi del Sannio, Benevento, Italy

Applicazioni e prospettive del controllo nei veicoli

Politecnico di Milano, 10 maggio 2007

2

OutlineIntroduction

Guidance and Navigation Control algorithms

Vehicle Dynamics Control

Control Oriented Vehicle Model

NLMPC (Non Linear MPC) approach

LTV-MPC (Linear Time Varying MPC) approach

Experimental results

3

Guidance and Navigation Control Algorithms

Trajectory-ModeGenerator

TrajectoryUpdate

Mode ofOperation

Inner Loop Control

State Estimator

ParameterEstimator

Vehicle andEnvironment

Trajectory-Mode Replanning

Low-Level Control System

Precomputed off-line

u

y

• On board cameras• Infrared• Radars• Gyro, GPS

Yaw, roll, pitch, lateral, longitudinal and vertical stabilization

4

Current Autonomous Vehicle Control Design

Trajectory-ModeGenerator

Vehicle andEnvironment

Trajectory-Mode Replanning

Low-Level Control System

Precomputed off-line

Obstacles Detection

and Avoidance Algorithms

PID Controllers Basedon Linear

Vehicle Models

u

yOnline NonlinearOptimization,based on Point-Mass Model andAccelerationConstraints

Used by some DARPA Grand Challenge Vehicles like Alice (Caltech), Stanley (Stanford University)

5

To Develop Advanced

Model Based Control Strategies for

Integrated Vehicle Dynamics Control

The Goal

6

Classical Vehicle Dynamics Control

Controlling Yaw, Roll, Pitch, Vertical, Lateral and Longitudinal Dynamics via Multiple Input

Active Front Steering (AFS) systemsAnti-lock Braking System (ABS)Electronic Stability Program(ESP)Traction Control (TC)Suspension control systemsActive differential control systems y

lateral

z yaw

pitch

vertical

longitudinal roll

ψ

θx φ Fx FyFz

Front steeringFour brakesEngine torqueActive suspensionsActive differential

Longitudinal, lateral and vertical velocity

Yaw, roll and pitch angles/rates

7

Integrated VDC via MPC

ylateral

z yaw

pitch

vertical

longitudinal roll

ψ

θx φ Fx FyFz

Front steeringFour brakesEngine torqueActive suspensionsActive differential

MIMO controller integrating

local and global measurements coming

from GPS, cameras, infrared and radar

...Position and velocity in a global

frame

Enabling path following capabilities

8

ScenarioProblem setup:

• Double lane change • Driving on snow/ice, withdifferent entry speeds

Control objective:

Minimize position and orientation errors from reference trajectory by changing the front wheel steering angle and braking at the four wheels

9

Challenges

6 DOF modelLongitudinal, lateral, vertical, roll, yaw and pitch dynamics

Highly nonlinear MIMO system with uncertaintiesTire characteristic, trigonometric functions, bilinear nonlinearities

Hard constraintsRate limit in the actuator, vehicle physical limits

Fast sampling timeTypically 20 ms

10

Motivations

Autonomous vehicleMilitary vehicles (DARPA Grand Challenge, Urban Challenge, etc…)Futuristic scenario for passenger cars in urban environment

Autonomous vehicle for civilian applicationsStop-and-go, lane assistant, obstacle avoidance, intelligent parking systems, proximity control systems

Improving guidance assistance systemsCombined AFS, ESP and brakes control (Integrated Vehicle Dynamics Control, IVDC)

11

OutlineIntroduction

Guidance and Navigation Control algorithms

Vehicle Dynamics Control

Control Oriented Vehicle Model

NLMPC (Non Linear MPC) approach

LTVMPC (Linear Time Varying MPC) approach

Experimental results

12

Four Wheels Model

States

XY

xy

ψψ&

&

& Lateral velocityLongitudinal velocityYaw angleYaw rateLateral position (I.F.)Longitudinal position (I.F.)

fα Front slip angle

fcF

flF

Front cornering force

Front longitudinal force

Other variables

fδ Front steering angleInputs

bF FL, FR, RL,RR brakesτ Desired engine torque

13

Simplified Driveline

( )( ))()(

)(),()1(tht

tutftξη

ξξ µ

=

=+

Dynamical Model

[ ]rrrlfrfl ,ω,ω,ω,Y,X,ωψ,,ψx,yξ &&&=

14

Pacejka Tire model

Semi-empirical model calibrated on

experimental data

),,,( zFsfF µα=

15

OutlineIntroduction

Guidance and Navigation Control algorithms

Vehicle Dynamics Control

Control Oriented Vehicle Model

NLMPC (Non Linear MPC) approach

LTVMPC (Linear Time Varying MPC) approach

Experimental results

16

Model Predictive Control

17

NLMPC Control design

( ) tosubj.

,min UJ tU∆

∆ξOptimization problem

( )( )

,,,

),( ,

,,

,,1,

,

,,

,,,,1

p

tktktk

tk

tktk

tktkstk

Httkuuu

th

uf

+=

∆+=

=

=

=

−

+

K

ξξξη

ξξ µ

Vehicle dynamics

max,,min, ftkf u δδ ≤≤Input constraints

1,, max,,min, −+=∆≤∆≤∆ cftkf Httku KδδConstraints on input changes

( )( ) ∑ ∑=

−

=++ ∆+−=∆

+

p c

tit

H

i

H

iRtitQreftit uUtJ

1

1

0

2,

2

, ,, ηηξCost function

Nonlinear Vehicle Dynamical

(Pacjeka Tire Model)

18

The NLMPC controllerNon-linear optimization problem

Non-linear optimization solver is required

High computational burden

Experimental tests are possible at low vehicle speed

Stability guaranteed

Nonlinear MPC in real time used for the first time in fast automotive applications with standard prototyping hardware and off-the-shelf nonlinear solver (sample time 50 ms)

19

Lateral and Yaw Stabilization via Active Front Steering (AFS)

20

83 Differential Equations

Simulation Enviroment: Carsim

21

OutlineIntroduction

Guidance and Navigation Control algorithms

Vehicle Dynamics Control

Vehicle modelling

NLMPC (Non Linear MPC) approach

LTVMPC (Linear Time Varying MPC) approach

Experimental results

22

The Gap…

NonlinearMPC

Online/ExplicitLTI/PWA MPC

Problem Domain: System Model, Sampling Time, Computational Resources

23

…Filling The Gap

For a MIMO “Fast System”• Nonlinear MPC is not implementable with

current methodologies/ technologies• Linear MPC is unstable / poor performance• Approximated PWA Model is complex (PWA

solution explodes)

Systematic Control Design Procedure with Constraints Fulfillments and Tuning Knobs

24

LTV MPC vs PWA

t

Y

PWA Prediction error. Can be large locally. Smaller over the horizon

t

⎥⎦

⎤⎢⎣

⎡

**

**

tt

tt

DCBAY

*t

ReferenceLinear PredictionNonlinear Prediction

LTV Prediction error. Small locally. Large over long horizons

⎥⎦

⎤⎢⎣

⎡

ii

ii

DCBA

ReferencePWA PredictionNonlinear Prediction

25

Summary

Nonlinear model. Nonlinear Programming PWA model. Mixed-Integer Programming/ Explicit Solution

Tire Slip

Tire Torqu

e

Piecewise affine approximation

x

)(xf

11

11

,,

++

++

kk

kk

DCBA

22

22

,,

++

++

kk

kk

DCBA

33

33

,,

++

++

kk

kk

DCBA

44

44

,,

++

++

kk

kk

DCBA

LTV model. Quadratic/Linear Programming

26

LTV MPC Control design( )( ) ∑ ∑

=

−

=++ +−=∆

+

p c

tit

H

i

H

iRtitQreftit uUtJ

1

1

0

2,

2

, ,, δηηξ

( ) tosubj.

,min UJ tU∆

∆ξ

1,, −−= ttktk uuuδ

max,min uuu tk ≤≤

1,, max,1,min −+=∆≤−≤∆ − ctktk Httkuuuu K

Optimization problem

Linearized Vehicle Dynamical

(Including Pacjeka Tire Model)

Input constraints

Constraints on input changes

Cost function

27

The LTV controller

A Quadratic Programming (QP) optimization problem has to be solved

A QP solver is required

Problem solved with small computational effort

Experimental tests even at high speed

Stability not guaranteed!

A Stability Condition has been proposed for such a scheme.

In summary: Stability depends on the prediction mismatch at each time step

28

The LTV controller (cont’d)Simulations results at 17 m/s

A linear model is not able to “predict” a slope change of the tire characteristic in the prediction horizon

29

Constraints on slip angle

minαmaxα

ptk Httktktk

+=≤≤ K ,, max,min ααα

Controller performs well up

to 21 m/s

State and input constraintThe system is still nonlinear

30

The NLMPC controller

The constraints on slip angle are effective both in simulations and in experimental tests

31

OutlineIntroduction

Guidance and Navigation Control algorithms

Vehicle Dynamics Control

Vehicle modelling

NLMPC (Non Linear MPC) approach

LTVMPC (Linear Time Varying MPC) approach

Experimental results

32

Revi Test Center in Arjeplog, Lapland

33

Experimental setup

Sampling time: 50 msThe experimental tests have been done using a dSpace rapid prototyping system equipped with a DS1005 processor boardMain limitation arising from dSpace: source code of the solver has to be availableDifferential GPS, gyros, lateral accelerometersJaguar X-type

34

NLMPC AFS controller at 7 m/s (25 Km/h)

The control and the prediction horizons are Hu=3, Hp=7

Problem dimension: 42 nonlinear constraints,12 linear constraints, 3 optimizersNPSOL has been used

35

LTVMPC AFS controller at 19 m/s (68.4 Km/h)

The control and the prediction horizons are Hu=10, Hp=25

Problem dimension: 54 linear constraints, 10 optimizersQPDANTZG is used

Bias in yaw angle measurement

36

Illustration of Measurement Bias (Single Antenna RT3000)

A

Local X

Bx

y

X

Vehicle coordinate andGlobal coordinate

Illustration of measurement bias takenin Controller B results

37

LTVMPC controller at 10 m/s (36 Km/h)

The control and the prediction horizons are Hu=1, Hp=25

Problem dimension: 54 linear constraints, 1 optimizersTailored QP solver is used

38

LTVMPC AFS controller at 20m/s (70 Km/h)

The control and the prediction horizons are Hu=10, Hp=25

Problem dimension: 54 linear constraints, 10 optimizersQPDANTZG is used

39

LTVMPC AFS controller at 21m/s (75.6 Km/h)

The control and the prediction horizons are Hu=1, Hp=25

Problem dimension: 54 linear constraints, 1 optimizersTailored QP solver is used

40

Braking and steering LTVMPC controller at 70 Kph

41

Braking and steering LTVMPV controller at 70 Kph (cont’d)

42

Braking and steering LTVMPV controller at 70 Kph (cont’d)

43

ConclusionsAFS and combined AFS and braking control problems have been presented

Nonlinear MPC controllers have been tested both in simulations and in experimental tests

A complex AFS NLMPC controller has been successfully implemented in real-time and experimentally validated at low speeds

A suboptimal MPC controller based on on-line linearizations has been designed and validated

44

Remarks

Sistematic control approach for Integrated VehicleDynamics ControlComputational burden can be decreased through suboptimal schemesStability and performance results have to beprovided for these non standard schemesExperimental setup