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*are the mass and moments of inertial of the quadrotor respectively; and

_{z }*J*and Ω

_{R }*are the moment of inertia and angular velocity(i.e. rotor speed) of the propeller blades.*

_{R }*U*

_{1 }is the collective thrust,

*U*

_{2}

*,U*

_{3}

*,*and

*U*

_{4 }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*_{1 }= *mg *+ ∆*U*_{1}, 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) |
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 |

I_{x} |
Moment of inertia around x-axis | kg mˆ2 | 0.000913855 |

I_{y} |
Moment of inertia around y-axis | kg mˆ2 | 0.00236242 |

I_{z} |
Moment of inertia around z-aixs | kg mˆ2 | 0.00279965 |

c_{t} |
thrust coefficient | N | 0.00026 |

c_{τ} |
torque coefficient | kg mˆ2 | 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*+

_{p}x_{p }*B*Λ

_{p}*u*(1)

where *B _{p }*∈

*R*

^{n}

^{p}^{×m }is constant and known,

*A*∈

_{p }*R*

^{n}

^{p}^{×n}

*is constant and unknown,*

^{p }*x*∈

_{p }*R*

^{n}*,*

^{p}*u*∈

*R*, Λ ∈

^{m}*R*

^{m}^{×m }is an unknown diagonal positive definite matrix. The state

*x*= [

_{p }*x,y,z,φ,θ,ψ,x,*˙

*y,*˙

*z,*˙

*φ,*

^{˙ }

*θ,*

^{˙ }

*ψ*

^{˙}]. The control input

*u*= [∆

*U*

_{1}

*,U*

_{2}

*,U*

_{3}

*,U*

_{4}]. The goal is to track a reference command

*r*(

*t*) ∈

*R*in the presence of the unknown

^{m }*A*, and Λ. We define the system output as

_{p}*y**p *= *C**px**p*

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 *e**yI *= *e**ydt, e*˙*yI *= *e**y*

leads to the extended open loop dynamics

*x*˙* _{t }*=

*A*+

_{t}x_{t }*B*Λ

_{t}*u*+

*B*(2)

_{c}rwhere is the extended system state vector. The extended open-loop

system matrices are given by

and the extended system output

*y**t *= [*C**p *0*m*×*m*]*x**t *= *C**tx**t*

## 2.2 Baseline LQR Controller

A baseline LQR controller *u**bl *= *K**blx**t*

can be designed for the system in (2) in the case where there is no model discrepancy, i.e., Λ = *I _{m}*

_{×m }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*can be selected using standard LQR design techniques.

_{bl }## 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 ^{T}PB_{t}*

*θ*

where Γ is a diagonal positive definite matrix of adaptive gains, *e *= *x*−*x _{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 ^{T}Pe *+

*Tr*(

*θ*

^{˜T}Γ

^{−1}

*θ*

^{˜})

where *θ*^{˜ }= *θ*^{ˆ}−*θ *is the parameter estimation error, it can be shown [7] that the derivative of the Lyapunov function candidate is given by

*V*^{˙ }= −*e ^{T}Pe *≤ 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 bar*−*your **error*)*/*(*upper bar*−*lower **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**)^{2}

in order to normalize the penalty weights.

**Exercise 2.**

**(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.**(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}ω^{2 }*τ*=*c*_{τ}ω^{2}

The control input *U*_{1 }is simply the sum of the thrust of all propellers, and the row, pitch torque *U*_{2}*,U*_{3 }are the sum of torque generated by the opposite propellers around certain axes. The yaw torque *U*_{4 }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*_{1 }*U*_{2 }*U*_{3 }*U*_{4}]* ^{T }*as:

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

- =
*H*Ω → Ω =*H*^{−1}*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.

## Reviews

There are no reviews yet.