Motion Planning of Multi-Joint Robotic Arm with Topological Dimension Reduction Method

1. INTRODUCDON

So far there are three main branches of approaches to motion planning. 'Ibey are the geometrical approach, which is represented by the Subdivision Method [6]; the expen-system-based approach, which is very uncenain, or task-dependent; and the topological approach, which is presented in [1],[8]. 1be same point of them is to map the physical space to the Configuration Space (C-Space) [5], which is actually the state-parameter space of the robotic arm. For example, suppose the robotic arm has n degrees of freedom, then the C-Space of it will be 9 1X9:Y< · · · xs. . With the mapping, a state of the robotic arm in physical space corresponds to a point in the C-Space; a physical obstacle corresponds to a C-Space-Obstacle, which means the obstacle is expressed with the C-Space coordinates. So the Findpath Problem in ph~sical space is equivalent to the investigation of C­ Space. Usually, C-Space is a high-dimensional space. To study the connectivity of such a high-dimensional space, the topological Dimension Reduction Method presented in [1] is superior in many aspects.

2. DIMENSION REDUCDON METHOD

Because the mathematical proofs and deductions about DRM [2] are very time-consuming and abstract, here we only outline the main idea of DRM. What is emphasized in this paper is to demonstrate how a computer realization is derived from the method.

In C-Space, the set made of points corresponding to collision­ free states of a moving rigid object A among obstacles {8; 1 (i=l...M) is called Free-C~Space (FCS), therefore the set of C-Space-Obstacles is C-Space I FCS, where I is the set difference.

Considering FCS as a graph in C-Space, the motion planning of A is equivalent to the connectivity investigation of FCS. Usually, FCS

is a high-dimensional graph. For a 3-dimensional moving rigid object A among obstacles, if x represents the coordinates of a reference point P of A and 9 represents the three Euler angles indicating the orientations of the reference axis PQ of A, then FCS = G(F) = { (x, F(x)) I x e D(F)I, where F: X -t 2 8 (9 is the range of possible members of the Euler angles.) is called the Rotation Mapping and D(F) ~ X. G(F) is a graph in C-Space Xx9 and is called the Rotation Mapping Graph (RMG). For a moving rigid object A among polyhedron obstacles, the Rotation Mapping F is always a Semi­ Continuous multi-valued mapping [2].

Now we introduce DRM in the form of theorems:

'Ibeorem 2.1: Assume C-Space is XxY, RMG is {x, F(x)l xeD(F)I,where F: X-tY is a Semi-Continuous mapping and D(F) is the domain of F, if D(F) is comected , F(x) is compact, and YxeD(F), F(x) is a connected set on Y, then G(F)=(x ,F(x)l xeD(F)I is a connected set on XxY .

'This conclusion leads the coooectivity investigation of a high­ dimensional space to that of lower-dimensional spaces. With the requirement of Semi-Continuous the DRM can be extended to a general space X = X 1xX 2x · · · xX., where X; (i=l...n) is a sub-space of X. That means the connectivity investigation of X can be dissolved into those of X; (i=l...n).

In order to fulfill the conditions of theorem 2.1 and then apply the principle to the motion planning problem, the domain D(F) has to be divided into sub-domains (D;(F)I (i=l...r), where r is determined by the square of the obstacle number. Of course every D;(F) must be a connected set, and also for each D;(F),Yx e D;(F), F(x) has the same combination of connected branches, noted as .,

F(D;'FUFi(D;), which means for all x in D;(F), Fi(D;) is a jsl

connected subset of F (D; ). F (D;) is the set made of all possible orientation angles on D; . See Fig 2.1, if the moving object A is a R-long rod PQ among obstacles (8; I (i=l...3) and P is selected as the reference point, the possible connected branches of PQ at x are F 1 (x) indicated as (8 1 , 8 2), F 2(x) indicated as (82, 8 3), F3(x) indicated as (8 3, 8 1 ) and finally all combine to indicate F(x) (all

It can be proved that every (D;(F), Fi(D; )) indicates a connected block on XxY . This implies that every inner point of (D;(F ), Fi(D; )) is equivalent in the connectivity representations. So we don't bother to deal with the actual value of every point and the whole continuous FCS, but to deal with the common combinatorial representation of every connected block (D;(F), Fi(D;)) and the coonectivity among these limited nwober of connected blocks {D;(F), Fi(D;)I. The continuous infinitely-divisible space of real, FCS, is at last partitioned into finite number of equivalent connected blocks. With this kind of abstract evolution, the planning can be realized accurately and efficiently.

There is something to do before we can see its feasibility. Flrst we must answer how to divide D(F) into (D;(F)I to adapting to the requirements. By close research on the problem we get to the following partition rule:

Theorem 2.2: D(F) is divided into {D;(F)I with the critical curves which consists of three kinds of curves: R-Growing Boundaries [2], Disappearance Curves (DC) [1],[4] and original obstacle boundaries. The critical curves can be expressed by segmental parametric equations.

See Fig 2.2(a),(b), R-Growing Boundary is produced by expanding the original obstacle boundary by R distance. Disappearance Curves emerge where the convex obstacles are crowded together or some concave obstacles exist. (A concave obstacle can be divided into several convex obstacles.) Disappearance means moving into the De-region some connected branch will disappear. These are the most interesting curves. In a planar polygon environment, there are only two types of DCs. Assume PQ is a R­ long line-segment and P is the reference point, then one type of DCs are generally curves traced by P as PQ rests against one obstacle's comer and another obstacle's edge, which means a DC is generated by a point and a line-segment. See Fig 2.2 (a), within the region closed by the DC, the connected branch (B 1 ,B 2 ) disappears. We can see one type of DC is a coochoid curve or a part loop of a conchoid curve, whose equation is readily calculated. Another type of DC is very simple. See Fig 2.2 (b), it is an arc caused by a concave comer.

ance Curve

Fig 2.2 (b) R-Growing boundaries and another type of Disap­ pearance Curve

Second how to jndge the connectivity of any two connected blocks? Also by applying the principle of DRM, we give a rule:

Theorem 2.3: For any two connected blocks (D;(F), Fj(D; )) and (Dk(F), F1(Dk)), if the domains D;(F) and Dk(F) are adjacent and the intersection of the connected branches Fi(D;) and F1(Dk) is non-empty, then the blocks (D;(F), Fi(D; )) and (Dk(F), F1(Dk)) are coonected.

Flnally simplify each block (D;(F), Fi(D; )) as a Small-Node, (D;(F), F(D;)) as a Big-Node, which may contain more than one Small-Nodes for there may be more than one connected branches in F(D; ). If the blocks (D;(F), Fi(D; )) and (Dk(F), F1(Dk)) are connected then the two corresponding Small-Nodes are linked with an arc. Two Big-Nodes are linked if any of their Small-Nodes are linked. It is obvious that the Small-Nodes belonging to the same Big-Node can't be linked. So the Characteristic Networlc (CN) [ 1 ]-[ 4) { Nodes,Arcs I has been constructed, which is a 2-step networlc. The hierarchical constructions again bring about efficiency. Locating the initial and desired final configurations to two Small-Nodes in CN and searching a way between the two Small-Nodes in CN with some graph-searching algorithm of graph theory, we can get a concrete motion planning if and only if there exists a coonected way between the two Small-Nodes in CN because CN is homotopically equivalent toRMG.

Here is an outline of the solution of Fmdpath problem with DRM:

1. Partition of RMG

D=the domain of the free space on X;

PARTITION D into {D; I (i=l...r) by R-Growing Boundaries, Disappearance Curves and obstacle boundaries;

for all D; of {D; I do

RMG={D;, Fi(D;) I (i=l...r, j=l...n; );

2. Construction of CN

Big-Node[i]=(D;, F (D; ));

Small-Node[ij]=(D;, Fi (D; ));

for all Small-Node[ij] and Small-Node[k,l] do

if CONNECTED((D;, Fi(D;)),(Dk, F1(Dk))) UNK(Small-Node[i,j],Small-Node[k,l]); then SMALL-

BIG-UNK(Big-Node[i],Big-Node[k]);

1. SEARCH FOR PATH

S=LOCATE(initial state,CN);

G=LOCATE(final state,CN);

Way=SEARCH(CN,S,G);

Path=TRANSFORM(Way);

The main features :

1. It solves the problem in a point of global view, just like mao doing.
2. It plans, instead of continuously or numerically, topologically and then combinatorially. As a result, it gains efficiency.
3. It is an accurate, or complete, algorithm theoretically, which means it can find the path if and only if one exists.
4. Not only does it judge whether there exists a path but also gives a practical solution.
5. The worst-case complexity is exponential in the freedom degrees of the robotic arm, but seen from the angle of obstacle combinations, its time complexity is O(M 2 ), where M is the number of obstacles.

3. SIMULATION SYSTEM BASED ON DRM

We establish a simulation system for 3-joint manipulator Flodpath planning based on DRM. Asswoe the three joints are {A;_1A;, 1;, r;, 9; I (i=l,2,3), where the arm A;-tA; (i=2,3) rotates around the endpoint A;_1 in the same plane determined by A;_:zA;_ 1 and A;_1A; except that the arm AoA 1 rotates around itself; A 0 is fixed; I; is the length of A;_1A;; r; is the radius of A;_1A; and 9; is the orientation of A;_1A;. Assume the environment is made up of obstacles such as pillars, cylinders. spheres and walls, which are not limited in number, size and location. See Fig 3.1.

cs: 6,x62x63

RMG: {x, F(x),G(x,y)lxeD(F),yeF(x)l

DRM is applied recursively to C-Space in this way: 6r·-+62x6 3 and 62-+63, and CN is generalized to CN-Tree, whose every level corresponds to every degree of the robotic arm respectively. We must realize the partition to enable every (D;, Fj ,t(D; ), G, (D;.F'j,t(D; )) ) is a connected block in RMG. It is obvious that the partition must be completed in two steps: first partition D into I D; I so that for every x in D; F(x) bas the same combination of connected branches and Fj (x ) has the same partition form, then compute IFj (D; )I; second partition Fj(D; ) into IFj,.t(D;)} so that for every y in Fj,.t(D;) G(D;.F'j,t(D1)) bas the same combination of connected branches, then compute IG1(D;, Fj,t(D; ))I .

lbis is a framework of our simulation system:

• Input and organize obstacles with mathematical model based data structures and exclude the obstacles which are out of radius of /2+/3.
• Expand all obstacles by r (assume r 1=r2=r-rr), so the robotic arm is simplified as three line-segments jointing together. To maintain the independence of the algorithm, let us suppose that the expanded obstacle is a similar -shaped figure of the original one.
• Initialize 0-level Small-Node of CN-Tree;
• Partition on 81 .
• Project the environment vertically and obtain the topview. On the topview, compute the Supporting Lines [10] of every obstacle from the origin where the robotic arm is fixed, and arrange them in the angular order. Partition the whole possible angle range on 9 1, (91 . .,1. ,91,....) into angle intervals I (a; ,a; +I) I with the sorted angle values.

After such partition, we can deduced that if we make a cross section at any angle value of (a; ,a1+1) , then on all oi the cross sections there 'U be the same number and the same shapes of cut obstacles, whereas on each cross section the sizes of obstacle sectional views may very and the distances between them may be different. See Fig 3.2 · Therefore the above partition is not enough for A 1A 2 and A~ 3 to have the same connected branches on the cross sections made at every angle value in each interval (a; ,a.;+1) . Tbe cross sections at different angles of the same D; may be different.

_,. ' ' ,..,. ,-=-----~

• Use /2, / 2+/ 3 and /2-/3 radius spheres to intersect with the obstacles. Project their intersection vertically and transfer them to polar-coordinate system, then select the maximum and minimum polar values and insert them to the above partition. See Fig 3.3.

lbis is a more detail partition. If the Disappearance Curves don't exist at all then the partition will be enough for A ~ 2 and A~ 3 to have the same connected branches in each partitioned angle interval on 9 1 · However, there are often Disappearance Curves when the obstacles are crowded together.

• With the liner-cutting-detection technique, which means making cross sections at regular intervals within the partitioned interval, (a; ,a1+1), detecting whether some DC emerges or vanishes, we can find the angle values on 81 where the Disappearance Curves corresponding to A 1A2 or A~3 start or end. On 9~o the interval, (a; ,a1+1), is so small after the former partition that the liner -cutting is very limited. Insert these angle values itto the above partition. Now the partition on 9 1 is at last completed.
• Split the corresponding 0-level Small-Node to 1-level Big­ Nodes on CN-Tree according to the complete partition. It is obvious that Big-Node levels maintain the adjacency information.
• Because we are sure that every angle value belonging to one partitioned angle interval on 9 1 , A 1A 2 and A~ 3 will have the same connected branches, we make a cross section at an arbitrary value of every angle interval.
• Compute the connected branches of A ~ 2 and expand the corresponding 1-level Big-Node to 1-level Small-Nodes on CN-Tree according to the computation.
• Partition each connected branch of A 1A 2 into sub-branches with the / 3-Growing Boundaries, ·Disappearance Curves corresponding to A~3 and r-expanded obstacle boundaries so that for every angle value belonging to each sub-branch of A 1A2, A~3 has the same connected branches. See Fig 4.2.
• Split the corresponding 1-level Small-Node to 2-level Big­ Nodes on CN-Tree according to the partition on the cross section.
• Compute the connected branches of A~ 3 and expand the corresponding 2-level Big-Node to 2-level Small-Nodes on CN-Tree.

By now the CN-Tree is accomplished, which is also a 2-step tree. On the CN-Tree, each branch from the root node to the leaf node indicates a connected block in the C-Space of the robotic arm, or corresponds to a complete

connected branch.

• Link the nodes of the CN-Tree with arcs by applying theorem 2.2 to the CN-Tree recursively, so all connectivity information of the problem is stored in the CN-Tree.
• For a branch from the root to leaf on the CN-Tree is homologous with a leaf node on the CN-Tree, we deliver all connectivity information down to the leaf level, so we gain s simple graph, called Leaf-Graph.
• Locate the initial and desired final state of the robotic arm to the corresponding leaf nodes on CN-Tree and search in the Leaf-Graph.

4. EXAMPLE

With one solution of our simulation system, we conclude this paper. Tbe program is composed in PASCAL and performed on SUN 3/60 workstation. Tbe CPU time of the following example is 1:17 seconds and we can still improve it by doing efforts.

See Fig 4.5, the robotic arm has three joints and the environment consists of a wall corner, a cylinder and two cuboids. Three special states are given: S 1 , S 2 ' S 3 , and they are all on the cross section shown in Fig 4.2. Because of the special behavior of A oA 1 and the difference between the real robotic arm and the simplified three jointing line-segments, a state in Fig 4.3 can really corresponds to two real configurations of the robotic arm, e.g. S0 and So'· If So is (9[ 0 >, 9i 0 >, ej 0 >) then S0 will be (9j 0 >+1,

2n-ef>, 2n~f>). So we have another three corresponding special states: S 1'· S2' and S3'.

In this case, the partition oo 9 1 is relatively simple, see Fig 4.1. The CN-Tree is illustrated ·in Fig 4.4, where the lines of dashes on the two Big-Node level represem the adjacency relations of D; and Fi.t(D;) respectively, the real lines on the Leaf-Node level represent the direct coonectivity relations obtained from every cross section and the lines of dashes on the Leaf-Node level represent the indirect cormectivity relations obtained by delivering connectivity information down. Finally all connectivity information is stored on the Leaf-Node level. The CN-Tree is symmetrical. The Leaf-Nodes where the states S 1, S 2 and S 3 are located are also symmetrical with those where S 1 ', S 2' and S 3' are located, see Fig 4.4. This reflects the real world perfectly.

Searching in the Leaf-Graph, we can see that every two configurations of IS to S 2· S J· S 1 ', S 2', S 3 'I are connected. If we abbreviate a Leaf-Node (D;, Fj,t(D;), G1(D;,Fi.t(D;))) to a four group (ij,k,l), then we can show a solution of S 1 -+ S 2 -+ S 3 by giving a series of the Leaf-Nodes: (3,2,1,1) -+ (3,2,2,1) -+ (3,2,3,1) -+ (4,1,1,1) -+ (5,2,1,1) -+ (4,2,2,1) -+ (3,3,2,2). Fig 4.5 is the hardcopies of the result displayed on the video-terminal of SUN 3/60 by selecting a arbitrary configuration in each of the nodes.

We adopt the Shortest-Way Algorithm of Graph Theory in our searching of the Leaf-Graph, then the solution is optimal in the sense of least sudden changes of states because a configuration can be turned into all configurations of the same Leaf-Node smoothly and continuously. This character is beneficial to the motion of a real robotic arm.

S. REFERENCE

1. R.T. Chien, Ling Zhang, Bo Zhang, "Planning Collision-Free Paths For Robotic Arm Among Obstacles", IEEE Trans.PAMI .. Vol.PA.MI-6, No.1, Jan.,l984.
2. Bo Zhang, Ling Zhang, "Planning Collision-Free Paths For 3Dimensional Object With Rotation", Report on ACADEMIA SINICA (China) and C.N.R.(France) Robotics Workshop, Oct.,l985.
3. Bo Zhang, Jianwei Zhang, Ling Zhang, "An Algorithm For Fmdpath With Rotation", Proc. of IEEE 1988 International Conference on SMC. Beijing, 795-798.
4. Bo Zhang, Jianwei Zhang, Ling Zhang, Tian Zhang, "A Findpath Algorithm For Manipulator By Fmite Division Of Configuration Space", Robotics and Manufacturing. Recent Trends in Research, Education and Application, M.Jamshidi(eds), ASME Press, New York, 1988, 99-106.
5. T.Lozano-Pc!rez, "Spatial Planning: A Configuration Space Approach", IEEE Trans. on Comput., Vol.c-32, No.2, Feb., l983.
6. R.A.Brooks, T.Lozano-Pc!rez, "A Subdivision Algorithm In Configuration Space for Fmdpath With Rotation", Proc. of 8th UCAJ-83, pp.789-806.
7. T.Lozano-Pc!rez, "A Simple Motion Planning Algorithm For General Robot Manipulators", IEEE Journal of Robotics and Automation, Vol.RA-3, No.3, June, 1987.
8. J.T.Schwartz, M.Sharir, J.Hopcroft, "Planning Geometry And Complexity of · Robot Motion", ABLEX PUBLISHING CORPORATION, Norwood,New Jersey, 1987.
9. S.H.Whitesides, "Computational Geometry And Motion Planning", G.T.Toussaint (Editor), Elserier Science Publishers B.V.(North-Holland), 1985.
10. F.P.Preparata, M.L.Shamos, S-Verley, "Computational Geometry-An Introduction", Texts and Mono-Graphs in Computer Science, 1985.