# How to use quaternions to feed a PID quadcopter stabilization loop?

I'm making a quadcopter. I have set up a PID loop to stabilize it to a given Euler angle (pitch and roll). The problem arises when the roll approaches 90 degrees (45 degrees and up). The values don't make sense anymore, as it approaches the gimbal lock. I intend to make it do complex maneuvers like looping etc., which exceeds the 45 degree roll limit.

How can I use quaternions to overcome this problem? (I get quaternions from the MPU-9150.) I have read many articles on the matter of quaternions, but they all talk about rotations in 3D software, and tweening between two rotation points. This makes little sense as I do not know imaginary numbers and matrices.

• Did you solve your problem? If not please let us know how it's going, otherwise you might want to accept one of the answers. Oct 18 '14 at 11:39
• @marcv81 Yes, it's working pretty well now :) Thank you for reminding me of accepting an answer :) Oct 18 '14 at 18:33
• I'm glad it's working. It made me think of my own code and fix a bug too :) Oct 18 '14 at 18:56

A quadcopter contains (among other things) two separate and independent algorithms: an attitude estimation algorithm, and a control algorithm.

The attitude estimation algorithm computes information about the orientation of the quadcopter: the roll, pitch and yaw angles.

The control algorithm is responsible for driving the motors so that the orientation of the quadcopter matches what the pilot (or the autopilot software) expects. This algorithm is what would read the estimated quadcopter angles (from the attitude estimation algorithm) and change the motors speed to attempt to match the desired angles. PIDs are a well-suited and common control algorithm for quadcopters.

Gimbal lock is a phenomenon which may happen in the attitude estimation algorithm. It has nothing to do with the control algorithm. As such you don't need ESCs, motors or propellers to test for gimbal lock: you could modify your code to display your roll, pitch and yaw angles, and test that the correct values are calculated as you manually move your quadcopter around. You might be able to do this with the quadcopter tethered to your computer, via Bluetooth, or using other methods depending on your platform.

If the angles are calculated correctly you don't need to worry about quaternions. If they are not calculated correctly, quaternions might help you. The attitude estimation algorithm shall output 3 angles for the control algorithm to use, however it might use a different internal representation such as quaternions or 3x3 matrices. In that case it would still convert the attitude information to angles so as to provide usable data to the control algorithm. Generally speaking quaternions are unintuitive but computationally efficient. This makes them well suited for slow platforms such as Arduino. Matrices or angles may be an easier choice for faster hardware. If you need me to elaborate on one solution or the other please let me know, but it would be quite premature for me to give details at this stage as I am not convinced you need to implement quaternions.

Finally if the angles are calculated correctly the way to make your quadcopter loop is to control the angular rate rather than the angle. If your sticks represent the quadcopter angle there is no way to make it do a full loop: try to visualise the sticks position as the quadcopter loops and you should understand why. However if the sticks control the angular rate then you can control the speed at which it loops.

Note: For the sake of simplicity I have not mentioned the theoretical option to manipulate the data as matrices or quaternions both in the attitude estimation algorithm and the control algorithm. I have never seen a quadcopter implementing such algorithms.

• Thank you for your thorough answer! A person working at InvenSense said that, in most applications, quaternions were easier and better than Euler angles. Looking at the data tethered, it seems that it is possible to correct one axis to stop the gimbal lock when this is about to happen. Sep 14 '14 at 19:12
• Euler angles are a poor choice for the internal representation of the orientation. They are simple to visualise, but very hard to manipulate correctly to avoid gimbal lock. Quaternions are better because you wouldn't run into gimbal lock, but they are hard to visualise. Matrices are somehow easier to visualise than quaternions, but not as efficient if your platform is slow. My quadcopter uses quaternions internally, but converts to Euler angles for the PIDs to use. Sep 15 '14 at 10:54

First, I think you need to go back and look at your code. Gimbal lock is only a problem when you get very near (within a couple degrees) of 90. If you are seeing strange behavior at 45 degrees something else is the cause.

As for your question, quaternions are usually not used directly in basic PID control since they have complicated behavior resulting in non-intuitive results. Usually they are either converted to Euler angles and then used in the normal PID controller, or special nonlinear controllers are designed to use them.

Note that for your looping maneuvers, PID is generally not a very good controller: the gains that work well near hover no longer work well at large angles. Usually, when someone wants to do a loop they go "open loop", i.e. they start the maneuver under control and then once they get past a certain angle just just apply a fixed series of commands until they have completed the loop. Figuring out what fixed series of commands to use is the tricky part and often uses reinforcement learning (kind of like a formal way of doing trial and error).

• Thank you for your insight. I'm completely new to bare metal programming. I have only done high level programming. I'm also very interested in physics and maths, although I just finished high school, so I still don't know too much yet... Jun 20 '14 at 19:27
• I'm making this with a friend, and we "invented" the PID loop ourselves. Being accustomed to a "correct" answer, it was very difficult for us to use it without knowing this was a common way to solve our problem. And when we discovered the PID loop on Wikipedia, our own invented PID loop was "approved". Jun 20 '14 at 19:29
• So I really appreciate your insight on how this is commonly done in working projects. The strange behavior I'm talking about revealed itself when looking at the graph in Serial Chart (code.google.com/p/serialchart) The input is the direct Euler output from the I2CDevLib (MPU-9150). I'll test a bit more. The "error" might have been caused by imprecise movements by myself. Jun 20 '14 at 19:34
• The roll is the blue line. Red is yaw, green is pitch, blue is roll. As you can see, the problems start from about pi/4 and outwards. Is this a problem with the I2CDevLib, or is it "supposed" to be like this? screencast.com/t/svPV3C8B I'm turning the gyroscope 360 degrees around the roll axis. Jun 20 '14 at 19:45
• I've seen AeroQuad being able to stabilize itself even after being thrown into the air. Is this because it takes care of the roll first, then the pitch, then the yaw? Jun 20 '14 at 21:07

This paper, Full Quaternion Based Attitude Control for a Quadrotor by Emil Fresk and George Nikolakopoulos, demonstrates what you are trying to achieve.

Abstract— The aim of this article is to present a novel quaternion based control scheme for the attitude control problem of a quadrotor. A quaternion is a hyper complex number of rank 4 that can be utilized to avoid the inherent geometrical singularity when representing rigid body dynamics with Euler angles or the complexity of having coupled differential equations with the Direction Cosine Matrix (DCM). In the presented approach both the quadrotor’s attitude model and the proposed non-linear Proportional squared (P2) control algorithm have been implemented in the quaternion space, without any transformations and calculations in the Euler’s angle space or DCM. Throughout the article, the merits of the proposed novel approach are being analyzed and discussed, while the efficacy of the suggested novel quaternion based controller are being evaluated by extended simulation results.

• Thanks for the relevant paper @jgkim2020. But can you elaborate on it? Perhaps summarize the paper's findings? (Links can go stale after all). You may also want to check out How to answer
– Ben
Feb 20 '16 at 15:54
• I agree with Ben - A summary of the paper would be great, as link death usually occurs after a while. Feb 20 '16 at 16:05

This MOOC free-of-charge course, Welcome to TUMx's AUTONAVx! Autonomous Navigation for Flying Robots, may help. It covers:

• Learning theory
• Exercise Quadcoptor programming that run on both stimulator and actual hardware
• The link doesn't work. Jun 21 '14 at 11:14
• The link works if you are logged in. Jun 21 '14 at 13:27
• Video 1.4 has demo of many quadrotor, performing amazing actions. Hope you get interested in the topic.
– EEd
Jun 21 '14 at 13:35
• May check FreeIMU software, which provides a) fusioned data from multiple sensors (rate gyro, compass, accelerometer), giving more stable, lower-drift and accurate data than just reading raw data from chip b) provided multiple output formats, including pitch, roll and yaw, which may be simpler to visualize, understand and use.
– EEd
Jun 21 '14 at 13:42
• Thank you for your help. I'm using the MPU-9150 which has an onboard DMP (sensor fusion) of both the gyroscope and an accelerometer. Fusing this with the magnetometer has to be done manually. Jun 21 '14 at 13:47