LMIs in Control/Click here to continue/Applications of Non-Linear Systems/LMI-based State Feedback Design for Quadcopter Optimal path control and Tracking

Introduction
An LMI-based  state  feedback  approach  that  ensures optimum  path  tracking  and  improved  steady  state performance  of  a  quadrotor  in  both  translational  and rotational movements.

Quadcopter Dynamics
The motion of Quad Copter in 6DOF is controlled by varying the rpm of four rotors individually, thereby changing the vertical, horizontal and rotational forces.


 * ASSUMPTIONS:
 * 1) The structure is symmetric, thus the inertia matrices are diagonal.
 * 2) The center of mass corresponds to the origin of the physical coordinate system.
 * 3) A quadcopter is a rigid body.

State Space Representation


\begin{align} \dot x(t)&=Ax(t)+Bu(t)\\ y(t)&=Cx(t)+Du(t)\\ \end{align}$$

where x(t) is state vector, y(t) is output vector and u(t) is Input or control vector.

$$ \dot x(t)=\frac{d}{dt} x(t) $$
 * A is the system matrix
 * B is the input matrix
 * C is the output matrix
 * D is the feed forward matrix

Quadcopter modelling with 6 degree of freedom

 * REQUIRED 12 STATES:

The state vector x is $$ x^T= $$ $$ \begin{bmatrix} x & y  & z  & x' & y' & z' & \phi & \theta & \psi & \phi' & \theta' & \psi' \end{bmatrix} $$

The Input matrix u is, $$ u^T=$$$$ \begin{bmatrix} U1 & U2 & U3  & U4 \end{bmatrix} $$, where


 * U1 is the Total Upward Force on the quadrotor along z-axis ( T-mg)
 * U2 is the Pitch Torque (about x-axis)
 * U3 is the Roll Torque (about y-axis)
 * U4 is the Yaw Torque (about z-axis)

The Output matrix y is $$ y^T=$$$$ \begin{bmatrix} x & y & z & \phi & \theta & \psi \end{bmatrix} $$

The State differential equations written in matrix form as,

$$ \begin{bmatrix} x'\\ y'\\ z' \\ x''\\ y''\\ z''\\ \phi'\\ \theta'\\ \psi'\\ \phi''\\ \theta''\\ \psi''\\

\end{bmatrix}$$     = $$ \begin{bmatrix} 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0\\               0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0\\                0 & 0 & 0 & 0 & 0 & 0 & 0 & -g & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 0 & 0 & g & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\               0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0\\                0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0\\                0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1\\                0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\                0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\                0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\ \end{bmatrix} $$ $$ \begin{bmatrix} x\\ y\\ z \\ x'\\ y'\\ z'\\ \phi\\ \theta\\ \psi\\ \phi'\\ \theta'\\ \psi'\\

\end{bmatrix}$$ + $$ \begin{bmatrix} 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ \frac{1}{m} & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & \frac{1}{l_{x}} & 0 & 0 \\ 0 & 0 & \frac{1}{l_{y}} & 0 \\ 0 & 0 & 0 & \frac{1}{l_{z}} \\ \end{bmatrix} $$$$ \begin{bmatrix} U1\\ U2 \\ U3 \\ U4 \end{bmatrix} $$

The above martices represents the equation $$ x'=Ax+Bu$$

$$ \begin{bmatrix} x\\ y\\ z \\ \phi\\ \theta\\ \psi\\\end{bmatrix} $$=$$ \begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\               0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\                0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0\\                0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0\\                0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0\\ \end{bmatrix} $$$$ \begin{bmatrix} x\\ y\\ z \\ x'\\ y'\\ z'\\ \phi\\ \theta\\ \psi\\ \phi'\\ \theta'\\ \psi'\\

\end{bmatrix}$$+$$ \begin{bmatrix} 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ \end{bmatrix}$$$$ \begin{bmatrix} U1\\ U2 \\ U3 \\ U4 \end{bmatrix} $$

The above martices represents the equation $$ y=Cx+Du$$

LMI
$$ \begin{bmatrix} PA^T-W^TB^T+AP-BW+\mu^2\delta & -PA^T+W^TB^T-DP & I \\ PD^T-AP+BW & D^TP+PD & -P \\ I & -P & -\delta I \\ \end{bmatrix} <0 $$


 * $$K=WP^-1$$

Solving the above LMI yields the unknown coefficients of the feedback control. The system will be then asymptotically stable and path track will be achieved.

Conclusion
This LMI can be used to analyze the state feedback control and path tracking of a quadcopter.

Implementation
This LMI can be used in a problem and can be solved using the solvers like Yalmip,sedumi,gurobi etc,.