US 5,546,508 AGrant
Controlling Flexible Robot Arms Using High Speed Dynamics Process
Issue Date:1996-08-13
•45 Claims
•7 Drawing Sheets
Abstract
A robot manipulator controller for a flexible manipulator arm having plural bodies connected at respective movable hinges and flexible in plural deformation modes corresponding to respective modal spatial influence vectors relating deformations of plural spaced nodes of respective bodies to the plural deformation modes, operates by computing articulated body quantities for each of the bodies from respective modal spatial influence vectors, obtaining specified body forces for each of the bodies, and computing modal deformation accelerations of the nodes and hinge accelerations of the hinges from the specified body forces, from the articulated body quantities and from the modal spatial influence vectors. In one embodiment of the invention, the controller further operates by comparing the accelerations thus computed to desired manipulator motion to determine a motion discrepancy, and correcting the specified body forces so as to reduce the motion discrepancy. The manipulator bodies and hinges are characterized by respective vectors of deformation and hinge configuration variables, and computing modal deformation accelerations and hinge accelerations is carried out for each one of the bodies beginning with the outermost body by computing a residual body force from a residual body force of a previous body and from the vector of deformation and hinge configuration variables, computing a resultant hinge acceleration from the body force, the residual body force and the articulated hinge inertia, and revising the residual body force modal body acceleration.
Metadata
Assignee
- The United States of America as represented by the Administrator of the National Aeronautics and Space Administration
Inventors
- Abhinandan Jain
- Guillermo Rodriguez
Application Information
Application Number:US 8628610
Filing Date:1992-04-03
Priority Date:1992-04-03
Art Unit:239
Classifications
IPC:
G05B 1940
Field of Search:
39595;96;97;98
Patent Drawings (7 sheets)
Description
BACKGROUND OF THE INVENTION
1. Technical Field
The invention relates to robot manipulators and more particularly to a method and apparatus for controlling robot arms having flexible links using a high speed recursive dynamics algorithm to solve for the accelerations of link deformation and hinge rotations from specified body forces applied to the links.
2. Background Art
Controlling robot manipulator arms is a well-known problem and has been described in a number of publications. The invention herein will be described with reference to the following publications by referring to each publication by number, such as Ref. [1], Ref. [2], or simply [1] or [2], for example.
References
[1] Rodriguez, G., Kreutz, K., and Jain, A., "A Spatial Operator Algebra for Manipulator Modeling and Control," The International Journal of Robotics Research, Vol. 10, No. 4, August 1991, pp. 371-381.
[2] Jain, A., "Unified Formulation of Dynamics for Serial Rigid Multibody Systems," Journal of Guidance, Control and Dynamics, Vol. 14, No. 3, May-June 1991, pp. 531-542.
[3] Kim, S. S. and Haug, E. J., "A Recursive Formulation for Flexible Multibody Dynamics, Part I: Open-Loop Systems," Computer Methods in Applied Mechanics and Engineering, Vol. 71, No. 3, 1988, pp. 293-314.
[4] Changizi, K. and Shabana, A. A., "A Recursive Formulation for the Dynamic Analysis of Open Loop Deformable Multibody Systems," ASME Jl. of Applied Mechanics, Vol. 55, No. 3, September 1988, pp. 687-693.
[5] Keat, J. E., "Multibody System Order n Dynamics Formulation Based in Velocity Transform Method," Journal of Guidance, Control and Dynamics, Vol. 13, No. 2, March-April 1990.
[6] Bodley, C. S., Devers, A. D., Park, A. C., and Frisch, H. P. "A Digital Computer Program for the Dynamic Interaction Simulation of Controls and Structure (DIS-COS)," NASA Technical Paper 1219, NASA, May 1978.
[7] Singh, R. P., VanderVoort, R. J., and Likins, P. W., "Dynamics of Flexible Bodies in Tree Topology--A Computer-Oriented Approach," Journal of Guidance, Control and Dynamics, Vol. 8, No. 5, September 1985, pp. 584-590.
[8] Rodriguez, G., "Kalman Filtering, Smoothing and Recursive Robot Arm Forward and Inverse Dynamics," IEEE Journal of Robotics and Automation, Vol. 3, No. 6, December 1987, pp. 624-639.
[9] Likins, P. W., "Modal Method for Analysis of Free Rotations of Spacecraft," AIAA Journal, Vol. 5, July 1967, pp. 1304-1308.
[10] Jain, A. and Rodriguez, G., "Recursive Dynamics for Flexible Multibody Systems using Spatial Operators," JPL Publication 90-26, Jet Propulsion Laboratory, Pasadena, Calif., December 1990.
[11] Rodriguez, G., Jain, A., and Kreutz, K., "Spatial Operator Algebra for Multibody System Dynamics," Journal of the Astronautical Sciences, Vol. 40, No. 1, January-March 1992.
[12] Walker, M. W. and Orin, D. E., "Efficient Dynamic Computer Simulation of Robotic Mechanisms," ASME Journal of Dynamic Systems, Measurement, and Control, Vol. 104, No. 3, September 1982, pp. 205-211.
[13] Luh, J. Y. S., Walker, M. W., and Paul, R. P. C., "On-line Computational Scheme for Mechanical Manipulators," ASME Journal of Dynamic Systems, Measurement, and Control, Vol. 102, No. 2, June 1980, pp. 69-76.
[14] Jain, A. and Rodriguez, G., "Kinematics and Dynamics of Under-Actuated Manipulators," in IEEE International Conference on Robotics and Automation, Sacramento, Calif., April 1991.
[15] Featherstone, R., "The Calculation of Robot Dynamics using Articulated-Body Inertias," The International Journal of Robotics Research, Vol. 2, No. 1, Spring 1983, pp. 13-30.
[16] Padilla, C. E. and von Flotow, A. H., "Nonlinear Strain-Displacement Relations and Flexible Multibody Dynamics," in Proceedings of the 3rd Annual Conference on Aerospace Computational Control, Vol. 1, Oxnard, Calif., pp. 230-245, August 1989. (JPL Publication 89-45, Jet Propulsion Laboratory, Pasadena, Calif., 1989).
[17] Kane, T. R., Ryan, R. R., and Banerjee, A. K., "Dynamics of a Cantilevered Beam attached to a Moving Base," Journal of Guidance, Control and Dynamics, Vol. 10, No. 2, March-April 1987, pp. 139-151.
The invention uses spatial operators to develop new spatially recursive dynamics algorithms for flexible multibody systems. The operator description of the dynamics is identical to that for rigid multibody systems. Assumed-mode models are used for the deformation of each individual body. The algorithms are based on two spatial operator factorizations of the system mass matrix. The first (Newton-Euler) factorization of the mass matrix leads to recursive algorithms for the inverse dynamics, mass matrix evaluation, and composite-body forward dynamics for the system. The second (Innovations) factorization of the mass matrix, leads to an operator expression for the mass matrix inverse and to a recursive articulated-body forward dynamics algorithm. The primary focus is on serial chains, but extensions to general topologies are also described. A comparison of computational costs shows that the articulated-body forward dynamics algorithm is much more efficient than the composite-body algorithm for most flexible multibody systems.
1. Nomenclature
We use coordinate-free spatial notation ([1, 2]) in this specification. A spatial velocity of a frame is a 6-dimensional quantity whose upper 3 elements are the angular velocity and whose lower 3 elements are the linear velocity. A spatial force is a 6-dimensional quantity whose upper 3 elements are a moment vector and whose lower 3 elements are a force vector.
A variety of indices are used to identify different spatial quantities. Some examples are: V.sub.s (j.sub.k) is the spatial velocity of the j.sup.th node on the k.sup.th body; V.sub.s (k)=col{V.sub.s (j.sub.k)} is the composite vector of spatial velocities of all the nodes on the k.sup.th body; V.sub.s =col{V.sub.s (k)} is the vector of spatial velocities of all the nodes for all the bodies in the serial chain. The index k will be used to refer to both the k.sup.th body as well as the k.sup.th body reference frame .sub.k, with the usage being apparent from the context. Some key quantities are defined below in accordance with FIGS. 1a and 1b.
General Quantities
x=[X].sup.x .epsilon. .sup.3.times.3 --the skew-symmetric cross-product matrix associated with the 3-dimensional vector x ##EQU1## the time derivative of x with respect to an inertial frame --the time derivative of x with respect to the body-fixed (rotating) frame
diag{x(k)}--a block diagonal matrix whose k.sup.th diagonal element is x(k)
col{x(k)}--a column vector whose k.sup.th element is x(k)
l(x,y) .epsilon. .sup.3 --the vector from point/frame x to point/frame y ##EQU2## the spatial transformation operator which transforms spatial velocities and forces between points/frames x and y
Individual Body Nodal Data
n.sub.s (k)--number of nodes on the k.sup.th body
.sub.k --body reference frame with respect to which the deformation field for the k.sup.th body is measured. The motion of this frame characterizes the motion of the k.sup.th body as a rigid body.
j.sub.k --j.sup.th node on the k.sup.th body
l.sub.0 (k,j.sub.k) .epsilon. .sub.3 --vector from .sub.k to the location (before deformation) of the j.sup.th node reference frame on the k.sup.th body
.delta..sub.l (j.sub.k) .epsilon. .sup.3 --translational deformation of the j.sup.th node on the k.sup.th body
l(k,j.sub.k)=l.sub.0 (k,j.sub.k)+.delta..sub.l (j.sub.k) .epsilon. .sup.3 --vector from .sub.k to the location (after deformation) of the j.sup.th node reference frame on the k.sup.th body
.delta..sub..omega. (i.sub.k) .epsilon. .sup.3 --deformation angular velocity of the j.sup.th node on the k.sup.th body with respect to the body frame .sub.k
.delta..sub..nu. (j.sub.k) .epsilon. .sup.3 --deformation linear velocity of the j.sup.th node on the k.sup.th body with respect to the body frame .sub.k
u(j.sub.k) .epsilon. .sup.6 --the spatial displacement of node j.sub.k. The translational component of u(j.sub.k) is .delta..sub.l (j.sub.k), while its time derivative with respect to the body frame .sub.k is ##EQU3## (j.sub.k) .epsilon. .sup.3.times.3 --inertia tensor about the nodal reference frame for the j.sup.th node on the k.sup.th body
p(j.sub.k) .epsilon. .sup.3 --vector from the nodal reference frame to the node center of mass for the j.sup.th node on the k.sup.th body
m(j.sub.k)--mass of the j.sup.th node of the k.sup.th body ##EQU4## spatial inertia about the nodal reference frame for the j.sup.th node on the k.sup.th body
M.sub.s (k) diag{M.sub.s (j.sub.k)} .epsilon. .sup.6n.sbsp.s.sup.(k).times.6n.sbsp.s.sup.(k) --structural mass matrix for the k.sup.th body
K.sub.s (k) .epsilon. .sup.6n.sbsp.s.sup.(k).times.6n.sbsp.s.sup.(k) --structural stiffness matrix for the k.sup.th body
Individual Body Modal Data
n.sub.m (k)--number of assumed modes for the k.sup.th body
(k)=n.sub.m (k)+6--number of deformation plus rigid-body degrees of freedom for the k.sup.th body
.eta.(k) .epsilon. .sup.n.sbsp.m.sup.(k) --vector of modal deformation variables for the k.sup.th body
II.sub.r.sup.j (k) .epsilon. .sup.6 --modal spatial displacement vector for the r.sup.th mode at the j.sub.k.sup.th nodal reference frame
II.sup.j (k)=[II.sub.1.sup.j (k), . . . , II.sub.n.sbsb.m.sub.(k) .sup.j (k)].epsilon. .sup.6.times.n.sbsp.m.sup.(k) --modal spatial influence vector for the j.sub.k.sup.th node. The spatial deformation of node j.sub.k is given by u(j.sub.k)=II.sup.j (k).eta.(k).
II(k)=col{II.sup.j (k)}.epsilon. .sup.6n.sbsp.s.sup.(k).times.n.sbsp.m.sup.(k) --the modal matrix for the k.sup.th body. The r.sup.th column of II(k) is denoted II.sub.r (k) .epsilon. .sup.6n.sbsp.s.sup.(k) and is the mode shape function for the r.sup.th assumed mode for the k.sup.th body. The deformation field for the k.sup.th body is given by u(k)=II(k).eta.(k), while (k)=II(k).eta.(k).
M.sub.m (k) .epsilon. .sup.(k).times.(k) --modal mass matrix for the k.sup.th body.
K.sub.m (k) .epsilon. .sup.(k).times.(k) --modal stiffness matrix for the k.sup.th body.
Multibody Data
N--number of bodies in the serial flexible multibody system ##EQU5## overall degrees of freedom in the serial chain obtain by disregarding the hinge constraints
n.sub.r (k)--number of degrees of freedom for the k.sup.th hinge
(k)=n.sub.m (k)+n.sub.r (k)--number of deformation plus hinge degrees of freedom for the k.sup.th body ##EQU6## overall deformation plus hinge degrees of freedom for the serial chain. d.sub.k --node on the k.sup.th body to which the k.sup.th hinge is attached
t.sub.k --node on the k.sup.th body to which the (k-1).sup.th hinge is attached
.sub.k --reference frame for the k.sup.th hinge on the k.sup.th body. This frame is fixed to node d.sub.k.
.sub.k.sup.+ --reference frame for the k.sup.th hinge on the (k+1).sup.th body. This frame is fixed to node t.sub.k+1.
.theta.(k) .epsilon. .sup.n.sbsp.r.sup.(k) --vector of configuration variables for the k.sup.th hinge
.beta.(k) .epsilon. .sup.n.sbsp.r.sup.(k) --vector of generalized velocities for the k.sup.th hinge ##EQU7## relative spatial velocity for the k.sup.th hinge defined as the spatial velocity of frame .sub.k with respect to frame .sub.k.sup.+
H*(k) .epsilon. .sup.6.times.n.sbsp.r.sup.(k) --joint map matrix for the k.sup.th hinge, whose columns comprise the unit vectors of the hinge. We have that .DELTA..sub.V (k)=H*(k).beta.(k). ##EQU8## vector of (deformation plus hinge) generalized configuration variables for the k.sup.th body ##EQU9## vector of (deformation plus hinge) generalized velocities for the k.sup.th body ##EQU10## spatial velocity of the k.sup.th body reference frame .sub.k, with .omega.(k) and .nu.(k) denoting the angular and linear velocities respectively of frame .sub.k
V(.sub.k) .epsilon. .sup.6 --spatial velocity of frame .sub.k
V(.sub.k.sup.+) .epsilon. .sup.6 --spatial velocity of frame .sub.k.sup.+
V.sub.s (j.sub.k) .epsilon. .sup.6 --spatial velocity of the j.sup.th node on the k.sup.th body.
.alpha..sub.s (j.sub.k) .epsilon. .sup.6 --spatial acceleration of the j.sup.th node on the k.sup.th body. ##EQU11## modal spatial velocity of the k.sup.th body .alpha..sub.m (k)=V.sub.m (k) .epsilon. .sup.(k) --modal spatial acceleration of the k.sup.th body
a.sub.m (k) .epsilon. .sup.(k) --modal Coriolis and centrifugal accelerations for the k.sup.th body
b.sub.m (k) .epsilon. .sup.(k) --modal gyroscopic forces for the k.sup.th body
f.sub.m (k) .epsilon. .sup.(k) --modal spatial force of interaction between the k.sup.th and (k+1).sup.th bodies
f.sub.s (j.sub.k) .epsilon. .sup.6 --spatial force at node j.sub.k
f(k) .epsilon. .sup.6 --effective spatial force at frame .sub.k
T(k) .epsilon. .sup.(k) --generalized force for the k.sup.th body
H(k)=H(k).phi.(.sub.k, k) .epsilon. .sup.n.sbsp.r.sup.(k).times.6 --joint map matrix referred to frame .sub.k for the k.sup.th hinge ##EQU12## (deformation plus hinge) modal joint map matrix for the k.sup.th body ##EQU13## relates spatial forces and velocities between node t.sub.k and frame .sub.k
(k+1,k)=[0, .phi.(t.sub.k+1, k)] .epsilon. .sup.6.times.(k) --relates spatial forces and velocities between node t.sub.k+1 and frame .sub.k ##EQU14## the interbody transformation operator which relates modal spatial forces and velocities between the k.sup.th and (k+1).sup.th bodies ##EQU15## B(k)=[.phi.(k,1.sub.k), .phi.(k,2.sub.k), . . . , .phi.(k,n.sub.s (k))] .epsilon. .sup.6.times.6n.sbsp.s.sup.(k) --relates the spatial velocity of frame .sub.k to the spatial velocities of all the nodes on the k.sup.th body when the body is regarded as being rigid
.epsilon. .sup..times. --the multibody system mass matrix
.epsilon. .sup. --the vector of Coriolis, centrifugal and elastic forces for the multibody system
2. Introduction
The invention uses spatial operators ([1, 2]) to formulate the dynamics and develop efficient recursive algorithms for flexible multibody systems. Flexible spacecraft, limber space manipulators, and vehicles are important examples of flexible multibody systems. Key features of these systems are the large number of degrees of freedom and the complexity of their dynamics models.
Some of the goals of the invention are: (1) providing a high-level architectural understanding of the structure of the mass matrix and its inverse; (2) showing that the high-level expressions can be easily implemented within the very well understood Kalman filtering and smoothing architecture; (3) developing very efficient inverse and forward dynamics recursive algorithms; and (4) analyzing the computational cost of the new algorithms. Accomplishing these goals adds to the rapidly developing body of research in the recursive dynamics of flexible multibody systems (see [3, 4, 5]).
It is assumed that the bodies undergo small deformations so that a linear model for elasticity can be used. However, large articulation at the hinges is allowed. No special assumptions are made regarding the geometry of the component bodies. To maximize applicability, the algorithms developed here use finite-element and/or assumed-mode models for body flexibility. For notational simplicity, and without any loss in generality, the main focus of this specification is on flexible multibody serial chains. Extensions to tree and closed-chain topologies are discussed.
In Section 3 we derive the equations of motion and recursive relationships for the modal velocities, modal accelerations, and modal forces. This section also contains a derivation of the Newton-Euler Operator Factorization of the system mass matrix. A recursive Newton-Euler inverse dynamics algorithm to compute the vector of generalized forces corresponding to a given state and vector of generalized accelerations is described in Section 4.
In Section 5, the Newton-Euler factorization of the mass matrix is used to develop a partly recursive composite-body forward dynamics algorithm for computing the generalized accelerations of the system. The recursive part is for computing the multibody system mass matrix. This forward dynamics algorithm is in the vein of well-established approaches ([6, 7]) which require the explicit computation and inversion of the system mass matrix. However, the new algorithm is more efficient because the mass matrix is computed recursively and because the detailed recursive computations follow the high-level architecture (i.e. roadmap) provided by the Newton-Euler factorization.
In Section 6 we derive new operator factorization and inversion results for the mass matrix that lead to the recursive articulated-body forward dynamics algorithm. A new mass matrix operator factorization, referred to as the Innovations factorization, is developed. The individual factors in the innovations factorization are square and invertible operators. This is in contrast to the Newton-Euler factorization in which the factors are not square and therefore not invertible. The Innovations factorization leads to an operator expression for the inverse of the mass matrix. Based on this expression, in Section 7 we develop the recursive articulated body forward dynamics algorithm for the multibody system. This algorithm is an alternative to the composite-body forward dynamics algorithm and requires neither the explicit formation of the system mass matrix nor its inversion. The structure of this recursive algorithm closely resembles those found in the domain of Kalman filtering and smoothing ([8]).
In Section 8 we compare the computational costs for the two forward dynamics algorithms. It is shown that the articulated body forward dynamics algorithm is much more efficient than the composite body forward dynamics algorithm for typical flexible multibody systems. In Section 9 we discuss the extensions of the formulation and algorithms in this specification to tree and closed-chain topology multibody systems.
SUMMARY OF THE INVENTION
A robot manipulator controller for a flexible manipulator arm having plural bodies connected at respective movable hinges and flexible in plural deformation modes corresponding to respective modal spatial influence vectors relating deformations of plural spaced nodes of respective bodies to the plural deformation modes, operates by computing articulated body quantities for each of the bodies from respective modal spatial influence vectors, obtaining specified body forces for each of the bodies, and computing modal deformation accelerations of the nodes and hinge accelerations of the hinges from the specified body forces, from the articulated body quantities and from the modal spatial influence vectors. In one embodiment of the invention, the controller further operates by comparing the accelerations thus computed to desired manipulator motion to determine a motion discrepancy, and correcting the specified body forces so as to reduce the motion discrepancy.
Computing the articulated body quantities is carried out for each body beginning at the outermost body by computing a modal mass matrix, computing an articulated body inertia from the articulated body inertia of a previous body and from the modal mass matrix, computing an articulated hinge inertia from the articulated body inertia, computing an articulated body to hinge force operator from the articulated hinge inertia, computing a null force operator from the articulated body to hinge force operator. This is followed by revising the articulated body inertia by transforming it by the null force operator.
The manipulator bodies and hinges are characterized by respective vectors of deformation and hinge configuration variables, and computing modal deformation accelerations and hinge accelerations is carried out for each one of the bodies beginning with the outermost body by computing a residual body force from a residual body force of a previous body and from the vector of deformation and hinge configuration variables, computing a resultant hinge acceleration from the body force, the residual body force and the articulated hinge inertia, and then, for each one of the bodies beginning with the innermost body, by computing a modal body acceleration from a modal body acceleration of a previous body, computing a modal deformation acceleration and hinge acceleration from the resulting hinge acceleration and from the modal body acceleration transformed by the body to hinge force operator.
Computing a resultant hinge force is followed by revising the residual body force by the resultant hinge force transformed by the body to hinge force operator, and computing a modal deformation acceleration and hinge acceleration is followed by revising the modal body acceleration based upon the deformation and hinge acceleration. The computing is performed cyclically in a succession of time steps, and the vector of deformation and hinge configuration variables is computed from the modal deformations and hinge accelerations of a previous time step, or is derived by reading robot joint sensors in real time.
In a preferred embodiment, the articulated body inertia, the articulated hinge inertia, the body to hinge force operator, the null force operator, the body force, the residual body force, the resultant hinge acceleration and the resultant hinge force are each partitioned into free and rigid versions. This embodiment operates by computing the flexible version of the resultant hinge force from the applied body force, and computing the flexible version of the residual body force and from the rigid version of the residual body force transformed by the modal spatial influence vector. The articulated body inertia is decomposed into rigid-free and rigid-rigid coupling components, and the rigid version of the residual body force is revised based upon a function of the rigid-rigid and rigid-free coupling components of the articulated body inertia and a flexible version of the articulated body inertia. This embodiment decomposes the manipulator's modal mass matrix into rigid-free and rigid-rigid coupling components and computes the rigid-rigid and rigid-free coupling components of the articulated body inertia from respective ones of the rigid-rigid and rigid-free coupling components of the modal mass matrix.
In this embodiment, free and rigid versions of a deformation and hinge modal joint map matrix are computed for each body so that the flexible version of the articulated hinge inertia is computed from the articulated body inertia transformed by the flexible version of the corresponding deformation and hinge modal joint map matrix, the rigid version of the articulated body inertia is computed from a function of the rigid-rigid and rigid-free coupling components of the articulated body inertia transformed by the flexible version of the corresponding deformation and hinge modal joint map matrix, the rigid version of the articulated hinge inertia is computed from the rigid version of the articulated body inertia, and the rigid version of the body to hinge force operator is computed from the rigid versions of the articulated body inertia and the articulated hinge inertia. The free and rigid versions of the deformation and hinge modal joint map matrix are formed by computing a joint map matrix corresponding to unit vectors of the hinges and computing the deformation and hinge modal joint map matrix from the joint map matrix and from the modal spatial influence vector.
In this embodiment, the flexible version of the resulting hinge acceleration is computed from the flexible versions of the articulated hinge inertia and resulting hinge force, and the rigid version of the resulting hinge acceleration is computed from the rigid versions of the articulated hinge inertia and resulting hinge force. The residual body force is revised in this embodiment by adding to the residual body force a product of the rigid versions of the resultant hinge force and the body to hinge force operator.
BRIEF DESCRIPTION OF THE DRAWINGS
FIG. 1a is a simplified diagram of a portion of a robot manipulator having flexible links, and illustrating the coordinate system employed in one embodiment of the invention.
FIG. 1b is a simplified diagram illustrating the finite element analysis employed in the invention, in which the displacement of plural spaced nodes along the length of a flexible link follows a well-recognized pattern for each mode of flexibility.
FIG. 2 is a block diagram illustrating how the articulated body quantities are produced in one embodiment of the invention.
FIG. 3 is a block diagram illustrating an articulated body forward dynamics algorithm for flexible link manipulators in accordance with the present invention.
FIG. 4 is a block diagram illustrating the process of the invention for controlling a robot manipulator having flexible links.
FIGS. 5a and 5b constitute a block diagram illustrating a preferred embodiment of the articulate body forward dynamics algorithm employed in the process of FIG. 4.
FIG. 6 is a simplified schematic block diagram of apparatus embodying the present invention.
DETAILED DESCRIPTION OF THE INVENTION
3. Equations of Motion for Flexible Serial Chains
In this section, we develop the equations of motion for a serial flexible multibody system with N flexible bodies. Each flexible body is assumed to have a lumped mass model consisting of a collection of nodal rigid bodies. Such models are typically developed using standard finite element structural analysis software. The number of nodes on the k.sup.th body is denoted n.sub.s (k). The j.sup.th node on the k.sup.th body is referred to as the j.sub.k.sup.th node. Each body has associated with it a body reference frame, denote .sub.k for the k.sup.th body. The deformations of the nodes on the body are described with respect to this body reference frame, while the rigid body motion of the k.sup.th body is characterized by the motion of frame .sub.k.
The 6-dimensional spatial deformation (slope plus translational) of node j.sub.k (with respect to frame .sub.k) is denoted u(j.sub.k) .epsilon. .sup.6. The overall deformation field for the k.sup.th body is defined as the vector u(k)=col{u(j.sub.k)} .epsilon. .sup.6n.sbsp.s.sup.(k). The vector from frame .sub.k to the reference frame on node j.sub.k is denoted l(k,j.sub.k) .epsilon. .sup.3.
With M.sub.s (k.sub.k) .epsilon. .sup.6.times.6 denoting the spatial inertia of the j.sup.th node, the structural mass matrix for the k.sup.th body M.sub.s (k) is the block diagonal matrix diag{M.sub.s (j.sub.k)} .epsilon. .sup.6n.sbsp.s.sup.(k).times.6n.sbsp.s.sup.(k). The structural stiffness matrix is denoted K.sub.s (k) .epsilon. .sup.6n.sbsp.s.sup.(k).times.6n.sbsp.s.sup.(k). Both M.sub.s (k) and K.sub.s (k) are typically generated using finite element analysis.
As shown in FIG. 1a, the bodies in the serial chain are numbered in increasing order from tip to base. We use the terminology inboard (outboard) to denote the direction along the serial chain towards (away from) the base body. The k.sup.th body is attached on the inboard side to the (k+1).sup.th body via the k.sup.th hinge, and on the outboard side to the (k-1).sup.th body via the (k-1).sup.th hinge. On the k.sup.th body, the node to which the outboard hinge (the (k-1).sup.th hinge) is attached is referred to as node t.sub.k, while the node to which the inboard hinge (the k.sup.th hinge) is attached is denoted node d.sub.k. Thus the k.sup.th hinge couples together nodes d.sub.k and t.sub.k+1. Attached to each of these pair of adjoining nodes are the k.sup.th hinge reference frames denoted .sub.k and .sub.k.sup.+, respectively. The number of degrees of freedom for the k.sup.th hinge is denoted n.sub.r (k). The vector of configuration variables for the k.sup.th hinge is denoted .theta.(k) .epsilon. .sup.n.sbsp.r.sup.(k), while its vector of generalized speeds is denoted .beta.(k) .epsilon. .sup.n.sbsp.r.sup.(k). In general, when there are nonholonomic hinge constraints, the dimensionality of .beta.(k) may be less than that of .theta.(k). For notational convenience, and without any loss generality, it is assumed here that the dimensions of the vectors .theta.(k) and .beta.(k) are equal. In most situations, .beta.(k) is simply .theta.. However there are many cases where the use of quasi-coordinates simplifies the dynamical equations of motion and an alternative choice for .beta.(k) may be preferable. The relative spatial velocity .DELTA..sub.v (k) across the hinge is given by H*(k).beta.(k), where H*(k) denotes the joint map matrix for the k.sup.th hinge.
Assumed modes are typically used to represent the deformation of flexible bodies, and there is a large body of literature dealing with their proper selection. There is however a close relationship between the choice of a body reference frame and the type of assumed modes. The complete motion of the flexible body is contained in the knowledge of the motion of the body reference frame and the deformation of the body as seen from this body frame. In the multibody context, it is often convenient to choose the location of the k.sup.th body reference frame .sub.k as a material point on the body and fixed to node d.sub.k at the inboard hinge. For this choice, the assumed modes are cantilever modes and node d.sub.k exhibits zero deformation (u(d.sub.k)=0). Free-free modes are also used for representing body deformation and are often preferred for control analysis and design. For these modes, the reference frame .sub.k is not fixed to any node, but is rather assumed to be fixed to the undeformed body, and as a result all nodes exhibit nonzero deformation. The dynamics modeling and algorithms developed here handle both types of modes, with some additional computational simplifications arising from Eq. (1) when cantilever modes are used. For a related discussion regarding the choice of reference frame and modal representations for a flexible body see [9].
We assume here that a set of n.sub.m (k) assumed modes has been chosen for the k.sup.th body. Let II.sub.r.sup.j (k) .epsilon. .sup.6 denote the modal spatial displacement vector at the j.sub.k.sup.th node for the r.sup.th mode. The modal spatial displacement influence vector II.sup.j (k) .epsilon. .sup.6.times.n.sbsp.m.sup.(k) for the j.sub.k.sup.th node and the modal matrix II(k) .epsilon. .sup.6n.sbsp.s.sup.(k).times.n.sbsp.m.sup.(k) for the k.sup.th body are defined as follows:
The r.sup.th column of II(k) is denoted II.sub.r (k) and defines the mode shape for the r.sup.th assumed mode for the k.sup.th body. Note that for cantilever modes we have
With .eta.(k) .epsilon. .sup.n.sbsp.m.sup.(k) denoting the vector of modal deformation variables for the k.sup.th body, the spatial deformation of node j.sub.k and the spatial deformation field u(k) for the k.sup.th body are given by
The vector of generalized configuration variables .nu.(k) and generalized speeds X(k) for the k.sup.th body are defined as ##EQU16## where (k)n.sub.m (k)+n.sub.r (k). The overall vectors of generalized configuration variables .nu. and generalized speeds X for the serial multibody system are given by
where ##EQU17## denotes the overall number of degrees of freedom for the multibody system. The state of the multibody system is defined by the pair of vectors {.nu.,X}. For a given system state {.nu.,X}, the equations of motion define the relationship between the vector of generalized accelerations X and the vector of generalized forces T .epsilon. .sup. for the system. The inverse dynamics problem consists of computing the vector of generalized forces T for a prescribed set of generalized accelerations X. The forward dynamics problem is the converse one and consists of computing the set of generalized accelerations X resulting from a set of generalized forces T. The equations of motion for the system are developed in the remained of this section.
3.1 Recursive Propagation of Velocities
Let V(k) .epsilon. .sup.6 denote the spatial velocity of the k.sup.th body reference frame .sub.k. The spatial velocity V.sub.s (t.sub.k+1) .epsilon. .sup.6 of node t.sub.k+1 (on the inboard of the k.sup.th hinge) is related to the spatial velocity V(k+1) of the (k+1).sup.th body reference frame .sub.k+1, and the modal deformation variable rates .eta.(k+1) as follows: ##EQU18## The spatial transformation operator .phi.(x,y) .epsilon. .sup.6.times.6 above is defined to be ##EQU19## where l(x,y) .epsilon. .sup.3 denotes the vector between the points x and y. Note that the following important (group) property holds:
for arbitrary points x, y and z. As in Eq. (5), and throughout this specification, the index k will be used to refer to both the k.sup.th body as well as to the k.sup.th body reference frame .sub.k with the specific usage being evident from the context. Thus for instance, V(k) and .phi.(k,t.sub.k) are the same as V(.sub.k), and .phi.(.sub.k, t.sub.k) respectively.
The spatial velocity V(.sub.k.sup.+) of frame .sub.k.sup.+ (on the inboard side of the k.sup.th hinge) is related to V.sub.s (t.sub.k+1) via
Since the relative spatial velocity .DELTA..sub.v (k) across the k.sup.th hinge is given by H*(k).beta.(k), the spatial velocity V(.sub.k) of frame .sub.k on the outboard side of the k.sup.th hinge is
The spatial velocity V(k) of the k.sup.th body reference frame is given by ##EQU20##
Putting together Eq. (5), Eq. (7), Eq. (8) and Eq. (3.1), it follows that
Thus, with (k)n.sub.m (k)+6, and using Eq. (10), the modal spatial velocity V.sub.m (k) .epsilon. .sup.(k) for the k.sup.th body is given by ##EQU21## where the interbody transformation operator .PHI.(.,.) and the modal joint map matrix (k) are defined as ##EQU22## where
Note that
where ##EQU23## Also, the modal joint map matrix (k) can be partitioned as ##EQU24## where
With ##EQU25## we define the spatial operator .sub..PHI. as ##EQU26## Using the fact that .sub.101 is nilpotent (i.e. .sub.1.PHI..sup.N =0), we define the spatial operator .PHI. as ##EQU27## where
Also define the spatial operator diag {(k)} .epsilon. .sup..times.. Using these spatial operators, and defining V.sub.m col{V.sub.m (k)} .epsilon. .sup., from Eq. (11) it follows that the spatial operator expression for V.sub.m is given by
3.2 Modal Mass Matrix for a Single Body
With V.sub.s (j.sub.k) .epsilon. .sup.6 denoting the spatial velocity of node j.sub.k, and V.sub.s (k)col{V.sub.s (j.sub.k)} .epsilon. .sup.6n.sbsp.s.sup.(k) the vector of all nodal spatial velocities for the k.sup.th body, it follows (see Eq. (5)) that
where
Since M.sub.s (k) is the structural mass matrix of the k.sup.th body, and using Eq. (21), the kinetic energy of the k.sup.th body can be written in the form ##EQU28## Corresponding to the generalized speeds vector X(k), M.sub.m (k) as defined above is the modal mass matrix of the k.sup.th body. In the block partitioning in Eq. (23), the superscripts f and r denote the flexible and rigid blocks respectively. Thus M.sub.m.sup.ff (k) represents the flex/flex coupling block, white M.sub.m.sup.fr (k) the flex/rigid coupling block of M.sub.m (k). We will use this notational convention through this specification. This partitioning is readily carried out by simply recognizing that the M.sub.m.sup.ff (k) block is a square matrix of dimensionality equal to the number of deformation modes while the M.sub.m.sup.rr (k) block is a square 6-by-6 matrix. Note that M.sub.m.sup.rr (k) is precisely the rigid body spatial inertia of the k.sup.th body. Indeed, M.sub.m (k) reduces to the rigid body spatial inertia when the body flexibility is ignored, i.e., no modes are used, since in this case n.sub.m (k)=0 (and II(k) is null).
Since the vector l(k,j.sub.k) from .sub.k to node j.sub.k depends on the deformation of the node, the operator B(k) is also deformation dependent. From Eq. (23) it follows that while the block .sub.m.sup.ff (k) is deformation independent, both the blocks M.sub.m.sup.fr (k) and M.sub.m.sup.rr (k) are deformation dependent. The detailed expression for the modal mass matrix can be defined using modal integrals which are computed as a part of the finite-element structural analysis of the flexible bodies. These expressions for the modal integrals and the modal mass matrix of the k.sup.th body can be found in [10]. Often the deformation dependent parts of the modal mass matrix are ignored, and free-free eigen-modes are used for the assumed modes II(k). When this is the case, M.sub.m.sup.fr (k) is zero and M.sub.m.sup.ff (k) is block diagonal.
3.3 Recursive Propagation of Accelerations
Differentiating the velocity recursive equation, Eq. (11), we obtain the following recursive expression for the modal spatial acceleration .alpha..sub.m (k) .epsilon. .sup.(k) for the k.sup.th body: ##EQU29## where .alpha.(k)=V(k), and the Coriolis and centrifugal acceleration term a.sub.m (k) .epsilon. .sup.(k) is given by ##EQU30## The detailed expressions for a.sub.m (k) can be found in [10]. Defining a.sub.m =col{a.sub.m (k)} .epsilon. .sup. and .alpha.m.sub.m =col{.alpha..sub.m (k)} .epsilon. .sup., and using spatial operators we can rexpress Eq. (24) in the form
The vector of spatial accelerations of all the nodes for the k.sup.th body, .alpha..sub.s (k)col{.alpha..sub.s (j.sub.k)} .epsilon. .sup.6n.sbsp.s.sup.(k), is obtained by differentiating Eq. (21):
where ##EQU31##
3.4 Recursive Propagation of Forces
Let f(k-1) .epsilon. .sup.6 denote the effective spatial force of interaction, referred to frame .sub.k-1, between the k.sup.th and (k-1).sup.th bodies across the (k-1).sup.th hinge. Recall that the (k-1).sup.th hinge is between node t.sub.k on the k.sup.th body and node d.sub.k-1 on the (k-1).sup.th body. With f.sub.s (j.sub.k).epsilon. .sup.6 denoting the spatial force at a node j.sub.k, the force balance equation for node t.sub.k is given by
For all nodes other than node t.sub.k on the k.sup.th body, the force balance equation is of the form
In the above f.sub.K (k)=K.sub.s (k)u(k) .epsilon. .sup.6n.sbsp.s.sup.(k) denotes the vector of spatial elastic strain forces for the nodes on the k.sup.th body, while b(j.sub.k) .epsilon. .sup.6 denotes the spatial gyroscopic force for the node j.sub.k and is given by ##EQU32## where .omega.(j.sub.k) .epsilon. .sup.3 notes the angular velocity of node j.sub.k. Collecting together the above equations and defining ##EQU33## it follows from Eq. (29) and Eq. (30) that
where f.sub.s (k)col{f.sub.s (j.sub.k)} .epsilon. .sup.6n.sbsp.s.sup.(k). Noting that
and using the principle of virtual work, it follows from Eq. (21) that the modal spatial forces f.sub.m (K) .epsilon. .sup.(k) for the k.sup.th body are given by ##EQU34## Premultiplying Eq. (33) by ##EQU35## and using Eq. (23), Eq. (27), and Eq. (35) leads to the following recursive relationship for the modal spatial forces: ##EQU36## Here we have defined ##EQU37## and the modal stiffness matrix ##EQU38## The expression for K.sub.m (k) in Eq. (38) uses the fact that the columns of B*(k) are indeed the deformation dependent rigid body modes for the k.sup.th body and hence they do not contribute to its elastic strain energy. Indeed, when a deformation dependent structural stiffness matrix K.sub.s (k) is used, we have that
However the common practice (also followed here) of using a constant, deformation-independent structural stiffness matrix leads to the anomalous situation wherein Eq. (39) does not hold exactly. We ignore these fictitious extra terms on the left-hand side of Eq. (39).
The velocity-dependent bias term b.sub.m (k) is formed using modal integrals generated by standard finite-element programs, and a detailed expression for it is given in [10]. From Eq. (36), the operator expression for the modal spatial forces f.sub.m col{f.sub.m (k)} .epsilon. .sup. for all the bodies in the chain is given by
where
From the principle of virtual work, the generalized forces vector T .epsilon. .sup. for the multibody system is given by the expression
3.5 Operator Expression for the System Mass Matrix
Collecting together the operator expressions in Eq. (20), Eq. (26), Eq. (40) and Eq. (41) we have: ##EQU39## where
Here is the system mass matrix for the serial chain and the expression .PHI.M.sub.m .PHI.** is referred to as the Newton-Euler Operator Factorization of the mass matrix. is the vector of Coriolis, centrifugal, and elastic forces for the system.
It is noteworthy that the operator expressions for and Z,13 are identical in form to those for rigid multibody systems (see [1, 11]). Indeed, the similarity is more than superficial, and the key properties of the spatial operators that are used in the analysis and algorithm development for rigid multibody systems also hold for the spatial operators defined here. As a consequence, a large part of the analysis and algorithms for rigid multibody systems can be easily carried over and applied to flexible multibody systems. This is the approach adopted here.
4. Inverse Dynamics Algorithm
This section describes a recursive Newton-Euler inverse dynamics algorithm for computing the generalized forces T, for a given set of generalized accelerations X and system state {.nu., X}. The inverse dynamics algorithm also forms a part of forward dynamics algorithms such as those based upon composite body inertias or the conjugate gradient method ([12]).
Collecting together the recursive equations in Eq. (11), Eq. (24), Eq. (36) and Eq. (41) we obtain the following recursive Newton-Euler inverse dynamics algorithm: ##EQU40## The structure of this algorithm closely resembles the recursive Newton-Euler inverse dynamics algorithm for rigid multibody systems (see [13, 1]). All external forces on the k.sup.th body are handled by absorbing them into the gyroscopic force term b.sub.m (k). Base mobility is handled by attaching an additional 6 degrees of freedom hinge between the mobile base and an inertial frame.
By taking advantage of the special structure of .PHI.(k+1,k) and (k) in Eq. (12) and Eq. (13), the Newton-Euler recursions in Eq. (44) can be further simplified. Using block partitioning and the superscripts f and r as before to denote the flexible and rigid components or versions of the various quantities, we have that ##EQU41##
It is easy to verify that Eq. (45) below is a simplified version of the inverse dynamics algorithm in Eq. (44). ##EQU42## In the foregoing algorithm, .eta.(k) and .eta.(k) are the modal deformation velocities and accelerations, respectively, computed from the results obtained for a previous time step by a forward dynamics algorithm of the type described below herein. Flexible multibody systems have actuators typically only at the hinges. Thus for the k.sup.th body, only the subset of the generalized forces vector T(k) corresponding to the hinge actuator forces T.sup.r (k) can be set, white the remaining generalized forces T.sup.f (k) are zero. Thus in contrast with rigid multibody systems, flexible multibody systems are under-actuated systems ([14]), since the number of available actuators is less than the number of motion degrees of freedom in the system. For such under-actuated systems, the inverse dynamics computations for the generalized force T are meaningful only when the prescribed generalized accelerations X form a consistent data set. For a consistent set of generalized accelerations, the inverse dynamics computations will lead to a generalized force vector T such that T.sup.f (.)=0.
5. Composite Body Forward Dynamics Algorithm
The forward dynamics problem for a multibody system requires computing the generalized accelerations X for a given vector of generalized forces T and state of the system {.nu.,X}. The composite body forward dynamics algorithm described below consists of the followings steps: (a) computing the system mass matrix , (b) computing the bias vector , and (c) numerically solving the following linear matrix equation for X:
Later in Section 6 we describe the recursive articulated body forward dynamics algorithm that does not require the explicit computation of either or .
It is evident from Eq. (46) that the components of the vector are the generalized forces for the system when the generalized accelerations X are all zero. Thus can be computed using the inverse dynamics algorithm in Eq. (45). We describe next an efficient composite-body-based recursive algorithm for the computation of the mass matrix . This algorithm is based upon the following lemma which contains a decomposition of the mass matrix into block diagonal, block upper triangular and block lower triangular components.
Lemma 5.1
Define the composite body inertias R(k) .epsilon. .sup.(k).times.(k) recursively for all the bodies in the serial chain as follows: ##EQU43## Also define Rdiag{R(k)} .epsilon. .sup..times.. Then we have the following spatial operator decomposition
where .PHI..PHI.-I:
Proof: See Appendix A.
Physically, R(k) is the modal mass matrix of the composite body formed from all the bodies outboard of the k.sup.th hinge by freezing all their (deformation plus hinge) degrees of freedom. It follows from Eq. (43) and Lemma 5.1 that
Note that the three terms on the right of Eq. (49) are block diagonal, block lower triangular and block upper triangular respectively. The following algorithm for computing the mass matrix computes the elements of these terms recursively. ##EQU44## The main recursive proceeds from tip to base, and computes the blocks along the diagonal of . As each such diagonal element is computed, a new recursion to compute the off-diagonal elements is spawned. The structure of this algorithm closely resembles the composite rigid body algorithm for computing the mass matrix of rigid multibody systems ([12, 8]). Like the latter, it is also highly efficient. Additional computational simplifications of the algorithm arising from the sparsity of both .sub.f (k) and .sub.r (k) are easy to incorporate.
6. Factorization and Inversion of the Mass Matrix
An operator factorization of the system mass matrix , denoted the Innovations Operator Factorization, is derived in this section. This factorization is an alternative to the Newton-Euler factorization in Eq. (43) and, in contrast with the latter, the factors in the Innovations factorizations are square and invertible. Operator expressions for the inverse of these factors are developed and these immediately lead to an operator expression for the inverse of the mass matrix. The operator factorization and inversion results here closely resemble the corresponding results for rigid multibody systems (see [1]).
Given below is a recursive algorithm illustrated in FIG. 2 which defines some required articulated body quantities. In the following algorithm, P(k) is the articulated body inertia of body k, D(k) is the articulated hinge inertia of hinge k, G(k) is a body to hinge force operator of body and hinge k, and .tau.(k) is a null force operator for hinge k which accounts for the component of applied force resulting in no hinge acceleration. ##EQU45## The operator P .epsilon. .sup..times. is defined as a block diagonal matrix with the k.sup.th diagonal element being P(k). The quantities defined in Eq. (51) form the component elements of the following spatial operators:
The only nonzero block elements of K and .sub..psi. are the elements' K(k+1,k)'s and .psi.(k+1,k)'s respectively along the first sub-diagonal.
As in the case for .sub.101 , .sub.104 is nilpotent, so we can define the operator .psi. as follows. ##EQU46## The structure of the operators .sub..psi. and .psi. is identical to that of the operators .sub..PHI. and .PHI. respectively except that the component elements are now .psi.(i,j) rather than .PHI.(i,j). Also, the elements of .psi. have the same semigroup properties as the elements of the operator .PHI., and as a consequence, high-level operator expressions involving them can be directly mapped into recursive algorithms, and the explicit computation of the elements of the operator .psi. is not required.
The Innovations Operator Factorization of the mass matrix is defined in the following lemma.
Lemma 6.1
Proof: See Appendix A.
Note that the factor [I+.PHI.K] .epsilon. .sup..times. is square, block lower triangular and nonsingular, while D is a block diagonal matrix. This factorization provides a closed-form expression for the block LDL* decomposition of . The following lemma gives the closed form operator expression for the inverse of the factor [I+.PHI.K].
Lemma 6.2
Proof: See Appendix A.
It follows from Lemma 6.1 and 6.2 that the operator expression for the inverse of the mass matrix is given by:
Lemma 6.3
Once again, note that the factor [I-.psi.K] is square, block lower triangular and nonsingular and so Lemma 6.3 provides a closed-form expression for the block LDL* decomposition of .sup.-1.
7. Articulated Body Forward Dynamics Algorithm
We first use the operator expression for the mass matrix inverse developed in Section 6 to obtain an operator expression for the generalized accelerations X. This expression directly leads to a recursive algorithm for the forward dynamics of the systems. The structure of this algorithm is completely identical in form to the articulated body algorithm for serial rigid multibody systems. The computational cost of this algorithm is further reduced by separately processing the flexible and hinge degrees of freedom at each step in the recursion, and this leads to the articulated body forward dynamics algorithm for serial flexible multibody systems. This algorithm is an alternative to the composite-body forward dynamics algorithm developed earlier.
The following lemma describes the operator expression for the generalized accelerations X in terms of the generalized forces T.
Lemma 7.1
Proof:
See Appendix A.
As in the case of rigid multibody systems ([1, 2]), the direct recursive implementation of Eq. (57) leads to the following recursive forward dynamics algorithm illustrated in FIG. 3. In the following algorithm, z(k) is a residual body force on body k, .epsilon.(k) is the resultant hinge force on hinge k, v(k) is the resultant hinge acceleration of hinge k and z.sup.+ (k) is the revised residual body force on body k: ##EQU47## The structure of this algorithm is closely related to the structure of the well known Kalman filtering and smoothing algorithms ([8]). All the degrees of freedom for each body (as characterized by its joint map matrix *(.)) are processed together at each recursion step in this algorithm. However, by taking advantage of the sparsity and special structure of the joint map matrix, additional reduction in computational cost is obtained by processing the flexible degrees of freedom and the hinge degrees of freedom separately. These simplifications are described in the following sections.
7.1 Simplified Algorithm for the Articulated Body Quantities
Instead of a detailed derivation, we describe here the conceptual basis for the separation of the modal and hinge degrees of freedom for each body. First we recall the velocity recursion equation in Eq. (11)
and the partitioned form of (k) in Eq. (13) ##EQU48## Introducing a dummy variable k', we can rewrite Eq. (59) as
where
Conceptually, each flexible body is now associated with two new bodies. The first one has the same kinematical and mass/inertia properties as the real body and is associated with the flexible degrees of freedom. The second body is a fictitious body and is massless and has zero extent. It is associated with the hinge degrees of freedom. The serial chain now contains twice the number of bodies as the original one, with half the new bodies being fictitious ones. The one * operator now has the same number of columns but twice the number of rows as the original * operator. The new .PHI. operator has twice as many rows and columns as the original one. Repeating the analysis described in the previous sections, we once again obtain the same operator expression as Eq. (57). This expression also leads to a recursive forward dynamics algorithm as in Eq. (58). However each sweep in the algorithm now contains twice as many steps as the original algorithm. But since each step now processes only a smaller number of degrees of freedom, this leads to a reduction in the overall cost. In the following algorithm, the subscript r denotes the rigid component or version of the subscripted quantity while the subscript f denotes the flexible component or version of the subscripted quantity. Thus, .sub.f (k) is a matrix including the corresponding modal spatial influence vector, while .sub.r (k) is a matrix including the corresponding transformed joint map matrix. The new algorithm (replacing Eq. (51) for computing the articulated body quantities is as follows: ##EQU49## We now use the sparsity of (k+1,k), .sub.f (k) and .sub.r (k) to further simplify the above algorithm. Using the symbol "x" to indicate "don't care" blocks, the structure in block partitioned form of some of the quantities in Eq. (62) is given below. In the following algorithm, the subscripts f and r have the same significance as that discussed previously herein, the subscript R denotes another rigid version of the subscripted quantity (defined below), while P.sup.rf (k) and P.sup.rr (k) denote the blocks of the articulated body inertia P(k) partitioned in the same manner as that discussed previously herein with reference to the partitioning of the modal mass matrix in Equation (23): ##EQU50## Using the structure described above the simplified algorithm for computing the articulated body quantities is as follows: ##EQU51##
7.2 Simplified Articulated Body Forward Dynamics Algorithm
The complete recursive articulated body forward dynamics algorithm for a serial flexible multibody system follows directly from the recursive implementation of the expression in Eq. (57). The algorithm consists of the following steps as illustrated in FIG. 4: (a) a base-to-tip recursion as in Eq. (45) for computing the modal spatial velocities V.sub.m (k) and the Coriolis and gyroscopic terms a.sub.m (k) and b.sub.m (k) for all the bodies; (b) computation of the articulated body quantities using Eq. (78) and Eq. (63); and (c) a tip-to-base recursion followed by a base-to-tip recursion for the joint accelerations X as described below and illustrated in FIGS. 5a and 5b: ##EQU52## The recursion in Eq. (64) is obtained by simplifying the recursions in Eq. (58) in the same manner as described in the previous section for the articulated body quantities. The rigid Coriolis and centrifugal acceleration a.sub.mR (k) is given in Appendix C below herein.
In contrast with the composite body forward dynamics algorithm described in Section 5, the articulated body forward dynamics algorithm does not require the explicit computation of either or . The structure of this articulated body algorithm closely resembles the recursive articulated body forward dynamics algorithm for rigid multibody systems described in references ([15, 1]).
The articulated body forward dynamics algorithm has been used to develop a dynamics simulation software package (called DARTS) for the high-speed, real-time, hardware-in-the-loop simulation capability for planetary spacecraft. Validation of the DARTS software was carried out by comparing simulation results with those from a standard flexible multibody simulation package ([6]). The results from the two independent simulations have shown complete agreement.
A System Embodying the Invention
Referring to FIG. 6, a robot manipulator 100 having flexible links (bodies), such as the manipulator illustrated in FIGS. 1a and 1b, includes joint servos 110 controlling respective articulating hinges of the manipulator. A robot control computer 120 includes a processor 125 which computes the articulated body quantities of the manipulator 100 from the current state of the manipulator 100 using the process of FIG. 2. The current state of the manipulator 100 is also used by a processor 130 to compute the Coriolis and centrifugal accelerations and gyroscopic forces of the manipulator links using the algorithm of Equation (44). A set of link (body) forces is specified to a processor 135. The processor 135 uses the specified body forces, the articulated body quantities computed by the processor 125 and the gyroscopic and Coriolis terms computed by the processor 130 to compute the deformation acceleration of the finite element nodes of each link (body) and the acceleration of each hinge by executing the algorithm of FIGS. 5a and 5b.
In one embodiment of the invention described above with reference to FIG. 4, the processor 135 repeats its operation over successive time steps, and the configuration vectors of the manipulator 100 required by the processors 125 and 130 are computed by a processor 140 from the accelerations computed by the processor 135 for the previous time step. In an alternative embodiment of the invention, the hinge configuration vectors are derived by the processors 125 and 130 directly from joint sensors 142 on the hinges of the manipulator 100.
As one example of the application of the results computed by the processor 135, a desired robot motion is defined by a set of user-specified node deformations and hinge accelerations for a succession of time steps. The node deformations and hinge accelerations computed during each time step by the processor 135 are compared by a processor 144 with a desired user-specified node deformations and hinge accelerations for the corresponding time step to determine an error and to correct the specified body forces to reduce the error using well-known feedback control techniques. Such feedback control techniques are well-understood in the art and need not be described here. The corrected body forces are then stored for later (or immediate) conversion by a processor 146 to joint servo commands for transmittal to the joint servos 110.
8. Computational Cost
This section discusses the computational cost of the composite body and the articulated body forward dynamics algorithms. For low-spin multibody systems, it has been suggested in [16] that using ruthlessly linearized models for each flexible body can lead to significant computational reduction without sacrificing fidelity. These linearized models are considerably less complex and do not require much of the modal integral data for the individual flexible bodies. All computational costs given below are based on the use of ruthlessly linearized models and the computationally simplified steps described in Appendix B.
Flexible multibody systems typically involve both rigid and flexible bodies and, in addition, different sets of modes are used to model the flexibility of each body. As a consequence, where possible, we described the contribution of a typical (non-extremal) flexible body, denoted the k.sup.th body, to the overall computational cost. Note that the computational cost for extremal bodies as well as for rigid bodies is lower than that for a non-extremal flexible body. Summing up this cost for all the bodies in the system gives a figure close to the true computational cost for the algorithm. Without any loss in generality, we have assumed here that all the hinges are single degree of freedom rotary joints and that free-free assumed modes are being used. The computational costs are given in the form of polynomial expressions for the number of floating point operations with the symbol M denoting multiplications and A denoting additions.
8.1 Computational Cost of the Composite Body Forward Dynamics Algorithm
The composite body forward dynamics algorithm described in Section 5 is based on solving the linear matrix equation.
The computational cost of this forward dynamics algorithm is given below:
1. Cost of computing R(k) for the k.sup.th body using the algorithm in Eq. (50) is ##EQU53## 2. Contribution of the k.sup.th body to the cost of computing (excluding cost of R(k)'s) using the algorithm in Eq. (50) is {k[12n.sub.m.sup.2 (k)+34n.sub.m (k)+13]}M+{k[11n.sub.m.sup.2 (k)+24n.sub.m (k)+13]} A.
3. Setting the generalized accelerations X=0, the vector can be obtained by using the inverse dynamics algorithm described in Eq. (45) for computing the generalized forces T. The contribution of the k.sup.th body to the computational cost for (k) is {2n.sub.m.sup.2 (k)+54n.sub.m (k)+206} M+{2n.sub.m.sup.2 (k)+50n.sub.m (k)+143} A.
4. The cost of computing T- is {} A.
5. The cost of solving the linear equation in Eq. (46) for the accelerations X is ##EQU54## The overall complexity of the composite body forward dynamics algorithm is O(.sup.3).
8.2 Computational Cost of the Articulated Body Forward Dynamics Algorithm
The articulated body forward dynamics algorithm is based on the recursions described in Eq. (78), Eq. (63) and Eq. (64). Since the computations in Eq. (78) can be carried out prior to the dynamics simulation, the cost of this recursion is not included in the cost of the overall forward dynamics algorithm described below:
1. The algorithm for the computation of the articulated body quantities is given in Eq. (63). The step involving the computation of D.sup.-1 (k) can be carried out either by an explicit inversion D(k) with O(n.sub.m.sup.3 (k)) cost, or by the indirect procedure described in Eq. (63) with O(n.sub.m.sup.2 (k)) cost. The first method is more efficient than the second one for n.sub.m (k).ltoreq.7.
Cost of Eq. (63) for the k.sup.th body based on the explicit inversion of D(k) (used when n.sub.m (k).ltoreq.7) is ##EQU55## Cost of Eq. (63) for the k.sup.th body based on the indirect computation of D.sup.-1 (k) (used when n.sub.m (k).gtoreq.8) is {12n.sub.m.sup.2 (k)+255n.sub.m (k)+572} M+{13n.sub.m.sup.2 (k)+182n.sub.m (k)
2. The cost for the tip-to-base recursion sweep in Eq. (64) for the k.sup.th body is {n.sub.m.sup.2 (k)+25n.sub.m (k)+49} M+{n.sub.m.sup.2 (k)+24n.sub.m (k)+50} A.
3. The cost for the base-to-tip recursion sweep in Eq. (64) for the k.sup.th body is {18n.sub.m (k)+52{19n.sub.m (k)+42{ A.
The overall complexity of this algorithm is O(Nn.sub.m.sup.2), where n.sub.m is an upper bound on the number of modes per body in the system.
From a comparison of the computational costs, it is clear that the articulated body algorithm is more efficient than the composite body algorithm as the number modes and bodies in the multibody system increases. The articulated body algorithm is faster by over a factor of 3 for 5 modes per body, and by over a factor of 7 for the case of 10 modes per body. The divergence between the costs for the two algorithms becomes even more rapid as the number of bodies is increased.
9. Extensions to General Topology Flexible Multibody Systems
For rigid multibody systems, [11] describes the extensions to the dynamics formulation and algorithms that are required as the topology of the system goes from a serial chain topology, to a tree topology and finally to a closed-chain topology system. The key to this progression is the invariance of the operator description of the system dynamics to increases in the topological complexity of the system. Indeed, as seen here, the operator description of the dynamics remains the same even when the multibody system contains flexible rather than rigid component bodies. Thus, using the approach in [11] for rigid multibody systems, the dynamics formulation and algorithms for flexible multibody systems with serial topology can be extended in a straightforward manner to systems with tree or closed-chain topology. Based on these observations, extending the serial chain dynamics algorithms described in this specification to tree topology flexible multibody systems requires the follow steps:
1. For each outward sweep involving a base to tip(s) recursion, at each body, the outward recursion must be continued along each outgoing branch emanating from the current body.
2. For each inward sweep involving a tip(s) to base recursion, at each body, the recursion must be continued inwards only after summing up contributions from each of the other incoming branches for the body.
A closed-chain topology flexible multibody system can be regarded as a tree topology system with additional closure constraints. As described in [11], the dynamics algorithm for closed-chain systems consists of recursions involving the dynamics of the tree topology system, and in addition the computation of the closure constraint forces. The computation of the constraint forces requires the effective inertia of the tree topology system reflected to the points of closure. The algorithm for closed-chain flexible multibody systems for computing these inertias is identical in form to the recursive algorithm described in [11].
10. Conclusions
This invention uses spatial operator methods to develop a new dynamics formulation for flexible multibody systems. A key feature of the formulation is that the operator description of the flexible system dynamics is identical in form to the corresponding operator description of the dynamics of rigid multibody systems. A significant advantage of this unifying approach is that it allows ideas and techniques for rigid multibody systems to be easily applied to flexible multibody systems. The Newton-Euler Operator Factorization of the mass matrix forms the basis for recursive algorithms such as those for the inverse dynamics, the computation of the mass matrix, and the composite body forward dynamics algorithm for the flexible multibody system. Subsequently, we develop the articulated body forward dynamics algorithm, which, in contrast to the composite body forward dynamics algorithm, does not require the explicit computations of the mass matrix. While the computational cost of the algorithms depends on factors such as the topology and the amount of flexibility in the multibody system, in general, the articulated body forward dynamics algorithm is by far the more efficient algorithm for flexible multibody systems containing even a small number of flexible bodies. All of the algorithms are closely related to those encountered in the domain of Kalman filtering and smoothing. While the major focus in this specification is on flexible multibody systems with serial chain topology, the extensions to tree and closed chain topologies are straightforward and are described as well.
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 departure from the true spirit of the invention.
Appendix A: Proofs of the Lemmas
At the operator level, the proofs of the lemmas in this publication are completely analogous to those for rigid multibody systems ([1, 2]).
Proof of Lemma 5.1: Using operators, we can rewrite Eq. (47) in the form
From Eq. (19) it follows that .PHI..sub.101 =.sub..PHI. .PHI.=.PHI.-I=.PHI.. Multiplying Eq. (65) from the left and right by .PHI. and .PHI.* respectively leads to
Proof of Lemma 6.1: It is easy to verify that .tau.P.tau.*=.tau.P. As a consequence, the recursion for P(.) in Eq. (51) can be rewritten in the form
Pre- and post-multiplying the above by .PHI. and .PHI.* respectively then leads to
Hence, ##EQU56## Proof of Lemma 6.2: Using a standard matrix identify we have that
Note that
from which it follows that
Using this with Eq. (67) it follows that
Proof of Lemma 7.1: From Eq. (42) and Eq. (43), the expression for the generalized accelerations X is given by
From Eq. (68) we have that
Thus, Eq. ({b 69) can be written as
From Eq. (66) it follows that
and so Eq. (71) simplifies to
From Eq. (68) we have that
Using this in Eq. (73) leads to the result.
Appendix B: Ruthless Linearization of Flexible Body Dynamics
It has been pointed out in recent literature ([17, 16]) that the use of modes for modeling body flexibility leads to "premature linearization" of the dynamics, in the sense that while the dynamics model contains deformation dependent terms, the geometric stiffening terms are missing. These missing geometric stiffening terms are the dominant terms among the first-order (deformation) dependent terms. In general, it is necessary to take additional steps to recover the missing geometric stiffness terms to obtain a "consistently" linearized model with the proper degree of fidelity. However for systems with low spin rate, there is typically little loss is model fidelity when the deformation and deformation rate dependent terms are dropped altogether from the dynamical equations of motion ([16]). Such models have been dubbed the ruthlessly linearized models. These linearized models are considerably less complex, and do not require most of the modal integrals data for each individual flexible body. In this model, the approximations to M.sub.m (k), a.sub.m (k), and b.sub.m (k) are as follows: ##EQU57## With this approximation, M.sub.m (k) is constant in the body frame, while a.sub.m (k) and b.sub.m (k) are independent of .eta.(k) and .eta.(k). With this being the case, the formation of D.sup.-1 in Eq. (51) can be simplified. Using the matrix identity.
which holds for general matrices, A, B and C, it is easy to verify that
where the matrices .LAMBDA.(k), .OMEGA.(k), and .UPSILON.(k) are precomputed just once prior to the dynamical simulation as follows: ##EQU58## Using Eq. (77) reduces the computational cost for computing the articulated body inertias to a quadratic rather than a cubic function of the number of modes.
Appendix C: Expressions for M.sub.m (k), a.sub.m (k) and b.sub.m (k)
The modal spatial displacement influence vector II.sup.j (k) for node j.sub.k has the structure: ##EQU59## The components of the vectors .lambda..sup.j (k) .epsilon. .sup.3.times.n.sbsp.m.sup.(k) and .gamma..sup.j (k) .epsilon. .sup.3.times.n.sbsp.m.sup.(k) are the modal slope displacement influence vector and the modal translational displacement influence vector respectively for node j.sub.k. They define the contribution of the various modes to the slope (or differential change in orientation) and translational deformation for the j.sub.k.sup.th node on the k.sup.th body. Define
Note that
where l.sub.0 (j.sub.k) denotes the undeformed vector from frame .sub.k to node j.sub.k. Note that M.sub.s (j.sub.k) denotes the spatial inertia of the j.sup.th node on the k.sup.th body and is given by ##EQU60## C.1 Modal Integrals for the Individual Bodies
Defined below are a set of modal integrals for the k.sup.th body which simplify the computation of the modal mass matrix M.sub.m (k) and the bias vector b.sub.m (k). These modal integrals can be computed as a part of the finite-element structural analysis of the individual bodies. ##EQU61## Note that
Also define, ##EQU62## C.2 Modal Mass Matrix
We have from Eq. (23) that the modal mass matrix of the k.sup.th body is given by ##EQU63## Define the matrices: ##EQU64## Also define the matrix G.sup.k .epsilon. .sup.n.sbsp.m.sup.(k).times.n.sbsp.m.sup.(k) so that its (r,s).sup.th element is given by the modal integral G.sup.k (r,s).
Using these matrices, and Eq. (84), it is easy to establish that ##EQU65## Hence, in block partitioned form ##EQU66## The superscript i=0,1,2 in M.sub.m.sup.i (k) denotes the order of dependency of the terms on the deformation variables.
C.3 Expression for a.sub.m (k)
In this section we derive explicit expressions for the Coriolis and centrifugal acceleration term a.sub.m (k). Since ##EQU67## it follows from Eq. (12) and Eq. (79) that ##EQU68## Recalling that the spatial velocity of frame .sub.k is ##EQU69## where .omega.(k) and .nu.(k) denote the angular and linear velocity respectively of .sub.k we have that ##EQU70## The vector above has bee partitioned so that the term on the top corresponds to modal accelerations, the term in middle to the angular accelerations and the term at the bottom to the linear acceleration of the body. Also ##EQU71## From Eq. (250 and the above expressions it follows that ##EQU72##
In the above a.sub.mR.sup.0 (k) denotes the deformation independent part of the Coriolis acceleration, while a.sub.mR.sup.1 (k), a.sub.mR.sup.2 (k) and a.sub.mR.sup.3 (k) denote the parts whose dependency on the deformation is up to first, second and third order respectively.
C.4 Expression for b.sub.m (k)
We have found Eq. (28) that ##EQU73## Since,
it follows that ##EQU74## Also from Eq. (31) we have that ##EQU75## From Eq. (37) we write ##EQU76## We develop expressions for b.sub..eta..sup.k (r), b.sub..omega..sup.k and b.sub..nu..sup.k in Eq. (93) below. From Eq. (92) and Eq. (93) we have that ##EQU77##
Using the modal integrals defined in Section C.1, the above terms can be expressed in the following manner: ##EQU78##
Once again from Eq. (92) and Eq. (93) we have that ##EQU79##
Once again, using modal integrals, the above terms can be reexpressed in the following manner: ##EQU80## This results in the following expression ##EQU81##
Using Eq. (92) and Eq. (93) it also follows that ##EQU82## and thus ##EQU83## Putting together Eq. (108), Eq. (121) and Eq. (129) we have that ##EQU84##
Claims
What is claimed is:
1. A method for controlling a manipulator relative to a desired manipulator motion, said manipulator comprising plural bodies including an outermost body, and a relatively stationary innermost body, said plural bodies being sequentially connected together by movable hinges disposed between each plural body so connected and servos controlling said movable hinges in accordance with servo command signals corresponding to specified body forces of respective ones of said plural bodies, at least some of said plural bodies being flexible in plural deformation modes corresponding to respective modal spatial influence vectors relating deformations of plural spaced nodes of respective plural bodies to said plural deformation modes, said method comprising the steps of: computing articulated body quantities for each of said plural bodies from respective modal spatial influence vectors; computing modal deformation accelerations of said plural spaced nodes of respective plural bodies and hinge accelerations of said movable hinges from said specified body forces, from said articulated body quantities and from said modal spatial influence vectors; comprising said modal deformation and hinge accelerations with said desired manipulator motion to determine an error, and correcting said specified body forces so as to reduce said error thereby producing corrected specified body forces; generating said servo command signals by converting in a processor means said corrected specified body forces to servo commands to correct manipulator motion to said desired manipulator motion, and transmitting said servo command signals to said servos.
2. The method of claim 1 wherein said step of computing articulated body quantities comprises, for each body beginning at said outermost body: computing a modal mass matrix; computing an articulated body inertia from the articulated body inertia of a previous body and from said modal mass matrix; computing an articulated hinge inertia from said articulated body inertia; computing an articulated body to hinge force operator from said articulated hinge inertia; computing a null force operator from said articulated body to hinge force operator.
3. The method of claim 2 wherein said step of computing a null force operator is followed by revising said articulated body inertia by transforming said articulated body inertia by said null force operator to produce a revised articulated body inertia.
4. The method of claim 2 wherein said plural bodies and movable hinges are characterized by respective vectors of deformation and hinge configuration variables, and wherein said computing modal deformation accelerations and hinge accelerations comprises: for each one of said plural bodies beginning with said outermost body: computing a residual body force from a residual body force of a previous body and from said vector of deformation and hinge configuration variables, computing a resultant hinge force from said specified body force and said residual body force, computing a resultant hinge acceleration from said resultant hinge force transformed by said articulated hinge inertia; and, for each one of said plural bodies beginning with said innermost body: computing a current modal body acceleration of a current body from a modal body acceleration of a previous body, computing a modal deformation acceleration and hinge acceleration from said resultant hinge acceleration and from said current modal body acceleration transformed by said articulated body to hinge force operator.
5. The method of claim 4 wherein: said step of computing a resultant hinge acceleration is followed by the step of revising said residual body force by said resultant hinge force transformed by said body to hinge force operator to produce a revised residual body force for use in said correcting of said specified body forces; and said step of computing a modal deformation acceleration and hinge acceleration is followed by the step of revising said current modal body acceleration based upon said modal deformation and hinge acceleration to produce a revised current modal body acceleration for use in said correcting of said specified body forces.
6. The method of claim 5 wherein all said computing comprises a single cycle corresponding to one of a succession of time steps, all said computing being repeated for subsequent time steps, wherein said vector of deformation and hinge configuration variables are computed from the modal deformations and hinge accelerations of a previous time step and wherein the revised articulated body inertia, revised residual body force and revised current modal body acceleration from the previous time step are used for computing in a current time step.
7. The method of claim 4 wherein said manipulator comprises joint sensors at each of said movable hinges, and wherein a hinge configuration portion of said vector of deformation and hinge configuration variables is derived from reading said joint sensors.
8. The method of claim 4 wherein said articulated body inertia, said articulated hinge inertia, said body to hinge force operator, said null force operator, said specified body force, said residual body force, said resultant hinge acceleration and said resultant hinge force each corresponds to a flexible and a rigid version thereof.
9. The method of claim 8 wherein said step of computing a resultant hinge force comprises computing the flexible version of said resultant hinge force from said specified body force, said flexible version of said residual body force and from said rigid version of said residual body force transformed by said modal spatial influence vector.
10. The method of claim 8 wherein said articulated body inertia comprises a rigid-flexible and rigid-rigid coupling components thereof, and wherein said method further comprises the step of revising said rigid version of said residual body force based upon a function of said rigid-rigid and rigid-flexible coupling components of said articulated body inertia and a flexible version of said articulated body inertia to produce a revised rigid version of said residual body force for use in said correcting of said specified body forces.
11. The method of claim 10 wherein said computing said articulated body inertia step comprises decomposing said modal mass matrix into rigid-flexible and rigid-rigid coupling components and computing said rigid-rigid and rigid-flexible coupling components of said articulated body inertia from respective ones of said rigid-rigid and rigid-flexible coupling components of said modal mass matrix.
12. The method of claim 11 wherein said computing said articulated body quantities step is preceded by the step of computing flexible and rigid versions of a deformation and hinge modal joint map matrix for each plural body, and wherein: the flexible version of said articulated hinge inertia is computed from said articulated body inertia transformed by the flexible version of the corresponding deformation and hinge modal joint map matrix; the rigid version of said articulated body inertia is computed from a function of said rigid-rigid and rigid-flexible coupling components of said articulated body inertia transformed by said flexible version of said corresponding deformation and hinge modal joint map matrix; the rigid version of said articulated body inertia is computed from said rigid version of said articulated body inertia; the rigid version of said body to hinge force operator is computed from said rigid versions of said articulated body inertia and said articulated hinge inertia.
13. The method of claim 12 wherein said computing flexible and rigid versions of a deformation and hinge modal joint map matrix step comprises computing a joint map matrix corresponding to unit vectors of said movable hinges and computing said deformation and hinge modal joint matrix from said joint map matrix and from said modal spatial influence vector.
14. The method of claim 8 wherein the flexible version of said resultant hinge acceleration is computed from the flexible versions of said articulated hinge inertia and resultant hinge force, and the rigid version of said resultant hinge acceleration is computed from the rigid versions of said articulated hinge inertia and resultant hinge force.
15. The method of claim 14 further comprising the step of revising said residual body force by adding to said residual body force a product of the rigid versions of said resultant hinge force and said body to hinge force operator to produce a revised residual body force for use in said correcting of said specified body forces.
16. Apparatus for controlling a manipulator relative to a desired manipulator motion based upon specified body forces, said manipulator comprising plural bodies including an outermost body, and a relatively stationary innermost body, said plural bodies being sequentially connected together by movable hinges disposed between each plural body so connected and servos controlling said movable hinges in accordance with servo command signals corresponding to specified body forces of respective ones of said plural bodies, at least some of said plural bodies being flexible in plural deformation modes corresponding to respective modal spatial influence vectors relating deformations of plural spaced nodes of respective plural bodies to said plural deformation modes, said apparatus comprising: means for computing articulated body quantities for each of said plural bodies from respective modal spatial influence vectors; means for computing modal deformation accelerations of said plural spaced nodes of respective plural bodies and hinge accelerations of said movable hinges from said specified body forces, from said articulated body quantities and from said modal spatial influence vectors; means for comparing said modal deformation and hinge accelerations with said desired manipulator motion so as to determine a motion discrepancy, and correcting said specified body forces so as to reduce said motion discrepancy; and means for generating said servo command signals by converting in a processor means said corrected specified body forces to servo commands to correct manipulator motion to said desired manipulator motion, and transmitting said servo command signals to said servos.
17. The apparatus of claim 16 wherein said means for computing articulated body quantities comprises a means, operative for each plural body, beginning at said outermost body for: computing a modal mass matrix; computing an articulated body inertia from the articulated body inertia of a previous body and from said modal mass matrix; computing an articulated body to hinge force operator from said articulated hinge inertia; computing a null force operator from said articulated body to hinge force operator.
18. The method of claim 17 further comprising means for revising said articulated body inertia by transforming said articulated body inertia by said null force operator to produce a revised articulated body inertia.
19. The apparatus of claim 17 wherein said plural bodies and movable hinges are characterized by respective vectors of deformation and hinge configuration variables, and wherein said means for computing modal deformation accelerations and hinge accelerations comprise: means operative for each one of said plural bodies beginning with said outermost body, for: computing a residual body force from a residual body force of a previous body and from said vector of deformation and hinge configuration variables, computing a resultant hinge force from said specified body force and said residual body force, computing a resultant hinge acceleration from said resultant hinge force transformed by said articulated hinge inertia; and, means operative for each one of said plural bodies beginning with said innermost body, for: computing a current modal body acceleration of a current body from a modal body acceleration of a previous body, computing a modal deformation acceleration and hinge acceleration from said resultant hinge acceleration and from said current modal body acceleration transformed by said articulated body to hinge force operator.
20. The apparatus of claim 19 further comprising: means for revising said residual body force by said resultant hinge force transformed by said body to hinge force operator to produce a revised residual body force for use in said correcting of said specified body forces; and means for revising said current modal body acceleration based upon said modal deformation and hinge acceleration to produce a revised current modal body acceleration for use in said correcting of said specified body forces.
21. The apparatus of claim 20 wherein said means for computing modal deformation accelerations of said plural spaced nodes of respective plural bodies and hinge accelerations of said movable hinges comprises means for computing said modal deformation accelerations and hinge accelerations once for each one of a succession of time steps, and wherein said means for computing modal deformation accelerations of said plural spaced nodes of respective plural bodies and hinge accelerations of said movable hinges further comprises means for computing said vector of deformation and hinge configuration variables from the modal deformations and hinge accelerations of a previous time step and wherein the revised articulated body inertia, revised residual body force and revised current modal body acceleration from the previous time step are used for computing said modal deformation accelerations and hinge accelerations during a current time step.
22. The apparatus of claim 19 further comprising means connected to joint sensors at each of said movable hinges for producing a hinge configuration portion of said vector of deformation and hinge configuration variables.
23. The apparatus of claim 19 wherein said articulated body inertia, said articulated hinge inertia, said body to hinge force operator, said null force operator, said specified body force, said residual body force, said resultant hinge acceleration and said resultant hinge force each comprises at least one of a flexible and rigid version thereof.
24. The apparatus of claim 23 wherein said means for computing a resultant hinge force comprises means for computing the flexible version of said resultant hinge force from said specified body force, said flexible version of said residual body force and from said rigid version of said residual body force transformed by said modal spatial influence vector.
25. The apparatus of claim 23 further comprising means for revising said rigid version of said residual body force based upon a function of rigid-rigid and rigid-flexible coupling components of said articulated body inertia and a flexible version of said articulated body inertia to produce a revised rigid version of said residual body force for use in said correcting of said specified body forces.
26. The apparatus of claim 25 wherein said means for computing said articulated body inertia comprises means for decomposing said modal mass matrix into rigid-flexible and rigid-rigid coupling components and for computing said rigid-rigid and rigid-flexible coupling components of said articulated body inertia from respective ones of said rigid-rigid and rigid-flexible coupling components of said modal mass matrix.
27. The apparatus of claim 26 further comprising means for computing flexible and rigid versions of a deformation and hinge modal joint map matrix for each plural body, and further comprising: a means for computing the flexible version of said articulated hinge inertia, comprising means for computing the flexible version of said articulated hinge inertia from said articulated body inertia transformed by the flexible version of the corresponding deformation and hinge modal joint map matrix; a means for computing the rigid version of said articulated body inertia, comprising means for computing the rigid version of said articulated body inertia from a function of said rigid-rigid and rigid-flexible coupling components of said articulated body inertia transformed by said flexible version of said corresponding deformation and hinge modal joint map matrix; a means for computing the rigid version of said articulated hinge inertia, comprising means for computing the rigid version of said articulated hinge inertia from said rigid version of said articulated body inertia; a means for computing the rigid version of said body to hinge force operator, comprising means for computing the rigid version of said body to hinge force operator from said rigid versions of said articulated body inertia and said articulated hinge inertia.
28. The apparatus of claim 27 wherein said means for computing flexible and rigid versions of a deformation and hinge modal joint map matrix comprises means for computing a joint map matrix corresponding to unit vectors of said movable hinges and means for computing said deformation and hinge modal joint map matrix from said joint map matrix and from said modal spatial influence vector.
29. The apparatus of claim 23 further comprising a means for computing the flexible version of said resultant hinge acceleration from the flexible versions of said articulated hinge inertia and resultant hinge force, and a means for computing the rigid version of said resultant hinge acceleration from the rigid versions of said articulated hinge inertia and resultant hinge force.
30. The apparatus of claim 29 further comprising means for revising said residual body force by adding to said residual body force a product of the rigid versions of said resultant hinge force and said body to hinge force operator to create a revised residual body force for use in said correcting of said specified body forces.
31. A manipulator controller for a manipulator responsive to specified body forces, said manipulator comprising plural bodies including an outermost body, and an innermost body, said plural bodies being sequentially connected together by movable hinges, disposed between each plural body so connected and servos controlling said movable hinges in accordance with servo command signals corresponding to specified body forces of respective ones of said plural bodies, at least some of said plural bodies being flexible in plural deformation modes corresponding to respective modal spatial influence vectors relating deformations of plural spaced nodes of respective plural bodies to said plural deformation modes, said manipulator controller comprising: means for computing articulated body quantities for each of said plural bodies from respective modal spatial influence vectors; means for computing modal deformation accelerations of said plural spaced nodes of respective plural bodies and hinge accelerations of said movable hinges from said specified body forces, from said articulated body quantities and from said modal spatial influence vectors; means for comparing said modal deformation and hinge accelerations with said desired manipulator motion so as to determine a motion discrepancy, and correcting said specified body forces so as to reduce said motion discrepancy; and means for generating said servo command signals by converting in a processor means said corrected specified body forces to servo commands to correct manipulator motion to a desired manipulator motion, and transmitting said servo command signals to said servos.
32. The apparatus of claim 31 wherein said means for computing articulated body quantities comprises a means, operative for each plural body, beginning at said outermost body for: computing a modal mass matrix; computing an articulated body inertia from the articulated body inertia of a previous body and from said modal mass matrix; computing an articulated hinge inertia from said articulated body inertia; computing an articulated body to hinge force operator from said articulated hinge inertia; computing a null force operator from said articulated body to hinge force operator.
33. The method of claim 32 further comprising means for revising said articulated body inertia by transforming said articulated body inertia by said null force operator to produce a revised articulated body inertia.
34. The apparatus of claim 32 wherein said plural bodies and movable hinges are characterized by respective vectors of deformation and hinge configuration variables, and wherein said means for computing modal deformation accelerations and hinge accelerations comprise: means operative for each one of said plural bodies beginning with said outermost body, for: computing a residual body force from a residual body force of a previous body and from said vector of deformation and hinge configuration variables, computing a resultant hinge force from said specified body force and said residual body force, computing a resultant hinge acceleration from said resultant hinge force transformed by said articulated hinge inertia; and, means operative for each one of said plural bodies beginning with said innermost body, for: computing a current modal body acceleration of a current body from a modal body acceleration of a previous body, computing a modal deformation acceleration and hinge acceleration from said resultant hinge acceleration and from said current modal body acceleration transformed by said articulated body to hinge force operator.
35. The apparatus of claim 34 further comprising: means for revising said residual body force by said resultant hinge force transformed by said body to hinge force operator to produce a revised residual body force for use in said correcting of said specified body forces; and means for revising said current modal body acceleration based upon said modal deformation and hinge acceleration to produce a revised current modal body acceleration for use in said correcting of said specified body forces.
36. The apparatus of claim 35 wherein said means for computing modal deformation accelerations of said plural spaced nodes of respective plural bodies and hinge accelerations of said movable hinges comprises means for computing said modal deformation accelerations and hinge accelerations once for each one of a succession of time steps, and wherein said means for computing modal deformation accelerations of said plural spaced nodes of respective plural bodies and hinge accelerations of said movable hinges further comprises means for computing said vector of deformation and hinge configuration variables from the modal deformations and hinge accelerations of a previous time step and wherein the revised articulated body inertia, revised residual body force and revised current modal body acceleration from the previous time step are used for computing said modal deformation accelerations and hinge accelerations during a current time step.
37. The apparatus of claim 34 further comprising means connected to joint sensors at each of said movable hinges for producing a hinge configuration portion of said vector of deformation and hinge configuration variables.
38. The apparatus of claim 34 wherein said articulated body inertia, said articulated hinge inertia, said body to hinge force operator, said null force operator, said specified body force, said residual body force, said resultant hinge acceleration and said resultant hinge force each comprises at least one of a flexible and rigid version thereof.
39. The apparatus of claim 38 wherein said means for computing a resultant hinge force comprises means for computing the flexible version of said resultant hinge force from said specified body force, said flexible version of said residual body force and from said rigid version of said residual body force transformed by said modal spatial influence vector.
40. The apparatus of claim 38 further comprising means for revising said rigid version of said residual body force based upon a function of rigid-rigid and rigid-flexible coupling components of said articulated body inertia and a flexible version of said articulated body inertia to produce a revised rigid version of said residual body force for use in said correcting of said specified body forces.
41. The apparatus of claim 40 wherein said means for computing said articulated body inertia comprises means for decomposing said modal mass matrix into rigid-flexible and rigid-rigid coupling components and for computing said rigid-rigid and rigid-flexible coupling components of said articulated body inertia from respective ones of said rigid-rigid and rigid-flexible coupling components of said modal mass matrix.
42. The apparatus of claim 41 further comprising means for computing flexible and rigid versions of a deformation and hinge modal joint map matrix for each plural body, and further comprising: a means for computing the flexible version of said articulated hinge inertia, comprising means for computing the flexible version of said articulated hinge inertia from said articulated body inertia transformed by the flexible version of the corresponding deformation and hinge modal joint map matrix; a means for computing the rigid version of said articulated body inertia, comprising means for computing the rigid version of said articulated body inertia from a function of said rigid-rigid and rigid-flexible coupling components of said articulated body inertia transformed by said flexible version of said corresponding deformation and hinge modal joint map matrix; a means for computing the rigid version of said articulated hinge inertia, comprising means for computing the rigid version of said articulated hinge inertia from said rigid version of said articulated body inertia; a means for computing the rigid version of said body to hinge force operator, comprising means for computing the rigid version of said body to hinge force operator from said rigid versions of said articulated body inertia and said articulated hinge inertia.
43. The apparatus of claim 42 wherein said means for computing flexible and rigid versions of a deformation and hinge modal joint map matrix comprises means for computing a joint map matrix corresponding to unit vectors of said movable hinges and means for computing said deformation and hinge modal joint map matrix from said joint map matrix and from said modal spatial influence vector.
44. The apparatus of claim 38 further comprising a means for computing the flexible version of said resultant hinge acceleration from the flexible versions of said articulated hinge inertia and resultant hinge force, and a means for computing the rigid version of said resultant hinge acceleration from the rigid versions of said articulated hinge inertia and resultant hinge force.
45. The apparatus of claim 44 further comprising means for revising said residual body force by adding to said residual body force a product of the rigid versions of said resultant hinge force and said body to hinge force operator to create a revised residual body force for use in said correcting of said specified body forces.
Patent Citations (20)
| Patent | Date | Inventor | Cited By |
|---|---|---|---|
| US4132318 | 1979-01-01 | Wang et al. | |
| US4156835 | 1979-05-01 | Whitney et al. | |
| US4362978 | 1982-12-01 | Pollard et al. | |
| US4547858 | 1985-10-01 | Horak | |
| US4594671 | 1986-06-01 | Sugimoto et al. | |
| US4680519 | 1987-07-01 | Chand et al. | |
| US4705999 | 1987-11-01 | Soji et al. | |
| US4706204 | 1987-11-01 | Hittori | |
| US4725942 | 1988-02-01 | Osuka | |
| US4725965 | 1988-02-01 | Keenan | |
| US4756662 | 1988-07-01 | Tanie et al. | |
| US4794547 | 1988-12-01 | Nishida | |
| US4808063 | 1989-02-01 | Haley | |
| US4835710 | 1989-05-01 | Schnelle et al. | |
| US4925312 | 1990-05-01 | Onaga et al. | |
| US4937759 | 1990-06-01 | Vold | |
| US4972347 | 1990-11-01 | Tarvin et al. | |
| US5020001 | 1991-05-01 | Yamamoto et al. | |
| US5053687 | 1991-10-01 | Merlet | |
| US5130632 | 1992-07-01 | Ezawa et al. |
Non-Patent Literature (21)
- Rodriquez, G., Kreutz, K., and Jain, A., "A Spatial Operator Algebra for Manipulator Modeling and Control," The International Journal of Robotics Research, Vol. 10, No. 4, Aug. 1991, pp. 371-381.
- Jain, A., "Unified Formulation of Dynamics for Serial Rigid Multibody Systems," Journal of Guidance, Control and Dynamics, Vol. 14, No. 3, May-Jun. 1991, pp. 531-542.
- Kim, S. S. and Haug, E. J., "A Recursive Formulation for Flexible Multibody Dynamics, Part I: Open-Loop Systems," Computer Methods in Applied Mechanics and Engineering, Vol. 71, No. 3, 1988, pp. 293-314.
- Changizi, K. and Shabana, A. A., "A Recursive Formulation for the Dynamics Analysis of Open Loop Deformable Multibody Systems," ASME J1. of Applied Mechanics, vol. 55, No. 3, Sep. 1988, pp. 687-693.
- Keat, J. E., "Multibody System Order n Dynamics Formulation Based in Velocity Transform Method," Journal of Guidance, Control and Dynamics, vol. 13, No. 2, Mar.-Apr. 1990.
- Bodley, C. S., Devers, A. D., Park, A. C., and Frisch, H. P., "A Digital Computer Program for the Dynamics Interaction Simulation of Controls and Structure (DISCOS)," NASA Technical Paper 1219, NASA, May 1978.
- Singh, R. P., VanderVoort, R. J., and Likins, P. W., "Dynamics of Flexible Bodies in Tree Topology--Q Computer-Oriented Approach," Journal of Guidance, Control and Dynamics, vol. 8, No. 5, Sep., 1985, pp. 584-590.
- Rodriquez, G., "Kalman Filtering, Smoothing and Recursive Robot Arm Forward and Inverse Dynamics," IEEE Journal of Robotics and Automation, vol. 3, No. 6, Dec. 1987, pp. 624-639.
- Likins, P. W., "Modal Method for Analysis of Free Rotations of Spacecraft," AIAA Journal, vol. 5, Jul. 1967, pp. 1304-1308.
- Jain, A. and Rodriquez, G., "Recursive Dynamics for Flexible Multibody Systems using Spatial Operators," JPL (internal) Publication 90-26, Jet Propulsion Laboratory, Pasadena, CA, Dec. 1990.
- Rodriquez, G., Jain, A., and Kruetz, K., "Spatial Operator Algebra for Multibody System Dynamics," Journal of the Astronautical Sciences, vol. 40, No. 1, Jan.-Mar. 1992.
- Walker, M. W. and Orin, D. E., "Efficient Dynamic Computer Simulation of Robotic Mechanisms," ASME Journal of Dynamic Systems, Measurement, and Control, vol. 104, No. 3, Sep. 1982, pp. 205-211.
- Luh, J. Y. S., Walker, M. W., and Paul, R. P. C., "On-line Computational Scheme for Mechanical Manipulators," ASME Journal of Dynamic Systems, Measurement, and Control, vol. 102, No. 2, Jun. 1980, pp. 69-76.
- Jain, A. and Rodriquez, "Kinematics and Dynamics of Under-Actuated Manipulators," IEEE International Conference on Robotics and Automation, Sacramento, CA, Apr. 1991.
- Featherstone, R., "The Calculation of Robot Dynamics using Articulated-Body Inertias," The International Journal of Robotics Research, vol. 2, No. 1, Spring 1983, pp. 13-30.
- Padilla, E. E. and von Flotow, A. H., "Nonlinear Strain--Displacement Relations and Flexible Multibody Dynamics," Proceedings of the 3rd Annual Conference on Aerospace Computational Control, vol. 1, Oxnard, CA, pp. 230-245, Aug. 1989.
- Kane, T. R., Ryan, R. R., and Banerjee, A. K., "Dynamics of a Cantilevered Beam attached to a Moving Base," Journal of Guidance, Control and Dynamics, vol. 10, No. 2, Mar.-Apr. 1987, pp. 139-151.
- Jain, Rodriguez "Recursive Dynamics for Flexible Multibody Systems Using Spatial Operators"JPL Pub 90-26 Dec. 1990.
- Jain, "Unified Formulation of Dynamics for Serial Rigid Multibody Systems"Journal of Guidance, Control & Dynamics vol. 14 #3 May-Jun. 1991.
- Singh et al "Dynamics of Flexible Bodies in Tree Topologies--A Computer-Oriented Approach" Journal of Guidance, Control & Dynamics vol. 8 #5 Sep. 1985.
- Rodriguez, "Spatial Operator Approach to Flexible Manipulator Inverse & Forward Dynamics", IEEE Computer Soc. Press vol. 3 xxxii&2184 1990 pp. 845-850.