Open access peer-reviewed chapter

New Strategy to Approach the Inverse Kinematics Model for Manipulators with Rotational Joints

Written By

José-Emilio Vargas-Soto, Efren Gorrostieta-Hurtado, Saúl Tovar- Arriaga, Jesús-Carlos Pedraza-Ortega and Juan-Manuel Ramos- Arreguín

Submitted: 14 November 2015 Reviewed: 11 April 2016 Published: 12 October 2016

DOI: 10.5772/63541

From the Edited Volume

Automation and Control Trends

Edited by Pedro Ponce, Arturo Molina Gutierrez and Luis M. Ibarra

Chapter metrics overview

1,818 Chapter Downloads

View Full Metrics

Abstract

The chapter describes a new strategy to approach the solution of the inverse kinematics problem for robot manipulators. A method to determine a polynomial model approximation for the joints positions is described by applying the divided differences with a new point of view for lineal path in the end-effector of the robot manipulator. Results of the mathematical approach are analysed by obtaining the kinematics inverse model and the approximate model for lineal trajectories of a manipulator for three degrees of freedom. Finally, future research approaches are commented.

Keywords

  • inverse kinematics approach

1. Introduction

The modeling of robot manipulators with rotational joints has been extensively studied for decades. A robot manipulator is formed from the mechanical point of view by a kinematic chain of rigid bodies (links) which are joined together by linear or rotational joints. The modeling of the kinematics for a robot manipulator allows us to analyze the movements of the end-effector in order to perform a specific task.

To determine the kinematics model for a robot manipulator, conventionally there are two types of analysis, the first is called “Direct Kinematics Problem” and the second analysis is “Inverse Kinematics Problem.” Depending on what is required for a specific task in the manipulator, different mathematical models can be used to control the behavior of a wide variety of robot manipulators [13]. The Direct Kinematics Problem determines the position of the end-effector of the robot manipulator as a function of the degrees of freedom (joint), wherein each degree of freedom is an independent movement in the mechanical structure of the robot manipulator. In contrast, the Inverse Kinematics Problem is oriented to determine the variation of the degrees of freedom of the manipulator according to the kinematics of the end-effector of the robot manipulator. This last situation is usually more complex from the point of view of mathematical modeling and its solution, given the high nonlinearity of behavior of the robot manipulators. A further disadvantage of conventional models of the Inverse Kinematics Problem is called redundancy. This happens when more than one solution is obtained by the mathematical model of the robot manipulator, causing theoretical configurations that do not occur in practical movements of an industrial manipulator. Additionally, another problem that occurs in the analysis of manipulators is when matrices for translations and rotations are used in the modeling. There is the possibility that this inverse matrix shows singular points, causing undefined mathematical solutions. Translations and rotations are essentials to solve the Kinematics Inverse Problem, to do that, auxiliary reference systems are used to refer the kinematics of the end-effector in the reference inertial system. In this regard, this chapter shows a new strategy to solve the Inverse Kinematics Problem for manipulators with rotational joints by approximating with cubic polynomial functions that define the positions of the joints.

Advertisement

2. Solutions for the Inverse Kinematics Problem

One of the first methods developed to solve the inverse kinematics for manipulator is by "Algebraic Method". In general, the algebraic methods are obtained by vector equations that respond to the links geometry of the manipulator. By this way, trigonometric relationships and algebraic equations are obtained to get mathematical functions that determine the behavior of the degrees of freedom in the manipulator [46]. It might be supposed that this method is the most effective however; it presents mathematical uncertainties, inconsistencies, and considerable complexity.

Another method developed to determine inverse kinematics is by spatial geometry. This method is based on the decomposition of the spatial geometry of the manipulator in planar geometric systems. For many serial or parallel manipulators, this decomposition is simple when the axes of consecutive degrees of freedom present changes as 0°, 90°, and 270°. However, it is not always the case, so this method cannot always be applied. The inverse kinematics of each degree of freedom usually has more than one solution. Also, given the trigonometric relationships of mathematical models obtained, they cannot always be solved because it is more complex to obtain an exact solution for a greater number of degrees of freedom [7, 8].

One of the most used alternative to solve the inverse kinematics of robot manipulators is based on Newton methods. These algorithms seek target configurations which posed as solutions to minimization problem [9]. Due to extreme complexity, these methods are known to be less practical. There are also methods based on statistical filtering [10] and sequential iterative approaches [11]. However, these statistical methods suffer high computational cost.

Research about the use of polynomial functions with some relationships with the inverse kinematics problem for manipulators is not new. However, some of the applications are oriented to trajectory planning [12], and also to solve the inverse kinematics problem by polynomial of n-degrees by using genetic algorithms [13, 14].

This work presents a novel approach to the inverse kinematic problem by cubic polynomial functions which are built under the definition of new parameters and the divided differences recursive method. The present approach is different to other strategies to get a solution for inverse kinematics problem. The benefits of the proposed method are essentially the simplicity to obtain a polynomial function for rotational joints that solve the inverse kinematics problem (position, speed, and acceleration), and the warranty to get a unique solution with no singularities. However, as many of the mathematical models that attempt to describe a real physical phenomenon, the method has several restrictions: bi-dimensional work space, the end-effector paths are straight lines, two joints are free and a third joint have fixed orientation, and four points of the path trajectory are needed.

Advertisement

3. The method of divided differences

From the mathematics point of view, the divided differences is a recursive division process of increments. The method can be used to calculate the coefficients in the interpolation polynomial in the Newton form when data points (xi, yi) are known, where i is an integer ranging from 0 to (n−1) and n is the number of points.

By definition, it is called divided difference of f in the points (xi, yi) to the value f[x0, x1, x2, .., xk] which is calculated recursively by these equations [15]:

f[xi]=f(xi)=yiE1
f[xi,xi+1]=f[xi+1]f[xi]xi+1xiE2
f[xi,xi+1,,,xi+k]=f[xi+1,xi+2,..,xi+k]f[xi,xi+1,..,xi+k1]xi+kxiE3

To properly apply the method, it is essential to order in a table the values corresponding to the points (xi, yi), so that the xi column is sorted from the highest to lowest number, or vice versa. Considering an approach with four known points, the divided differences are defined as shown in Figure 1:

Figure 1.

Sequence of Divided Differences to four points.

Such polynomial function f(x) is determined as:

f( x )=f[ x 0 ]+f[ x 0 , x 1 ]( x x 0 )+f[ x 0 , x 1 , x 2 ]( x x 0 )( x x 1 )+ f[ x 0 , x 1 , x 2 , x 3 ]( x x 0 )( x x 1 )( x x 2 ) E4

or expressed in the reduced form:

f(x)=f[x0]+i=1n1(f[x0,,xi]*ji1(xxj))E5
Advertisement

4. Polynomial approach strategy

A new feature in the mathematical development shown in this work is to adapt the method of divided differences to approximate the functions that define the changes of the rotational joints θ1i y θ2i. For the approximation by polynomials, a table of four points known of the manipulator robot is formed; then the polynomial approach strategy can be applied. It is important to mention that for the analysis developed in this work, it is considered a robot manipulator with three rotational joints.

Point i xi yi θ1i θ2i θ3i
1 x1 y1 θ11 θ21 θ31
2 x2 y2 θ12 θ22 θ32
3 x3 y3 θ13 θ23 θ33
4 x4 y4 θ14 θ24 θ34

Table 1.

Known values of the four positions of the manipulator.

Before showing how to construct polynomials, it is relevant to note that the movement of the end-effector of the robot manipulator performs a linear motion by keeping constant the orientation of the end-effector (θ3). In the case of analysis shown in the X-Y coordinate plane, the positions changes occur in both coordinates. In this case, proportionality of change of θ1 and θ2 will be done considering the changes of x with respect to y. This is because both coordinates participate proportionally varying θ1 and θ2. To this end, we have defined two new indices denominated: incidence factor of x, and incidence factor of y. Both indices respond to the model of linear proportionality by the following equations:

θ1=(Incidencefactorofx)θ1+(incidencefactorofy)θ1E6
θ2=(incidencefactorofx)θ2+(incidencefactorofy)θ2E7

Taking into account that the sum of the percentages of both coordinates is 100%, the incidence factor of y in θ (in percentage) is determined as:

%ofy=100%%ofxE8

For example, for the line defined by the equation y = x, the proportionality of the change is the same for both coordinates, so the variation of the x coordinate has a 50% incidence in the changes of θ1 and θ2. Similarly, the variation of the y-coordinate has a 50% incidence in the changes of θ1 and θ2.

The determination % of x is obtained by the value of the slope of the straight line m, the incidence factor of x in θ (percentage) is calculated using the following equation:

%ofx=1001+|m|E9

The absolute value of m is obtained because it is only of our interest to obtain the proportionality of the changes of the angles θ1 and θ2 as a function of lineal movements. So that,

θ1=θ1(x,y)=(%ofx100%)θ1+(%ofy100%)θ1E10

similarly,

θ2=θ2(x,y)=(%ofx100%)θ2+(%ofy100%)θ2E11

The tables to determine the polynomials that approximate the behavior of the angle θ1 are built based on the new approach strategy presented here, remaining as:

i Xi (% ofxi100%)θ1i
1 x1 (% ofx1100%)θ11
2 x2 (% ofx2100%)θ12
3 x3 (% ofx3100%)θ13
4 x4 (% ofx4100%)θ14

Table 2.

x vs. influence of x in θ1.

i Yi (% ofyi100%)θ1i
1 y1 (% ofy1100%)θ11
2 y2 (% ofy2100%)θ12
3 y3 (% ofy3100%)θ13
4 y4 (% ofy4100%)θ14

Table 3.

y vs. influence of y in θ1.

In a similar way,

I Xi (% ofxi100%)θ2i
1 x1 (% ofx1100%)θ21
2 x2 (% ofx2100%)θ22
3 x3 (% ofx3100%)θ23
4 x4 (% ofx4100%)θ24

Table 4.

x vs. influence of x in θ2.

i Yi (% ofyi100%)θ2i
1 y1 (% ofy1100%)θ21
2 y2 (% ofy2100%)θ22
3 y 3 (% ofy3100%)θ23
4 y4 (% ofy4100%)θ24

Table 5.

y vs. influence of y in θ2.

One of the contributions of the strategy presented here is the composition of polynomial functions of the joints of the robot manipulator θ1 and θ2, both as independent functions of "x" and "y". These compositions are defined as the following equations:

θ1(x,y)=(a0+a1x+a2x2+a3x3)+(b0+b1y+b2y2+b3y3)E12
θ2(x,y)=(c0+c1x+c2x2+c3x3)+(d0+d1y+d2y2+d3y3)E13

where ai, bi are coefficients obtained from Tables 2 and 3, and the coefficients ci, di are obtained by Table 4 respectively. Thus, divided differences are shown in Figure 2.

Figure 2.

The Divided Differences of x vs. θi for four points.

where,

Δ11=θ1θ0x1x0E14
Δ12=θ2θ1x2x1E15
Δ13=θ3θ2x3x2E16
Δ21=Δ12Δ11x2x0E17
Δ22=Δ13Δ12x3x1E18
Δ31=Δ22Δ21x3x0E19

developing the general equation,

ϑ(x)=ϑ[x0]+i=1n1(ϑ[x0,,xi]×ji1(xxj))E20

This equation allows us to obtain the model of the polynomial that approximates the angular position θ with respect to the variable x.

ϑ(x)=f[x0]+Δ11(xx0)+Δ21(xx0)(xx1)+Δ31(xx0)(xx1)(xx2)E21

In developing this equation and simplifying results,

θ( x )=( θ 0 Δ 11 + Δ 21 x 0 x 1 Δ 31 x 0 x 1 x 2 )+[ Δ 11 + Δ 21 ( x 1 x 0 )+ Δ 31 ( x 0 x 1 + x 2 ( x 1 + x 0 ) ) ]x +[ Δ 21 + Δ 21 ( x 2 x 1 x 0 ) ] x 2 + Δ 31 x 3 E22

Similarly, the polynomial θ(y) is obtained, which has the following form:

θ( y )=( θ 0 Δ 11 + Δ 21 y 0 y 1 Δ 31 y 0 y 1 y 2 )+[ Δ 11 + Δ 21 ( y 1 y 0 )+ Δ 31 ( y 0 y 1 + y 2 ( y 1 + y 0 ) ) ]y +[ Δ 21 + Δ 21 ( y 2 y 1 y 0 ) ] y 2 + Δ 31 y 3 E23

Clearly, the differences divided for this equation refers to the rate of change of the θ with respect to y, being defined by the polynomial approximation model of joint θ1 and θ2 of the robot manipulator, which in simplified form are expressed as:

θ1(x,y)=θ1(x)+θ1(y)E24
θ2(x,y)=θ2(x)+θ2(y)E25
Advertisement

5. Development of the approach by polynomials

In this section, the strategy proposed to approach the Kinematics Inverse Problem by polynomial functions of the rotational joints of a robot manipulator is shown. The procedures to three different cases of lineal path in the end-effector are analysed: a) slope of a straight line, m = 1, b) slope of a straight line, m < 1 and c) slope of a straight line, m > 1. To show the procedures, it is important to describe the mechanical configuration of the robot manipulator that is shown in Figure 3.

Figure 3.

Configuration of the robot manipulator, Θ1=90°, Θ2=0°, Θ3=90°.

The three degrees of freedom that are considered known in the plane of the robot manipulator moves are (x, y, θ3). The link in the base of the robot manipulator is 35 units of effective length, the second link is 30 units of length, and third link is 10 units of length. The orientation of the end-effector is considered constant during the lineal movements.

Remembering that the procedure exposes that four positions of the configuration robot are known, the approach by polynomial function are described in the next lines:

a) Case 1. Slope of a straight line, m = 1.

Figure 4.

The four Robot positions, slope = 1.

In order to show the effectiveness of the developed strategy, the movement of the end-effector of the robot manipulator was selected by considering that the maximum length of the lineal trajectory is close to the effective length of link 1. For this case, the orientation of the end-effector is 90° and is keeping constant during its moves. Under these considerations, Figure 4 shows the selected lineal trajectory.

The values of the four known positions of the robot manipulator are shown in Table 6.

x y Θ1 [°] Θ2 [°] Θ3 [°]
−15 25 193.51° 50.58° 90°
−5 35 158.15° 23.53° 90°
0 40 144.28° 18.59° 90°
10 50 121.37° 19.71° 90°

Table 6.

The four known positions of the lineal trajectory.

The incidence factor of x in θ, according to Eq. (9) and the value of the slope of a straight line is calculated as:

%ofx=1001+|m|=1001+|1|1002=50E26

Then, from Eq. (8) the incidence factor of y in θ takes the next value:

%ofy=100%%ofx=10050=50E27

Considering both factors, the tables to obtain the polynomial functions for the joint θ1 are defined as:

i Xi θ1i (x)
1 −15 96.755
2 −5 79.075
3 0 72.14
4 10 60.685

Table 7.

x vs. incidence of x in θ1.

i Yi θ1i (y)
1 25 96.755
2 35 79.075
3 40 72.14
4 50 60.685

Table 8.

y vs. incidence of y in θ1.

The polynomial approach of Table 7 is determined by the divided differences according to Eqs. (14) to (19). In this way, next table shows the values obtained:

Xi θ1i (x) Δ1i Δ2i Δ3i
−15 96.755 −1.768 0.0254 −0.000372
−5 79.075 −1.387 0.0161
0 72.14 −1.1455
10 60.685

Table 9.

Values of the divided differences for θ1(x).

Applying Eq. (22) and simplifying results:

θ1(x)=72.141.2879x+0.0179x20.000372x3E28

This polynomial function represents the incidence of x in joint of θ1. To complete the approach, it is also necessary to add the incidence of y in the same joint of θ1. So the polynomial approach from Table 8 is obtained by applying Eqs. (14) to (19).

Yi θ1i (y) Δ1i Δ2i Δ3i
25 96.755 −1.768 0.0254 −0.000372
35 79.075 −1.387 0.0161
40 72.14 −1.1455
50 60.685

Table 10.

Values of the divided differences for θ1(y).

Applying Eq. (23) and simplifying it gives:

θ1(y)=176.24.5103y+0.0626y20.000372y3E29

According to Eq. (24) the complete function approximation of θ1 becomes:

θ1(x,y)=248.341.2879x+0.0179x20.000372x34.5103y+0.0626y20.000372y3E30

The next step is to obtain the function approximation of θ2(x). According to the above procedure, the differences divided for Table 8 to get the polynomial approximation with respect of x takes the form:

Xi θ2i (x) Δ1i Δ2i Δ3i
−15 25.29 −1.3525 0.05723 −0.0008267
−15 11.765 −0.494 0.0366
0 9.295 0.056
10 9.855

Table 11.

Values of the divided differences for θ2(x).

Applying Eq. (22) and simplifying it gives:

θ2(x)=9.2950.26953x+0.04078x20.00082267x3E31

Similarly, the divided differences of Table 8 to get the polynomial approximation for θ2(y) takes the form:

Yi θ2i (y) Δ1i Δ2i Δ3i
25 25.29 −1.3525 0.05723 −0.0008267
35 11.765 −0.494 0.0366
40 9.295 0.056
50 9.855

Table 12.

The divided differences values for θ2(y).

Applying Eq. (23) and simplifying it gives:

θ2(y)=137.9757.48073y+0.1395y20.00082267y3E32

According to Eq. (25) the complete function approximation of θ2 becomes:

θ2(x,y)=147.270.26953x+0.04078x20.00082267x37.48073y+0.1395y20.00082267y3E33

Obtaining result from the complete approximation by polynomials θ1 and θ2:

θ1(x,y)=248.341.2879x+0.0179x20.000372x34.5103y+0.0626y20.000372y3E34
θ2(x,y)=147.270.26953x+0.04078x20.00082267x37.48073y+0.1395y20.00082267y3E35
x(15,10);y(25,50)

After obtaining the desired approach, the next step is to check the positions of the end-effector of the robot manipulator in order to verify that a lineal trajectory is made. According to the Direct Kinematics Model of the robot manipulator, the approximate position of the end-effector is determined by:

[xaproxyaprox]=[L1cosθ1(x,y)+L2cosθ2(x,y)+L3cosθ3L1sinθ1(x,y)+L2sinθ2(x,y)+L3sinθ3]E36

Remembering that L1 = 35, L2 = 30, L3 = 10 y θ3 = 90°, the Eq. (36) is evaluated in the range x(15,50) where y(x) = x + 40. Considering an increment Δx = 2.5, the following table is obtained:

x y Θ1(x,y) [°] Θ2(x,y) [°] xapprox yapprox
−15 25 193.497 50.580 −14.983 25.007
−12.5 27.5 183.534 41.286 −12.391 27.637
−10 30 174.368 33.782 −9.896 30.116
−7.5 32.5 165.930 27.915 −7.441 32.554
−5 35 158.149 23.530 −4.980 35.004
−2.5 37.5 150.955 20.473 −2.493 37.485
0 40 144.280 18.590 0.019 39.998
2.5 42.5 138.053 17.726 2.544 42.530
5 45 132.205 17.728 5.063 45.061
7.5 47.5 126.665 18.440 7.560 47.565
10 50 121.364 19.710 10.026 50.003

Table 13.

Values of θ1, θ2, xapprox and yapprox.

In order to ensure the validity of the polynomial function, the Error of x, Error of y, and Error(x, y) are calculated by the next equations:

Errorofx=xxaproxE37
Errorofy=yyaproxE38
Error(x,y)=(xxaprox)2+(yyaprox)2E39

Getting the following table for the values of Table 13:

x y Error of x Error of y Error (x, y)
−15 25 −0.017 −0.007 0.0179
−12.5 27.5 −0.109 −0.137 0.1754
−10 30 −0.104 −0.116 0.1555
−7.5 32.5 −0.059 −0.054 0.0801
−5 35 −0.020 −0.004 0.0206
−2.5 37.5 −0.007 0.015 0.0161
0 40 −0.019 0.002 0.0190
2.5 42.5 −0.044 −0.030 0.0529
5 45 −0.063 −0.061 0.0879
7.5 47.5 −0.060 −0.065 0.0881

Table 14.

Errors of the polynomial approach.

The error of interest is the maximum error, in this case the maximum error is 0.1754 units and occurs in the point (−12.5, 27.5). The percentage of maximum error is determined by the following equation:

%Maximumerror=|Maximumerror(x,y)|(xfxi)2+(yfyi)2×100E40

where (xi, yi) are the coordinates of the starting point and (xf, yf) are the coordinates of the endpoint. That is, for an error rate, the maximum error is compared with the path length. Substituting values in Eq. (40), it gives:

%Maximumerror=|0.1754|(10(15))2+(5025)2×100=0.496%E41

Considering that this value is acceptable, Eqs. (34) and (35) can be used to do a lineal trajectory in the end-effector of the robot manipulator.

b) Case 2. Slope of a straight line, m < 1.

Similarly, as in the previous case, the end-effector of the robot manipulator makes a lineal path, which is shown in Figure 5. In this case it is considered θ3 as a constant, with the value of 45°.

Figure 5.

The four Robot positions, negative slope.

For this case, the values of four known positions of the manipulator robot are shown in the following table.

x y Θ1[°] Θ2[°] Θ3[°]
−10 45 159.34 58.46 45
0 40 153.87 35.69 45
10 35 139.48 9.94 45
20 30 117.01 -15.99 45

Table 15.

Four known positions of the manipulator, negative slope.

The four known positions correspond to the linear relationship y = −0.5 x + 40. Considering that slope: m = −0.5, the incidence factors can be calculated by the (8) and (9) equations, like this:

%ofx=1001+|m|=1001+|0.5|=1001.5=66.66E42
%ofy=100%%dex=10066.66=33.34E43

According to the presented strategy, the incidence table to approximate polynomial functions θ1 takes the next form:

i xi θ1i(x) [°]
1 −10 106.216
2 0 102.569
3 10 92.977
4 20 77.998

Table 16.

x vs. incidence of x in θ1.

i yi θ1i (y) [°]
1 45 53.108
2 40 51.284
3 35 46.488
4 30 38.999

Table 17.

y vs. incidence of y in θ1.

Calculating the polynomial approximation to θ1(x), the next table is obtained:

Xi θ1i (x) Δ1i Δ2i Δ3i
−10 106.2165 −0.3647 −0.029725 9.3x10-5
0 102.569 −0.9592 −0.026935
10 92.977 −1.4979
20 77.998

Table 18.

Divided differences values for θ1(x).

Applying Eq. (22) and simplifying it gives:

θ1(x)=102.5690.67125x0.029725x2+9.3×105x3E44

Likewise,

Yi θ1i (y) Δ1i Δ2i Δ3i
45 53.108 −0.3648 −0.05944 −0.000372
40 51.284 0.9592 −0.05386
35 46.488 1.4978
30 38.999

Table 19.

Divided differences values for θ1(y).

Applying Eq. (23) and simplifying it gives:

θ1(y)=46.864+3.6409y0.0148y20.000372y3E45

The complete function approximation of θ1 becomes:

θ1(x,y)=55.7050.67125x0.029725x2+9.3×105x3+3.6409y0.0148y20.000372y3E46

Also, the tables to approximate polynomial functions θ2 are:

i Xi θ2i (x)
1 −10 38.969
2 0 23.790
3 10 6.626
4 20 −10.658

Table 20.

x vs. Incidence of x in θ2.

i Yi θ2i (y)
1 45 19.484
2 40 11.895
3 35 3.313
4 30 −5.329

Table 21.

y vs. incidence of y in θ2.

Calculating the polynomial approximation for the Tables 20 and 21, we obtain:

θ2(x)=23.791.64823x0.009925x2+0.00031083x3E47
θ2(y)=6.1932.7342y+0.12942y20.001244y3E48

According to Eq. (25) the function approximation of θ2 becomes:

θ2(x,y)=17.5971.64823x0.009925x2+0.00031083x32.7342y+0.12942y20.001244y3E47

Determining the complete polynomial approximation of θ1 and θ2 by Eqs. (46) and (47) respectively, it results to:

θ1(x,y)=55.7050.67125x0.29725x2+9.3×105x3+3.6409y0.0148y20.000372y3E48
θ2(x,y)=17.5971.64823x0.009925x2+0.00031083x32.7342y+0.12942y20.001244y3E49
x(10,20);y(45,30)

Applying the direct kinematic model of the robot manipulator defined by Eq. (36), it proceeds to verify polynomials approach θ1 and θ2. In this case, θ3 is a constant with the value of 45°, where y = −0.5 x + 40, with x ∈ (−10.20). Considering an increase Δx = 3 units, the following table is obtained:

x y Θ1(x,y) [°] Θ2(x,y) [°] xapprox yapprox
−10 45 159.324 58.453 −9.979 44.995
−7 43.5 158.669 52.102 −7.103 43.476
−4 42 157.158 45.306 −4.085 41.984
−1 40.5 154.815 38.142 −1.007 40.493
2 39 151.662 30.685 2.065 38.994
5 37.5 147.721 23.009 5.093 37.489
8 36 143.016 15.192 8.065 35.989
11 34.5 137.568 7.308 10.995 34.502
14 33 131.401 −0.566 13.923 33.028
17 31.5 124.536 −8.357 16.910 31.543
20 30 116.997 −15.987 20.023 29.995

Table 22.

Values θ1, θ2, xapprox and yapprox.

Applying Eqs. (37), (38), and (39) the table of the errors become:

X y Error of x Error of y Error(x, y)
−10 45 −0.021 0.005 0.0218
−7 43.5 0.103 0.024 0.1062
−4 42 0.085 0.016 0.0863
−1 40.5 0.007 0.007 0.0099
2 39 −0.065 0.006 0.0654
5 37.5 −0.093 0.011 0.0938
8 36 −0.065 0.011 0.0656
11 34.5 0.005 −0.002 0.0059
14 33 0.077 −0.028 0.0816
17 31.5 0.090 −0.043 0.0995

Table 23.

Errors of the polynomial approach.

Figure 6.

The four Robot positions, infinite slope.

The maximum error occurs at (−7, 43.5). From Eq. (40) the maximum percentage of error becomes:

%Maximunerror=|0.1062|(20(10))2+(3045)2×100=0.316%E50

Similarly as in the previous case, this value is reasonably acceptable. So for this case, Eqs. (48) and (49) can be used to do the lineal trajectory in the end-effector of the robot manipulator.

c) Case 3. Slope of a straight line, m > 1.

As in the two previous cases, the end-effector of the robot performs a linear path. In this case, the geometric robot positions were made considering an infinite slope (x = constant). Figure 6 shows these positions, and the angle of the end-effector is considered constant with a value of 180°.

The values of the known positions are shown in the Table 24:

X y Θ1 [°] Θ2 [°] Θ3 [°]
−50 −20 248.6 155.16 180
−50 −10 239.46 137.78 180
−50 0 226.55 122.06 180
−50 15 203.45 105.24 180

Table 24.

Four know positions of the manipulator, infinite slope.

Considering that slope: m = ∞, the incidence factors can be calculated by the (8) and (9) equations like this:

%ofx=1001+|m|=1001+||=100=0E51
%ofy=100%%dex=100E52

As expected, the change in x has no any incidence on changes in θ1 and θ2, it is in this case that the change of y causes the change in both degrees of freedom of the robot manipulator.

The incidence tables to approximate polynomial functions θ1 and θ2 take the next form:

I yi θ1i (X) [°]
1 -20 248.6
2 -10 239.46
3 0 226.55
4 15 203.45

Table 25.

y vs. incidence of y in θ1.

I yi θ2i (X) [°]
1 -20 155.16
2 -10 137.78
3 0 122.06
4 15 105.24

Table 26.

y vs. incidence of y in θ2.

Performing the same procedure as in the previous cases, the desired functions of polynomial approximation are obtained for both Tables 25 and 26:

θ1(y)=226.551.4287y0.01123y2+0.000254y3E53
θ2(y)=122.061.4334y+0.01663y2+0.0002779y3E54

In this case θ3 = 180°, with y(20,15) and x = 50. Considering an increase Δx = 3.5 units, the following Table 27 is obtained:

y Θ1(X, Y) [°] Θ2(X, Y) [°] yaprox Error of y
−20 248.600 155.16 −19.983 −19.983
−16.5 245.925 148.990 −16.500 0.000
−13 242.667 142.894 −12.994 −0.006
−9.5 238.891 136.940 −9.484 −0.016
−6 234.663 131.199 −5.979 −0.021
−2.5 230.048 125.743 −2.481 −0.019
1 225.110 120.644 1.014 −0.014
4.5 219.917 115.972 4.512 −0.012
8 214.532 111.799 8.015 −0.015
11.5 209.021 108.198 11.520 −0.020
15 203.450 105.239 15.017 −0.017

Table 27.

Values θ1, θ2, yaprox, and Error of y.

The maximum error in this case is in y =−6. From Eq. (40) the maximum percentage of error becomes:

%errormáximo=|0.021|(5050)2+(15(20))2×100=0.06%E55

This value is so small that it can provide security to use the approximation obtained by Eqs. (53) and (54).

Until now, there have been three different cases that demonstrate an acceptable approximation in the position of θ1 and θ2 by polynomial functions. Given that the position of the end-effector of the robot is known, it is considered in this work that the speed and acceleration of the end-effector are well-known. Considering that the polynomial approximation for any degree of freedom has the following form:

θ(x,y)=c0+c1x+c2x2+c3x3+c1y+c2y2+c3y3E56

Deriving this equation, the speed for any degree of freedom is defined as:

θ˙(x,y)=ddt(θ(x,y))=θ(x,y)dx*dxdt+θ(x,y)dy*dydtE57

Deriving newly, the acceleration for any degree of freedom becomes:

θ¨ (x,y)= ddt (θ (x,y)dx) * dxdt+ θ (x,y)dx * ddt ( dxdt)+  ddt (θ (x,y)dy) * dydt+ θ (x,y)dy * ddt ( dydt)E58

By applying Eqs. (57) and (58), speed and acceleration for any degrees of freedom of the manipulator can be obtained respectively.

Advertisement

6. Conclusions

One of the basic activities related to the movement of industrial robots is the solution of the Inverse Kinematics Problem. This chapter has presented a new strategy that allows for an approximate solution without the use of conventional methods. However, the approach method has several restrictions. Thus, it is possible to achieve an alternative solution to the Inverse Kinematics Problem by polynomial functions that define the behaviour of the rotational joints. An advantage of this strategy is that it is easy to implement, and after getting the polynomial function for the position of the joints, the speed and acceleration can be obtained by conventional derivation.

Moreover, because this approach is made from four known configurations of the robot, it can be considered that the methodology described is a way of discretizing the continuous kinematics of the robot, which is an interpolation technique to the inverse kinematic problem.

Given the restrictions described in the analysed cases, further research is needed to determine if this strategy could be used for nonlinear paths; or for a kind of paths that require changes in the orientation of the end-effector, and also explore the possibility to extend the method for manipulator with more degrees of freedom.

References

  1. 1. R. Paul. Robot manipulators: mathematics, programming and control: the computer control of robot manipulators. MIT Press; 1981.
  2. 2. L. Sciavicco, B. Siciliano. Modeling and Control of Robot Manipulators. 2nd ed. London: Springer Verlag; 2000.
  3. 3. E. Dombre, W. Khalil. Robot Manipulators: Modeling, Performance Analysis and Control. USA: Wiley-ISTE; 2007.
  4. 4. J. Craig. Geometric Algorithms in AdeptRAPID. In: P. Agarwal, L. Kavraki y M. Mason, editors. Robotics: The algorithmic Perspective: 1998. Massachusetts: AK Peters, Natick; 1988.
  5. 5. H. Durrant_White. Uncertain Geometry in Robotics. IEEE Conference on Robotics and Automation; Raleigh, NC., IEEE; 1987.
  6. 6. G. A. Kramer. Solving geometric constraint systems: a case study in kinematics. Cambridge, MA, USA: MIT Press; 1992.
  7. 7. C. G. Lee, M. Ziegler. Geometric approach in solving inverse kinematics of PUMA robots. IEEE Transactions on Aerospace and Electronic Systems. 1984;6:695-706.
  8. 8. D. Chablat, P. Wenger, I. Bonev. Kinematic analysis of the 3-RPR parallel manipulator. In: 10th International Symposium on Advances in Robot Kinematics; June 2006; Ljubljana, Slovenia. Kluwer Academic Publishers; 2006. pp. 221-228.
  9. 9. A. Aristidou, J. Lasenby. Inverse Kinematics: a review of existing techniques and introduction of a new fast interactive solver. CUED/F-INFENG/TR-632 ed. Cambridge: University of Cambridge; 2009.
  10. 10. N. Courty, E. Arnaud. Inverse kinematics using sequential monte carlo methods. In: Proceedings of the V Conference on Articulated Motion and Deformable Objects; Mallorca, Spain. 2008. pp. 1-10.
  11. 11. L. Unzueta, M. Peinado, R. Boulic, A. Suescum. Full-body performance animation with sequential inverse kinematics. Journal of Graphical Models. 2008;70(5) pp. 87-104.
  12. 12. H. R. E. Haghighi, M. A. Nekoui. Inverse kinematic for an 8 degrees of freedom biped robot based on cubic polynomial trajectory generation. In: International Conference on Control, Instrumentation and Automation (ICCIA 2011); Shiraz, Iran. 2011. pp. 935-940. DOI: 10.1109/ICCIAutom.2011.6356787.
  13. 13. A. Machmudah, S. Parman, A. Zainuddin. nth degree polynomials joint angle path by approximation of inverse kinematics data using Genetic Algorithm. In: 2010 International Conference on Intelligent and Advanced Systems (ICIAS); Kuala Lumpur, Malaysia. 2010. p. 1-6. DOI: 10.1109/ICIAS.2010.5716213.
  14. 14. F. Chapelle, P. Bidaud. A closed form for inverse kinematics approximation of general 6R manipulators using genetic programming. In: IEEE, editor. IEEE International Conference on Robotics and Automation (ICRA 2001); Seoul, Korea. 2001. p. 3364-3369. DOI: 10.1109/ROBOT.2001.933137.
  15. 15. L. M. Milne-Thomson. The Calculus of Finite Differences. American Mathematical Society; Providence, RI, USA. 2000.

Written By

José-Emilio Vargas-Soto, Efren Gorrostieta-Hurtado, Saúl Tovar- Arriaga, Jesús-Carlos Pedraza-Ortega and Juan-Manuel Ramos- Arreguín

Submitted: 14 November 2015 Reviewed: 11 April 2016 Published: 12 October 2016