Path Planning and Collision Avoidance for Redundant Manipulators in 3D Alistair Mclean & Stephen Cameron Department of Engineering Science and Computing Laboratory, University of Oxford, Oxford OX1 3PJ, UK. falistair,
[email protected] Abstract. We have developed a planner for redundant manipulators that is fast, e ective and versatile, based on the idea of treating each link of a rigid-linked robot as if it were slightly exible and under the in uenceof a potential eld. We justify the method by exploringsome simple examples, and brie y describe the extensions to the basic method that allow it to plan paths for manipulators with revolute joints (linear and spherical joints having been covered in an earlier paper). We also outline the use of the method for manipulators with exible links and with a large number of degrees of freedom. The planner can either be used within an advanced tele-operated system, or as a path re nement stage of a practical planner that we propose, which includes a rule-based global planning stage. Key Words. Path re nement; motion planning; teleoperation; redundant manipulators
1. Introduction We are interested in the path planning problem for redundant manipulators, and in particular, those manipulators which are designed to operate in highly constrained environments such as within a nuclear reactor or in manufacturing workcells. Most manipulators of this type tend to be long and relatively thin and this method is particularly suited to these arms. It can also, however, be applied to general shaped manipulators (see gure 3). An example of the former is the space shuttle arm and other high degree of freedom arms which are planned for use in space. Potential eld approaches use a scalar function to represent the topology of the local free space and so these approaches, although fast, are easily trapped by local minima. It would seem sensible, therefore, to try to employ both potential eld and graph-based approaches in a complementary manner. We thus promote such a division, with a global planner providing coarse plans to a path re nement module, which uses a local planner to re ne (or refute) the path, and to pass only high-quality paths on to the control level. A similar structure has been proposed elsewhere (Faverjon and Tournassoud, 1987; Hague, 1993), and one advantage of this division of responsibilities is that the global planner could either be a computer (probably using a graph-based approach), or a human using the system in an advanced teleoperation mode. The work described here is aimed at the path
re nement level for redundant manipulators. In our case the re nement is of a nominal path for the payload which temporarily ignores the arm planning problem. We have previously detailed the basis of the method employed (McLean and Cameron, 1993; McLean and Cameron, 1995), but for completeness we brie y provide an overview of those results in x2. The main contribution of this paper is to detail the extensions that enable the method to tackle a further range of interesting manipulators, namely exible robots, hyperredundant robots, and those (the majority) with revolute joints (rather than the spherical joints we previously considered).
2. The Virtual Spring Method A major problem with using potential eld methods with large dof manipulators is the existence of a large number of potential minima. Many of these are caused by interaction between the links and as we described in (McLean and Cameron, 1993), their number and depth can be signi cantly reduced by treating the rigid-linked manipulator as if it were composed of a number of sti linear springs (which e ectively reduces the magnitude of the reaction forces at the joints). Although this approach (which we have re-christened the virtual springs method1) must introduce some errors (as the links will be expected to have varying length), we showed that the errors are tiny (e.g., 0.5%) and 1 Originally called the snake-based method, which caused confusion with the concept of a hyper-redundant manipulator.
control system level.
empirically, that allowing oscillations to occur in the links can aid the planner. In examples with many links there will be many modes of vibration and, from the experiments we have undertaken, we believe this results in a form of pseudo-random motion similar to that used in (Barraquand and Latombe, 1990).
We use a Lagrangian approach to solve the system. We obtain the analytical equations of the kinetic and potential energies of the arm and, using the principle of least action, solve these to obtain the equations of motion of the arm. Of the potential terms, the arm has its own internal energy which is the potential energy of the virtual springs and other terms which alter the behaviour of the arm (see below) and, since we use a potential eld representation of the workspace, the environment provides external potential energy which is used to generate the external constraint forces which drive the arm away from the obstacles and towards the valleys of potential minima. The Cartesian coordinates pi of the joints are used as the generalized coordinates of the Lagrangian equations. This allows us to de ne the equations of the virtual springs in a simple form and to use the potential eld calculated in the workspace (rather than in the con guration space). By presenting the problem in a dynamical framework (energy minimization then occurs when the dynamical system is in equilibrium), suitable dynamic models of the manipulator can be constructed which give the model intuitively appealing physical behaviours.
A more realistic example is shown in gure 2(a), where we show a 5 dof arm in two dimensions approaching a local minima as the penultimate joint approaches the top-most barrier, and to escape from this the rest of the arm must drop towards the bottom-most barrier. Figure 2(a) plots the potential energy in the system for both the rigid (top-line) and virtual spring (bottom-line) case for the entire motion, plotted against the progress (angle of the 3rd link) of the system. The position of the local minima is shown by a vertical dash, and the standard rigid-body implementation halts here as the termination condition is met. In fact if the termination condition is disabled the system does make its way to the goal, but takes about 75 times as long as in the virtual-spring case. It is also possible to see examples of `ringing' in the system in the virtual springs case; such oscillations also occur in the rigid system model, but without the same e ect as the changes in energy are generally much smaller. A secondary benign artifact that we have noticed in other examples is the way that our numerical scheme essentially changes the goal attraction in a discontinuous manner|this causes some `jerking' of the system that encourages escape from sticky positions.
3. Paths of Lower Energy This section describes how our mathematical model of the arm allows it to nd paths of lower energy. By making the links elastic and sti and adding the potential energy of their extension to the Lagrangian (equation 1), we derive a system that is less prone to local minima than if we were to use rigid links. It is probably surprising that
exible links can aid the path planning process. To illustrate why, consider the simple example of a one-link manipulator in gure 1(a), which is being pulled to the right with constant force. The pulling force is being opposed by the repulsion of the topmost obstacle, giving rise to a classical local minima in the potential eld. Figure 1(b) graphs the sum of the potential energy of the system for the exible case (lower trace), where we get about 1% error in the link lengths, and for a very sti case (upper trace), where we get about 0.005% error. (No scale is shown on the graphs, as the potential energy terms have an arbitrary constant associated with them. However the range of energy shown is a signi cant fraction of the total change in energy over a longer motion.) The low gradient in the sti case means that little progress is made per iteration, and eventually the planner gives up and signals failure, whereas the exible case succeeds. The oscillations which occur in the graph for the exible case occur due to the initial
4. Derivation of Equations of Motion We employ Rayleigh dissipation functional into the model in order to dissipate the kinetic energy of the arm. The energy terms are then substituted into Lagrange's equation:
d @L ? @@pL = ? @@Fp_ (i = 1; 2; . . . ; n) (1) dt @ p_ i i i where F is the dissipative function and n is the number of degrees of freedom. Thus there are n
second order di erential equations to be solved. The matrix di erential equations have the form P = Psg ? AP ? CP ? BP_ ? DP_
where P = (p 1 p 2 . . . p N ) ; Psg and A describe the external elastic forces which guide the arm; B represents the damping forces; D is the internal damping forces; and C represents the internal spring forces. Only the A matrix changes as the con guration changes and so the system is easily solved by an implicit Euler scheme (McLean and Cameron, 1993). 2
Energy
Constant Force
Base
Angular Displacement
(a)
(b)
Figure 1 The system falls into a local minima when the links are very sti and succeeds when they are given some degree of exibility.
We can parameterise each span by r for 0 r 1, so that a general point si(r) on span i is given by
Goal
si(r) = rMGi P where r = (1; r; r2) GiP = (p i p i+1; p i+2)
(2)
and M is the shape matrix (Curwen et al., 1991) for quadratic Bezier curves, namely: 2
M=4
1 0 0 ?2 2 0 1 ?2 1
3 5
We de ne an energy term that is added to the Lagrangian and which grows as the curvature of the span increases. This energy term is simply the square of the second derivative of the spline multiplied by a constant. Ucurvi = 21 i (srri (r))2 srri (r) = pi ? 2pi+1 + pi+2 = (pi+2 ? pi+1) ? (pi+1 ? pi ) So (srri (r))2 = pi?1p i+1 ? 2pi p i+1 + pi+1p i+1 ?2pi?1p i + 4pip i ? 2pi+1 p i + pi?1p i?1 ? 2pip i?1 + pi+1p i?1
Figure 3 Here, the planner is in teleoperation
mode and two nominal paths have been entered by the user in order to solve this problem. The two paths ensure that the end-e ector passes through the central gap ahead of the base link.
5. Adding Curvature Constraints Simpli ed bending of bars can also be easily incorporated into this system. It is simpli ed since the type of bending under consideration is the equivalent to applying two equal and opposite moments to the bar, producing a curve of constant curvature. To parameterise the curvature we use an extra control point per link, positioned mid-way between the two points that de ne the ends of the link. A quadratic Bezier curve can then be tted to each triple of control points.
Taking the partial derivatives gives : 1 @ (srri (r))2 2 @8 pj = 3
> > < > > :
pi?1 ? 2pi + pi+1 j = i?1 ?2pi?1 + 4pi ? 2pi+1 j = i pi?1 ? 2pi + pi+1 j = i+1 0
otherwise
Energy
Constant Force Base
Angular Displacement
(a)
(b)
Figure 2 The system follows a path of higher energy when the links are very sti and of lower energy
when they are given some degree of exibility. Note that the sti example is only at the mark shown when the exible example has nished.
gives the new di erence equation
Each real span has a lateral sti ness and (since it is only the real spans which have this sti ness) the lateral sti ness matrix has zeros at the points corresponding to the joints of the arm. The total energy is: so
where is the time step.
N ?1 X Ucurv = 21 i(srri (r))2 i=1
N ?1 1 @ (s (r))2 X @ Ucurv rri i = @ pj 2 @ pj i=1 8 < j +1 (pj ? 2pj +1 + pj +2) j = 1; 3; 5; . .. ; N ? 3 =: ?2 j (pj ?1 ? 2pj + pj +1) j = 2; 4; 6; . .. ; N ? 2 j ?1(pj ?2 ? 2pj ?1 + pj ) j = 3; 5; 7; . .. ; N ? 1
This gives a new term in the Lagrangian
I + (B + D + F) + 2 (A + C + E) Pn2 +1 = Pn2 + Pnsg+1 ? (A + C + E) Pn1 (3)
@L = ?? ?P @ P curv
where ? is a tri-diagonal, second order di erence matrix and is a diagonal matrix of lateral sti nesses with zeros at the points corresponding to the joints of the arm. Both matrices are constant for a given manipulator. The curvature constraint produces a damping term, and we therefore add the two terms ?EP ? FP_ to the di erential equation, where
6. Results Although the terms that have to be added may look complex, the resulting equations are only a little more complicated than in the linear springs case. Computation times are increased and, for example, a exible manipulator planning the same path as that shown in gure 6 takes 150 seconds. This extension, however, allows us to consider the motion of truly exible manipulators (albeit using a quite crude model).
7. Tentacles There has been much work recently on researching tentacle-like robots (or snake-robots as they are conventionally called). These hyper-redundant manipulators can be easily modelled by a virtual spring system. Instead of modelling the links as linear spans, a B-spline is tted to the entire set of control points. A B-spline is chosen as opposed to a Bezier curve since to interpolate over n control points would require an order n Bezier curve, which is costly to compute.
E = H?m1 ? ? F = H?m1 ? ?
and is a diagonal matrix of lateral damping coecients with zeros at the points corresponding to the joints of the arm. Propagating these changes
The energy term de ned for each span is the same 4
(a)
(b)
(c)
(d)
Figure 4 Path planning for this 10 link exible manipulator takes 25s on a Silicon Graphics Iris Indigo R4000. The manipulator starts from a concertina-like initial con guration at the bottom of the workspace. as that presented in the last section, i.e.
fer to modify the current energy term for the length constraint, which results in a simpler and more ecient Lagrangian function. In the former method, the force which acts to extend or contract a link, so that it maintains its proper length, is directed along the link (see gure 7). Consider what would happen, however, if the point that pi+1 should be at (i.e. p^ i+1 ) is moved so that it does not necessarily lie along the link. If this is the case then the partial derivatives of the energy term along each of the coordinate axes can no longer be multiplied by the same value, i (McLean and Cameron, 1993). Therefore i must be rede ned. Ultimately this leads to three di erent matrix equations, one for each of the Cartesian coordinates, and hence three matrix inversions. On the other hand, by placing p^ i+1 in the correct location, revolute joints can be incorporated
@L = ?? ?P @ P curv
where ? is as before and is a diagonal matrix of lateral sti nesses, as before, but with all elements non-zero. All the other terms are as in the basic mechanism. Figure 5 shows an example.
8. Revolute Joints In order to model systems with revolute joints we have to introduce additional constraints into our mathematicalmodel. One method is to add an energy term for each revolute joint to the Lagrangian function that is proportional to the square of the out-of-the-joint-plane angle, . Instead we pre5
(a)
(b)
(c)
(d)
Figure 6 These gures show the path found for a manipulator which has 6 revolute joints and 3 ball
joints (which are joints 5,6 and 8). The joint axes and the nominal path for the end-e ector are shown in (a) and gures (b) and (c) show the actual path which the manipulator follows. The nominal path is extracted from the graph shown in (d). Planning takes about 75 seconds on an SGI Indigo R4000.
(Lxi ? L^xi )2 = L2xi ? 2Lxi L^ xi + L^ 2xi = f (L2xi ) p where f (l) = l ? 2L^ xi l + L^ 2xi
into the system. The method is as follows. First of all, each joint has a joint axis which is de ned by using the Denavit-Hartenberg notation (Craig, 1986) and associated method as described below. Using the current values of pi and pi+1, pi+1 is then calculated which is the projection of link i onto its joint plane. p^ i+1 is then computed which is the true position of pi+1 for link i in its joint plane (see gure 8). The energy term, for each coordinate axis, dir = x; y; z , is
@L2 @f = xi xi @ pj @ pj 0 where xi = f (L2xi ) = 1 ? L^ xi =Lxi
0
Thus summing the energy over the whole snake and taking partial derivatives gives
S 1X ^ 2 Uintdir = 2 i=1 ki(Ldir i ? Ldiri )
@L @P
intdir
= ? Ldir P
(4)
where dir = x; y; z , Ldir is a diagonal matrix with elements ki diri , representing the sti ness of each link multiplied by a coecient that varies as the length of the link varies, and is the rst order di erence matrix.
where Ldir i and L^ dir i are the current length (jpdiri+1 ? pdiri j) and calculated length (jp^ diri ? pdiri j) of link i in the respective coordinate axis, and ki is its sti ness. So for dir = x L2xi = (xi+1 ? xi)2
As mentioned above, each joint must maintain its 6
^
^
j
k F
p^ i+1 ’
p i+1 ^
p^i+1= pi + ti . original_lengthi
p
i ^
ti
^
i
Figure 8 With the new length constraint, the
force which acts so that the link maintains its original length is no longer along the link. It can now be in any direction as it is also required to constrain the link to its joint plane.
Figure 5 An example of the path planned by a
tentacle robot. This takes 60 seconds on an SGI Indigo R4000.
joint axes for each link can be found by the simple equation :
^
p i+1 L
original
Z^ i+1 = Z^ i rotate (X^ i ; i)
where vec1 rotate( vec2 , angle) rotates vec1 about vec2 by an angle angle.
F
pi+1
Figure 6 shows an example of a path found for a 3 dimensional workspace. We use a global planner (McLean, 1994) that extracts sections of the skeleton of the work-space (a structure akin to the Voronoi diagram), and uses these sections as nominal paths for the tool point of the end-e ector to follow. The choice of nominal path is important and for some cases we need to process these further. This is the subject of current research in which we are improving the global planner so that it can perform some geometric reasoning. The nominal path can also be input by hand when the system is operating in a teleoperation mode ( gure 3). Once the nominal path has been chosen, the planner attaches a \virtual" span between the control point on the manipulator and one end of the extracted \pathlet". This span is given a \shrinking" behaviour and so tries to reach zero length2 . By moving this virtual span along the pathlet towards the (sub)goal, the manipulator's equations of motion allow the control point to be drawn towards the (sub)goal whilst ensuring that all the spans of the arm avoid obstacles.
L
current
p
i
Figure 7 With the original length constraint,
the force which acts so that the link maintains its original length is along the link.
joint axis and these, obviously, must be updated. This is done by starting at the base of the arm and computing the next joint axis from the current joint axis using initial state information. In a similar fashion to the Denavit-Hartenberg notation, a frame is assigned to to each joint and the initial joint axes are input to the system. The Z^ i is coincident with joint axis ki and, as with the ^ i is calculated to Denavit-Hartenberg notation, X ^ be along link i unless Zi and Z^ i+1 intersect, in ^ i is the rewhich case Y^ i is along link i and X maining axis of the frame. Thus Xi = (pi+1 ? pi) ? (pi+1 ? pi) Z^ i X^ i = normalise( Xi)
9. Conclusions And Further Work
0
0
This paper has described the extensions required for the path planning system to operate in a 3 dimensional workspace; the resulting system is still fast, e ective, and versatile. Although the planner
Note that this would be Yi and Y^ i if joint axes Z^i and Z^ i+1 intersect. 0
2 The situation is rather like pulling the manipulator, from the speci ed control point, along the nominal path using a piece of string.
In order to compute Z^ i , the i in the DenavitHartenberg notation must be input. Then the 7
shaped links can be incorporated into the interpolation function (Qin et al., 1995). We have also started to develop a global path planner which sits above the aforementioned path re nement scheme (McLean, 1994). This higher level plans paths for simple point robots on a skeleton structure which represents the free space. Paths are selected which satisfy certain constraints and used as nominal paths input to the low level mechanism. This then plans a path for the entire arm such that the speci ed control point moves as closely as possible to the nominal path, but whilst ensuring that the entire manipulator remains in free space. An example of the high level planner working as a supervisor to the low level planner is shown in gure 9.
planner can plan complex manuvres in more dif cult workspaces.
Acknowledgements The authors gratefully
acknowledge the support of the McDonnellDouglas Space Systems Company, of the Engineering and Physical Sciences Research Council, and of ESPRIT-BRA grant SECOND.
REFERENCES Barraquand, Jer^ome and Jean-Claude Latombe (1990). A monte-carlo algorithm for path planning with many degrees of freedom. In: Int. Conf. Robotics & Automation. Cincinnati. pp. 1712{1717. Craig, John J. (1986). Introduction to Robotics: Mechanisms and Control. Addison-Wesley. Curwen, R., A. Blake and R. Cipolla (1991). Parallel implementation of lagrangian dynamics for real-time snakes. In: British Machine Vision Conference (P. Mowforth, Ed.). Springer Verlag. pp. 29{35. Faverjon, Bernard and Pierre Tournassoud (1987). A local based approach for path planning of manipulators with a high number of degrees of freedom. In: Int. Conf. Robotics & Automation. Raleigh. pp. 1152{1159. Hague, Tony (1993). Motion Planning for Autonomous Guided Vehicles. PhD thesis. Oxford University Department of Enginering Science. McLean, A. W. and S. A. Cameron (1995). The virtual springs method: Path planning and collision avoidance for redundant manipulators. To appear, Int. J. Robotics Research. McLean, Alistair (1994). A framework for global planning for redundant manipulators.. In: Int. Conf. Intelligent Robotics and Systems. Munich. McLean, Alistair and Stephen Cameron (1993). Snake-based path planning for redundant manipulators. In: Int. Conf. Robotics & Automation. Vol. 2. Atlanta. pp. 275{282. Qin, Caigong, Stephen Cameron and Alistair McLean (1995). A modelling scheme for ecient robot motion planning. Submitted Solid Modeling 95.
Goal
8
2 4
6 7
c’
d’’
5
d’ b’
d
3
1
c’’ 3
c
b a
Figure 9 The high level planner generates a
number of subgoals that specify manuvres the low level planner can perform to move to the goal. The arm withdraws from its constrained con guration (1) and joint c is opened (2) so that a \ ip" manuvre can be performed (3). The planner tries to get c to c' but fails due to the obstacle (4) and so moves b to b' (5). Positions for c" and d" are computed. Then c is nally moved to c' (6) and d moves to align the 3rd link with the constrained region (7) and nally reaches the goal (8). The high level planning takes a couple of seconds and the actual trajectories, (these are smooth because of the dynamics) are generated in 15 seconds on an SGI Indigo R4000.
Although a signi cant development (and certainly a useful prototype of a tele-operator's assistant), the current implementation is still only capable of autonomously planning relatively simple paths for manipulators in complex 3 dimensional workspaces. Current research is aimed at improv-
This paper is to appear in Int. Conf. Intelligent Autonomous Systems{4, Karlsruhe, March 1995 8
