1666
Sticky and Slippery Collision Avoidance for Tele-Excavation Michael Greenspan
John Ballantyne
Mike Lipsett
National Research Council of Canada Ottawa, Ontario, Canada greensp
[email protected] c.ca
Spar Aerospace Ltd. Brampton, Ontario, Canada j ballant @spar. ca
Syncrude Canada Ltd. Edmonton, Alberta, Canada lipsett
[email protected]
Abstract Two n e w m o d e s of real-time collision avoidance are presented wh.ich aid in t h e e.ffectiveness of remote tele-operation. S t i c k y collision m o d e improves s y s t e m safety by disallowing contact with modelled obstacles. Slippery m o d e is used t o improve t h e efficiency of -the process by reducing t h e required level of operator skill. B o t h m o d e s execute an real-time and have been ampdemented a n d tested on a r e m o t e excavator system. It w a s f o u n d that precise geometries could be excavated using extremely simplified cutting strategies that requared v e r y little f i n e control o n t h e part of the operator, thereby improving t h e eflectiveness of remote tele-excavation.
1
Introduction
In normal operation of an excavator, the operator is seated in the cab of the machine and utilizes a variety of familiar sensory cues. There are situations where the environment of the worksite is too harsh for human presence, requiring the use of a tele-excavation system [I, 21. As the operator is at a remote location many of the familiar sensory cues, such as direct view, sound, engine vibrations, tilt, etc., are unavailable, increasing the difficulty of effective operation. Fortunately, the communication equipment required to remotize standard excavation equipment also provides the opportunity t o introduce advanced functions which can increase the system's controllability. One such function is real-time collision avoidance (RTCA) [3, 4, 51. Unlike path planning, where the collision-free motions can take seconds or minutes t,o generate, RTCA executes within a joystick-controlled teleoperation system, where the operator is continuously commanding some change in the device's configuration. Each new commanded configuration must be verified as collisionfree in real-time. This verification must be computationally efficient, ideally calculable within the controller cycle time, which is typically on the order of 10 Proc. IROS 97 0-7803-4119-8/97/$1001997IEEE
msec. This paper describes two modes of model-based RTCA. The sticky collision mode is a safety critical function which prevents unwanted contact between a manipulator and workspace obstacles. The slipp e r y collision mode uses geometric stencils, which are boundaries of the excavation regions, to reduce the required level of positioning accuracy within the boundaries of excavation, thereby increasing the effectiveness of the system. The paper continues with a brief review of the collision detection method. RTCA is presented as a filter within a generic teleoperation system, and the sticky and slippery modes are described. The implementation of both modes on a remote excavation system is then presented, and some experiments are described which result in the development of simplified excavation strategies using the slippery mode. The paper concludes with a summary and a description of continuing research.
2
Collision Detect ion
The collision detection method is similar t o the numerical potential field method [6], and has been previously described in [5]. The method uses a spherical model to represent the manipulator, and a voxel map to represent static obstacles, and executes in real-time, independent of the number of obstacles in the workspace.
2.1
Spherical Model
Let M represent the exterior surface geometry of an object. A spherical model of Ad is denoted S M ,and is the union of a set of various sized spheres. By design SM overapproximates N , so that M is completely contained within s ~The . amount of overapproximation is bounded, so that every point jii on the surface of M has a corresponding point jij on the surface of SM that is within some Cartesian distance [ a ,/3] :
1667
geometry
clearance formula
Table 1: Clearance Formulae
2.3
a) surface model
Clearance Test
The clearance of a model is defined as the minimum distance between any point on the surface of the model with any point on the surface of the set of obstacles. The spherical and voxel representations are used to calculate a conservative underestimate of clearance. The clearance calculations for a point, sphere, model, and manipulator, are developed in [7] and listed in Table 1 The manipulator clearance is simply the minimum of the model clearances for all links in the manipulator. ~
3
Teleoperative Collision Avoidance
A single cycle of a simplified generic direct teleoperation control system C, is illustrated in Figure 2a. The cycle occurs at a regular interval with period t,. At the beginning of each cycle, the controller C reads the current measured configuration X , fr2m the joint sensors, and a change in configuration AX, is_commanded from the joystick. Note that the symbol X is used here generically to denote a robot configuration in either joint, Cartesian, or any_other suitable space. The controller C converts AX, to a sequence of joint values {2c,i, i = l..nc) which position the joints throughout the cycle. At the end of the cycle the manipulator is in configuration 4
b) spherical model with 957 spheres Figure 1: Excavator Models
The surface and spherical models for the excavator are illustrated in Figure 1.
2.2
Voxel Space
A voxel is a small cube of space, and a voxel space is a collection of voxels quantizing a region of 3D space. The size of a voxel is called the resolution of the voxel space, denoted by p. For example, a voxel space with resolution p = 2 cm contains voxels of volume 8 cubic cm. The region represented by the voxel space is the reachable envelope of the manipulator, and contains a set of obstacles 0. Each voxel v is assigned an integer value D ( v ) which indicates its distance to the nearest surface of the nearest obstacle in 0. The value of the voxel containing point l? is denoted V ( 3 . Much of the efficiency of the collision detection method is attributed to the fact that all voxel values are precomputed for a given 0, and that V(p3 is a trivial calculation.
where e is the positioning error. RTCA is implemented as a filter f to the joystick input, as illustrated in Figure 2b. The filter tests each A g I to determine if the resulting configuration would result in a collision state. If so, the filter modifies the input to prevent the collision from occurring. The requirement for computational efficiency is clear, as the RTCA filter must work within the controller period t,, which is typically on the order of tens of milliseconds. All A 2 1 which do not result in an impending collision pass through the filter unaltered. If AX, does 'For an illustration describing the clearance calculation for a sphere, see [5, Figure I].
1668
a) Generic Teleoperation Cycle
0
A
Figure 3: Slippery Collision. Component A' is reqoved from input command A 2 1 , leaving component B
A%,i
b) RTCA Filter Added
Figure 2: Teleoperation Control System
result in an impending collision, then there are two alternative filter modes which can be applied. The first alternative, called sticky mode, is to send no motion command so the manipulator stops and does not move into the collision state. The second alternative, called slippery mode, is to send some non-zero variation of AX1 which results in a collision-free configuration.
3.1
Sticky Mode
In sticky collision mode the response to an impending collision is simply to stop the manipulator. Its name is derived from the feeling that the operator experiences that the manipulator sticks at the point, of contact with an obstacle when commanded into a collision. If k is the minimum allowable clearance, and RE21 denotes a manipulator R in configuration 2 , then the sticky mode filters the input at each cycle according to the following rule :
-
AX, =
+
{ O
if .fT(R[>?, AZI]) 2 k otherwise
(3)
Note that in practise a sticky collision does not terminate the control of the device, as all subsequent noncolliding input commands are executed unchanged.
3.2
Slippery Mode
The slippery collision mode derives its name from the feeling that the operator experiences that the manipulator is sliding freely tangent to the surface of a contacted obstacle. Rather than zeroing the entire input command, 2s in the sticky mode, only those components of AX, which axe normal to and in the direction of the obstacle's surface are zeroed, with others passing through unchanged. An example of the allowed and filtered motion components in a slippery collision is illustrated in Figure 3. It is believed to be undesirable, from a human factors perspective, for the filter to add any components
to AZI which are not explicitly commanded by the operator, as this could lead to unexpected motions and the impression that the operator does not have direct control over the manipulator. Rather, offending motion components are removed, and those that do not contribute to the impending collision are allowed unaltered. In slippery mode, the input command is multiplied by a gain matrix Ax. All non-diagonal elements of Ax are equal to zero, and diagonal elements are either zero or one :
with A i E (0, l}, i = 1,...In. To determine the value of Ax in a given cycle, consider the cas: of a single sphere at collision-free configuration X u , such that .f8(s[X,]) k . The input command resul? in an impending colkion at configuration = X, A z I , with fs(s[Xa]) < k . As a sphere is rotationally invariagt, without loss of generality we will assume that AX1 has only translational components, i.e. Ad1 = ( d z , dy, d z ) . We wish to determine the values of A, Ay, A, so that fs(s[dj]) k. Let the voxel at the center of ~ [ d be , ] denoted U,, and let the neighbouring voxels of va be denoted . u [ ~ l [ ~ l [ k l , i l i k E {--LO, 11. The A i are calculated by processing the voxels in the neighbourhood of U,. Whereas a voxel has a total of 26 ceighbours ', it is only necessary to process those neighbours in the direction of the input command, as there can only be danger of collision with obstacles that the sphere is moving towards. At most, 7 neighbours need be considered for any input motion. The X i are calculated by comparing the value of v, with those of the v , [ i ] [ j ] [ kin ] the direction of the input
:
z~bl
>
+
>
1669
command. If a neighbour voxel wa[i][j][k]has a value less than w,, then there is an obstacle in the direction of that neighbour, and the sphere will collide if its center enters that neighbour. Otherwise, if a neighbour’s value is greater than or equal to that of v u , then the sphere will exceed the clearance if its center travels through this neighbour. The complete procedure is listed in psuedocode in Figure 4. There are cases where pure motions along one commanded direction are collision-free, but a combined motion causes collision. This is equivalent to being on the corner of a surface, and the slippery filter must choose which direction t o allow. By convention, the direction with the largest magnitude of input is selected, as this is the direction which the operator is commanding with greater intention. This situation also occurs with combinations of three components, and is resolved the same way. The extension of the slippery collision filter from a single sphere to a model is straightforward if the assumption is maintained that the input command contains only translational components The A i are calculated for each colliding sphere in S M , and the minimum is selected for inclusion in Ax. In this way, any single sphere collision can restrict a motion component for the entire model. If the rotational components of the input are not zeroed, then the calculation is complicated by the fact that a model rotation is manifested as a translation for every sphere in S M ,which must be decoupled in calculating the A i .
4
A 2 1 = ( d z , dy, d z ) A, = l , A y = 1,A, = 1
Experimentation
The sticky and slippery filters were implemented and tested on the remote excavator system described in [2]. The RTCA filters were executed in real-time on a 33 MHz PC386, showing that the method requires minimal computational resources. The cycle-time t, was 50 msec, and the RTCA filter was calculated in less than 20 msec. The voxel space covered a region which was approximately 20 x 20 x 14 m3 in extent, containing 1.6 x lo6 voxels with a resolution of 15 cm, which was approximately equal to the absolute positioning accuracy of the excavator. The spherical model is illustrated in Figure l, and modeled the excavator with a 10 cm accuracy with 957 spheres. The slippery mode was applied t o the bucket, and the sticky mode was applied to all other links. This was believed t o be consistent with the operation of an excavator, as only the bucket-is required to move earth. The motion command AX1 was in cylindrical coordinates in the bucket frame, and was converted to Cartesian coordinates for the filter calculations. When in slippery collision, only the translational components of the motion were allowed, so that the bucket retained its attitude w.r.t. the world frame.
else if (Idyl 2 Ida:!) and (Idyl 2 1d.I) then A, = 0, A, = 0 else if (ldzl 2 ( d z / )and (/&I 2 Idyl) then A, = 0, A, = 0 Figure 4: Sphere Slippery Mode Pseudocode
1670
I ' "
\rP
'
I 02
Figure 5: Cutting Strategy for Corner Stencil
a) Corner a
h
A set of three geometric stencils, which represented the boundary of volumetric regions to be excavated, were designed and tested. The stencils are illustrated in Figure 6, and are believed to be typical of the geometries encountered in an excavation process. With the aid of the graphical user interface, the stencils were positioned in the 3D simulation of the workspace so that they were partially buried. The slippery mode allowed bucket motions which were parallel to the surface of the stencil. The bucket slid along the stencil surface, removing earth up t o but not beyond the stencil SUPface. After some experimentation, simple cutting strategies were developed for each stencil. Each strategy consisted of a series of simple motion commands, called phases. A phase is a combination of motion components which correspond to a particular geometric feature of the stencil. The components of motion which the operator is to command in each phase are illustrated beside each geometry stencil in Figure 6. In Figure 5, the commanded motion components for each phase of the corner stencil are illustrated. In phase a , the bucket is not in contact with either the boundary planes or the earth, and can move freely in any direction. In phase b, the bucket is in contact with the vertical boundary plane 0 2 , behind which may lie a building or other obstacle which cannot be collided with. The operator provides a motion component towards the plane and downward, and the slippery collision allows the bucket to slide parallel to and flush with 0 2 without extending beyond it. In phase c, the operator provides a motion component downwards and towards the body of the excavator. The bucket will slide along the boundary plane 01, removing the earth above it, but not below it.
A e
b) Trench a
A
c) Wave
Figure 6: Geometry Stencils
1671
To date, the evaluation of the method has been purely subjective and qualitative. A more thorough evaluation would require a greater expertise of human factors engineering and testing than has been available to the project. It was the belief of the handful of project members who used the system that the strategies were easy to execute, and that complex shapes could be excavated without a great deal of operator skill. The stencils added positioning precision so that the operator was responsible only for providing the motion components required in the current phase. The relative magnitude of the motion components affected the efficiency of the process, but did not affect the shape of the region under excavation. Also, incorrect motion components did not affect the shape of the region, as the filter prevented contact with any earth outside of the stencil.
5
Conclusion
Two new modes of real-time collision avoidance were developed and tested on a tele-excavation system. As well as preventing contact with any obstacles in the workspace, the slippery mode increased the effectiveness of the remote tele-excavation process by reducing the degree of required operator precision. The excavation of fairly complicated regions was accomplished by placing geometry stencils in the simulated scene, and using the slippery collision mode to guide the operation along the borders of these stencils. The process was thus reduced to a strategic sequence of simple phases, which were tolerant to imprecisions. In practise, there are other factors, such as the conditions of the muck and inclusions, which affect the excavation process. Because the contact with the earth is a horizontal line of teeth, excavator tasks typically are designed for chiselling about an axis. The excavator task geometries require a point on the end of the line to follow the stencil. In some cases, it may be desirable t o use a spherical model which does not enclose the bucket geometry. For example a row of spheres positioned along the bucket teeth may be sufficient. The main focus of future work will be to compensate for the effects of bucket rotations, allowing them in the slippery mode. It is also of interest to allow fractional values of the gain matrix elements X i , which will cause the excavator to gently slow its motion as it approaches closer to an obstacle. The work will also be extended to develop more automated excavation processes based upon this and other methods.
Acknowledgements The authors would like t o thank PRECARN Associates Inc. for partially funding this research. Thanks also to Nestor Burtnyk for his many helpful suggestions, Eldon Wong and Eric Gagnon for their con-
tributions in implementing the sydem, and the other members of the VERO team.
References Derek Seward, Frank Margrave, Ian Sommerville and Richard Morrey, “LUCIE the Robotic Excavator - Design for System Safety,” Proc. 1996 Intl. Conf. Rob. Aut., Vol. 1. pp. 963-968, 1996.
Michael Greenspan, Michael Lipsett, John Ballantyne, Pam Renton, Eric Gagnon and Nestor Burtnyk, “Laser Range Vision for Tele-Excavation,” Proc. 1996 CSA/DND Workshop on Rob. and Exp. Sys., 1996. Wyatt S. Newman and Michael S. Branicky, “Experiments in Reflex Control for Industrial Manipulators,” Proc. 1990 Intl. Conf. Rob. Aut., Vol. 1, pp. 266-271, 1990.
C.A. Shaffer and G.M. Herb, “A Real-Time Robot Arm Collision Avoidance System”, IEEE Trans. Rob. Aut., Vol. 8, No. 2 , pp. 149-160, April 1992. Michael Greenspan and Nestor Burtnyk, “Obstacle Count Independant Real-Time Collision Avoidance,” Proc. 1996 Intl. Conf. Rob. Aut., Vol. 2, pp. 1073-1080, 1996. JCr6me Barraquand, Bruno Langlois and Jean-Claude Latombe, “Numerical Potential FieEd Technicpes for Robot Path Planning,” IEEE Trans. Sys., Man, Cyb., Vol. 22, No. 2, pp. 224-241, MarchIApril 1992. Michael Greenspan, “Collision Avoidance for Automated Inspection,” SPIE Photonacs East 96 : Proceedings of the International Symposium on Advanced Manufacturing, Boston Mass., pp. 67-78, Nov. 18-22, 1996.
