# Cruise control design example (as a nonlinear I/O system)¶

## Code¶

  1# cruise-control.py - Cruise control example from FBS
2# RMM, 16 May 2019
3#
4# The cruise control system of a car is a common feedback system encountered
5# in everyday life. The system attempts to maintain a constant velocity in the
6# presence of disturbances primarily caused by changes in the slope of a
7# road. The controller compensates for these unknowns by measuring the speed
8# of the car and adjusting the throttle appropriately.
9#
10# This file explore the dynamics and control of the cruise control system,
11# following the material presenting in Feedback Systems by Astrom and Murray.
12# A full nonlinear model of the vehicle dynamics is used, with both PI and
13# state space control laws.  Different methods of constructing control systems
14# are show, all using the InputOutputSystem class (and subclasses).
15
16import numpy as np
17import matplotlib.pyplot as plt
18from math import pi
19import control as ct
20
21#
22# Section 4.1: Cruise control modeling and control
23#
24
25# Vehicle model: vehicle()
26#
27# To develop a mathematical model we start with a force balance for
28# the car body. Let v be the speed of the car, m the total mass
29# (including passengers), F the force generated by the contact of the
30# wheels with the road, and Fd the disturbance force due to gravity,
31# friction, and aerodynamic drag.
32
33def vehicle_update(t, x, u, params={}):
34    """Vehicle dynamics for cruise control system.
35
36    Parameters
37    ----------
38    x : array
39         System state: car velocity in m/s
40    u : array
41         System input: [throttle, gear, road_slope], where throttle is
42         a float between 0 and 1, gear is an integer between 1 and 5,
44
45    Returns
46    -------
47    float
48        Vehicle acceleration
49
50    """
51    from math import copysign, sin
52    sign = lambda x: copysign(1, x)         # define the sign() function
53
54    # Set up the system parameters
55    m = params.get('m', 1600.)
56    g = params.get('g', 9.8)
57    Cr = params.get('Cr', 0.01)
58    Cd = params.get('Cd', 0.32)
59    rho = params.get('rho', 1.3)
60    A = params.get('A', 2.4)
61    alpha = params.get(
62        'alpha', [40, 25, 16, 12, 10])      # gear ratio / wheel radius
63
64    # Define variables for vehicle state and inputs
65    v = x                           # vehicle velocity
66    throttle = np.clip(u, 0, 1)     # vehicle throttle
67    gear = u                        # vehicle gear
68    theta = u                       # road slope
69
70    # Force generated by the engine
71
72    omega = alpha[int(gear)-1] * v      # engine angular speed
73    F = alpha[int(gear)-1] * motor_torque(omega, params) * throttle
74
75    # Disturbance forces
76    #
77    # The disturbance force Fd has three major components: Fg, the forces due
78    # to gravity; Fr, the forces due to rolling friction; and Fa, the
79    # aerodynamic drag.
80
81    # Letting the slope of the road be \theta (theta), gravity gives the
82    # force Fg = m g sin \theta.
83
84    Fg = m * g * sin(theta)
85
86    # A simple model of rolling friction is Fr = m g Cr sgn(v), where Cr is
87    # the coefficient of rolling friction and sgn(v) is the sign of v (+/- 1) or
88    # zero if v = 0.
89
90    Fr  = m * g * Cr * sign(v)
91
92    # The aerodynamic drag is proportional to the square of the speed: Fa =
93    # 1/\rho Cd A |v| v, where \rho is the density of air, Cd is the
94    # shape-dependent aerodynamic drag coefficient, and A is the frontal area
95    # of the car.
96
97    Fa = 1/2 * rho * Cd * A * abs(v) * v
98
99    # Final acceleration on the car
100    Fd = Fg + Fr + Fa
101    dv = (F - Fd) / m
102
103    return dv
104
105# Engine model: motor_torque
106#
107# The force F is generated by the engine, whose torque is proportional to
108# the rate of fuel injection, which is itself proportional to a control
109# signal 0 <= u <= 1 that controls the throttle position. The torque also
110# depends on engine speed omega.
111
112def motor_torque(omega, params={}):
113    # Set up the system parameters
114    Tm = params.get('Tm', 190.)             # engine torque constant
115    omega_m = params.get('omega_m', 420.)   # peak engine angular speed
116    beta = params.get('beta', 0.4)          # peak engine rolloff
117
118    return np.clip(Tm * (1 - beta * (omega/omega_m - 1)**2), 0, None)
119
120# Define the input/output system for the vehicle
121vehicle = ct.NonlinearIOSystem(
122    vehicle_update, None, name='vehicle',
123    inputs = ('u', 'gear', 'theta'), outputs = ('v'), states=('v'))
124
125# Figure 1.11: A feedback system for controlling the speed of a vehicle. In
126# this example, the speed of the vehicle is measured and compared to the
127# desired speed.  The controller is a PI controller represented as a transfer
128# function.  In the textbook, the simulations are done for LTI systems, but
129# here we simulate the full nonlinear system.
130
131# Construct a PI controller with rolloff, as a transfer function
132Kp = 0.5                        # proportional gain
133Ki = 0.1                        # integral gain
134control_tf = ct.tf2io(
135    ct.TransferFunction([Kp, Ki], [1, 0.01*Ki/Kp]),
136    name='control', inputs='u', outputs='y')
137
138# Construct the closed loop control system
139# Inputs: vref, gear, theta
140# Outputs: v (vehicle velocity)
141cruise_tf = ct.InterconnectedSystem(
142    (control_tf, vehicle), name='cruise',
143    connections = (
144        ['control.u', '-vehicle.v'],
145        ['vehicle.u', 'control.y']),
146    inplist = ('control.u', 'vehicle.gear', 'vehicle.theta'),
147    inputs = ('vref', 'gear', 'theta'),
148    outlist = ('vehicle.v', 'vehicle.u'),
149    outputs = ('v', 'u'))
150
151# Define the time and input vectors
152T = np.linspace(0, 25, 101)
153vref = 20 * np.ones(T.shape)
154gear = 4 * np.ones(T.shape)
155theta0 = np.zeros(T.shape)
156
157# Now simulate the effect of a hill at t = 5 seconds
158plt.figure()
159plt.suptitle('Response to change in road slope')
160vel_axes = plt.subplot(2, 1, 1)
161inp_axes = plt.subplot(2, 1, 2)
162theta_hill = np.array([
163    0 if t <= 5 else
164    4./180. * pi * (t-5) if t <= 6 else
165    4./180. * pi for t in T])
166
167for m in (1200, 1600, 2000):
168    # Compute the equilibrium state for the system
169    X0, U0 = ct.find_eqpt(
170        cruise_tf, [0, vref], [vref, gear, theta0],
171        iu=[1, 2], y0=[vref, 0], iy=, params={'m':m})
172
173    t, y = ct.input_output_response(
174        cruise_tf, T, [vref, gear, theta_hill], X0, params={'m':m})
175
176    # Plot the velocity
177    plt.sca(vel_axes)
178    plt.plot(t, y)
179
180    # Plot the input
181    plt.sca(inp_axes)
182    plt.plot(t, y)
183
184# Add labels to the plots
185plt.sca(vel_axes)
186plt.ylabel('Speed [m/s]')
187plt.legend(['m = 1000 kg', 'm = 2000 kg', 'm = 3000 kg'], frameon=False)
188
189plt.sca(inp_axes)
190plt.ylabel('Throttle')
191plt.xlabel('Time [s]')
192
193# Figure 4.2: Torque curves for a typical car engine. The graph on the
194# left shows the torque generated by the engine as a function of the
195# angular velocity of the engine, while the curve on the right shows
196# torque as a function of car speed for different gears.
197
198plt.figure()
199plt.suptitle('Torque curves for typical car engine')
200
201# Figure 4.2a - single torque curve as function of omega
202omega_range = np.linspace(0, 700, 701)
203plt.subplot(2, 2, 1)
204plt.plot(omega_range, [motor_torque(w) for w in omega_range])
205plt.xlabel('Angular velocity $\omega$ [rad/s]')
206plt.ylabel('Torque $T$ [Nm]')
207plt.grid(True, linestyle='dotted')
208
209# Figure 4.2b - torque curves in different gears, as function of velocity
210plt.subplot(2, 2, 2)
211v_range = np.linspace(0, 70, 71)
212alpha = [40, 25, 16, 12, 10]
213for gear in range(5):
214    omega_range = alpha[gear] * v_range
215    plt.plot(v_range, [motor_torque(w) for w in omega_range],
216             color='blue', linestyle='solid')
217
218# Set up the axes and style
219plt.axis([0, 70, 100, 200])
220plt.grid(True, linestyle='dotted')
221
223plt.text(11.5, 120, '$n$=1')
224plt.text(24, 120, '$n$=2')
225plt.text(42.5, 120, '$n$=3')
226plt.text(58.5, 120, '$n$=4')
227plt.text(58.5, 185, '$n$=5')
228plt.xlabel('Velocity $v$ [m/s]')
229plt.ylabel('Torque $T$ [Nm]')
230
231plt.show(block=False)
232
233# Figure 4.3: Car with cruise control encountering a sloping road
234
235# PI controller model: control_pi()
236#
237# We add to this model a feedback controller that attempts to regulate the
238# speed of the car in the presence of disturbances. We shall use a
239# proportional-integral controller
240
241def pi_update(t, x, u, params={}):
242    # Get the controller parameters that we need
243    ki = params.get('ki', 0.1)
244    kaw = params.get('kaw', 2)  # anti-windup gain
245
246    # Assign variables for inputs and states (for readability)
247    v = u                    # current velocity
248    vref = u                 # reference velocity
249    z = x                    # integrated error
250
251    # Compute the nominal controller output (needed for anti-windup)
252    u_a = pi_output(t, x, u, params)
253
254    # Compute anti-windup compensation (scale by ki to account for structure)
255    u_aw = kaw/ki * (np.clip(u_a, 0, 1) - u_a) if ki != 0 else 0
256
257    # State is the integrated error, minus anti-windup compensation
258    return (vref - v) + u_aw
259
260def pi_output(t, x, u, params={}):
261    # Get the controller parameters that we need
262    kp = params.get('kp', 0.5)
263    ki = params.get('ki', 0.1)
264
265    # Assign variables for inputs and states (for readability)
266    v = u                    # current velocity
267    vref = u                 # reference velocity
268    z = x                    # integrated error
269
270    # PI controller
271    return kp * (vref - v) + ki * z
272
273control_pi = ct.NonlinearIOSystem(
274    pi_update, pi_output, name='control',
275    inputs = ['v', 'vref'], outputs = ['u'], states = ['z'],
276    params = {'kp':0.5, 'ki':0.1})
277
278# Create the closed loop system
279cruise_pi = ct.InterconnectedSystem(
280    (vehicle, control_pi), name='cruise',
281    connections=(
282        ['vehicle.u', 'control.u'],
283        ['control.v', 'vehicle.v']),
284    inplist=('control.vref', 'vehicle.gear', 'vehicle.theta'),
285    outlist=('control.u', 'vehicle.v'), outputs=['u', 'v'])
286
287# Figure 4.3b shows the response of the closed loop system.  The figure shows
288# that even if the hill is so steep that the throttle changes from 0.17 to
289# almost full throttle, the largest speed error is less than 1 m/s, and the
290# desired velocity is recovered after 20 s.
291
292# Define a function for creating a "standard" cruise control plot
293def cruise_plot(sys, t, y, t_hill=5, vref=20, antiwindup=False,
294                linetype='b-', subplots=[None, None]):
295    # Figure out the plot bounds and indices
296    v_min = vref-1.2; v_max = vref+0.5; v_ind = sys.find_output('v')
297    u_min = 0; u_max = 2 if antiwindup else 1; u_ind = sys.find_output('u')
298
299    # Make sure the upper and lower bounds on v are OK
300    while max(y[v_ind]) > v_max: v_max += 1
301    while min(y[v_ind]) < v_min: v_min -= 1
302
303    # Create arrays for return values
304    subplot_axes = list(subplots)
305
306    # Velocity profile
307    if subplot_axes is None:
308        subplot_axes = plt.subplot(2, 1, 1)
309    else:
310        plt.sca(subplots)
311    plt.plot(t, y[v_ind], linetype)
312    plt.plot(t, vref*np.ones(t.shape), 'k-')
313    plt.plot([t_hill, t_hill], [v_min, v_max], 'k--')
314    plt.axis([0, t[-1], v_min, v_max])
315    plt.xlabel('Time $t$ [s]')
316    plt.ylabel('Velocity $v$ [m/s]')
317
318    # Commanded input profile
319    if subplot_axes is None:
320        subplot_axes = plt.subplot(2, 1, 2)
321    else:
322        plt.sca(subplots)
323    plt.plot(t, y[u_ind], 'r--' if antiwindup else linetype)
324    plt.plot([t_hill, t_hill], [u_min, u_max], 'k--')
325    plt.axis([0, t[-1], u_min, u_max])
326    plt.xlabel('Time $t$ [s]')
327    plt.ylabel('Throttle $u$')
328
329    # Applied input profile
330    if antiwindup:
331        # TODO: plot the actual signal from the process?
332        plt.plot(t, np.clip(y[u_ind], 0, 1), linetype)
333        plt.legend(['Commanded', 'Applied'], frameon=False)
334
335    return subplot_axes
336
337# Define the time and input vectors
338T = np.linspace(0, 30, 101)
339vref = 20 * np.ones(T.shape)
340gear = 4 * np.ones(T.shape)
341theta0 = np.zeros(T.shape)
342
343# Compute the equilibrium throttle setting for the desired speed (solve for x
344# and u given the gear, slope, and desired output velocity)
345X0, U0, Y0 = ct.find_eqpt(
346    cruise_pi, [vref, 0], [vref, gear, theta0],
347    y0=[0, vref], iu=[1, 2], iy=, return_y=True)
348
349# Now simulate the effect of a hill at t = 5 seconds
350plt.figure()
351plt.suptitle('Car with cruise control encountering sloping road')
352theta_hill = [
353    0 if t <= 5 else
354    4./180. * pi * (t-5) if t <= 6 else
355    4./180. * pi for t in T]
356t, y = ct.input_output_response(cruise_pi, T, [vref, gear, theta_hill], X0)
357cruise_plot(cruise_pi, t, y)
358
359#
360# Example 7.8: State space feedback with integral action
361#
362
363# State space controller model: control_sf_ia()
364#
365# Construct a state space controller with integral action, linearized around
366# an equilibrium point.  The controller is constructed around the equilibrium
367# point (x_d, u_d) and includes both feedforward and feedback compensation.
368#
369# Controller inputs: (x, y, r)    system states, system output, reference
370# Controller state:  z            integrated error (y - r)
371# Controller output: u            state feedback control
372#
373# Note: to make the structure of the controller more clear, we implement this
374# as a "nonlinear" input/output module, even though the actual input/output
375# system is linear.  This also allows the use of parameters to set the
376# operating point and gains for the controller.
377
378def sf_update(t, z, u, params={}):
379    y, r = u, u
380    return y - r
381
382def sf_output(t, z, u, params={}):
383    # Get the controller parameters that we need
384    K = params.get('K', 0)
385    ki = params.get('ki', 0)
386    kf = params.get('kf', 0)
387    xd = params.get('xd', 0)
388    yd = params.get('yd', 0)
389    ud = params.get('ud', 0)
390
391    # Get the system state and reference input
392    x, y, r = u, u, u
393
394    return ud - K * (x - xd) - ki * z + kf * (r - yd)
395
396# Create the input/output system for the controller
397control_sf = ct.NonlinearIOSystem(
398    sf_update, sf_output, name='control',
399    inputs=('x', 'y', 'r'),
400    outputs=('u'),
401    states=('z'))
402
403# Create the closed loop system for the state space controller
404cruise_sf = ct.InterconnectedSystem(
405    (vehicle, control_sf), name='cruise',
406    connections=(
407        ['vehicle.u', 'control.u'],
408        ['control.x', 'vehicle.v'],
409        ['control.y', 'vehicle.v']),
410    inplist=('control.r', 'vehicle.gear', 'vehicle.theta'),
411    outlist=('control.u', 'vehicle.v'), outputs=['u', 'v'])
412
413# Compute the linearization of the dynamics around the equilibrium point
414
415# Y0 represents the steady state with PI control => we can use it to
416# identify the steady state velocity and required throttle setting.
417xd = Y0
418ud = Y0
419yd = Y0
420
421# Compute the linearized system at the eq pt
422cruise_linearized = ct.linearize(vehicle, xd, [ud, gear, 0])
423
424# Construct the gain matrices for the system
425A, B, C = cruise_linearized.A, cruise_linearized.B[0, 0], cruise_linearized.C
426K = 0.5
427kf = -1 / (C * np.linalg.inv(A - B * K) * B)
428
429# Response of the system with no integral feedback term
430plt.figure()
431plt.suptitle('Cruise control with proportional and PI control')
432theta_hill = [
433    0 if t <= 8 else
434    4./180. * pi * (t-8) if t <= 9 else
435    4./180. * pi for t in T]
436t, y = ct.input_output_response(
437    cruise_sf, T, [vref, gear, theta_hill], [X0, 0],
438    params={'K':K, 'kf':kf, 'ki':0.0, 'kf':kf, 'xd':xd, 'ud':ud, 'yd':yd})
439subplots = cruise_plot(cruise_sf, t, y, t_hill=8, linetype='b--')
440
441# Response of the system with state feedback + integral action
442t, y = ct.input_output_response(
443    cruise_sf, T, [vref, gear, theta_hill], [X0, 0],
444    params={'K':K, 'kf':kf, 'ki':0.1, 'kf':kf, 'xd':xd, 'ud':ud, 'yd':yd})
445cruise_plot(cruise_sf, t, y, t_hill=8, linetype='b-', subplots=subplots)
446
448plt.legend(['Proportional', 'PI control'], frameon=False)
449
450# Example 11.5: simulate the effect of a (steeper) hill at t = 5 seconds
451#
452# The windup effect occurs when a car encounters a hill that is so steep (6
453# deg) that the throttle saturates when the cruise controller attempts to
454# maintain speed.
455
456plt.figure()
457plt.suptitle('Cruise control with integrator windup')
458T = np.linspace(0, 70, 101)
459vref = 20 * np.ones(T.shape)
460theta_hill = [
461    0 if t <= 5 else
462    6./180. * pi * (t-5) if t <= 6 else
463    6./180. * pi for t in T]
464t, y = ct.input_output_response(
465    cruise_pi, T, [vref, gear, theta_hill], X0,
466    params={'kaw':0})
467cruise_plot(cruise_pi, t, y, antiwindup=True)
468
469# Example 11.6: add anti-windup compensation
470#
471# Anti-windup can be applied to the system to improve the response. Because of
472# the feedback from the actuator model, the output of the integrator is
473# quickly reset to a value such that the controller output is at the
474# saturation limit.
475
476plt.figure()
477plt.suptitle('Cruise control with integrator anti-windup protection')
478t, y = ct.input_output_response(
479    cruise_pi, T, [vref, gear, theta_hill], X0,
480    params={'kaw':2.})
481cruise_plot(cruise_pi, t, y, antiwindup=True)
482
483# If running as a standalone program, show plots and wait before closing
484import os
485if __name__ == '__main__' and 'PYCONTROL_TEST_EXAMPLES' not in os.environ:
486    plt.show()
487else:
488    plt.show(block=False)