Python-control/Example: Vertical takeoff and landing aircraft
This 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. The following files contain the code that is demonstrated here:
- pvtol-lqr.py - state feedback control design
- pvtol-nested.py - inner/outer loop design using transfer functions
System Description
This example uses a simplified model for a (planar) vertical takeoff and landing aircraft (PVTOL), as shown below:
|
|
The position and orientation of the center of mass of the aircraft is denoted by Failed to parse (SVG (MathML can be enabled via browser plugin): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle (x,y,\theta)} , Failed to parse (SVG (MathML can be enabled via browser plugin): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle m} is the mass of the vehicle, Failed to parse (SVG (MathML can be enabled via browser plugin): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle J} the moment of inertia, Failed to parse (SVG (MathML can be enabled via browser plugin): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle g} the gravitational constant and Failed to parse (SVG (MathML can be enabled via browser plugin): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle c} the damping coefficient. The forces generated by the main downward thruster and the maneuvering thrusters are modeled as a pair of forces Failed to parse (SVG (MathML can be enabled via browser plugin): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle F_1} and Failed to parse (SVG (MathML can be enabled via browser plugin): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle F_2} acting at a distance Failed to parse (SVG (MathML can be enabled via browser plugin): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle r} below the aircraft (determined by the geometry of the thrusters).
It is convenient to redefine the inputs so that the origin is an equilibrium point of the system with zero input. Letting Failed to parse (SVG (MathML can be enabled via browser plugin): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle u_1 = F_1} and Failed to parse (SVG (MathML can be enabled via browser plugin): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle u_2 = F_2 - mg} , the equations can be written in state space form as
|
|
LQR state feedback controller
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:
from numpy import * # Grab all of the NumPy functions from matplotlib.pyplot import * # Grab MATLAB plotting functions from control.matlab import * # MATLAB-like functions
The parameters for the system are given by
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)
The linearization of the dynamics near the equilibrium point Failed to parse (SVG (MathML can be enabled via browser plugin): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle x_e = (0, 0, 0, 0, 0, 0)} , Failed to parse (SVG (MathML can be enabled via browser plugin): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle u_e = (0, mg)} are given by
# State space dynamics xe = [0, 0, 0, 0, 0, 0]; # equilibrium point of interest ue = [0, m*g]; # (note these are lists, not matrices)
# 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 Failed to parse (SVG (MathML can be enabled via browser plugin): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle z = z - z_e} and Failed to parse (SVG (MathML can be enabled via browser plugin): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle v = u - u_e} represent the local coordinates around the desired equilibrium point Failed to parse (SVG (MathML can be enabled via browser plugin): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle (z_e, u_e)} . We begin with diagonal matrices for the state and input costs:
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 Failed to parse (SVG (MathML can be enabled via browser plugin): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle v = -K z} , which can then be used to derive the control law in terms of the original variables:
where Failed to parse (SVG (MathML can be enabled via browser plugin): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle u_d = (0, mg)} and Failed to parse (SVG (MathML can be enabled via browser plugin): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle 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 Failed to parse (SVG (MathML can be enabled via browser plugin): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle K x_d} as the input vector (assumes that the "input" is unit size, with Failed to parse (SVG (MathML can be enabled via browser plugin): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle xd} corresponding to the desired steady state. The following code performs these operations:
xd = matrix([[1], [0], [0], [0], [0], [0]]); yd = matrix([[0], [1], [0], [0], [0], [0]]);
# 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 = B[lat, 0]; Cx = C[0, lat]; Dx = D[0, 0]; Ay = (A[alt, :])[:, alt]; #! not sure why I have to do it this way By = B[alt, 1]; Cy = C[1, alt]; Dy = 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));
plot(Tx, Yx[0,:].T, '-', Ty, Yy[0,:].T, '--'); hold(True);
plot([0, 10], [1, 1], 'k-'); hold(True);
ylabel('position');
legend(('x', 'y'), loc='lower right');
The response of the closed loop system to a step change in the desired position is shown below.
|
| |
| Step response in Failed to parse (SVG (MathML can be enabled via browser plugin): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle x} and Failed to parse (SVG (MathML can be enabled via browser plugin): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle y} | Effect of control weight Failed to parse (SVG (MathML can be enabled via browser plugin): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle \rho} |
The plot on the left shows the Failed to parse (SVG (MathML can be enabled via browser plugin): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle x} and Failed to parse (SVG (MathML can be enabled via browser plugin): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle y} positions of the aircraft when it is commanded to move 1 m in each direction.
The response can be tuned by adjusting the weights in the LQR cost. The plot on the right shows the Failed to parse (SVG (MathML can be enabled via browser plugin): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle x} motion for control weights Failed to parse (SVG (MathML can be enabled via browser plugin): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle \rho = 1} , Failed to parse (SVG (MathML can be enabled via browser plugin): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle 10^2} , Failed to parse (SVG (MathML can be enabled via browser plugin): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle 10^4} . A higher weight of the input term in the cost function causes a more sluggish response. It is created using the code:
# 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));
plot(T1, Y1[0,:].T, 'b-'); hold(True); plot(T2, Y2[0,:].T, 'b-'); hold(True); plot(T3, Y3[0,:].T, 'b-'); hold(True); plot([0 ,10], [1, 1], 'k-'); hold(True); axis([0, 10, -0.1, 1.4]); # arcarrow([1.3, 0.8], [5, 0.45], -6); text(5.3, 0.4, 'rho');
Lateral control using inner/outer loop design
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 [http:www.cds.caltech.edu/~murray/amwiki Astrom and Murray]. The python code listed here are contained the the file pvtol-nested.py.
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
where
The controller is constructed by splitting the process dynamics and controller into two components: an inner loop consisting of the roll dynamics Failed to parse (SVG (MathML can be enabled via browser plugin): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle P_i} and control Failed to parse (SVG (MathML can be enabled via browser plugin): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle C_i} and an outer loop consisting of the lateral position dynamics Failed to parse (SVG (MathML can be enabled via browser plugin): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle P_o} and controller Failed to parse (SVG (MathML can be enabled via browser plugin): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle C_o} .
The closed inner loop dynamics Failed to parse (SVG (MathML can be enabled via browser plugin): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle H_i} control the roll angle of the aircraft using the vectored thrust while the outer loop controller Failed to parse (SVG (MathML can be enabled via browser plugin): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle C_o} commands the roll angle to regulate the lateral position.
The following code imports the libraries that are required and defines the dynamics:
from matplotlib.pyplot import * # Grab MATLAB plotting functions from control.matlab import * # MATLAB-like functions
# 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)
# 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
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, Failed to parse (SVG (MathML can be enabled via browser plugin): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle H_i} , are given by
Hi = parallel(feedback(Ci, Pi), -m*g*feedback(Ci*Pi, 1));
Finally, we design the lateral compensator using another lead compenstor
# 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
L = Co*Hi*Po; S = feedback(1, L); T = feedback(L, 1);
The frequency response and Nyquist plot for the loop transfer function are computing using the commands
bode(L)); nyquist(L, (0.0001, 1000)); axis([-700, 5300, -3000, 3000]); gangof4(Hi*Po, Co);
The corresponding plots are shown below:
|
|
| ||
| Bode plot | Nyquist plot | Gang of 4 |
