Robotics Kinematics and Dynamics/Description of Position and Orientation

In general, a rigid body in three-dimensional space has six degrees of freedom: three rotational and three translational.

The position and orientation of a rigid body can be described by attaching a frame to it.

A conventional way to describe the position and orientation of a rigid body is to attach a frame to it. After defining a reference coordinate system, the position and orientation of the rigid body are fully described by the position of the frame's origin and the orientation of its axes, relative to the reference frame.

Rotation Matrix

A rotation matrix describes the relative orientation of two such frames. The columns of this 3 × 3 matrix consist of the unit vectors along the axes of one frame, relative to the other, reference frame. Thus, the relative orientation of a frame $\{b\}$ with respect to a reference frame $\{a\}$ is given by the rotation matrix $_{a}^{b}R\,$:

$_{b}^{a}R={\begin{pmatrix}_{a}x^{b}&_{a}y^{b}&_{a}z^{b}\end{pmatrix}}={\begin{pmatrix}x^{b}\cdot x^{a}&y^{b}\cdot x^{a}&z^{b}\cdot x^{a}\\x^{b}\cdot y^{a}&y^{b}\cdot y^{a}&z^{b}\cdot y^{a}\\x^{b}\cdot z^{a}&y^{b}\cdot z^{a}&z^{b}\cdot z^{a}\end{pmatrix}}$

Rotation matrices can be interpreted in two ways:

1. As the representation of the rotation of the first frame into the second (active interpretation).
2. As the representation of the mutual orientation between two coordinate systems (passive interpretation).

The coordinates, relative to the reference frame $\{a\}$, of a point $p$, of which the coordinates are known with respect to a frame $\{b\}$ with the same origin, can then be calculated as follows: $_{a}p=\,_{b}^{a}R\,_{b}p$.

Properties

Some of the properties of the rotation matrix that may be of practical value, are:

1. The column vectors of $_{a}^{b}R\,$ are normal to each other.
2. The length of the column vectors of $_{a}^{b}R\,$ equals 1.
3. A rotation matrix is a non-minimal description of a rigid body's orientation. That is, it uses nine numbers to represent an orientation instead of just three. (The two above properties correspond to six relations between the nine matrix elements. Hence, only three of them are independent.) Non-minimal representations often have some numerical advantages, though, as they do not exhibit coordinate singularities.
4. Since $_{a}^{b}R\,$ is orthonormal, $_{b}^{a}R=\,_{a}^{b}R^{-1}=\,_{a}^{b}R^{T}$.

Elementary Rotations about Frame Axes

Rotation by an angle $\gamma$ about the z-axis.

The expressions for elementary rotations about frame axes can easily be derived. From the figure on the right, it can be seen that the rotation of a frame by an angle $\gamma$ about the z-axis, is described by:

$R_{z}(\gamma )={\begin{pmatrix}\cos(\gamma )&-\sin(\gamma )&0\\\sin(\gamma )&\cos(\gamma )&0\\0&0&1\end{pmatrix}}$

Similarly, it can be shown that the rotation of a frame by an angle $\alpha$ about the x-axis, is given by:

$R_{x}(\alpha )={\begin{pmatrix}1&0&0\\0&\cos(\alpha )&-\sin(\alpha )\\0&\sin(\alpha )&\cos(\alpha )\end{pmatrix}}$

Derived in exactly the same manner, the rotation of a frame by an angle $\beta$ about the y-axis, is described by:

$R_{y}(\beta )={\begin{pmatrix}\cos(\beta )&0&\sin(\beta )\\0&1&0\\-\sin(\beta )&0&\cos(\beta )\end{pmatrix}}$

Compound Rotations

Compound rotations are found by multiplication of the different elementary rotation matrices.

The matrix corresponding to a set of rotations about moving axes can be found by postmultiplying the rotation matrices, thus multiplying them in the same order in which the rotations take place. The rotation matrix formed by a rotation by an angle $\alpha$ about the z-axis followed by a rotation by an angle $-\beta$ about the moved y-axis, is then given by:

$R(zy,\alpha ,-\beta )=R(z,\alpha )R(y,-\beta )$

The composition of rotations about fixed axes, on the other hand, is found by premultiplying the different elementary rotation matrices.

Inverse Rotations

The inverse of a single rotation about a frame axis is a rotation by the negative of the rotation angle about the same axis:

$R^{-1}(z,\alpha )=R(z,-\alpha )$

The inverse of a compound rotation follows from the inverse of the matrix product:

$R^{-1}(zy,\alpha ,-\beta )=R^{-1}(y,-\beta )R^{-1}(z,\alpha )$

MATLAB Example

Note: These examples require the Robotics Toolbox to be properly installed.

theta = pi/2;
T_x = rotx(theta); % Returns a 4x4 pose matrix. The upper-left 3x3 submatrix is the
                   % rotation matrix representing a rotation by theta about the x-axis.
R_x = tr2rt(T_x); % Returns the 3x3 rotation matrix corresponding with T_x.
T_y = roty(theta); % A rotation about the y-axis.
T_z = rotz(theta); % A rotation about the z-axis.

Euler Angles

Contrary to the rotation matrix, Euler angles are a minimal representation (a set of just three numbers, that is) of relative orientation. This set of three angles describes a sequence of rotations about the axes of a moving reference frame. There are, however, many (12, to be exact) sets that describe the same orientation: different combinations of axes (e.g. ZXZ, ZYZ, and so on) lead to different Euler angles. Euler angles are often used for the description of the orientation of the wrist-like end-effectors of many serial manipulator robots.

Note: Identical axes should not be in consecutive places (e.g. ZZX). Also, the range of the Euler angles should be limited in order to avoid different angles for the same orientation. E.g.: for the case of ZYZ Euler angles, the first rotation about the z-axis should be within $[-\pi ,\pi ]$. The second rotation, about the moved y-axis, has a range of $[-\pi /2,\pi /2]$. The last rotation, about the moved z-axis, has a range of $[-\pi ,\pi ]$.

Forward Mapping

Forward mapping, or finding the orientation of the end-effector with respect to the base frame, follows from the composition of rotations about moving axes. For a rotation by an angle $\alpha$ about the z-axis, followed by a rotation by an angle $\beta$ about the moved x-axis, and a final rotation by an angle $\gamma$ about the moved z-axis, the resulting rotation matrix is:

$_{bs}^{ee}R\,=R(zxz,\alpha ,\beta ,\gamma )=R(z,\alpha )R(x,\beta )R(z,\gamma )$

After writing out:

$R(zxz,\alpha ,\beta ,\gamma )={\begin{pmatrix}c_{\gamma }c_{\alpha }-s_{\gamma }c_{\beta }s_{\alpha }&-s_{\gamma }c_{\alpha }-c_{\gamma }c_{\beta }s_{\alpha }&s_{\beta }s_{\alpha }\\c_{\gamma }s_{\alpha }+s_{\gamma }c_{\beta }c_{\alpha }&-s_{\gamma }s_{\alpha }+c_{\gamma }c_{\beta }c_{\alpha }&-s_{\beta }c_{\alpha }\\s_{\gamma }s_{\beta }&c_{\gamma }s_{\beta }&c_{\beta }\end{pmatrix}}$

Note: Notice the shorthand notation: $s_{\alpha }$ stands for $\sin \alpha$, $c_{\alpha }$ stands for $\cos \alpha$, and so on.

Inverse Mapping

In order to drive the end-effector, the inverse problem must be solved: given a certain orientation matrix, which are the Euler angles that accomplish this orientation?

For the above case, the Euler angles $\alpha$, $\beta$ and $\gamma$ are found by inspection of the rotation matrix:

$\alpha ={\textrm {atan2}}(R_{13},-R_{23})$

$\beta ={\textrm {atan2}}(-R_{23}c_{\alpha }+R_{13}s_{\alpha },R_{33})$

$\gamma ={\textrm {atan2}}(R_{31},R_{32})$

Coordinate Singularities

In the above example, a coordinate singularity exists for $\beta =0$. The above equations are badly numerically conditioned for small values of $\beta$: the first and last equation become undefined. This corresponds with an alignment of the first and last axes of the end-effector. The occurrence of a coordinate singularity involves the loss of a degree of freedom: in the case of the above example, small rotations about the y-axis require impossibly large rotations about the x- and z-axes.

No minimal representation of orientation can globally describe all orientations without coordinate singularities occurring.

Roll-Pitch-Yaw Angles

The orientation of a rigid body can equally well be described by three consecutive rotations about fixed axes. This leads to a notation with Roll-Pitch-Yaw (RPY) angles.

Forward Mapping

The forward mapping of RPY angles to a rotation matrix similar to that of Euler angles. Since the frame now rotates about fixed axes instead of moving axes, the order in which the different rotation matrices are multiplied is inversed:

$R(RPY,r,p,y)=R(z,y)R(y,p)R(x,r)$

After writing out:

$R(RPY,r,p,y)={\begin{pmatrix}c_{y}c_{p}&c_{y}s_{p}s_{r}-s_{y}c_{r}&c_{y}s_{p}c_{r}+s_{y}s_{r}\\s_{y}c_{p}&s_{y}s_{p}s_{r}+c_{y}c_{r}&s_{y}s_{p}c_{r}-c_{y}s_{r}\\-s_{p}&c_{p}s_{r}&c_{p}c_{r}\end{pmatrix}}$

Inverse Mapping

The inverse relationships are found from inspection of the rotation matrix above:

$r={\textrm {atan2}}(R_{32},R_{33})$

$y={\textrm {atan2}}(R_{21},R_{11})$

$p={\textrm {atan2}}(-R_{31},c_{y}R_{11}+s_{y}R_{21})$

Note: The above equations are badly numerically conditioned for values of $p$ near $\pi /2$ and $-\pi /2$.

Unit Quaternions

Unit quaternions (quaternions of which the absolute value equals 1) are another representation of orientation. They can be seen as a compromise between the advantages and disadvantages of rotation matrices and Euler angle sets.

Homogeneous Transform

The notations above describe only relative orientation. The coordinates of a point, relative to a frame $\{b\}$, rotated and translated with respect to a reference frame $\{a\}$, are given by:

$_{a}p=\,_{a}^{b}R\,_{b}p+_{a}p^{a,b}$

This can be compacted into the form of a homogeneous transformation matrix or pose (matrix). It is defined as follows:

$_{a}^{b}T=\left({\begin{array}{cc}^{b}_{a}R&_{a}p^{a,b}\\0_{1\times 3}&1\end{array}}\right)$

This matrix represents the position and orientation of a frame $\{b\}$ whose origin, relative to a reference frame $\{a\}$, is described by $_{a}p^{a,b}$, and whose orientation, relative to the same reference frame $\{a\}$, is described by the rotation matrix $_{a}^{b}R\,$.

$_{a}^{b}T\,$ is, thus, the representation of a frame in three-dimensional space. If the coordinates of a point $p$ are known with respect to a frame $\{b\}$, then its coordinates, relative to ${a}$ are found by:

${\begin{pmatrix}_{a}p\\1\end{pmatrix}}=\,_{a}^{b}T{\begin{pmatrix}_{b}p\\1\end{pmatrix}}$

This is the same as writing:

$_{a}p=\,_{a}^{b}R\,_{b}p+_{a}p^{a,b}$

Note that the above vectors are extended with a fourth coordinate equal to one: they're made homogeneous.

As was the case with rotation matrices, homogeneous transformation matrices can be interpreted in an active ("displacement"), and a passive ("pose") manner. It is also a non-minimal representation of a pose, that does not suffer from coordinate singularities.

Compound Poses

If the pose of a frame $\{c\}$ is known, relative to $\{b\}$, whose pose is known with respect to a third frame $\{a\}$, the resulting pose $_{a}^{c}T\,$ is found as follows:

$_{a}^{c}T=\,_{a}^{b}T\,_{b}^{c}T$

MATLAB Example

x = 1;
y = 1.3;
z = 0.4;
T = transl(x,y,z); % Returns the pose matrix corresponding with a translation over
                   % the vector (x, y, z)'.

Finite Displacement Twist

A pose matrix is a non-minimal way of describing a pose. A frequently used minimal alternative is the finite displacement twist:

$t={\begin{pmatrix}\alpha \\\beta \\\gamma \\x\\y\\z\end{pmatrix}}$

Here, $\alpha$, $\beta$ and $\gamma$ are a valid set (any) of Euler angles, while $x$, $y$ and $z$ are the coordinates of a reference point on the rigid body.