motion planning of serial manupalator using …

MOTION PLANNING OF SERIAL MANUPALATOR USING

PSEUDOINVERSE JACOBIAN METHOD AND DYNAMIC

MODELING USING RECURSIVE NEWTON-EULER METHOD

R. Yogeshwar Raoa, V. Pandu Ranga

b, B. Pattabhi Ramaiah

c , Mihir K Pandit

4

IIT Bhubaneswar, Argul, Bhubaneswar, India

ABSTRACT

Motion planning of the manipulator is an essential step while designing the robot to perform a task by moving it

from the start point to a goal point. It is important to note that the determination of inverse kinematics of an N

degree of freedom (DOF) manipulator finding the joint trajectories to trace the path between the said points is

difficult task. This paper tries to simulate motion trajectory of a N-DOF serial manipulator from an initial point to

target point through via-points. For finding inverse kinematics, in the present manuscript, an attempt is made to

utilize the pseudo-inverse Jacobian method. Further, the torques required by various joints of the manipulator while

carrying a specified payload is calculated by Recursive Newton-Euler dynamics. This torque data is finally used to

control the motor present at each joint of the manipulator via controller. The algorithm used in this paper can be

applied to any multi-degree robotic manipulator like PUMA and KUKA robots. This motion planning approach has

a wide application in medical and industrial fields. For example, robot assisted surgery, arc welding etc.

Keywords: Recursive Newton-Euler dynamics., Pseudo-inverse Jacobian method, Parabolic blend, DENOC

1. INTRODUCTION

A lot of research is already done on planning the motion of mobile robot where the robot

determines its path automatically to reach its target. In 1969 Nilsson suggested a Road Map

where it uses a visibility graph find its path without hitting any obstacle. In 1987 Chazelle[6]

came with a theory called Cell Decomposition where it divides the configuration space of robot

environment into cells by an algorithm in which it rejects those cells which contain obstacle and

find the optimum path using Dijkstra’s algorithm. In 1986 Khatib[7]

developed an algorithm

called Potential field where goal-position generates an attractive potential and obstacles generate

a repulsive potential. Since 2000, people also use A.I for motion planning. In 2005 Wu[8]

planned

a path for general manipulator using fuzzy reasoning.

Our aim is to move our manipulator from one point to other point by overcoming any obstacle.

In order to do that, first we performed forward kinematics of simple 3 DOF manipulator where

its link-length, orientation and joint parameter are given to find its end effector position. On this

basis, we extend it to N-DOF. Second, the inverse kinematics of the 3-DOF is simple to find but

when we increase the number of links, it becomes computationally expensive to get each joint

Contributory Session

Indian Institute of Technology, Bhubanesawar

2

parameter since non- linear equations are difficult to solve. So, we have to use some other

method to solve and develop algorithm according to that. Third, we have to find Jacobian which

provides a mapping between joint space and Cartesian space with which we can find velocity of

end effector when joint velocity is given. Fourth, we have to plan a trajectory for which we have

to set some via-points to fit a spline easily without any jerks. After getting the kinematics

parameters, we have to move towards dynamics part which is done by different methods like

Euler-lagrangian, Newton-Euler, Virtual Work but all this method does not give any direct

equation to solve we have to do some simplification. And this calculation increases with number

of links. So, we have to use some simplified and numerically stable mathematical technique to

solve this problem.

For solving inverse kinematics, we can’t use standard geometric approach as it is difficult to

generate algorithm which can applied to any no. of DOF So, we have to use an iterative

approach. In the above problem, we used a Pseudoinverse Jacobian method to solve the inverse

kinematics. This method first calculates the error between initial and final point in cartesian

space and then try to reduce this error in next iteration. We developed an algorithm in MATLAB

where we have to input the no. of DOF, Initial and, Final position and we get an output in terms

of joint angle between each link to reach that target. After that we have to generate algorithm for

dynamics. Here, we used Recursive Newton Euler approach. This approach is numerically stable

and computationally efficient and it does not give any spikes in graph like the other approach. In

this we have to evaluate DENOC (Decoupled Natural Orthogonal Complement) Matrix. When

this matrix is multiplied to newton Euler matrix consisting equilibrium equation, we get a

simplified analytic equation which can be solved easily to get torque.

2. METHOD

2.1 Synthesis of Kinematics

Kinematic model describes the spatial position of joints and link, and position and orientation of

the end effector. The derivatives of kinematics deal with mechanics of motion without

considering the forces that cause it. In this paper we have taken 3 DOF planar manipulator

having revolute joints where first link end point is taken as reference point.

64th Congress of

Indian Society of Theoretical and Applied Mechanics (ISTAM 2019), December 9-12, 2019

Indian Institute of Technology Kharagpur

3

2.1.1 Forward and Inverse Kinematics

In forward kinematics joint and link parameter are known to us with the help of transformation

matrix position and orientation of the end effectors can be calculated .it is easy as compared to

inverse kinematics

In inverse kinematics, coordinate of end effector are known to us from that orientation of each

link is to be calculated. This is difficult to solve as there is possibility of multiple solution .It can

be solved geometrically but we get non-linear equation which become more complex with higher

DOF links though it gives exact solution .In this paper a numerically approach is choosen

because it is easy to implement and can be used for higher DOF manupulators.

n n 0 n

0 0 o 0

n

0

E = T .E w h e re E = e n d e ffe c to r p o s it io n w .r .t b a s e fra m e

T = h o m o g e n o u s tra n fo rm a tio n m a tr ix

0

o E = p o s it io n o f b a s e fra m e

2.1.2 Pseudo-Inverse Jacobian method

In this method inverse kinematics of Multi-DOF link can be solved numerically[5]

Jacobian is the

linear mapping between change in output and change in input it can be calculated but it not

invertible in order to get invertible matrix.differentia the lagrangian function with and equate

it to zero. From this a psedo-inverse relation is founded out which can each iteration converges

to zero error in angles .according to algorithm error is being reduced in each iteration and it stop

when error in coordinates is low which means final coordinate reach the target position. From the

relation we get angles of each joint



4

n n T T

0 0 x y z 1 2

1 2

n

0

1 2

1 2

F ro m F K w e k n o w th a t

E = F (θ ) w h e re E = [e e e ] θ = [θ θ . . . . .θ ]

. . . . . .

d E = J d θ J = . . . . . .

. . . . . .

n

x x x

n

y y y

n

z z z

n

e e e

e e e

e e e

-1 n

0

-1 n n

0 0

d θ = J d E J = J a c o b ia n M a tr ix (3 × n )

Δ θ = J Δ E Δ θ = e rro r in jo in t a n g le Δ E = e rro r in p o s i t io n

As J-1

is not invertible we have to define another way to find error

T T -1 n T T -1

0

o u r o b jec tive is to m in im ize Δ θ S .T ab o ve eq u a tio n u s in g lan g arg ian m u ltip lie r w e g e t

Δ θ = J (JJ ) Δ E w h e re J (JJ ) = P seu d o -In verse Jaco b ian M atrix

Yes

No

Flow Chart 1 Algorithm for Inverse Kinematics using Pseudo-Inverse Jacobian method

E =T-r(i)

Forward kinematics we get r(i)=f(0 )

Calculate =JT

(J JT

)-1

* En

Updated angle

i=

0+

If E> reference error

Angle for every joint ()

Initial angle 0 and target coordinate T

64th Congress of



5

Figure 1 error vs iteration curve for 6 link mechanism

2.2 Trajectory Planning

The end effector of the manipulator is required to move in a particular fashion to accomplish a

specified task. The goal of trajectory planning is to describe the requisite motion of the

manipulator as a time sequence of joint /link/end effector. joint space trajectory planning is used

in this paper. In order to find a smooth function q(t) (joint variable vector) for each of the n-joint

of n-DOF manipulator. We have to fit an interpolation function. Which generate a trajectory

between initial position(qs) to a target position (q

g) within the given time.there are two ways to

do so

First fit a cubic polynomial with four constraints being initial and final velocity and position in

this we get a smooth position curve but discontinuity at end point is present in acceleration

curve. which in turn produce infinite jerk one way to avoid this difficulty is to increase the order

of polynomial , requiring more complex computation.

Second by linear function with parabolic blends It imposes a constant acceleration in start phase

cruise velocity and a constant deceleration in the final phase. Parabolic blends near the path

points are of same duration (tb) and the whole trajectory is symmetric about the halfway point in

time and position(tm). This approach takes less computation time and also acceleration and

velocity can be set to our manipulator need limit.



6

Figure 2 Trajectory equation for blended curve[1]

In order to solve the complex trajectory having sharp twist and turns some via points has been

setup to maintain the continuity at via points the parabolic blends of two segments also have a

smooth transition. This is achieved by choosing a continuous parabola for the two-blend segment

at via points.

Initial, final and intermediate coordinate

,time , no of link in a manupulator

Inverse kinematics [i], [f] , [im]

Trajectory equation by fitting curves using

initial,intermediate and final angles

Forward kinematics

Get the trajectory of end effector En(t)

64th Congress of



7

Flow chart 2 Trajectory algorithm

2.3 Dynamic Modeling

The Dynamic model of a manipulator is useful for torque and forces required for execution of a

typical work cycle. which is vital for the design of links, joints, drives, actuators. Dynamic

behavior of the manipulator provides relationship between joint torque and motion of links.

There are different methods to evaluate equilibrium equation i.e. torque required at each joint as

it is having revolute joint. Most common methods are Euler lagrangian and newton Euler method

both has its advantages and disadvantages .In euler-langragian we have to partially differentiate

lagrangian(K-P).and with increase of DOF , length of this lagrangian also increases and the

mathematical operation required to compute the torque at n (no. of DOF) joints is of the order of

O(n4) but it will give simplified analytical torque equations. In newton Euler it is easy to

formulate equation, and order of operation is also O(n) but hear we get one extra equation which

force equation at each joint. Which is not required and as the DOF increase computation time

also increases by using these techniques we also found out that they are not numerically stable.

They give lot of spikes in acceleration curve. Which makes difficult to get positions of joint

while integrating the curve.

•

ii i

E u le r-la g ra n g e (E L ) w h e re L = la g ra n g ia n = K -P q = jo in t v a r ia b le

N e w to n E u le r (N E ) m v = f w h e re f = e x te rn a l fo rc e

i

ii

d L L

d x qq

i

ii i i i i

n = e x te rn a l to rq u e

I ω + ω × I ω = n

2.3.1 DeNOC (Decoupled Natural Orthogonal Complement) Matrices

It is a velocity transformation matrix which convert angular velocity of joint to link velocity.

Which is reported by saha in 1997[4]

T T T

l d 1 2 n

1 2 n

t= N q w h e re N = N N t= tw is t v e c to r = [ t t . . . . t ] 6 n d im e n s io n a l v e c to r

= [ θ , θ ... . . θ ] n d im e n s io n a l v e c to r N = D E N O C M a tr ix (6 n × n )

q

T

T

kl k

d

k ,k -1

N = tw is t-p ro p o g a tio n p a r t o f D E N O C m a tr ix o f o rd e r 6 n × 6 n t [ω , o ]

N = jo in t m o tio n p ro p o g a tio n p a r t o f D N E O C (6 n n )

A = tw is t p ro p o g a tio n

k

m a tr ix



8

i i i i i i

i i ii

i i i i

i i i

W ritin g N e w to n e u le r e q u a tio n in c o m p a c t fo rm fo r i b o d y (6 1 )

M t + W M t = w

I O ω nω × 1 Ow h e re M = ,W = ,t = ,w =

O m 1 v fO O

W rit in g N e w to n e u le r e q u a tio n in c o m p a c t fo rm fo r n

th

E C

i i

1 2 n

1 2 n

b o d ie s (6 n 1 )

M t + W M t= w + w

w h e re M = d ia g [M M .... . .M ]

W = d ia g [W W .... .W ]

64th Congress of



9

2.3.2 Recursive Newton-Euler

In this method we try to combine the advantages of both methods. Recursion is a method of

solving problem where second last link solution depend on the solution of last link.so we to solve

the equation serially starting from the last link [3]

.

multiply newton Euler matrix with DENOC we get simplified n-dimensional analytic torque

equation which can be solved easily If we

3. RESULTS

E CN (M t + W M t)= N (w + w )

e q u a tio n in c o m p a c t fo rm

I θ + C θ = τ n -c o u p le d E L e q u a tio n w i th n o p a r t ia l d iffe re n tia t io n

I: n × n G e n e ra liz e d in te r t ia m a tr ix (G IM )

C : n × n M a tr ix o f c o n v e c tiv e in e r t ia (M C I ) te rm s

τ : n -d im e n s io n a l v e c to r o f g e n e rliz e d f o rc e s d u e to d r iv in g to rq u e a n d th o s e re s u lt in g fro m g ra v ity



10

Figure 3 Position of 6 DOF manipulator during estimation of Inverse kinematics at iteration number (a) 4 (b) 65

Figure 4 Trajectory of 6 DOF manipulator with via points

64th Congress of



11

Figure 7 Joint angle, velocity, acceleration of 6 DOF manipulator at 6 joints

Figure 6 Torque required by each joint during motion of manipulator.

S. No Input parameter of 6 links Values

1 Mass of link (m1, m2, m3, m4, m5, m6) in kg 0.3,0.5,0.6,0.7,0.4,0.1

2 Length of links (l1, l2, l3, l4, l5, l6) in m 8,7,6,9,7,4

3 Initial angles (01,02,03,04,05,06) in degree 60,30,30,60,30,60

4 Via points (3 coordinates) (-17,20), (-10,25), (0,30)

5 Target position (17,19)

6 Time to reach goal in seconds 9

7 Maximum acceleration in rad/s2

25

Table 1 Input parameter for 6 DOF manipulator



12

All the above figures are generated in MATLAB 2018a figure 2 shows the error which is

reducing iteration after iteration and finally converges to reference error. Figure 5 shows the line

diagram of 6-dof manipulator try to reach its final position at different iteration numbers during

estimation of inverse kinematics using Pseudo-Jacobian. figure 6 shows the trajectory of end

effector (blue line) with 3 via points (dark brown) at time equal to 6.8s. figure 7 shows motion

curves for different joint while performing task. Figure 8 shows torque curves which we can get

bafter putting angular acceleration values and other parameters in analytical equations.

4. CONCLUSION

Through this paper it is concluded that motion planning of general serial manipulator subjected

to obstacle can be done easily by pseudoinverse method. This approach can be helpful in

trajectory planning of arc welding and for other application involving complex

geometry.Dynamic modeling by Recursive Newton Euler approach solve the problem efficiently

without giving any spikes.Due to his stability and efficiency it can be used in real time torque

calculation of manipulator any order.

REFERENCES

1. Robotics and Control by R K Mittal and I J Nagrath ,McGraw Hill publication.

2. Mohan, A., and Saha, S.K., A recursive, numerically stable, and efficient simulation

3. Dynamics of Tree-Type Robotic Systems by S V Shah ,S K Saha and J K Dutt by springer

4. S K Saha,” Dynamics of Serial Multibody Systems Using the Decoupled Natural Orthogonal Complement

Matrices” Journal of Applied Mechanics 66(4) · December 1999

5. Patrick F. Muir and Charles P. Neuman, “Kinematic modeling of wheeled mobile robots” Journal of Robotic

Systems 1987

6. A. Aggarwal,B. Chazelle,L. Guibas,C. Ó'Dúnlaing,C. Yap “Parallel computational geometry” Algorithmica

3:293-327 1988

7. Oussama Khatib “The Potential Field Approach And Operational Space Formulation In Robot Control”

Adaptive and Learning Systems pp 367-377 1986

8. X.J. Wu, Q. Li, K.H. Heng “Development of a general manipulator path planner using fuzzy reasoning”

Industrial Robot 0143-991 2005

motion planning of serial manupalator using …

Documents