python-control Example: Vertical takeoff and landing aircraftThis page 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.
This example uses a simplified model for a (planar) vertical takeoff and landing aircraft (PVTOL), as shown below:
It is convenient to redefine the inputs so that the origin is an equilibrium point of the system with zero input. Letting $u_1 =
F_1$ and $u_2 = F_2 - mg$, the equations can be written in state space form as:
This section demonstrates the design of an LQR state feedback controller for the vectored thrust aircraft example. This example is pulled from Chapter 6 (State Feedback) of [http:www.cds.caltech.edu/~murray/amwiki Astrom and Murray]. The python code listed here are contained the the file pvtol-lqr.py.
To execute this example, we first import the libraries for SciPy, MATLAB plotting and the python-control package:
In [1]:
from numpy import * # Grab all of the NumPy functions
from matplotlib.pyplot import * # Grab MATLAB plotting functions
from control.matlab import * # MATLAB-like functions
%matplotlib inline
The parameters for the system are given by
In [2]:
m = 4 # mass of aircraft
J = 0.0475 # inertia around pitch axis
r = 0.25 # distance to center of force
g = 9.8 # gravitational constant
c = 0.05 # damping factor (estimated)
print("m = %f" % m)
print("J = %f" % J)
print("r = %f" % r)
print("g = %f" % g)
print("c = %f" % c)
The linearization of the dynamics near the equilibrium point $x_e = (0, 0, 0, 0, 0, 0)$, $u_e = (0, mg)$ are given by
In [3]:
# State space dynamics
xe = [0, 0, 0, 0, 0, 0] # equilibrium point of interest
ue = [0, m*g] # (note these are lists, not matrices)
In [4]:
# Dynamics matrix (use matrix type so that * works for multiplication)
A = matrix(
[[ 0, 0, 0, 1, 0, 0],
[ 0, 0, 0, 0, 1, 0],
[ 0, 0, 0, 0, 0, 1],
[ 0, 0, (-ue[0]*sin(xe[2]) - ue[1]*cos(xe[2]))/m, -c/m, 0, 0],
[ 0, 0, (ue[0]*cos(xe[2]) - ue[1]*sin(xe[2]))/m, 0, -c/m, 0],
[ 0, 0, 0, 0, 0, 0 ]])
# Input matrix
B = matrix(
[[0, 0], [0, 0], [0, 0],
[cos(xe[2])/m, -sin(xe[2])/m],
[sin(xe[2])/m, cos(xe[2])/m],
[r/J, 0]])
# Output matrix
C = matrix([[1, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0]])
D = matrix([[0, 0], [0, 0]])
To compute a linear quadratic regulator for the system, we write the cost function as
where $z = z - z_e$ and $v = u - u_e$ represent the local coordinates around the desired equilibrium point $(z_e, u_e)$. We begin with diagonal matrices for the state and input costs:
In [5]:
Qx1 = diag([1, 1, 1, 1, 1, 1])
Qu1a = diag([1, 1])
(K, X, E) = lqr(A, B, Qx1, Qu1a); K1a = matrix(K)
This gives a control law of the form $v = -K z$, which can then be used to derive the control law in terms of the original variables:
$$u = v + u_d = - K(z - z_d) + u_d.$$ where $u_d = (0, mg)$ and $z_d = (x_d, y_d, 0, 0, 0, 0)$
Since the python-control package only supports SISO systems, in order to compute the closed loop dynamics, we must extract the dynamics for the lateral and altitude dynamics as individual systems. In addition, we simulate the closed loop dynamics using the step command with $K x_d$ as the input vector (assumes that the "input" is unit size, with $xd$ corresponding to the desired steady state. The following code performs these operations:
In [6]:
xd = matrix([[1], [0], [0], [0], [0], [0]])
yd = matrix([[0], [1], [0], [0], [0], [0]])
In [7]:
# Indices for the parts of the state that we want
lat = (0,2,3,5)
alt = (1,4)
# Decoupled dynamics
Ax = (A[lat, :])[:, lat] #! not sure why I have to do it this way
Bx, Cx, Dx = B[lat, 0], C[0, lat], D[0, 0]
Ay = (A[alt, :])[:, alt] #! not sure why I have to do it this way
By, Cy, Dy = B[alt, 1], C[1, alt], D[1, 1]
# Step response for the first input
H1ax = ss(Ax - Bx*K1a[0,lat], Bx*K1a[0,lat]*xd[lat,:], Cx, Dx)
(Tx, Yx) = step(H1ax, T=linspace(0,10,100))
# Step response for the second input
H1ay = ss(Ay - By*K1a[1,alt], By*K1a[1,alt]*yd[alt,:], Cy, Dy)
(Ty, Yy) = step(H1ay, T=linspace(0,10,100))
In [8]:
plot(Yx.T, Tx, '-', Yy.T, Ty, '--')
plot([0, 10], [1, 1], 'k-')
ylabel('Position')
xlabel('Time (s)')
title('Step Response for Inputs')
legend(('Yx', 'Yy'), loc='lower right')
show()
The plot above shows the $x$ and $y$ positions of the aircraft when it is commanded to move 1 m in each direction. The following shows the $x$ motion for control weights $\rho = 1, 10^2, 10^4$. A higher weight of the input term in the cost function causes a more sluggish response. It is created using the code:
In [9]:
# Look at different input weightings
Qu1a = diag([1, 1])
K1a, X, E = lqr(A, B, Qx1, Qu1a)
H1ax = ss(Ax - Bx*K1a[0,lat], Bx*K1a[0,lat]*xd[lat,:], Cx, Dx)
Qu1b = (40**2)*diag([1, 1])
K1b, X, E = lqr(A, B, Qx1, Qu1b)
H1bx = ss(Ax - Bx*K1b[0,lat], Bx*K1b[0,lat]*xd[lat,:],Cx, Dx)
Qu1c = (200**2)*diag([1, 1])
K1c, X, E = lqr(A, B, Qx1, Qu1c)
H1cx = ss(Ax - Bx*K1c[0,lat], Bx*K1c[0,lat]*xd[lat,:],Cx, Dx)
[T1, Y1] = step(H1ax, T=linspace(0,10,100))
[T2, Y2] = step(H1bx, T=linspace(0,10,100))
[T3, Y3] = step(H1cx, T=linspace(0,10,100))
In [10]:
plot(Y1.T, T1, 'b-')
plot(Y2.T, T2, 'r-')
plot(Y3.T, T3, 'g-')
plot([0 ,10], [1, 1], 'k-')
title('Step Response for Inputs')
ylabel('Position')
xlabel('Time (s)')
legend(('Y1','Y2','Y3'),loc='lower right')
axis([0, 10, -0.1, 1.4])
show()
This section demonstrates the design of loop shaping controller for the vectored thrust aircraft example. This example is pulled from Chapter 11 Frequency Domain Design of Astrom and Murray.
To design a controller for the lateral dynamics of the vectored thrust aircraft, we make use of a "inner/outer" loop design methodology. We begin by representing the dynamics using the block diagram
The controller is constructed by splitting the process dynamics and controller into two components: an inner loop consisting of the roll dynamics $P_i$ and control $C_i$ and an outer loop consisting of the lateral position dynamics $P_o$ and controller $C_o$.
The following code imports the libraries that are required and defines the dynamics:
In [11]:
from matplotlib.pyplot import * # Grab MATLAB plotting functions
from control.matlab import * # MATLAB-like functions
In [12]:
# System parameters
m = 4 # mass of aircraft
J = 0.0475 # inertia around pitch axis
r = 0.25 # distance to center of force
g = 9.8 # gravitational constant
c = 0.05 # damping factor (estimated)
print("m = %f" % m)
print("J = %f" % J)
print("r = %f" % r)
print("g = %f" % g)
print("c = %f" % c)
In [13]:
# Transfer functions for dynamics
Pi = tf([r], [J, 0, 0]) # inner loop (roll)
Po = tf([1], [m, c, 0]) # outer loop (position)
For the inner loop, use a lead compensator
In [14]:
k = 200
a = 2
b = 50
Ci = k*tf([1, a], [1, b]) # lead compensator
Li = Pi*Ci
The closed loop dynamics of the inner loop, $H_i$, are given by
In [15]:
Hi = parallel(feedback(Ci, Pi), -m*g*feedback(Ci*Pi, 1))
Finally, we design the lateral compensator using another lead compenstor
In [16]:
# Now design the lateral control system
a = 0.02
b = 5
K = 2
Co = -K*tf([1, 0.3], [1, 10]) # another lead compensator
Lo = -m*g*Po*Co
The performance of the system can be characterized using the sensitivity function and the complementary sensitivity function:
In [17]:
L = Co*Hi*Po
S = feedback(1, L)
T = feedback(L, 1)
In [18]:
t, y = step(T, T=linspace(0,10,100))
plot(y, t)
title("Step Response")
grid()
xlabel("time (s)")
ylabel("y(t)")
show()
The frequency response and Nyquist plot for the loop transfer function are computed using the commands
In [19]:
bode(L)
show()
In [20]:
nyquist(L, (0.0001, 1000))
show()
In [21]:
gangof4(Hi*Po, Co)
In [ ]:
In [ ]:
In [ ]:
In [ ]: