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

Notes

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