Any two independent orthonormal coordinate frames can be related by a sequence of rotations (not more than three) about coordinate axes, where no two successive rotations may be about the same axis.

The illustration of the world reference frame, {M}, and the body reference frame, {B}, is shown in the following figure.

Pan angle rotation, α, is acquired using computer vision. And, tilt angle rotation, β, and roll angle rotation, γ, are acquired by sensing the gravity, g, using onboard accelerometers. Let us consider the roll angle first, assuming there are no pan and tilt angles. When the roll angle, γ, is zero Y-axis is upward vertical and X-axis is horizontal pointing according to right handed rule. The gravity sensed by X-axis accelerometer and Y-axis accelerometer are denoted by gx and gy respectively. The definition of the roll angle is illustrated in the following figure.

The roll angle, γ, is calculated as

$$ \gamma = \tan^{-1}\frac{gx}{gy} $$.

Then, it is checked for second and third quadrants as

if(gy<0) if(gx>=0) γ=π+γ; else γ=-π+γ; end endThe roll angle rotation matrix,

**R**

_{γ}, is obtained as follows.

$$ \mathbf{R}_{\gamma}= \begin{bmatrix} \cos(\gamma) & -\sin(\gamma) & 0 \\ \sin(\gamma) & \cos(\gamma) & 0 \\ 0 & 0 & 1 \end{bmatrix}$$

After the roll angle is corrected, let us consider for tilt angle. The definition of the tilt angle is illustrated in the following figure.

The tilt angle, β, can be calculated from Y and Z components of the gravity. It is important to note that these gravity component values should be in the roll angle rotated frame. The new values for the gravity components are obtained as

$$ \begin{bmatrix} gx2 \\ gy2 \\ gz2 \end{bmatrix} = \mathbf{R}_{\gamma} \begin{bmatrix} gx \\ gy \\ gz \end{bmatrix} $$

The tilt angle, β, is calculated as

$$\beta = \tan^{-1}\frac{gy2}{gz2} $$.

Then, it is checked for second and third quadrants as

if(gz2>=0) if(gy2>=0) β=-π+β; else β=π+β; end endThe tilt angle rotation matrix,

**R**

_{β}, is obtained as follows.

$$ \mathbf{R}_{\beta}= \begin{bmatrix} 1 & 0 & 0\\ 0 & \cos(\gamma) & -\sin(\gamma) \\ 0 & \sin(\gamma) & \cos(\gamma) \end{bmatrix}$$

Similarly, calculation of the pan angle rotation matrix,

**R**

_{α}, and description of pan angle definition are as follows.

$$ \mathbf{R}_{\alpha}= \begin{bmatrix} \cos(\alpha) & -\sin(\alpha) & 0 \\ \sin(\alpha) & \cos(\alpha) & 0 \\ 0 & 0 & 1 \end{bmatrix}$$

The product of rotation matrices is itself a rotation matrix. $$ \mathbf{R}=\mathbf{R}_{\alpha} \mathbf{R}_{\beta} \mathbf{R}_{\gamma}$$

And,

**M**=

**R**

**B**

Since

**R**is a rotation matrix, inverse of

**R**is obtained by transposing

**R**. The implementation of this 3D rotation in LabVIEW using MabLab code is shown in the following figure.

Quaternion algebra is also easy and popular approach for such 3D transformation.

Reference:

Kuipers, Jack B., Quaternions and rotation sequences : a primer with applications to orbits, aerospace, and virtual reality, Princeton University Press, 1999, ISBN: 0691058725.

## No comments:

## Post a Comment

Comments are moderated and don't be surprised if your comment does not appear promptly.