Converting a direction vector to an angle

932 Views Asked by At

I have a machine that controls its own velocity via the use of a fixed thruster mounted on its rear.

The machine knows its current velocity, and it knows the velocity it must attain. By subtracting the current from the target, it gets an error vector that represents how much it must alter its velocity for that velocity to match the target. The machine then negates that error vector and orients itself to the resulting direction vector, then applies thrust until the velocity vector is correct.

The trouble is, the part that controls the machine's orientation only accepts pitch-yaw-roll values. How do I calculate the angular values I need from the direction vector I have?

1

There are 1 best solutions below

2
On

Suppose $d$ is the starting direction vector of the machine. Then, $d$ is also the axis of roll rotation. Suppose $y$ is the axis of pitch rotation and suppose we want to end with direction vector that is pointing the same way as direction $v$. If we apply the roll rotation the axis of pitch rotation rotates also.

We will first roll by an angle $\theta$ until the axis of pitch rotation moves to the position perpendicular to $d$ and $v$. We will then pitch by an angle $\phi$. Thus, $\phi$ can be deduced as the angle between $d$ and $v$: $$ \phi = \arccos \left( \frac{d\cdot v}{\Vert d \Vert \Vert v \Vert} \right) $$

$\theta$ seems more problematic to discover. In order to apply the pitch we want to have the pitch axes in the direction of the vector $ d \times v $. The angle $$\psi = \arccos \left( \frac{(d \times v)\cdot y}{\Vert d \times v \Vert \Vert y \Vert} \right) $$ is the smallest angle between $y$ and $ d \times v $. The right roll angle $\theta$ is thus either $\psi$ or $\psi+\pi$: $$ \theta=\begin{cases} 0, & \mbox{if }y\cdot\left(d\times v\right)=\Vert y\Vert\,\Vert d\times v\Vert,\\ \psi, & \mbox{if }\left(y\times\left(d\times v\right)\right)\cdot d\geq 0,\\ \psi+\pi, & \mbox{if }\left(y\times\left(d\times v\right)\right)\cdot d<0. \end{cases} $$ The angle $\theta$ was set to zero when $y$ is already in the direction of $d\times v$.

It is important that these two rotations are applied as follows: first the roll by the angle $\theta$ and second the pitch with the angle $\phi$.

There are also other combinations of roll, pitch and yaw that can result in the same maneuver.