Open access peer-reviewed chapter

# Forward and Inverse Kinematics Using Pseudoinverse and Transposition Method for Robotic Arm DOBOT

Written By

Ondrej Hock and Jozef Šedo

Submitted: April 5th, 2017Reviewed: October 3rd, 2017Published: December 20th, 2017

DOI: 10.5772/intechopen.71417

From the Edited Volume

## Kinematics

Chapter metrics overview

View Full Metrics

## Abstract

Kinematic structure of the DOBOT manipulator is presented in this chapter. Joint coordinates and end-effector coordinates of the manipulator are functions of independent coordinates, i.e., joint parameters. This chapter explained forward kinematics task and issue of inverse kinematics task on the structure of the DOBOT manipulator. Linearization of forward kinematic equations is made with usage of Taylor Series for multiple variables. The inversion of Jacobian matrix was used for numerical solution of the inverse kinematics task. The chapter contains analytical equations, which are solution of inverse kinematics task. It should be noted that the analytical solution exists only for simple kinematic structures, for example DOBOT manipulator structure. Subsequently, simulation of the inverse kinematics of the above-mentioned kinematic structure was performed in the Matlab Simulink environment using the SimMechanics toolbox.

### Keywords

• forward kinematics
• inverse kinematics
• robotic arm
• Jacobian matrix
• pseudoinverse method
• SimMechanics

## 1. Introduction

Robots and manipulators are very important and powerful instruments of today’s industry. They are making lot of different tasks and operations and they do not require comfort, time for rest, or wage. However, it takes many time and capable workers for right robot function [6].

The movement of robot can be divided into forward and inverse kinematics. Forward kinematics described how robot’s move according to entered angles. There is always a solution for forward kinematics of manipulator. Solution for inverse kinematics is a more difficult problem than forward kinematics. The relationship between forward kinematics and inverse kinematics is illustrated in Figure 1. Inverse kinematics must be solving in reverse than forward kinematics. But we know to always find some solution for inverse kinematics of manipulator. There are only few groups of manipulators (manipulators with Euler wrist) with simple solution of inverse kinematics [8, 9].

Two main techniques for solving the inverse kinematics are analytical and numerical methods. In the first method, the joint variables are solved analytical, when we use classic sinus and cosine description. In the second method, the joint variables are described by the numerical techniques [9].

The whole chapter will be dedicated to the robot arm DOBOT Magician (hereafter DOBOT) shown in Figure 2. The basic parameters of the robotic manipulator are shown in Figure 3 and its motion parameters are shown in Table 1.

Joint 1 base−90° to +90°320°/s
Joint 2 rear arm0° to +85°320°/s
Joint 3 fore arm−10° to +95°320°/s
Joint 4 rotation servo+90° to −90°480°/s

### Table 1.

Axis movement of DOBOT Magician [10].

This chapter is organized in the following manner. In the first section, we made the forward and inverse kinematics transformations for DOBOT manipulator. Secondly, we made the DOBOT Magician simulation in Matlab environment. Thirdly, we describe the explanation of Denavit-Hartenberg parameters. Finally, we made the pseudoinverse and transposition methods of Jacobian matrix in the inverse kinematics.

## 2. Kinematics structure RRR in 3D

Kinematic structure of the DOBOT manipulator is shown in Figure 4. It is created from three rotation joints and three links. Joint A rotates about the axis zand joints B and C rotate about the axis x1.

Figure 5 shows a view from the direction of axis z and Figure 6 shows a perpendicular view of the plane defined by z axis and line c.

Kinematic equations of the points B, C, and D, respectively:

x0B=0E1
y0B=0E2
z0B=l1E3
x0C=l2.cosϕ1.sinδE4
y0C=l2.cosϕ1.cosδE5
z0C=l1+l2.sinϕ1E6
x0D=l2.cosϕ1+l3.cosϕ2.sinδE7
y0D=l2.cosϕ1+l3.cosϕ2.cosδE8
z0D=l1+l2.sinϕ1+l3.sinϕ2E9

Where ϕ2 = ϕ + γ.

### 2.1. Forward kinematics

Forward kinematics task is defined by Eq. (10)

X=fQE10

where

X=x0Dy0Dz0DE11

Xis position vector of manipulator endpoint coordinates.

Q=ϕγδE12

Qis vector of independent coordinates: ϕ = ϕ1, γ, δ.

Because the function X = f(Q) is nonlinear, it is difficult to solve the inverse task Q = f(X) when looking for a vector of independent coordinates (rotation of individual manipulator joints) as a function of the desired manipulator endpoint coordinates. An analytical solution to the inverse task is possible only in the case of a relatively simple kinematic structure of the manipulator (see next chapter).

Therefore, the function X = f(Q) linearized using Taylor series, taking into account only the first four (linear) members of the development:

x0D=x0Dϕγδx0Dϕ0γ0δ0+x0Dϕγδϕϕ0,γ0,δ0ϕϕ0+x0Dϕγδγϕ0,γ0,δ0γγ0+x0Dϕγδδϕ0,γ0,δ0δδ0E13
y0D=y0Dϕγδy0Dϕ0γ0δ0+y0Dϕγδϕϕ0,γ0,δ0ϕϕ0+y0Dϕγδγϕ0,γ0,δ0γγ0+y0Dϕγδδϕ0,γ0,δ0δδ0E14
z0D=z0Dϕγδz0Dϕ0γ0δ0+z0Dϕγδϕϕ0,γ0,δ0ϕϕ0+z0Dϕγδγϕ0,γ0,δ0γγ0+z0Dϕγδδϕ0,γ0,δ0δδ0E15

After editing:

x0Dϕγδx0Dϕ0γ0δ0=x0Dϕγδϕϕ0,γ0,δ0ϕϕ0+x0Dϕγδγϕ0,γ0,δ0γγ0+x0Dϕγδδϕ0,γ0,δ0δδ0E16
y0Dϕγδy0Dϕ0γ0δ0=y0Dϕγδϕϕ0,γ0,δ0ϕϕ0+y0Dϕγδγϕ0,γ0,δ0γγ0+y0Dϕγδδϕ0,γ0,δ0δδ0E17
z0Dϕγδz0Dϕ0γ0δ0=z0Dϕγδϕϕ0,γ0,δ0ϕϕ0+z0Dϕγδγϕ0,γ0,δ0γγ0+z0Dϕγδδϕ0,γ0,δ0δδ0E18

We denoted:

Δx0D=x0Dϕγδx0Dϕ0γ0δ0E19
Δy0D=y0Dϕγδy0Dϕ0γ0δ0E20
Δz0D=z0Dϕγδz0Dϕ0γ0δ0E21
Δϕ=ϕϕ0E22
Δγ=γγ0E23
Δδ=δδ0E24

Then, we obtained:

Δx0D=x0Dϕγδϕϕ0,γ0,δ0Δϕ+x0Dϕγδγϕ0,γ0,δ0Δγ+x0Dϕγδδϕ0,γ0,δ0ΔδE25
Δy0D=y0Dϕγδϕϕ0,γ0,δ0Δϕ+y0Dϕγδγϕ0,γ0,δ0Δγ+y0Dϕγδδϕ0,γ0,δ0ΔδE26
Δz0D=z0Dϕγδϕϕ0,γ0,δ0Δϕ+z0Dϕγδγϕ0,γ0,δ0Δγ+z0Dϕγδδϕ0,γ0,δ0ΔδE27

In matrix form:

Δx0DΔy0DΔz0D=x0Dϕγδϕx0Dϕγδγx0Dϕγδδy0Dϕγδϕy0Dϕγδγy0Dϕγδδz0Dϕγδϕz0Dϕγδγz0Dϕγδδϕ0,γ0,δ0ΔϕΔγΔδE28

Where matrix:

J=x0Dϕγδϕx0Dϕγδγx0Dϕγδδy0Dϕγδϕy0Dϕγδγy0Dϕγδδz0Dϕγδϕz0Dϕγδγz0Dϕγδδϕ0,γ0,δ0E29

is Jacobian matrix. We denoted:

ΔX0D=Δx0DΔy0DΔz0DE30

and

ΔQ=ΔϕΔγΔδE31

Then, we obtained the matrix equation, which represents linearized forward kinematics in incremental form:

ΔX0D=J.ΔQE32

After we multiplied the Eq. (32) with inverse matrix J−1 from the left, we obtained the equation of inverse kinematics.

J1.ΔX0D=J1.J.ΔQE33
J1.ΔX0D=I.ΔQE34

Where Iis the identity matrix. After that:

ΔQ=J1.ΔX0DE35

Derivative of the kinematic equations with respect to the independent coordinates for kinematic structure of DOBOT manipulator:

x0Dϕ=l2.sinϕ+l3.sinϕ+γ.sinδE36
x0Dγ=l3.sinϕ+γ.sinδE37
x0Dδ=l2.cosϕ+l3.cosϕ+γ.cosδE38
y0Dϕ=l2.sinϕ+l3.sinϕ+γ.cosδE39
y0Dγ=l3.sinϕ+γ.cosδE40
y0Dδ=l2.cosϕ+l3.cosϕ+γ.sinδE41
z0Dϕ=l2.cosϕ+l3.cosϕ+γE42
z0Dγ=l3.cosϕ+γE43
z0Dδ=0E44

Jacobian matrix:

J=x0Dϕx0Dγx0Dδy0Dϕy0Dγy0Dδz0Dϕz0Dγz0DδE45

### 2.2. Analytical Solution of the Inverse Kinematics of DOBOT manipulator

The following equations are derived from Figure 4.

c2=x2+y2E46
d2=c2+z2=x2+y2+z2E47
e2=l22+l322l2l3cosπγE48
γ=±arccose2l22l322l2l3E49
e2=c2+zl12E50
ϕ=αβE51
α=arctgzl1c=arctgzl1x2+y2E52
l32=l22+e22l2.e.cosβE53
β=±arccosl22+e2l322l2eE54
ϕ=arctgzl1x2+y2arccosl22+e2l322l2eE55
δ=arctgxyE56

## 3. Simulation DOBOT Magician in Matlab environment

For simulation movement of the manipulator, Matlab Simulink environment and SimMechanics toolbox are suitable to use. Some blocks from SimMechanics toolbox are shown in Figure 7, which represents the model of DOBOT manipulator.

We used basic block from SimMechanics toolbox in simulation model:

• Joint actuator

• Revolute

• Body

• Body sensor

• Machine environment

The joint actuator block transfers the requested angles to the connected joint. The revolute block defined the rotation of body in space. The body block describes the parameters of body, like dimension, inertia, etc. The body sensor block transfers the coordinates, velocity, and others to simulation, and the last block is the machine environment which defines the parameters of the environment in which the manipulator is located. You can find all necessary data about these blocks in [7].

The simulation model, shown in Figure 8, was designed for considerate results from SimMechanics model, model used D-H parameters and analytical model, which was described in previous chapter. Every result from these models is shown in Figure 9. The fourth part in Figure 9 is the results from analytical simulation model of inverse kinematics. Figure 10 represented the reference angles in the first part of the chart, calculated angles from analytical inverse kinematics model in the second part of chart, and finally the error between both angles. As we can see in Figure 10, the angles are same. This is proof that analytical model of DOBOT manipulator is usable for simulation and implementation to some DSP or microcontroller.

## 4. Denavit-Hartenberg parameters

The steps to get the position in using D-H convention are finding the Denavid-Hartenberg (D-H) parameters, building A matrices, and calculating T matrix with the coordinate position which is desired.

### 4.1. D-H parameters

D-H notation describes coordinates for different joints of a robotic manipulator in matrix entry. The method includes four parameters:

1. Twist angle αi

4. Joint angle θi.

Based on the manipulator geometry, twist angle and link length are constants and link offset and joint angle are variables depending on the joint, which can be prismatic or revolute. The method has provided 10 steps to denote the systematic derivation of the D-H parameters, and you can find them in [5] or [6].

### 4.2. A matrix

The A matrix is a homogenous 4 × 4 transformation matrix. Matrix describes the position of a point on an object and the orientation of the object in a three-dimensional space [6]. The homogenous rotation matrix along an axis is described by the Eq. (57) (Figures 1113).

Rotzi1=cosθicosαisinθisinαisinθi0sinθicosαicosθisinαisinθi00sinαicosαi00001E57

The homogeneous translation matrix is described by Eq. (58).

Transzi1=100ai0100001di0001E58

In rotation matrix and translation matrix, we can find the four parameters θi, di, ai, and αi. These parameters derive from specific aspects of the geometric relationship between two coordinate frames. The four parameters are associated with link iand joint i. In Denavit-Hartenberg convention, each homogeneous transformation matrix Ai is represented as a product of four basic transformations as follows [6]:

Tii1=Transzi1di.Rotzi1θi.Transxiri.RotxiαiE59

D-H convention matrix is given in Eq. (60).

Tii1=cosθisinθicosαisinθisinαiricosθisinθicosθicosαicosθisinαirisinθi0sinαicosαidi0001E60

The previous matrix can be simplified by following equation Aimatrix. The matrix Ai is composed from 3 × 3 rotation matrix Ri, 3 × 1 translation vector Pi, 1 × 3 perspective vector and scaling factor.

Ai=Ri3x3Pi3x101x31E61

### 4.3. T matrix

The T matrix can be formulated by Eq. (62). The matrix is a sequence of D-H matrices and is used for obtaining end-effector coordinates. The T matrix can be built from several A matrices depending on the number of manipulator joints.

T03=T01.T12.T23E62

Inside the Tmatrix is the translation vector Pi, which includes joint coordinates, where the X, Y, and Zpositions are P1, P2, and P3, respectively [6].

## 5. The pseudoinverse method

If the number of independent coordinates n(joint parameters) is larger than the number of reference manipulator endpoint coordinates m(three in Cartesian coordinate system for the point), it shows that a redundancy problem has occurred. In this case, it can exist in infinite combinations of independent coordinates for the only endpoint position. Jacobian matrix J has a size of mrows and ncolumns (m ≠ n), i.e., J is a non-square matrix. In general, it cannot be computed inverse matrix from non-square matrix.

In order to solve inverse kinematics task for this case, pseudoinverse of Jacobian matrix (denotes J+) is used. This method uses singular value decomposition (SVD) of Jacobian matrix to determine J+.

Every matrix J can be decomposed with the usage of SVD to three matrices Eq. (63):

J=VTE63

Where

J is m× nmatrix.

U is m× morthogonal matrix, i.e. U−1 = UT.

V is n× northogonal matrix, i.e. V−1 = VT.

Σ is m× ndiagonal matrix, which contains singular values of matrix J on its major diagonal.

j11j12j1nj21j22j2njm1jm2jmn=u11u12u1mu21u22u2mum1um2umm.σ1000σ2000σd.v11v21vn1v12v22vn2v1nv2nvnnE64

Where d = mfor m < nand d = nfor m > n, because Σ is a non-square matrix.

To determine matrices U and Σ, we multiply matrix J by its transpose matrix JT from the right:

J.JT=UΣVT.UΣVTTE65
J.JT=UΣVT.VΣTUTE66
J.JT=UΣΣTUTE67

We multiply the above Eq. (67) by matrix U from the right:

JJT.U=U.ΣΣT.UTUE68
JJT.U=U.ΣΣTE69

It leads to eigenvalue problem for JJT matrix. U is m× msquare matrix, which contains eigenvectors of JJT matrix in its columns and ΣΣT is diagonal matrix of eigenvalues λ1, …, λm.

To determine matrices V and Σ, we multiply matrix J by its transpose matrix JT from the left:

JT.J=UΣVTT.UΣVTE70
JT.J=VΣTUT.UΣVTE71
JT.J=VΣTΣVTE72

We multiply the above Eq. (72) by matrix V from the right:

JTJ.V=V.ΣTΣ.VT.VE73
JTJ.V=V.ΣTΣE74

It leads to eigenvalue problem for JTJ matrix. V is n× nsquare matrix, which contains eigenvectors of JTJ matrix in its columns and ΣTΣ is diagonal matrix of eigenvalues λ1, …, λn.

Matrices JJT and JTJ are symmetric matrices and they have the same nonzero eigenvalues. Eigenvalues and eigenvectors of the real symmetric matrices are always real numbers and real vectors.

The eigenvalues are equal to square of the singular values: λi=σi2, where i = 1, …, d. The number of nonzero eigenvalues is d = mfor m < nand d = nfor m > n. The number of zero eigenvalues is |mn|.

When values of matrices U, Σ, and V were computed, we can determine pseudoinverse of Jacobian matrix as follows:

J+=U.Σ.VT1E75
J+=V.Σ+.UTE76

Where

Σ+=1σ10001σ20001σdE77

Now, we can solve inverse kinematics task for the cases, when Jacobian matrix is non-square:

ΔQ=J+.ΔXE78

Pseudoinverse J+, also called Moore-Penrose inverse of Jacobian matrix, gives the best possible solution in the sense of least squares [1].

## 6. The Jacobian matrix transpose method

We designed Jacobian matrix transpose method simulation [13]. The basic idea was written using Eq. (79). We used the transpose of Jacobian matrix, instead of the inverse of Jacobian matrix, in this method. We set Δθequal to

Δθ=αJTeE79

Where α is:

α=eJJTeJJTeJJTeE80

Whole simulation is described by block diagram shown in Figure 14. In the first step, we defined requested error. This error represented difference between reference coordinates and actual coordinates. Error that we consider as unacceptable, we set to 200 μm. This is position repeatability of DOBOT. We calculate the increment of requesting angles Δθin each iteration. In the first iteration, Δθis equal to zero.

Figures 15 and 16 represent simulation result of DOBOT movement same as in simulation of Jacobian matrix pseudoinverse. Simulation was split on three parts. First part (solid line in chart) is movement from starting position to position (x, y, z) = (100, 150, 160) mm. Second part (dotted line in chart) is movement from previous position to position (50, 90, 80). And third part (dashed line in chart) is movement to position (150, 180, 140).

## 7. Conclusion

As we can see in simulation results from previous subchapters, every method for inverse kinematics has some positives and negatives. Comparison of both methods is shown in Table 2. Pseudoinverse method is faster than transposition method, but is harder to implement in a DSP or a microcontroller. In Matlab environment, pseudoinverse method is easily made by the pinv() command. If we want to simplify inverse kinematics and we don’t need fast calculating time, it is more readily to use transposition method. In the case of using DOBOT manipulator, it is considered to use the analytical model. In the case of more complicated manipulator, this method is inapplicable.

Simulation partPseudoinverse method (number of iterations)Transposition method (number of iterations)
Part 1 (solid line)2255
Part 2 (dotted line)534
Part 3 (dashed line)668

### Table 2.

Comparison of pseudoinverse and transposition method.

Comparison of both methods is shown in Table 2. As we can see in Table 2, the main criteria are number of iterations. Pseudoinverse method is much better, but only for simulation. If we can use this method in real-time application, like dSPACE from MathWorks® or implementation to DSP, we will not achieve such results like in Table 2. It is caused by using singular value decomposition (SVD), which is very demanding for a computation performance. In the other case, transposition of Jacobian matrix is much easier for implementation and need lower performance.

In the next research, we considerate the use suitable iterative method, like damped least squares. We also designed several implementation methods of Jacobian matrix transposition to DSP (TMS430, C2000™). It is very important to try more implementation methods for the most possible shortening of the calculation time.

## Acknowledgments

The results of this work are supported by Grant No. APVV-15-0571: research of the optimum energy flow control in the electric vehicle system.

## References

1. 1.Buss SR. Introduction to inverse kinematics with Jacobian transpose, pseudoinverse and damped least squares methods. IEEE Journal of Robotics and Automation. 2004;17:1-19
2. 2.Balestrino A, De Maria G, Sciavicco L. Robust control of robotic manipulators. In: Proceedings of the 9th IFAC World Congress; 1984;5:2435-2440
3. 3.Wolovich WA, Elliot H. A computational technique for inverse kinematics. In: Proceedings of the 23rd IEEE Conference on Decision and Control; 1984. pp. 1359-1363
4. 4.Denavit–Hartenberg [Internet]. 2017. Available from:https://en.wikipedia.org/wiki/Denavit%E2%80%93Hartenberg_parameters[Accessed: 03-08-2017]
5. 5.Desai JP. D-H Convention, Robot and Automation Handbook. USA: CRC Press; 2005 ISBN: 0-8493-1804-1
6. 6.Rehiara AB. Kinematics of Adept Three Robot Arm. In: Satoru Goto, editor. Robot Arms. 2011. InTech. ISBN: 978-953-307-160-2. Available from:http://www.intechopen.com/books/robot-arms/kinematicsof-adeptthree-robot-arm[Accessed: 01-08-2017]
7. 7.The MathWorks, Inc., SimMechanics 2User’s Guide [Internet], 2007. Available from:https://mecanismos2mm7.files.wordpress.com/2011/09/tutorial-sim-mechanics.pdf[Accessed: 08-04-2017]
8. 8.Bingul KS. The inverse kinematics solutions of industrial robot manipulators. In: IEEE Conference on Mechatronics; Istanbul, Turkey; 2004. pp. 274-279
9. 9.Serdar Kucuk, Zafer Bingul. In: Sam Cubero, editor. Robot Kinematics: Forward and Inverse Kinematics, Industrial Robotics: Theory, Modelling and Control. 2006. InTech. ISBN: 3-86611-285-8. Available from:http://www.intechopen.com/books/industrial_robotics_theory_modelling_and_control/robot_kinematics__forward_and_inverse_kinematics[Accessed 01-08-2017]
10. 10.WebPage [Internet], 2017. Available from:http://www.dobot.cc/[Accessed: April 04, 2017]

Written By

Ondrej Hock and Jozef Šedo

Submitted: April 5th, 2017Reviewed: October 3rd, 2017Published: December 20th, 2017