Motion Planning of Humanoid Robot Arm for graspingtask

Altaf Hussain RAJPAR', Mohammad Usman KEERIO2, Attaullah KAWAJA31, 2 Department of Mechatronics Engineering, 3 Department of Electronics Engineering,

Beijing Institute of Technology,5 Nandajie Zhongguancn, Haidian, Beijing,

China, 10008 1.

1 altafrajpar@yahoo.com, 2 usmankeerio@yahoo.com, 3 atta_khawaja@yahoo.com

Abstract -. A new method for computing numerical solution tothe motion planning of humanoid robot arm is developed. Theproposed method is based on combination of two nonlinearprogramming techniques. Forward recursion formula withbackward cycle computation method is used to reach in thevicinity of target object and then FBS method is used to graspobject in real-time. Proposed method is numerically stable,computationally effective and it is not sensitive to singularconfiguration of the manipulator. Effectiveness of the proposedmethod is achieved through results.

Index Terms- Transportation, course alignment, Inversekinematics and trajectories.

I. INTRODUCTION

Motion of mechanical manipulator is an active area ofresearch, critical problem is the control of pose and positionof the end effe-effector for performing specific tasks.Proposed solution to this problem is often called path planningdifferent approaches have been adopted for problem solutionsuch as geometrical, algebraic and numerical. Geometrical andalgebraic have limited flexibility, numerical solutions arecomputationally expensive and do not deal with singularityproblems [1] [2]Motion planning of end-effector at low level is inversekinematics control of robot manipulators. . Inverse kinematicsis the crucial problem in the control of the redundant robotmanipulators. The transformation of the position andorientation of manipulator from Cartesian coordinate to jointcoordinate is known as inverse kinematics problem.Kinematics of the manipulators involve the study of thegeometric and time based properties of motion and inparticular how the various links move with respect to one another and with speculated time. In general the inversekinematics solution is obtained geometrically, analytically andnumerically. The closed form solution of the manipulators islimited to the simple mechanical manipulator where as forredundant robot arm there is a problem of singularity incomputation. To avoid singularity problems, numericalsolution is applied to redundant robot manipulators. There arebasically two types of the numerical methods that have beendeveloped for solving the inverse kinematics problems, firstone is the Newton rophson method to solve the nonlinearkinematics equation, this method is sensitive to singularity or(ill-conditioned matrix). Several methods have been adopted

to overcome these difficulties [3] [4]. The second method isbased on optimization techniques, instead of solving theinverse kinematics problem directly, this method usesgradient-based nonlinear programming algorithm to solve anequivalent minimization problem [5] [6].[7]The method developed in this paper is based on combinedoptimization approach in which desired position of themanipulator is solved by using forward recursion formula andbackward cycle where as small difference of location andorientation is adjusted by using Broyden-Fletcher-shanno(BFS) variable metric method [8] to obtain solution at a desiredegree of precision.

The organization of this paper is as follows Section IIpresent the system description, in section III Kinematicsmodel of humanoid robot arm is presented. In section IVInverse kinematics and numerical algorithm Motion controlstrategy is presented Section V. Section VI describes velocityand trajectory generation. Section VII comprises Experiments.In VIII conclusion of the work is summarized

II. SYSTEM OVERVIEW OF BHR-2

Fig.1 shows the overview of the system of BHR-2. Theheight and the weight are approximately 160cm and 63 kg.BHR-2 has more than 32 Degrees of Freedom (DOF).Eacharm has 6- DOF and each hand has 3-DOF. For each leg, thereare 6- DOF. For the head, there are 2- DOF.

Fig. 1 BHR-2

1-4244-1494-6/07/$25.00 C 2007 IEEE

The stereo vision system mounted on the pan-tilt robothead uses SVS cameras, which samples 15 frame/s. The sizeof the original image is 320x240 pixels. Ttwo computersembedded in the body of BHR-2. One computer is used forinformation processing and named information sub-system,the other is used for motion control and named control sub-system. The information sub-system adopts Windows as theoperating system with PIll 2.4GB and memory 512MB. Thecontrol sub-system adopts Linux and RT-Linux as theoperating system with PIll 700MHz and memory 256MB. RT-Linux is a real-time operating system, which can satisfy thereal-time quality of moving control. Windows is an idealplatform of image processing because of its strong multimediafunction support. Memolink is the bridge of communicationbetween two sub-systems which adopts PCI interface and itstransmission rate is 1 Mbytes/s. Detail about this system isdescribed in [9]

III. KINEMATICS MODEL OF THE HUMANOID ROBOTARM

Fig. 2 shows the reference and link coordinate systems ofthe 6-DOF arm, the link length from centre of CCD camerasto shoulder plane (neck height) is 175 mm, neck to shoulder is210 mm, upper arm length is 220 mm and lower arm length is175 mm, where as length of the hand is 130 mm. Theseparameters correspond to the home position pictured in thelink coordinate diagram. The World coordinate of definedsystem is considered at the shoulder position of the arm asshown in fig. 2

1.,

I. -.. -.YerbMUef

-L.

~~I

leLt jr 4 I1 (1;

L]

:

I a-

Fig 2 Kinematics model of the BHR-2

IV. INVERSE KINEMATICS AND NUMERICAL ALGORITHM

The Cartesian coordinates of the end-effector are definedas the position vector Ph(q) and the orientation matrix [Rh(q)],

where Ph(q) is the current position of the end-effector and[Rh(q)] is the orientation matrix of the end-effector at initialposition and (q) is the vector of joint variables, q =

[ql,q2.. q6]. When desire Cartesian coordinate of the end-effector (detected through vision system) Pd and [Rd]= [d,ld2ld3]where dj =(1 to3)then find the joint variable vector (q) suchthat Ph(q) = Pd and [Rh(q)] = [Rd]. In addition the solutionvector should satisfy the Physical limitation of the joints i.e. q1'< qi < q1" for i = 1 to n, where qi; and qi" are respectively thelower and upper bounds of the ith joint variable. For any givenq the Cartesian location (position and orientation) of the ende-ffector can be computed by using the forward recursionformula [2]. If q is not the solution of the problem, then thecomputed location will not agree with the desired location.However we can find the correction vector 6q such that q* =

q+ 6q is a solution to problem then point Oh (hand coordinate)should coincide with Od(desired coordinate) and theorientation of the two coordinate system should be identical.This implies that 11 6p(q*) 11 = 11 Pd -Ph(q*)Il = 0 dj and hj(q*) =1for j =1 to 3, where 11.11 denotes the vector dot product.Therefore errors between the current and desired location ofthe end- effecor is scalar function of q as defined below

Position error AP(q) = 6P(q). 6P(q) (1)

Orientation error: Acq-I(cqiosqIc -1) (2)Where as orientation error in each of three direction has theequal weighting =1.Total error: E(q) = AP(q) + AO(q) (3)The original inverse kinematics problem is now transformedin to the minimization problem so that find q* such that E(q*)min {E(q)l qi' < qi < qi",i = 1 to n and E(q*) < >-*O where £ isa prescribed small tolerance.

For the solution of the minimization problem the modifiedapproach [10] is adopted, where author has generalizedapproach for optimization problems here we have tested andimplemented on humanoid robot arm. Formulated problemdescribed above is a finite dimensional nonlinearprogramming , the method developed in this paper is based onthe combined optimization, where forward recursion formulaand backward cycle is used to calculate feasible point that isnear to true solution then Broyden-fletcher-shanno (BFS)method described in [10] is used to obtain solution at desiredegree of precision. The forward recursion formula andbackward cycle consist of n steps, at the ith step ( i varyingfrom n to 1 ), only the ith joint variable is allowed to bechanged to minimize the objective function. Usage of theforward recursion and backward cycle is as follows P, indicatethe current position of point 0, (origin of ith coordinate frame)Pih is the vector from O1to the current position of the ende-ffector and Pid is vector from O1to the desired position of theend-effector. In order to minimize the position and orientationerror vector attached to the end-effector and the coordinatesystem defined at the end-effector is allowed to be rotate ortranslate as required.The joint variable attached to the coordinate system is (q ) Theposition vector P1h is free vector it can be rotated then rotatedvector can be written as

zw

AIF

I

la

P (q)= [R(z ,q)]P (6) Where R(z1,q) is 3x3 spacial rotationmatrix and z, is a unit vector along the z-axis., Consequentlythe position error become a single variable function of the (q)

then AP(q) ( - Ph (q)) (P - P (q)) (4)Substituting (6) into (7) and noting that [R(z1,q)] is anorthogonal matrixAP(q) =d*P d PP (q)) * ([R(z,,q)] [R(z,,q)]P ) (5)

Pd *( [R( I q)] fih

Where [.]t indicates transpose matrix, Since Pid- Pid and Pih Pihare positive constants to minimize AP(q) would be the same

as to maximize g, (q) = Pd *( [R(z ,q)]Ph (9) Similarly if

we rotate the last coordinate system about the z, axis with anangle (q) then the orientation vector becomes

hj(q) = [Rz,,q]hj forj = I to 3 (10) and theorientation error becomes

Ao(q)=Z (d .h (q) - 1) (6)

Both dj and hj'(q) are unit vectors describing direction angle asdefined here, dj. hj'(q) = Cos Pj(q) where P (q) is the directionvector between vector dj and h, as shown in fig. 3.Consequently the orientation error can be calculated as

Ao(q)= (Cosoi5(q) - 1) (7)

Zh

/

t

Since the optimal solution for q in two functions would bedifferent however as q is iteratively adjusted so that cos(Dj(q)converges to one for all j, reason for maximization problem isthat computation involved is less as compared to minimizationproblem Forward recursion formula and back ward cyclemethod bring the solution in the neighbourhood of the truesolution then it take time to a true solution at a desire degreeof precision however algorithm switched to the BFS method toget the convergence at a super linear or quadratic convergencerate. The criteria for switching between the two methods isdesignated as after each completed cycle of the forwardrecursion method, compare the current value of the objectivefunction (E,) with the one computed from the previous cycle(Ep). If the value Ep < D and Ec > Ep2 (where D is a smallpositive constant <1, to indicate the closeness of the solutionvector). If the solution vector is in neighbourhood of the truesolution and the convergence rate is less then quadratic switchto the BFS method.

B. Numerical AlgorithmStep 1:

Define the link parameters of the robot arm and theconstant used in the program. Define the boundaryconstrain qi' and qi", ((i = 1 to n).Specify the desiredlocation of the object through the vision system in to thealgorithm (Pd and [Rd]) and the termination criteria.

Step2:Compute the Pi, xyi, zi using the forward recursionformula. Also compute Pih = P,1 - Pi for i = n to I.Set hi= Xn+l, h2 = Yn+l, h3 = Zn+1 and Ph = Pn+..Compute thecurrent total error by using (1) (2) and (3). If Ec < 6 thenstop other wise go to switch method criterion: if Ep is lessthen D and Ep is greater then EB then go to step 4 (theinitial value of Ep can be specified by a positive realnumber)

Step 3:fori =nto doCompute qi'= qi + qi', and Pih = Pih(q'), hj' = hj(q')to 3) by using (6) and (9)If i > 1 then set P, and h= Pih +PEnd do: go to step 2

Step4:/

Xh

Fig.3.Direction angle definition

The physical meaning of (12) can be interpreted with thehelp of the fig.3 that the orientation error between the twocoordinate system would be minimized as Dj(q) approacheszero for all j, or when the three direction cosines Cos(Dj(q)simultaneously approaches one this can be achieved byminimizing, since cos(Dj(q) is bounded between 1 so

orientation error can also be effectively maximized as

g (q)=E cos(q) (q) =E dij h (q) (8)

1(j

Start the BFS method, the BFS algorithm [10]. Due toconstrained, imposed on the joint variables, all finite solutioncould not be obtained for a redundant arm. A line searchtechnique is used in BFS algorithm to match the configurationof detected object and end-effector. The line search techniquereduces searching time when object is in the vicinity of thetrue solution.

V. MOTION C ONTROL STRETEGY

In motion planning we categories our approach astransportation, course alignment and grasping

1) Transportation: Movement of the endeffector frominitial position to the vicinity of the target object , thismovement is mainly effected by the shoulder andalbow joints, visual feedforward control is proposed

h3

to reach the arm in visual field of CCD cameras asdescribed in my previous work [11] [12].

2) Course alignment: In course alignment Robot alignhand with the target object to achieve a desire poserelative to target object at high accuracy , this isachieved by combined visual feedforward and visualfeedback approach to reach the arm in desiredposition for grasping

3) Grasping: A model based grasping method is applied[13][14]

.VI.. VELOCITY CONTROL AND TRAJECTORY PLANNING

An optimization criterion that has been shown to match theexperimental data of robot arm movement control to achieve amaximal smoothness in hand path is used. Eq () is used forvelocity control

(9)

Where Xo and XT, are the start and target position T is thetotal sample time for the trajectory.

Trajectory planning is the specification of desired timedependent path of humanoid robot arm . The motion control isproposed through numerical algorithm, which has finite no ofsolution for reaching the target position. A simple linearinterpolation function is applied for trajectory generationwhen there is no desired path as shown in fig. For the desiredtrajectory planning spline function is used to generate multiplepoints through desired path. Spline function used is ageneralized form in which the user can generate more then 40points to achieve the desired path as shown in results.

VII. EXPERIMENTS

A Dynamic simulationDeveloped numerical algorithm has been tested on the

dynamic model of humanoid robot developed in the DADSsimulation software. Simulation of the desired position vectoris done off-line, The desired position and orientation of thetarget object is assumed in simulated model where as in actualpractical this location of the target object is detected throughthe vision system mounted on the head of humanoid robot. Anobject [ball] is placed on the desired location in front of thedeveloped model. The position vector is given as input to thedeveloped algorithm. The output of the algorithm is (6xl)joint variable vector for all six joints of the arm. This output isgiven to the dynamic model of the humanoid robot and otherparameters of the dynamic model are set to home in position.The movement in the arm is due to input of this calculated(6xl) joints variable vector. The result of the simulateddynamic model (when there is no desired path) is shown infig. 4.

Fig.4 Simulation process

The upper row shows side view of the simulated dynamicmodel and lower row shows the front view of the simulation.The movement of the hand is not considered in the dynamicmodel

B Desired trajectory generation

Desired trajectory is generated for position vector Px=[5mm,20 mm,70mm,100 mm,200 mm,150mm, 100 ]T Py =

[5 mm, 80 mm,100 mm,150mm,200mm,300mm,370mm]T andPz = [-375mm,-300mm,-250,-200mm, -150mm,-100mm,-20mm] T Total sampled time for the trajectory generation is 27seconds. Fig.5 given below shows the simulated behaviour ofthe dynamic model ofBHR-2

Right side view of simulated model

Front view of simulated model

Back side view of simulated model

Fig.5 Desired path simulation process

4 5xi (t) = x(, + (x,, x,).(l O', 15 + 6T T TI

For the desired path it is necessary to generate the desiredpoints along the path through spline function which reducethe jerk in the acceleration and at the same time high degreeof precision can be obtained as shown in table 1 given below .

TABLE 1

Behaviour of the spline function

a,

0

40

a,QLn)

10

8

6

4

2

0

-2

4

-6

-8

-10

- X-Direction

- Y-Direction

Z-Direction

sampled Time (s)

Velocity of the end-effector is a very important factor inMotion planning here in this experiment we considered a

constant velocity of 0.3 m/s so there is no jerk in the motionplanning. Behaviour of the velocity in three directions isshown in table 2 given below.

TABLE 2Velocity profile in three directions

When spline function is generated through the desired pointsthen this input is given to the developed numerical algorithmwhich calculates the movement of the arm joints. Table 3shows the behaviour of the joint of the arm from the table it isclear that the movement of the joints is smooth and there is nojerk in desired path of the humanoid robot arm

The humanoid robot arm is redundant so have finitenumber of solutions for a particular desired position of theend-effector. For example Pd= [100 mm, 400mm, 300 mm]Tand [Rd] is orthogonal matrix and initial guess for the jointvariable q, = 0 (for i = Ito 6) .The algorithm successfullyconverges (epsilon = 10-6) to a solution of q = [120.94, -14.83,2.045, 41.30, -12.04, -73.65] (degrees). Simultaneously usingthe FBS algorithm, we can approach to the optimum solution

TABLE 3Model joint angle behaviour

VIII. CONCLUSION AND DISCUSSION

Combined optimization approach to find the numericalsolution to the inverse kinematics problem of humanoid robotarm has been developed and tested on dynamic platform ofhumanoid robot BHR-2. Joint variable boundary constraintsare handled implicitly. Method developed is not sensitive toinitial guess and singular configuration. Gradient vector inproposed algorithm is solved analytically which reduces thecomputational load. Velocity of the arm joints is constantduring the simulation . In future this developed algorithm willbe tested on the actual system ofBHR-2 for motion planning.

REFERENCES

[1] Yong gang Ping, Wei Wei"A new tTajectory Planning Method Based on

Adeptive Simulated Anneling Genetic Algorithm"IEEE Conference2006pp262-265 "

[2] Luo Xiaping ,Wei Wei" A New Ammune Genetic Algorith and itdApplication in Redudant Manipulator Path Planning" JOURNAL OFROBOTIC SYSTEM 21(3) MAR 2004 pp 141-151

[3] Qu Yanchen, wang Changhong."A new robust visual servoing algorithmtheory and experiment" Proc. IEEE Confpp 1459-1462, 2003

[4] S.Hutchinson, G. Hager, P. Corke, "A Tutorial on Visual ServoControl", IEEE Transaction on Robotics and Automation, Vol. 2, No5pp. 651-570, Oct.1996

[5] Minh Chinh Nguyen,Volkergraefe,"Object Manipulation by LearningStereo-vision based robots" Proc. IEEE Conf: Intelligent Robot andsystems pp 147-151. 2001.

[6] J.Angeles "On the numerical solution for the inverse kinematicsproblem", Int J. Robotics res. vol.4, no>2, pp, 21-37, 1985.

[7] S.Elgazzar, "Efficient Kinematics transformationsfor the PUMA 560robot "IEE J.Robotics Automat,vol RA-1, no.3, ppl42-151.Sept 1985.

3-

2--Joint # 1

-Joint # 2

a2

_;| ~~~~~~~~~~~~~~~~~Joint

-2 -Joint # 6

-3

Sample Time (s)

0.6

0.4 ~~~~~~~~~~XDirection

0.2

o -~~~~~~~~~~~~Y-Direction

-0.6

-0.8-

Sample Time (s)

[8] Serdar Kuck, Zafer Bingal, "The inverse Kinematics Solution ofFundamental Robot Manipulator with Offset Wrist" Proc IEEE confMechatronics, pp 197-202, July 10-12, 2005.

[9] L.T Wang and D.Kohli "closed and expended form of manipulatordynamics using lagrangian approach" ASME.J Mechanics Transmission,Automat Design.vol 107, no. 2pp 223-225, June 1985.

[10] K.Kazerounian " On the numerical inverse kinematics of roboticsmanipulator." ASME.J Mechanics Transmission, Automat Design.vol109 pp 9-13, Mar 1987.

[11] S. Mahalingam and A.M Sharan, The non linear displacement analysis ofrobotic manipulator using the complex optimization method" Mechanisms Machine theory, vol. 22, no 3 pp 89-95, 1987.

[12] G.V Raklaitis et al " Engineering Optimization Methods and application" New Yark: Wiley 1983.

[13] D.I Pieper, The Kinematics of manipulators under computercontro, "Ph.D Thesis, Stanford university, Department of Computerscience, 1968