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'],
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
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
192# Add axis labels
193mpl.xlabel('Time (s)')
194mpl.ylabel('x vel (m/s), y pos (m)')
195mpl.legend((v_line, y_line), ('v', 'y'), loc='center right', frameon=False)

Notes