# Gain scheduled control for vehicle steeering (I/O system)¶

## Code¶

```  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 mpl
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.InterconnectedSystem(
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'],
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
170mpl.figure();
171
172# Plot the reference trajectory for the y position
173mpl.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    mpl.plot([0, 5], [vref, vref], 'k--')
187
188    # Plot the system output
189    y_line, = mpl.plot(tout, yout[y_index, :], 'r')  # lateral position
190    v_line, = mpl.plot(tout, yout[v_index, :], 'b')  # vehicle velocity
191