During the last weeks I have been thinking about how the basic control loop of my Quadrocopter should look like. That’s the piece of software that takes sensor readings and determines the turning speed for each of the four motors. If it works well, the Quadrocopter stays calm in the air, even under windy conditions. Otherwise, the Quadrocopter starts to oscillate, which uses lots of energy (the motors are busy with alternately compensating each other’s thrusts) and can resonate to a really unstable (and uncontrollable) flight condition, healthy for neither the environment or the vehicle. As we see, this is a really important component and deserves a considerable amount of thought.

When talking about the event loop, it is not generally clear what exactly the input should be. The only commonality seems to be that it is in some sense a ‘desired state’. For many commercial toys, this desired state is the orientation, which should match the stick on the remote control. In my case, there is no remote control and the commands come from a higher level planning module. For a first shot, the higher-level plan would just be to elevate to about 1m, hover for some seconds and then descent for a (hopefully) smooth landing.

##### Current status

As mentioned in this blog post, I use the RTIMULib for fusing the data from my MPU-9150 (accelerometer, gyroscope, magnetometer) and it works good, which should be more than enough for a first test flight. Additionally, I managed to access the Adafruit PCA9685 with Python 3 (which was mainly a task of using the print functions and adjusting the Exception handling). So the biggest step on my Todo list is to implement the control loop logic.

##### Choice of input

For an autonomous vehicle the remote control in the upper diagram should be replaced by some higher level planning instance. This instance must have a precise idea about the 3d path to fly. Now an interesting choice is how to encode the target state for the control loop. Some ideas:

1. Thrust and orientation
This may be the most common configuration, where four PID controllers directly control the 3 orientation angles and the vertical thrust. A great introduction can be found here and a little demo on how to tune the PID constants in this scenario is available on diydrones.
2. Thrust and angle velocities
The control loop logic is equally simple as in (1). However, the higher level logic gets more complicated. Consider a remote control, where the stick directly controls the rotation velocity of the quad. Some humans really fly in this mode, which realizes great maneuverability and seemingly crazy stunts.
3. Thrust, orientation and angle velocities
This builds on the simple cases. Not only setting a target orientation, but also target angle velocities can most probably lead to a more precise and stable path following, if the control loop can make a short-time plan for itself to achieve this target state (maybe at a time-scale of up to 0.5s).
4. Target speed vector (and linear acceleration vector)
I found this idea in a talk from Andy Baker: the control loop gets a (3d) speed vector as a target state.  From this, a target acceleration for every axis is computed. The orientation is then chosen to achieve the target accelerations. I really like this idea, since it enables the higher level planning module to ignore the whole complexity around how the Quadrocopter has to be oriented. Instead, it can focus on planning a path in the real world, which is difficult enough. One problem may be that there is no actual measurement of the linear speed vector: we can only integrate the accelerations, which is inherently flawed with discrete time steps and real-life noise. GPS can in principle solve the problem, but it is not accurate enough to provide speeds for small movements. In my opinion, only visual perception (like the already mentioned raspi camera motion detection) is a solution here.
##### My control loop

In the schema below, red boxes mean that own work is involved. Purple boxes are nothing new: PID controllers are older than I am and transformations between 3d coordinate systems are equally boring. Blue boxes just describe flowing data, while hardware components are ‘colored’ grey.

I want to follow the idea of setting a target orientation and angular velocities for attitude control. In my opinion, it is a nice combination between doing something own (my axis rotation planning, which is shown below) and proven old-school tech like setting a target orientation, treating the axes separately and using PIDs to generate force differences between opposite motors.

Using rotational speed PIDs with axis rotation planning as a preprocessor has several benefits as opposed to directly controlling the orientation with PIDs:

• The PIDs work with angular speeds, which are measured directly by the gyroscope. Orientation on the other hand is not directly measured, so that the feedback can be delayed through sensor fusion. So the PIDs can run at a higher frequency than axis rotation planning.
• The PID tuning is slightly less important: even if the PIDs are not perfectly tuned, the quadrocopter will mainly oscillate in angle speeds because axis rotation planning dampens the orientation oscillations by compensating errors over time and should yield a stable flight.
##### Axis Rotation Planning

I introduce axis rotation planning as the problem of finding a smooth transition from a given axis’ rotation state (angle and speed) towards a target rotation state (angle and speed) at some time $t$ in the near future. The upper sketch shows an example where the quad is leaning to the left (roll is negative) and still has some rotation speed towards the left (green arrow), but some higher logic says that it shoud lean to the right (positive roll) with a very small rotation speed towards the right (red arrow). We want to find a function that models this transition with minimal oscillations/energy usage so that we can use that function as a guideline on what to do next (in order to reach the target).

###### Axis Rotation Planning: Math

One common way to approximate unknown functions is to use polynoms. For this easy application, we assume that a polynome of degree 4 is sufficing. There are many ways the energy consumption could be modelled. One idea is to use the total amount of acceleration needed to perform some flight maneuver. As a differentiable version of this, the squared angle acceleration is used.

$\begin{array}{cccl} \textbf{minimize } & \text{energy}(f) \\ \textbf{s. t. } & f (0) &=& \text{angleStart} \\ & f'(0) &=& \text{speedStart} \\ & f (t) &=& \text{angleTarget} \\ & f'(t) &=& \text{speedTarget} \\ & f (x) &=& ax^4+bx^3+cx^2+dx+e \text{ for } x\in \left[0,t\right]\\ & \text{energy}(f) &=& \int_0^t{\left(f''\left(x\right)\right)^2 dx} \\ & f &:& \text{time} \rightarrow \text{angle} \end{array}$

First, $t$ is assumed to be $1$. This is no limitation, because we can choose whatever time unit we like (what about halfsecond?). With some hand-crafted equation fun, Mathematica solves this optimization problem (see the Notebook) and the result is:

$\begin{array}{ccl} f(x) &=& ax^4+bx^3+cx^2+dx+e \\ a &=& 0 \\ b &=& 2\cdot\text{angleStart}+\text{speedStart}-2\cdot\text{angleTarget}+\text{speedTarget} \\ c &=& -3\cdot\text{angleStart}-2\cdot\text{speedStart}+3\cdot\text{angleTarget}-\text{speedTarget} \\ d &=& \text{speedStart} \\ e &=& \text{angleStart} \end{array}$

As we see, the fourth polynome degree is not needed for our task and the result in the upper diagram looks very appealing. In the next blog post, I will show how this result is used to get a target rotation speed for the PIDs.