The development of programmed manipulators or robots is progressing at a rapidly increasing pace in the United States as elsewhere in the world. Currently, the industrial robots, such as taught by Hohn in U.S. Pat. No. 3,909,600, employ dynamic control laws which ignore the complex nonlinear dynamics of the various moving elements which result in less than optimal response. The computational bottleneck of many advanced dynamic control schemes is an algorithm for computation of the actuator torques and/or forces required to produce the desired joint accelerations, for a given set of joint velocities and angles.
The dynamics of an "n" link manipulator can be described by the following set of n nonlinear differential equations. EQU T=J(q)q+f(q,q)+g(q) (1)
where
______________________________________ q = n .times. 1 vector of joint angles (displacements) J(q) = n .times. n inertia matrix f(q,.q) = n .times. 1 vector of centripetal and Coriolis torques (forces) g(q) = n .times. 1 vector of gravity torques (forces) T = n .times. 1 vector of actuator torques (forces) ______________________________________
For a typical 6 link manipulator, the functions J, f and g in Equation 1 are very complex, nonlinear expressions. Because of this complexity, most proposed control schemes are based on breaking down or decoupling Equation 1 into n linear differential equations. The decoupling feedback is given by: EQU T=J(q)u+f(q,q)+g(q) (2)
where EQU u=n.times.1 vector of desired joint accelerations
Equation 2 transforms the original system given by equation 1 into the following linear, decoupled system. ##EQU1##
The system described by Equation 3 is easily controlled by any standard control technique.
Equation 2 constantly changes when the manipulator moves, because J, f and g are dependent on the joint angles. Thus to achieve decoupling at all times, the decoupling torques T must be continuously recomputed in real time. In order to avoid introduction of undesirable dynamic effects the required frequency of computation is about 100 times per second. This is the bottleneck in any control scheme, because it involves an excessive number of floating point additions and multiplications.
Numerous schemes for computation of Equation 2 have been proposed during the last fifteen years. A comprehensive comparison of these techniques is presented by J. M. Hollerbach in his article "A Recursive Lagangian Formulation of Manipulator Dynamics and a Comparative Study of Dynamics Formulation Complexity" IEEE Transactions on Systems, Man and Cybernetics, Vol. 10, No. 11, November 1980, pp 730-736. The most elegant formulation of the problem, based on Lagrange's equation, is due to J. J. Uicker "On the Dynamic Analysis of Spatial Linkages Using 4.times.4 Matrices" Ph. D. Dissertation, Northwestern University, August 1965, and M. E. Kahn "The Near-Minimum-Time Control of Open Loop Articulated Kinematic Chains", Stanford University A. I. Memo 177 September 1969. Unfortunately, the (Uicker/Kahn) formulation is unsuitable for real-time computation, because for a typical six link manipulator it involves about 118,000 additions and multiplications per trajectory point. J. Y. S. Luh, M. M. Walker and R. P. C. Paul, in their article "On Line Computational Scheme for Mechanical Manipulators" ASME Journal of Dynamic Systems, Measurement and Control, Vol. 102, No. 2, June 1980, pp 69-76 disclosed that the execution time of Equation 2 for a six link manipulator using Lagrange's equation is 7.9 seconds. This is approximately 790 times longer than the 0.01 second goal for current systems. The fastest exact computational scheme known today is the recursive Newton-Euler technique disclosed by Luh et al in the above article. This scheme first computes the velocities and the accelerations of the manipulator links, going from the manipulator base to the last link. It then computes the torques and forces acting on the links, going from the last link back to the base. All the computations are performed in link coordinates, which significantly reduces the number of coordinate transformations required and reduces the number of arithmetic operations to 1590 for a six link manipulator. This techniques requires 0.0335 seconds to perform the necessary arithmetic operations. Hollerbach concluded that only very minor improvements can be made to the recursive Newton-Euler formulation disclosed by Luh et al. However, Hollerbach's conclusion only applies to formulations which are very general and can handle manipulators with any number of links and any configuration.
The dynamic manipulator control described herein is based on the fact that most industrial manipulators are relatively simple geometrically and have from 4 to 7 links. Its computational time, in high level languages, is about five or ten times less than that using the technique taught by Luh et al depending on the implementation. The proposed control divides the manipulator into two parts. The first three links (counting from the base) are modeled using classical (non-matrix) Lagrange's equations, with velocities computed in link coordinates. The remaining links (i.e., the wrist) are modeled using recursive Newton-Euler equations.