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.nlsys(
50 vehicle_update, vehicle_output, states=3, name='vehicle',
51 inputs=('v', 'phi'), outputs=('x', 'y', 'theta'),
52 params={'wheelbase': 3, 'maxsteer': 0.5})
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.nlsys(
93 None, control_output, name='controller', # static system
94 inputs=('ex', 'ey', 'etheta', 'vd', 'phid'), # system inputs
95 outputs=('v', 'phi'), # system outputs
96 params={'longpole': -2, 'latpole1': -1/2 + sqrt(-7)/2,
97 'latpole2': -1/2 - sqrt(-7)/2, 'wheelbase': 3}
98)
99
100#
101# Reference trajectory subsystem
102#
103# The reference trajectory block generates a simple trajectory for the system
104# given the desired speed (vref) and lateral position (yref). The trajectory
105# consists of a straight line of the form (vref * t, yref, 0) with nominal
106# input (vref, 0).
107#
108# System state: none
109# System input: vref, yref
110# System output: xd, yd, thetad, vd, phid
111# System parameters: none
112#
113def trajgen_output(t, x, u, params):
114 vref, yref = u
115 return np.array([vref * t, yref, 0, vref, 0])
116
117# Define the trajectory generator as an input/output system
118trajgen = ct.nlsys(
119 None, trajgen_output, name='trajgen',
120 inputs=('vref', 'yref'),
121 outputs=('xd', 'yd', 'thetad', 'vd', 'phid'))
122
123#
124# System construction
125#
126# The input to the full closed loop system is the desired lateral position and
127# the desired forward velocity. The output for the system is taken as the
128# full vehicle state plus the velocity of the vehicle. The following diagram
129# summarizes the interconnections:
130#
131# +---------+ +---------------> v
132# | | |
133# [ yref ] | v |
134# [ ] ---> trajgen -+-+-> controller -+-> vehicle -+-> [x, y, theta]
135# [ vref ] ^ |
136# | |
137# +----------- [-1] -----------+
138#
139# We construct the system using the InterconnectedSystem constructor and using
140# signal labels to keep track of everything.
141
142steering = ct.interconnect(
143 # List of subsystems
144 (trajgen, controller, vehicle), name='steering',
145
146 # Interconnections between subsystems
147 connections=(
148 ['controller.ex', 'trajgen.xd', '-vehicle.x'],
149 ['controller.ey', 'trajgen.yd', '-vehicle.y'],
150 ['controller.etheta', 'trajgen.thetad', '-vehicle.theta'],
151 ['controller.vd', 'trajgen.vd'],
152 ['controller.phid', 'trajgen.phid'],
153 ['vehicle.v', 'controller.v'],
154 ['vehicle.phi', 'controller.phi']
155 ),
156
157 # System inputs
158 inplist=['trajgen.vref', 'trajgen.yref'],
159 inputs=['yref', 'vref'],
160
161 # System outputs
162 outlist=['vehicle.x', 'vehicle.y', 'vehicle.theta', 'controller.v',
163 'controller.phi'],
164 outputs=['x', 'y', 'theta', 'v', 'phi'],
165
166 # Parameters
167 params=trajgen.params | vehicle.params | controller.params,
168)
169
170# Set up the simulation conditions
171yref = 1
172T = np.linspace(0, 5, 100)
173
174# Set up a figure for plotting the results
175plt.figure();
176
177# Plot the reference trajectory for the y position
178plt.plot([0, 5], [yref, yref], 'k--')
179
180# Find the signals we want to plot
181y_index = steering.find_output('y')
182v_index = steering.find_output('v')
183
184# Do an iteration through different speeds
185for vref in [8, 10, 12]:
186 # Simulate the closed loop controller response
187 tout, yout = ct.input_output_response(
188 steering, T, [vref * np.ones(len(T)), yref * np.ones(len(T))])
189
190 # Plot the reference speed
191 plt.plot([0, 5], [vref, vref], 'k--')
192
193 # Plot the system output
194 y_line, = plt.plot(tout, yout[y_index], 'r') # lateral position
195 v_line, = plt.plot(tout, yout[v_index], 'b') # vehicle velocity
196
197# Add axis labels
198plt.xlabel('Time (s)')
199plt.ylabel('x vel (m/s), y pos (m)')
200plt.legend((v_line, y_line), ('v', 'y'), loc='center right', frameon=False)
201
202#
203# Alternative formulation, using create_statefbk_iosystem()
204#
205# A different way to implement gain scheduling is to use the gain scheduling
206# functionality built into the create_statefbk_iosystem() function, where we
207# pass a table of gains instead of a single gain. To generate a more
208# interesting plot, we scale the feedforward input to generate some error.
209#
210
211import itertools
212from math import pi
213
214# Define the points for the scheduling variables
215speeds = [1, 10, 20]
216angles = np.linspace(-pi, pi, 4)
217points = list(itertools.product(speeds, angles))
218
219# Create controllers at each scheduling point
220Q = np.diag([1, 1, 1])
221R = np.diag([0.1, 0.1])
222gains = [np.array(ct.lqr(vehicle.linearize(
223 [0, 0, angle], [speed, 0]), Q, R)[0]) for speed, angle in points]
224
225# Create the gain scheduled system
226controller, _ = ct.create_statefbk_iosystem(
227 vehicle, (gains, points), name='controller', ud_labels=['vd', 'phid'],
228 gainsched_indices=['vd', 'theta'], gainsched_method='linear',
229 params=vehicle.params | controller.params)
230
231# Connect everything together (note that controller inputs are different)
232steering = ct.interconnect(
233 # List of subsystems
234 (trajgen, controller, vehicle), name='steering',
235
236 # Interconnections between subsystems
237 connections=(
238 ['controller.xd[0]', 'trajgen.xd'],
239 ['controller.xd[1]', 'trajgen.yd'],
240 ['controller.xd[2]', 'trajgen.thetad'],
241 ['controller.x', 'vehicle.x'],
242 ['controller.y', 'vehicle.y'],
243 ['controller.theta', 'vehicle.theta'],
244 ['controller.vd', ('trajgen', 'vd', 0.2)], # create some error
245 ['controller.phid', 'trajgen.phid'],
246 ['vehicle.v', 'controller.v'],
247 ['vehicle.phi', 'controller.phi']
248 ),
249
250 # System inputs
251 inplist=['trajgen.vref', 'trajgen.yref'],
252 inputs=['yref', 'vref'],
253
254 # System outputs
255 outlist=['vehicle.x', 'vehicle.y', 'vehicle.theta', 'controller.v',
256 'controller.phi'],
257 outputs=['x', 'y', 'theta', 'v', 'phi'],
258
259 # Parameters
260 params=steering.params
261)
262
263# Plot the results to compare to the previous case
264plt.figure();
265
266# Plot the reference trajectory for the y position
267plt.plot([0, 5], [yref, yref], 'k--')
268
269# Find the signals we want to plot
270y_index = steering.find_output('y')
271v_index = steering.find_output('v')
272
273# Do an iteration through different speeds
274for vref in [8, 10, 12]:
275 # Simulate the closed loop controller response
276 tout, yout = ct.input_output_response(
277 steering, T, [vref * np.ones(len(T)), yref * np.ones(len(T))])
278
279 # Plot the reference speed
280 plt.plot([0, 5], [vref, vref], 'k--')
281
282 # Plot the system output
283 y_line, = plt.plot(tout, yout[y_index], 'r') # lateral position
284 v_line, = plt.plot(tout, yout[v_index], 'b') # vehicle velocity
285
286# Add axis labels
287plt.xlabel('Time (s)')
288plt.ylabel('x vel (m/s), y pos (m)')
289plt.legend((v_line, y_line), ('v', 'y'), loc='center right', frameon=False)