In the previous projects, we have learned how to apply modern control techniques to an Unmanned Ground Vehicle (UGV). In this project, we will step up the difficulty and investigate how to control an Unmanned Aerial Vehicle (UAV).

Quadrotor drones has risen in popularity in recent year for a variety of uses, such as aerial photography for topology and agriculture, search and rescue missions, and product delivery. Ever-increasing demands on safety and performance have necessitated the development of more and more sophisticate flight control systems. Unlike ground vehicle, these flying vehicles could be inherently unstable due to many sources of uncertainty such as actuator degradation, external disturbances, and complex unmodeled dynamics (blade flapping and asymmetric angular speed of propellers), which often results in a crash.

In this project, we will make use of adaptive control theory to augment a LQR-based

MPC controller and test its effectiveness in the event of a single quadrotor motor of a DJI^TM Mavic 2 Pro experience a 50% pr higher percent lost of thrust during hovering. As before, the experiment will be performed in the Webots simulator.

This project is the last part of the course project series:

P5 (a) Design a baseline LQR controller to fly the quadrotor without any motor failure.

(b) Design a MRAC to fly the quadrotor with 50% lost of thrust in one motor.

1 Model

We will use a popular model of the quadrotor as the control model. As shown in Figure 1, the inertial frame is fixed to the ground. The roll axis of the quadrotor is pointing forward w.r.t. to the drone body. The pitch axis is pointing to the left. The yaw axis is pointing upwards. We will present the simplified nonlinear dynamics model and linearized dynamics model in Section 2.1 and 2.2. The model parameters are defined in Table 1.

Figure 1: Coordinate systems

1.1 Nonlinear dynamics

The dynamics of quadrotor helicopters have been studied in detail by many researchers. Assuming the vehicle as a rigid-body and operating at low speed, the dynamics are given by:

where x,y, and z are the positions of the center of mass in the inertial frame; ,, and are the Euler angles which describe the orientation of the body-fixed frame with respect to the inertial frame; m,I_x,I_y, and I_z are the mass and moments of inertial of the quadrotor respectively; and J_R and _R are the moment of inertia and angular velocity(i.e. rotor speed) of the propeller blades. U₁ is the collective thrust, U₂,U₃, and U₄ are the roll, pitch and yaw torques generated by the four propellers.

1.2 Linear dynamics

Since the quadrotor typically operates very near the hover position, we can linearize the system by making small angle approximations, neglecting higher order terms and letting U₁ = mg + U₁, which will result in the linear dynamics:

Although this linear system seems a bit over simplified than the nonlinear dynamics, it still captures the dominant features of the quadrotor, and is sufficient near the hover position. As expected, the roll, pitch and yaw input command moments about their respective axes and the collective input commands acceleration in the positive z-direction. Accelerations in the x and y-directions are achieved primarily through vectoring the collective thrust.

For the purposes of the controller design, we will use the linearized dynamics.

1.3 Physical Parameters

All relevant physical parameters are listed here. The definitions of d_1x, d_2x, d_1y and d_1y are showd in Figure 2.

Figure 2: Definition of d_1x, d_2x, d_1y and d_1y

Table 1: Model parameters.

Name	Description	Unit	Value
(X,Y,Z)	Vehicles coordinates in the world frame	m	State
(x, y, z)	Vehicles velocity along the direction of vehicle frame	m/s	State
(,,)	Body row, pitch, yaw angle	rad	State
	Body row, pitch, yaw angle rate	rad/s	State
	Vehicle mass	kg	0.4
d1x	Length in x-axis from front rotors to the center of mass	m	0.1122
d2x	Length in x-axis from rear rotors to the center of mass	m	0.1171
d1y	Length in y-axis from front rotors to the center of mass	m	0.1515
d2y	Length in x-axis from rear rotors to the center of mass	m	0.1280
Ix	Moment of inertia around x-axis	kg m2	0.000913855
Iy	Moment of inertia around y-axis	kg m2	0.00236242
Iz	Moment of inertia around z-aixs	kg m2	0.00279965
ct	thrust coefficient	N	0.00026
c	torque coefficient	kg m2	5.2e-06
T	Simulation timestep	sec	0.01

2 Controllers

In order to access the effectiveness of adaptive controller in the presence of model uncertainty, We will use a LQR controller as the baseline controller for comparison.

2.1 Problem statement

The primary function of the adaptive controller is to accommodate any uncertainties which may arise in the dynamics. We can write the equations of motion in section (2.2) along with these uncertainties as

x_p = A_px_p + B_pu (1)

where B_p R^npm is constant and known, A_p R^npnp is constant and unknown, x_p R^np, u R^m, R^mm is an unknown diagonal positive definite matrix. The state x_p = [x,y,z,,,,x, y, z, , , ]. The control input u = [U₁,U₂,U₃,U₄]. The goal is to track a reference command r(t) R^m in the presence of the unknown A_p, and . We define the system output as

yp = Cpxp

In the case of the quadrotor, the output states are x, y, z, and m = 4. The output tracking error is then given by

e_y = y_p r

Augmenting (1) with the integrated output tracking error,

Z eyI = eydt, eyI = ey

leads to the extended open loop dynamics

x_t = A_tx_t + B_tu + B_cr (2)

where is the extended system state vector. The extended open-loop

system matrices are given by

and the extended system output

yt = [Cp 0mm]xt = Ctxt

2.2 Baseline LQR Controller

A baseline LQR controller ubl = Kblxt

can be designed for the system in (2) in the case where there is no model discrepancy, i.e., = I_mm and A is taken at some nominal value A where actually A = A + A and A contains all the unmodeled terms. The feedback gains K_bl can be selected using standard LQR design techniques.

2.3 Model Reference Adaptive Controller

The reference model used by MRAC is the closed loop system given by (2), again in the case of no uncertainty, along with the control input in Section 3.2

(3)

We can design an adaptive control input

(4)

such that the resulted system (5) can track the reference model in (3)

(5) The canonical adaptive law is given by

= e^TPB_t

where is a diagonal positive definite matrix of adaptive gains, e = xx_m is the model tracking error, and P is the unique symmetric positive definite solution of the Lyapunov equation

where Q is also symmetric positive definite.

The augmented structure of the adaptive controller implies that in the nominal case, i.e., the case with no parameter uncertainty, the overall system is equivalent to the baseline control. However, when failures or other uncertainties arise, the adaptive controller works to assist the baseline controller in maintaining stability and performance. With the Lyapunov function candidate given by

V = e^TPe + Tr(^T1)

where = is the parameter estimation error, it can be shown [7] that the derivative of the Lyapunov function candidate is given by

V = e^TPe 0

The system is globally asymptotically stable and the tracking error asymptotically converges to 0, that is

lim e(t) = 0 t

3 P5: Problems

Exercise 1. (50 points) Design a discrete-time infinite horizon LQR controller for the quadrotor to track the command when there is no motor failure. You should complete the code in lqr controller.py. Attach the plot generated by completing 60s simulation and report the error generated at the end of simulation in your write-up. If your error is lower than lower bar, you will receive full score. Otherwise your score will be calculated as (upper baryour error)/(upper barlower bar)50 where lower bar is 0.5 and upper bar is 0.8.

You are strongly recommended to go through the code base provided. Some hints that may be useful: Make sure to discretize your continuous state-space system with the provided timestep (T) prior to solving the ARE.

• Using LQR requires designing the Q and R matrices. Q penalizes state variables, while R penalizes control input. Try to think about what form your Q and R matrices should take for good performance.
• For Q, large values will restrict changes to the respective states, while smaller values will allow the states to easily change.
• Similarly, in R, larger values will restrict control input, while smaller values will allow the control input to vary more.
• Empirically, it is reasonable to set the entries in the Q and R to be

1

(max value of the corresponding state/input)²

in order to normalize the penalty weights.

Exercise 2.

1. (30 points) Design a MRAC to fly the quadrotor with 50% lost of thrust in one motor. You should complete the code in adaptive controller.py. Attach the plot generated by completing 60s simulation and report your percentage lost of thrust. Please see Appendix 1 for details on how we simulate motor failure. If your LQR and MRAC controller can stablize the system with 50% lost of thrust, you will get full score. Otherwise, you will get (your percentage lost of thrust)/50 30 points.
2. (20 points) Find a percentage lost of thrust such that LQR fails but MRAC works. Report the percentage lost of thrust and attach the plot.

In order to change the percentage lost of thrust, go to AdaptiveController.py (not adaptive controller.py) and change the value of the variable lossOfThust to your percentage lost of thrust. If you want to change which controller to use, edit line 28 of AdaptiveController.py to either customController = AdaptiveController(driver, lossOfThust) or

customController = LQRController(driver, lossOfThust)

Some hints that may be useful:

• Initialize the gain matrix to your LQR gain. It will help you stabilize the system.

4 Appendix

4.1 Conversion from control input to rotor speed

For our quadrotor model in webots, the propeller turns the motor angular velocity into a thrust and a (resistant) torque. The resultant thrust T of each motor is given by the product of thrust constants c_t and the square of the angular velocity, where as the resultant torque is given by the product of torque coefficient c and the the square of the angular velocity.

• = c_t² = c²

The control input U₁ is simply the sum of the thrust of all propellers, and the row, pitch torque U₂,U₃ are the sum of torque generated by the opposite propellers around certain axes. The yaw torque U₄ is the sum of the torque generated by all the propellers around the z-aixs.

Hence, we can write the conversion matrix H from rotor speed = [ to U = [U₁ U₂ U₃ U₄]^T as:

Inversely, to get the desired motor speeds from control input, we use the inverse of H matrix.

• = H = H¹U.

4.2 Simulating Actuator failure

To simulate around 50% lost of thrust in motor number 1, the control input to be used for the simulation U_fail is given by:

0.7Ufail = H 000

0100

0 010

5 Reference

[1] Dydek, Zachary T., Anuradha M. Annaswamy, and Eugene Lavretsky. Adaptive control of quadrotor UAVs: A design trade study with flight evaluations. IEEE Transactions on control systems technology 21.4 (2012): 1400-1406.