# LQR control of motor speed for a quadrotor

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.

• Under what conditions does the motor command go negative? Isn't the command from LQR added to a fixed positive value to ensure that the total motor command is positive and sufficient to keep the quad rotor in the air? Since you are working with a linearised system, the commands to motors (excluding the fixed value needed to ensure lift) are expected to be small values. To ensure small values increase the weight for the input signal (usually denoted as the R matrix).
– AJN
Aug 7 at 17:03
• LQR can't deal with inequality constraints. Instead you could look at model predictive control (MPC). Aug 7 at 20:25
• @fibonatic yeah that the plan in the future. My only pb with mpc is that its quite slow. Aug 10 at 12:53
• @AJN i have negative motor command, when my altitude is above the desired set point. i tried using value found in different articles for the lqr to no avail. what's bothering me is that if i give the general command it work, but if i tried to convert them to motor speed and after that recontruct the command it fail. (dont know if im clear) Aug 10 at 13:03
• Please edit your question to add the details of your system; including plant original equations, linearised equations, controller equations and sturcture, LQR methodology, LQR weight matrices, block diagram of plant and controller, some wave-forms (step response, negative speed etc.), gneral command equations, motor command (re?) construction equations. More details, the better. It is difficult to understand from qualitative descriptions alone.
– AJN
Aug 10 at 13:20

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.

• If you note any error in the above please let me know.
– AJN
Aug 11 at 12:48
• First of all for your answer. it brings ligth on some part i didnt understand. Aug 14 at 14:24
• Regarding the thrust mixing, i wasnt clear enough but i add the $\Omega$ to the steady part mg/4b. Regarding the $\delta u = -Kx$ i rectify it and now use $\delta u = -K(q-q_{desired})$ (in fact i use $K*(q_{desired} -q$ but its the same) which is then add to the steady part $u$. I can restate the problem as follow. if i gave to the physics model, the $u = u_0 +\delta u$ with $\delta u$ given by the lqr everything works fine. But when i try to use the $\Omega$s (as given by the mixing law + the steady part) it failed. and i have no idea why, as its supposed to be a simple change of variable. Aug 14 at 16:16
• @Staufenbach what software are you using to test your model and controller?
– AJN
Aug 14 at 17:36
• I will try to simulate for myself and see when i get free time. I have access only to Octave software though.
– AJN
Aug 14 at 17:37