Thruster Mixing - MRoboSub/mrobosub GitHub Wiki

Out thruster mixing code lives in mrobosub_fcu and is based off of TartanAUV's thruster mixing.

The code first defines the location/orientation of all thrusters. It uses these descriptions to create a matrix call the Thruster Allocator Matrix (TAM). The TAM is a 6x8 matrix (8 is the number of motors, 6 is the number of degrees of freedom) that can be right-multiplied by a vector of motor forces to create a 6-vector of the amount of force/torque generated in each DOF. The pseudo-inverse is then taken to generate the Inverse TAM. The 8x6 Inverse TAM matrix can be right-multiplied by a vector of force/torques in each DOF and creates an 8-vector of the amount of force required from each motor to produce that force/torque vector.

The motor controller (ESC, or Electronic Speed Controller) takes in a PWM value from 1100μs-1900μs (1500μs is neutral/zero power) that determines how much power the motor outputs. However, the power produced is not linearly related to the PWM value. Data relating input PWM to the output force is available on the BlueRobotics website. With the code in motor_fits, we have fitted cubic bivariate polynomial approximations relating input (PWM), output (force) and current, and all are also dependent on voltage. The polynomials are piecewise about 0 (or 1500 for PWM).

In the main loop, the thruster mixing code reads /output_wrench/*, which are used to generate a 6-vector of forces/torques in all degrees of freedom. This is then multiplied by the inverse TAM to calculate a vector of forces for each motor that will generate the target force vector. For each motor force, it calculates the required PWM for the motor to output that much force at the current battery voltage. It then estimates the amount of current draw for the motors. If any individual motor exceeds a predefined motor limit or if the sum of currents exceeds a larger limit, the input force vector is scaled down. The process repeats until a scaled-down force vector is found which can be achieved without exceeding the motor current limits.