Predictive driver controller to track longitudinal speed and lateral path
Vehicle Dynamics Blockset / Vehicle Scenarios / Driver
The Predictive Driver block implements a controller that generates normalized steering, acceleration, and braking commands to track longitudinal velocity and a lateral reference displacement. The normalized commands can vary between 1 to 1. The controller uses a singletrack (bicycle) model for optimal singlepoint preview control.
Use the External Actions parameters to create input ports for signals that you can use to simulate standard test maneuvers. The block uses this priority order for the input commands: disable (highest), hold, override.
This table summarizes the external action parameters.
Goal  External Action Parameter  Input Ports  Data Type 

Override the accelerator command with an input acceleration command.  Accelerator override 
 Boolean 
 double  
Hold the acceleration command at the current value.  Accelerator hold  AccelHld  Boolean 
Disable the acceleration command.  Accelerator disable  AccelZero  Boolean 
Override the decelerator command with an input deceleration command.  Decelerator override 
 Boolean 
 double  
Hold the decelerator command at current value.  Decelerator hold  DecelHld  Boolean 
Disable the decelerator command.  Decelerator disable  DecelZero  Boolean 
Override the steering command with an input steering command.  Steering override 
 Boolean 
 double  
Hold the steering command at the current value.  Steering hold  SteerHld  Boolean 
Disable the steering command.  Steering disable  SteerZero  Boolean 
Use the Longitudinal control type, cntrlType parameter to specify one of these control options.
Setting  Block Implementation 

 Proportionalintegral (PI) control with tracking windup and feedforward gains. 
 PI control with tracking windup and feedforward gains that are a function of vehicle velocity. 
 Optimal singlepoint preview (look ahead) control model developed by C. C. MacAdam^{1, 2, 3}. The model represents driver steering control behavior during pathfollowing and obstacle avoidance maneuvers. Drivers preview (look ahead) to follow a predefined path. To implement the MacAdam model, the block:

Use the Lateral control type, controlTypeLat parameter to specify the type of lateral control. The table specifies the block implementation.
Setting  Block Implementation  

 Optimal singlepoint preview (look ahead) control model developed by C. C. MacAdam^{1, 2, 3}. The model represents driver steering control behavior during pathfollowing and obstacle avoidance maneuvers. Drivers preview (look ahead) to follow a predefined path.  
 Controller that uses the Stanley^{4} method to minimize the position error and the angle error of the current pose with respect to the reference pose. On the Reference Control pane, use the:

Use the Shift type, ShftType parameter to specify one of these shift options.
Setting  Block Implementation 

 No transmission. Block outputs a constant gear of 1. Use this setting to minimize the number of parameters you need to generate acceleration and braking commands to track forward vehicle motion. This setting does not allow reverse vehicle motion. 
 Block uses a Stateflow^{®} chart to model reverse, neutral, and drive gear shift scheduling. Use this setting to generate acceleration and braking commands to track forward and reverse vehicle motion using simple reverse, neutral, and drive gear shift scheduling. Depending on the vehicle state and vehicle velocity feedback, the block uses the initial gear and time required to shift to shift the vehicle up into drive or down into reverse or neutral. For neutral gears, the block uses braking commands to control the vehicle speed. For reverse gears, the block uses an acceleration command to generate torque and a brake command to reduce vehicle speed. 
 Block uses a Stateflow chart to model reverse, neutral, park, and Nspeed gear shift scheduling. Use this setting to generate acceleration and braking commands to track forward and reverse vehicle motion using reverse, neutral, park, and Nspeed gear shift scheduling. Depending on the vehicle state and vehicle velocity feedback, the block uses these parameters to determine the:
For neutral gears, the block uses braking commands to control the vehicle speed. For reverse gears, the block uses an acceleration command to generate torque and a brake command to reduce vehicle speed. 
 Block uses the input gear, vehicle state, and velocity feedback to generate acceleration and braking commands to track forward and reverse vehicle motion. For neutral gears, the block uses braking commands to control the vehicle speed. For reverse gears, the block uses an acceleration command to generate torque and a brake command to reduce vehicle speed. 
Use the and Longitudinal velocity units, velUnits and Angular units, angUnits parameter to specify the units for the input and output ports.
Use the Output gear signal parameter to create the
GearCmd
output port. The GearCmd
signal contains the integer value of the commanded vehicle gear.
Gear  Integer 

Park 

Reverse 

Neutral 

Drive 

Gear 

Use the Output handwheel angle parameter to specify the units for the steering ports.
Setting  Block Implementation  Port  

 Commanded steer angle, normalized from 1 through 1. The block uses the tire wheel angle saturation limit Tire wheel angle limit, theta parameter to normalize the command. 
 
Overrides the steering command with an input steering command normalized from 1 through 1. 
 
 Commanded steer angle, in units specified by Angular units, angUnits. 
 
Overrides the steering command with an input steering command, in units specified by Angular units, angUnits. 

If you set the control type to PI
or Scheduled
PI
, the block implements proportionalintegral (PI) control with tracking
windup and feedforward gains. For the Scheduled PI
configuration, the block uses feed forward gains that are a function of vehicle
velocity.
To calculate the speed control output, the block uses these equations.
Setting  Equation 


$$y=\frac{{K}_{ff}}{{v}_{nom}}{v}_{ref}+\frac{{K}_{p}{e}_{ref}}{{v}_{nom}}+{\displaystyle \int \left(\frac{{K}_{i}{e}_{ref}}{{v}_{nom}}+{K}_{aw}{e}_{out}\right)dt+{K}_{g}\theta}$$


$$y=\frac{{K}_{ff}(v)}{{v}_{nom}}{v}_{ref}+\frac{{K}_{p}(v){e}_{ref}}{{v}_{nom}}+{\displaystyle \int \left(\frac{{K}_{i}(v){e}_{ref}}{{v}_{nom}}+{K}_{aw}{e}_{out}\right){e}_{ref}dt+{K}_{g}(v)\theta}$$

$$\begin{array}{l}\text{where:}\\ \\ {e}_{ref}={v}_{ref}v\\ {e}_{out}={y}_{sat}y\\ \\ {y}_{sat}=\{\begin{array}{cc}1& y<1\\ y& 1\le y\le 1\\ 1& 1<y\end{array}\end{array}$$
The velocity error lowpass filter uses this transfer function.
$$H(s)=\frac{1}{{\tau}_{err}s+1}\text{for}{\tau}_{err}0$$
To calculate the acceleration and braking commands, the block uses these equations.
$$\begin{array}{l}{y}_{acc}=\{\begin{array}{cc}0& {y}_{sat}<0\\ {y}_{sat}& 0\le {y}_{sat}\le 1\\ 1& 1<{y}_{sat}\end{array}\\ \\ {y}_{dec}=\{\begin{array}{cc}0& {y}_{sat}>0\\ {y}_{sat}& 1\le {y}_{sat}\le 0\\ 1& {y}_{sat}<1\end{array}\end{array}$$
The equations use these variables.
v_{nom}  Nominal vehicle speed 
K_{p}  Proportional gain 
K_{i}  Integral gain 
K_{aw}  Antiwindup gain 
K_{ff}  Velocity feedforward gain 
K_{g}  Grade angle feedforward gain 
θ  Grade angle 
τ_{err}  Error filter time constant 
y  Nominal control output magnitude 
y_{sat}  Saturated control output magnitude 
e_{ref}  Velocity error 
e_{out}  Difference between saturated and nominal control outputs 
y_{acc}  Acceleration signal 
y_{dec}  Braking signal 
v  Velocity feedback signal 
v_{ref}  Reference velocity signal 
If you set the Longitudinal control type, cntrlType or
Lateral control type, cntrlType to
Predictive
, the block implements an optimal
singlepoint preview (look ahead) control model developed by C. C.
MacAdam^{1, 2, 3}. The model represents driver steering
control behavior during pathfollowing and obstacle avoidance maneuvers. Drivers
preview (look ahead) to follow a predefined path. To implement the MacAdam model,
the block:
Represents the dynamics as a linear single track (bicycle) vehicle
Minimizes the previewed error signal at a single point T* seconds ahead in time
Accounts for the driver lag deriving from perceptual and neuromuscular mechanisms
For lateral and yaw motion, the block implements these linear dynamic equations.
$\begin{array}{l}{x}_{1}=U\\ {\dot{x}}_{1}={x}_{2}=\frac{{K}_{pt}}{m}+vrg\text{sin}(\gamma )+{F}_{r}{x}_{1}\\ \dot{y}=v+U\psi \\ \dot{v}=\left[\frac{2\left({C}_{\alpha F}+{C}_{\alpha R}\right)}{mU}\right]v+\left[\frac{2\left(b{C}_{\alpha R}a{C}_{\alpha F}\right)}{mU}U\right]r+\left(\frac{2{C}_{\alpha F}}{m}\right){\delta}_{F}\\ \dot{r}=\left[\frac{2\left(b{C}_{\alpha R}a{C}_{\alpha F}\right)}{IU}\right]v+\left[\frac{2\left({a}^{2}{C}_{\alpha F}+{b}^{2}{C}_{\alpha R}\right)}{IU}\right]r+\left(\frac{2a{C}_{\alpha F}}{I}\right){\delta}_{F}\\ \dot{\psi}=r\end{array}$
In matrix notation:
$\begin{array}{l}\dot{x}=Fx+gu\\ \\ \text{where:}\\ \\ x=\left[\begin{array}{c}\begin{array}{c}{x}_{1}\\ {x}_{2}\\ y\\ v\\ r\end{array}\\ \psi \end{array}\right]\\ \\ F=\left[\begin{array}{cccccc}0& 1& 0& 0& 0& 0\\ \frac{{F}_{r}}{m}& 0& 0& 0& v& 0\\ 0& 0& 0& 1& 0& U\\ 0& 0& 0& \frac{2\left({C}_{\alpha F}+{C}_{\alpha R}\right)}{mU}& \frac{2\left(b{C}_{\alpha R}a{C}_{\alpha F}\right)}{mU}U& 0\\ 0& 0& 0& \frac{2\left(b{C}_{\alpha R}a{C}_{\alpha F}\right)}{IU}& \frac{2\left({a}^{2}{C}_{\alpha F}+{b}^{2}{C}_{\alpha R}\right)}{IU}& 0\\ 0& 0& 0& 0& 1& 0\end{array}\right]\\ \\ g=\left[\begin{array}{c}\begin{array}{cc}0& 0\\ \frac{{K}_{pt}}{m}& 0\\ 0& 0\\ 0& \frac{2{C}_{\alpha F}}{m}\\ 0& \frac{2a{C}_{\alpha F}}{I}\\ 0& 0\end{array}\end{array}\right]\\ \\ u=\left[\begin{array}{c}\begin{array}{c}\overline{u}\\ {\delta}_{F}\end{array}\end{array}\right]\\ \\ \overline{u}=u\frac{{m}^{2}}{{K}_{pt}}g\text{sin}(\gamma )\end{array}$
The singlepoint model assumes a minimum previewed error signal at a single point T* seconds ahead in time. a* is the driver ability to predict the future vehicle response based on the current steering control input. b* is the driver ability to predict the future vehicle response based on the current vehicle state. The block uses these equations.
$\begin{array}{l}{a}^{*}=({T}^{*}){m}^{T}[I+{\displaystyle \sum}_{n=1}^{\infty}\frac{{F}^{n}{({T}^{*})}^{n}}{\left(n+1\right)!}]g\\ {b}^{*}={m}^{T}[I+{\displaystyle \sum}_{n=1}^{\infty}\frac{{F}^{n}{({T}^{*})}^{n}}{n!}]\\ {m}^{T}=[\begin{array}{cccccc}1& 1& 1& 0& 0& 0\end{array}]\end{array}$
The equations use these variables.
a, b  Forward and rearward tire location, respectively 
m  Vehicle mass 
I  Vehicle rotational inertia 
C_{ɑF}  Front tire cornering coefficient 
C_{ɑR}  Rear tire cornering coefficient 
a*, b*  Driver prediction scalar and vector gain, respectively 
x  Predicted vehicle state vector 
v  Lateral velocity 
r  Yaw rate 
Ψ  Front wheel heading angle 
y  Lateral displacement 
F  System matrix 
δ, δ_{F}  Steer angle and front axle steer angle, respectively 
γ  Grade angle 
g  Control coefficient vector 
U  Forward (longitudinal) vehicle velocity 
T*  Preview time window 
ƒ(t+T*)  Previewed path input T* seconds ahead 
u  Tractive force 
m^{T}  Constant observer vector; provides vehicle lateral position 
a_{r}  Static rolling and driveline resistance 
b_{r}  Linear rolling and driveline resistance 
c_{r}  Aerodynamic rolling and driveline resistance 
F_{r}  Rolling resistance 
The singlepoint model implemented by the block finds the steering command that minimizes a local performance index, J, over the current preview interval, (t, t+T).
$$J=\frac{1}{T}{\displaystyle {\int}_{t}^{t+T}{[f(\eta )y(\eta )]}^{2}}d\eta $$
To minimize J with respect to the steering command, this condition must be met.
$$\frac{dJ}{du}=0$$
You can express the optimal control solution in terms of a current nonoptimal and corresponding nonzero preview output error T* seconds ahead^{1, 2, 3}.
$${u}^{o}(t)=u(t)+\frac{e(t+{T}^{*})}{{a}^{*}}$$
The block uses the preview distance and vehicle longitudinal velocity to determine the preview time window.
$${T}^{*}=\frac{L}{U}$$
The equations use these variables.
T*  Preview time window 
ƒ(t+T*)  Previewed path input T* sec ahead 
y(t+T*)  Previewed plant output T* sec ahead 
e(t+T*)  Previewed error signal T* sec ahead 
u(t), u^{o}(t)  Steer angle and optimal steer angle, respectively 
L  Preview distance 
J  Performance index 
U  Forward (longitudinal) vehicle velocity 
The singlepoint model implemented by the block introduces a driver lag. The driver lag accounts for the delay when the driver is tracking tasks. Specifically, it is the transport delay deriving from perceptual and neuromuscular mechanisms. To calculate the driver transport delay, the block implements this equation.
$$H(s)={e}^{s\tau}$$
The equations use these variables.
τ  Driver transport delay 
y(t+T*)  Previewed plant output T* sec ahead 
e(t+T*)  Previewed error signal T* sec ahead 
u(t), u^{o}(t)  Steer angle and optimal steer angle, respectively 
J  Performance index 
If you set Lateral control type, controlTypeLat to
Stanley
, the block implements the Stanley
method^{4}. To compute the steering angle command, the Stanley
controller minimizes the position error and the angle error of the current pose with respect
to the reference pose. The driving direction of the vehicle determines these error
values.
To compute the steering angle command, the controller minimizes the position error and the angle error of the current pose with respect to the reference pose.
The position error is the lateral distance from the vehicle centerofgravity (CG) to the reference point on the path.
The angle error is the angle of the vehicle with respect to reference path.
[1] MacAdam, C. C. "An Optimal Preview Control for Linear Systems". Journal of Dynamic Systems, Measurement, and Control. Vol. 102, Number 3, Sept. 1980.
[2] MacAdam, C. C. "Application of an Optimal Preview Control for Simulation of ClosedLoop Automobile Driving ". IEEE Transactions on Systems, Man, and Cybernetics. Vol. 11, Issue 6, June 1981.
[3] MacAdam, C. C. Development of Driver/Vehicle Steering Interaction Models for Dynamic Analysis. Final Technical Report UMTRI8853. Ann Arbor, Michigan: The University of Michigan Transportation Research Institute, Dec. 1988.
[4] Hoffmann, Gabriel M., Claire J. Tomlin, Michael Montemerlo, and Sebastian Thrun. "Autonomous Automobile Trajectory Tracking for OffRoad Driving: Controller Design, Experimental Validation and Racing." American Control Conference. 2007, pp. 2296–2301. doi:10.1109/ACC.2007.4282788