Wei Zhang
University of Bridgeport
Bridgeport, CT 06601

Abstract

This paper presents an obstacle avoidance approach for manipulators based on obstacle avoidance path planning concept. Although the obstacle dealt with in the paper is of cubic type, it can be transformed to the spherical type as showed in the paper, which has a simpler analytical description than the former one. Obstacle avoidance problem has been formulated in terms of collision avoidance of links rather than points. Link collision avoidance is achieved by continuously controlling the link's closest point to the obstacle. For the robot SIR-1 which has an articulated chain, a link can be represented as the line segment defined by the Cartesian position of its two neighboring joints. By doing so we can implement a single and simple obstacle avoidance path planning. A program which simulates this approach is also included.

Demo

1. General consideration

For the most part, we will consider motions of a manipulator as motions of tool frame, {T}, relative to the station frame, {S}. This is the same manner in which an eventual user of the system would think, and designing a path description and generation system in these terms will result in a few important advantages.

When we specify paths as motion of the tool frame relative to the station frame, we decouple the motion description from any particular robot, end-effector, or workpieces. This results in a certain modularity, and would allow the same path description to be used with a different manipulator, or with the same manipulator with a different tool size. Further, we can specify and plan motions relative to a moving work station ( perhaps a conveyor belt) by planning motions relative to the station frame as always, and at run time causing the definition of {S} to be changing with time.

The basic problem is to move the manipulator from an initial position to some desired final position. That is, we wish to move the tool frame from its current value, {Tinit}, to a desired final value, {Tfinal}. Note that this motion in general involves a change in orientation as well as a change in position of the tool relative to the station.

For the purpose of obstacle avoidance, the path of the end-effector can be further constrained by the addition of via points intermediate to the initial and final configurations. It is necessary to specify the motion in much more detail than simply stating the desired final configuration. One way to include more detail in a path description is to give a sequence of via points or intermediate points between the initial and final positions. Thus, in completing the motion, the tool frame must pass through a set of intermediate positions and orientations as described by the via points. Each of these via points is actually a frame which specifies both the position and orientation of the tool relative to the station. The name path points includes all the via points plus the initial and final points. Remember that "points" mentioned here are actually frames which give both position and orientation. Along with these spatial constraints on the motion, the user may also wish to specify temporal attributes of the motion. For example, the time elapsed between via points might be specified in the description of the path.

Usually, it is desired for the motion of the manipulator to be smooth. For our purposes, we will define a smooth function as one which is continuous and has a continuous first derivative. Sometimes, a continuous second derivative is also desirable. Rough, jerky motions tend to cause increased wear on the mechanism, and cause vibrations by exciting resonance in the manipulator. In order to guarantee smooth paths, we must put some sort of constraints on the spatial and temporal qualities of the path between the via points. At this point there are many choices that may be made, and consequently a great variety in the ways that paths might be specified and planned. Any smooth functions of time which pass through the via points could be used to specify the exact path shape.

2. Path description and generation

For path planning, we are concerned only with position of the end-effector in spatial space, since the orientation of the end-effector will not change the path planned.

Path generation usually includes the human interface problem of how we wish to specify a path through space. In order to make the description of manipulator motion easy for human user of a robot system, the user should not be required to write down complicated functions of space and time to specify the task. Rather, we must allow the capability of specifying paths with simple descriptions of the desired motion, and let the system figure out the details. For example, the user may just specify the desired goal position and orientation of the end-effector, and leave it to the system to decide on the exact shape of the path to get there, the duration, the velocity profile, and other details.

We also are concerned with how paths are represented in the computer after they have been planned. Finally, there is a problem of actually computing the path from the internal representation, or generating the path. Generation occurs at run time and, in the most general case, position, velocity, and acceleration are computed. Since these paths are computed on digital computers, the path points are computed at certain rate, called the path update rate. In typical manipulator system this rate lies between 20 and 200 Hz.

3. Joint space schemes

Here we consider methods of path generation in which the path shapes (in space and in time) are described in terms of functions of joint angles.

Each path point is usually specified in terms of a desired position and orientation of the tool frame, {T}, relative to the station frame, {S}. Each of these via points is "converted" into a set of desired joint angles by application of the inverse kinematics. Then a smooth function is found for each of the n joints which pass through the via points and end at the goal point. the time required for each segment is the same for each joint so that all joints will reach the via point. Other than specifying the same duration for each joint, the determination of the desired joint angle function for a particular joint does not depend on the functions for the other joints.

Hence, joint space schemes achieve the desired position and orientation at the via points. In between via points the shape of the path, while rather simple in joint space, is complex if described in Cartesian space. Joint space schemes are usually the easiest to compute, and, because we make no continuous correspondence between joint space and Cartesian space, there is essentially no problem with singularities of the mechanism.

4. Inverse kinematics for SIR-1 robot manipulator

For SIR-1 robot manipulator, given the position as P={x, y, z}thus the required first three joint variables are evaluated as follows:

The motion of the first three joints is calculated by computing the joint variables corresponding to P. So we can use above formula to convert any P in Cartesian space to its corresponding Joint space.

5. Cubic polynomials function for path smoothing

Consider the problem of moving the tool frame from its initial position to a goal position in a certain amount of time. Using the inverse kinematics the set of joint angles that corresponding to the goal position and orientation can be calculated. The initial position of the manipulator is also known in the form of a set of joint angles. What is required is a function for each joint whose value at is the initial position of the joint, and whose value at is the desired goal position of that joint. There are many smooth functions, , which might be used to interpolate the joint value.

In making a single smooth motion, at least four constraints onare evident. Two constraints on the function's value come from the selection of initial and final value:

(1)

An additional two constraints are that the function is continuous in velocity, which in this case means that the initial and final velocity are zero:

(2)

These four constraints can be satisfied by a polynomial of at least third degree. Since a cubic polynomial has four coefficients, it can be made to satisfy the four constraints given by (1) and (2). These constraints uniquely specify a particular cubic. A cubic has the form

(3)

and so the joint velocity and acceleration along this path are clearly

(4)

Combining (3) and (4) with the four desired constraints yields four equations in four unknowns:

(5)

Solving these equations for thewe obtain:

(6)

Using (6) we can calculate the cubic polynomial that connects any initial joint angle position with any desired final position. This solution is for the case when the joint starts and finishes at zero velocity.

6. Cubic polynomials for a path with via points

So far we have considered motions described by a desired duration and a final goal point. In general, we wish to allow paths to be specified which include intermediate via points. If the manipulator is to come to rest at each via point, then we can use the cubic solution as before. Usually, we wish to be able to pass through a via point without stopping, and so we need to generalize the way in which we fit cubics to the path constraints.

As in the case of a single goal point, each via point is usually specified in terms of a desired position and orientation of the tool frame relative to the station frame. Each of these via points is converted into a set of desired joint angles by application of the inverse kinematics. We then consider the problem of computing cubic which connect the via point values for each joint together in a smooth way.

If desired velocities of the joints at the via points are known, then we can determine cubic polynomials as before, but now the velocity constraints at each end are not zero, but rather, some known velocity. The constraints become:

(7)

The four equations describing this general cubic are

(8)

Solving these equations for thewe obtain

(9)

Using (9) we can calculate the cubic polynomial that connects any initial and final positions with any initial and final velocities.

Another way is let the system automatically chooses reasonable intermediate velocities using some kind of heuristic. This choice is the result of applying a conceptually and computationally simple heuristic. Imagine the via points connected with straight line segments: if the slope of these lines changes sign at the via point, choose zero velocity, if the slope of these lines does not change sign, choose the average of the two slopes as the via velocity. In this way, from specification of the desired via points alone, the system can choose the velocity at each point.

7. Collision-free path planning for SIR-1 robot manipulator

Now, let us consider the obstacle avoidance, or collision-free, path planning for SIR-1 robot manipulator.

It would be extremely convenient if we could simply tell the robot system what the desired goal point of the manipulator motion is, and let the system determine where and how many via points are required so that the goal is reached without the manipulator hitting any obstacles. In order to do this, the system must have models of the manipulator, the work area, and all potential obstacle in the area. A second manipulator may even be working in the same area and hence each arm must be considered as a moving obstacle for the other.

The obstacle we will deal with is of the cubic form. For simplifying the problem we assume that there is only one cubic in work area, so the problem becomes how to find a set of via points so that the SIR-1 robot manipulator can go from initial position to final position without hitting the cubic.

Considering the presentation of cubic in Cartesian space we noticed that for any cubic in work area we can always find a sphere to cover it completely, but its description in Cartesian space is much simpler than cubics'. So we found a way to find via points: in order to avoid hitting obstacle each via point must resides on or above the surface of the sphere not allowing the end-effector as well as the links of the robot to cut through the surface. Following we will find mathematics formula by applying this rule on them.


8. Robot obstacle avoidance: A geometric approach

The manipulator obstacle avoidance problem has been formulated in terms of collision avoidance of links rather than points. Link collision avoidance is achieved by continuously controlling the link's closest point to the obstacle. Additional links can be artificially introduced or the length of the last link can be extended to account for the manipulator tool or load. For SIR-1 robot, a link can be represented as the line segment defined by the Cartesian position of its two neighboring joints.

The axes of the frame of reference R are chosen such that its z-axis is the base z-axis of manipulator and its origin is at its bottom. The manipulator and obstacle's parameters are designated by respectively.


First let us take a look down along the z-axis and we can get the obstacle range for, that is:

; . To guarantee link not to hit the obstacle, D must be greater than R, otherwise the obstacle is too large to be avoided. The range for of the obstacle thus can be expressed in.

Then we calculate the distances of link 2 and link 3 to the surface of the sphere. We project sphere onto the plane which is formed by line L (see figure). In this way we reduce the 3-D problem into a 2-D problem. From the figure we obtain:

These are distances of link 2 and link 3 to the surface of the sphere. Now let L2 and L3 be r, then we get the minimum as follow:

or let r be L2 or L3 we can get minimum:

So now in order to avoid link collision with an obstacle, must satisfy the above conditions as well as point avoidance conditions discussed bellow.

9. Summary and Discussion

Carrying out the above operations, we have an algorithm for a spherical-obstacle avoidance path planning. Given initial, final and obstacle configurations, first convert them into joint space representations, then calculate the path with cubic polynomial functions without obstacle, then take the obstacle into consideration: when enters the range, that means the manipulator already hit the object, then apply the following algorithm to calculate the in-between path; when goes out of the range, operation resumes.

For the point avoidance it occurs when do not satisfy the conditions above, that is , if this happens following conditions must be met:

Here, K2 and K3 stand for the lengths between start point of a link to the surface of sphere. Now let us summarize the algorithms of finding via points that are not only point-avoidance but also link-avoidance.

Algorithm:

  1. Calculate inverse kinematics of initial and final points;
  2. Decide what mode the algorithm will work on;
  3. For each, check, if YES, goto step 4; otherwise continue step 3;
  4. For each, calculate L2;
  5. If , link 2 not hit the obstacle(link avoidance condition), goto step 8;
  6. If, link 2 not hit the obstacle(point avoidance condition), goto step 8;
  7. Link 2 hit the obstacle, set for mode 1, or for mode 0;
  8. For each and L2, calculate L3;
  9. If , link 3 not hit the obstacle(link avoidance condition), goto step 12;
  10. If, link 3 not hit the obstacle(point avoidance condition), goto step 12;
  11. Link 3 hit the obstacle, set for mode 1, or for mode 0;
  12. Stop. The results will be the set of via points represented by joint space angle and can be used for controlling.

Demo

10. Results and Comparisons

In order to have a general picture of the above algorithm, let's take a look at two paths planned:
One without an obstacle; and one with an obstacle.

The parameters for the manipulator are: d1 = 1, a2 = 3, a3 = 2; initial and final points are:
P0 = (0, 3, 1), Pf = (0, -3, 1) and parameters for obstacle are D = 4, R = 2.

Following are the via points generated with the algorithm discussed above:
Via points w/o an obstacle: (21 points)
# Point(x, y, z) point()
00.0-3.01.0-90.0-38.942109.471
10.469-2.9631.0-81.0-38.942109.471
2 0.927 -2.853 1.0 -72.0 -38.942 109.471
3 1.361 -2.673 1.0 -63.0 -38.942 109.471
4 1.763 -2.427 1.0 -54.0 -38.942 109.471
5 2.121 -2.121 1.0 -45.0 -38.942 109.471
6 2.427 -1.763 1.0 -36.0 -38.942 109.471
7 2.673 -1.361 1.0 -27.0 -38.942 109.471
8 2.853 -0.927 1.0 -18.0 -38.942 109.471
9 2.963 -0.469 1.0 -9.0 -38.942 109.471
10 3.0 0.0 1.0 0.0 -38.942109.471
11 2.963 0.469 1.0 9.0 -38.942 109.471
12 2.853 0.927 1.0 18.0 -38.942 109.471
13 2.673 1.361 1.0 27.0 -38.942 109.471
14 2.4271.7631.036.0-38.942109.471
15 2.121 2.121 1.0 45.0 -38.942 109.471
16 1.763 2.427 1.0 54.0 -38.942 109.471
17 1.361 2.673 1.0 63.0 -38.942 109.471
18 0.927 2.853 1.0 72.0 -38.942 109.471
19 0.469 2.963 1.0 81.0 -38.942 109.471
20 0.0 3.0 1.0 90.0 -38.942 109.471
Via points w an obstacle: (21 points)
#Point(x, y, z)point()
00.0-3.01.0-90.0-38.942109.471
10.530-3.3510.793-81.0-39.21996.914
21.153-3.5490.502-72.0-39.49584.358
31.816-3.565 0.141 -63.0 -39.772 71.802
4 2.460 -3.385 -0.272 -54.0 -40.049 59.245
5 3.022 -3.022 -0.719 -45.0 -40.326 46.689
6 3.450 -2.506 -1.177 -36.0 -40.602 34.133
7 3.702 -1.886 -1.624 -27.0 -40.879 21.576
8 3.758 -1.221 -2.038 -18.0 -41.156 9.020
9 3.619 -0.573 -2.398 -9.0 -41.433 -3.536
10 3.473 0.0 -2.570 0.0 -41.710 -10.196
11 3.619 0.573 -2.398 9.0 -41.433 -3.536
12 3.758 1.221 -2.038 18.0 -41.156 9.020
13 3.702 1.886 -1.624 27.0 -40.879 21.576
14 3.450 2.506 -1.177 36.0 -40.602 34.133
15 3.022 3.022 -0.719 45.0 -40.326 46.689
162.4603.385-0.27254.0-40.04959.245
171.8163.5650.14163.0-39.77271.802
181.1533.5490.50272.0-39.49584.358
190.5303.3510.79381.0-39.21996.914
200.03.01.090.0-38.942109.471

We can see the differences between points and joint angles with or without obstacle. And because the algorithm's simplicity and efficiency it might be used in real-time robot path generating with an obstacle avoidance.

Acknowledgments

I'd like to thank Dr. Sobh for his guide and support for this paper.

References

Craig, J. "Introduction To Robotics", Addison-Wesley, 1989

Spong Vidyasagar. "Robot Dynamics and Control", Wiley 1989

Khatib, "Real time obstacle avoidance for manipulators and mobile robots", The International Journal of Robotics Research. 1984

Tomas Lozano-Perez, "A simple Motion-Planning Algorithm for General Robot Manipulators", IEEE journal of Robotics and Automation, 1987