LMIs in Control/Click here to continue/Applications of Linear systems/LPV state-feedback controller for Attitude and Altitude stabilization of a mass-varying quadcopter

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.

Derivations

 * Roll motion

The torque about x-axis is modeled as $$\tau _{x} $$i.e. rolling moment. Now r (moment arm) is distance of individual forces from x-axis

$$\tau _{x} =\sum_{i=1}^{4} r_{i}f_{i} $$

$$\tau _{x}= r_{1} f_{1}+r_{2} f_{2}+r_{3} f_{3}+r_{4} f_{4}$$

As the rotors 1 and 3 are placed along with body x-axis so their moment arm is zero and they do not contribute in rolling moment

$$ r_{2}=d;r_{4}=-d$$

so $$ \tau_{x}=d_{f2}-d_{f4}$$

Thus, when rotor 4 applies force it tends to rotate the system about x-axis opposite to curl of fingers so using the right hand rule (RHR) its torque contribution in x-axis is negative.

Similarly the rotor 2 produces positive torque about x-axis according to Right Hand Rule.


 * Pitch motion

The torque about y-axis is modeled as i.e. pitching moment. Now r(moment arm) is distance of individual forces

$$\tau _{}= r_{1} f_{1}+r_{2} f_{2}+r_{3} f_{3}+r_{4} f_{4}$$

where r is the distance of individual force form vertical axis

$$r_{2}=r_{4}=0; r_{1}=-r_{3}=d$$

so \tau_{y}=d_{f1}-d_{f3}

when rotor 3 applies force it tends to rotate the system about y-axis opposite to curl of fingers.so using Right Hand rule its torque contribution in y-axis is negative. Similarly the rotor 1 produces positive torque about x-axis according to Right Hand Rule.


 * Yaw motion

Finally the yawing moment is modeled as $$\tau_{z}$$ and its torque about Z axis.As the motors rotate the rotors encounter a reactive torque due to air drag on rotor blade, this reactive torque is modeled as Q.The yaw is accomplished by rotor rpm imbalance between clockwise and counter-clockwise rotating propellers. The reactive torque as a convention is taken positive for counter clockwise rotating motors/rotors and negative for clockwise rotating ones. All the four rotors contribute in yawing moment.

$$\tau _{z} =\sum_{i=1}^{4} Q_{i}f_{i} $$

Whereas $$Q=cF_{i}$$ if rotation of the ith rotor is counterclockwise and $$-cF_{i}$$, if clockwise. So opening the summation becomes:

$$ \tau_{z}=c(-F_{1}+F_{2}-F_{3}+F_{4})$$


 * Vertical motion

The vertical motion is governed by the total thrust produced

$$T =\sum_{i=1}^{4} F_{i} $$

$$Z=\frac{T-mg}{m}$$


 * Horizontal motion

The horizontal motion in the x & y-axes is accomplished by decreasing the rpm of the rotor in whose direction it is intended to move, and subsequently increasing the rpm of the rotor on the opposite side of the same arm.

The force imbalance will cause the quad copter to tilt to one side and the horizontal component of the thrust force will impart horizontal linear motion to the quadcopter.

If the mass of the quad copter is ‘m’, then referring to (figure 4), the linear acceleration in the horizontal x-direction can be written as:

$$ x=-\frac{\tau cos\theta}{m}$$

using small angle approximation $$sin\theta \approx. 0 $$ and as 0 is small $$T \approx. mg$$, $$x=-g\theta,y=g\theta$$

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 for stability Analysis
$$ \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.

LPV Attitude State Feedback Controller Design
To design a $$ H\infty $$ LPV feedback control scheme for the altitude/attitude stabilization of the quadrotor aircraft,


 * Output $$y=\begin{bmatrix} \phi & \theta& \psi&  \Z \end{bmatrix}^T$$ of the system must track r =$$\begin{bmatrix} \phi_{ref} & \theta_{ref}&  \psi_{ref}&  \Z_{ref} \end{bmatrix}^T$$, a reference trajectory.


 * To achieve the objective outputs of the integrator are considered as extra state variables $$x_{e}=\begin{bmatrix}x_{\phi} & x_{\theta}& x_{\psi}&  x_{Z} \end{bmatrix}^T$$ as

$$x_{\phi}=\int_{0}^{t} e_{\phi} (\delta) d\delta \,dx ,e_{\phi}=\phi_{ref}-\phi$$

$$x_{\theta}=\int_{0}^{t} e_{\theta} (\delta) d\delta \,dx ,e_{\theta}=\theta_{ref}-\theta$$

$$x_{\psi}=\int_{0}^{t} e_{\psi} (\delta) d\delta \,dx ,e_{\psi}=\psi_{ref}-\psi$$

$$x_{z}=\int_{0}^{t} e_{z} (\delta) d\delta \,dx ,e_{z}=z_{ref}-z$$

In this case the error signal e=y-r ,for the outputs $$ U_{1},U_{2},U_{3},U_{4}$$,the weight functions $$ W_{ui},i=1,2,3,4 $$are added to the system. The system matrices of weight functions are $$A_{ui},B_{ui},C_{ui}and D_{ui}.$$

The dynamic of all the weight functions $$ W_{u1},W_{u2},W_{u3},W_{u4}$$ can be constituted as,

\begin{align} \dot{x}_{u}=A_{u}x_{u}+B_{u}u\\ y_{u}=C_{u}x_{u}+D_{u}u \end{align}$$

where $$x_{u}=\begin{bmatrix} x_{u1}&x_{u2}&x_{u3}&x_{u4}\end{bmatrix}^T$$ is the state,$$u=\begin{bmatrix}U_{1} & U_{2}&U_{3}&U_{4}\end{bmatrix}^T$$ represents the input $$y_{u}=\begin{bmatrix} Z_{1}&Z_{2}&Z_{3}&Z_{4}\end{bmatrix}^T$$

$$\Delta_{u}=\begin{bmatrix} \Delta_{u1}&0&0&0\\0&\Delta_{u2}&0&0\\0&0&\Delta_{u3}\\0&0&0&\Delta_{u4}\end{bmatrix}$$,$$\Delta\in{A,B,C,D}$$
 * The system matrices of the weight function can be deducted as,

$$w=\begin{bmatrix} r&d\end{bmatrix}^T$$,$$z=\begin{bmatrix} y_{u}&e\end{bmatrix}^T$$,and $$\bar{x}=\begin{bmatrix} x&x_{e}&x_{u}\end{bmatrix}^T$$ are the exogenous input and exogenous output respectively.


 * The system differential equations with augumented ststes and weight functions are,

\begin{align} \dot{\bar x}=\sum_{i=1}^{16} \mu_{i} (\bar A_{i}\bar x+\bar B1_{i}w+\bar B_{2}u)\\ z=C_{\bar x}+D11 w+D12 u \end{align}$$

where,

$$\bar A_{i}=\begin{bmatrix} \bar A_{i} &0&0&0\\ -C&0 &0\\0 &0&A_{u} \end{bmatrix}$$;

$$ \bar B_{1}=\begin{bmatrix} 0 & \bar E_{i}\\ -I_{4} & 0\\0 &0 \end{bmatrix}$$

$$ \bar B_{2}=\begin{bmatrix} \bar B_{i}\\ 0\\ B_{u}\end{bmatrix}$$

$$ C_{1}=\begin{bmatrix} 0& 0& C_{u}\\ C &0&0\end{bmatrix}$$

$$ D_{11}=\begin{bmatrix} 0 &0\\-I_{4}&0\end{bmatrix}$$

$$ D_{12}=\begin{bmatrix} D_{u}\\0\end{bmatrix}$$

Making the closed loop system,

$$dot{\bar x}=\sum_{i=1}^{16} \mu_{i} ((\bar A_{i}+\bar B_{2}ki)\bar x(t)+\bar B_{1}w)$$

LMI for H∞ optimal state-feedback
$$ X=X^T>0$$

$$\begin{bmatrix} He(\bar A_{i}X+\bar B_{2}Y_{i} &(*)^T &(*)^T\\ \bar B_{1i} ^T & -\gamma I&(*)^T \\ \bar C_{1i}X+D_{12}Y_{i}& \bar D_{11} &-\gamma I \end{bmatrix}<0 $$

$$He(\bar A_{i}X+\bar B_{2}Y_{i}+2\alpha X <0$$ ; i=1 to 16

By solving the LMI,the optimal H\inf state feed-back controller with the smallest attenuation level \gamma >0 for the attitude/altitude subsystem of the mass-varying quadcopter is $$K(\rho)=\sum_{i=1}^{16} \mu_{i} Y_{i} X^-1$$

Conclusion
This LMIs can be used model the state-feedback controller for attitude/altitude stabilization of a mass-varying quadcopter

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