Quadrocopter control loop: Recombining axis accelerations to motor speeds

First, as I always have to look this up myself – the following image explains yaw, pitch and roll rotations:

“Yaw Axis Corrected” by Yaw_Axis.svg: Auawise derivative work: Jrvz (talk) – Yaw_Axis.svg. Licensed under CC BY-SA 3.0 via Commons

At some point in the quadrocopter control loop, we might have calculated that we need significantly more thrust on the left side than on the right, slightly emphasizing the front rotors to increase pitch and reducing the overall thrust a little to stop elevating.

axesAccCombination

This situation is usually modelled as four values: three dimensions for the axes and one for the overall thrust. The three values for the axes can be positive or negative, while the overall thrust is strictly positive. From this four-dimensional vector, we need to derive the motor accelerations of all four motors (strictly positive, they typically cannot spin backwards). The following calculation for a quadcopter can be done with different orientations in mind, the usual variants are “+” (one rotor in front/left/right/back) and “X” (front/left/right/back direction is always in between two rotors). Somehow I always liked the “X” configuration, so I use it, but mathematically the approaches are of course equivalent. The proof consists in turning a remote control by 45 degrees ;). The following model describes how the axis accelerations and vertical acceleration map to the individual motors:

\begin{array}{rccccrrrr} \text{pitchAcc}&=&\text{frontLeftAcc} &+ &\text{frontRightAcc}&-&\text{rearLeftAcc}&-&\text{rearRightAcc}\\  \text{rollAcc}&= &\text{frontLeftAcc}&-&\text{frontRightAcc}&+&\text{rearLeftAcc}&-&\text{rearRightAcc}\\  \text{yawAcc}&= &\text{frontLeftAcc}&-&\text{frontRightAcc}&-&\text{rearLeftAcc}&+&\text{rearRightAcc}\\  \text{upAcc}&= &\text{frontLeftAcc}&+&\text{frontRightAcc}&+&\text{rearLeftAcc}&+&\text{rearRightAcc}\\  \end{array}

A quadrocopter can turn to the right (yaw) because opposite motors are set up to rotate in the same direction, different from the other two. If the clockwise rotation motors turn faster than the anticlockwise motors, the quadrocopter turns anti-clockwise. So the yawAcc equation might be different (multiply one side with -1) for a different motor setup.

Because at our position in the control loop, the vector (pitchAcc, rollAcc, yawAcc, upAcc) is given for the current timestep, we need to rearrange the equations above to calculate the individual motor thrusts:

\left( \begin{array}{c} \text{frontLeft} \\  \text{frontRight}\\  \text{rearLeft}\\  \text{rearRight}\\  \end{array} \right) = \frac{1}{4} \left[ \left( \begin{array}{c} \text{upAcc} \\  \text{upAcc}\\  \text{upAcc}\\  \text{upAcc}\\  \end{array} \right) + \left( \begin{array}{r} \text{pitchAcc} \\  \text{pitchAcc}\\  \text{-pitchAcc}\\  \text{-pitchAcc}\\  \end{array} \right) + \left( \begin{array}{r} \text{rollAcc} \\  \text{-rollAcc}\\  \text{rollAcc}\\  \text{-rollAcc}\\  \end{array} \right)+ \left( \begin{array}{r} \text{yawAcc} \\  \text{-yawAcc}\\  \text{-yawAcc}\\  \text{yawAcc}\\  \end{array} \right)\right]

This is what we need! We now have a simple linear transformation from the four accelerations to four motor powers. The factor \frac{1}{4} on the right side is often omitted because we get our inputs from PIDs, whose constants have to be tuned. If we leave out the constant factor here, the final PID constants will compensate for it.

Advertisements

One thought on “Quadrocopter control loop: Recombining axis accelerations to motor speeds

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s