Advanced Autonomous Control for Quadrotor UAVs
Project overview
I assembled a quadcopter robot and designed algorithms to achieve autonomous hovering and motion control of the robot using motion capture systems and drone control software. I conducted tests on a real drone using my control algorithms. I tested both a linear control algorithm and an advanced SE3 (Special Euclidean Group SE(3)) control algorithm, and verified that the SE3 algorithm outperformed the linear algorithm. During the experiment, I first manually took off the drone using the remote controller. Once the drone was stable, I switched it to programmatic control. At this point, the drone's yaw would rotate 180 degrees, indicating the transition from manual mode to automatic hover mode.
Dynamic Model of Quadcopter Robot
Motor model: $\dot{\omega}_{i}=k_{m}\left(\omega_{i}^{d e s}-\omega_{i}\right)$. Single motor thrust: $F_{i}=k_{F} \omega_{i}^{2} = b\Omega_{i}^{2}$. Single motor torque: $M_{i}=k_{M} \omega_{i}^{2} = -d\Omega_{i}^{2}$. The system status:$X=[x\ \dot{x}\ y\ \dot{y}\ z\ \dot{z}\ \phi\ \dot{\phi}\ \theta\ \dot{\theta}\ \psi\ \dot{\psi}]$. System input:$u = [U_1 \ U_2 \ U_3 \ U_4]$。Among them, $U_1$ is the combined thrust, $U_2,U_3,U_4$ represents the three-axis torque of x, y, and z, respectively.
Through simple force analysis, the relationship between system input and speed can be obtained as follows, where $l$ is the distance between the center of the four axis aircraft and the center of the propeller, and $b$ and $d$ are the thrust and torque coefficients, respectively.
\[ \boldsymbol{u}=\left[\begin{array}{cccc} b & b & b & b \\ 0 & -b l & 0 & b l \\ b l & 0 & -b l & 0 \\ -d & d & -d & d \end{array}\right]\left[\begin{array}{l} \Omega_{1}^{2} \\ \Omega_{2}^{2} \\ \Omega_{3}^{2} \\ \Omega_{4}^{2} \end{array}\right] \]Solve the quadcopter dynamics model using the Newton Euler equation. The Newton's equation is as follows:
\[ m \ddot{\boldsymbol{p}}=\left[\begin{array}{c} 0 \\ 0 \\ -m g \end{array}\right]+\boldsymbol{R}\left[\begin{array}{c} 0 \\ 0 \\ F_{1}+F_{2}+F_{3}+F_{4} \end{array}\right] = \left[\begin{array}{c} 0 \\ 0 \\ -m g \end{array}\right]+\boldsymbol{R}\left[\begin{array}{c} 0 \\ 0 \\ U_1 \end{array}\right] \]Among them, R is the ZXY Euler angle rotation matrix, and after calculation:
\[ \boldsymbol{R}=\left[\begin{array}{ccc} c \psi c \theta-s \phi s \psi s \theta & -c \phi s \psi & c \psi s \theta+c \theta s \phi s \psi \\ c \theta s \psi+c \psi s \phi s \theta & c \phi c \psi & s \psi s \theta-c \psi c \theta s \phi \\ -c \phi s \theta & s \phi & c \phi c \theta \end{array}\right] \]The Euler equation is as follows:
\[ \boldsymbol{I}^{b} \cdot\left[\begin{array}{c} \dot\omega_{x} \\ \dot\omega_{y} \\ \dot\omega_{z} \end{array}\right]+\left[\begin{array}{c} \omega_{x} \\ \omega_{y} \\ \omega_{z} \end{array}\right] \times \boldsymbol{I}^{b} \cdot\left[\begin{array}{c} \omega_{x} \\ \omega_{y} \\ \omega_{z} \end{array}\right]=\left[\begin{array}{c} U_2 \\ U_3 \\ U_4 \end{array}\right] \]Among them, $\ boldsymbol {I} ^ {b}$ is the inertia tensor matrix of the quadcopter aircraft with respect to the body coordinate system.
\[ \boldsymbol{I}^{b}=\left[\begin{array}{ccc} I_x & 0 & 0 \\ 0 & I_y &0\\ 0 &0 &I_z \end{array}\right] \]The conversion between the derivative of instantaneous angular velocity and Euler angle is as follows:
\[ \left[\begin{array}{c} \omega_{x} \\ \omega_{y} \\ \omega_{z} \end{array}\right]=\left[\begin{array}{ccc} c \theta & 0 & -c \phi s \theta \\ 0 & 1 & s \phi \\ s \theta & 0 & c \phi c \theta \end{array}\right]\left[\begin{array}{l} \dot{\phi} \\ \dot{\theta} \\ \dot{\psi} \end{array}\right] \]Combining the Newton Euler equations, we can solve the dynamic model of a quadcopter aerial robot as follows:
\[ \begin{array}{l} M=-M_{1}+M_{2}-M_{3}+M_{4} \\ \ddot{x}=(\sin \psi \sin \phi+\cos \phi \sin \theta \cos \psi) \frac{U_{1}}{m} \\ \ddot{y}=(-\cos \psi \sin \phi+\sin \psi \sin \theta \cos \phi) \frac{U_{1}}{m} \\ \ddot{z}=-g+(\cos \theta \cos \phi) \frac{U_{1}}{m} \\ \ddot{\phi}=\frac{I_{y}-I_{z}}{I_{x}} \dot{\theta} \dot{\psi}-\frac{J_{r}}{I_{x}} \dot{\theta} M+\frac{U_{2}}{I_{x}} \\ \ddot{\theta}=\frac{I_{z}-I_{x}}{I_{y}} \dot{\phi} \dot{\psi}+\frac{J_{r}}{I_{y}} \dot{\phi} M+\frac{U_{3}}{I_{y}} \\ \ddot{\psi}=\frac{I_{x}-I_{y}}{I_{z}} \dot{\phi} \dot{\theta}+\frac{\mathrm{U}_{4}}{I_{z}} \end{array} \]Nonlinear Control
1. SE3 Control
Differential flattening idea: It is possible to take a finite number of flat outputs and their derivatives as differential parameters to represent the original state variable x and input variable u.
In quadcopters, (x, y, z, yaw) is used as the flat output to represent quadcopter $\mathbf{X}=\left[x, y, z, \phi, \theta, \psi, \dot{x}, \dot{y}, \dot{z}, \omega_{x}, \omega_{y}, \omega_{z}\right]^{T}$
The overall control framework and the definition of heading are shown in Figures 3 and 4, respectively. The four control variables of SE3 control itself are thrust in the z-axis direction and torque in the x, y, and z directions.
2. Trajectory Tracking
Define position and velocity error: $e_p = r-r_T, \quad e_v= \dot{r} - \dot{r}_T$. Calculate the expected thrust: $F_{des}=-K_pe_p - K_ve_v+mgz_W + m\ddot{r}_T $. Project the calculated thrust onto the z-axis of the current actual aircraft coordinate system as input: $u_1 = F_{des} z_B$.
3. Attitude Tracking
According to the heading definition shown in Figure 4, the expected attitude can be calculated:
\[z_{B,des}=\frac{F_{des}}{\| F_{des} \|}\] \[x_{C,des} = [cos(\Psi_T), sin(\Psi_T), 0]^T\] \[y_{B,des}=\frac{z_{B,des} \times x_{c,des}}{\| z_{B,des} \times x_{c,des} \|}\] \[x_{B,des}= y_{B,des} \times z_{B,des}\] \[R_{B,des}= \begin{bmatrix} x_{B,des} & y_{B,des} & z_{B,des} \end{bmatrix}\]
Coordinate Transformation
In this experiment, there are three coordinate systems, "body coordinate system", "world coordinate system", and "IMU northeast coordinate system".
1. $x_{des},y_{des},z_{des},\Phi_{des}, \Theta_{des}, \Psi_{des}, \dot{x}, \dot{y}, \dot{z}$, which are in the expected set values $s_{des}$, are in the world coordinate system
2. In the real machine inner loop interface $[u_1, \Phi_{des}, \Theta_{des}, \Psi_{des}]$, $u_1$ needs to be in the dynamic capture world coordinate system, and $[\Phi_{des}, \Theta_{des}, \Psi_{des}]$ needs to be located in the IMU northeast coordinate system
3. The attitudes that can be obtained in real aircraft include the odometer attitude representation $^WR_B$ after EKF fusion with the IMU, the IMU attitude representation $^ER_B$ obtained directly from the IMU in the northeast coordinate system, and the expected attitude representation $^WR_{B\prime}$ in the world coordinate system that we can obtain through linearization formulas or differential flattening transformation.
Based on the above, the attitude conversion formula is: \[ ^ER_{B\prime} = ^ER_B (^WR_B)^{-1} {}^WR_{B\prime} \]
Results and Analysis
| simulation data | real data | |||
|---|---|---|---|---|
| overshoot(%) | adjust time(s) | rise time(s) | Average steady-state error of xyz axis(m) | |
| linear control | 4.02 | 7.43 | 1.88 | 0.067 |
| SE3 control | 0.005 | 2.53 | 2.28 | 0.024 |
It is evident from table that nonlinear control achieves better hover control performance.
Linear control assumes that the drone is in a balanced hovering state, and linearizes the approximation. This algorithm is only applicable to the control of balanced hovering of drones, while considering drones as particles, and cannot take into account the influence of wind resistance and other factors in the control. However, SE3 control has not undergone linearization approximation and is suitable for controlling any attitude of unmanned aerial vehicles, taking into account the effects of wind resistance and other factors. Therefore, SE3 control can achieve better results than linear control.