Configuration Control of Seven Degree of Freedom Arms

1. Technical Field

The invention is related to the use of the configuration control method disclosed in U.S. Pat. No. 4,999,553 by one of the inventors herein to the control of seven degree of freedom robot arms, using a forward kinematic approach.

2. Background Art

U.S. Pat. No. 4,999,553, the disclosure of which is hereby incorporated herein by reference, discloses a configuration control method employed in the present invention.

References

The background of the present invention is discussed below relative to the following references by referring to them by the bracketed numbers associated with each reference as follows:

[1] J. M. Holierbach: "Optimum kinematic design for a seven degree of freedom manipulator," Proc. 2nd Intern. Symp. on Robotics Research, Kyoto, Japan, August 1984, pp. 215-222.

[2] J. Lenarcic, and A. Umek: "Experimental evaluation of human arm kinematics," Proc. 2nd Intern. Symp. on Experimental Robotics, Toulouse, France, June 1991.

[3] D. E. Whitney: "Resolved motion rate control of manipulators and human prostheses," IEEE Trans. Man-Machine Systems, 1969, Vol. MMS-10, No. 2, pp. 47-53.

[4] A. Li'egeois: "Automatic supervisory control of the configuration and behavior of multibody mechanisms," IEEE Trans. Systems, Man and Cybernetics, 1977, Vol. SMC-7, No. 12, pp. 868-871.

[5] R. V. Dubey, J. A. Euler, and S. M. Babcock: "An efficient gradient projection optimization scheme for a 7 DOF redundant robot with spherical wrist," Proc. IEEE Intern. Conf. on Robotics and Automation, Philadelphia, April 1988, pp. 28-36.

[6] Y. Nakamura and H. Hanafusa: "Task priority based redundancy control of robot manipulators," Proc. 2nd Intern. Symp. on Robotics Research, Kyoto, August 1984.

[7] A. A. Maciejewski and C. A. Klein: "Obstacle avoidance for kinematically redundant manipulators in dynamically varying environments," Intern. Journ. of Robotics Research, 1985, Vol. 4, No. 3, pp. 109-117.

[8] C. W. Wampler: "Inverse kinematic functions for redundant manipulators," Proc. IEEE Intern. Conf. on Robotics and Automation, Raleigh, April 1987, pp. 610-617.

[9] J. Baillieul: "Kinematic programming alternatives for redundant manipulators," Proc. IEEE Intern. Conf. on Robotics and Automation, St. Louis, March 1985, pp. 722-728.

[10] K. Anderson and J. Angeles: "Kinematic inversion of robotic manipulators in the presence of redundancies," Intern. Jour. of Robotics Research, 1989, Vol. 8, No. 6, pp. 80-97.

[11] H. Seraji: "Configuration control of redundant manipulators: Theory and implementation," IEEE Trans. on Robotics and Automation, 1989, Vol. 5, No. 4, pp. 472-490.

[12] J. D. Farrell, J. M. Thompson, J. P. Karlen, H. I. Vold and P. H. Eismann: "Modular, configurable, kinematically redundant manipulators," Proc. Japan-USA Symposium on Flexible Automation, Kyoto, July 1990, pp. 303-308.

[13] J. J. Craig: Robotics--Mechanics and Control/, Addison Wesley Publishing Company, New York, 1986.

[14] D. E. Whitney: "The mathematics of coordinated control of prosthetic arms and manipulators," ASME Journ. Dyn. Syst., Meas. and Control, 1972, Vol. 94, No. 14, pp. 303-309.

[15] K. Kreutz, M. Long, and H. Seraji: "Kinematic analysis of 7 DOF anthropomorphic arms," Proc. IEEE Intern. Conf. on Robotics and Automation, Cincinnati, May 1990, Vol. 2, pp. 824-830.

[16] R. Colbaugh, H. Seraji, and K. Glass: "Obstacle avoidance for redundant robots using configuration control," Journal of Robotic Systems, 1989, Vol. 6, No. 6, pp. 721-744.

[17] H. Seraji and R. Colbaugh: "Improved configuration control for redundant robots," Journal of Robotic Systems, 1990, Vol. 7, No. 6, pp. 897-928.

[18] Y. Nakamura and H. Hanafusa: "Inverse kinematic solutions with singularity robutness for robot manipulator control," ASME Journ. Dyn. Syst., Meas. and Control, 1986, Vol. 108, No. 3, pp. 163-171.

[19] C. W. Wampler and L. J. Leifer: "Applications of damped least-squares methods to resolved-rate and resolved-acceleration control of manipulators," ASME Journ. Dyn. Syst., Meas. and Control, 1988, Vol. 110, No. 1, pp. 31-38.

[20] H. Seraji: "Task-based configuration control of redundant manipulators," Journal of Robotic Systems, 1992, Vol. 9, No. 3.

[21] E. G. Gilbert and D. W. Johnson: "Distance functions and their application to robot path planning in the presence of obstacles," IEEE Journ. of Robotics and Automation, 1985, Vol. 1, No. 1, pp. 21-30.

[22] C. A. Klein and C. H. Huang: "Review of pseudoinverse control for use with kinematically redundant manipulators," IEEE Trans. Systems, Man and Cybernetics, 1983, Vol. SMC-13, No. 3, pp. 245-250.

[23] R. Schnurr, M. O'Brien, and S. Cofer: "The Goddard Space Flight Center Robotics Technology Testbed," Proc. Second NASA Conf. on Space Telerobotics, Pasadena, January 1989, Vol. 3, pp. 491-500.

1. Introduction

It has been recognized that robot arms with seven or more degrees-of-freedom (DOF) offer considerable dexterity and versatility over conventional six DOF arms [1]. These high-performance robot arms are kinematically redundant since they have more than the six joints required for arbitrary placement of the end-effector in the three-dimensional workspace. Kinematically redundant arms have the potential to approach the capabilities of the human arm, which also has seven independent joint degrees-of-freedom [2].

Although the availability of the "extra" joints can provide dexterous motion of the arm, proper utilization of this redundancy poses a challenging and difficult problem. Redundant manipulators have an infinite number of joint motions which lead to the same end-effector trajectory. This richness in the choice of joint motions complicates the manipulator control problem considerably. Typically, the kinematic component of a redundant manipulator control scheme must generate a set of joint angle trajectories, from the infinite set of possible trajectories, which causes the end-effector to follow a desired trajectory while satisfying additional constraints, such as collision avoidance, servomotor torque minimization, singularity avoidance, or joint limit avoidance. Developing techniques to simultaneously achieve end-effector trajectory control while meeting additional task requirements is known as the redundancy resolution/problem, since the motion of the manipulator joints must be "resolved" to satisfy both objectives.

Since redundancy is an important evolutionary step toward versatile manipulation, research activity in redundancy resolution and related areas has grown considerably in recent years, [e.g. 3-10]. For the most part, researchers have been working with a set of analytical tools based on linearized differential/kinematics models. Previous investigations of redundant manipulators have often focused on local/optimization for redundancy resolution by using the Jacobian pseudoinverse to solve the instantaneous relationship between the joint and end-effector velocities. Redundancy resolution based on the Jacobian pseudoinverse was first proposed by Whitney [3] in 1969, and the null-space projection improvement was proposed by Liegeois [4] in 1977. Over the past two decades, most researchers have continued to develop variations of the pseudoinverse approach primarily because the complex nonlinear forward and inverse kinematics models have deterred further investigations into new redundancy resolution schemes. A conceptually simple approach to control of redundant manipulator configuration has been developed recently based on augmentation of the manipulator forward kinematics [11]. This approach covers a wide range of applications and enables a major advancement in both understanding and developing new redundancy resolution methods. This paper presents the applications of the configuration control approach to a large class of redundant industrial robot arms with seven degrees-of-freedom.

The paper is organized as follows. Section 2 describes the kinematics of the 7 DOF Robotics Research arm and gives an overview of the configuration control approach. Various applications of the configuration control approach to the 7 DOF arm providing elbow control, collision avoidance, and optimal joint movement are given in Section 3. Section 4 describes the laboratory setup and the implementation of configuration control for real-time motion control of the 7 DOF arm, with elbow positioning for redundancy resolution. Conclusions drawn from this work are given in Section 5.

2. Motion Control of 7 DOF Arms

In this section, we describe the kinematics of the 7 DOF Robotics Research arm under study and discuss the motion control of this arm using the configuration control approach.

2.1 Kinematics of 7 DOF Robotics Research Arm

The Robotics Research (RR) arm is one of the few kinematically-redundant manipulators that is commercially available at the present time [12]. The Model K1207 RR arm has been purchased by JPL and similar models by other NASA centers for research and development of technologies applicable to the NASA Space Telerobotics Projects.

The Robotics Research arm has an anthropomorphic design with seven revolute joints, as shown in FIG. 1 and has nonzero offsets at all the joints. The arm is composed of a number of "modules" with roll and pitch motions. The shoulder joint with roll and pitch motions moves the upper-arm; the elbow joint with roll and pitch actions drives the forearm; and the wrist roll and pitch rotations together with the tool-plate roll move the hand. Essentially, the 7 DOF arm is obtained by adding the upper-arm roll as the 7th joint to a conventional 6 DOF arm design. The RR arm is supported by a pedestal at the base.

For kinematic analysis of the RR arm, coordinate frames are assigned to the links in such a way that the joint rotation .theta..sub.i is about the coordinate axis z.sub.i and the base frame {x.sub.0,y.sub.0,z.sub.0 } is attached to the pedestal. The two consecutive frames {x.sub.i-1,y.sub.i-1,z.sub.i-1.sub. with origin O.sub.i-1 and {x.sub.i,y.sub.i,z.sub.i } with origin O.sub.i are related by the 4.times.4 homogeneous transformation matrix [13] ##EQU1## where d.sub.i, a.sub.i, and .alpha..sub.i are the link length, joint offset and twist angle respectively, given in Table 1. The transformation that relates the hand frame {7} to the base frame {0} is obtained as ##EQU2## where R={r.sub.ij } is the 3.times.3 hand rotation matrix and p=[x, y, z].sup.T is the 3.times.1 hand position vector with respect to the base. One common representation of the hand orientation is the triple roll-pitch-yaw Euler angles (.rho., .beta., .gamma.). This three-parameter representation of hand orientation is subtracted from the hand rotation matrix R as follows [13]: ##EQU3## where Atan2 is the two-argument arc tangent function, and it is assumed that the pitch angle .beta. is not equal to or greater than .+-.90.degree.. Therefore, the hand position and orientation can be described by the 6.times.1 vector Y=[x, y, z, .rho., .beta., .gamma.].sup.T the three-dimensional workspace.

The 6.times.7 Jacobian matrix J.sub.v relates the 6.times.1 hand rotational and translational velocity vector ##EQU4## to the 7.times.1 joint angular velocity vector .theta. as V=J.sub..nu. .theta.. The hand Jacobian matrix is computed using the vector cross-product form [14] ##EQU5## where z.sub.i is the unit vector along the z-axis of link frame {i}, and P.sup.i is the position vector from the origin O.sub.i of link frame {i}to the origin of hand frame {7}. The Jacobian matrix in (4) can be partitioned as ##EQU6## where J.sub.vr and J.sub.vt designate the rotational and translational components of the Jacobian, that is, .omega.=J.sub..nu.r .theta. and .nu.=J.sub..nu.t .theta.. In order to relate the joint velocities to the rate of change of the roll-pitch-yaw angles that represent the hand orientation, the rotational Jacobian J.sub.vr (4) is modified to yield [13] ##EQU7## where the transformation matrix II (5) maps .omega. to ##EQU8## and det[II]=-cos.beta..noteq.0 since .beta..noteq..+-.90.degree..

From (4) and (5), we obtain the 6.times.7 hand Jacobian matrix ##EQU9## which relates Y to .theta. as Y=J.sub.e (.theta.).theta.. It is important to note that the computational efficiency can be creased significantly by exploiting the commonality of terms between the hand transformation matrix .sup.0 T.sub.7 and the hand Jacobian matrix J.sub.e.

Since the Robotics Research arm has seven joints, it offers one extra degree of joint redundancy for the task of controlling the six hand coordinates. The resolution of this single degree-of-redundancy is the subject of the next section.

A seven-degree-of-freedom robot arm with a six-degree-of-freedom end effector is controlled by a processor employing a 6-by-7 Jacobian matrix for defining location and orientation of the end effector in terms of the rotation angles of the joints, a 1 (or more)-by-7 Jacobian matrix for defining 1 (or more) user-specified kinematic functions constraining location or movement of selected portions of the arm in terms of the joint angles, the processor combining the two Jacobian matrices to produce an augmented 7 (or more)-by-7 Jacobian matrix, the processor effecting control by computing in accordance with forward kinematics from the augmented 7-by-7 Jacobian matrix and from the seven joint angles of the arm a set of seven desired joint angles for transmittal to the joint servo loops of the arms. One of the kinematic functions constrains the orientation of the elbow plane of the arm. Another one of the kinematic functions minimizing a sum of gravitational torques on the joints. Still another one of the kinematic functions constrains the location of the arm to perform collision avoidance. Generically, one of the kinematic functions minimizes a sum of selected mechanical parameters of at least some of the joints associated with weighting coefficients which may be changed during arm movement. The mechanical parameters may be velocity errors or position errors or gravity torques associated with individual joints.

FIG. 1 is a perspective view of a seven degree of freedom robot arm of the type controlled in the present invention.

FIG. 2 is a block diagram of an architecture embodying the present invention.

FIG. 3 is a diagram of the robot arm of FIG. 1 in one position of interest.

FIGS. 4a, 4b, 4c and 4d are diagrams of the robot arm of FIG. 1 in various positions of interest.

FIG. 5 is a diagram illustrating the coordinates employed in the detailed description of the invention below.

FIG. 6 is a graph illustrating the arm angle as a function of the number of sampling steps in one implementation of the invention.

FIGS. 7a and 7b are graphs illustrating joint angles of respective joints of the arm of FIG. 1 as a function of the number of sampling steps in an implementation of the invention.

FIGS. 8a and 8b are graphs illustrating a collision weighting factor and a collision avoidance critical distance, respectively, as a function of the number of sampling steps in an implementation of the invention.

FIG. 9 is a graph illustrating the variation of the arm angle as a function of the number of sampling steps in an implementation of the invention.

FIG. 10 is a graph illustrating various joint angles as a function of the number of sampling steps in an implementation of the invention.

FIG. 11 is a block diagram of a hardware system employed in carrying out one embodiment of the present invention.

FIGS. 12a, 12b, 12c, 12d, 12e, 12f and 12g are graphs illustrating errors in respective parameters of joint position and joint angle as a function of time in an implementation of the invention.

2.2 Configuration Control of the 7 DOF Arm

The configuration control approach introduced in [11] is a viable technique for resolution of redundancy and motion control of redundant manipulators. This approach is based on redundancy resolution at the position (i.e., task) level through augmentation of the manipulator forward kinematics by a set of user-defined kinematic functions .PHI.(.theta.)={.phi..sub.1 (.theta.), . . . .phi..sub.r (.theta.)}, where r is the number of redundant manipulator joints. This is contrast to the conventional Jacobian pseudoinverse methods which resolve the redundancy at the velocity (i.e., differential kinematics) level.

For the 7 DOF Robotics Research arm, the six hand position and orientation coordinates obtained in Section 2.1 are augmented by the scaler user-defined kinematic function .phi. to yield the 7.times.1 configuration vector X=[Y.sup.T,.phi.].sup.T. The redundancy resolution goal is then expressed as the additional task constraint

that will be accomplished simultaneously with the basic task of controlling the hand motion Y(.theta.)=Y.sub.d (t), where .phi..sub.d (t) and Y.sub.d (t) are the desired time variations of .phi.(.theta.) and Y(.theta.) respectively. Since the functional forms of the kinematic function and its desired time evolution are at the user's discretion, this approach can accommodate a wide range of redundancy resolution goals such as arm posture control (i.e. elbow positioning [15]), satisfaction of a task constraint (e.g. collision avoidance [16]), or optimization of a kinematic performance measure (e.g. minimal joint movement [17]). This formulation puts the redundancy resolution on the same footing as the end-effector task, and treats them equally within a common format. As a consequence, configuration control schemes ensure cyclicity (i.e., conservativeness) of arm motion, in contrast to pseudoinverse-based methods.

The configuration control approach can be implemented either as a dynamic or a kinematic control law. In the dynamic control implementation [11], the configuration controller produces the appropriate joint torques r(t) using a joint-space or a task-space formulation. In the kinematic control implementation [17], the controller generates the appropriate joint angle trajectories .theta..sub.d (t) which are then used as setpoints for the low-level joint servo-loops. In this paper, we adopt the kinematic configuration control approach due to ease of implementation. Since the Robotics Research arm has non-zero joint offsets, there are no closed-form analytical inverse kinematic solutions and therefore a differential kinematics approach must be adopted. The augmented differential kinematics model of the arm is obtained as ##EQU10## where J.sub.e (.theta.) is the 6.times.7 hand Jacobian matrix obtained Section 2.1, ##EQU11## is the 1.times.7 Jacobian matrix associated with the kinematic function .phi., and J(.theta.) is the 7.times.7 augmented Jacobian matrix*Note that when .phi.(.theta.) is defined as the gradient of an objective function to be optimized, J becomes the "extended" Jacobian proposed by Baillieul [9] for redundancy resolution. Therefore, the extended Jacobian method is retrieved as a special case of the configuration control approach when the latter is implemented as a differential kinematic controller with an optimization additional task. Assuming det=0, equation (7) is solved in discrete-time as

where N is the sampling instant, .theta. and X are the actual values while .theta..sub.d and X.sub.d are the desired values. Note that the use of X in (8) corrects for linearization errors due to differential kinematics. The next desired joint angle is then computed from .theta..sub.d (N+1)=.theta..sub.d (N)+.DELTA..theta..sub.d (N), and is sent as a setpoint to the joint servo-loops for tracking.

The configuration control framework allows the user to specify multiple additional tasks to be accomplished simultaneously with the basic task of hand motion. Suppose that r(>1) additional task constraints are defined as .phi..sub.i (.theta.)=.phi..sub.di (t), i=1, . . . , r. Then, the augmented differential kinematics model becomes ##EQU12##

The optimal (i.e. damped least-squares) solution of the over-determined set of equations (9) that has the smallest joint velocity .parallel..theta..parallel. is given by [17-19] as

or in discrete-time implementation

where W=diag{W.sub.e,W.sub.c } and W.sub.v are the (6+r).times.(6+r) and 7.times.7 matrices of task error and joint velocity weighting factors specified by the user. Note that when W.sub.v =0, r=1 and det[J].noteq.0, equation (8) is retrieved from (10). The acquired solution (10) minimizes the scalar cost function

where E.sub.e =Y.sub.d -J.sub.e .theta. and E.sub.c =.PHI..sub.d -J.sub.c .theta. are the basic task and additional task velocity errors. The task weighting factors W.sub.e, W.sub.c enable the user to assign priorities to the different basic and additional task requirements. The joint velocity weighting factor W.sub.v allows the user to suppress large joint velocities near singularities, at the expense of small task errors. This is particularly important in redundant arm control because the complicated nature of the augmented Jacobian singularities deters any analytical characterization of the singular configurations.

An architecture corresponding to Equations (8) and (10) is illustrated in FIG. 2.

The ability to change the weighting factors on-line based on the task performance provides a general framework for incorporation of multiple constraints in redundant arm control. Equation (10) can be written as ##EQU13## where J.sub.ci is the Jacobian related to .phi..sub.i. Equation (13) shows the contribution of each additional task constraint to the optimal joint motion. This formulation can be used to "blend" multiple additional tasks or to "switch" between different additional tasks by proper selection of their weighting factors. For instance, for the 7 DOF arm, we can switch between elbow control and collision avoidance during task execution so that when the arm is far from workspace obstacles, w.sub.el =1 and w.sub.co =0 and direct elbow control will take precedence. As soon as potential collision is detected (from world model or sensory data), the collision avoidance goal becomes dominant and the corresponding weighting factor w.sub.co creases as the arm gets closer to the obstacle, at the expense of loss of direct elbow control by setting w.sub.el =0. This feature is illustrated in Section 3, and is discussed in detail in [17].

The configuration control formulation can be used to meet diverse additional task constraints for redundancy resolution [20]. For instance, the redundancy can be used to control directly: a geometrical variable (such as coordinates of a point on the arm), a physical variable (e.g. a joint gravity torque), or a mathematical function (such as projected gradient of an optimization function). In the next section, we demonstrate three applications of configuration control for motion control of the 7 DOF Robotics Research arm. In each application, the single degree-of-redundancy is utilized to accomplish a different additional task objective; namely, elbow control, collision avoidance, and optimal joint movement.

3. Graphics Simulation of 7 DOF Arm Control

This section describes some of the simulations of the configuration control scheme for redundancy resolution and kinematic motion control of the Model K1207 7 DOF Robotics Research arm.

The Silicon Graphics IRIS 4D70-GT is a Workstation with both high-speed computing and graphics capabilities, and is used in this simulation study. A three-dimensional color rendering of the Robotics Research K1207 arm is built with a set of primitives that use the IRIS "C" language graphics library. When the program is run, it initially displays the arm and its state information on the IRIS screen as shown in FIG. 3. The rendering of the arm is centered on the screen with the joint angles, Cartesian hand coordinates, arm angle, manipulability indices, and trajectory time information displayed in a table in the lower left corner, the redundancy control mode is displayed in the upper left, and the user menu box (not shown) appears as needed in the upper right corner of the screen. Since the zero configuration of this particular arm is a singular configuration, the arm shown in this figure is in the user-defined "home" configuration. Simulation software is written in "C" and animates the kinematic control results as they are computed so as to move the arm continuously on the screen. FIGS. 4(a)-(d) show the evolution of the arm as it moves from an initial to a user-specified final configuration. The control law is computed and used to continuously change the arm configuration and the state information in the lower left corner of the screen is updated at every sampling instant. A simple cycloidal trajectory generator provides point-to-point straight-line Cartesian paths based on Cartesian goal points input by the user either from the keyboard or from the mouse. Alternatively, the user may use the mouse in teleoperation mode to directly control the arm in joint or Cartesian space, activating different degrees-of-freedom with the mouse buttons. Using a simple stacking feature, the user may save a sequence of intermediate points to a file for a later run. The user can also select from a number of redundancy resolution schemes for each task, adjust optimization parameters or obstacle location, plot the results of each run, or save the data for later analysis. The user may also rerun the simulation program, adjusting his viewing location and perspective on each run.

This interactive graphics simulation environment serves as an essential tool for development and validation of new control schemes for redundant 7 DOF arms. The IRIS also allows the user to simulate the robot workspace graphically and plan the task sequence. It can then be used for "task preview" by simulating the robot control algorithms and animating the task scenario. In this mode of operation, the IRIS can be used for operator training and rehearsal, prior to actual task execution. This preview mode is important in dealing with redundant arms, since it enables the user to explore various alternatives for redundancy resolution and can reveal unexpected behavior of the robot.

Several configuration control schemes for the 7 DOF Robotics Research arm have been designed and verified by simulation on the IRIS. The case studies presented here are samples selected from an extensive computer simulation study which was carried out to test the performance of the proposed control schemes. These cases are chosen for presentation because they illustrate the flexibility and versatility of the configuration control approach to redundant manipulators. Three case studies are presented in this section, namely: elbow control, collision avoidance, and optimal joint movement.

3.1 Elbow Control

The presence of a redundant joint in the 7 DOF Robotics Research arm results in infinite distinct arm configurations with the same/hand position and orientation. This leads to a physical phenomenon known as "self-motion" or "orbiting," which is a continuous movement of the joints that leaves the hand motionless. The self-motion of the RR arm corresponds to the elbow point E traversing a circle around the line SW joining the shoulder S to the hand W, without moving the hand frame. Thus the elbow position, together with the hand coordinates, forms a complete representation of the geometrical posture (i.e., the physical shape) of the whole arm in the entire workspace. One natural representation of the elbow position is the "arm angle" .PSI. defined as the angle between the arm plane SEW and a reference plane, such as the vertical plane passing through the line SW, [15], as depicted in FIG. 5. The angle .PSI. succinctly characterizes the self-motion of the arm and uniquely specifies the elbow position for a given hand frame. Other viable representations of the elbow position are the x, y, or z Cartesian coordinates of the elbow (i.e., E.sub.x, E.sub.y, or E.sub.z) in the base frame. The choice of .PSI. or a particular elbow coordinate is clearly dictated by the task that the arm is required to perform. In a recent paper [15], simple and computationally efficient methods of computing the arm angle .PSI. and the associated constraint Jacobian J.sub..PSI. are given, where .PSI.=J.sub..PSI. .theta.. Following , .PSI. and J.sub..PSI. are computed from ##EQU14## where E and W are the Jacobian matrices related to the elbow and the wrist linear velocities and other symbols are defined in FIG. 5, with `caret` designating a unit vector.

The user interacts with the IRIS Workstation by using the keyboard to enter the desired target position and orientation of the hand (x.sub.f,y.sub.f, z.sub.f, .rho..sub.f, .beta..sub.f, .gamma..sub.f) and the desired final arm angle .PSI..sub.f, as well as the duration of motion .tau. and the sampling period .DELTA.t. The hand frame can alternatively be input using the mouse which essentially emulates a 6 DOF cursor. The trajectory generator software then computes smooth cycloidal trajectories for these seven variables to change them from their initial values (x.sub.0, Y.sub.0, z.sub.0, .rho..sub.0, .beta..sub.0, .gamma..sub.0, .PSI..sub.0) to the final values in the specified time duration. For instance, a typical cycloidal trajectory for the desired arm angle .PSI..sub.d is ##EQU15##

Note that only the ratio of elapsed time to motion time .tau./t is needed for the trajectory generator. In discrete-time implementation, the number of samples during motion is equal to .DELTA.t/.tau.. Note that, using the cycloidal functions, the hand moves on a straight-line path; since we obtain ##EQU16##

In this simulation study, the Robotics Research arm is initially at the joint configuration .theta.(0)=[-90.degree.,-43.3.degree.,0.degree.,-101.degree.,-180.degree., -54.3.degree.,-90.degree.].sup.T This yields the initial hand configuration P.sub.0 ={X=0,y=90,z=0,.rho.=-90.degree.,.beta.=0.degree.,.gamma.=0.degree.} relative to the base frame and the initial arm angle .PSI..sub.0 =0.degree., where the position coordinates are in centimeters and the angles are in degrees. The hand is commanded to trace a triangle by making the successive moves: ##EQU17## where P.sub.1 ={50, 50, 50, 0.degree., 0.degree., 0.degree.}, .PSI..sub.1 =-90.degree., .tau..sub.1 =2.0

P.sub.2 ={-50, 50, 50, 0.degree., 0.degree., 0.degree.}, .PSI..sub.2 =+45.degree., .tau..sub.2 =4.0, .tau..sub.3 =1.0

while .DELTA.t=0.025 in all cases and the unit of time is the second. The kinematic configuration control scheme is used to compute the required joint motions that result in the commanded hand and arm angle trajectories. Note that J.sub..PSI. from (15) is used in (10), and we set W.sub.e =I.sub.6, W.sub.c =1 and W.sub.v =0 since no arm singularities are encountered during the motion. FIG. 6 shows the executed motion of the elbow, in which the arm angle changes from 0.degree. to -90.degree. and then to +45.degree. during the hand motion. The variations of the joint angles .theta..sub.1, . . . , .theta..sub.7 to achieve the commanded arm motion are shown in FIGS. 7a-7b. These figures illustrate that all the seven joint angles return to their initial values at completion of the task. Thus, the initial and final arm configurations are identical and the robot has executed a cyclic (i.e., conservative) motion under configuration control.

3.2 Collision Avoidance

One of the advantages of the 7 DOF arm is the potential to use the "extra" DOF to maneuver in a congested workspace and avoid collision with obstacles by configuring the arm appropriately without perturbing the hand trajectory. In this formulation, all workspace obstacles are enclosed in convex volumes and each volume defines a "space of influence" (SOI) for the control law. In this study, the SOIs are assumed to be spheres in the three-dimensional workspace, but extension to other geometrical shapes is possible using distance functions [21]. In the configuration control framework, the collision avoidance requirement is formulated as a kinematic equality/constraint

where d.sub.c (.theta.)=.parallel.X.sub.c (.theta.)-X.sub.0.parallel. is the critical distance between the arm and the obstacle, X.sub.0 is the position of the SOI center, r.sub.0 is the radius of the SOI, and X.sub.c is the position of the "critical point" on the arm currently at minimum distance from the obstacle. Note that the location of the critical point X.sub.c and the critical distance d.sub.c are both configuration dependent and are continuously recomputed as described [16]. Two modes of operation are possible:

Case One d.sub.c (.theta.).gtoreq.r.sub.0 : In this case, the equality constraint (17) is satisfied and the entire arm is outside the obstacle SOI. Therefore, the constraint is active/and the manipulator redundancy can be used to achieve other additional tasks, such as those in Sections 3.1 and 3.3.

Case Two d.sub.c (.theta.)<r.sub.0 : In this case, the equality constraint (17) is active/and the arm is inside the obstacle SOI. Thus, the redundancy is utilized to avoid collision with the obstacle by inhibiting the motion of the critical point on the arm in the direction toward the obstacle. To this end, (17) is replaced by the equality constraint d.sub.c (.theta.)=r.sub.0, and the constraint Jacobian is obtained as ##EQU18## The configuration control scheme can now be employed to achieve the desired hand motion as well as collision avoidance. However, in this formulation, the additional task constraint is either "on" or "off." This can lead to an undesirable rapid switching between the "on" and "off" conditions thus resulting a "chattering" phenomenon on the SOI boundary. Furthermore, switching between the collision avoidance task in Case Two and another additional task (such as elbow control) in Case One can cause discontinuity problems. The variable task weighting scheme alleviates both the chattering and discontinuity problems. In this scheme, the weighting factors w.sub.el and w.sub.co for the elbow control and collision avoidance tasks Cases One and Two are chosen as functions of the critical distance d.sub.c (.theta.), instead of predefined constants. The use of variable weighting factors for the additional tasks allows the collision avoidance constraint to be incorporated gradually with the basic task, and furthermore circumvents the discontinuity problem in switching between different additional tasks.

In the simulation study, the Robotics Research arm is initially at the joint configuration .theta.(0)=[-90.degree., -44.7.degree., 0.degree., -89.4.degree., 0.degree., -135.8.degree., 90.degree.].sup.T. The task is to move the hand on a straight-line from the initial location P.sub.0 =[0, 90,0, 0.degree., 0.degree., 90.degree.] to the target location P.sub.1 =[-90, 30, -30, 0.degree., 0.degree., 90.degree.] .tau.=8.75 seconds with .DELTA.t=0.025 such that during motion the arm avoids collision with a workspace obstacle. The obstacle is enclosed by two SOIs: an inner SOI which touches the actual obstacle boundary, and an outer SOI which allows for some "buffer." The inner and outer SOIs are concentric spheres with centers at z.sub.0 =13.3] and radii r.sub.i =8.5 cm and r.sub.0 =37.5 cm. Each hand coordinate is required to track a cycloidal trajectory as described in Section 3.1. Initially, before the obstacle is encountered, it is required to keep the arm angle constant at its initial value of .PSI.=0.degree. to resolve the redundancy. When the obstacle is encountered, the redundancy is used for collision avoidance at the expense of loss of elbow control. After the obstacle encounter, the arm angle should remain constant. In this simulation, when the arm is outside the outer SOI (Case One), we set w.sub.el =1 and w.sub.co =0 to achieve elbow control. As soon as potential collision is detected (Case Two), the redundancy resolution goal switches smoothly to collision avoidance by setting w.sub.el =0 and increasing wco as an inverse square function of d.sub.c (.theta.), that is ##EQU19## for d.sub.c .ltoreq.r.sub.O. Using (18), when the arm is at the outer SOI boundary (d.sub.c =r.sub.o), we have w.sub.co =0; and as the arm moves closer to the obstacle, w.sub.co creases rapidly so that w.sub.co .fwdarw..infin. as d.sub.c .fwdarw.r.sub.i. The variations of w.sub.co and the critical distance d.sub.c (.theta.) are shown in FIGS. 8a-8b. It is seen that the increase in w.sub.co has hindered motion of the arm inside the inner SOI, thus ensuring that collision avoidance is successfully accomplished throughout the arm motion. The variation of the arm angle .PSI.(.theta.) is shown in FIG. 9, and illustrates that the arm angle is held constant when the obstacle is not encountered, as expected.

3.3 Optimal Joint Movement

In this case study, the redundancy resolution goal is to distribute the hand motion among the joints in such a way that a weighted sum of joint movements is kept at minimum. Toward this end, the optimization objective function is selected as ##EQU20## where k.sub.i is the weighting factor for joint i movement and [.theta..sub.i (t)-.theta..sub.i (0)].sup.2 denotes the current deviation of joint angle .theta..sub.i (t) from its initial position .theta..sub.i (0). The objective function G(.theta.) (19) represents the total instantaneous potential energy stored in seven hypothetical springs attached to the robot joints with stiffness coefficients {k.sub.i } and natural lengths {.theta..sub.i (0)}. By choosing appropriate numerical values for {k.sub.i }, the user can resolve the hand motion among the joints such that the joints with larger k move less at the expense of those with smaller k. The ability to penalize individual joint movement may also lead to a desirable distribution of joint torques for a given hand trajectory [17]. The condition for optimality of G(.theta.) subject to the end-effector constraint Y=Y(.theta.) has been found [17] to be ##EQU21## where N.sub.e is a 1.times.7 vector which lies in the null-space of the hand Jacobian J.sub.e, that is J.sub.e N.sub.e.sup.T =0. This implies that for optimality, the projection of the gradient of the objective function onto the null-space of the hand Jacobian must be zero. To achieve optimal joint movement, the kinematic function is defined as ##EQU22## and its desired value is set to .phi..sub.d (t)=0, to represent the optimality condition. The configuration control approach can then be applied to obtain the joint trajectories which cause the hand to attain the commanded motion with an optimal total joint spring energy.

In this simulation study, the arm is initially at .theta.(0)=[-89.1.degree., -32.1.degree., -45.degree., -91.5.degree., -47.degree., -126.6.degree., 29.7.degree.].sup.T giving the initial hand coordinates as P.sub.0 =[50, 70, 30, 0.degree., 0.degree., 90.degree.]. The hand is commanded to move on a straight-line to the target location P.sub.1 =[-50, 70, -30, 90.degree., 0.degree., 0.degree.] .tau.=2.5 seconds with .DELTA.t=0.025, while the arm redundancy is used to achieve the hand trajectory with optimal joint movement. The user types in the stiffness coefficients of the joint springs {k.sub.i }={20,1,1,1,1,1,1}, where a large value for k.sub.1 dictates the heavy penalty on joint 1 movement. The program then computes ##EQU23## and augments the hand Jacobian J.sub.e to obtain J. The required joint trajectories are then found by using (10) with W.sub.e =I.sub.6, W.sub.c =1, W.sub.v =0. The variations of the joint angles are given in FIG. 10, which shows that the first joint with a large weighting factor has moved considerably less than the other joints, as desired.

3.4 Alternative Redundancy Resolution Goals

In addition to the redundancy resolution goals discussed in Section 3.1-3.3, the user can select other criteria from a menu presented to him on the IRIS screen. This menu of redundancy resolution options is an area of current research, and analytical techniques that are being developed are added to the menu for test and validation. In this section, we shall present some of the items on the redundancy resolution menu.

(i) Joint Locking: The user can select a particular joint, say .theta..sub.j, to be locked during the commanded hand motion. In this case, the relationship .theta..sub.j (t)=.theta..sub.j (0) is treated as the additional task, with J.sub.c =[0, . . . ,1, . . . 0]. The configuration control approach then attempts to move the hand using the remaining six joints while keeping .theta..sub.j constant. This is equivalent to deleting the j th column of J.sub.e to obtain the 6.times.6 matrix J.sub.e and then solving Y=J.sub.e .theta. for the remaining six joints .theta.. The acquired solution for .theta. depends on the locked value of .theta..sub.j, namely .theta..sub.j (0). Note that for some selections of .theta..sub.j, the resulting Jacobian J.sub.e is always singular, which implies that from a physical point of view, the hand position and orientation can not be changed arbitrarily while .theta..sub.j is locked. The joint locking option is useful in investigating the fail-tolerance feature of the robot joints, i.e., preservation of hand motion despite a joint failure. In addition, this option can be exercised when the operator only wishes to perform the basic task of hand placement and orientation.

(ii) Joint Limit Avoidance: The joints of any robot have rotational limitations that can typically be expressed as .alpha..sub.j .ltoreq..theta..sub.j .ltoreq..beta..sub.j, where .alpha..sub.j and .beta..sub.j are the lower and upper joint limits. One of the applications of redundancy is to resolve the hand motion among the joints such that their limits are not violated. The joint limit equality constraint is treated within the configuration control framework in a similar manner to the obstacle avoidance constraint in Section 5.2. The user can select the joint limits and command hand motion, and examine the robot performance. Since inequality constraints are treated as equality conditions for redundancy resolution, for some joint angles the augmented Jacobian can be singular and the problem may not have a solution.

(iii) Manipulability Maximization: A common objective function to be maximized by the utilization of redundancy is the hand manipulability index [15] defined as .mu.(.theta.)=.sqroot.det. This scalar index vanishes at the hand singular configurations where J.sub.e (.theta.) is rank-deficient. Therefore, maximizing .mu.(.theta.) during a prescribed hand motion leads to arm configurations which avoid the hand Jacobian singularities as much as possible. This solution can be obtained by following Section 5.3 with G(.theta.) replaced by .mu.(.theta.). Note that in this case .sub..differential..theta..sup..differential..mu. must be computed numerically.

4. Real-Time Control of the 7 DOF Arm

In this section, we describe the implementation and experimental validation of the configuration control scheme on the 7 DOF Robotics Research arm. The laboratory setup is described first, followed by a description of a simple experiment. In this experiment, the configuration control approach is implemented for real-time control of the Robotics Research arm, with elbow positioning for redundancy resolution.

The Robotics Research Laboratory at JPL consists of one Model K1207 7 DOF arm and control unit from the Robotics Research Corporation, a VME-based chassis with MC 68020 processor boards, two 3 DOF industrial rate joysticks, a motorized lathe-bed, and a Silicon Graphics IRIS Workstation. The arm pedestal is mounted on a mobile platform of the lathe-bed which provides one additional degree-of-freedom. The arm control unit has an electronic servo-level interface, which allows the user to communicate directly with the joint servomotors at a sampling frequency of f.sub.s =400 Hz, i.e., a sampling period of T.sub.s =2.5 ms. The joint servomotors can be commanded in any of the four modes: position, velocity, torque, and current. This makes it possible to operate the arm under either kinematic or dynamic control schemes, and therefore provides a testbed for validation of different 7 DOF control laws. In the present implementation, all seven joints are commanded in the position mode.

The hardware diagram of the experimental setup is shown in FIG. 11. The IRIS can operate in two different modes. First, it creates an interactive graphics simulation environment for analysis and control of the 7 DOF arm, as discussed in Section 3. Second, the IRIS serves as the graphical user interface through which the operator interacts with the actual arm in real-time and issues motion commands in joint or task space. Using this dual-mode functionality, the IRIS can be used initially in "preview mode" for animating the task scenario, and subsequently in "execution mode" to command the arm to duplicate the simulated motion. The software which provides graphical user interface and simulation capabilities resides on the IRIS.

The VME-based real-time robot control system receives commands from the IRIS to move the actual arm. This is the part of the system which handles all real-time operations including computation of control laws and transmission of appropriate signals to the multibus-based arm control unit. The control unit dispatches the commands for execution to the seven joint motors of the arm to perform the task. The VME chassis configuration contains five CPU boards that communicate through a shared memory board to perform all the necessary computations to provide real-time manipulator control. The first CPU interfaces with the high-level software residing on the IRIS, receives commands from the operator and obtains acknowledgment and state information from the low level after command execution. This processor also serves as the master by scheduling the synchronous operations of the slave processors that perform the real-time computations. The second CPU performs real-time trajectory generation and kinematic computations. This includes generating the desired end-effector trajectories and computing the necessary kinematic and Jacobian transformations. The second CPU also accesses and updates the world model and performs computations to resolve the manipulator redundancy. The third CPU is designated to perform all the computations associated with invoking various dynamic control algorithms (not used at present). The fourth CPU solely communicates with the arm control unit by executing the arm interface driver at every 2.5 milliseconds. A two-card VME-to-multibus adaptor set from the BIT3 Corporation is employed to provide shared memory servo interface with the arm control unit at high speed. The role of the driver is to perform handshake with the arm control unit and to convert data into appropriate format for usage. Some of its features include translating data representation in the multibus to VMEbus format and vice versa and safety checking to avoid hitting physical joint limits and collision with the floor. The fifth CPU hosts various drivers that manipulate the shared memory board which contains global memory formation, read in joystick inputs, control the motorized lathe-bed, and interface with other devices such as a force/torque sensor and a gripper. All software executing on the VME environment is written in the "C" language. Code is developed on a SUN 3/60 UNIX computer utilizing SUN's "C" compiler and Wind River's VxWorks/Wind real-time library. The code is then downloaded through Ethernet to the target processor boards for immediate execution.

To perform initial experiments, a computer program is written for trajectory generation, kinematic computations, and arm interface via the driver. At the present time, all of these computations are performed sequentially on one MC68020 processor with a cycle period of 25 milliseconds. First, a simple cycloidal Cartesian-space trajectory is generated based on the operator's input of the desired arm goal configuration. The 7.times.1 arm configuration vector X includes the 6.times.1 vector Y of position and orientation coordinates of the hand and the scalar arm angle .PSI. for redundancy resolution. At each computation cycle, the output from the trajectory generator is the 7.times.1 vector of Cartesian increments .DELTA.X. The 7.times.7 augmented Jacobian J is also computed which embodies the redundancy resolution goal, namely .PSI. control in this case. The Jacobian is then inverted and multiplied by the Cartesian increments to generate the seven joint increments .DELTA..theta.=J.sup.-1 .DELTA. X. Finally, the joint setpoints are computed by adding the increments to the current joint angles and are dispatched to the arm interface driver to move the arm under position mode.

In the present implementation, because of the slow sampling rate of 25 milliseconds, the Jacobian matrix J is computed using the desired joint angles instead of the actual joint angles. In addition, the Cartesian increment .DELTA.X is calculated using the difference between the two consecutive desired Cartesian setpoints, not by subtracting the actual Cartesian values from the desired Cartesian setpoints. To improve performance, we plan to increase the servo rate by splitting the algorithm on two MC68020 processors. The first processor will be designated solely to communicate with the arm at every 2.5 milliseconds (running the driver as CPU 4). The second processor will perform cycloidal trajectory generation and Jacobian computation and version. The first processor will then obtain the joint setpoints at every 25 milliseconds, but will linearly interpolate these points into ten via-points which are then sent one at a time to the arm controller every 2.5 milliseconds.

In the experiment, the Robotics Research arm is initially at the predefined Cartesian "home" ("cstart") position with the end-effector coordinates (x, y, z, roll, pitch, yaw) and arm angle .PSI. as X(0)=[-900, 297, 316, 0.degree., 0.degree., 44.degree., 60.degree.] measured relative to a fixed reference frame attached to the shoulder, where the lengths are millimeters and the angles are in degrees. This position corresponds to the joint angular values of .theta.(0)=[-63.degree., -61.degree., 78.degree.,-88.degree., 79.degree., -85.degree., 159.degree.] which is away from the arm singular configuration. Data are collected as all seven Cartesian coordinates move simultaneously from the "cstart" position to the user-specified goal position X(.tau.)=, where the motion duration .tau. is chosen as 10 seconds. This corresponds to the hand translational motion of 866 millimeters. Preliminary experimental results which demonstrate trajectory tracking are presented in FIGS. 12a-12g. For each end-effector coordinate (x, y, z, roll, pitch, yaw) and the arm angle .PSI., the tracking-error is computed by using the difference between the actual trajectory and the desired trajectory. Note that the maximum error occurs in the middle of the trajectory, i.e. at time t=.tau./2=5 seconds. This is because for a cycloidal position trajectory, the velocity is at its peak in the middle of the trajectory, which attributes to the maximum occurrence of linearization errors. From FIGS. 12a-g, in each positional coordinate, the maximum tracking-error does not exceed 16 millimeters, and in each orientational coordinate, the maximum error is less than 3 degrees. Therefore, the experimental results demonstrate the efficacy of configuration control for the 7 DOF arm. Note that the tracking performance will be improved considerably when the computations are split on two processors so that the joint setpoints are updated every 2.5 milliseconds.

5. Conclusions

The problem of motion control of 7 DOF arms is addressed in this paper. To provide dexterous motion of the arm, the configuration control approach is adopted in which the redundancy in joint space is effectively transferred into task space by adding a user-defined kinematic constraint to the end-effector task. The configuration control schemes are robust when singularities are encountered and allow the user to assign appropriate priorities to the task requirements. In this paper, applications of configuration control approach to motion control of the 7 DOF Robotics Research arm are described. Diverse redundancy resolution goals such as elbow control, collision avoidance and optimal joint movement are demonstrated using computer graphics simulations. A simple laboratory experiment on configuration control of the Robotics Research arm is described, and experimental results are presented.

In contrast to Jacobian pseudoinverse methods which resolve the redundancy in joint space, the configuration control approach provides direct control of the manipulator in task space, where the task is performed. Furthermore, unlike pseudoinverse methods, the redundancy resolution goal is not restricted to optimization of a kinematic objective function. Finally, in contrast to pseudoinverse methods which do not ensure cyclicity of motion [22], the configuration control approach guarantees cyclic (i.e., conservative) motions of the manipulator, which is particularly important for repetitive tasks. By way of an example, in a 7 DOF arm under pseudoinverse control, the elbow is allowed to move without restraint during the hand motion, and the arm assumes different configurations for a closed-path hand movement [23]; whereas under configuration control, both of these problems are circumvented.

Current work is focused on expanding the redundancy resolution goals, improving the computational efficiency, and performing further experiments on real-time motion control of the 7 DOF Robotics Research arm.

While the invention has been described in detail by specific reference to preferred embodiments thereof, it is understood that variations and modifications may be made without departing from the true spirit and scope of the invention.