|
| 1 | +# 3D Control of Quadcopter |
| 2 | +# based on https://github.com/juanmed/quadrotor_sim/blob/master/3D_Quadrotor/3D_control_with_body_drag.py |
| 3 | +# The dynamics is from pp. 17, Eq. (2.22). https://www.kth.se/polopoly_fs/1.588039.1550155544!/Thesis%20KTH%20-%20Francesco%20Sabatino.pdf |
| 4 | +# The linearization is from Different Linearization Control Techniques for |
| 5 | +# a Quadrotor System (many typos) |
| 6 | + |
| 7 | +import nonlinear_dynamics |
| 8 | +import argparse |
| 9 | +import numpy as np |
| 10 | +import scipy |
| 11 | +from scipy.integrate import odeint |
| 12 | +import matplotlib.pyplot as plt |
| 13 | +from mpl_toolkits.mplot3d import Axes3D |
| 14 | +from nonlinear_dynamics import g, m, Ix, Iy, Iz |
| 15 | + |
| 16 | + |
| 17 | +def lqr(A, B, Q, R): |
| 18 | + """Solve the continuous time lqr controller. |
| 19 | + dx/dt = A x + B u |
| 20 | + cost = integral x.T*Q*x + u.T*R*u |
| 21 | + """ |
| 22 | + # http://www.mwm.im/lqr-controllers-with-python/ |
| 23 | + # ref Bertsekas, p.151 |
| 24 | + |
| 25 | + # first, try to solve the ricatti equation |
| 26 | + X = np.matrix(scipy.linalg.solve_continuous_are(A, B, Q, R)) |
| 27 | + |
| 28 | + # compute the LQR gain |
| 29 | + K = np.matrix(scipy.linalg.inv(R) * (B.T * X)) |
| 30 | + |
| 31 | + eigVals, eigVecs = scipy.linalg.eig(A - B * K) |
| 32 | + |
| 33 | + return np.asarray(K), np.asarray(X), np.asarray(eigVals) |
| 34 | + |
| 35 | + |
| 36 | +parser = argparse.ArgumentParser( |
| 37 | + description='3D Quadcopter linear controller simulation') |
| 38 | +parser.add_argument( |
| 39 | + '-T', |
| 40 | + type=float, |
| 41 | + help='Total simulation time', |
| 42 | + default=10.0) |
| 43 | +parser.add_argument( |
| 44 | + '--time_step', |
| 45 | + type=float, |
| 46 | + help='Time step simulation', |
| 47 | + default=0.01) |
| 48 | +parser.add_argument( |
| 49 | + '-w', '--waypoints', type=float, nargs='+', action='append', |
| 50 | + help='Waypoints') |
| 51 | +parser.add_argument('--seed', help='seed', type=int, default=1024) |
| 52 | +args = parser.parse_args() |
| 53 | + |
| 54 | +np.random.seed(args.seed) |
| 55 | + |
| 56 | + |
| 57 | +# The control can be done in a decentralized style |
| 58 | +# The linearized system is divided into four decoupled subsystems |
| 59 | + |
| 60 | +# X-subsystem |
| 61 | +# The state variables are x, dot_x, pitch, dot_pitch |
| 62 | +Ax = np.array( |
| 63 | + [[0.0, 1.0, 0.0, 0.0], |
| 64 | + [0.0, 0.0, g, 0.0], |
| 65 | + [0.0, 0.0, 0.0, 1.0], |
| 66 | + [0.0, 0.0, 0.0, 0.0]]) |
| 67 | +Bx = np.array( |
| 68 | + [[0.0], |
| 69 | + [0.0], |
| 70 | + [0.0], |
| 71 | + [1 / Ix]]) |
| 72 | + |
| 73 | +# Y-subsystem |
| 74 | +# The state variables are y, dot_y, roll, dot_roll |
| 75 | +Ay = np.array( |
| 76 | + [[0.0, 1.0, 0.0, 0.0], |
| 77 | + [0.0, 0.0, -g, 0.0], |
| 78 | + [0.0, 0.0, 0.0, 1.0], |
| 79 | + [0.0, 0.0, 0.0, 0.0]]) |
| 80 | +By = np.array( |
| 81 | + [[0.0], |
| 82 | + [0.0], |
| 83 | + [0.0], |
| 84 | + [1 / Iy]]) |
| 85 | + |
| 86 | +# Z-subsystem |
| 87 | +# The state variables are z, dot_z |
| 88 | +Az = np.array( |
| 89 | + [[0.0, 1.0], |
| 90 | + [0.0, 0.0]]) |
| 91 | +Bz = np.array( |
| 92 | + [[0.0], |
| 93 | + [1 / m]]) |
| 94 | + |
| 95 | +# Yaw-subsystem |
| 96 | +# The state variables are yaw, dot_yaw |
| 97 | +Ayaw = np.array( |
| 98 | + [[0.0, 1.0], |
| 99 | + [0.0, 0.0]]) |
| 100 | +Byaw = np.array( |
| 101 | + [[0.0], |
| 102 | + [1 / Iz]]) |
| 103 | + |
| 104 | +####################### solve LQR ####################### |
| 105 | +Ks = [] # feedback gain matrices K for each subsystem |
| 106 | +for A, B in ((Ax, Bx), (Ay, By), (Az, Bz), (Ayaw, Byaw)): |
| 107 | + n = A.shape[0] |
| 108 | + m = B.shape[1] |
| 109 | + Q = np.eye(n) |
| 110 | + Q[0, 0] = 10. # The first state variable is the one we care about. |
| 111 | + R = np.diag([1., ]) |
| 112 | + K, _, _ = lqr(A, B, Q, R) |
| 113 | + Ks.append(K) |
| 114 | + |
| 115 | +######################## simulate ####################### |
| 116 | +# time instants for simulation |
| 117 | +t_max = args.T |
| 118 | +t = np.arange(0., t_max, args.time_step) |
| 119 | + |
| 120 | + |
| 121 | +def cl_linear(x, t, u): |
| 122 | + # closed-loop dynamics. u should be a function |
| 123 | + x = np.array(x) |
| 124 | + X, Y, Z, Yaw = x[[0, 1, 8, 9]], x[[2, 3, 6, 7]], x[[4, 5]], x[[10, 11]] |
| 125 | + UZ, UY, UX, UYaw = u(x, t).reshape(-1).tolist() |
| 126 | + dot_X = Ax.dot(X) + (Bx * UX).reshape(-1) |
| 127 | + dot_Y = Ay.dot(Y) + (By * UY).reshape(-1) |
| 128 | + dot_Z = Az.dot(Z) + (Bz * UZ).reshape(-1) |
| 129 | + dot_Yaw = Ayaw.dot(Yaw) + (Byaw * UYaw).reshape(-1) |
| 130 | + dot_x = np.concatenate( |
| 131 | + [dot_X[[0, 1]], dot_Y[[0, 1]], dot_Z, dot_Y[[2, 3]], dot_X[[2, 3]], dot_Yaw]) |
| 132 | + return dot_x |
| 133 | + |
| 134 | + |
| 135 | +def cl_nonlinear(x, t, u): |
| 136 | + x = np.array(x) |
| 137 | + dot_x = nonlinear_dynamics.f(x, u(x, t) + np.array([m * g, 0, 0, 0])) |
| 138 | + return dot_x |
| 139 | + |
| 140 | + |
| 141 | +if args.waypoints: |
| 142 | + # follow waypoints |
| 143 | + signal = np.zeros([len(t), 3]) |
| 144 | + num_w = len(args.waypoints) |
| 145 | + for i, w in enumerate(args.waypoints): |
| 146 | + assert len(w) == 3 |
| 147 | + signal[len(t) // num_w * i:len(t) // num_w * |
| 148 | + (i + 1), :] = np.array(w).reshape(1, -1) |
| 149 | + X0 = np.zeros(12) |
| 150 | + signalx = signal[:, 0] |
| 151 | + signaly = signal[:, 1] |
| 152 | + signalz = signal[:, 2] |
| 153 | +else: |
| 154 | + # Create an random signal to track |
| 155 | + num_dim = 3 |
| 156 | + freqs = np.arange(0.1, 1., 0.1) |
| 157 | + weights = np.random.randn(len(freqs), num_dim) # F x n |
| 158 | + weights = weights / \ |
| 159 | + np.sqrt((weights**2).sum(axis=0, keepdims=True)) # F x n |
| 160 | + signal_AC = np.sin(freqs.reshape(1, -1) * t.reshape(-1, 1) |
| 161 | + ).dot(weights) # T x F * F x n = T x n |
| 162 | + signal_DC = np.random.randn(num_dim).reshape(1, -1) # offset |
| 163 | + signal = signal_AC + signal_DC |
| 164 | + signalx = signal[:, 0] |
| 165 | + signaly = signal[:, 1] |
| 166 | + signalz = 0.1 * t |
| 167 | + # initial state |
| 168 | + _X0 = 0.1 * np.random.randn(num_dim) + signal_DC.reshape(-1) |
| 169 | + X0 = np.zeros(12) |
| 170 | + X0[[0, 2, 4]] = _X0 |
| 171 | + |
| 172 | +signalyaw = np.zeros_like(signalz) # we do not care about yaw |
| 173 | + |
| 174 | + |
| 175 | +def u(x, _t): |
| 176 | + # the controller |
| 177 | + dis = _t - t |
| 178 | + dis[dis < 0] = np.inf |
| 179 | + idx = dis.argmin() |
| 180 | + UX = Ks[0].dot(np.array([signalx[idx], 0, 0, 0]) - x[[0, 1, 8, 9]])[0] |
| 181 | + UY = Ks[1].dot(np.array([signaly[idx], 0, 0, 0]) - x[[2, 3, 6, 7]])[0] |
| 182 | + UZ = Ks[2].dot(np.array([signalz[idx], 0]) - x[[4, 5]])[0] |
| 183 | + UYaw = Ks[3].dot(np.array([signalyaw[idx], 0]) - x[[10, 11]])[0] |
| 184 | + return np.array([UZ, UY, UX, UYaw]) |
| 185 | + |
| 186 | + |
| 187 | +# simulate |
| 188 | +x_l = odeint(cl_linear, X0, t, args=(u,)) |
| 189 | +x_nl = odeint(cl_nonlinear, X0, t, args=(u,)) |
| 190 | + |
| 191 | + |
| 192 | +######################## plot ####################### |
| 193 | +fig = plt.figure(figsize=(20, 10)) |
| 194 | +track = fig.add_subplot(1, 2, 1, projection="3d") |
| 195 | +errors = fig.add_subplot(1, 2, 2) |
| 196 | + |
| 197 | +track.plot(x_l[:, 0], x_l[:, 2], x_l[:, 4], color="r", label="linear") |
| 198 | +track.plot(x_nl[:, 0], x_nl[:, 2], x_nl[:, 4], color="g", label="nonlinear") |
| 199 | +track.plot(signalx, signaly, signalz, color="b", label="command") |
| 200 | +if args.waypoints: |
| 201 | + for w in args.waypoints: |
| 202 | + track.plot(w[0:1], w[1:2], w[2:3], 'ro', markersize=10.) |
| 203 | +else: |
| 204 | + track.text(signalx[0], signaly[0], signalz[0], "start", color='red') |
| 205 | + track.text(signalx[-1], signaly[-1], signalz[-1], "finish", color='red') |
| 206 | +track.set_title( |
| 207 | + "Closed Loop response with LQR Controller to random input signal {3D}") |
| 208 | +track.set_xlabel('x') |
| 209 | +track.set_ylabel('y') |
| 210 | +track.set_zlabel('z') |
| 211 | +track.legend(loc='lower left', shadow=True, fontsize='small') |
| 212 | + |
| 213 | +errors.plot(t, signalx - x_l[:, 0], color="r", label='x error (linear)') |
| 214 | +errors.plot(t, signaly - x_l[:, 2], color="g", label='y error (linear)') |
| 215 | +errors.plot(t, signalz - x_l[:, 4], color="b", label='z error (linear)') |
| 216 | + |
| 217 | +errors.plot(t, signalx - x_nl[:, 0], linestyle='-.', |
| 218 | + color='firebrick', label="x error (nonlinear)") |
| 219 | +errors.plot(t, signaly - x_nl[:, 2], linestyle='-.', |
| 220 | + color='mediumseagreen', label="y error (nonlinear)") |
| 221 | +errors.plot(t, signalz - x_nl[:, 4], linestyle='-.', |
| 222 | + color='royalblue', label="z error (nonlinear)") |
| 223 | + |
| 224 | +errors.set_title("Position error for reference tracking") |
| 225 | +errors.set_xlabel("time {s}") |
| 226 | +errors.set_ylabel("error") |
| 227 | +errors.legend(loc='lower right', shadow=True, fontsize='small') |
| 228 | + |
| 229 | +plt.show() |
0 commit comments