LQR control design for vertical takeoff and landing aircraft

This script demonstrates the use of the python-control package for analysis and design of a controller for a vectored thrust aircraft model that is used as a running example through the text Feedback Systems by Astrom and Murray. This example makes use of MATLAB compatible commands.

Code

  1# pvtol_lqr.m - LQR design for vectored thrust aircraft
  2# RMM, 14 Jan 03
  3#
  4# This file works through an LQR based design problem, using the
  5# planar vertical takeoff and landing (PVTOL) aircraft example from
  6# Astrom and Murray, Chapter 5.  It is intended to demonstrate the
  7# basic functionality of the python-control package.
  8#
  9
 10import os
 11import numpy as np
 12import matplotlib.pyplot as plt  # MATLAB plotting functions
 13from control.matlab import *  # MATLAB-like functions
 14
 15#
 16# System dynamics
 17#
 18# These are the dynamics for the PVTOL system, written in state space
 19# form.
 20#
 21
 22# System parameters
 23m = 4       # mass of aircraft
 24J = 0.0475  # inertia around pitch axis
 25r = 0.25    # distance to center of force
 26g = 9.8     # gravitational constant
 27c = 0.05    # damping factor (estimated)
 28
 29# State space dynamics
 30xe = [0, 0, 0, 0, 0, 0]  # equilibrium point of interest
 31ue = [0, m*g]  # (note these are lists, not matrices)
 32
 33# TODO: The following objects need converting from np.matrix to np.array
 34# This will involve re-working the subsequent equations as the shapes
 35# See below.
 36
 37# Dynamics matrix (use matrix type so that * works for multiplication)
 38A = np.matrix(
 39    [[0, 0, 0, 1, 0, 0],
 40     [0, 0, 0, 0, 1, 0],
 41     [0, 0, 0, 0, 0, 1],
 42     [0, 0, (-ue[0]*np.sin(xe[2]) - ue[1]*np.cos(xe[2]))/m, -c/m, 0, 0],
 43     [0, 0, (ue[0]*np.cos(xe[2]) - ue[1]*np.sin(xe[2]))/m, 0, -c/m, 0],
 44     [0, 0, 0, 0, 0, 0]]
 45)
 46
 47# Input matrix
 48B = np.matrix(
 49    [[0, 0], [0, 0], [0, 0],
 50     [np.cos(xe[2])/m, -np.sin(xe[2])/m],
 51     [np.sin(xe[2])/m, np.cos(xe[2])/m],
 52     [r/J, 0]]
 53)
 54
 55# Output matrix 
 56C = np.matrix([[1, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0]])
 57D = np.matrix([[0, 0], [0, 0]])
 58
 59#
 60# Construct inputs and outputs corresponding to steps in xy position
 61#
 62# The vectors xd and yd correspond to the states that are the desired
 63# equilibrium states for the system.  The matrices Cx and Cy are the 
 64# corresponding outputs.
 65#
 66# The way these vectors are used is to compute the closed loop system
 67# dynamics as
 68#
 69#   xdot = Ax + B u  =>  xdot = (A-BK)x + K xd
 70#      u = -K(x - xd)       y = Cx
 71#
 72# The closed loop dynamics can be simulated using the "step" command, 
 73# with K*xd as the input vector (assumes that the "input" is unit size,
 74# so that xd corresponds to the desired steady state.
 75#
 76
 77xd = np.matrix([[1], [0], [0], [0], [0], [0]])
 78yd = np.matrix([[0], [1], [0], [0], [0], [0]])
 79
 80#
 81# Extract the relevant dynamics for use with SISO library
 82#
 83# The current python-control library only supports SISO transfer
 84# functions, so we have to modify some parts of the original MATLAB
 85# code to extract out SISO systems.  To do this, we define the 'lat' and
 86# 'alt' index vectors to consist of the states that are are relevant
 87# to the lateral (x) and vertical (y) dynamics.
 88#
 89
 90# Indices for the parts of the state that we want
 91lat = (0, 2, 3, 5)
 92alt = (1, 4)
 93
 94# Decoupled dynamics
 95Ax = (A[lat, :])[:, lat]  # ! not sure why I have to do it this way
 96Bx = B[lat, 0]
 97Cx = C[0, lat]
 98Dx = D[0, 0]
 99
100Ay = (A[alt, :])[:, alt]  # ! not sure why I have to do it this way
101By = B[alt, 1]
102Cy = C[1, alt]
103Dy = D[1, 1]
104
105# Label the plot
106plt.clf()
107plt.suptitle("LQR controllers for vectored thrust aircraft (pvtol-lqr)")
108
109#
110# LQR design
111#
112
113# Start with a diagonal weighting
114Qx1 = np.diag([1, 1, 1, 1, 1, 1])
115Qu1a = np.diag([1, 1])
116K, X, E = lqr(A, B, Qx1, Qu1a)
117K1a = np.matrix(K)
118
119# Close the loop: xdot = Ax - B K (x-xd)
120# Note: python-control requires we do this 1 input at a time
121# H1a = ss(A-B*K1a, B*K1a*concatenate((xd, yd), axis=1), C, D);
122# (T, Y) = step(H1a, T=np.linspace(0,10,100));
123
124# TODO: The following equations will need modifying when converting from np.matrix to np.array
125# because the results and even intermediate calculations will be different with numpy arrays
126# For example:
127# Bx = B[lat, 0]
128# Will need to be changed to:
129# Bx = B[lat, 0].reshape(-1, 1)
130# (if we want it to have the same shape as before)
131
132# For reference, here is a list of the correct shapes of these objects:
133# A: (6, 6)
134# B: (6, 2)
135# C: (2, 6)
136# D: (2, 2)
137# xd: (6, 1)
138# yd: (6, 1)
139# Ax: (4, 4)
140# Bx: (4, 1)
141# Cx: (1, 4)
142# Dx: ()
143# Ay: (2, 2)
144# By: (2, 1)
145# Cy: (1, 2)
146
147# Step response for the first input
148H1ax = ss(Ax - Bx*K1a[0, lat], Bx*K1a[0, lat]*xd[lat, :], Cx, Dx)
149Yx, Tx = step(H1ax, T=np.linspace(0, 10, 100))
150
151# Step response for the second input
152H1ay = ss(Ay - By*K1a[1, alt], By*K1a[1, alt]*yd[alt, :], Cy, Dy)
153Yy, Ty = step(H1ay, T=np.linspace(0, 10, 100))
154
155plt.subplot(221)
156plt.title("Identity weights")
157# plt.plot(T, Y[:,1, 1], '-', T, Y[:,2, 2], '--')
158plt.plot(Tx.T, Yx.T, '-', Ty.T, Yy.T, '--')
159plt.plot([0, 10], [1, 1], 'k-')
160
161plt.axis([0, 10, -0.1, 1.4])
162plt.ylabel('position')
163plt.legend(('x', 'y'), loc='lower right')
164
165# Look at different input weightings
166Qu1a = np.diag([1, 1])
167K1a, X, E = lqr(A, B, Qx1, Qu1a)
168H1ax = ss(Ax - Bx*K1a[0, lat], Bx*K1a[0, lat]*xd[lat, :], Cx, Dx)
169
170Qu1b = (40 ** 2)*np.diag([1, 1])
171K1b, X, E = lqr(A, B, Qx1, Qu1b)
172H1bx = ss(Ax - Bx*K1b[0, lat], Bx*K1b[0, lat]*xd[lat, :], Cx, Dx)
173
174Qu1c = (200 ** 2)*np.diag([1, 1])
175K1c, X, E = lqr(A, B, Qx1, Qu1c)
176H1cx = ss(Ax - Bx*K1c[0, lat], Bx*K1c[0, lat]*xd[lat, :], Cx, Dx)
177
178[Y1, T1] = step(H1ax, T=np.linspace(0, 10, 100))
179[Y2, T2] = step(H1bx, T=np.linspace(0, 10, 100))
180[Y3, T3] = step(H1cx, T=np.linspace(0, 10, 100))
181
182plt.subplot(222)
183plt.title("Effect of input weights")
184plt.plot(T1.T, Y1.T, 'b-')
185plt.plot(T2.T, Y2.T, 'b-')
186plt.plot(T3.T, Y3.T, 'b-')
187plt.plot([0, 10], [1, 1], 'k-')
188
189plt.axis([0, 10, -0.1, 1.4])
190
191# arcarrow([1.3, 0.8], [5, 0.45], -6)
192plt.text(5.3, 0.4, 'rho')
193
194# Output weighting - change Qx to use outputs
195Qx2 = C.T*C
196Qu2 = 0.1*np.diag([1, 1])
197K, X, E = lqr(A, B, Qx2, Qu2)
198K2 = np.matrix(K)
199
200H2x = ss(Ax - Bx*K2[0, lat], Bx*K2[0, lat]*xd[lat, :], Cx, Dx)
201H2y = ss(Ay - By*K2[1, alt], By*K2[1, alt]*yd[alt, :], Cy, Dy)
202
203plt.subplot(223)
204plt.title("Output weighting")
205[Y2x, T2x] = step(H2x, T=np.linspace(0, 10, 100))
206[Y2y, T2y] = step(H2y, T=np.linspace(0, 10, 100))
207plt.plot(T2x.T, Y2x.T, T2y.T, Y2y.T)
208plt.ylabel('position')
209plt.xlabel('time')
210plt.ylabel('position')
211plt.legend(('x', 'y'), loc='lower right')
212
213#
214# Physically motivated weighting
215#
216# Shoot for 1 cm error in x, 10 cm error in y.  Try to keep the angle
217# less than 5 degrees in making the adjustments.  Penalize side forces
218# due to loss in efficiency.
219#
220
221Qx3 = np.diag([100, 10, 2*np.pi/5, 0, 0, 0])
222Qu3 = 0.1*np.diag([1, 10])
223(K, X, E) = lqr(A, B, Qx3, Qu3)
224K3 = np.matrix(K)
225
226H3x = ss(Ax - Bx*K3[0, lat], Bx*K3[0, lat]*xd[lat, :], Cx, Dx)
227H3y = ss(Ay - By*K3[1, alt], By*K3[1, alt]*yd[alt, :], Cy, Dy)
228plt.subplot(224)
229# step(H3x, H3y, 10)
230[Y3x, T3x] = step(H3x, T=np.linspace(0, 10, 100))
231[Y3y, T3y] = step(H3y, T=np.linspace(0, 10, 100))
232plt.plot(T3x.T, Y3x.T, T3y.T, Y3y.T)
233plt.title("Physically motivated weights")
234plt.xlabel('time')
235plt.legend(('x', 'y'), loc='lower right')
236
237if 'PYCONTROL_TEST_EXAMPLES' not in os.environ:
238    plt.show()

Notes

1. The environment variable PYCONTROL_TEST_EXAMPLES is used for testing to turn off plotting of the outputs.