Conveyor Belt Calibration for a Robot - Rotation Matrix

Question

Short Question:

As demonstrated in the image below, how can I calculate the rotation matrix that relates the base coordinate and the rectangle coordinate, given 3 known points P1, P2, P3 on the surface of the rectangle with respect to the base coordinate?

Long Question:

I am currently working on OMRON pick-and-place robots. Basically, the overall layout of the system is similar to what is shown in the following picture:

.

The system has a rotary encoder that reflects the conveyor belt motion.

What I am trying to do is to come out with a transformation matrix T that represents the belt with respect to the robot coordinates. This transformation matrix is automatically calculated using a calibration wizard, however I am trying to understand how such matrix is calculated based on the 3-point belt calibration carried out as below:

Simple Calibration Steps:

The calibration can simply be completed after teaching 3 points on the belt as shown in the following image:

The steps are as follows:

1. A product is placed on the belt at the upstream limit that a robot can start tracking the product from.
2. The robot tip is then placed on the product. Its location and the encoder value are recorded. Let the point be P₁ and encoder value be E₁.
3. The belt is then allowed to move in the product feeding direction until the it reaches to another location which the robot tip can reach.
4. Robot tip is placed on top of the product. This is the second calibration point P₂. The encoder value is also recorded. Let it be E₂.
5. Lastly the robot tip is moved and placed on the calibration product on a side location of the belt called downstream pick limit. This would be the the third point P₃ and it is recorded.

After I followed these steps for a test robot in hand, I got the following quantities:

$$P_1 = \begin{bmatrix}-286.8 & -338.0 & 244.6\end{bmatrix} ; E_{1}=-71873$$

$$P_2 = \begin{bmatrix}140.3 & -422.2 & 246.7\end{bmatrix} ; E_{2}=-158710$$ $$P_3 = \begin{bmatrix}-18.004 & -590.452 & 248.746\end{bmatrix} $$

Let b = [b_x b_y b_z] be a belt point. Since the belt motion is having a magnitude along the x direction, then the scale factor in units of mm/pulses can be calculated by:

$$s = \frac {|P2-P1|}{E2-E1} = \frac {\sqrt{(140.3-(-286.8))^2 + (-422.2 -(-338.0))^2 + (246.7-244.6)^2}}{−158710 -(-71873)} =-0.005013$$

This is one important scale factor that we need to calculate during the calibration process (the minus sign means the encoder counts opposite to belt rotation)

Along with this scale that we get during calibration, we need to calculate all members of the transformation matrix T which relates belt coordinates with robot coordinates. This has the rotation matrix R and translation matrix D. $$ p=Tb = \begin{bmatrix}& R & & D\\ 0 & 0 & 0 & 1 \end{bmatrix}b $$

$$p=\begin{bmatrix} p_x \\ p_y\\ p_z\\ 1 \end{bmatrix} = \begin{bmatrix} r_{11} & r_{12} & r_{13} & d_{x}\\ r_{21} & r_{22} & r_{23} & d_{y}\\ r_{31} & r_{32} & r_{33} & d_{z}\\ 0 & 0 & 0 & 1\\ \end{bmatrix} \begin{bmatrix} b_x \\ b_y\\ b_z\\ 1 \end{bmatrix} $$

Since b always has one component along the x axis of the belt, then the coordinate of the first calibration point with respect to belt coordinate is (Belt did not move yet): $$ b_1 = \begin{bmatrix} 0 \\ 0\\ 0\\ 1 \end{bmatrix} $$

and the coordinate of the second calibration point with respect to belt coordinate is (Belt moved from P1 to P2):

$$ b_2 = \begin{bmatrix} s\times(E2-E1) \\ 0\\ 0\\ 1 \end{bmatrix} = \begin{bmatrix} 435.3 \\ 0\\ 0\\ 1 \end{bmatrix} $$

Therefore, from P1 calibration, we have:

$$p_1=\begin{bmatrix} −286.8 \\ −338.0\\ 244.6\\ 1 \end{bmatrix} = \begin{bmatrix} r_{11} & r_{12} & r_{13} & d_{x}\\ r_{21} & r_{22} & r_{23} & d_{y}\\ r_{31} & r_{32} & r_{33} & d_{z}\\ 0 & 0 & 0 & 1\\ \end{bmatrix} \begin{bmatrix} 0 \\ 0\\ 0\\ 1 \end{bmatrix} $$

Solving for T matrix members, we get d_x = −286.8, d_y = −338.0 and d_z = 244.6

From P2 calibration, we have: $$p_2=\begin{bmatrix} 140.3 \\ −422.2\\ 246.7\\ 1 \end{bmatrix} = \begin{bmatrix} r_{11} & r_{12} & r_{13} & −286.8\\ r_{21} & r_{22} & r_{23} & −338.0\\ r_{31} & r_{32} & r_{33} & 244.6\\ 0 & 0 & 0 & 1\\ \end{bmatrix} \begin{bmatrix} 435.3 \\ 0\\ 0\\ 1 \end{bmatrix} $$

Solving for the Rotation matrix, we get: $$ R = \begin{bmatrix} 0.981112257 & r_{12} & r_{13}\\ -0.193378015 & r_{22} & r_{23}\\ 0.004866365 & r_{32} & r_{33} \end{bmatrix} $$

Based on OMRON calibration wizard, the following matrix is obtained after finishing teaching the 3 points: $$ T = \begin{bmatrix} 0.981112257 & -0.193427089 & 0.00216786 & −286.8\\ -0.193378015 & -0.981022085 & -0.01416372 & −338.0\\ 0.004866365 & 0.013476983 & -0.999897339 & 244.6\\ 0 & 0 & 0 & 1\\ \end{bmatrix} $$

As seen, the first column and 4th conform with the results that we got. However, I am not really sure what the mathematical way to follow using the third calibration point to calculate the rest of the transformation matrix that I have. How would I be able to calculate r₁₂, r₂₂, r₃₂, r₁₃, r₂₃ and r₃₃?

-EDIT: (Response to the answer provided by @50k4)

I have tried your equations to find P4 for a sample coordinate that I got from the Google Sketchup 3D Designer, however I am getting big illogical values a and b and consequently for its x, y and z. I created a Google sheet which contains the design with assigned values of each point and I also implemented your equations in it to calculate a, b, and P4 as well as $\vec v_1 = P_3 - P_4$ and $\vec v_2 = P_1 - P_4$. Here is the link for this sheet that I shared and can be publicly edited:

https://docs.google.com/spreadsheets/d/1XGTqjk7kfwNi0hfktoVEtkkTNgrD1LUz8ui2Tyu455Y/edit?usp=sharing

Why I Need to Know This in The First Place!

I am an experienced software engineer with robotics and control engineering qualification. I am planning to program 3D models and create a simulation platform and solve a problem in hand for the robotics world. Clear understanding of this field in mathematics is required.

Answer 1

A general approach would be to construct a so called axis-angle representation and convert that to a rotation matrix representation.

On order to do so, one could start with a normal vector of the triangle.

Let's take the $P_1P_2$ line and construct the perpendicular of the triangle from $P_3$ to $P_1P_2$. Let's call $P_4$ the point where the perpendicular intersects $P_1P_2$ .Based on this answer the coordinates of this point can be found as:

$\alpha = \frac{(x_3-x_1) \cdot (x_2-x_1) + (y_3-y_1) \cdot (y_2-y_1) + (z_3-z_1) \cdot (z_2-z_1)} {(x_2-x_1) \cdot (x_2-x_1) + (y_2-y_1) \cdot (y_2-y_1) + (z_2-z_1) \cdot (z_2-z_1)}$

$P_4 = (x_1 + \alpha \cdot (x_2-x_1), y_1+\alpha \cdot (y_2-y_1), z_1 + \alpha \cdot (z_2-z_1))$ where:

$P1 = (x_1, y_1, z_1)$

$P2 = (x_2, y_2, z_2)$

$P3 = (x_3, y_3, z_3)$

so $P_1P_2$ and $P_3P_4$ are perpendicular and their intersection point is $P_4$.

Applied to the coordinates described in the attached google sheet file the triangle (in blue) and the perpendicular in red is presented in the following figure:

We can now construct 2 perpendicular vectors, in the $P_1P_2P_3$ plane which share their origin in $P_4$

$\vec v_1 = P_3 - P_4$

$\vec v_2 = P_1 - P_4$ (we could also use $\vec v_2 = P_2 - P_4$, this would cause an 180 deg offset around the z axis)

The cross product of these two vectors will be the normal of the $P_1P_2P_3$ plane.

$\vec v_3 = \vec v_1 \times \vec v_2$

care must be taken to make sure that $\vec v_3$ points "upwards or downwards", depending on the cross product order ($\vec v_1 \times \vec v_2$ or $\vec v_2 \times \vec v_1$). The preferred one has to be chosen.

The three vectors now form an orthogonal coordinate system. in order to get the axes of this coordinate system the vectors should be normalized.

$\vec x_n = normalize(\vec v_2) = [x_{xn}, y_{xn}, z_{xn}]$

$\vec y_n = normalize(\vec v_1) = [x_{yn}, y_{yn}, z_{yn}]$

$\vec z_n = normalize(\vec v_3) = [x_{zn}, y_{zn}, z_{zn}]$

Please note that $\vec v_2$ has been chosen as $\vec x_n$ based on the additional information in the question, that $P_1P_2$ is parallel to the $X$ axis of the belt/rectangle frame.

A rotation matrix can also be interpreted as a direction cosine matrix, based in this, the rotation matrix of the plane can be written as:

$$ R = \begin{bmatrix} x_{xn} & y_{xn} &z_{xn} \\ x_{yn} & y_{yn} & z_{yn} \\ x_{zn} & y_{zn} & z_{zn} \end{bmatrix}. $$