Collision-free trajectory planning for robotic manipulators is investigated. The task of the manipulator is to move its end-effector from one point to another point in an environment with polyhedral obstacles. An on-line algorithm is developed based on finding the required joint angles of the manipulator, according to goals with different priorities. The highest priority is to avoid collisions, the second priority is to plan the shortest path for the end effector, and the lowest priority is to minimize the joint velocity for smooth motion. The pseudo-inverse of the Jacobian matrix is applied for inverse kinematics. When a possible collision is detected, a constrained inverse kinematic problem is solved such that the collision is avoided. This algorithm can also be applied to a time-variant environment

Han, J. Y.

Pourboghrat, F.

English

NASA Technical Reports Server

Collision-Free Trajectory Planning Algorithm for Manipulators 
F. Pourboghrat and J.Y. Han 
Southern Illinois University 
Carbondale, IL 62901 
1. A b & r a c t  
Colllslon-free trajectory plannlng for robotlc manlpulators 1s lnvestl#ated,l- 
The task of the manipulator 1s to cove Its end-effector from one polnt to another polnt In an 
envlronment wlth polyhedral obstacles. An on-llne algorlthm 1s developed based on flndlng the 
requlred jolnt angles of the manlpulaCor,accordlng to goals wlth dlffrrent prlorltles. 
highest prlorlty 1s to avold colllslons, the second prlorlty 1s to plan the shortest path for 
the end effector, and the lowest prlorlty 1s to mlnlmlze the jolnt veloclty for smooth motion. 
The pseudo-inverse of the Jacoblan matrlx 1s a3plled for Inverse klnematlcs. When a posslble 
colllslon 1s detected, a constralned lnverse klnematlc problem 1s solved such that the colllslon 
1s avolded. Thls algorlthm can also be applled to a time-variant envlronment. 
2. Introductlon 
The 
Ordlnary tasks for a robotic manlpulator are to move Its end effector from an admlsslble 
polnt to another admlsslble polnt In an envlronment wlth obstacles. For that, the lnltlal and 
final conflguratlon of the manipulator are often glven for the trajectory plannlng. Usually. 
there are lnflnlte paths for the end effector. Even for a speclflc path of the end effector, 
there are stlll lnflnlte trajectorles posslble for the manlpulators. However, some of the 
trajectorles are not feasible becasue of arm geometry, obstacles, and some klnematlc or dynamic 
constralnts. Even wlth the klnematlcally feaslble trajectorles, some computational or loglc 
problems In the algorlthms may make them lmpractlcal. 
3. Trajectory Plannlna 
angular Informatlon from the spatlal Informatlon, uslng the tnverse klnematlc relatlonshlp. 
Conslder a robotlc manlpulator wlth n degrees of freedom. Let the klnematlc relatlonshlp 
between Jolnt angles and the end-effector posltlon and orlentatlon be glven by 
I n  order to move from one polnt to another, In the task space, one needs to solve for the 
x - f(q) (1 1 
where X 13 the m-dlmentlonal vector of the end-effector posltlon and orlentatlon, and q la the 
n-dimensional vector of joint angles. For a klnematlcally redundant manipulator, the dlmenslon 
of q 1s greater than the dlmenslon of X, (n>m). Dlfferentlatlng the above relatlon, we get 
where J(q) - df/dq Is the mxn Jacoblan matrlx, [ 7 1 .  For a redundant manipulator, the Jacoblan 
matrlx ulll have more columns than rows. Moreover, the Inverse of such non-square matrlx I s  not 
deflned In a regular sense. However, useful soiutlons of equatlon ( 2 )  can be found, by uslng 
the generallzed-lnverses of the Jacoblan matrlx J. and 1s glven by 
;1 - J'; (I-J'JIZ ( 3 )  
where Jt - JT(JJT)-' Is the pseudo-Inverse of J ,  I 1s the nxn ldentlty matrlx, and 2 1s an 
arDltrary n-dlmenslonal vector. iihcn the 'vectcr 1. Is selected t o  be zero, equation ( 3 )  reduce3 
to 
* t' 9 - J X  
which glves the best approx:mate solutlon to equatlon ( 2 ) .  This 1s i n  the sense that if q, Is 
the 3olutlon of ( 2 ) ,  glven by ( U ) ,  then I (q,l I < I I q l  1 ,  where q 1s any other solution Of (2 )  
that Is given by ( 3 ) .  [5-61. It should be noted that. such mlnlmum norm, or best approximate 
solution.1s defined when there 1s no restrlctlon in the task space. This Deans that, In a 
restrlcted envlronment. the above mentloned best approximate. solutlon may not be feaslble for 
appllcatlon and may result In colllslon. 
't -ii 125 
https://ntrs.nasa.gov/search.jsp?R=19890017182 2020-03-20T02:07:26+00:00Z
Let ua deflne COlll8lOn p o l n t  to  be the polnt on the manipulator body uhloh has the 
potentlal t o  col l lde v l th  t h e  obstacle. 
t o  develop an algorlthm, for the on-llne detemlnatlon of the requlred jolnt  angle ratee ,  q, for 
rare manlpulator laotlon. 
poaalble collisions. 
generAted, uslng t h e  beat rpproxlmate solutlon, to  move the end-effwtor on a shortest dlatance. 
But  vhen a potentlal col l ls lon polnt la detoated. t he  trafectory la  m d l f l d  In order t o  avold 
colllslon. 
The colllslon-free trajectory plannlng problem here Is 
The approach 1s to  continuously monitor the task apace, for detrctlng 
If  no potentlal colllslon 1s detected, the reqdlred jolnt angle rat08 are 
4. O b S t A C l e  bvoldance 
In order to  avoid obstacles, one needs to  urn the klnenatlc relatlonshlp for the oolllalon 
polnts, slmllar t o  that of equatlons ( 1 )  and (2 ) .  
task space, be denoted by Xc. 
Let t h e  potentla1 colllslon polnt, ln the 
Then, slmllar to  rquatlon (21, we can wrlte 
where J 1s the mxn Jacoblan matrlx f o r  the colllslon polfit. The inverse klnemAtlC Solutlon to  
the abose is slmllar to  ( 3 )  and 1s glven by 
where J~ is the pseudo-inverse of J ~ ,  and 2' 1s an arbltrAry n-dimensional vector. 
hlghest pr lor l ty  is t o  avold the obstacle, and,lf needed modlfy the posltlon of end-effector. 
In order for the trajectory plannlng to have mlnlmun norm, we choose 2 - 0 as  I n  (4). On 
the other hand YO choose 2'00, l lke  I n  ( 6 ) .  to  account for both colllslon avoldance and 
traJectory p lann lng .  From ( 3 )  and ( 6 ) .  a min lmum norm soiutlon for 2 '  1s 
Now. the problem of obstacle avoldap.ce is that, when a potential colllslon Is detected. the 
P lugg lng  t h l s  back lnto (61,  we get 
Then, u s l n g  the following identlty, C61 
we get 
The above relat ion,  generates the Jolnt angle rates q such that the obstacle is avolded and the 
end effector  v e l o c l t j  Is modlflrd. for t h e  on-llne t ra jectory p lann lng .  
Now the questlon 1s how and I n  what dlrectlon the end effector spa t la l  veloclty should be 
changed. For the aIgortthm t o  be  fas t  and fmplemntable, a f l n i t e  search for the minimum norm 
solutlon Is consldered. 
modlflcatlon Is also preselected by a value of c .  The value of q may now be fodnd by eramlnlng 
seven dlfferent dlrectlons for ra te  modlflcatlon. e.g.. c-varlatlon In p l u s  or minus X.Y.2 
coordlnates and a l so  no modlflcatlon. 
modlflcatlon, from seven different posulbllltles. 
The value of Xcls preselected by the user, and the value of X 
The smallest norm 11q11 1s then chosen for  trajectory 
The overall algorithm Is such that when no potential colllslon 1s detected, a mlnimum norm 
solutlon q l a  planned accordlng t o  (10. But .  when a potentlal colllslon Is detected, the 
obstacle 1s avoided and the path In mdlfled accordlng t o  ( 7 ) .  
( 1 )  the solutlon ex ls t s  
( 2 )  the obstacles are  represented by polyhedrals. 
( 3 )  the geometrlcal lnformatlon about the task area Is known, e.g., u s l n g  sensory systems. the 
posltlons of obstacles and manlpulator l l n k s  are known. 
( L o  the potentlal C O l l l 5 l O n  polnt on the manlpulator l l n k s  can be detected. 
( 5 )  only a slngle col l ls lon may occur, and u l l l  be  detected, a t  any glven tlme. 
In order to develop the algorlthm, i t  1s assumed that :  
5. The Almrlthm 
colllslon-free trajectory planning of M e  robotlc manlpulatore. The steps of the algorithm are: 
The following sumrarlzes the steps, Involved in the proposed algorithm, for the on-line 
h t e m l n e  a minimum-length path for the end-effector, f r o m  the arrent posltlon to target 
posit Ion. 
Check If there Is a potential colllslon polnt. 
continue. 
N o  potential folllslon Is deteoted. Make an Incremental move accordlng to the jolnt aryle 
rates vector q, gfven by equation (0) .  
If the end-effector has not reached the target, 80 to step (2).  
target. go to step ( 7 ) .  
Potentlal col!lslon Is detected. Make an Incrementa! move accordlng to the joint angle 
rates vector q, given by equation (71, such that 11q11 IS minimized in a finite search. 
If there is, go to step (51, otherwise 
I f  It has reached the 
Go to step (1). 
stop. 
Concluslon 
On-line, collision-free trajectory p annln s d  cu 5.9 1. An lgorlthm. uhlch utlllzes 
sensed lnformatlon about the conflguratlon of the manlpulator and obstacles, Is developed based 
on the task prlorltles. The order of the task prlorltles are: to w o l d  colllslon, to plan the 
shortest path for the end effector. and to choose the mlnlmum norm solution. 
fast and could be Implemented on robotic manlpulators for On-line trajectory plmnlng. 
The algorlthm Is 
c 1 3  
c21 
c31 
E 4 1  
C5 3 
C63 
17 1 
REFERENCES 
Lozano-Perez, T. and Wesley, M.A., "Algorithm for Plannlng Colllslon-Free Paths Among 
Polyhedral Obstacles," C m m .  of ACM, Vol. 22, No. 10. Oct. 1979. 
Couzenes, L. ,  "Strategles for Solvlng Colllslon-Free Trajecforles Problems for Hoblle and 
Manipulator Robots," I n t .  J. Robotlcs Research, Vol. 3, No. 4 ,  1984. 
Khatlb, 0.. "Real-Time Obstacle Avoidance for Manlpulators and Mobile Robots," Int. J. 
Robotlcs Research, Vol. 5, No. 1 ,  1986. 
Gilbert, E . C .  and Johnson, D.W., "Distance Functlons and thelr Application to Robot Path 
Plannlng I n  the Presence of Obstacles." IEEE J. Robotlcs and Automatlon, Vol. RA-1, NO. 1, 
Marck 1985. 
K l e l n .  C.A. and Huang. C.H.. "Revlaw of Pseudolnverse Control for Use 4 t h  Klnematlcally 
Redundant Manlpulators," IEEE Trans. Syst. Man Cyb., Vol. SMC-13, No. 3 ,  llarch/Aprll 1983. 
MajcleJeuskI, A . A .  and Klein. C.A., "Obstacle Avoldance for Klnematlcally Redundant 
Hanlpulators in Dynamlcally Varylng Envlromnts," Lnt. J .  Robotlcs ReMarch, Vol. 4 ,  No. 
3 ,  1985. 
Lee, C.S.C., et. al., Tutorial on Robotlcs, IEEE Computer Society, 1983. 
127 


Collision-free trajectory planning algorthm for manipulators

Abstract

Similar works

Full text

Available Versions

NASA Technical Reports Server