**2. Rigid body motion**

Rigid motion of an object is a motion that preserves distance between points [4]. Rigid body is a set of particles such that the distance between any two particles remains constant in time, regardless of any motions of the body or forces exerted on the body. If we consider **p** and **q** as two points on rigid body, while rigid body moves, **p** and **q** must satisfy k k **p**ðÞ�*t* **q**ð Þ*t* ¼ k k **p**ð Þ� 0 **q**ð Þ 0 ¼ *constant*, see **Figure 1**.

Let us consider an object, described as a subset *O* of <sup>3</sup> . Then a motion of object (rigid body) is represented by mapping *f t*ð Þ : *<sup>O</sup>* ! <sup>3</sup> . This mapping describes how the points of this object move as a function of time, relative to some fixed coordinate system.

Let the inertial reference frame be *O* ¼ *xr*, *yr*, *zr* and **i***r*, **j** *<sup>r</sup>*, **k***<sup>r</sup>* represent unit vectors of the reference frame. The vector **p** can be expressed with respect to inertial reference frame *O* ¼ *xr*, *yr*, *zr* by the following equation

**Figure 1.** *Rigid body.*

*Kinematics of Serial Manipulators DOI: http://dx.doi.org/10.5772/intechopen.93138*

**Figure 2.** *Rotation of coordinate system.*

$$\mathbf{p} = p\_{xr}\mathbf{i}\_r + p\_{yr}\mathbf{j}\_r + p\_{xr}\mathbf{k}\_r \tag{1}$$

where **p** ¼ *px*, *py*, *pz* h i*<sup>T</sup>* ∈ <sup>3</sup> . Coordinates of vector **p** can be also expressed as its projections in directions of individual unit vectors as scalar product. In order to find the relation, the vector **p** needs to be expressed in coordinates *O*<sup>1</sup> ¼ *x*1, *y*1, *z*<sup>1</sup> � �

$$p\_{xr} = \mathbf{i}\_r \mathbf{p} = \mathbf{i}\_r p\_{x1} \mathbf{i}\_1 + \mathbf{i}\_r p\_{y1} \mathbf{j}\_1 + \mathbf{i}\_r p\_{x1} \mathbf{k}\_1$$

$$p\_{yr} = \mathbf{j}\_r \mathbf{p} = \mathbf{j}\_r p\_{x1} \mathbf{i}\_1 + \mathbf{j}\_r p\_{y1} \mathbf{j}\_1 + \mathbf{j}\_r p\_{x1} \mathbf{k}\_1 \tag{2}$$

$$p\_{xr} = \mathbf{k}\_r \mathbf{p} = \mathbf{k}\_r p\_{x1} \mathbf{i}\_1 + \mathbf{k}\_r p\_{y1} \mathbf{j}\_1 + \mathbf{k}\_r p\_{x1} \mathbf{k}\_1$$

which can be rewritten in matrix form

$$
\begin{bmatrix} p\_{xr} \\ p\_{yr} \\ p\_{zr} \end{bmatrix} = \begin{bmatrix} \mathbf{i}\_r \mathbf{i}\_1 & \mathbf{i}\_r \mathbf{j}\_1 & \mathbf{i}\_r \mathbf{k}\_1 \\ \mathbf{j}\_r \mathbf{i}\_1 & \mathbf{j}\_r \mathbf{j}\_1 & \mathbf{j}\_r \mathbf{k}\_1 \\ \mathbf{k}\_r \mathbf{i}\_1 & \mathbf{k}\_r \mathbf{j}\_1 & \mathbf{k}\_r \mathbf{k}\_1 \end{bmatrix} \begin{bmatrix} p\_{x1} \\ p\_{y1} \\ p\_{z1} \end{bmatrix} \tag{3}
$$

that is **p***<sup>b</sup>* ¼ **R***<sup>r</sup>*1**p**1. The meaning of this term is as follows. Coordinates of the vector **p** expressed in *O*<sup>1</sup> ¼ *x*1, *y*1, *z*<sup>1</sup> � � are computed to *<sup>O</sup>* <sup>¼</sup> *xr*, *yr*, *zr* � � so that they are left multiplied by transformation matrix **R***r*1.

As can be seen in **Figure 2**, coordinate system *x*0, *y*0, *z*<sup>0</sup> is rotated with respect to coordinate system *xr*, *yr*, *zr* by angle *q* around the axis *zr*. By consideration of previous equations; and by consideration of the facts that scalar product of two perpendicular vectors equals zero, scalar product of two parallel unit vectors is one, and scalar product of concurrent unit vectors is cos *α*; and by assuming that cos *<sup>π</sup>* <sup>2</sup> � *<sup>α</sup>* � � <sup>¼</sup> <sup>∓</sup>sin *<sup>α</sup>*, that is

$$\mathbf{i}^T \mathbf{i} = \mathbf{1}, \mathbf{j}^T \mathbf{j} = \mathbf{1}, \mathbf{k}^T \mathbf{k} = \mathbf{1}$$

$$\mathbf{i}^T \mathbf{j} = \mathbf{0}, \mathbf{j}^T \mathbf{k} = \mathbf{0}, \mathbf{k}^T \mathbf{i} = \mathbf{0}$$

one can obtain the following rotation matrix

$$\mathbf{R}\_{\mathbf{x}} = \begin{bmatrix} \mathbf{1} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \cos a & -\sin a \\ \mathbf{0} & \sin a & \cos a \end{bmatrix} \tag{4}$$

where **R***<sup>x</sup>* is a rotation matrix for rotation around the *x*-axis by angle *α*. Subsequently, rotation matrices can be also be expressed for rotation around y-axis and z-axis

$$\mathbf{R}\_{\mathcal{Y}} = \begin{bmatrix} \cos \beta & 0 & \sin \beta \\ 0 & 1 & 0 \\ -\sin \beta & 0 & \cos \beta \end{bmatrix} \tag{5}$$
 
$$\mathbf{R}\_{\mathcal{z}} = \begin{bmatrix} \cos \gamma & -\sin \gamma & 0 \\ \sin \gamma & \cos \gamma & 0 \\ 0 & 0 & 1 \end{bmatrix} \tag{6}$$

Since rotation matrix **R** is an orthogonal matrix, for this reason

$$\mathbf{R}^T \mathbf{R} = \mathbf{I}\_3 \tag{7}$$

where **I**<sup>3</sup> is a 3 � 3 identity matrix. Considering the case when there is displacement of local coordinate system and at the same time also its rotation, it would be expressed as

$$
\begin{bmatrix} x\_r \\ y\_r \\ z\_r \end{bmatrix} = \mathbf{R}\_{axis, angle} \begin{bmatrix} x\_1 \\ y\_1 \\ z\_1 \end{bmatrix} + \begin{bmatrix} p\_x \\ p\_y \\ p\_x \end{bmatrix} \tag{8}
$$

Eq. (8) represents a system of three equations, which will be extended by fourth equation 1 = 0 + 0 + 0 + 1, which is

$$
\begin{bmatrix} \mathbf{x}\_r \\ \mathbf{y}\_r \\ z\_r \\ \mathbf{1} \end{bmatrix} = \begin{bmatrix} & \vdots & p\_x \\ & \mathbf{R}\_{axis, angle} & \vdots & p\_y \\ \cdots & \cdots & \vdots & p\_z \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{1} \end{bmatrix} \begin{bmatrix} \mathbf{x}\_1 \\ \mathbf{y}\_1 \\ z\_1 \\ \mathbf{1} \end{bmatrix} \tag{9}
$$

#### **3. Forward kinematics**

The forward kinematic model determines the position and orientation of the endeffector relating to base frame of the mechanism or to global coordinate system (GCS).

#### **3.1 Open kinematic chain**

We will focus on robots, which contain a set of links connected together by joints. The joints are usually revolute or prismatic or they can be more complex,

#### *Kinematics of Serial Manipulators DOI: http://dx.doi.org/10.5772/intechopen.93138*

such as socket joint or ball joint [5]. Within this chapter will be considered only revolute and prismatic joints, which have only a single degree-of-freedom motion. Let us consider a mechanism with *N* links connected together by *N* � 1 joints. The *i*th joint connects link *i* � 1 to link *i*. The number of the joints starts with 1 and ends with *N* � 1. The next consideration for the following mathematical model is that the first link is connected to the base fixed to inertial reference frame, while the last link is free and able to move.

The *i*-th joint is associated with joint variable *qi* , while *qi* may contain *θ<sup>i</sup>* and *di* for revolute and prismatic joints, respectively. The local coordinate frame is attached to each link, so to *i*-th link is attached to *Ii* frame, *Ii* ¼ *Oi*, *xi*, *yi* , *zi* � �. When a mechanism performs any motion in its workspace, the coordinates of each point on *i*-th link are constant with respect to their coordinate frame

$$I\_i = \left\{O\_i, \underbrace{x\_i, y\_i, z\_i}\_{\cdot}\right\}.$$

Let **A***<sup>i</sup>* be a homogeneous transformation matrix, which holds position and orientation of frame *Ii* ¼ *Oi*, *xi*, *yi* , *zi* � � with respect to *Ii*�<sup>1</sup> <sup>¼</sup> *Oi*�1, *xi*�1, *yi*�<sup>1</sup>, *zi*�<sup>1</sup> � �. It should be noticed that values of matrix **A***<sup>i</sup>* are not constant, but they change with changing configuration of the mechanism. In general, a homogeneous transformation matrix expressing the position and orientation of *Ij* ¼ *O <sup>j</sup>*, *x <sup>j</sup>*, *y <sup>j</sup>* , *z <sup>j</sup>* n o with respect to *Ii* ¼ *Oi*, *xi*, *yi* , *zi* � � is called a transformation matrix *<sup>i</sup>* **T** *<sup>j</sup>*. We can also define the following matrix

$$\mathbf{H} = \begin{bmatrix} \,^0 \mathbf{R}\_n & \,^0 \mathbf{o}\_n \\ \mathbf{0} & \mathbf{1} \end{bmatrix} \tag{10}$$

where <sup>0</sup>**R***<sup>n</sup>* is a 3 � 3 rotation matrix with and <sup>0</sup>**o***<sup>n</sup>* is a 3 � 1 vector expressing position and orientation of end-effector (the last point of mechanism) with respect to inertial reference frame (base of mechanism). Eq. (10) can then be written as

$$\mathbf{H} = {}^{0}\mathbf{T}\_{n} = \prod\_{i=1}^{N} \mathbf{A}\_{i} \tag{11}$$

while **A***<sup>i</sup>* equals

$$\mathbf{A}\_{i} = \begin{bmatrix} \mathbf{^{i-1}R\_{i}} & \mathbf{^{i-1}o\_{i}} \\ \mathbf{0} & \mathbf{1} \end{bmatrix} \tag{12}$$

### **3.2 Denavit–Hartenberg convention**

For the computation of forward kinematics for open-chain robot according to Eq. (11), a general approach was derived in order to determine the relative position and orientation of two consecutive links. This approach determines two frames attached to two links (rigid bodies) and computes the coordinate transformations between them [6].

For utilization of the Denavit-Hartenberg convention, some rules need to be observed. Let us consider **Figure 3**. Let axis *i* represent the axis connecting link *i* � 1 and link *i* þ 1. In order to define link frame *i*, the procedure is as follows. First of all, the axis *zi* and axis *zi*�<sup>1</sup> are chosen. Next, origin *Oi* is located at the intersection of axis *zi* with the common normal to axes *zi* and *zi*�1. By this step be get points *Oi* and *O*0 *i* . The common normal of these two axes is a minimum distance between them. Subsequently, the axis *xi* is chosen along the common normal to axes *zi*�<sup>1</sup> and *zi* in

**Figure 3.** *Denavit-Hartenberg approach.*

the direction from joint *i* to the joint *i* þ 1. In the last step, axis *yi* is chosen so as to complete a right-handed frame.

After these steps, the link frames have been established and now the position and orientation of frame *i* with respect to frame *i* � 1 can be determined by following DH parameters [7]:


It should be also noted that parameters *ai* and *α<sup>i</sup>* are always constant, because they depend on the geometric aspect of mechanism. Considering the two other parameters *di* and *ϑi*, depending on the joint type, one is constant and other one may change as follows:


In general, six parameters are necessary in order to describe the position and orientation of a rigid body in the 3D space. Based on previously mentioned facts, we can say about DH convention that only four parameters are required by assuming that the axis *xi* intersects *zi*�1, and that axis *xi* is perpendicular to *zi*�1.

### *3.2.1 Example of forward kinematics using the Denavit-Hartenberg convention*

Let us consider some kind of industrial robot, namely SCARA (Selective Compliance Assembly Robot Arm) robot, which has RRP structure. Its kinematic structure is shown in **Figure 4**.

### *Kinematics of Serial Manipulators DOI: http://dx.doi.org/10.5772/intechopen.93138*

Considering the basic principles of the Denavit-Hartenberg convention intro-

duced in the previous section, we are able to introduce D-H parameters, see **Table 1**. Based on DH parameters, which are obvious from **Figure 4**, particular homogeneous transformation matrices can be established.

$${}^{0}
\mathbf{A}\_{1} = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & d\_{1} \\ 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} \cos\left(q\_{1}\right) & -\sin\left(q\_{1}\right) & 0 & 0 \\ \sin\left(q\_{1}\right) & \cos\left(q\_{1}\right) & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} 1 & 0 & 0 & L\_{1} \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \tag{13}$$

$${}^{0}\mathbf{A}\_{1} = \begin{bmatrix} \cos\left(q\_{1}\right) & -\sin\left(q\_{1}\right) & 0 & L\_{1}\cos\left(q\_{1}\right) \\ \sin\left(q\_{1}\right) & \cos\left(q\_{1}\right) & 0 & L\_{1}\sin\left(q\_{1}\right) \\ 0 & 0 & 1 & d\_{1} \\ 0 & 0 & 0 & 1 \end{bmatrix} \tag{14}$$

$${}^{1}\mathbf{A}\_{2} = \begin{bmatrix} \cos\left(q\_{2}\right) & -\sin\left(q\_{2}\right) & 0 & 0 \\ \sin\left(q\_{2}\right) & \cos\left(q\_{2}\right) & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} 1 & 0 & 0 & L\_{2} \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \tag{15}$$

**Figure 4.** *SCARA robot.*


**Table 1.** *Denavit-Hartenberg parameters.*

$$\begin{aligned} \mathbf{^1A\_2} &= \begin{bmatrix} \cos\left(q\_2\right) & -\sin\left(q\_2\right) & 0 & L\_2\cos\left(q\_2\right) \\ \sin\left(q\_2\right) & \cos\left(q\_2\right) & 0 & L\_2\sin\left(q\_2\right) \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \\ \mathbf{^2A\_3} &= \begin{bmatrix} \mathbf{1} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{1} \\ \mathbf{0} & \cos\left(\pi\right) & -\sin\left(\pi\right) & \mathbf{0} \\ \mathbf{0} & \sin\left(\pi\right) & \cos\left(\pi\right) & d\_2 + q\_3 \\ \mathbf{0} & \sin\left(\pi\right) & \cos\left(\pi\right) & d\_2 + q\_3 \end{bmatrix} \\ \mathbf{^3A\_4} &= \begin{bmatrix} \cos\left(q\_4\right) & -\sin\left(q\_4\right) & \mathbf{0} & \mathbf{0} \\ \sin\left(q\_4\right) & \cos\left(q\_4\right) & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{1} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{1} \end{bmatrix} \tag{16} \end{aligned} \tag{17}$$

So, the final transformation matrix is

$$\mathbf{^0T\_4 = {}^0\mathbf{A\_2}^1\mathbf{A\_2}^2\mathbf{A\_3}^3\mathbf{A\_4}}\tag{17}$$

$$\begin{aligned} \,^0 \mathbf{T}\_4 = \begin{bmatrix} p\_x \\ & \mathbf{R} & p\_y \\ & & p\_z \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{1} \end{bmatrix} \end{aligned} \tag{18}$$

By vector <sup>0</sup>**o**<sup>4</sup> <sup>¼</sup> *pxpypz* h i*<sup>T</sup>* is defined position of end-effector of SCARA manipulator with respect to its base inertial reference frame.
