Description

24-677 Special Topics: Modern Control – Theory and Design
Prof. D. Zhao
• Your online version and its timestamp will be used for assessment.
• We will use Gradescope to grade. The link is on the panel of CANVAS. If you are confused about the tool, post your questions on Campuswire.
• Submit lqr controller.py and adaptive controller.py to Gradescope under P5code and your solutions in .pdf format to P5-writeup. Insert the output tracking performance plot image in the .pdf. We will test your controller and manually check all answers.
• We will make extensive use of Webots, an open-source robotics simulation software, for this project. Webots is available here for Windows, Mac, and Linux.
• Please familiarize yourself with Webots documentation, specifically their User Guide and their Webots for Automobiles section, if you encounter difficulties in setup or use. It will help to have a good understanding of the underlying tools that will be used in this assignment. To that end, completing at least Tutorial 1 in the user guide is highly recommended. • If you have issues with Webots that are beyond the scope of the documentation (e.g. the software runs too slow, crashes, or has other odd behavior), please let the TAs know via Campuswire. We will do our best to help.
• We advise you to start with the assignment early. All the submissions are to be done before the respective deadlines of each assignment. For information about the late days and scale of your Final Grade, refer to the Syllabus in Canvas.
1 Introduction
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).
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 DJITM 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.
2 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
2.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,Ix,Iy, and Iz are the mass and moments of inertial of the quadrotor respectively; and JR and ΩR are the moment of inertia and angular velocity(i.e. rotor speed) of the propeller blades. U1 is the collective thrust, U2,U3, and U4 are the roll, pitch and yaw torques generated by the four propellers.
2.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 U1 = mg + ∆U1, 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.
2.3 Physical Parameters
All relevant physical parameters are listed here. The definitions of d1x, d2x, d1y and d1y are showd in Figure 2.

Figure 2: Definition of d1x, d2x, d1y and d1y
Table 1: Model parameters.
Name Description Unit Value
(X,Y,Z) Vehicle’s coordinates in the world frame m State
(x,˙ y,˙ z˙) Vehicle’s 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 mˆ2 0.000913855
Iy Moment of inertia around y-axis kg mˆ2 0.00236242
Iz Moment of inertia around z-aixs kg mˆ2 0.00279965
ct thrust coefficient N 0.00026
cτ torque coefficient kg mˆ2 5.2e-06
∆T Simulation timestep sec 0.01
3 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.
3.1 Problem statement
x˙p = Apxp + BpΛu (1)
where Bp ∈ Rnp×m is constant and known, Ap ∈ Rnp×np is constant and unknown, xp ∈ Rnp, u ∈ Rm, Λ ∈ Rm×m is an unknown diagonal positive definite matrix. The state xp = [x,y,z,φ,θ,ψ,x,˙ y,˙ z,˙ φ,˙ θ,˙ ψ˙]. The control input u = [∆U1,U2,U3,U4]. The goal is to track a reference command r(t) ∈ Rm in the presence of the unknown Ap, 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
ey = yp − r
Augmenting (1) with the integrated output tracking error,
Z eyI = eydt, e˙yI = ey
leads to the extended open loop dynamics
x˙ = Ax + BΛu + Bcr (2)
where is the extended system state vector. The extended open-loop system
matrices are given by

and the extended system output (notice that we abuse the notations here: the output y is different from the the y positions of the center of mass in the inertial frame)
y = [Cp 0m×m]x = Cx
3.2 Baseline LQR Controller
A baseline LQR controller ubl = Kblx
can be designed for the system in (2) in the case where there is no model discrepancy, i.e.,
Λ = Im×m and Areal is taken at some nominal value A¯ where actually Areal = A¯+A∗ and A∗ contains all the unmodeled terms (A¯ is the A in section 3.1). The feedback gains Kbl can be selected using standard LQR design techniques.
3.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 adaptive rate of the control gain is given by
˙
where Γ is a diagonal positive definite matrix, e = x − xm 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 = eTPe + Tr(θ˜TΓ−1θ˜)
where θ˜ = θˆ−θ is the control gain estimation error, it can be shown [7] that the derivative of the Lyapunov function candidate is given by
V˙ = −eTPe ≤ 0
The system is globally asymptotically stable and the tracking error asymptotically converges to 0, that is
lim e(t) = 0 t→∞
4 P5: Problems
• The physical parameters are defined in the base controller.py.
• Make sure to discretize your continuous state-space system with the provided discrete timestep (self.delT) 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. To help you started the tuning, you can take a look at the Example code part of the comment in the code base.
– 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)2
in order to normalize the penalty weights.
Exercise 2.
1. (30 points) (all the files for this exercise are in ex2 folder) Design a MRAC to fly the quadrotor with 50% lost of thrust in one motor. The motor failure happens after 14s in simulation. 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 refer to 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 in one motor, you will get full score. Otherwise, you will get (your percentage lost of thrust)/50 ∗ 30 points.
To get the correct plot, you first need to copy your lqr controller.py from Exercise 1 to the folder of Exercise 2 (ex2/controllers/ex2 controller/). Run the code with LQR controller and obtain the saved data for LQR controller. Follow the instruction below to run either the LQR or adaptive controller again to obtain the data and the plot. Every time you change the percentage lost of thrust, you need to repeat this process.
In order to change the percentage lost of thrust, go to ex2 controller.py and change the value of the variable lossOfThust (line 24 of ex2 controller.py) to your preferred percentage lost of thrust. If you want to change which controller to use, edit line 27 of ex2 controller.py to either customController = AdaptiveController(driver, lossOfThust) or customController = LQRController(driver, lossOfThust)
2. (20 points) Find a percentage lost of thrust such that LQR fails (the drone will fall onto the floor) but MRAC is still able to maintain tracking. Report the percentage lost of thrust and attach the plot.
Tips and Warnings:
• Initialize the gain matrix to your LQR gain. It will help you stabilize the system.
5 Appendix
5.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 ct 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.
T = ctω2 τ = cτω2
The control input U1 is simply the sum of the thrust of all propellers, and the row, pitch torque U2,U3 are the sum of torque generated by the opposite propellers around certain axes. The yaw torque U4 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 = [U1 U2 U3 U4]T as:

Inversely, to get the desired motor speeds from control input, we use the inverse of H matrix.
U = HΩ → Ω = H−1U.
5.2 Simulating Actuator failure
To simulate around 50% lost of thrust in motor number 1, the control input to be used for the simulation Ufail is given by:
0.7
Ufail = H  00

0 0
1
0
0 0 0
1
0
6 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.