Inner/outer 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-nested.py - inner/outer design for vectored thrust aircraft
  2# RMM, 5 Sep 09
  3#
  4# This file works through a fairly complicated control design and
  5# analysis, corresponding to the planar vertical takeoff and landing
  6# (PVTOL) aircraft in Astrom and Murray, Chapter 11.  It is intended
  7# to demonstrate the basic functionality of the python-control
  8# package.
  9#
 10
 11from __future__ import print_function
 12
 13import os
 14import matplotlib.pyplot as plt  # MATLAB plotting functions
 15from control.matlab import *    # MATLAB-like functions
 16import numpy as np
 17
 18# System parameters
 19m = 4               # mass of aircraft
 20J = 0.0475          # inertia around pitch axis
 21r = 0.25            # distance to center of force
 22g = 9.8             # gravitational constant
 23c = 0.05            # damping factor (estimated)
 24
 25# Transfer functions for dynamics
 26Pi = tf([r], [J, 0, 0])  # inner loop (roll)
 27Po = tf([1], [m, c, 0])  # outer loop (position)
 28
 29#
 30# Inner loop control design
 31#
 32# This is the controller for the pitch dynamics.  Goal is to have
 33# fast response for the pitch dynamics so that we can use this as a 
 34# control for the lateral dynamics
 35#
 36
 37# Design a simple lead controller for the system
 38k, a, b = 200, 2, 50
 39Ci = k*tf([1, a], [1, b])  # lead compensator
 40Li = Pi*Ci
 41
 42# Bode plot for the open loop process
 43plt.figure(1) 
 44bode(Pi)
 45
 46# Bode plot for the loop transfer function, with margins
 47plt.figure(2)
 48bode(Li)
 49
 50# Compute out the gain and phase margins
 51#! Not implemented
 52# gm, pm, wcg, wcp = margin(Li)
 53
 54# Compute the sensitivity and complementary sensitivity functions
 55Si = feedback(1, Li)
 56Ti = Li*Si
 57
 58# Check to make sure that the specification is met
 59plt.figure(3)
 60gangof4(Pi, Ci)
 61
 62# Compute out the actual transfer function from u1 to v1 (see L8.2 notes)
 63# Hi = Ci*(1-m*g*Pi)/(1+Ci*Pi)
 64Hi = parallel(feedback(Ci, Pi), -m*g*feedback(Ci*Pi, 1))
 65
 66plt.figure(4)
 67plt.clf()
 68plt.subplot(221)
 69bode(Hi)
 70
 71# Now design the lateral control system
 72a, b, K = 0.02, 5, 2
 73Co = -K*tf([1, 0.3], [1, 10])  # another lead compensator
 74Lo = -m*g*Po*Co
 75
 76plt.figure(5)
 77bode(Lo)  # margin(Lo)
 78
 79# Finally compute the real outer-loop loop gain + responses
 80L = Co*Hi*Po
 81S = feedback(1, L)
 82T = feedback(L, 1)
 83
 84# Compute stability margins
 85gm, pm, wgc, wpc = margin(L)
 86print("Gain margin: %g at %g" % (gm, wgc))
 87print("Phase margin: %g at %g" % (pm, wpc))
 88
 89plt.figure(6)
 90plt.clf()
 91bode(L, np.logspace(-4, 3))
 92
 93# Add crossover line to the magnitude plot
 94#
 95# Note: in matplotlib before v2.1, the following code worked:
 96#
 97#   plt.subplot(211); hold(True);
 98#   loglog([1e-4, 1e3], [1, 1], 'k-')
 99#
100# In later versions of matplotlib the call to plt.subplot will clear the
101# axes and so we have to extract the axes that we want to use by hand.
102# In addition, hold() is deprecated so we no longer require it.
103#
104for ax in plt.gcf().axes:
105    if ax.get_label() == 'control-bode-magnitude':
106        break
107ax.semilogx([1e-4, 1e3], 20*np.log10([1, 1]), 'k-')
108
109#
110# Replot phase starting at -90 degrees
111#
112# Get the phase plot axes
113for ax in plt.gcf().axes:
114    if ax.get_label() == 'control-bode-phase':
115        break
116
117# Recreate the frequency response and shift the phase
118mag, phase, w = freqresp(L, np.logspace(-4, 3))
119phase = phase - 360
120
121# Replot the phase by hand
122ax.semilogx([1e-4, 1e3], [-180, -180], 'k-')
123ax.semilogx(w, np.squeeze(phase), 'b-')
124ax.axis([1e-4, 1e3, -360, 0])
125plt.xlabel('Frequency [deg]')
126plt.ylabel('Phase [deg]')
127# plt.set(gca, 'YTick', [-360, -270, -180, -90, 0])
128# plt.set(gca, 'XTick', [10^-4, 10^-2, 1, 100])
129
130#
131# Nyquist plot for complete design
132#
133plt.figure(7)
134plt.clf()
135nyquist(L, (0.0001, 1000))
136
137# Add a box in the region we are going to expand
138plt.plot([-2, -2, 1, 1, -2], [-4, 4, 4, -4, -4], 'r-')
139
140# Expanded region  
141plt.figure(8)
142plt.clf()
143nyquist(L)
144plt.axis([-2, 1, -4, 4])
145
146# set up the color
147color = 'b'
148
149# Add arrows to the plot
150# H1 = L.evalfr(0.4); H2 = L.evalfr(0.41);
151# arrow([real(H1), imag(H1)], [real(H2), imag(H2)], AM_normal_arrowsize, \
152#  'EdgeColor', color, 'FaceColor', color);
153
154# H1 = freqresp(L, 0.35); H2 = freqresp(L, 0.36);
155# arrow([real(H2), -imag(H2)], [real(H1), -imag(H1)], AM_normal_arrowsize, \
156#  'EdgeColor', color, 'FaceColor', color);
157
158plt.figure(9)
159Yvec, Tvec = step(T, np.linspace(0, 20))
160plt.plot(Tvec.T, Yvec.T)
161
162Yvec, Tvec = step(Co*S, np.linspace(0, 20))
163plt.plot(Tvec.T, Yvec.T)
164
165plt.figure(10)
166plt.clf()
167P, Z = pzmap(T, plot=True, grid=True)
168print("Closed loop poles and zeros: ", P, Z)
169
170# Gang of Four
171plt.figure(11)
172plt.clf()
173gangof4(Hi*Po, Co)
174
175if 'PYCONTROL_TEST_EXAMPLES' not in os.environ:
176    plt.show()

Notes

1. Importing print_function from __future__ in line 11 is only required if using Python 2.7.

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