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 explores the dynamics and control of the cruise control system,
11# following the material presented 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 shown, 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,
43 and road_slope is in rad.
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[0] # vehicle velocity
66 throttle = np.clip(u[0], 0, 1) # vehicle throttle
67 gear = u[1] # vehicle gear
68 theta = u[2] # 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.TransferFunction(
135 [Kp, Ki], [1, 0.01*Ki/Kp], name='control', inputs='u', outputs='y')
136
137# Construct the closed loop control system
138# Inputs: vref, gear, theta
139# Outputs: v (vehicle velocity)
140cruise_tf = ct.InterconnectedSystem(
141 (control_tf, vehicle), name='cruise',
142 connections=[
143 ['control.u', '-vehicle.v'],
144 ['vehicle.u', 'control.y']],
145 inplist=['control.u', 'vehicle.gear', 'vehicle.theta'],
146 inputs=['vref', 'gear', 'theta'],
147 outlist=['vehicle.v', 'vehicle.u'],
148 outputs=['v', 'u'])
149
150# Define the time and input vectors
151T = np.linspace(0, 25, 101)
152vref = 20 * np.ones(T.shape)
153gear = 4 * np.ones(T.shape)
154theta0 = np.zeros(T.shape)
155
156# Now simulate the effect of a hill at t = 5 seconds
157plt.figure()
158plt.suptitle('Response to change in road slope')
159vel_axes = plt.subplot(2, 1, 1)
160inp_axes = plt.subplot(2, 1, 2)
161theta_hill = np.array([
162 0 if t <= 5 else
163 4./180. * pi * (t-5) if t <= 6 else
164 4./180. * pi for t in T])
165
166for m in (1200, 1600, 2000):
167 # Compute the equilibrium state for the system
168 X0, U0 = ct.find_eqpt(
169 cruise_tf, [0, vref[0]], [vref[0], gear[0], theta0[0]],
170 iu=[1, 2], y0=[vref[0], 0], iy=[0], params={'m': m})
171
172 t, y = ct.input_output_response(
173 cruise_tf, T, [vref, gear, theta_hill], X0, params={'m': m})
174
175 # Plot the velocity
176 plt.sca(vel_axes)
177 plt.plot(t, y[0])
178
179 # Plot the input
180 plt.sca(inp_axes)
181 plt.plot(t, y[1])
182
183# Add labels to the plots
184plt.sca(vel_axes)
185plt.ylabel('Speed [m/s]')
186plt.legend(['m = 1000 kg', 'm = 2000 kg', 'm = 3000 kg'], frameon=False)
187
188plt.sca(inp_axes)
189plt.ylabel('Throttle')
190plt.xlabel('Time [s]')
191
192# Figure 4.2: Torque curves for a typical car engine. The graph on the
193# left shows the torque generated by the engine as a function of the
194# angular velocity of the engine, while the curve on the right shows
195# torque as a function of car speed for different gears.
196
197# Figure 4.2
198fig, axes = plt.subplots(1, 2, figsize=(7, 3))
199
200# (a) - single torque curve as function of omega
201ax = axes[0]
202omega = np.linspace(0, 700, 701)
203ax.plot(omega, motor_torque(omega))
204ax.set_xlabel(r'Angular velocity $\omega$ [rad/s]')
205ax.set_ylabel('Torque $T$ [Nm]')
206ax.grid(True, linestyle='dotted')
207
208# (b) - torque curves in different gears, as function of velocity
209ax = axes[1]
210v = np.linspace(0, 70, 71)
211alpha = [40, 25, 16, 12, 10]
212for gear in range(5):
213 omega = alpha[gear] * v
214 T = motor_torque(omega)
215 plt.plot(v, T, color='#1f77b4', linestyle='solid')
216
217# Set up the axes and style
218ax.axis([0, 70, 100, 200])
219ax.grid(True, linestyle='dotted')
220
221# Add labels
222plt.text(11.5, 120, '$n$=1')
223ax.text(24, 120, '$n$=2')
224ax.text(42.5, 120, '$n$=3')
225ax.text(58.5, 120, '$n$=4')
226ax.text(58.5, 185, '$n$=5')
227ax.set_xlabel('Velocity $v$ [m/s]')
228ax.set_ylabel('Torque $T$ [Nm]')
229
230plt.suptitle('Torque curves for typical car engine')
231plt.tight_layout()
232plt.show(block=False)
233
234# Figure 4.3: Car with cruise control encountering a sloping road
235
236# PI controller model: control_pi()
237#
238# We add to this model a feedback controller that attempts to regulate the
239# speed of the car in the presence of disturbances. We shall use a
240# proportional-integral controller
241
242def pi_update(t, x, u, params={}):
243 # Get the controller parameters that we need
244 ki = params.get('ki', 0.1)
245 kaw = params.get('kaw', 2) # anti-windup gain
246
247 # Assign variables for inputs and states (for readability)
248 v = u[0] # current velocity
249 vref = u[1] # reference velocity
250 z = x[0] # integrated error
251
252 # Compute the nominal controller output (needed for anti-windup)
253 u_a = pi_output(t, x, u, params)
254
255 # Compute anti-windup compensation (scale by ki to account for structure)
256 u_aw = kaw/ki * (np.clip(u_a, 0, 1) - u_a) if ki != 0 else 0
257
258 # State is the integrated error, minus anti-windup compensation
259 return (vref - v) + u_aw
260
261def pi_output(t, x, u, params={}):
262 # Get the controller parameters that we need
263 kp = params.get('kp', 0.5)
264 ki = params.get('ki', 0.1)
265
266 # Assign variables for inputs and states (for readability)
267 v = u[0] # current velocity
268 vref = u[1] # reference velocity
269 z = x[0] # integrated error
270
271 # PI controller
272 return kp * (vref - v) + ki * z
273
274control_pi = ct.NonlinearIOSystem(
275 pi_update, pi_output, name='control',
276 inputs=['v', 'vref'], outputs=['u'], states=['z'],
277 params={'kp': 0.5, 'ki': 0.1})
278
279# Create the closed loop system
280cruise_pi = ct.InterconnectedSystem(
281 (vehicle, control_pi), name='cruise',
282 connections=[
283 ['vehicle.u', 'control.u'],
284 ['control.v', 'vehicle.v']],
285 inplist=['control.vref', 'vehicle.gear', 'vehicle.theta'],
286 outlist=['control.u', 'vehicle.v'], outputs=['u', 'v'])
287
288# Figure 4.3b shows the response of the closed loop system. The figure shows
289# that even if the hill is so steep that the throttle changes from 0.17 to
290# almost full throttle, the largest speed error is less than 1 m/s, and the
291# desired velocity is recovered after 20 s.
292
293# Define a function for creating a "standard" cruise control plot
294def cruise_plot(sys, t, y, label=None, t_hill=None, vref=20, antiwindup=False,
295 linetype='b-', subplots=None, legend=None):
296 if subplots is None:
297 subplots = [None, None]
298 # Figure out the plot bounds and indices
299 v_min = vref-1.2; v_max = vref+0.5; v_ind = sys.find_output('v')
300 u_min = 0; u_max = 2 if antiwindup else 1; u_ind = sys.find_output('u')
301
302 # Make sure the upper and lower bounds on v are OK
303 while max(y[v_ind]) > v_max: v_max += 1
304 while min(y[v_ind]) < v_min: v_min -= 1
305
306 # Create arrays for return values
307 subplot_axes = list(subplots)
308
309 # Velocity profile
310 if subplot_axes[0] is None:
311 subplot_axes[0] = plt.subplot(2, 1, 1)
312 else:
313 plt.sca(subplots[0])
314 plt.plot(t, y[v_ind], linetype)
315 plt.plot(t, vref*np.ones(t.shape), 'k-')
316 if t_hill:
317 plt.axvline(t_hill, color='k', linestyle='--', label='t hill')
318 plt.axis([0, t[-1], v_min, v_max])
319 plt.xlabel('Time $t$ [s]')
320 plt.ylabel('Velocity $v$ [m/s]')
321
322 # Commanded input profile
323 if subplot_axes[1] is None:
324 subplot_axes[1] = plt.subplot(2, 1, 2)
325 else:
326 plt.sca(subplots[1])
327 plt.plot(t, y[u_ind], 'r--' if antiwindup else linetype, label=label)
328 # Applied input profile
329 if antiwindup:
330 # TODO: plot the actual signal from the process?
331 plt.plot(t, np.clip(y[u_ind], 0, 1), linetype, label='Applied')
332 if t_hill:
333 plt.axvline(t_hill, color='k', linestyle='--')
334 if legend:
335 plt.legend(frameon=False)
336 plt.axis([0, t[-1], u_min, u_max])
337 plt.xlabel('Time $t$ [s]')
338 plt.ylabel('Throttle $u$')
339
340 return subplot_axes
341
342# Define the time and input vectors
343T = np.linspace(0, 30, 101)
344vref = 20 * np.ones(T.shape)
345gear = 4 * np.ones(T.shape)
346theta0 = np.zeros(T.shape)
347
348# Compute the equilibrium throttle setting for the desired speed (solve for x
349# and u given the gear, slope, and desired output velocity)
350X0, U0, Y0 = ct.find_eqpt(
351 cruise_pi, [vref[0], 0], [vref[0], gear[0], theta0[0]],
352 y0=[0, vref[0]], iu=[1, 2], iy=[1], return_y=True)
353
354# Now simulate the effect of a hill at t = 5 seconds
355plt.figure()
356plt.suptitle('Car with cruise control encountering sloping road')
357theta_hill = [
358 0 if t <= 5 else
359 4./180. * pi * (t-5) if t <= 6 else
360 4./180. * pi for t in T]
361t, y = ct.input_output_response(cruise_pi, T, [vref, gear, theta_hill], X0)
362cruise_plot(cruise_pi, t, y, t_hill=5)
363
364#
365# Example 7.8: State space feedback with integral action
366#
367
368# State space controller model: control_sf_ia()
369#
370# Construct a state space controller with integral action, linearized around
371# an equilibrium point. The controller is constructed around the equilibrium
372# point (x_d, u_d) and includes both feedforward and feedback compensation.
373#
374# Controller inputs: (x, y, r) system states, system output, reference
375# Controller state: z integrated error (y - r)
376# Controller output: u state feedback control
377#
378# Note: to make the structure of the controller more clear, we implement this
379# as a "nonlinear" input/output module, even though the actual input/output
380# system is linear. This also allows the use of parameters to set the
381# operating point and gains for the controller.
382
383def sf_update(t, z, u, params={}):
384 y, r = u[1], u[2]
385 return y - r
386
387def sf_output(t, z, u, params={}):
388 # Get the controller parameters that we need
389 K = params.get('K', 0)
390 ki = params.get('ki', 0)
391 kf = params.get('kf', 0)
392 xd = params.get('xd', 0)
393 yd = params.get('yd', 0)
394 ud = params.get('ud', 0)
395
396 # Get the system state and reference input
397 x, y, r = u[0], u[1], u[2]
398
399 return ud - K * (x - xd) - ki * z + kf * (r - yd)
400
401# Create the input/output system for the controller
402control_sf = ct.NonlinearIOSystem(
403 sf_update, sf_output, name='control',
404 inputs=('x', 'y', 'r'),
405 outputs=('u'),
406 states=('z'))
407
408# Create the closed loop system for the state space controller
409cruise_sf = ct.InterconnectedSystem(
410 (vehicle, control_sf), name='cruise',
411 connections=[
412 ['vehicle.u', 'control.u'],
413 ['control.x', 'vehicle.v'],
414 ['control.y', 'vehicle.v']],
415 inplist=['control.r', 'vehicle.gear', 'vehicle.theta'],
416 outlist=['control.u', 'vehicle.v'], outputs=['u', 'v'])
417
418# Compute the linearization of the dynamics around the equilibrium point
419
420# Y0 represents the steady state with PI control => we can use it to
421# identify the steady state velocity and required throttle setting.
422xd = Y0[1]
423ud = Y0[0]
424yd = Y0[1]
425
426# Compute the linearized system at the eq pt
427cruise_linearized = ct.linearize(vehicle, xd, [ud, gear[0], 0])
428
429# Construct the gain matrices for the system
430A, B, C = cruise_linearized.A, cruise_linearized.B[0, 0], cruise_linearized.C
431K = 0.5
432kf = -1 / (C * np.linalg.inv(A - B * K) * B)
433
434# Response of the system with no integral feedback term
435plt.figure()
436plt.suptitle('Cruise control with proportional and PI control')
437theta_hill = [
438 0 if t <= 8 else
439 4./180. * pi * (t-8) if t <= 9 else
440 4./180. * pi for t in T]
441t, y = ct.input_output_response(
442 cruise_sf, T, [vref, gear, theta_hill], [X0[0], 0],
443 params={'K': K, 'kf': kf, 'ki': 0.0, 'kf': kf, 'xd': xd, 'ud': ud, 'yd': yd})
444subplots = cruise_plot(cruise_sf, t, y, label='Proportional', linetype='b--')
445
446# Response of the system with state feedback + integral action
447t, y = ct.input_output_response(
448 cruise_sf, T, [vref, gear, theta_hill], [X0[0], 0],
449 params={'K': K, 'kf': kf, 'ki': 0.1, 'kf': kf, 'xd': xd, 'ud': ud, 'yd': yd})
450cruise_plot(cruise_sf, t, y, label='PI control', t_hill=8, linetype='b-',
451 subplots=subplots, legend=True)
452
453# Example 11.5: simulate the effect of a (steeper) hill at t = 5 seconds
454#
455# The windup effect occurs when a car encounters a hill that is so steep (6
456# deg) that the throttle saturates when the cruise controller attempts to
457# maintain speed.
458
459plt.figure()
460plt.suptitle('Cruise control with integrator windup')
461T = np.linspace(0, 70, 101)
462vref = 20 * np.ones(T.shape)
463theta_hill = [
464 0 if t <= 5 else
465 6./180. * pi * (t-5) if t <= 6 else
466 6./180. * pi for t in T]
467t, y = ct.input_output_response(
468 cruise_pi, T, [vref, gear, theta_hill], X0,
469 params={'kaw': 0})
470cruise_plot(cruise_pi, t, y, label='Commanded', t_hill=5, antiwindup=True,
471 legend=True)
472
473# Example 11.6: add anti-windup compensation
474#
475# Anti-windup can be applied to the system to improve the response. Because of
476# the feedback from the actuator model, the output of the integrator is
477# quickly reset to a value such that the controller output is at the
478# saturation limit.
479
480plt.figure()
481plt.suptitle('Cruise control with integrator anti-windup protection')
482t, y = ct.input_output_response(
483 cruise_pi, T, [vref, gear, theta_hill], X0,
484 params={'kaw': 2.})
485cruise_plot(cruise_pi, t, y, label='Commanded', t_hill=5, antiwindup=True,
486 legend=True)
487
488# If running as a standalone program, show plots and wait before closing
489import os
490if __name__ == '__main__' and 'PYCONTROL_TEST_EXAMPLES' not in os.environ:
491 plt.show()
492else:
493 plt.show(block=False)
Notes
1. The environment variable PYCONTROL_TEST_EXAMPLES is used for testing to turn off plotting of the outputs.