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