# Cruise control

GitHub URL https://github.com/murrayrm/fbs2e-python/blob/main/cruise.py python-control

This page documents the cruise control system that is used as a running example throughout the text. A detailed description of the dynamics of this system are presented in Chapter 4 - Examples. This page contains a description of the system, including the models and commands used to generate some of the plots in the text.

Note: the Python code on this page makes use of the Python Control Systems Library, an open source software toolbox for control systems analysis.

## Introduction

Cruise control is the term used to describe a control system that regulates the speed of an automobile. Cruise control was commercially introduced in 1958 as an option on the Chrysler Imperial. The basic operation of a cruise controller is to sense the speed of the vehicle, compare this speed to a desired reference, and then accelerate or decelerate the car as required. The figure to the right shows a block diagram of this feedback system.

A simple control algorithm for controlling the speed is to use a "proportional plus integral" feedback. In this algorithm, we choose the amount of gas flowing to the engine based on both the error between the current and desired speed, and the integral of that error. The plot on the right shows the results of this feedback for a step change in the desired speed and a variety of different masses for the car (which might result from having a different number of passengers or towing a trailer). Notice that independent of the mass (which varies by 25% of the total weight of the car), the steady state speed of the vehicle always approaches the desired speed and achieves that speed within approximately 10-15 seconds. Thus the performance of the system is robust with respect to this uncertainty.

## Dynamic model

To develop a mathematical model we start with a force balance for the car body. Let ${\displaystyle v}$ be the speed of the car, ${\displaystyle m}$ the total mass (including passengers), ${\displaystyle F}$ the force generated by the contact of the wheels with the road, and ${\displaystyle F_{\text{d}}}$ the disturbance force due to gravity, friction and aerodynamic drag. The equation of motion of the car is simply

${\displaystyle m{\frac {dv}{dt}}=F-F_{\text{d}}.}$

The force ${\displaystyle F}$ is generated by the engine, whose torque is proportional to the rate of fuel injection, which is itself proportional to a control signal ${\displaystyle 0\leq u\leq 1}$ that controls the throttle position. The torque also depends on engine speed ${\displaystyle \omega }$. A simple representation of the torque at full throttle is given by the torque curve

${\displaystyle T(\omega )=T_{\text{m}}\left(1-\beta {\biggl (}{\frac {\omega }{\omega _{\text{m}}}}-1{\biggr )}^{2}\right),}$

where the maximum torque ${\displaystyle T_{\text{m}}}$ is obtained at engine speed ${\displaystyle \omega _{\text{m}}}$. Typical parameters are ${\displaystyle T_{m}=190}$ Nm, ${\displaystyle \omega _{m}}$ = 420 rad/s (about 4000 RPM) and ${\displaystyle \beta =0.4}$.

Let ${\displaystyle n}$ be the gear ratio and ${\displaystyle r}$ the wheel radius. The engine speed is related to the velocity through the expression

${\displaystyle \omega ={\frac {n}{r}}v=:\alpha _{n}v,}$

and the driving force can be written as

${\displaystyle F={\frac {nu}{r}}T(\omega )=\alpha _{n}uT(\alpha _{n}v).}$

Typical values of ${\displaystyle \alpha _{n}}$ for gears 1 through 5 are ${\displaystyle \alpha _{1}=40}$, ${\displaystyle \alpha _{2}=25}$, ${\displaystyle \alpha _{3}=16}$, ${\displaystyle \alpha _{4}=12}$ and ${\displaystyle \alpha _{5}=10}$. The inverse of ${\displaystyle \alpha _{n}}$ has a physical interpretation as the effective wheel radius. The figure to the right shows the torque as a function of vehicle speed. The figure shows that the effect of the gear is to "flatten" the torque curve so that an almost full torque can be obtained almost over the whole speed range.

The disturbance force ${\displaystyle F_{\text{d}}}$ has three major components: ${\displaystyle F_{\text{g}}}$, the forces due to gravity; ${\displaystyle F_{\text{r}}}$, the forces due to rolling friction; and ${\displaystyle F_{\text{a}}}$, the aerodynamic drag. Letting the slope of the road be ${\displaystyle \theta }$, gravity gives the force ${\displaystyle F_{\text{g}}=mg\sin \theta }$, where ${\displaystyle g=9.8\,{\text{m}}/{\text{s}}^{2}}$ is the gravitational constant. A simple model of rolling friction is

${\displaystyle F_{\text{r}}=mgC_{\text{r}}\,{\text{sgn}}(v),}$

where ${\displaystyle C_{\text{r}}}$ is the coefficient of rolling friction and sgn(${\displaystyle v}$) is the sign of ${\displaystyle v}$ or zero if ${\displaystyle v=0}$. A typical value for the coefficient of rolling friction is ${\displaystyle C_{\text{r}}=0.01}$. Finally, the aerodynamic drag is proportional to the square of the speed:

${\displaystyle F_{\text{a}}={\frac {1}{2}}\rho C_{\text{d}}Av^{2},}$

where ${\displaystyle \rho }$ is the density of air, ${\displaystyle C_{d}}$ is the shape-dependent aerodynamic drag coefficient and ${\displaystyle A}$ is the frontal area of the car. Typical parameters are ${\displaystyle \rho =}$ 1.3 k/m${\displaystyle {}^{3}}$, ${\displaystyle C_{\text{d}}=0.32}$ and ${\displaystyle A=}$ 2.4 m${\displaystyle {}^{2}}$.

### Python code

The model for the system above can be built using the Python Control Toolbox. The code blocks in this section can be used to generate the plots above.

Package initialization
import numpy as np
import matplotlib as mpl
import matplotlib.pyplot as plt
from math import pi
import control as ct

Vehicle model
def vehicle_update(t, x, u, params={}):
"""Vehicle dynamics for cruise control system.

Parameters
----------
x : array
System state: car velocity in m/s
u : array
System input: [throttle, gear, road_slope], where throttle is
a float between 0 and 1, gear is an integer between 1 and 5,

Returns
-------
float
Vehicle acceleration
"""

from math import copysign, sin
sign = lambda x: copysign(1, x)         # define the sign() function

# Set up the system parameters
m = params.get('m', 1600.)              # vehicle mass, kg
g = params.get('g', 9.8)                # gravitational constant, m/s^2
Cr = params.get('Cr', 0.01)             # coefficient of rolling friction
Cd = params.get('Cd', 0.32)             # drag coefficient
rho = params.get('rho', 1.3)            # density of air, kg/m^3
A = params.get('A', 2.4)                # car area, m^2
alpha = params.get(
'alpha', [40, 25, 16, 12, 10])      # gear ratio / wheel radius

# Define variables for vehicle state and inputs
v = x[0]                           # vehicle velocity
throttle = np.clip(u[0], 0, 1)     # vehicle throttle
gear = u[1]                        # vehicle gear
theta = u[2]                       # road slope

# Force generated by the engine
omega = alpha[int(gear)-1] * v      # engine angular speed
F = alpha[int(gear)-1] * motor_torque(omega, params) * throttle

# Disturbance forces
#
# The disturbance force Fd has three major components: Fg, the forces due
# to gravity; Fr, the forces due to rolling friction; and Fa, the
# aerodynamic drag.

# Letting the slope of the road be \theta (theta), gravity gives the
# force Fg = m g sin \theta.
Fg = m * g * sin(theta)

# A simple model of rolling friction is Fr = m g Cr sgn(v), where Cr is
# the coefficient of rolling friction and sgn(v) is the sign of v (±1) or
# zero if v = 0.
Fr  = m * g * Cr * sign(v)

# The aerodynamic drag is proportional to the square of the speed: Fa =
# 1/2 \rho Cd A |v| v, where \rho is the density of air, Cd is the
# shape-dependent aerodynamic drag coefficient, and A is the frontal area
# of the car.
Fa = 1/2 * rho * Cd * A * abs(v) * v

# Final acceleration on the car
Fd = Fg + Fr + Fa
dv = (F - Fd) / m

return dv

Engine model
def motor_torque(omega, params={}):
# Set up the system parameters
Tm = params.get('Tm', 190.)             # engine torque constant
omega_m = params.get('omega_m', 420.)   # peak engine angular speed
beta = params.get('beta', 0.4)          # peak engine rolloff

return np.clip(Tm * (1 - beta * (omega/omega_m - 1)**2), 0, None)

Input/output model for the vehicle system
vehicle = ct.NonlinearIOSystem(
vehicle_update, None, name='vehicle',
inputs = ('u', 'gear', 'theta'), outputs = ('v'), states=('v'))

Input/output torque curves (plot)
# Figure 4.2a - single torque curve as function of omega
omega_range = np.linspace(0, 700, 701)
plt.subplot(2, 2, 1)
plt.plot(omega_range, [motor_torque(w) for w in omega_range])
plt.xlabel('Angular velocity $\omega$ [rad/s]')
plt.ylabel('Torque $T$ [Nm]')
plt.grid(True, linestyle='dotted')

# Figure 4.2b - torque curves in different gears, as function of velocity
plt.subplot(2, 2, 2)
v_range = np.linspace(0, 70, 71)
alpha = [40, 25, 16, 12, 10]
for gear in range(5):
omega_range = alpha[gear] * v_range
plt.plot(v_range, [motor_torque(w) for w in omega_range],
color='blue', linestyle='solid')

# Set up the axes and style
plt.axis([0, 70, 100, 200])
plt.grid(True, linestyle='dotted')

plt.text(11.5, 120, '$n$=1')
plt.text(24, 120, '$n$=2')
plt.text(42.5, 120, '$n$=3')
plt.text(58.5, 120, '$n$=4')
plt.text(58.5, 185, '$n$=5')
plt.xlabel('Velocity $v$ [m/s]')
plt.ylabel('Torque $T$ [Nm]')

plt.suptitle('Torque curves for typical car engine');
plt.tight_layout()
plt.savefig("cruise-gearcurves.png", bbox_inches='tight')

PI controller
# Construct a PI controller with rolloff, as a transfer function
Kp = 0.5                        # proportional gain
Ki = 0.1                        # integral gain
control_pi = ct.tf2io(
ct.TransferFunction([Kp, Ki], [1, 0.01*Ki/Kp]),
name='control', inputs='u', outputs='y')

cruise_pi = ct.InterconnectedSystem(
(vehicle, control_pi), name='cruise',
connections = [('control.u', '-vehicle.v'), ('vehicle.u', 'control.y')],
inplist = ('control.u', 'vehicle.gear', 'vehicle.theta'), inputs = ('vref', 'gear', 'theta'),
outlist = ('vehicle.v', 'vehicle.u'), outputs = ('v', 'u'))

Plotting function
# Define a generator for creating a "standard" cruise control plot
def cruise_plot(sys, t, y, t_hill=5, vref=20, antiwindup=False, linetype='b-',
subplots=[None, None]):
# Figure out the plot bounds and indices
v_min = vref-1.2; v_max = vref+0.5; v_ind = sys.find_output('v')
u_min = 0; u_max = 2 if antiwindup else 1; u_ind = sys.find_output('u')

# Make sure the upper and lower bounds on v are OK
while max(y[v_ind]) > v_max: v_max += 1
while min(y[v_ind]) < v_min: v_min -= 1

# Create arrays for return values
subplot_axes = subplots.copy()

# Velocity profile
if subplot_axes[0] is None:
subplot_axes[0] = plt.subplot(2, 1, 1)
else:
plt.sca(subplots[0])
plt.plot(t, y[v_ind], linetype)
plt.plot(t, vref*np.ones(t.shape), 'k-')
plt.plot([t_hill, t_hill], [v_min, v_max], 'k--')
plt.axis([0, t[-1], v_min, v_max])
plt.xlabel('Time $t$ [s]')
plt.ylabel('Velocity $v$ [m/s]')

# Commanded input profile
if subplot_axes[1] is None:
subplot_axes[1] = plt.subplot(2, 1, 2)
else:
plt.sca(subplots[1])
plt.plot(t, y[u_ind], 'r--' if antiwindup else linetype)
plt.plot([t_hill, t_hill], [u_min, u_max], 'k--')
plt.axis([0, t[-1], u_min, u_max])
plt.xlabel('Time $t$ [s]')
plt.ylabel('Throttle $u$')

# Applied input profile
if antiwindup:
plt.plot(t, np.clip(y[u_ind], 0, 1), linetype)
plt.legend(['Commanded', 'Applied'], frameon=False)

return subplot_axes

Trajectory description
# Define the time and input vectors
T = np.linspace(0, 25, 101)
vref = 20 * np.ones(T.shape)
gear = 4 * np.ones(T.shape)
theta0 = np.zeros(T.shape)

# Effect of a hill at t = 5 seconds
theta_hill = np.array([
0 if t <= 5 else
4./180. * pi * (t-5) if t <= 6 else
4./180. * pi for t in T])

Simulated responses (plot)
# Simulate and plot
plt.figure()
plt.suptitle('Response to change in road slope')

subplots = [None, None]
linecolor = ['red', 'blue', 'green']
handles = []
for i, m in enumerate([1200, 1600, 2000]):
# Compute the equilibrium state for the system
X0, U0 = ct.find_eqpt(
cruise_pi, [vref[0], 0], [vref[0], gear[0], theta0[0]],
iu=[1, 2], y0=[vref[0], 0], iy=[0], params={'m':m})

t, y = ct.input_output_response(
cruise_pi, T, [vref, gear, theta_hill], X0, params={'m':m})

subplots = cruise_plot(cruise_pi, t, y, t_hill=5, subplots=subplots,
linetype=linecolor[i][0] + '-')
handles.append(mpl.lines.Line2D([], [], color=linecolor[i],
linestyle='-', label="m = %d" % m))

# Add labels to the plots
plt.sca(subplots[0])
plt.ylabel('Speed [m/s]')
plt.legend(handles=handles, frameon=False, loc='lower right');

plt.sca(subplots[1])
plt.ylabel('Throttle')
plt.xlabel('Time [s]');
plt.savefig("cruise-speedresp.png", bbox_inches='tight')


### Linearized Dynamics

To explore the behavior of the cruise control system near the equilibrium point we will linearize the system. A Taylor series expansion of the dynamics around the equilibrium point gives

${\displaystyle {\frac {d(v-v_{\text{e}})}{dt}}=-a(v-v_{\text{e}})-b_{\text{g}}(\theta -\theta _{\text{e}})+b(u-u_{\text{e}})+{\text{higher-order terms,}}}$

where

${\displaystyle a={\frac {\rho C_{\text{d}}A|v_{\text{e}}|-u_{\text{e}}\alpha _{n}^{2}T'(\alpha _{n}v_{\text{e}})}{m}},\qquad b_{\text{g}}=g\cos {\theta _{\text{e}}},\qquad b={\frac {\alpha _{n}T(\alpha _{n}v_{\text{e}})}{m}}.}$

Notice that the term corresponding to rolling friction disappears if ${\displaystyle v>0}$. For a car in fourth gear with ${\displaystyle v_{\text{e}}=20}$ m/s, ${\displaystyle \theta _{\text{e}}=0}$, and the numerical values from above, the equilibrium value for the throttle is ${\displaystyle u_{\text{e}}=0.1687}$ and the parameters are ${\displaystyle a=0.01}$, ${\displaystyle b=1.32}$, and ${\displaystyle b_{\text{g}}=9.8}$. This linear model describes how small perturbations in the velocity about the nominal speed evolve in time.

We apply the PI controller above to the case where the car is running with constant speed on a horizontal road and the system has stabilized so that the vehicle speed and the controller output are constant. The figure to the right shows what happens when the car encounters a hill with a slope of 4 degrees and a hill with a slope of 6 degrees at time ${\displaystyle t=\,}$ 5 seconds. The results for the nonlinear model are dashed curves and those for the linear model are solid curves. The differences between the curves are very small (especially for ${\displaystyle \theta =\,}$ 4 degrees), and control design based on the linearized model is thus validated.

### Python code

Linearized dynamics
# Find the equilibrium point for the system
Xeq, Ueq = ct.find_eqpt(
vehicle, [vref[0]], [0, gear[0], theta0[0]], y0=[vref[0]], iu=[1, 2])
print("Xeq = ", Xeq, "\nUeq = ", Ueq)

# Compute the linearized system at the eq pt
vehlin = ct.linearize(vehicle, Xeq, [Ueq[0], gear[0], 0], name='vehlin', copy=True)

PI controller with anti-windup compensation
def pi_update(t, x, u, params={}):
# Get the controller parameters that we need
ki = params.get('ki', 0.1)
kaw = params.get('kaw', 2)  # anti-windup gain

# Assign variables for inputs and states (for readability)
v = u[0]                    # current velocity
vref = u[1]                 # reference velocity
z = x[0]                    # integrated error

# Compute the nominal controller output (needed for anti-windup)
u_a = pi_output(t, x, u, params)

# Compute anti-windup compensation (scale by ki to account for structure)
u_aw = kaw/ki * (np.clip(u_a, 0, 1) - u_a) if ki != 0 else 0

# State is the integrated error, minus anti-windup compensation
return (vref - v) + u_aw

def pi_output(t, x, u, params={}):
# Get the controller parameters that we need
kp = params.get('kp', 0.5)
ki = params.get('ki', 0.1)

# Assign variables for inputs and states (for readability)
v = u[0]                    # current velocity
vref = u[1]                 # reference velocity
z = x[0]                    # integrated error

# PI controller
return kp * (vref - v) + ki * z

control_pi_aw = ct.NonlinearIOSystem(
pi_update, pi_output, name='control',
inputs = ['v', 'vref'], outputs = ['u'], states = ['z'],
params = {'kp':0.5, 'ki':0.1})

# Create a closed loop controller for the linear system
cruise_lin = ct.InterconnectedSystem(
(vehlin, control_pi_aw), name='cruise_lin',
connections=(
('vehlin.u', 'control.u'),
('control.v', 'vehlin.v')),
inplist=('control.vref', 'vehlin.gear', 'vehlin.theta'),
outlist=('control.u', 'vehlin.v'), outputs=['u', 'v'])

# Create a closed loop controller for the nonlinear system
cruise_nonlin = ct.InterconnectedSystem(
(vehicle, control_pi_aw), name='cruise_nonlin',
connections=(
('vehicle.u', 'control.u'),
('control.v', 'vehicle.v')),
inplist=('control.vref', 'vehicle.gear', 'vehicle.theta'),
outlist=('control.u', 'vehicle.v'), outputs=['u', 'v'])

Simulations
# Linear response
X0_lin, U0 = ct.find_eqpt(
cruise_lin, [vref[0], 0], [vref[0], gear[0], theta0[0]],
y0=[0, vref[0]], iu=[1, 2], iy=[1])

t, y = ct.input_output_response(cruise_lin, T, [vref, gear, theta_hill], X0_lin)
subplots = cruise_plot(cruise_lin, t, y, t_hill=5, linetype='r-')

# Nonlinear response
X0_nonlin, U0 = ct.find_eqpt(
cruise_nonlin, [vref[0], 0], [vref[0], gear[0], theta0[0]],
y0=[0, vref[0]], iu=[1, 2], iy=[1])

t, y = ct.input_output_response(cruise_nonlin, T, [vref, gear, theta_hill], X0_nonlin)
subplots = cruise_plot(cruise_nonlin, t, y, t_hill=5, subplots=subplots, linetype='b--')

# Add a legend to identify linear vs nonlinear
plt.legend(['linear', 'nonlinear'], frameon=False, loc='lower right')

# Add two more simulations for 6 degree hill instead of 4 degree
t, y = ct.input_output_response(cruise_lin, T, [vref, gear, theta_hill*1.5], X0_lin)
subplots = cruise_plot(cruise_lin, t, y, t_hill=5, subplots=subplots, linetype='r-')

t, y = ct.input_output_response(cruise_nonlin, T, [vref, gear, theta_hill*1.5], X0_nonlin)
subplots = cruise_plot(cruise_nonlin, t, y, t_hill=5, subplots=subplots, linetype='b--')

# Add titles and legends and save the figure
plt.suptitle('Linear versus nonlinear model response')
plt.sca(subplots[1]); plt.axis([0, 25, 0, 1.2])

plt.savefig("cruise-linear_vs_nonlinear.png", bbox_inches='tight')


## State Space Control

The linearized dynamics of the process around an equilibrium point ${\displaystyle v_{\text{e}}}$, ${\displaystyle u_{\text{e}}}$ are given by

{\displaystyle {\begin{aligned}{\frac {dx}{dt}}&=-ax-b_{\text{g}}\theta +bw,\\y&=v=x+v_{\text{e}},\end{aligned}}}

where ${\displaystyle x=v-v_{\text{e}}}$, ${\displaystyle =u-u_{\text{e}}}$, ${\displaystyle m}$ is the mass of the car, and ${\displaystyle \theta }$ is the angle of the road. The constants ${\displaystyle a}$, ${\displaystyle b}$, and ${\displaystyle b_{\text{g}}}$ depend on the properties of the car and are given above.

If we augment the system with an integrator, the system dynamics become

{\displaystyle {\begin{aligned}{\frac {dx}{dt}}&=-ax-b_{\text{g}}\theta +bw,\\{\frac {dz}{dt}}&=y-v_{\text{r}}=v_{\text{e}}+x-v_{\text{r}},\end{aligned}}}

or, in state space form,

${\displaystyle {\frac {d}{dt}}{\begin{bmatrix}x\\z\end{bmatrix}}={\begin{bmatrix}-a&0\\1&0\end{bmatrix}}{\begin{bmatrix}x\\z\end{bmatrix}}+{\begin{bmatrix}b\\0\end{bmatrix}}w+{\begin{bmatrix}-b_{\text{g}}\\0\end{bmatrix}}\theta +{\begin{bmatrix}0\\v_{\text{e}}-v_{\text{r}}\end{bmatrix}}.}$

Note that when the system is at equilibrium, we have that ${\displaystyle {\dot {z}}=0}$, which implies that the vehicle speed ${\displaystyle v=v_{\text{e}}+x}$ should be equal to the desired reference speed ${\displaystyle v_{\text{r}}}$. Our controller will be of the form

{\displaystyle {\begin{aligned}{\frac {dz}{dt}}&=y-v_{\text{r}},\\w&=-k_{\text{p}}x-k_{\text{i}}z+k_{\text{f}}v_{\text{r}},\end{aligned}}}

and the gains ${\displaystyle k_{\text{p}}}$, ${\displaystyle k_{\text{i}}}$, and ${\displaystyle k_{\text{f}}}$ will be chosen to stabilize the system and provide the correct input for the reference speed.

Assume that we wish to design the closed loop system to have the characteristic polynomial

${\displaystyle \lambda (s)=s^{2}+a_{1}s+a_{2}.}$

Setting the disturbance ${\displaystyle \theta =0}$, the characteristic polynomial of the closed loop system is given by

${\displaystyle \det {\bigl (}sI-(A-BK){\bigr )}=s^{2}+(bk_{\text{p}}+a)s+bk_{\text{i}},}$

and hence we set

${\displaystyle k_{\text{p}}={\frac {a_{1}-a}{b}},\qquad k_{\text{i}}={\frac {a_{2}}{b}},\qquad k_{\text{f}}={-1}/{\bigl (}C(A-BK)^{-1}B{\bigr )}={\frac {a_{1}}{b}}.}$

The resulting controller stabilizes the system and hence brings ${\displaystyle {\dot {z}}=y-v_{\text{r}}}$ to zero, resulting in perfect tracking. Notice that even if we have a small error in the values of the parameters defining the system, as long as the closed loop eigenvalues are still stable, then the tracking error will approach zero. Thus the exact calibration required in our previous approach (using ${\displaystyle k_{\text{f}}}$) is not needed here. Indeed, we can even choose ${\displaystyle k_{\text{f}}=0}$ and let the feedback controller do all of the work. However, ${\displaystyle k_{\text{f}}}$ does influence the transient response to reference signals and setting it properly will generally give a more favorable response.

Integral feedback can also be used to compensate for constant disturbances. The figure to the right shows the results of a simulation in which the car encounters a hill with angle ${\displaystyle \theta =4}$ degrees at ${\displaystyle t=}$ 5 seconds. The steady-state values of the throttle for a state feedback controller and a controller with integral action are very close, but the corresponding values of the car velocity are quite different. The reason for this is that the zero frequency gain from throttle to velocity is ${\displaystyle -b/a=130}$ is high. The stability of the system is not affected by this external disturbance, and so we once again see that the car's velocity converges to the reference speed. This ability to handle constant disturbances is a general property of controllers with integral feedback.

### Python code

State feedback controller
def sf_update(t, z, u, params={}):
y, r = u[1], u[2]
return y - r

def sf_output(t, z, u, params={}):
# Get the controller parameters that we need
K = params.get('K', 0)
ki = params.get('ki', 0)
kf = params.get('kf', 0)
xd = params.get('xd', 0)
yd = params.get('yd', 0)
ud = params.get('ud', 0)

# Get the system state and reference input
x, y, r = u[0], u[1], u[2]

return ud - K * (x - xd) - ki * z + kf * (r - yd)

# Create the input/output system for the controller
control_sf = ct.NonlinearIOSystem(
sf_update, sf_output, name='control',
inputs=('x', 'y', 'r'),
outputs=('u'),
states=('z'))

# Create the closed loop system for the state space controller
cruise_sf = ct.InterconnectedSystem(
(vehicle, control_sf), name='cruise',
connections=(
('vehicle.u', 'control.u'),
('control.x', 'vehicle.v'),
('control.y', 'vehicle.v')),
inplist=('control.r', 'vehicle.gear', 'vehicle.theta'),
outlist=('control.u', 'vehicle.v'), outputs=['u', 'v'])

# Construct the gain matrices for the system
A, B, C = vehlin.A, vehlin.B[0, 0], vehlin.C
K = 0.5
kf = -1 / (C * np.linalg.inv(A - B * K) * B)

# Compute the steady state velocity and throttle setting
xd = Xeq[0]
ud = Ueq[0]
yd = vref[-1]

Simulation (plot)
# Response of the system with no integral feedback term
t, y_sfb = ct.input_output_response(
cruise_sf, T, [vref, gear, theta_hill], [Xeq[0], 0],
params={'K':K, 'ki':0.0, 'kf':kf, 'xd':xd, 'ud':ud, 'yd':yd})
subplots = cruise_plot(cruise_sf, t, y_sfb, t_hill=5, linetype='b--')

# Response of the system with state feedback + integral action
t, y_sfb_int = ct.input_output_response(
cruise_sf, T, [vref, gear, theta_hill], [Xeq[0], 0],
params={'K':K, 'ki':0.1, 'kf':kf, 'xd':xd, 'ud':ud, 'yd':yd})
cruise_plot(cruise_sf, t, y_sfb_int, t_hill=5, linetype='b-', subplots=subplots)

plt.suptitle('Cruise control with state feedback, integral action')
p_line = mpl.lines.Line2D([], [], color='blue', linestyle='--', label='State feedback')
pi_line = mpl.lines.Line2D([], [], color='blue', linestyle='-', label='w/ integral action')
plt.legend(handles=[p_line, pi_line], frameon=False, loc='lower right')
plt.savefig("cruise-statefbk.png", bbox_inches='tight')


## PID Control

The dynamics of the system are given by a first-order system with the transfer function

${\displaystyle P(s)={\frac {b}{s+a}}.}$

With a PI controller the closed loop system has the characteristic polynomial

${\displaystyle s(s+a)+bk_{\text{p}}s+bk_{\text{i}}=s^{2}+(a+bk_{\text{p}})s+bk_{\text{i}}.}$

The closed loop poles can thus be assigned arbitrary values by proper choice of the controller gains ${\displaystyle k_{\text{p}}}$ and ${\displaystyle k_{\text{i}}}$. Requiring that the closed loop system have the characteristic polynomial

${\displaystyle p(s)=s^{2}+a_{1}s+a_{2},}$

we find that the controller parameters are

${\displaystyle k_{\text{p}}={\frac {a_{1}-a}{b}},\qquad k_{\text{i}}={\frac {a_{2}}{b}}.}$

If we require a response of the closed loop system that is slower than that of the open loop system, a reasonable choice is ${\displaystyle a_{1}=a+\alpha }$ and ${\displaystyle a_{2}=\alpha a}$, where ${\displaystyle \alpha determines the closed loop response. If a response faster than that of the open loop system is required, a possible choice is ${\displaystyle a_{1}=2\zeta _{\text{c}}\omega _{\text{c}}}$ and ${\displaystyle a_{2}=\omega _{\text{c}}^{2}}$, where ${\displaystyle \omega _{\text{c}}}$ and ${\displaystyle \zeta _{\text{c}}}$ are the undamped natural frequency and damping ratio of the dominant mode.

To design a PI controller we choose ${\displaystyle \zeta _{\text{c}}=1}$ to obtain a response without overshoot. The choice of ${\displaystyle \omega _{\text{c}}}$ is a compromise between response speed and control actions: a large value gives a fast response, but it requires fast control action. The largest velocity error decreases with increasing ${\displaystyle \omega _{\text{c}}}$, but the control signal also changes more rapidly. In the simple model it was assumed that the force responds instantaneously to throttle commands. For rapid changes there may be additional dynamics that have to be accounted for. There are also physical limits to the rate of change of the force, which also restricts the admissible value of ${\displaystyle \omega _{\text{c}}}$. A reasonable choice of ${\displaystyle \omega _{\text{c}}}$ is in the range 0.5--1.0.

### Python code

Get transfer function parameters
# Get the transfer function from throttle input + hill to vehicle speed
P = ct.ss2tf(vehlin[0, 0])

# Construction a controller that cancels the pole
a = -P.pole()[0]
b = np.real(P(0)) * a

Fix ${\displaystyle \omega _{\text{c}}}$ and vary ${\displaystyle \zeta _{\text{c}}(plot)}$
# Fix \omega_0 and vary \zeta
w0 = 0.5
subplots = [None, None]
for zeta in [0.5, 1, 2]:
# Create the controller transfer function (as an I/O system)
kp = (2*zeta*w0 - a)/b
ki = w0**2 / b
control_tf = ct.tf2io(
ct.TransferFunction([kp, ki], [1, 0.01*ki/kp]),
name='control', inputs='u', outputs='y')

# Construct the closed loop system by interconnecting process and controller
cruise_tf = ct.InterconnectedSystem(
(vehicle, control_tf), name='cruise',
connections = [('control.u', '-vehicle.v'), ('vehicle.u', 'control.y')],
inplist = ('control.u', 'vehicle.gear', 'vehicle.theta'),
inputs = ('vref', 'gear', 'theta'),
outlist = ('vehicle.v', 'vehicle.u'), outputs = ('v', 'u'))

# Plot the velocity response
X0, U0 = ct.find_eqpt(
cruise_tf, [vref[0], 0], [vref[0], gear[0], theta_hill[0]],
iu=[1, 2], y0=[vref[0], 0], iy=[0])

t, y = ct.input_output_response(cruise_tf, T, [vref, gear, theta_hill], X0)
subplots = cruise_plot(cruise_tf, t, y, t_hill=5, subplots=subplots)

plt.suptitle('PID controller with varying zeta')
plt.savefig("cruise-pid-sweep-zeta.png", bbox_inches='tight')

Fix vary ${\displaystyle \zeta _{\text{c}}}$ and ${\displaystyle \omega _{\text{c}}(plot)}$
zeta = 1
subplots = [None, None]
for w0 in [0.2, 0.5, 1]:
# Create the controller transfer function (as an I/O system)
kp = (2*zeta*w0 - a)/b
ki = w0**2 / b
control_tf = ct.tf2io(
ct.TransferFunction([kp, ki], [1, 0.01*ki/kp]),
name='control', inputs='u', outputs='y')

# Construct the closed loop system by interconnecting process and controller
cruise_tf = ct.InterconnectedSystem(
(vehicle, control_tf), name='cruise',
connections = [('control.u', '-vehicle.v'), ('vehicle.u', 'control.y')],
inplist = ('control.u', 'vehicle.gear', 'vehicle.theta'),
inputs = ('vref', 'gear', 'theta'),
outlist = ('vehicle.v', 'vehicle.u'), outputs = ('v', 'u'))

# Plot the velocity response
X0, U0 = ct.find_eqpt(
cruise_tf, [vref[0], 0], [vref[0], gear[0], theta_hill[0]],
iu=[1, 2], y0=[vref[0], 0], iy=[0])

t, y = ct.input_output_response(cruise_tf, T, [vref, gear, theta_hill], X0)
subplots = cruise_plot(cruise_tf, t, y, t_hill=5, subplots=subplots)

plt.suptitle('PID controller with varying omega')
plt.savefig("cruise-pid-sweep-omega.png", bbox_inches='tight')


### Anti-windup compensation

The windup effect is illustrated in the left figure below, which shows what happens when a car encounters a hill that is so steep (6 degrees) that the throttle saturates when the cruise controller attempts to maintain speed. When encountering the slope at time ${\displaystyle t=5}$, the velocity decreases and the throttle increases to generate more torque. However, the torque required is so large that the throttle saturates. The error decreases slowly because the torque generated by the engine is just a little larger than the torque required to compensate for gravity. The error is large and the integral continues to build up until the error reaches zero at time 25, but the controller output is still larger than the saturation limit and the actuator remains saturated. The integral term starts to decrease and the velocity settles to the desired value at time ${\displaystyle t=40}$. Also notice the large overshoot.

The right figure below shows what happens when a controller with anti-windup is applied to the system. Because of the feedback from the actuator model, the output of the integrator is quickly reset to a value such that the controller output is at the saturation limit. The behavior is drastically different from that on the left and the large overshoot is avoided. The tracking gain used in the simulation is ${\displaystyle k_{\text{aw}}=2}$ which is an order of magnitude larger than the integral gain ${\displaystyle k_{\text{i}}=0.2}$.

### Python code

Redefine input with longer duration
T = np.linspace(0, 50, 101)
vref = 20 * np.ones(T.shape)
theta_hill = [
0 if t <= 5 else
6./180. * pi * (t-5) if t <= 6 else
6./180. * pi for t in T]

# Compute the equilibrium throttle setting for the desired speed
X0, U0 = ct.find_eqpt(
cruise_nonlin, [vref[0], 0], [vref[0], gear[0], theta0[0]],
y0=[0, vref[0]], iu=[1, 2], iy=[1])

Effects of windup (plot)
t, y = ct.input_output_response(
cruise_nonlin, T, [vref, gear, theta_hill], X0,
params={'kaw':0})
cruise_plot(cruise_nonlin, t, y, antiwindup=True);

plt.suptitle('Cruise control with integrator windup')
plt.savefig("cruise-windup.png", bbox_inches='tight')

Anti-windup compensation (plot)
t, y = ct.input_output_response(
cruise_nonlin, T, [vref, gear, theta_hill], X0,
params={'kaw':2})
cruise_plot(cruise_nonlin, t, y, antiwindup=True);

plt.suptitle('Cruise control with anti-windup')
plt.savefig("cruise-antiwindup.png", bbox_inches='tight')