Kinematics of Serial Manipulators

Abstract

This book chapter deals with kinematic modeling of serial robot manipulators (open-chain multibody systems) with focus on forward as well as inverse kinematic model. At first, the chapter describes basic important definitions in the area of manipulators kinematics. Subsequently, the rigid body motion is presented and basic mathematical apparatus is introduced. Based on rigid body conventions, the forward kinematic model is established including one of the most used approaches in robot kinematics, namely the Denavit-Hartenberg convention. The last section of the chapter analyzes inverse kinematic modeling including analytical, geometrical, and numerical solutions. The chapter offers several examples of serial manipulators with its mathematical solution.

1. Introduction and basic definitions

In the following sections, this chapter will deal with direct and inverse kinematics of open-chain multibody systems consisting of rigid bodies. The whole problematics is analyzed from the view of robotics. Each manipulator or mechanism investigated in this chapter will be of serial kinematic structure (open chain).

Open-chain multibody systems are mechanically constructed by connecting a set of bodies, called links, by means of various types of joints. In general, the joints can be passive or active. The joints, which are moved by actuators, are active joints.

In general, from the view of robotics, there are two tasks in kinematics:

• Forward kinematics—the forward kinematics problem represents relationship between individual joints of investigated robot and end-effector.

• Inverse kinematics—the problem of inverse kinematics is as follows: given a desired configuration of end-effector of robot, find the joint angles that achieve that configuration.

Before these terms are explained and demonstrated by some study cases, we have to mention the basic definitions, necessary for the further analyses.

Degrees of freedom (DOF): is the smallest number of coordinates needed to represent the robot configuration. Thus, the number of DOF equals to the dimension of configuration space.

Joint space: Let us define all the joint variables in a vector $\mathbf{q}={\left[q_1,q_2,\dots ,q_n\right]}^T\in \mathbb{Q}\subset {\mathbb{R}}^N$ . The set $\mathbb{Q}$ we call the so-called joint space and it contains all the possible values, which joint variables may acquire.

Workspace: Workspace is a subset of the Euclidean space $\mathbb{E}$ , in which the robot executes its tasks. From the view of robotics, workspace is the set of all the points that mechanism may reach in Euclidean space $\mathbb{E}$ by end-effector. The workspace can be categorized as follows [1, 2]:

Maximal workspace—it is defined as locations that can be reached by end-effector at least with one orientation.

Inclusive-orientation workspace—it is defined as locations that can be reached by end-effector with at least one orientation among a range of orientations (maximal workspace is particular case).

Constant-orientation workspace—it is defined as location that can be reached by the end-effector with fixed orientation of joints.

Total-orientation workspace—it is defined as location that can be reached by the end-effector with any orientation.

Dexterity workspace—it is defined as location that can be reached by the end-effector with any orientation and without kinematic singularities.

Task space–space of positions and orientations of the end-effector frame. The workspace is a subset of task space that the end-effector frame can reach [3].

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 $\mathbf{p}$ and $\mathbf{q}$ as two points on rigid body, while rigid body moves, $\mathbf{p}$ and $\mathbf{q}$ must satisfy $‖\mathbf{p}\left(t\right)-\mathbf{q}\left(t\right)‖=‖\mathbf{p}\left(0\right)-\mathbf{q}\left(0\right)‖=\mathit{\text{constant}}$ , see Figure 1.

Figure 1.

Rigid body.

Let us consider an object, described as a subset O of ${\mathbb{R}}^3$ . Then a motion of object (rigid body) is represented by mapping $f\left(t\right):O\to {\mathbb{R}}^3$ . 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=\left\{x_r,y_r,z_r\right\}$ and ${\mathbf{i}}_r,{\mathbf{j}}_r,{\mathbf{k}}_r$ represent unit vectors of the reference frame. The vector $\mathbf{p}$ can be expressed with respect to inertial reference frame $O=\left\{x_r,y_r,z_r\right\}$ by the following equation

$$\mathbf{p}=p_{\mathit{xr}}{\mathbf{i}}_r+p_{\mathit{yr}}{\mathbf{j}}_r+p_{\mathit{zr}}{\mathbf{k}}_r$$ E1

where $\mathbf{p}={\left[p_x,p_y,p_z\right]}^T\in {\mathbb{R}}^3$ . Coordinates of vector $\mathbf{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 $\mathbf{p}$ needs to be expressed in coordinates $O_1=\left\{x_1,y_1,z_1\right\}$

$$\begin{array}{c}{p}_{\mathit{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}_{z1}{\mathbf{k}}_1\\ p_{\mathit{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}_{z1}{\mathbf{k}}_1\\ p_{\mathit{zr}}={\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}_{z1}{\mathbf{k}}_1\end{array}$$ E2

which can be rewritten in matrix form

$$\left[\begin{array}{c}{p}_{\mathit{xr}}\\ p_{\mathit{yr}}\\ p_{\mathit{zr}}\end{array}\right]=\left[\begin{array}{ccc}{\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{array}\right]\left[\begin{array}{c}{p}_{x1}\\ p_{y1}\\ p_{z1}\end{array}\right]$$ E3

that is ${\mathbf{p}}_b={\mathbf{R}}_{r1}{\mathbf{p}}_1$ . The meaning of this term is as follows. Coordinates of the vector $\mathbf{p}$ expressed in $O_1=\left\{x_1,y_1,z_1\right\}$ are computed to $O=\left\{x_r,y_r,z_r\right\}$ so that they are left multiplied by transformation matrix ${\mathbf{R}}_{r1}$ .

As can be seen in Figure 2, coordinate system $x_0,y_0,z_0$ is rotated with respect to coordinate system $x_r,y_r,z_r$ by angle q around the axis $z_r$ . 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\alpha$ ; and by assuming that $cos\left(\frac{\pi }{2}\pm \alpha \right)=\mp sin\alpha$ , that is

Figure 2.

Rotation of coordinate system.

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

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

one can obtain the following rotation matrix

$${\mathbf{R}}_x=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\alpha & -sin\alpha \\ 0& sin\alpha & cos\alpha \end{array}\right]$$ E4

where ${\mathbf{R}}_x$ is a rotation matrix for rotation around the x-axis by angle $\alpha$ . Subsequently, rotation matrices can be also be expressed for rotation around y-axis and z-axis

$${\mathbf{R}}_y=\left[\begin{array}{ccc}cos\beta & 0& sin\beta \\ 0& 1& 0\\ -sin\beta & 0& cos\beta \end{array}\right]$$ E5

$${\mathbf{R}}_z=\left[\begin{array}{ccc}cos\gamma & -sin\gamma & 0\\ sin\gamma & cos\gamma & 0\\ 0& 0& 1\end{array}\right]$$ E6

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

$${\mathbf{R}}^T\mathbf{R}={\mathbf{I}}_3$$ E7

where ${\mathbf{I}}_3$ is a $3\times 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

$$\left[\begin{array}{c}{x}_r\\ y_r\\ z_r\end{array}\right]={\mathbf{R}}_{\mathit{\text{axis}},\mathit{\text{angle}}}\left[\begin{array}{c}{x}_1\\ y_1\\ z_1\end{array}\right]+\left[\begin{array}{c}{p}_x\\ p_y\\ p_z\end{array}\right]$$ E8

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

$$\left[\begin{array}{c}{x}_r\\ y_r\\ z_r\\ 1\end{array}\right]=\left[\begin{array}{cccc}& & ⋮& p_x\\ & {\mathbf{R}}_{\mathit{\text{axis}},\mathit{\text{angle}}}& ⋮& p_y\\ \cdots & \dots & ⋮& p_z\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_1\\ y_1\\ z_1\\ 1\end{array}\right]$$ E9

3. Forward kinematics

The forward kinematic model determines the position and orientation of the end-effector 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, 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 $q_i$ , while $q_i$ may contain ${\theta }_i$ and $d_i$ for revolute and prismatic joints, respectively. The local coordinate frame is attached to each link, so to i-th link is attached to $I_i$ frame, $I_i=\left\{O_i,x_i,y_i,z_i\right\}$ . 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,x_i,y_i,z_i\right\}$ .

Let ${\mathbf{A}}_i$ be a homogeneous transformation matrix, which holds position and orientation of frame $I_i=\left\{O_i,x_i,y_i,z_i\right\}$ with respect to $I_{i-1}=\left\{O_{i-1},x_{i-1},y_{i-1},z_{i-1}\right\}$ . It should be noticed that values of matrix ${\mathbf{A}}_i$ are not constant, but they change with changing configuration of the mechanism. In general, a homogeneous transformation matrix expressing the position and orientation of $I_j=\left\{O_j,x_j,y_j,z_j\right\}$ with respect to $I_i=\left\{O_i,x_i,y_i,z_i\right\}$ is called a transformation matrix ${{}^i\mathbf{T}}_j$ . We can also define the following matrix

$$\mathbf{H}=\left[\begin{array}{cc}{{}^0\mathbf{R}}_n& {{}^0\mathbf{o}}_n\\ 0& 1\end{array}\right]$$ E10

where ${{}^0\mathbf{R}}_n$ is a $3\times 3$ rotation matrix with and ${{}^0\mathbf{o}}_n$ is a $3\times 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$$ E11

while ${\mathbf{A}}_i$ equals

$${\mathbf{A}}_i=\left[\begin{array}{cc}{{}^{i-1}\mathbf{R}}_i& {{}^{i-1}\mathbf{o}}_i\\ 0& 1\end{array}\right]$$ E12

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 $z_i$ and axis $z_{i-1}$ are chosen. Next, origin $O_i$ is located at the intersection of axis $z_i$ with the common normal to axes $z_i$ and $z_{i-1}$ . By this step be get points $O_i$ and $O_i^{\prime }$ . The common normal of these two axes is a minimum distance between them. Subsequently, the axis $x_i$ is chosen along the common normal to axes $z_{i-1}$ and $z_i$ in the direction from joint $i$ to the joint $i+1$ . In the last step, axis $y_i$ is chosen so as to complete a right-handed frame.

Figure 3.

Denavit-Hartenberg approach.

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]:

• $a_i$ : distance between the points $O_i$ and $O_i^{\prime }$

• $d_i$ : distance between $O_{i-1}$ and $O_i^{\prime }$ along the axis $z_{i-1}$

• ${\alpha }_i$ : angle between the axes $z_{i-1}$ and $z_i$ about axis $x_i$ (positive direction—counter-clockwise rotation)

• ${\vartheta }_i$ : angle between axes $x_i$ and $x_{i-1}$ (positive direction—counter-clockwise rotation)

It should be also noted that parameters $a_i$ and ${\alpha }_i$ are always constant, because they depend on the geometric aspect of mechanism. Considering the two other parameters $d_i$ and ${\vartheta }_i$ , depending on the joint type, one is constant and other one may change as follows:

• Revolute joint: ${\vartheta }_i$ is the joint variable and $d_i$ is constant

• Prismatic joint: $d_i$ is the joint variable and ${\vartheta }_i$ is constant

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 $x_i$ intersects $z_{i-1}$ , and that axis $x_i$ is perpendicular to $z_{i-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.

Figure 4.

SCARA robot.

Considering the basic principles of the Denavit-Hartenberg convention introduced in the previous section, we are able to introduce D-H parameters, see Table 1.

Link	ai	αi	di	ϑi
1	L1	0	d1	q1
2	L2	0	0	q2
3	0	π	d2 + q3	0
4	0	0	0	q4

Table 1.

Denavit-Hartenberg parameters.

Based on DH parameters, which are obvious from Figure 4, particular homogeneous transformation matrices can be established.

$${}^0{\mathbf{A}}_1=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& d_1\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}\mathit{cos}\left(q_1\right)& -\mathit{sin}\left(q_1\right)& 0& 0\\ \mathit{sin}\left(q_1\right)& \mathit{cos}\left(q_1\right)& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}1& 0& 0& L_1\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$$ E13

$$\begin{array}{l}{}^0{\mathbf{A}}_1=\left[\begin{array}{cccc}\mathit{cos}\left(q_1\right)& -\mathit{sin}\left(q_1\right)& 0& L_1\mathit{cos}\left(q_1\right)\\ \mathit{sin}\left(q_1\right)& \mathit{cos}\left(q_1\right)& 0& L_1\mathit{sin}\left(q_1\right)\\ 0& 0& 1& d_1\\ 0& 0& 0& 1\end{array}\right]\\ {}^1{\mathbf{A}}_2=\left[\begin{array}{cccc}\mathit{cos}\left(q_2\right)& -\mathit{sin}\left(q_2\right)& 0& 0\\ \mathit{sin}\left(q_2\right)& \mathit{cos}\left(q_2\right)& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}1& 0& 0& L_2\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\end{array}$$ E14

$$\begin{array}{l}{}^1{\mathbf{A}}_2=\left[\begin{array}{cccc}\mathit{cos}\left(q_2\right)& -\mathit{sin}\left(q_2\right)& 0& L_2\mathit{cos}\left(q_2\right)\\ \mathit{sin}\left(q_2\right)& \mathit{cos}\left(q_2\right)& 0& L_2\mathit{sin}\left(q_2\right)\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\\ {}^2{\mathbf{A}}_3=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& \mathit{cos}\left(\pi \right)& -\mathit{sin}\left(\pi \right)& 0\\ 0& \mathit{sin}\left(\pi \right)& \mathit{cos}\left(\pi \right)& d_2+q_3\\ 0& 0& 0& 1\end{array}\right]\end{array}$$ E15

$${}^3{\mathbf{A}}_4=\left[\begin{array}{cccc}\mathit{cos}\left(q_4\right)& -\mathit{sin}\left(q_4\right)& 0& 0\\ \mathit{sin}\left(q_4\right)& \mathit{cos}\left(q_4\right)& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$$ E16

So, the final transformation matrix is

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

$${}^0{\mathbf{T}}_4\mathbf{=}\left[\begin{array}{cccc}& & & p_x\\ & \mathbf{R}& & p_y\\ & & & p_z\\ 0& 0& 0& 1\end{array}\right]$$ E18

By vector ${}^0{\mathbf{o}}_4={\left[p_x{p}_y{p}_z\right]}^T$ is defined position of end-effector of SCARA manipulator with respect to its base inertial reference frame.

4. Inverse kinematics

The solution exists only if the given end-effector position and orientation are in dexterous workspace of the solved mechanism. While the forward kinematic model is expressed as

$$\mathbf{x}=\mathbf{f}\left(\mathbf{q}\right)$$ E19

where $\mathbf{f}$ is a function defined between joint space ${\mathbb{R}}^n$ and workspace ${\mathbb{R}}^m$ , which maps the joint position variables $\mathbf{q}\in {\mathbb{R}}^n$ to the position/orientation of the end-effector of mechanism, the inverse kinematic model is based on

$$\mathbf{q}={\mathbf{f}}^{-1}\left(\mathbf{x}\right)$$ E20

where $\mathbf{q}\in {\mathbb{R}}^n$ and $\mathbf{x}\in {\mathbb{R}}^m$ . In the case of the forward kinematic model, end-effector position and orientation are computed for various kinds of mechanisms like manipulators, in a unique manner, for example, by above-mentioned transformation matrices. The inverse kinematic problem is more complex and finding the solution could be in many cases very complicated. While forward kinematics has a closed-form solution, an inverse kinematics in most cases does not have a closed-form solution. A forward kinematic model has a unique solution, while an inverse kinematic model may have multiple solutions or infinite number of solutions, especially for kinematically redundant mechanisms. In order to obtain a closed-form solution, there are two main approaches, namely algebraic approach and geometric approach.

4.1 Closed-form solution of inverse kinematics

Let us consider a two-link mechanism moving in the 2D plane, see Figure 5.

Figure 5.

Inverse kinematics solution for two-link mechanism.

Considering the forward kinematic model, while the angles of joints ${\vartheta }_1$ and ${\vartheta }_2$ are given, the aim is to find the position of end-effector ${\mathbf{x}}_E={\left[xy\right]}^T\in {\mathbb{R}}^m$ . The forward kinematic model can be easily determined by the following equations

$$x=l_{1}cos{\vartheta }_1+l_{2}cos\left({\vartheta }_1+{\vartheta }_2\right)$$ E21

$$y=l_{1}sin{\vartheta }_1+l_{2}sin\left({\vartheta }_1+{\vartheta }_2\right)$$ E22

Now, the inverse kinematic problem is to find angles ${\vartheta }_1$ and ${\vartheta }_2$ , while the end-effector position $x$ and y are given by vector ${\mathbf{x}}_E={\left[xy\right]}^T\in {\mathbb{R}}^m$ .

$$c=x^2+y^2$$ E23

$$\alpha =\mathit{\text{atan}}2\left(y,x\right)$$ E24

$${\vartheta }_2=\pm \mathit{arccos}\left(\frac{x^2+y^2-L_1^2-L_2^2}{2L_1{L}_2}\right)$$ E25

$$\beta =\mathit{arccos}\left(\frac{L_1^2-L_2^2+c^2}{2L_{1}c}\right)$$ E26

$${\vartheta }_1=\alpha -\beta$$ E27

As can be seen in Figure 5, the presented configuration of the mechanism has two solutions in inverse kinematics. These two solutions are based on signum in Eqs. (25) and (27).

5. Differential kinematics

5.1 Analytical Jacobian

In order to describe the relation between joint angles and end-effector configuration, often the relation between the joint and end-effector velocities is used. Let us consider a set of coordinates $\mathbf{x}\in {\mathbb{R}}^m$ , their velocity is $\dot{\mathbf{x}}=d\mathbf{x}/\mathit{dt}\in {\mathbb{R}}^m$ . Then, we can apply Eq. (19). Then, one obtains

$$\dot{\mathbf{x}}=\frac{\partial \mathbf{f}\left(\mathbf{q}\right)}{\partial \mathbf{q}}\frac{d\mathbf{q}}{\mathit{dt}}=\mathbf{J}\left(\mathbf{q}\right)\dot{\mathbf{q}}$$ E28

$$\mathbf{J}\left(\mathbf{q}\right)=\left[\begin{array}{ccc}\frac{\partial x_1}{\partial q_1}& \cdots & \frac{\partial x_1}{\partial q_n}\\ ⋮& \ddots & ⋮\\ \frac{\partial x_m}{\partial q_1}& \cdots & \frac{\partial x_m}{\partial q_n}\end{array}\right]$$ E29

where $\mathbf{J}\left(\mathbf{q}\right)\in {\mathbb{R}}^{m\times n}$ is the analytical Jacobian matrix, which is very often used in kinematics and dynamics of robotic systems. Jacobian reflects differences between joint and configurations space of the investigated mechanism. In robotics, Jacobian is often used for several purposes such as for the definition of the relation between joint and configuration space, definition of the relation between forces/torques between spaces, the study of kinematic singularities, the definition of numerical solution for inverse kinematic problem, and the study of manipulability properties. We can look at Jacobian from a different perspective. Particular Jacobian columns represent the influence of i-th joint on the end-effector velocity.

The following example will demonstrate the derivation of analytical Jacobian for a three-link mechanism (Figure 6).

Figure 6.

Three-link mechanism.

In order to utilize Eq. (29), we need to define position of point $\mathbf{E}={\left[x_E{y}_E\right]}^T\in {\mathbb{R}}^2$ .

$$x_E=L_{1}cos{q}_1+L_{2}cos\left(q_1+q_2\right)+L_{3}cos\left(q_1+q_2+q_3\right)$$ E30

$$y_E=L_{1}sin{q}_1+L_{2}sin\left(q_1+q_2\right)+L_{3}sin\left(q_1+q_2+q_3\right)$$ E31

Assuming Eqs. (30) and (31), Jacobian according to Eq. (29) will be matrix $\mathbf{J}\left(\mathbf{q}\right)\in {\mathbb{R}}^{2\times 3}$

$$\mathbf{J}\left(\mathbf{q}\right)=\left[\begin{array}{ccc}\frac{\partial x_E}{\partial q_1}& \frac{\partial x_E}{\partial q_2}& \frac{\partial x_E}{\partial q_3}\\ \frac{\partial y_E}{\partial q_1}& \frac{\partial y_E}{\partial q_2}& \frac{\partial y_E}{\partial q_3}\end{array}\right]$$ E32

where the elements of the Jacobian matrix are

$$\begin{array}{cc}\frac{\partial x_E}{\partial q_1}& =-L_1\mathit{sin}\left(q_1\right)-L_2\mathit{sin}\left(q_1+q_2\right)-L_3\mathit{sin}\left(q_1+q_2+q_3\right)\\ \frac{\partial x_E}{\partial q_2}& =-L_2\mathit{sin}\left(q_1+q_2\right)-L_3\mathit{sin}\left(q_1+q_2+q_3\right)\\ \frac{\partial x_E}{\partial q_3}& =-L_3\mathit{sin}\left(q_1+q_2+q_3\right)\\ \frac{\partial y_E}{\partial q_1}& =L_1\mathit{cos}\left(q_1\right)+L_2\mathit{cos}\left(q_1+q_2\right)+L_3\mathit{cos}\left(q_1+q_2+q_3\right)\\ \frac{\partial y_E}{\partial q_2}& =L_2\mathit{cos}\left(q_1+q_2\right)+L_3\mathit{cos}\left(q_1+q_2+q_3\right)\\ \frac{\partial y_E}{\partial q_3}& =L_3\mathit{cos}\left(q_1+q_2+q_3\right)\end{array}$$

5.2 Geometric Jacobian

Besides analytically expressing the Jacobian, we can express it by a geometric approach. To establish function $\mathbf{f}\left(\mathbf{q}\right)$ in closed-form, a symbolic formalism is necessary, which could be difficult from the view of implementation. For this reason, a different way of Jacobian expression, the so-called geometric Jacobian, was developed. The geometric Jacobian can be obtained by consideration of rotational velocity vector $\mathbf{\omega }$ . Let us consider link according to Figure 6. The Jacobian can be expressed as

$$\mathbf{J}=\left[\begin{array}{c}{\mathbf{J}}_v\\ {\mathbf{J}}_{\omega }\end{array}\right]=\left[\begin{array}{ccc}{\mathbf{J}}_{v1}& \dots & {\mathbf{J}}_{\mathit{vn}}\\ {\mathbf{J}}_{\omega 1}& \dots & {\mathbf{J}}_{\mathit{\omega n}}\end{array}\right]$$ E33

The first term expresses the effect of ${\dot{q}}_1$ on linear velocity $\mathbf{v}$ and the second term expresses the effect on the rotational velocity $\mathbf{\omega }$ . Thus,

$$\begin{array}{c}\mathbf{v}={\mathbf{J}}_{v1}{\dot{q}}_1+\dots +{\mathbf{J}}_{\mathit{vn}}{\dot{q}}_n\\ \mathbf{\omega }={\mathbf{J}}_{\omega 1}{\dot{q}}_1+\dots +{\mathbf{J}}_{\mathit{\omega n}}{\dot{q}}_n\end{array}$$ E34

That is, the analytical Jacobian differs from the geometrical Jacobian for the rotational part. Considering the revolute joint, the i-th column of Jacobian can be computed as

$$\left[\begin{array}{c}{\mathbf{J}}_{\mathit{vi}}\\ {\mathbf{J}}_{\mathit{\omega i}}\end{array}\right]=\left[\begin{array}{c}{{}^0\mathbf{z}}_{i-1}\times \left({{}^0\mathbf{p}}_n-{{}^0\mathbf{p}}_{i-1}\right)\\ {{}^0\mathbf{z}}_{i-1}\end{array}\right]$$ E35

For prismatic joint, the i-th column of Jacobian can be computed as

$$\left[\begin{array}{c}{\mathbf{J}}_{\mathit{vi}}\\ {\mathbf{J}}_{\mathit{\omega i}}\end{array}\right]=\left[\begin{array}{c}{{}^0\mathbf{z}}_{i-1}\\ \mathbf{0}\end{array}\right]$$ E36

where ${{}^0\mathbf{p}}_n$ is the end-effector position defined in transformation matrix ${{}^0\mathbf{T}}_n$ defined in the previous section. Next, ${{}^0\mathbf{p}}_{i-1}$ is the position of frame $I_{i-1}=\left\{O_{i-1},x_{i-1},y_{i-1},z_{i-1}\right\}$ , defined in transformation matrix ${{}^0\mathbf{T}}_{i-1}$ . Finally, ${{}^0\mathbf{z}}_{i-1}$ is the third column of rotation matrix ${{}^0\mathbf{R}}_{i-1}$ , while ${{}^0\mathbf{R}}_{i-1}={{}^0\mathbf{R}}_1\left(q_1\right){{}^1\mathbf{R}}_2\left(q_2\right)\dots {{}^{i-2}\mathbf{R}}_{i-1}\left(q_{i-1}\right)$ .

The following example will demonstrate the derivation of geometric Jacobian for a two-link mechanism (Figure 7).

$$\mathbf{J}\left(\mathbf{q}\right)=\left[\begin{array}{cc}{\mathbf{z}}_0\times \left({\mathbf{p}}_2-{\mathbf{p}}_0\right)& {\mathbf{z}}_1\times \left({\mathbf{p}}_2-{\mathbf{p}}_1\right)\\ {\mathbf{z}}_0& {\mathbf{z}}_1\end{array}\right]$$ E37

where ${\mathbf{p}}_0=\left[\begin{array}{c}0\\ 0\\ 0\end{array}\right]$ , ${\mathbf{p}}_1=\left[\begin{array}{c}{L}_{1}cos{q}_1\\ L_{1}sin{q}_1\\ 0\end{array}\right]$ , ${\mathbf{p}}_2=\left[\begin{array}{c}{L}_{1}cos{q}_1+L_{2}cos\left(q_1+q_2\right)\\ L_{1}sin{q}_1+L_{2}sin\left(q_1+q_2\right)\\ 0\end{array}\right]$ and rotational axes are ${\mathbf{z}}_0={\mathbf{z}}_1=\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]$ . By solving Eq. (37), we get

Figure 7.

Two-link mechanism.

$$\mathbf{J}\left(\mathbf{q}\right)=\left[\begin{array}{cc}\begin{array}{c}-L_{1}sin{q}_1-L_{2}sin\left(q_1+q_2\right)\end{array}& \begin{array}{c}-L_{2}sin\left(q_1+q_2\right)\end{array}\\ L_{1}cos{q}_1+L_{2}cos\left(q_1+q_2\right)& L_{2}cos\left(q_1+q_2\right)\\ 0& 0\\ 0& 0\\ 0& 0\\ 1& 1\end{array}\right]$$ E38

Now, the Jacobian is a 6 × 2 matrix and its maximum rank is 2. That is, at most two components of angular/linear end-effector velocity can be independently assigned. In this case, when orientation is not required, only first two rows are considered.

5.3 Kinematically redundant manipulators

The next approach to inverse kinematic solution we want to focus on is numerical solutions. Nevertheless, many times, it is hard to find a closed-form solution for inverse kinematics; the basic kinematics of industrial robots have developed an approach to solve it. The problems arise with nonconventional kinematic structures, especially with kinematically redundant manipulators. A kinematically redundant manipulator has more number of DOFs than is absolutely necessary to perform the desired task. For example, conventional industrial robot has usually six DOFs, by which it is able to reach any point in its workspace. By adding an additional DOF, this robot becomes kinematically redundant due to this additional DOF.

A numerical solution is usually used when a closed-form solution for $\mathbf{q}$ does not exist or is difficult to find. In this section, we will focus on kinematically redundant mechanisms. Considering the dimension of joint space $n$ and dimension of task space $m$ , for kinematically redundant mechanisms $n>m$ . The level of redundancy can be expressed by $r=n-m$ . Kinematic redundancy is used for many tasks such as kinematic singularities avoidance, obstacle avoidance, joint limits avoidance, increasing the manipulability in specified directions, minimizing the energy consumption, minimum of motion torques, optimizing execution time, etc. As can be seen, kinematic redundancy allows many optimization tasks to be solved. On the other hand, kinematic redundancy brings some disadvantages as well; for example, a greater structural complexity of construction caused by many of DOFs (mechanical, actuators, sensors), which have an influence on final cost of this kind of mechanism. Next field of potential disadvantages is the field of control, due to complicated algorithms for inverse kinematic computation or motion control. From this reason redundant manipulators could be difficult in real-time control.

There are many approaches within numerical solution of inverse kinematics, which are still in focus of research. Most approaches deal with Jacobian matrix in order to find a linear approximation to the inverse kinematic problem. Among the most used of them are damped least squares (DLSs), Jacobian transpose, and damped least squares with singular value decomposition (SVD) [8, 9].

Another kind of approach is the approach based on Newton methods [6]. The aim of these algorithms is to find the final configuration of joints with focus on minimization problem. For this reason, the final motion of robot is smooth. This family of methods includes methods such as Powell’s method [10] or Broyden, Fletcher, Goldfarb and Shanno (BFGS).

A very well-known and used method for inverse kinematics of kinematically redundant mechanisms is the so-called cyclic coordinate descent (CCD) algorithm [11]. The CCD method is a very simple and at the same time a very strong method. It is a heuristic iterative method with low computational cost per iteration. Next very know heuristic iterative method is FABRIK (Forward And Backward Reaching Inverse Kinematics) [12, 13]. The FABRIK method minimizes the system error by adjusting each joint angle one at a time.

Most of inverse kinematics numerical methods could be divided into two classes: linearization algorithms and minimization algorithms. Concerning the linearization algorithms, the idea is piecewise linearization of nonlinear inverse kinematic problem, which is based on the Jacobian matrix. An example of this kind of method is the Jacobian transpose method. In minimization algorithms, the idea is to formulate some cost function, which will be minimized, for example cyclic coordinate descent algorithm. Besides the mentioned methods, there are many other such as pseudoinverse methods, such as the Levenberg–Marquardt damped least squares methods, quasi-Newton and conjugate gradient methods, neural network and artificial intelligence methods.

The basic technique is based on Eq. (39)

$$\dot{\mathbf{x}}=\mathbf{J}\left(\mathbf{q}\right)\dot{\mathbf{q}}$$ E39

The above relation can be inverted to so-called Jacobian control method

$$\dot{\mathbf{q}}={\mathbf{J}}^{†}\left(\mathbf{q}\right)\dot{\mathbf{x}}$$ E40

which leads to joint velocity vector $\mathbf{q}$ with minimum norm. The term ${\mathbf{J}}^{†}$ represents the Moore-Penrose pseudoinverse given by ${\mathbf{J}}^{†}={\mathbf{J}}^T{\left(\mathbf{J}{\mathbf{J}}^T\right)}^{-1}$ for kinematically redundant mechanisms where $m<n$ $\left(\mathbf{J}{\mathbf{J}}^{†}\mathit{=}\mathbf{I}\right)$ or by ${\mathbf{J}}^{†}={\left({\mathbf{J}}^T\mathbf{J}\right)}^{-1}{\mathbf{J}}^T$ for m $>n\left({\mathbf{J}}^{†}\mathbf{J}\mathit{=}\mathbf{I}\right)$ .

A very common method on which the solution is based is the Newton-Rhaphson method. The Newton-Rhaphson method is a root-finding algorithm that produces approximations of the roots of a real-valued function. The method starts with differentiable function $f$ defined for a real variable $x$ , derivative of function $f^{\prime }$ , and initial guess $x_0$ for a root of function $f$ . If these assumptions are satisfied and initial guess is close, then

$$x_1=x_0-\frac{f\left(x_0\right)}{f^{\prime }\left(x_0\right)}$$ E41

Talking about numerical motion optimization of kinematically redundant mechanisms, there are two approaches. The first approach deals with local methods, which are solved typically online, and the second one with global methods, which require quantity of computation. For this reason, the global methods are computed usually offline.

One of the commonly used local methods is the family of null-space methods. This method uses the extension of Eq. (40) and gives

$$\dot{\mathbf{q}}={\mathbf{J}}^{†}\left(\mathbf{q}\right)\dot{\mathbf{x}}+\left(\mathbf{I}-{\mathbf{J}}^{†}\mathbf{J}\right){\dot{\mathbf{q}}}_0$$ E42

where ${\dot{\mathbf{q}}}_0\in {\mathbb{R}}^n$ is an arbitrary joint space velocity vector, chosen according to desired behavior; so it is chosen for optimization purposes. Next, $\mathbf{I}\in {\mathbb{R}}^{\mathit{n}\mathit{\times }\mathit{n}}$ is the identity matrix. The term $\left(\mathbf{I}-{\mathbf{J}}^{†}\mathbf{J}\right)$ represents the orthogonal projection matrix in the null space of $\mathbf{J}$ , ${\mathbf{J}}^{†}\left(\mathbf{q}\right)\dot{\mathbf{x}}$ is orthogonal to $\left(\mathbf{I}-{\mathbf{J}}^{†}\mathbf{J}\right){\dot{\mathbf{q}}}_0$ . For this reason, $\mathbf{q}=0$ . Physically, this term corresponds to self-motion, where the combined joints motion generates no motion in the task space (no motion of end-effector). So, the term $\left(\mathbf{I}-{\mathbf{J}}^{†}\mathbf{J}\right)$ is symmetric and idempotent ( ${\left(\mathbf{I}-{\mathbf{J}}^{†}\mathbf{J}\right)}^2=\left(\mathbf{I}-{\mathbf{J}}^{†}\mathbf{J}\right)$ ). Also, it ensures ${\left(\mathbf{I}-{\mathbf{J}}^{†}\mathbf{J}\right)}^{†}=\left(\mathbf{I}-{\mathbf{J}}^{†}\mathbf{J}\right)$ . The inverse kinematic solution expressed by Eq. (42) is equivalent to solving a quadratic programming (QP) problem based on $\mathbf{H}\left(\dot{\mathbf{q}}\right)={min}_{\dot{\mathbf{q}}}\frac{1}{2}{\left(\dot{\mathbf{q}}-{\dot{\mathbf{q}}}_0\right)}^T\mathbf{W}\left(\dot{\mathbf{q}}-{\dot{\mathbf{q}}}_0\right)$ subjected to $\dot{\mathbf{x}}=\mathbf{J}\left(\mathbf{q}\right)\dot{\mathbf{q}}$ .

Now the question is, how the vector ${\dot{\mathbf{q}}}_0$ can be chosen. One of the basic ways to choose it is the so-called projected gradient method ${\dot{\mathbf{q}}}_0={\nabla }_q\mathbf{H}\left(\mathbf{q}\right)$ . Supposing that $\dot{\mathbf{x}}=0$ , that is, the mechanism performs only self-motion, it can be written $\dot{\mathbf{q}}=\left(\mathbf{I}-{\mathbf{J}}^{†}\mathbf{J}\right){\nabla }_q\mathbf{H}$ ; so, $\left(\mathbf{I}-{\mathbf{J}}^{†}\mathbf{J}\right){\nabla }_q\mathbf{H}=0$ is a necessary condition of constrained optimality. Based on these facts, an objective function can be chosen for some optimization of motion:

• Manipulability—maximize the distance from kinematic singularities

$$\mathbf{H}\left(\mathbf{q}\right)=\sqrt{\mathit{det}\left[\mathbf{J}\left(\mathbf{q}\right){\mathbf{J}}^T\left(\mathbf{q}\right)\right]}$$ E43

• Joint limit avoidance—minimize the distance from the middle of joints range

$$\mathbf{H}\left(\mathbf{q}\right)=\frac{1}{2}\sum _{i=1}^n{\left(\frac{q_i-q_i^{-}}{q_{M,i}-q_{m,i}}\right)}^2$$ E44

where $q_i\in \left[q_{m,i},q_{M,i}\right]$ and $q_i^{-}=\frac{q_{M,i}-q_{m,i}}{2}$ .

• Obstacle avoidance—maximize the minimum distance to obstacle

$$\mathbf{H}\left(\mathbf{q}\right)={min}_{\begin{array}{l}\mathbf{a}\in \mathit{\text{robot}}\hfill \\ \mathbf{b}\in \mathit{\text{obstacle}}\end{array}}{‖\mathbf{a}\left(\mathbf{q}\right)-\mathbf{b}‖}^2$$ E45

where $\mathbf{a}\left(\mathbf{q}\right)$ represents points on investigated mechanism and $\mathbf{b}$ represents points on the obstacle.

Example: Let us consider a planar six-link robot connected by six revolute joints. All links have the same length $L=100\mathit{mm}$ . The purpose of the simulation is path tracking (circle form) described by matrix ${\mathbf{X}}_{\mathit{\text{path}}}=\left[\begin{array}{c}{\mathbf{x}}_d\\ {\mathbf{y}}_d\end{array}\right]\in {\mathbb{R}}^{m\times p}$ , where $p$ is the number of geometric points of the desired path, while ${\mathbf{x}}_d=-2.5L+Lcos\phi$ , ${\mathbf{y}}_d=Lsin\phi$ , $\phi \in \left\{0,\dots ,2\pi \right\}$ assuming the step of $\phi$ increase to be 0.2. That is, ${\mathbf{x}}_d=\left[x_1,\dots ,x_p\right]\in {\mathbb{R}}^p$ and ${\mathbf{y}}_d=\left[y_1,\dots ,y_p\right]\in {\mathbb{R}}^p$ . From the matrix ${\mathbf{X}}_{\mathit{\text{path}}}$ will be in each path point determined the desired point ${\mathbf{e}}_p={\left[x_p{y}_p\right]}^T\in {\mathbb{R}}^m$ . Since, there is consideration of planar task with focus on end-effector position, the task space is $m=2$ . The expected solution assumes only primary solution without any secondary tasks. For inverse kinematic solution, damped least squares method, which avoids many of pseudoinverse method’s problems, will be used. This method is also known as the Levenberg–Marquardt method, which arises from cost function ${‖\mathbf{J}∆\mathbf{q}-∆\mathbf{x}‖}^2+{\lambda }^2{‖∆\mathbf{q}‖}^2$ where $\lambda \in \mathbb{R}$ is a non-zero scalar constant. By minimizing this term, one obtains

$$∆\mathbf{q}={\mathbf{J}}^T{\left(\mathbf{J}{\mathbf{J}}^T+{\lambda }^2\mathbf{I}\right)}^{-\mathbf{1}}∆\mathbf{x}$$ E46

where $\mathbf{I}\mathit{\in }{\mathbb{R}}^{m\times m}$ is the identity matrix. The simulation will work according to the following algorithm.

Algorithm: Inverse kinematic model for 6-link manipulator.

1: Set desired path for end-effector by matrix ${\mathbf{X}}_{\mathit{\text{path}}}$ and parameters such as $L$ , $\lambda$ .

2: FOR step = 1 $\to p.$

3:    Set the desired position of end-effector from ${\mathbf{X}}_{\mathit{\text{path}}}⟹{\mathbf{e}}_p={\left[x_p{y}_p\right]}^T\in {\mathbb{R}}^m$

4:    WHILE ${\mathbf{x}}_{\mathit{\text{actual}}}\ne {\mathbf{e}}_p$

5:        Compute Jacobian

6:        Compute ${\mathbf{x}}_{\mathit{\text{actual}}}={\left[x_{\mathit{act}}{y}_{\mathit{act}}\right]}^T\in {\mathbb{R}}^m$

7:        Compute $∆\mathbf{q}\mathbf{=}\mathbf{q}\mathbf{+}{\mathbf{J}}^T{\left(\mathbf{J}{\mathbf{J}}^T\mathit{+}{\lambda }^2\mathbf{I}\right)}^{\mathit{-}\mathbf{1}}∆\mathbf{x}$

8:        $\mathbf{q}=∆\mathbf{q}$

9:    END WHILE

10: END FOR

The results of the simulation can be seen in Figures 8 and 9. Figure 8 presents the motion of planar robot. The aim is to tracking of path with circle shape (green color) by end-effector of the robot. The robot has a fixed frame in point [0,0]. The initial position of all joints is $\mathbf{q}={\left[000000\right]}^T$ .

Figure 8.

Simulation of inverse kinematics for six-link manipulator.

Figure 9.

Variations of joint angles.

During the simulation, no restriction such as joint limit was considered. The simulation was done with a tolerance $\pm 5$ mm. Figure 9 presents the variation of individual robot joints during the path tracking.

Example: Let us consider a planar 20-link robot connected with revolute joints. The links have the same length $L=16.75\mathit{mm}$ . The aim is to move the end-effector from its initial position to the end position by tracking the desired path. We will consider two cases. The first one considers free robot environment, the second one considers obstacles in the robot environment. The second solution will also consider kinematic singularities avoidance task and joint limit avoidance task.

Now, let us consider the cost function dealing with all mentioned secondary tasks [10].

$$\mathbf{H}={‖\mathbf{J}\dot{\mathbf{q}}-\dot{\mathbf{x}}‖}^2+{‖{\mathbf{J}}_c\dot{\mathbf{q}}-{\mathbf{x}}_c‖}^2+{‖{\mathbf{J}}_L\dot{\mathbf{q}}-{\mathbf{x}}_L‖}^2+{‖\rho \dot{\mathbf{q}}‖}^2$$ E47

After mathematical adjustment, we get the final formula for kinematic control

$$\dot{\mathbf{q}}={\left({\mathbf{J}}^T\mathbf{WJ}+{\mathbf{J}}_c^T{\mathbf{W}}_c{\mathbf{J}}_c+{\mathbf{J}}_L^T{\mathbf{W}}_L{\mathbf{J}}_L+{\mathbf{W}}_s\right)}^{-1}\left({\mathbf{J}}^T\mathbf{W}\dot{\mathbf{x}}\right)$$ E48

where $\mathbf{W}$ , ${\mathbf{W}}_c$ , ${\mathbf{W}}_L$ , and ${\mathbf{W}}_s$ are in our case 20 × 20 weight matrices of primary task, obstacle avoidance task, joint limit avoidance task, and kinematic singularities avoidance task, respectively. The setting of weight matrices is subjective. For example, if obstacle avoidance task should have higher priority above primary task by 100 times, the matrix ${\mathbf{W}}_c$ would consist of values 100 while $\mathbf{W}$ consist of values 1.

Let us consider individual secondary tasks. The joint limit avoidance task deals with the range of motion of individual manipulator links. How many joints will have their limit range is up to us. Of course, in real robots, usually all joints are limited in their motion. There are several ways on how to model the range of joint motion. In this case, an approach with changing of value of weight variable $W_{\mathit{li}}$ based on joint position will be used. If the joint is in admissible range, the value of the weight variable is set to be zero. When the joint reaches the boundary of its range motion, the value of the weight variable increases. When the joint reaches a value outside its admissible range, the value of the weight variable increases to its maximum. This approach can be expressed by Eq. (49)

$${\mathrm{W}}_{\mathrm{li}}=\left\{\begin{array}{c}{\mathrm{W}}_{\mathrm{w}}\leftarrow {\mathrm{q}}_{\mathrm{i}}<{\mathrm{q}}_{\text{imin}}\\ \begin{array}{c}\frac{{\mathrm{W}}_{\mathrm{w}}}{2}\left\{1+cos\left[\mathrm{\pi }\left(\frac{{\mathrm{q}}_{\mathrm{i}}-{\mathrm{q}}_{\text{imin}}}{{\mathrm{\rho }}_{\mathrm{i}}}\right)\right]\right\}\leftarrow {\mathrm{q}}_{\text{imin}}\le {\mathrm{q}}_{\mathrm{i}}\le {\mathrm{q}}_{\text{imin}}+{\mathrm{\rho }}_{\mathrm{i}}\\ 0\leftarrow {\mathrm{q}}_{\text{imin}}+{\mathrm{\rho }}_{\mathrm{i}}<{\mathrm{q}}_{\mathrm{i}}<{\mathrm{q}}_{\text{imax}}-{\mathrm{\rho }}_{\mathrm{i}}\end{array}\\ \begin{array}{c}\frac{{\mathrm{W}}_{\mathrm{w}}}{2}\left\{1+cos\left[\mathrm{\pi }\left(\frac{{\mathrm{q}}_{\text{imax}}-{\mathrm{q}}_{\mathrm{i}}}{{\mathrm{\rho }}_{\mathrm{i}}}\right)\right]\right\}\leftarrow {\mathrm{q}}_{\text{imax}}-{\mathrm{\rho }}_{\mathrm{i}}\le {\mathrm{q}}_{\mathrm{i}}\le {\mathrm{q}}_{\text{imax}}\\ {\mathrm{W}}_{\mathrm{w}}\leftarrow {\mathrm{q}}_{\mathrm{i}}>{\mathrm{q}}_{\text{imax}}\end{array}\end{array}\right.$$ E49

The value of the weight variable has to be set for every joint of the manipulator that needs to be limited in the range of motion. Individual weight variables $W_{\mathit{li}}$ where $i\in \left\{0,\dots ,N\right\}$ are parts of the final weight matrix of the joint limit avoidance task $W_l\in {\mathbb{R}}^{n\times n}$ . The final weight matrix ${\mathbf{W}}_{\mathrm{l}}$ is the diagonal matrix [14]:

$${\mathbf{W}}_{\mathrm{l}}=\left[\begin{array}{ccccc}{\mathrm{W}}_{\mathrm{l}1}& & & & \\ & {\mathrm{W}}_{\mathrm{l}2}& & & \\ & & {\mathrm{W}}_{\mathrm{l}3}& & \\ & & & \dots & \\ & & & & {\mathrm{W}}_{\ln }\end{array}\right]$$ E50

The weight matrix ${\mathbf{W}}_{\mathrm{l}}$ is used with the corresponding Jacobian matrix ${\mathbf{J}}_L\in {\mathbb{R}}^{n\times n}$ . The Jacobian matrix for the joint limit avoidance task is ${\mathbf{J}}_L=\partial \mathbf{e}/\partial \mathbf{q}$ . If a particular joint does not consider the joint limit avoidance task, the value of ${\mathbf{J}}_L$ is set to be zero; otherwise it is set to be one. The limit of all joints in motion of the manipulator investigated in this study is set to be ±100°. Different way of joint limit control is according to above mentioned Eq. (44).

During the obstacle avoidance task, the control system investigates the relation between manipulator links and obstacles in their environment. In general, this task can be solved from two views. At first, one group of obstacles can represent static obstacles or other robots in an investigated environment. The second group of obstacles can be represented by dynamic obstacles, which means that these obstacles change their position relating to global reference system in the time. Of course, the second group of obstacles is more difficult from the view of control in comparison with static obstacles. It is more difficult especially in the cases of requirements for real-time control [15].

The aim of obstacle avoidance is to prevent the collision between any part of the manipulator and potential obstacles, other robots, or collision with itself. Again, there are many methods on how to control robot motion at a safe distance from other objects, regardless of whether the obstacles have regular or irregular shape. For simplification, the irregular shapes are usually replaced by appropriate regular shape. This simplification can also significantly simplify the mathematical model and obtaining the numerical solution can be faster and the solution more stable. One of the methods on how to simplify irregular shapes is to replace all irregular shapes by a cylinder, with the obstacle being situated in the center of the cylinder, see Figure 10. The diameter of the cylinder determines the distance of influence of this obstacle.

Figure 10.

Relation between manipulator link and obstacle.

At first, we set the coordinate of an obstacle in the task space as ${\mathbf{s}}_o$ . The projection of the line from the i-th joint of the manipulator link to the center of a cylinder (obstacle) on the i-th link is:

$${\mathbf{p}}_{\mathrm{i}}={\mathbf{e}}_{\mathrm{i}}^{\mathrm{T}}\left({\mathbf{s}}_0-{\mathbf{s}}_{\mathrm{i}}\right)$$ E51

The coordinate of the link point with potential to get into collision is:

$${\mathbf{s}}_{\mathrm{ci}}={\mathbf{s}}_{\mathrm{i}}+{\mathbf{p}}_{\mathrm{i}}{\mathbf{e}}_{\mathrm{i}}$$ E52

The distance between the potential point of collision and the center of the cylinder is:

$${\mathbf{d}}_{\mathrm{ci}}=\parallel {\mathbf{s}}_{\mathrm{ci}}-{\mathbf{s}}_0\parallel$$ E53

Subsequently, the unit vector of the potential point of collision can be given by:

$${\mathbf{u}}_{\mathrm{i}}=\frac{{\mathbf{s}}_{\mathrm{ai}}-{\mathbf{s}}_0}{{\mathbf{d}}_{\mathrm{ci}}}$$ E54

Now, the Jacobian matrix for the obstacle avoidance task can be given. The i-th row of the Jacobian matrix can be expressed as

$${\mathbf{J}}_{\mathrm{ci}}=-{\mathbf{u}}_{\mathrm{i}}^{\mathrm{T}}{\mathbf{J}}_{{\mathrm{s}}_{\mathrm{ci}}}$$ E55

where matrix ${\mathrm{J}}_{{\mathrm{s}}_{\mathrm{ci}}}$ is:

$${\mathbf{J}}_{{\mathrm{s}}_{\mathrm{ci}}}=\frac{\partial {\mathbf{s}}_{\mathrm{ci}}}{\partial \mathbf{q}}$$ E56

The Jacobian matrix ${\mathrm{J}}_{\mathrm{c}}$ consists of submatrices ${\mathrm{J}}_{\mathrm{ci}}$ . The dimension of the Jacobian matrix is ${\mathrm{J}}_{\mathrm{c}}\in {\mathrm{R}}^{\mathrm{c}\times \mathrm{c}}$ , where c represents the number of manipulator links that could collide with the obstacles.

For numerical simulation, we will use following algorithm: Inverse kinematic model for 20-link manipulator [14].

Algorithm: Inverse kinematic model for 20-link manipulator

1: CYCLE WHILE 1

2:   Determination of new required vector ${\mathrm{x}}_{\mathrm{d}}\in {\mathrm{R}}^{\mathrm{m}}$ from the matrix of planned path $\mathrm{P}\in {\mathrm{R}}^{\mathrm{r}\times 2}$

3:   CYCLE WHILE 2

4:      Computation of Jacobian matrix J (damped least squares method)

5:   Determination of actual end-effector position in the task space $\mathrm{x}\in {\mathrm{R}}^{\mathrm{m}}$ with actual generalized variables $\mathrm{q}\in {\mathrm{R}}^{\mathrm{n}}$

6:      Computation of general equation

       $\dot{\mathrm{q}}={\left({\mathrm{J}}^{-1}\mathrm{WJ}+{{\mathrm{J}}_{\mathrm{c}}}^{-1}{\mathrm{W}}_{\mathrm{c}}{\mathrm{J}}_{\mathrm{c}}+{{\mathrm{J}}_{\mathrm{l}}}^{-1}{\mathrm{W}}_{\mathrm{l}}{\mathrm{J}}_{\mathrm{l}}+{\mathrm{W}}_{\mathrm{s}}\right)}^{-1}\left({\mathrm{J}}^{\mathrm{T}}\mathrm{W}\dot{\mathrm{x}}\right)$

7:      $\mathrm{q}={\mathrm{q}}_{\text{previous}}+\dot{\mathrm{q}}\mathrm{dt}$

8:      ${\mathrm{q}}_{\text{previous}}=\mathrm{q}$

9:      IF ${\mathrm{x}}_{\mathrm{d}}=\mathrm{x}$ THEN

 END CYCLE WHILE 2

      ELSE

  CYCLE WHILE 2 continues

  END IF

10:  END CYCLE WHILE 2

11: END CYCLE WHILE 1

In Figure 11 can be seen the simulation of motion of a 20-link manipulator, which moves in free environment without any obstacles. This case also does not consider any joint limit avoidance task. The aim is to move the end-effector of manipulator by predefined path (green color).

Figure 11.

Simulation of 20-link manipulator in free environment.

Figure 12 depicts a different situation. A 20-link manipulator moves according to predefined path between four obstacles. The solver also assumes joint limit avoidance task for all 20 joints. From this figure can be seen the difference in “self-motion” in joint space, while it does not affect the motion of end-effector in task space. The end-effector always tracks the same path (by neglecting end-effector orientation).

Figure 12.

Simulation of 20-link manipulator in constrained environment.

The second case is significantly difficult from the view of computational complexity in comparison with the case without any constraints in motion.

6. Conclusion

This chapter was focused on forward and inverse kinematics of open-chain mechanisms, namely manipulators with serial kinematic structures. We have introduced rigid motion and subsequently we have focused on forward kinematics. The chapter presents a kinematic model of SCARA by the well-known Denavit-Hartenberg convention. Within inverse kinematics was introduces several methods, including analytical, geometrical, and numerical. The last section dealt with modeling of kinematically redundant planar robots. At first, we introduced a six-link planar robot with focus on numerical solution using DLS. The last example presented a 20-link redundant manipulator moving between the obstacles. The solution includes, besides the primary solution, secondary tasks such as singularity avoidance, joint limit avoidance, and obstacle avoidance.

Acknowledgments

The authors would like to thank to Slovak Grant Agency VEGA 1/0389/18 “Research on kinematically redundant mechanisms” and to KEGA 018TUKE-4/2018 “Implementation of new technologies and education methods in area of controlling systems for improvement of educational level and practical experiences of graduate students of Mechatronics study program” and also to KEGA 030TUKE-4/2020 “Transfer of knowledge from the field of industrial automation and robotics to the education process in the teaching program mechatronics.”