LQR control of motor speed for a quadrotor

Question

Im working around with a simulation of a quadrotor.

So i've read articles, did the math to derive the set of equation using lagrangian approach. So far so good.

Then i want to control the system. so i did another round of math to linearize (with the help of the paper but also other:https://ieeexplore.ieee.org/document/6417914) and i implement a lqr controller.

So the lqr controller give me a set of command (for the thrust and each moment), that if feed directly in the model work well to reach a command. Great !

My problem is that i want to have something more 'realistic', since i want to control the speed of the motor directly. So using thrust mixing describe in many papers, i transform the set of command given by the lqr to a set of command for the motor. And then everything falls apart.

As i want to keep a certain touch of realism, i dont want to have negative command for the motor (meaning that they will have to spin backward so not possible) nor have then spin too fast. The problem is with negative command.

So, i tried to clip the motor command but it doesnt work, the system is unable to reach a command. I tried to rewrite the state space equation so instead of the traditional command i have directly motor command and then redo a lqr from there but it didnt work (i may have done some mistake).

I feel like i'm missing something but i dont know what. Any advice ?

Edit: So after applying lagrangian mechanics we end up with:

$$ \ddot{q} = \begin{pmatrix} u1/m(\cos(\phi)\sin(\theta)\cos(\psi)+\sin(\psi)\sin(\phi))-A_xu/m \\ u1/m (\sin(\psi)*\sin(\theta)*\cos(\phi) - \cos(\psi)*\sin(\phi)) - A_yv/m \\ u1/m \cos(\theta)*\cos(\phi) - g - A_zw/m \\ \dot{\psi}\dot{\theta}(I_y-I_z)/I_x + lu_2/I_x + J/I_x\dot{\theta}W \\ \dot{\phi}\dot{\psi}(I_z-I_x)/I_y + lu_3/I_y - J/I_y\dot{\phi}W \\ \dot{\phi}\dot{\theta}(I_x-I_y)/I_z + u_4/I_z \end{pmatrix} $$ where q is {x, y,z, $\phi, \theta, \psi$} the position and attitude of the quadcopter, $\ddot{q}$ the translational and rotational acceleration, m is the masse of the quadcopter, l the length of the arm, $I_X,I_y,I_z$ the moment of inertia, u v w the velocity, A drag coefficient J drag moment $u_1 = \Sigma b\Omega_i^2$, $\Omega$ the motor rpm, b the thrust coefficient $u_2= lb(-\Omega_2^2 +\Omega_4^2)$ $u_3 = lb(\Omega_3^2 - \Omega_1^2)$ $u_4 = -d\Omega_1^2 +d\Omega_2^2 -d\Omega_3^2 + d\Omega_4^2$

this what i call the reconstruct command, as i reconstruct command using the motor speed

i linearize around equilibrium (hoovering) which give $\Omega_i^2=mg/4b$ and everything else at 0.

LQR design $$ A = \begin{bmatrix} 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& g& 0& 0& 0\\ 0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& -g& 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& 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& 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& 1\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ \end{bmatrix}$$

$$ B = \begin{bmatrix} 0& 0& 0& 0\\ 0& 0& 0& 0\\ 0& 0& 0& 0\\ 0& 0& 0& 0\\ 0& 0& 0& 0\\ 1/m& 0& 0& 0\\ 0& 0& 0& 0\\ 0& 1/Ix& 0& 0\\ 0& 0& 0& 0\\ 0& 0& 1/Iy& 0\\ 0& 0& 0& 0\\ 0& 0& 0& 1/Iz \end{bmatrix} $$ $$ Q= \begin{bmatrix} 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 5& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 2& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 5& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 2& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 5& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 2\\ \end{bmatrix} $$ $$ R= \begin{bmatrix} 10& 0& 0& 0\\ 0& 10& 0& 0\\ 0& 0& 10& 0\\ 0& 0& 0& 10 \end{bmatrix} $$

i use the classic U = - K x to get the command (K the lqr gain and x the state that i define as q in the physics part) and i mixed them as: $$ \Omega_1^2 = u_1/4b +u_3/2b + u_4/4d\\ \Omega_2^2 = u_1/4b -u_2/2b - u_4/4d\\ \Omega_3^2 = u_1/4b -u_3/2b + u_4/4d\\ \Omega_4^2 = u_1/4b +u_2/2b - u_4/4d $$ to get the motor command. i take the signed square root and add the result to the $\sqrt{mg/4b}$ that have arise during the linearization to have everything in term of $\Omega$ and not $\Omega^2$

if i fed the Omegas to get the command my simulation failed, especially when my altitude is higher than my command bc of negative command that cant be transcribe ($100^2 = -100^2$ so it fails), if i fed directly the result from the U=-Kx operation as $u_1, u_2, u_3, u_4$ in the model part it works and i dont understand why.Hope its clearer

Solution: In the end, my error reside in how i code the $u$

i code the thrust of each propeller in a list, and then use this list to compute the $u$. so i had: $F = [b\Omega_1, b\Omega_2, b\Omega_3, b\Omega_4]$

then $u_1$ is the sum of F and $u_2 = l*(Ft[3]-Ft[1])$ etc.

but i mistankenly wrote $u_2 = bl*(Ft[3]-Ft[1])$ so i had a b in excess which cause the whole system to misbehave. I probably had in mind the expression of $u_2$ in term of $\Omega$ while in code the $u$ are in term of thrust.

Answer 1

A few waveforms would have helped but here is one problem that could have occured.

and I mixed them as $$\Omega_1^2 = u_1/4b +u_3/2b + u_4/4d\\ \Omega_2^2 = u_1/4b -u_2/2b - u_4/4d\\ \Omega_3^2 = u_1/4b -u_3/2b + u_4/4d\\ \Omega_4^2 = u_1/4b +u_2/2b - u_4/4d $$

The output of the LQR is not the input that can be fed directly to the above equation! In the process of linearisation, you have missed some steps. This usually occurs when one uses the same symbols for variable before and after linearisation. See one of my other answers regarding linearisation.

Let's call your original non-linear model and its equations as $$ \ddot{q} = f(q, u_i)\\ u_i = \sum{c_{ij}\Omega_j} = h(\Omega_i) $$

Before linearisation we assume that every signal above has two parts a steady part and a dynamic part (also called small signal in electronics).

$$ q = q_0 + \delta_q\\ u_i = u_{i0} + \delta_{u_i}\\ \Omega_i = \Omega_{i0} + \delta_{\Omega_i} $$

The "zero" subscripted symbols are the steady part that appears during the equilibrium.

Equilibrium (steady) state is $ q_0 = (x_0,0,y_0,0,z_0,0,\ \ \ 0,0, 0,0, 0,0) = q_{desired} $

Equilibrium control (feed forward) is $ u_0 = (u_{10}, u_{20}, u_{30}, u_{40}) ={} \color{red}{???} % = (mg,0,0,0) $

Equilibrium motor command is $ \Omega_0 = (\Omega_{10}, \Omega_{20}, \Omega_{30}, \Omega_{40}) ={} \color{red}{???} % = (\frac{mg}{4b},\frac{mg}{4b}, \frac{mg}{4b}, \frac{mg}{4b}) $

We use the Taylor series of the non linear functions involved for linearisation

$$ \frac{d^2}{d t^2}(q_0 +\delta_{q}) = f(q_0,u_0) + \frac{1}{1!} \frac{\partial f}{\partial u}|_{q_0,u_0}\cdot \delta_u + \frac{1}{2!} \frac{\partial^2 f}{\partial u^2}|_{q_0,u_0} \cdot \delta^2_u + \dots + \frac{1}{1!} \frac{\partial f}{\partial q}|_{q_0,u_0}\cdot \delta_q + \frac{1}{2!} \frac{\partial^2 f}{\partial q^2}|_{q_0,u_0} \cdot \delta^2_q + \dots + \dots \frac{\partial^2 f}{\partial u \partial q}|_{q_0,u_0} \cdot \delta_u \delta_q+ \dots $$

From the above equation we can

1. neglect the higher order terms $\frac{\partial^2 f}{\partial u^2}\cdot \delta^2_u$ etc.

2. extract the steady part $\ddot{q}_0 \triangleq \mathbf{0}_{6 \times 1} = f(q_0, u_0)$. You already solved this to get $$u_0 = (u_{10}, u_{20}, u_{30}, u_{40}) = (mg,0,0,0) $$ and $$ \Omega_0 = (\Omega_{10}, \Omega_{20}, \Omega_{30}, \Omega_{40}) = (\frac{mg}{4b},\frac{mg}{4b}, \frac{mg}{4b}, \frac{mg}{4b}) $$

   But you might have named it $u$ and $\Omega$.

3. extract the linear part of the system $$ \ddot{\delta}_q = \frac{\partial f}{\partial u}|_{q_0, u_0}\cdot \delta_u + \frac{\partial f}{\partial q}|_{q_0, u_0}\cdot \delta_q = Ax + Bv\\ x = q - q_0\\ v = \delta_u = u - u_0 $$ This linear part is the system for which you have designed LQR. The LQR control brings $\delta_q$ towards 0. It doesn't bring $q$ towards the desired equilibrium! So the LQR control equation $\delta_u = -Kx$ is not a substitute for $u_0$! For the final system,the total control input is $u = u_0 + \delta_u$.

In short,

1. We cannot use the states of the original plant directly to the designed LQR. We have to apply it on the small signal $q - q_0$.
2. Total control input is not the LQR control input; it is the sum of $u_0$ and LQR control input.
3. Follow the linearisation procedure step-by-step and introduce the small signal variables were necessary. Don't re-use the symbols of original variables for the small signal variables, if you are a beginner.

i use the classic U = - K x to get the command

Error signal $q_{desired}-q$ when the quad rotor reaches desired equilibrium is $\mathbf{0}_{6 \times 1}$. A control equation of the form $Kx$ will give zero as the control input at equilibrium. But the non linear system requires $u_0 \neq 0$ as the control input! If what I have understood from your description of the problem is correct, your LQR control will give zero control input when the quadrotor reaches equilibrium.

i use the classic U = - K x to get the command

Also, shouldn't the control equation be $-K (q_{desired} - q)$ ? $u = -Kq$ is the control required to bring the system to zero state $\mathbf{0}_{6 \times 1}$

Note

After using the feed forward / steady input $u_0$ along with the LQR control input, If you still get negative motor speed, increase the value of entries in the $R$ matrix.