Acceleration Control Maths

Hi,

I’m trying to figure out the purpose and maths behind the _accelerationControl() function (code below) within the Position Control code (PositionControl.cpp).

From what I can tell the purpose of the function is to convert the acceleration setpoints output by the Velocity controller into thrust setpoint values which are normalised to the hover thrust.

I would have thought that the simplest way to do this would be to add CONSTANTS_ONE_G to the z component of the acceleration setpoint and then to scale the acceleration setpoints by _hover_thrust / CONSTANTS_ONE_G.

The maths for the Z setpoint does work out like this but the setpoints for X & Y are set by a slightly different scale factor (which is related to the collective_thrust variable) which is confusing me.

Can anyone shed light on the maths for me? Is there a paper somewhere that is is based on? Maybe I’m getting the purpose of the function slightly wrong. Any information or resources that could point me in the right direction would be very much appreciated!

accelerationControl
void PositionControl::_accelerationControl()
{
// Assume standard acceleration due to gravity in vertical direction for attitude generation
Vector3f body_z = Vector3f(-_acc_sp(0), -_acc_sp(1), CONSTANTS_ONE_G).normalized();
ControlMath::limitTilt(body_z, Vector3f(0, 0, 1), _constraints.tilt);
// Scale thrust assuming hover thrust produces standard gravity
float collective_thrust = _acc_sp(2) * (_hover_thrust / CONSTANTS_ONE_G) - _hover_thrust;
// Project thrust to planned body attitude
collective_thrust /= (Vector3f(0, 0, 1).dot(body_z));
collective_thrust = math::min(collective_thrust, -_lim_thr_min);
_thr_sp = body_z * collective_thrust;
}

Hi Pattius,

The Px4 controller documentation points to this report as the basis for the attitude controller, but I’m not sure that covers your question very well.

I agree with your assessment of the function’s purpose. The _accelerationControl() function is responsible for converting the desired acceleration vector to the desired normalised thrust vector. This is conducted in a world aligned Front-Right-Down (FRD) xyz coordinate frame, which is standard for aerospace systems, and might explain some of the confusion.
It also limits the normalised thrust setpoint to account for the maximum tilt the drone can safely reach while remaining airborne. I think this is might be why the XY components are initially handled seperately to the Z setpoint, rather than combining them as you suggest.

Looking at the code, I would agree that the scaling factor of the _thr_sp x-y components will be slightly off versus the desired acceleration. Use of the modified collective_thrust to scale these components introduces effects due to the - hover_thrust term and its scaling to prioritise the _thr_sp z-component. The tilit limits will also have an effect, though this one is desired.
That said, I’d expect that the _thr_sp x-y components will still be roughly proportional to the desired value and point in the correct direction, allowing the larger controller structure to correct for the difference at the position and velocity control levels, so I don’t know how much of a difference this makes to flight performance.

It might make sense to look into the ControlMath::limitTilt() function and a simulation to compare your suggested approach with what already exists.

Hope this helps,
Robert.

Code Explanation

As far as the maths of this function goes, my understanding of each of the line of the function is:

// Assume standard acceleration due to gravity in vertical direction for attitude generation
Vector3f body_z = Vector3f(-_acc_sp(0), -_acc_sp(1), CONSTANTS_ONE_G).normalized();

This line computes an initial value for the unit vector orientation of the drone’s body frame z axis, expressed in the world aligned FRD frame {W}. The drone thrust is going to act in the negative body z-axis so, to provide the desired acceleration in the {W} xy-axes, the body z-axis must point in the opposite direction to the acceleration setpoint. To hover, the drone will also be providing thust upwards to give acceleration of negative CONSTANTS_ONE_G in the {W} z-axis, so the body z-axis must again have this component in the opposite direction.
Normalising this converts it to a unit vector expression of orientation. The rest of the code then caluclates a scaling factor to give the thrust in normalised units.

ControlMath::limitTilt(body_z, Vector3f(0, 0, 1), _constraints.tilt);

This function alters the x-y components of the unit body_z vector to ensure the drone does not exceed the required tilt angle. It modifies body_z in place, returning a re-normalised unit vector that obeys the tilt limits.

// Scale thrust assuming hover thrust produces standard gravity
float collective_thrust = _acc_sp(2) * (_hover_thrust / CONSTANTS_ONE_G) - _hover_thrust;

This line gives the required normalised thrust setpoint in the {W} z-axis. Again, the result will likely be negative due to the FRD frame.
The first term gives the thrust required to meet the z-axis acceleration setpoint. Essentially the (_hover_thrust / CONSTANTS_ONE_G) is a unit conversion factor. I’d expect its units to be something like norm_thrusts/(m/s^2), calculated with _horver_thrust being in the range [0,1] norm_thrusts and CONSTANTS_ONE_G being 9.81 m/s^2.
The second term adds - _hover_thrust to account for thrust required to hover with drone weight acting in the positive {W} z-axis.

// Project thrust to planned body attitude
collective_thrust /= (Vector3f(0, 0, 1).dot(body_z));

This divides the collective thrust by the component of the body_z vector acting in the {W} z-axis. This scaling step means that the _thr_sp z-componet will have the magnitude of collective_thrust calculated in the line above when the two are multiplied together later.

collective_thrust = math::min(collective_thrust, -_lim_thr_min);

This limits the thrust scale to the minimum output that can be produced without stopping the propellers or spinning them in reverse. It prevents excessive downwards acceleration.

_thr_sp = body_z * collective_thrust;

The magnitude of the desired, re-scaled collective_thrust is applied to the body_z orientation to give the final thrust setpoint. The negative value of collective_thrust also flips the body_z direction giving the correct thrust in the {W} FRD coordinate frame.

Hi Robert,

Thanks for such a good and thorough answer! Definitely clears things up a bit for me and will be a great reference for anyone else looking to understand this function.

I guess I was getting hung up on the fact the XY thrust scale factor is off a bit too much, as you said this will just get absorbed by the rest of the controller. There’s separate tuning parameters for XY & Z on the velocity controller which I imagine would counteract any discrepancies. I suppose it just wasn’t worth an additional line of code/ extra variables to correct the scale factor.

I was curious to see what difference it would actually make so I wrote a little python script (code below) that works out outputs of the acceleration function using both methods.

Method 1: Original PX4 method described in Roberts Post above.
Method 2: Scaling adding gravity to the acceleration demands and scaling everything by Hover_Thrust/Gravity.

Some results:
Input: Acceleration Setpoint [2,2,0] Hover Thrust = 0.5
Output: Method 1 [ 0.1019368 0.1019368 -0.5 ]
Method 2 [ 0.1019368 0.1019368 -0.5 ]

Input: Acceleration Setpoint [2,2,-3] Hover Thrust = 0.5
Output: Method 1 [ 0.13311013 0.13311013 -0.6529052 ]
Method 2 [ 0.1019368 0.1019368 -0.6529052]

As you can see the the XY acceleration output gets scaled differently proportional to how much Z acceleration is demanded (as a consequence of scaling the whole output by the collective_thrust term). Still seems a bit odd to me but willing to accept that it probably makes no difference!

import numpy as np
import math
def main():
a_sp = np.array([2,2,-3])
g = 9.81
Th = 0.5
acc_control_px4(a_sp,g,Th)
acc_control_alt(a_sp,g,Th)

def acc_control_px4(a_sp,g,Th):

    body_z = np.array([-a_sp[0],-a_sp[1],g])
    body_z = body_z/np.sqrt(body_z.dot(body_z))
    body_z = limitTilt(body_z,np.array([0,0,1]), 0.5)

    collective_thrust = a_sp[2]*Th/g-Th
    collective_thrust /= np.array([0,0,1]).dot(body_z)

    thrust_sp = body_z * collective_thrust

    print("Orignal PX4 Output" ,thrust_sp)

def acc_control_alt(a_sp,g,Th):
    SF = g/Th
    A_ac = a_sp + np.array([0,0,-g])
    thrust_sp = A_ac/SF

    print("Alternative Output", thrust_sp)

def limitTilt(body_unit,  world_unit, max_angle):

    # determine tilt
    dot_product_unit = body_unit.dot(world_unit)
    angle = math.acos(dot_product_unit)
    # limit tilt
    angle = np.minimum(angle, max_angle)
    rejection = body_unit - (dot_product_unit * world_unit)
    rejection_unit = rejection/np.sqrt(rejection.dot(rejection))
    return math.cos(angle) * world_unit + math.sin(angle) * rejection_unit

main()
1 Like

Hi,
Thanks for your excellent explanation for the codes, but in the code for getting the body_z orientation, why not use the -acc_sp(2)+CONSTANTS_ONE_G as input but use CONSTANTS_ONE_G only? If for gravity acceleration g, the -acc_sp(2) is too small so it can be ignored, it really confuse of me.

Vector3f body_z = Vector3f(-_acc_sp(0), -_acc_sp(1), -_acc_sp(2)+CONSTANTS_ONE_G).normalized();

Hope this helps.

I have found the explanation for it :slightly_smiling_face: