Thrust Mapper - Finding a Solution

Last post, we defined a couple matrices which map thruster forces to forces on the robot.

$$ A = \begin{bmatrix} 0 & 1 & 0 & 1 \\\ 1 & 0 & 1 & 0 \\\ r_{5z} & r_{6z} & -r_{7z} & -r_{8z} \end {bmatrix}, \quad B = \begin{bmatrix} r_{1x} & r_{2x} & -r_{3x} & -r_{4x} \\\ -r_{1y} & -r_{2y} & r_{3y} & r_{4y} \\\ 1 & 1 & 1 & 1 \end{bmatrix} $$

$$ A \begin{bmatrix} T_5 \\\ T_6 \\\ T_7 \\\ T_8 \end {bmatrix} = \begin{bmatrix} F_x \\\ F_y \\\ M_z \end{bmatrix}, \quad B \begin{bmatrix} T_1 \\\ T_2 \\\ T_3 \\\ T_4 \end {bmatrix} = \begin{bmatrix} M_x \\\ M_y \\\ F_z \end {bmatrix} $$

The Inverse

If you remember your first linear algebra course, you’ll know that we need the inverses of \(A\) and \(B\) to find the thruster forces that gives us a solution.

$$ A^{-1} \begin{bmatrix} F_x \\\ F_y \\\ M_z \end {bmatrix} = \begin{bmatrix} T_5 \\\ T_6 \\\ T_7 \\\ T_8 \end {bmatrix} $$

But \(A\) isn’t a square matrix so it does not have an inverse! This actually makes sense if we look at the physical system. If there was an inverse, it would imply that there is a unique solution to every system.

The figure above illustrates the robot doing a yaw maneuver - spinning about the z-axis - using two configurations. On the left, only the bow and stern thrusters are used. On the right, only the port and starboard thrusters are used.¹

In fact, we can redistribute this effort between these thrusters in an infinite number of ways to get the same overall effect. So no, A is not invertible, but there are many solutions. This where the pseudo-inverse, \(A^+\) comes in handy.

The pseudo-Inverse

I won’t go into all the details of pseudo-inverses here but, for our purposes, it’s helpful to note these facts:

if \(Ax=b\) has \(> 1\) solutions then \(x = A^+b\)

\(A^+\) is unique

\(A^+\) exists for all \(A\)

The popular python matrix library, numpy, comes with a function for obtaining the pseudo-inverse, numpy.linalg.pinv

from numpy import matrix
from numpy.linalg import pinv

A = matrix([1,   1,   0,   0],
           [0,   0,   1,   1],
           [r5z, 56z, 57z, r8z])
Ap = pinv(A)

Now, whenever we want to find the equivalent thruster forces to get a desired response, we let numpy do the heavy lifting. For example, if we want the robot to dive deeper while accelerating forward, we might do this:

# create the desired response as a vector
Fx_Fy_Mz = matrix([1, 0, 1]).T
T5678    = Ap * Fx_Fy_Mz

And voilá, T5678 contains a vector for the thrusts required of thrusters 5, 6, 7, and 8 to perform our maneuver.