# Patent

## Title
Regenerative actuation in motion control

## Abstract

A motion controlled system for a robotic system stores energy when the robotic system uses negative power and releases the stored energy when the robotic system uses positive power. The motion controlled system calculates the power for causing the robotic system to follow a prescribed motion and applies the appropriate control signals to the robotic system in response to detected parameters of the robotic system.

## Background
RELATED APPLICATIONS
This application claims the benefit of and priority from U.S. provisional application No. 60/689,502 which is incorporated by reference herein in its entirety.


This application is related to U.S. patent application Ser. No. 11/402,487 filed on Apr. 11, 2006; U.S. patent application Ser. No. 11/395,654, filed on Mar. 30, 2006; U.S. patent application Ser. No. 11/038,978 filed on Jan. 19, 2005; U.S. patent application Ser. No. 11/038,692 filed on Jan. 19, 2005; U.S. patent application Ser. No. 11/038,691 filed on Jan. 19, 2005; U.S. patent application Ser. No. 10/151,647 filed on May 16, 2002; U.S. provisional application No. 60/301,891 filed on Jun. 29, 2001; No. 60/353,378 filed on Jan. 31, 2002; and No. 60/670,732 filed on Apr. 12, 2005, the contents of all are incorporated by reference herein in their entirety.
FIELD OF THE INVENTION
The present invention relates to motion control, and more particularly to motion control with regenerative actuation.
BACKGROUND OF THE INVENTION
Locomotion of robotic or bio-robotic systems such as humanoid robots or exoskeletons typically involves periods of energy generation and energy absorption at each body segment. The generated energy is associated with positive work and is typically produced from actuation, external forces, and/or the natural dynamics of the system. The absorbed energy is associated with negative work, or the energy that is to be dissipated by antagonist actuation, external forces, or through passive structures in the system. In humans, the energy absorption phase, typically associated with deceleration of the limbs is achieved through a combination of antagonist muscle activation and the passive elements in the muscles and other supporting structures in the joint. In robotic systems, such as humanoids, the energy is sometimes absorbed through motor clutches, passive structures in the system, or by reverse actuation. In reverse actuation, the motors generate torque that opposes the angular velocity of the segment. However, generating torque that opposes the angular velocity of the segment is highly inefficient in terms of power consumption.
What is needed is a system and method for providing more efficient power consumption for robotic and bio-robotic systems.
SUMMARY OF THE INVENTION
A method for controlling a motion controlled system includes computing power for executing a motion that may or may not be predetermined; generating a first control signed to apply energy to the motion controlled system if the computed power is above a first predetermined level; generating a second control signal to draw energy from the motion controlled system if the computed power is below a second predetermined level; and generating a third control signal to store the energy drawn from the motion controlled system. The motion controlled system may be a robotic joint.
The features and advantages described in the specification are not all inclusive and, in particular, many additional features and advantages will be apparent to one of ordinary skill in the art in view of the drawings, specification, and claims. Moreover, it should be noted that the language used in the specification has been principally selected for readability and instructional purposes, and may not have been selected to delineate or circumscribe the inventive subject matter.



BRIEF DESCRIPTION OF THE DRAWINGS
FIG. 1 is a block diagram illustrating a motion controlled system for controlling a robotic/bio-robotic system according to one embodiment of the present invention.
FIG. 2 is a diagram illustrating a free body diagram of human lower extremity for one side of the body with forces and moments acting thereon.
FIG. 3 is a timing diagram of ankle joint power generated or absorbed during one cycle of gait.
FIG. 4 is a timing diagram of knee joint power generated or absorbed during one cycle of gait.
FIG. 5 is a timing diagram of hip joint power generated or absorbed during one cycle of gait.
FIG. 6 is a block diagram of a robot that includes an inverted pendulum controlled by a pair of actuators according to the present invention.
FIG. 7 is a free body diagram illustrating of a single body with forces and moments acting thereon.
FIG. 8 is a flowchart illustrating one embodiment of a methodology for controlling power to and from the robot of FIG. 1 according to the present invention.



DETAILED DESCRIPTION
A preferred embodiment of the present invention is now described with reference to the figures where like reference numbers indicate identical or functionally similar elements. Also in the figures, the left most digits of each reference number corresponds to the figure in which the reference number is first used.
Reference in the specification to “one embodiment” or to “an embodiment” means that a particular feature, structure, or characteristic described in connection with the embodiments is included in at least one embodiment of the invention. The appearances of the phrase “in one embodiment” in various places in the specification are not necessarily all referring to the same embodiment.
The system and method provides a motion controlled system for a robotic system that stores energy when the robotic system uses negative power and that releases the stored energy when the robotic system uses positive power. The motion controlled system calculates the power for causing the robotic system to follow a prescribed motion and applies the appropriate control signals to the robotic system in response to detected parameters of the robotic system.
For a robotic system for human locomotion, the system absorbs and stores energy in an ankle and decelerates when a heel strikes the ground. As the gait continues, the system releases the stored energy.
1 System
FIG. 1 is a block diagram illustrating a motion controlled system 100 for controlling a robotic/bio-robotic system according to one embodiment of the present invention. The motion controlled system 100 comprises a controller 102, a robot 104, and a summer 106. The robot 104 may be, for example, a humanoid robot, or a wearable human exoskeleton system. The robot 104 includes actuators for controlling the motion of the robot in response to control signals from the controller 102. The controller 102 generates the control signals based on a desired motion and feedback, such as position and angular velocity, from the robot 104. The controller 102 may be a processor of any known type.
In one embodiment, the robot 104 is an orthosis controlled by an actuator, such as an Ankle Foot Orthosis. The initial step in the control process of the controller 102 may be to generate the desired trajectory of the robot 104. One embodiment of the present invention provides several methods for computing a trajectory in multi-dimensional space that describes the desired motion of the robot 104. A trajectory described in joint space can be specified by the vector q, and determines the motion of each degree of freedom, its velocity, and its acceleration. Alternatively, a trajectory can be described in task space by a set of Cartesian variables which are related to joint varibles q. For example, depending on the application, the desired motion of the system can be planned off-line, or generated at run-time using a variety of trajectory generation schemes. In another embodiment, the user specifies only the desired goal position and orientation of a point on the robot, and the controller 102 determines the exact shape of the path to get there, the duration, the velocity profile, and other details.
Many types of controllers 102 can be designed to achieve the desired motion. For example, an inverse dynamics based tracking controller can be used to generate the control commands that when applied to the robot, will reproduce the desired motion. For example, the controller 102 may be one of those described in the related patent applications cited above and incorporated herein by reference.
The control objective may not necessarily be trajectory tracking. Other performance criteria such as stability, balance, energy minimization, etc. may also be used when designing the controller 102. In general, the function of the controller 102 is to produce the control action (typically specified as a force or torque) that will generate a motion satisfying a specified performance criterion. The control action is transformed to a quantity that is accepted as input by the actuators of the robot 104, such position commands, current, or voltage.
As an overview, controlled motion of any physical system (or load) involves periods of energy generation as well as energy absorption. Energy is generated in order for a physical system to do positive work while energy is absorbed if the work is negative. Energy can be put into a system in different ways. For example, externally powered actuators or external forces may deliver energy to a system. Energy may also be added as a result of the system's natural dynamics. For example, gravitational forces can convert potential energy into kinetic energy, thereby producing energy. Like energy generation, energy may also be absorbed in different ways. In motion control, a typical way to absorb (or dissipate) energy is through externally powered antagonistic (or reverse) actuation. Energy may also be dissipated by an external force. In one embodiment, energy is absorbed through passive structures, such as springs and dampers.
During periods of energy generation or absorption, actuators in most physical systems use supply power, regardless of the direction in the power flow. In principal, there is no need to supply power to the actuators during the energy absorption phase if, for example, the absorbed energy can be precisely controlled and dissipated passively. Furthermore, the dissipated energy can be potentially stored and later recovered by a regenerative mechanism. The ability to identify the generation or absorption phase during the operation of the system, coupled with an approach to passively control and regenerate the absorbed energy can significantly reduced the power requirements in motion controlled systems.
Although, energy regeneration in hybrid-electric vehicles is an example where energy is recovered when a load is decelerated, that technology currently does not involve the motion control problem considered here. In particular, the control action to decelerate the vehicle is not based on tracking a prescribed motion profile. Rather, an integral part of the controller is a human in-the-loop which provides the necessary feedback to decelerate the vehicle. Furthermore, the bandwidth of the control in hybrid-electric vehicles is much different from the majority of applications using motion control. The system 100 of the present invention is described for the control of single, or multi-link rigid body mechanical systems, such as industrial robots, humanoid robots, or wearable human exoskeletons. However, the principles of regenerative actuation and control of the system 100 apply to control of any physical system.
The energetics of motion controlled systems are described herein through power analysis. A systematic procedure identifies the energy absorption phase and the energy generation phase of the system 100 that is controlled to follow a prescribed motion. Based on these criteria, a hybrid actuator system may be designed and integrated into the robot 104 of the motion controlled system whereby the actuation mechanism provides active control, or passive control and regeneration, depending on the direction of the power flow. One example is an actuator/control mechanism to regeneratively control a single link inverted pendulum by using spring based actuation as described below in conjunction with FIG. 6. Spring based actuation concepts have the advantage that springs come in a variety of shapes and sizes and are available in large quantities. An actuator constructed upon the properties of a spring is intrinsically compliant and can dissipate energy. Furthermore, the use of a spring as part of the actuator allows the use of stored energy. One of the first realizations of a compliant actuator was the MIT Series Elastic Actuator. See, for example, G. A. Pratt and W. M. Williamson, “Series elastic actuators”, IEEE Int'l Conf. on Intelligent Robots and Systems, pages 399-406 (1995), the contents of which are incorporated herein in their entirety. However, the spring stiffness in the Series Elastic Actuator could not be modulated. A spring actuator that can adjust its stiffness will have a resonance behavior over a range of frequencies. In order for an actuator to passively control the force or torque, a variable compliance actuator is used. One such actuator is described in J. W. Hurst, J. E. Chestnut, and A. A. Rizzi, “An actuator with physically variable stiffness for highly dynamic legged locomotion”, IEEE Int'l Conf. on Robotics and Automation, pages 4662-4667, New Orleans, La., (2004), the contents of which are incorporated herein in their entirety.
2 Energetic Studies of Human Locomotion
The mechanical energies are excellent means of quantifying and describing the efficiencies associated with the controller 102. The instantaneous power calculation at each joint reveals whether energy is absorbed or generated. If the computed power at a given joint is positive, then the actuators are doing positive work and energy is delivered to the segment. If joint power is negative, then energy is absorbed by the system. The controller 102 controls the delivery of power to the segment and the absorption of the energy by the system.
In order to illustrate the positive and negative power flows, the energetics of human motion during gait are considered. Human motion analysis is compelling because the body segment parameters, as well as kinesiological measurements such as ground reaction forces and motion capture are readily available, providing the necessary inputs to determine joint torques and joint powers. Furthermore, human motion analysis provides insight about the bandwidth requirements in applications involving humanoid robots, or wearable human exoskeleton systems.
FIG. 2 is a diagram illustrating a free body of human lower extremity for one side of the body with forces and moments acting thereon. FIG. 7 is a free body diagram illustrating of a single body with forces and moments acting thereon. The free body of FIG. 2 may be analyzed as single bodies such as shown in FIG. 2.
For the sake of clarity, the pelvis, thigh, shank and foot are shown spaced apart, but these parts are coupled at joints C2, C3 and C4. The body segments are enumerated starting with the pelvis (B1) and working outward to the foot (B4) and the Bi pointing to the center of mass of the body segment. The body segments have a mass mi. The ground reaction force and moment, denoted by Γgr and Ngr respectively, correspond to the reaction loads acting at the distal end of the last segment in the chain, for example, the foot. The point of application of the ground reaction force is the center of pressure (cop).
To determine the joint powers at the ankle, knee, and hip, standard gait measurements including motion capture and force plate data were obtained from the Gait CD. See, for example, C. L. Vaughan, B. Davis, and J. O'Connor. Dynamics of Human Gait, Kiboho Publishers, Cape Town South Africa, 2nd edition (1999), the contents of which are incorporated herein in their entirety. The gait data obtained from the Gait CD contained 15 lower extremity marker data based on the Helen Hayes marker set, anthropometric measurements such as limb-dimensions and body bass, and force plate data under two feet. The marker and anthropometric data were used to calculate the body segment parameters and joint centers based on the statistical regression equations provided by Vaughan et al, noted above. The marker data were filtered using a second order Butterworth low-pass filter with a cut-off frequency of 6 HZ. The force plate data was filtered at 50 HZ. The sampling frequency of the marker data was 60 HZ. The Euler angles and center of gravity of each body segment were calculated in order to determine the generalized coordinates (or configuration) of each segment.
At a particular joint, the inner product of the joint moments (denoted by N) and the joint angular velocities, (denoted by W) are then used to determine the joint powers. The power P is calculated as follows:

P=NTW  (1)

FIG. 3 is a timing diagram of ankle joint power generated or absorbed during one cycle of gait. FIG. 4 is a timing diagram of knee joint power generated or absorbed during one cycle of gait. FIG. 5 is a timing diagram of hip joint power generated or absorbed during one cycle of gait.
A kinematics and inverse dynamics procedure, as described in U.S. Patent Application Publication US2005/0209535, filed as U.S. patent application Ser. No. 11/038,692, the contents of which are incorporated herein in their entirety, may be used to calculate, respectively, the angular velocity W and joint moments N at the ankle, knee, and hip. The associated joint powers can be calculated from Equation 1. The results are illustrated in FIGS. 3-5, which clearly illustrate that in human gait, the muscles generate and absorb energy during the entire gait cycle. Each period of energy generation and absorption can be very short (a fraction of a second); therefore, control with large bandwidths are essential by the muscle actuators. Consider, for example, the ankle power plot. At heel strike (which occurs at t=0.25 sec), the ankle joint power absorbs energy to decelerate the limb for about 0.5 seconds. During this phase, the muscle actuators at the ankle are doing negative work. The ankle is doing negative work approximately 50% of the time.
3 Inverted Pendulum Model
FIG. 6 is a diagram illustrating a regenerative actuation and motion controlled system 600 including a simple planar inverted pendulum according to one embodiment of the present invention. A more complex multi-link system, such as the system of FIG. 2, can be viewed as a series of coupled inverted pendulums stacked on top of each other. See, for example, B. Dariush and K. Fujimura, “Fuzzy logic based control model of human postural dynamics”, SAE Conference on Digital Human Modeling for Design and Engineering, Detroit, Mich. (2000), the contents of which are incorporated herein in their entirety. (See also U.S. Pat. No. 6,633,783, the contents of which are incorporated herein in their entirety.).
The regenerative actuation and motion controlled system 600 comprises a pendulum 602 that is constrained on a base 603 at a pivot point 604 and further comprises actuators 606-1 and 606-2 that generate respective forces F1 and F2 which rotate the pendulum 602 clockwise or counterclockwise, respectively. Suppose the forces F1 and F2 correspond to two separate actuator forces that are actively or passively generated. In one embodiment, a single actuator 606 generates the forces F1 and F2. For simplicity, and without loss of generality, assume the line of action of the forces F1 and F2 is along the circumference of a circle centered at the joint pivot 604 and of radius d, which represents the moment arm of the two forces. In one embodiment, the actuators 606 comprise cable and pulley systems. In humans, the analogy can be made to the control of a sagittal arm by a flexor/extensor muscle pair. See, for example, J. A. Dinneen and H. Hemami, “Stability and movement of a one-link neuro-musculo-skeletal sagittal arm”, IEEE Transactions on Biomedical Engineering, 40(6):541-548 (1993), the contents of which are incorporated herein in their entirety. Analogous to human muscles that produce a force under contraction of muscle fibers, in one embodiment, the actuators 606 produce a positive force under compression.
The physical parameters of the system 600 are the mass m of the pendulum 602, considered to be concentrated at a distance k from the axis (pivot 604) of the joint, and the moment of inertia I. The angle θ is measured from the vertical and is positive in the clockwise direction.
The effective length and incremental length of the actuators 606-1 and 606-2 that generate the respective forces F1 and F2 as a function of θ are given by Equations (2) and (3):




  
    
      
        L
        =
        
          
            L
            o
          
          +
          
            
              
                ∂
                
                  L
                  T
                
              
              
                ∂
                θ
              
            
            ⁢
            
              (
              
                θ
                -
                
                  θ
                  o
                
              
              )
            
          
        
      
    
    
      
        (
        2
        )
      
    
  
  
    
      
        dL
        =
        
          
            
              ∂
              
                L
                T
              
            
            
              ∂
              Θ
            
          
          ⁢
          d
          ⁢
          
            
          
          ⁢
          θ
        
      
    
    
      
        (
        3
        )
      
    
  




where L=[L1 L2]T correspond to the vector form of lengths in the actuators 606-1 and 606-2, respectively, Lo is the 2×1 vector corresponding to the initial length of the actuators 606, and θo is angle at the initial length. The term ∂LT/∂Θ represents the moment arm matrix and is given by Equation (4):





  
    
      
        
          
            ∂
            
              L
              T
            
          
          
            ∂
            Θ
          
        
        =
        
          [
          
            
              
                d
              
              
                
                  -
                  d
                
              
            
          
          ]
        
      
    
    
      
        (
        4
        )
      
    
  




The relationship between the joint torque and the actuator forces is given by Equation (5):




  
    
      
        τ
        =
        
          
            
              -
              
                
                  ∂
                  
                    L
                    T
                  
                
                
                  ∂
                  Θ
                
              
            
            ⁢
            F
          
          =
          
            d
            ⁡
            
              (
              
                
                  F
                  2
                
                -
                
                  F
                  1
                
              
              )
            
          
        
      
    
    
      
        (
        5
        )
      
    
  
  
    
      where
    
    
      
        
      
    
  
  
    
      
        F
        =
        
          
            [
            
              
                
                  
                    F
                    1
                  
                
                
                  
                    F
                    2
                  
                
              
            
            ]
          
          T
        
      
    
    
      
        (
        6
        )
      
    
  




Note the convention is used that positive torque is in the clockwise direction. The dynamic equations of motion for this system are described by:

I{umlaut over (θ)}−mkg sin(θ)=τ  (7)

where g is the gravitational acceleration constant. A model based method to control the inverted pendulum 602 to track a desired trajectory θd is given by,

τ=I{umlaut over (θ)}*−mkg sin(θ)  (8)

where {umlaut over (θ)}* is given by,

{umlaut over (θ)}*={umlaut over (θ)}d+Kd({dot over (θ)}d−{dot over (θ)})+Kp(θd−θ)  (9)

The terms Kp and Kd are positive definite position and derivative feedback gains, respectively.

In an alternative embodiment to the model based control approach, the motion of the pendulum 602 can be controlled using one of many different control laws, such as a proportional-integral-derivative (PID) controller given by Equation (10):

τ=Kp(θd−θ)+KI∫(θd−θ)dt+Kd({dot over (θ)}d−{dot over (θ)})  (10)

where KI is the integral feedback gain. The tracking performance of the control law in Equation 10 may be inferior to that of Equation 8; however, Equation 8 is robust to modeling errors and parameter uncertainties.

The control laws in Equations 8 and 10 compute the actuator torque for executing a desired motion profile. The actuator 606 most widely used in robotic applications is the permanent magnet DC motor. Such motors are driven by a controllable voltage or current source. Most motor manufacturers allow the user to indirectly command a desired torque by applying a voltage that is proportional to the armature current. Because motor torque is proportional to armature current, the torque can be directly and accurately controlled.
FIG. 8 is a flowchart illustrating one embodiment of a methodology for controlling power to and from the robot 104 of FIG. 1 or the actuators 606 of FIG. 6 according to the present invention.
The controller 102 distributes torque to the linear actuators 606 to produce forces F1 and F2 while satisfying the torque relation given by Equation 5 and by applying the appropriate control signals to the actuators 606. The controller 102 distributes torque using a policy that depends on the direction of the instantaneous joint power. The controller 102 passively controls the joint when the system is absorbing energy and actively controls the joint when the system is generating energy. This principle can be applied to any joint by first computing the joint power and then determining the mode of the actuator as a generator or a motor.
The controller 102 obtains 802 or determines the desired motion of the joint, and determines 804 (from sensors or by prediction) the angular or linear velocity at the joint. The controller 102 determines 806 a generalized torque or force to cause the joint to move in accordance with the desired motion, and computes 808 the power of the joint. If the computed power is positive 810, the controller 102 provides control signals to cause the actuators 406 to generate 512 the corresponding energy for rotating the joint, and actively controls 814 the joint. On the other hand, the joint power is negative 810, the joint absorbs 816 energy, and the controller 102 commands the actuators to store 818 energy and passively controls the joint.
The control of the actuator as shown in FIG. 8 is next described for a rotational joint.
For a rotational joint, the quantities used to determine the power P generated by an actuator is the product of net joint torque τ, and the joint angular velocity, ω={dot over (θ)}, as described in Equation (11):

P=τω  (11)

Based on the direction of the instantaneous power flow, the forces F1 and F2 have an active component which use supply power and a passive component which stores energy and later releases the stored energy.
If the instantaneous power is negative 810 (P<0), the system can be controlled by dissipating 818 energy passively. Under this scenario, suppose the actuators 606 store the dissipated energy by using passive forces F1p and F2p. This may be achieved with an actuator 606 with spring-like properties according to Hooke's law,

F1p(store)=ks1δt1  (12)

F2p(store)=ks2δt2  (13)

where ks1 and ks2 are tunable spring coefficient and δt1 and δl2 represents the displacement of the spring. Since this system is redundantly actuated (2 actuators control 1 joint), the distribution of stored energy in the two passive actuators is not unique. The actuator mechanism provides the passive forces in each spring that does not oppose the direction of the calculated torque to execute the desired motion. In other words, the stored energy should be a byproduct of resistive torques, which are used to dissipate energy with the same magnitude and direction of the control torque. Controlling the magnitude of the stored force (and therefore joint torque) may be achieved by using an actuator with an adjustable spring constant. An example of a spring actuator with adjustable stiffness was developed by Hurst et al. See, for example, J. W. Hurst, J. E. Chestnut, and A. A. Rizzi, “An actuator with mechanically adjustable series compliance”, Carnegie Mellon University Technical Report: CMU-RI-TR-04-24, Pittsburgh, Pa. (2004), the contents of which are incorporated herein in their entirety. See also, for example, Hurst et al. “An actuator with physically variable stiffness for highly dynamic legged locomotion” noted above. Alternatively, several springs can be combined in parallel and recruited at different times in order to control the force. The stored energy is released when the system goes into energy generation phase 812.
If the instantaneous power is positive 810 (P>0), the system can be controlled by delivering 814 active power by either forces F1 or F2, depending on the direction of the torque. Let F1a and F2a be the active component of the forces generated by the two actuators 606. The joint torque can be expressed as,

τ=d(F1a+Fp(release)) if τ≦0  (14)

τ=d(F2a+Fp(release)) if τ≧0  (15)

where Fp (release)) is the force that is released from one or both of the passive springs, depending on the current state. The released spring force of each actuator 606 is in the same direction as the active force component. The active component of actuator force F1 is computed as follows,




  
    
      
        
          F
          
            1
            ⁢
            
              
            
            ⁢
            a
          
        
        =
        
          {
          
            
              
                
                  
                  ⁢
                  
                    
                      
                        -
                        τ
                      
                      /
                      d
                    
                    -
                    
                      F
                      
                        p
                        ⁡
                        
                          (
                          release
                          )
                        
                      
                    
                  
                
              
              
                
                  
                  ⁢
                  
                    
                      if
                      ⁢
                      
                        
                      
                      ⁢
                      τ
                    
                    ≤
                    0
                  
                
              
            
            
              
                
                  
                  ⁢
                  0
                
              
              
                
                  
                  ⁢
                  otherwise
                
              
            
          
        
      
    
    
      
        (
        16
        )
      
    
  




where τ is obtained by the control law in Equation 8 or Equation 10. The active component of actuator force F2 is computed by,




  
    
      
        
          F
          
            2
            ⁢
            a
          
        
        =
        
          {
          
            
              
                
                  
                  ⁢
                  
                    
                      
                        -
                        τ
                      
                      /
                      d
                    
                    -
                    
                      F
                      
                        p
                        ⁡
                        
                          (
                          release
                          )
                        
                      
                    
                  
                
              
              
                
                  
                  ⁢
                  
                    
                      if
                      ⁢
                      
                        
                      
                      ⁢
                      τ
                    
                    ≤
                    0
                  
                
              
            
            
              
                
                  
                  ⁢
                  0
                
              
              
                
                  
                  ⁢
                  otherwise
                
              
            
          
        
      
    
    
      
        (
        17
        )
      
    
  




Another embodiment of the present invention provides energy regeneration, which uses actuators 606 as motors when energy needs to be generated by the system 812 and uses them as generators when energy needs to be absorbed/regenerated by the system 816. According to a further embodiment, absorbed/regenerated energy can be stored 818 and used to provide active actuation 814 when the actuators are used as motors.
4 Multi-Link Systems
The equations of motion of an n degree of freedom, unconstrained, multi-link rigid body system, such as shown in FIG. 2, can be described by the following differential equation,

M(q){umlaut over (q)}+h(q,{dot over (q)})=T  (18)

where vector q is an n×1 vector of generalized coordinates that describe the configuration of the system, M is an n×n joint space inertia matrix, and h is an n×1 vector describing the effects of the coriolis, centrifugal, gravitational, and friction forces. The vector T is n×1 and represents the generalized forces (or torques). The multi-link version of the tracking controller 102 in Equation 8 is given by,

T=M(q){umlaut over (q)}*+h(q,{dot over (q)})  (19)

where

{umlaut over (q)}*={umlaut over (q)}d+Kd({dot over (q)}d−{dot over (q)})+Kp(qd−q)  (21)

where qd is an n×1 vector of the desired generalized coordinates, and the feedback gains Kd and Kp are n×n positive definite, diagonal matrices. Without loss of generality, the system is assumed to have i=1 . . . n rotational joints and each joint is actuated. The control law for the ith actuator is denoted by Ti.
The PID control law in Equation 10 is an independent joint controller. Therefore, with modification of the variable names, the multi-link version of the controller is identical to the single link control presented in Equation 10.

Ti=Kpi(qdi−qi)+KIi∫(qdi−qi)dt+Kdi(qdi−qi)  (22)

where the feedback gains are all scalars, as in Equation 10. If Ti actuates the ith joint in this system, and wi is the associated angular velocity of the ith joint, then the power to actuate the joint is given by,

Pi=Tiwi  (23)

The initial step in the control process is to generate the desired trajectory. There are several methods for computing the trajectory in multi-dimensional space which describes the desired motion of a robot. A trajectory described in joint space can be specified by the vector q, and determines the motion of each degree of freedom, its velocity, and its acceleration. Alternatively, a trajectory can be described in task space by a set of Cartesian variables which are related to joint varibles q. Depending on the application, the desired motion of the system can be planned off-line, or generated at run-time using a variety of trajectory generation schemes. It is sometimes sufficient that the user specify only the desired goal position and orientation of a point on the robot, and leave the system to decide on the exact shape of the path to get there, the duration, the velocity profile, and other details.
While particular embodiments and applications of the present invention have been illustrated and described herein, it is to be understood that the invention is not limited to the precise construction and components disclosed herein and that various modifications, changes, and variations may be made in the arrangement, operation, and details of the methods and apparatuses of the present invention without departing from the spirit and scope of the invention as it is defined in the appended claims.

## Claims

1. A method for controlling a motion controlled system, the method comprising:
generating, using the controller, a trajectory specifying a desired motion of the motion controlled system to a desired position for the motion controlled system in joint space or in task space;
computing power to be applied to the motion controlled system to produce a motion moving the motion controlled system to a first position along the trajectory;
generating, using the controller, a first control signal to supply energy to the motion controlled system to move the motion controlled system along the trajectory when the computed power is above a first predetermined level;
generating, using the controller, a second control signal to draw energy from the motion controlled system as moves the motion controlled system along the trajectory when the computed power is below a second predetermined level; and
generating, using the controller, a third control signal to store the regenerated energy.

2. The method of claim 1 wherein the motion controlled system is a robotic joint.

3. The method of claim 1 further comprising:
generating, using the controller, a fourth control signal to supply the stored energy to the motion controlled system when the computed power is above the first predetermined level,
wherein the generating, using the controller, the first control signal includes generating the first control signal to supply stored energy to the motion controlled system.

4. The method of claim 1 further comprising:
measuring angular velocity of the motion controlled system,
wherein the computing power to be supplied to the motion controlled system includes:
determining the torque to be applied to the motion controlled system in response to the measured angular velocity, and
computing the power to be supplied to the motion controlled system based on the computed torque.

5. The method of claim 4 wherein the determining the torque includes computing the torque to be applied to the motion controlled system in response to the measured angular velocity.

6. The method of claim 4 wherein determining the torque includes measuring the torque to be applied to the motion controlled system in response to the measured angular velocity.

7. The method of claim 1 further comprising:
detecting motion of the motion controlled system,
wherein computing power to be applied to the motion controlled system further includes computing power to be applied to the motion controlled system based on the detected motion and the motion moving the motion controlled system to a first position along the trajectory.

8. The method of claim 1 wherein the motion controlled system includes an actuator that brakes the motion controlled system.

9. The method of claim 8 wherein the actuator includes a regeneration mode.

10. The method of claim 1 wherein the motion controlled system includes an actuator system comprising first and second actuators, the first and second actuators cause the motion controlled system to move in first and second directions, respectively, the first actuator using positive power in the first direction and storing or releasing energy in the second direction, the second actuator using positive power in the second direction and storing or releasing energy in the first direction.

11. A system comprising:
a motion controlled system for controlling motion of a structure in response to first, second and third control signals;
a controller for generating a trajectory specifying a desired motion of the motion controlled system to a desired position for the motion controlled system in joint space or in task space, computing power to be applied to the motion controlled system to generate a motion moving the motion controlled system to a first position along the trajectory, for generating the first control signal to supply energy to move the motion controlled system along the trajectory when the computed power is above a first predetermined level, for generating the second control signal to draw energy from the motion controlled system as the motion controlled system moves along the trajectory when the computed power is below a second predetermined level; and for generation the third control signal to store the energy drawn from the motion controlled system.

12. The system of claim 11 wherein the motion controlled system includes a robotic joint.

13. The system of claim 11 wherein the controller further generates the fourth control signal to supply the stored energy to the motion controlled system when the computed power is above the first predetermined level, and generating the first control signal includes generating the first control signal to supply stored energy to the motion controlled system.

14. The system of claim 11 wherein the motion controlled system measures angular velocity of the structure,
wherein the controller computes power to be applied to the motion controlled system by determining the torque to be applied to the motion controlled system in response to the measured angular velocity, and computing the power to be applied to the motion controlled system based on the computed torque.

15. The system of claim 11 wherein the motion controlled system further detects motion of the structure,
wherein the controller computes power to be applied to the motion controlled system based on the detected motion and the motion moving the motion controlled system to a first position along the trajectory.

16. The system of claim 11 wherein the motion controlled system includes an actuator that brakes the motion controlled system.

17. The system of claim 16 wherein the actuator includes a regeneration mode.

18. The system of claim 11 wherein the motion controlled system includes an actuator system comprising first and second actuators, the first and second actuators cause the motion controlled system to move in first and second directions, respectively, the first actuator using positive power in the first direction and storing or releasing energy in the second direction, the second actuator using positive power in the second direction and storing or releasing energy in the first direction.

19. The system of claim 11 wherein the motion controlled system includes a movable member, and includes an actuator system for moving the member in first and second directions, the actuator system comprises first and second actuators, the first and second actuators provide a positive force in response to a compression thereof in response to movement in the first and second directions, respectively, of the movable member, and provide a negative force in response to an expansion thereof in response to movement in the second and first directions, respectively, of the movable member.

20. The system of claim 11 wherein the motion controlled system includes a base, a movable member coupled to the base and rotatable about an axis, and includes an actuator system for rotating the moveable member in first and second directions about the axis, the actuator system comprises first and second actuators, each first and second actuators including a first end coupled to the base and including a second end coupled to the movable member, the second ends of the first and second actuators being coupled to opposite sides of the movable member.

21. The system of claim 20 wherein, the first and second actuators provide a positive force in response to a compression thereof in response to movement in the first and second directions, respectively, of the movable member, and provide a negative force in response to an expansion thereof in response to movement in the second and first directions, respectively, of the movable member.