How to represent a (known and fixed) point in 3D space in a mobile frame of reference (in term of rotation matrices)

Question

Say that I have a fixed point $X$ in the world frame of reference (that I'm gonna call $X^{\{w\}}$). Moreover, I have a flying robot that can be anywhere in this space.

Its position in the world is denoted $P^{\{w\}}$. The robot can not only move freely but it can also rotate freely in all three axes.

Using $X^{\{w\}}$, $P^{\{w\}}$ and the 3 angle rotations on the three axes made by the robot ($\phi_{x}$, $\phi_{y}$ and $\phi_{z}$), how can I obtain $X^{\{P\}}$ (the point $X$ as seen by the robot)?

Edit: by the angles $\phi_{x}$, $\phi_{y}$ and $\phi_{z}$, I mean the robot's roll, pitch and yaw. An illustration of the angle $\phi_{z}$ in the 2D XY plane:

$P$ and angle $\phi_z$ in 2D">

Answer 1

NOTE: this answer is the "easy" method. There are a lot of different and more efficient ways to represent rotations, but this will solve your problem.

To begin we need to define three rotation matrices: each one representing a rotation about a different axis. More information here. We have:

Rotate about the x axis:

$$R_x(\theta_x) = \begin{bmatrix} 1 & 0 &0 \\ 0 & \cos(\theta_x) & -\sin(\theta_x) \\ 0 & \sin(\theta_x) & \cos(\theta_x) \end{bmatrix}$$

Rotate about the y axis:

$$R_y(\theta_y) = \begin{bmatrix} \cos(\theta_y) & 0 & \sin(\theta_y) \\ 0 & 1 & 0 \\ -\sin(\theta_y) & 0 & \cos(\theta_y) \end{bmatrix}$$

Rotate about the z axis:

$$R_z(\theta_z) = \begin{bmatrix} \cos(\theta_z) & -\sin(\theta_z) &0 \\ \sin(\theta_z) & \cos(\theta_z) & 0 \\ 0 & 0 & 1 \end{bmatrix}$$

We now need to figure out an order of rotation. This is usually up to you and for much more information please see here. For now we'll just assume you're doing the order x-y-z. It's up to you to figure out the order you want! Once you rotate with one matrix your coordinate axes move, so the order is semi-important. As long as you are consistent you're OK.

With the order I chose, you need to create a rotation matrix, $R = R_z(\theta_z)R_y(\theta_y)R_x(\theta_x)$. Note the order of the matrices - x is the first rotation, and thus it appears at the end of that expression.

We are now about to do something odd and something else that is convenient.

The odd thing: The rotation matrix will only rotate a vector, but it will not translate a point. You know $X^{\{W\}}$ and want that point in the robot's frame, but the robot and the world do not share an origin. You'll need to simultaneously rotate the point so that it is referenced with respect to the robot's coordinate axes, and then translate it so that the point is reckoned from the robot's origin.

The convenient thing: I'm very lazy today. I'm going to assume that you actually have $X^{\{P\}}$ and you want $X^{\{W\}}$, instead of what you said. This is because you have the robot's angles but a world point. In order to go the other way you need to figure out the opposite angles - what are the orientations of the world's axis with respect to the robot. There are negatives involved.

There are MANY ways to do this. If you just need to know and don't want a single matrix equation, then the answer is:

$$X^{\{W\}} = RX^{\{P\}} - P^{\{W\}}$$

This is read as: "First we rotate the point so that the point's coordinates are with respect to a frame that is centered at the world's origin but oriented to align with the robot. Next, we move the point into a new frame so that the origin is where the robot is.