1# steering-gainsched.py - gain scheduled control for vehicle steering
2# RMM, 8 May 2019
3#
4# This file works through Example 1.1 in the "Optimization-Based Control"
5# course notes by Richard Murray (avaliable at http://fbsbook.org, in the
6# optimization-based control supplement). It is intended to demonstrate the
7# functionality for nonlinear input/output systems in the python-control
8# package.
9
10import numpy as np
11import control as ct
12from cmath import sqrt
13import matplotlib.pyplot as plt
14
15#
16# Vehicle steering dynamics
17#
18# The vehicle dynamics are given by a simple bicycle model. We take the state
19# of the system as (x, y, theta) where (x, y) is the position of the vehicle
20# in the plane and theta is the angle of the vehicle with respect to
21# horizontal. The vehicle input is given by (v, phi) where v is the forward
22# velocity of the vehicle and phi is the angle of the steering wheel. The
23# model includes saturation of the vehicle steering angle.
24#
25# System state: x, y, theta
26# System input: v, phi
27# System output: x, y
28# System parameters: wheelbase, maxsteer
29#
30def vehicle_update(t, x, u, params):
31 # Get the parameters for the model
32 l = params.get('wheelbase', 3.) # vehicle wheelbase
33 phimax = params.get('maxsteer', 0.5) # max steering angle (rad)
34
35 # Saturate the steering input
36 phi = np.clip(u[1], -phimax, phimax)
37
38 # Return the derivative of the state
39 return np.array([
40 np.cos(x[2]) * u[0], # xdot = cos(theta) v
41 np.sin(x[2]) * u[0], # ydot = sin(theta) v
42 (u[0] / l) * np.tan(phi) # thdot = v/l tan(phi)
43 ])
44
45def vehicle_output(t, x, u, params):
46 return x # return x, y, theta (full state)
47
48# Define the vehicle steering dynamics as an input/output system
49vehicle = ct.NonlinearIOSystem(
50 vehicle_update, vehicle_output, states=3, name='vehicle',
51 inputs=('v', 'phi'),
52 outputs=('x', 'y', 'theta'))
53
54#
55# Gain scheduled controller
56#
57# For this system we use a simple schedule on the forward vehicle velocity and
58# place the poles of the system at fixed values. The controller takes the
59# current vehicle position and orientation plus the velocity velocity as
60# inputs, and returns the velocity and steering commands.
61#
62# System state: none
63# System input: ex, ey, etheta, vd, phid
64# System output: v, phi
65# System parameters: longpole, latpole1, latpole2
66#
67def control_output(t, x, u, params):
68 # Get the controller parameters
69 longpole = params.get('longpole', -2.)
70 latpole1 = params.get('latpole1', -1/2 + sqrt(-7)/2)
71 latpole2 = params.get('latpole2', -1/2 - sqrt(-7)/2)
72 l = params.get('wheelbase', 3)
73
74 # Extract the system inputs
75 ex, ey, etheta, vd, phid = u
76
77 # Determine the controller gains
78 alpha1 = -np.real(latpole1 + latpole2)
79 alpha2 = np.real(latpole1 * latpole2)
80
81 # Compute and return the control law
82 v = -longpole * ex # Note: no feedfwd (to make plot interesting)
83 if vd != 0:
84 phi = phid + (alpha1 * l) / vd * ey + (alpha2 * l) / vd * etheta
85 else:
86 # We aren't moving, so don't turn the steering wheel
87 phi = phid
88
89 return np.array([v, phi])
90
91# Define the controller as an input/output system
92controller = ct.NonlinearIOSystem(
93 None, control_output, name='controller', # static system
94 inputs=('ex', 'ey', 'etheta', 'vd', 'phid'), # system inputs
95 outputs=('v', 'phi') # system outputs
96)
97
98#
99# Reference trajectory subsystem
100#
101# The reference trajectory block generates a simple trajectory for the system
102# given the desired speed (vref) and lateral position (yref). The trajectory
103# consists of a straight line of the form (vref * t, yref, 0) with nominal
104# input (vref, 0).
105#
106# System state: none
107# System input: vref, yref
108# System output: xd, yd, thetad, vd, phid
109# System parameters: none
110#
111def trajgen_output(t, x, u, params):
112 vref, yref = u
113 return np.array([vref * t, yref, 0, vref, 0])
114
115# Define the trajectory generator as an input/output system
116trajgen = ct.NonlinearIOSystem(
117 None, trajgen_output, name='trajgen',
118 inputs=('vref', 'yref'),
119 outputs=('xd', 'yd', 'thetad', 'vd', 'phid'))
120
121#
122# System construction
123#
124# The input to the full closed loop system is the desired lateral position and
125# the desired forward velocity. The output for the system is taken as the
126# full vehicle state plus the velocity of the vehicle. The following diagram
127# summarizes the interconnections:
128#
129# +---------+ +---------------> v
130# | | |
131# [ yref ] | v |
132# [ ] ---> trajgen -+-+-> controller -+-> vehicle -+-> [x, y, theta]
133# [ vref ] ^ |
134# | |
135# +----------- [-1] -----------+
136#
137# We construct the system using the InterconnectedSystem constructor and using
138# signal labels to keep track of everything.
139
140steering = ct.interconnect(
141 # List of subsystems
142 (trajgen, controller, vehicle), name='steering',
143
144 # Interconnections between subsystems
145 connections=(
146 ['controller.ex', 'trajgen.xd', '-vehicle.x'],
147 ['controller.ey', 'trajgen.yd', '-vehicle.y'],
148 ['controller.etheta', 'trajgen.thetad', '-vehicle.theta'],
149 ['controller.vd', 'trajgen.vd'],
150 ['controller.phid', 'trajgen.phid'],
151 ['vehicle.v', 'controller.v'],
152 ['vehicle.phi', 'controller.phi']
153 ),
154
155 # System inputs
156 inplist=['trajgen.vref', 'trajgen.yref'],
157 inputs=['yref', 'vref'],
158
159 # System outputs
160 outlist=['vehicle.x', 'vehicle.y', 'vehicle.theta', 'controller.v',
161 'controller.phi'],
162 outputs=['x', 'y', 'theta', 'v', 'phi']
163)
164
165# Set up the simulation conditions
166yref = 1
167T = np.linspace(0, 5, 100)
168
169# Set up a figure for plotting the results
170plt.figure();
171
172# Plot the reference trajectory for the y position
173plt.plot([0, 5], [yref, yref], 'k--')
174
175# Find the signals we want to plot
176y_index = steering.find_output('y')
177v_index = steering.find_output('v')
178
179# Do an iteration through different speeds
180for vref in [8, 10, 12]:
181 # Simulate the closed loop controller response
182 tout, yout = ct.input_output_response(
183 steering, T, [vref * np.ones(len(T)), yref * np.ones(len(T))])
184
185 # Plot the reference speed
186 plt.plot([0, 5], [vref, vref], 'k--')
187
188 # Plot the system output
189 y_line, = plt.plot(tout, yout[y_index], 'r') # lateral position
190 v_line, = plt.plot(tout, yout[v_index], 'b') # vehicle velocity
191
192# Add axis labels
193plt.xlabel('Time (s)')
194plt.ylabel('x vel (m/s), y pos (m)')
195plt.legend((v_line, y_line), ('v', 'y'), loc='center right', frameon=False)
196
197#
198# Alternative formulation, using create_statefbk_iosystem()
199#
200# A different way to implement gain scheduling is to use the gain scheduling
201# functionality built into the create_statefbk_iosystem() function, where we
202# pass a table of gains instead of a single gain. To generate a more
203# interesting plot, we scale the feedforward input to generate some error.
204#
205
206import itertools
207from math import pi
208
209# Define the points for the scheduling variables
210speeds = [1, 10, 20]
211angles = np.linspace(-pi, pi, 4)
212points = list(itertools.product(speeds, angles))
213
214# Create controllers at each scheduling point
215Q = np.diag([1, 1, 1])
216R = np.diag([0.1, 0.1])
217gains = [np.array(ct.lqr(vehicle.linearize(
218 [0, 0, angle], [speed, 0]), Q, R)[0]) for speed, angle in points]
219
220# Create the gain scheduled system
221controller, _ = ct.create_statefbk_iosystem(
222 vehicle, (gains, points), name='controller', ud_labels=['vd', 'phid'],
223 gainsched_indices=['vd', 'theta'], gainsched_method='linear')
224
225# Connect everything together (note that controller inputs are different
226steering = ct.interconnect(
227 # List of subsystems
228 (trajgen, controller, vehicle), name='steering',
229
230 # Interconnections between subsystems
231 connections=(
232 ['controller.xd[0]', 'trajgen.xd'],
233 ['controller.xd[1]', 'trajgen.yd'],
234 ['controller.xd[2]', 'trajgen.thetad'],
235 ['controller.x', 'vehicle.x'],
236 ['controller.y', 'vehicle.y'],
237 ['controller.theta', 'vehicle.theta'],
238 ['controller.vd', ('trajgen', 'vd', 0.2)], # create error
239 ['controller.phid', 'trajgen.phid'],
240 ['vehicle.v', 'controller.v'],
241 ['vehicle.phi', 'controller.phi']
242 ),
243
244 # System inputs
245 inplist=['trajgen.vref', 'trajgen.yref'],
246 inputs=['yref', 'vref'],
247
248 # System outputs
249 outlist=['vehicle.x', 'vehicle.y', 'vehicle.theta', 'controller.v',
250 'controller.phi'],
251 outputs=['x', 'y', 'theta', 'v', 'phi']
252)
253
254# Plot the results to compare to the previous case
255plt.figure();
256
257# Plot the reference trajectory for the y position
258plt.plot([0, 5], [yref, yref], 'k--')
259
260# Find the signals we want to plot
261y_index = steering.find_output('y')
262v_index = steering.find_output('v')
263
264# Do an iteration through different speeds
265for vref in [8, 10, 12]:
266 # Simulate the closed loop controller response
267 tout, yout = ct.input_output_response(
268 steering, T, [vref * np.ones(len(T)), yref * np.ones(len(T))])
269
270 # Plot the reference speed
271 plt.plot([0, 5], [vref, vref], 'k--')
272
273 # Plot the system output
274 y_line, = plt.plot(tout, yout[y_index], 'r') # lateral position
275 v_line, = plt.plot(tout, yout[v_index], 'b') # vehicle velocity
276
277# Add axis labels
278plt.xlabel('Time (s)')
279plt.ylabel('x vel (m/s), y pos (m)')
280plt.legend((v_line, y_line), ('v', 'y'), loc='center right', frameon=False)