Abstract:
A digital servo system for causing a mechanical element (48) to track a desired path and hold various positions along the path and employing a lead/lag integrator (18) in parallel with a feedback control loop. The input to the integrator (18) is switched between velocity (V EK ) error during pathtracking and position error (E K ) during holding to increase the resistance of the mechanical element (48) to position changes caused by outside forces during both pathtracking and holding. The integrator function employed uses a term (U integrator (k-1)) representing a previous integrator output value to assist in smoothing perturbations caused by switching between position and velocity error.

Description:
BACKGROUND OF THE INVENTION 
     1. Field of the Invention 
     The subject invention relates generally to servo positioning systems, and more particularly to a digital servo positioning system employing a lead/lag integrator to achieve high stiffness during both path tracking and holding. 
     2. Description of Related Art 
     Prior art servo position control systems are known wherein a control system moves a mechanical element so as to track a designated path and hold the element at selected points along the path. The concept of &#34;stiffness&#34; in such systems implies a high resistance to external forces which would move the element from its chosen path. 
     Prior art servo position control systems commonly employ so-called &#34;PID&#34; control wherein an error signal is provided and individual control signals are generated. These individual control signals are typically proportional to the error signal, the integral of the error signal, and the derivative of the error signal. In such systems the integrator which provides the integral of the error signal is typically nested within a feedback compensator, is constrained to integrate the error signal, which typically represents position error. 
     Either position integration or velocity integration has its drawbacks. Systems which use only position error integration are susceptible to &#34;windup overcompensation,&#34; especially during pathtracking, which leads to large position overshoots and oscillations at the end of the move when holding is required. Velocity error integration increases system stiffness during pathtracking, while significantly reducing windup overcompensation compared to position integration. However, velocity integration yields unsatisfactory holding stiffness for many applications. Using both velocity error and position error integration at the same time introduces conflicting control signals under some conditions, which compromises optimal system performance. 
     SUMMARY OF THE INVENTION 
     It is therefore an object of the invention to improve servo positioning systems; 
     It is another object of the invention to increase stiffness of such systems during holding and pathtracking; 
     It is another object of the invention to employ both position and velocity integration in a servo positioning system in such a manner that stiffness is improved during both holding and pathtracking; and 
     It is another object of the invention to provide an improved servo position system adaptable to digital implementation. 
     According to the invention, the integrator in a position control servo system is moved out of the feedback loop and placed in parallel therewith. This placement permits switching the integrator input from velocity error during pathtracking to position error during holding, resulting in superior stiffness in both the holding and moving states. The scheme is adaptable to a wide range of multiple axis incremental motion control systems where high stiffness is required during path tracking and holding. A feature of the particular switch mode integrator employed is that a new integrator output value is always dependent on the new integrator input plus the old integrator output value, a feature which smooths perturbations which might be introduced by switching between position and velocity error. 
    
    
     BRIEF DESCRIPTION OF THE DRAWINGS 
     The just-summarized invention will now be described in detail in conjunction with the drawings of which: 
     FIG. 1 is a graph illustrating a position trajectory profile; 
     FIG. 2 is a graph illustrating a velocity trajectory profile; 
     FIG. 3 is a schematic block diagram of a digital servo position system a switch mode integrator according to the embodiment; 
     FIG. 4 is a schematic diagram of a typical system plant; 
     FIG. 5 is a block diagram of a hardware embodiment of the preferred embodiment; 
     FIGS. 6 and 7 are flow charts illustrating programming of the digital processor of FIG. 5; and 
     FIG. 8 is a schematic block diagram illustrating an alternate switch mode integrator embodiment. 
    
    
     DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS 
     A typical incremental or digital servo positioning system performs moves of a mechanical element such as a robot arm from position A to position B along some prescribed path, as shown in FIG. 1. This path has some inherent velocity profile as shown in FIG. 2. The position and velocity profiles in FIGS. 1 and 2 only illustrate one possible trajectory path. Most applications of such digital servo positioning systems require very high position accuracy and holding stiffness at the end positions A and B where the velocity of the element is zero. High stiffness during pathtracking, i.e., when velocity of the element is nonzero, is also required to minimize the effect of external disturbances on system performance. A servo positioning system which employs a switch mode integrator to generate a control signal U total  for maintaining stiffness of a mechanical element both at the end positions A, B and during pathtracking is shown in FIG. 3. 
     The system of FIG. 3 includes a path generator 13, which generates path data comprising path position signals X k-n , the path position data for one-dimensional path motion. The path position signals X k-n  indicate the desired position of a mechanical element or member included in the system plant 28. The path generator 13 outputs the desired path position signals X k-1 , X k , X k+1  and X k  over three output lines 14, 15, 16 to a computing element 17, a switch mode integrator 18, and a summer 19. 
     The computing element 17 applies a transfer function 1/G(Z) to the path data and outputs an open loop control voltage U openloop (k) on an output line 21. The switch mode integrator 18 supplies an integrator control voltage U integrator (k) on an output line 22. The summer 19 outputs an error signal E k  on a line 23 to a computational element 25. The computational element 25 applies a feedback control transfer function D(Z) on the input error signal E k  and outputs a feedback control voltage U feedback (k) on an output line 27. 
     The three output lines 21, 23, 27 are connected to a summer 29 which adds the open loop, feedback and integrator control voltages U openloop (k), U feedback (k), U integrator (k) to produce a total output control voltage U total (k) on an output line 30. The output control voltage U total (k) on the output line 30 is applied to the system plant 28, resulting in an actual path location of the mechanical element of the plant 28 represented by an actual position signal Y k . This actual position signal Y k  is outputted on a line 26, and is also fed back over line 24 to the first summer 19 and subtracted from the desired path signal X k  to yield the feedback error signal E k , where E k  =X k  -Y k . The transfer function of the actual plant 28 is represented by G*(Z). 
     The switch mode integrator 18 includes a reference velocity translator 31 receiving the path location data X k-1 , X k , X k+1  . . . , and outputting commanded velocity data X k  over a line 33 to a third summer 35. As used throughout, the subscript k indicates the value of the variable, e.g., X, during various time intervals or &#34;iterations.&#34; For example, the subscripts (k-1) and (k+1) indicate the values of the variable during intervals immediately preceding and succeeding time interval k. 
     The third summer 35 receives a second input Y k  on a line 37 from an actual velocity translator 39 which is connected to receive the actual position data Y k  over a line 41 and calculates the actual velocity Y k  according to the expression Y k  =Y k  -Y k-1 . The velocity error output V e (k), V e (k-1) of the third summer 35 is supplied to switch control logic 43 over a line 45. The switch control logic 43 receives a second input comprising the position error signals E k , E k-1  over a line 47 and supplies an integrator input error signal I e (k-1), I ek  to a lead/lag integrator 49. 
     In operation of the system of FIG. 3, the path generator 13 supplies the reference positions, X k , equally spaced in time. The actual positions, Y k , are subtracted from the reference positions to yield the position error signals, E k  =X k  -Y k . The position error is compensated using a conventional lead/lag control, D(z), according to the difference equation, U feedback (k) =A 1  E k  -A 2  E k-1  +A 3  U feedback (k-1), where A 1 , A 2 , and A 3  are constants. In conventional control systems, this signal U feedback (k) is the only component of U total (k) and is sent out to the plant, G*(z) which may comprise, for example, an amplifier, a motor, and a load. The system in FIG. 3, however, employs two additional components to determine U total (k). The first additional component, the open loop control signal, U openloop (k), is added to reduce the pathtracking errors inherent in feedback control systems. This open loop control signal is not essential to advantageous use of the switch mode integrator 18 of the preferred embodiment. 
     The switch mode integrator control signal, U integrator (k), is added to supply stiffness during holding and pathtracking within the plant G*(z). The integrator 49 is a lead/lag integrator of the form, U integrator (k) =C 1  I ek  -C 2  I e (k-) +U integrator (k-1), where C 1  and C 2  are constants and where the input integrator error signal, I ek , is either a velocity error signal, V ek , or a position error signal, E k , as determined by the switch 43. The conditions for operation of the switch 43 are: 
     If (X k  ≠0) 
     switch to &#34;D,&#34; V ek , V ek-1   
     else 
     switch to &#34;U,&#34; E k , E k-1   
     where X k  is the commanded velocity. 
     The reference velocity translator 31 generates commanded velocities represented by velocity signals, X k , from position references, X k , while the actual velocity translator 39 calculates actual velocity signals, Y k , from actual positions, Y k . Velocity error signals, V ek , are computed by the summer 35 from the reference and actual velocities, V ek  =X k  -Y k . The switch mode integrator 18 integrates velocity error, V ek , during pathtracking, and position error, E k , during holding. 
     FIG. 4 shows an example of an actual plant 101, with an input voltage U(Z) resulting in an output position X(Z). The digital transfer function of this plant 101 is ##EQU1## The transfer function G*(Z) represents a plant including a voltage mode amplifier, motor, load and encoder, as illustrated, for example, in FIG. 5, to be described in further detail below. 
     In application of the described open loop and feedback control to a digital servo positioning system, for example, for a wire bonder, the feedback control, D(z), is a standard lead-lag control based on the position error and is implemented in difference equation form, i.e., (U feedback (k) =(A 1 ) Error k  -(A 2 ) Error k-1  +(A 3 ) U feedback  (k-)), where the parameters A 1 , A 2 , and A 3  are chosen to yield a stable system. While in this case a lead-lag control is employed, the feedback control can be any one of the many conventional forms used in the industry. 
     The open loop control is preferably generated by solving a difference equation derived from the plant model G(z), which represents the dynamics of a DC motor, amplifier, lead screw, load and encoder. The open loop difference equation is solved for U openloop (k) =(B 1 ) X k+1  -(B 2 ) X k  +(B 3 ) X k-1  +(B 4 ) U openloop (k-1). An equation of this form is called noncausal since the computation of the present control signal, U openloop (k), requires the knowledge of a future event, namely X k+1 . Noncausal equations are not possible in feedback control since a future error, E k+1  is not known. However, the open loop calculation requires the future target point, X k+1  along the path. In a digital positioning system, such as the one under discussion, the target positions are precomputed using the well-known trapezoidal velocity profile and are thus available. If the target positions are not known in advance, then the target command could be delayed one sample period. Thus, the next target would be known one sample period before it is commanded. 
     To illustrate, a difference equation may be found which will predict U k  for X k+1  for the plant shown in FIG. 4. The transfer function G*(Z) is inverted as follows: ##EQU2## Simplifying this equation (2) yields: ##EQU3## Thus, U k  is a feed-forward value which is dependent on the plant parameters (dynamics) and the next desired position X k+1 . Equation (3) is implemented by computing element 17 in FIG. 3. Equation (3) may also be rewritten in terms of velocities V n  as follows: ##EQU4## 
     A hardware embodiment of the preferred embodiment is illustrated in FIG. 5. This embodiment includes a path generator 13, a buffer 32, a digital control card 34, a voltage mode amplifier 42, a motor 46, and an encoder 44. The motor 46 controls movement of a robotic element or member 48 such as a mechanical arm along a selected axis, as is well-known in the art. 
     The digital control card or control section 34 includes a digital filter 40 embodied in, for example, a Motorola 68000 microprocessor operating at 121/2 MHz. The digital filter 40 may also be embodied in various forms of digital logic, other programmed processors, or special purpose signal processor circuitry. 
     The digital filter 40 performs the function of the filter 17, filter 25, velocity translators 31, 39, the switch 43, the integrator 49 and the first, second and third summing junctions 19, 29 and 35 of FIG. 1. The digital filter 40 receives the path or trajectory information provided by the path generator 13 and stored in the buffer 32. The digital filter 40 outputs the total control signal U total  to the digital-to-analog converter 41. The digital-to-analog converter 41 in turn provides an analog control signal to the amplifier 42 for controlling the motor 46. The encoder 44 tracks the actual motor position and provides a feedback signal on line 24 to the digital filter 40. The DAC 41, amplifier 42, motor 46, encoder 44, and load 48 comprise the plant 28 illustrated in FIG. 3 having the transfer function G*(Z). 
     As illustrated schematically by cards 36, . . . 38, according to the preferred embodiment, seven additional cards and corresponding digital filters, amplifier, motor and encoder circuits may be provided in order to control up to eight-dimensional motion. Each additional card 36 . . . 38 contains its own buffer 32. Each buffer 32 receives path data for controlling motion on its particular dimension. The buffer 32 of card 34 may receive X path data, buffer 32 of card 36 may receive Y path data, and the buffer 32 of card 38 may receive Z path data. Other dimensions include roll, pitch, yaw, etc., as known in the robotics art. 
     In an actual embodiment, a Pacific Scientific 2VM-6202-7 DC servo motor has been used for the motor 46 and a Disk Instruments rotary optical encoder M-98A-1000-ICLP yielding 4,000 counts per revolution with quadrature has been used for encoder 44. In that embodiment, the amplifier 42 is a 40-volt, 12-amp Glentek GA45555 linear voltage mode amplifier. The digital-to-analog converter 41 is a Burr-Brown AD667. 
     A flowchart for programming the Motorola 68000 microprocessor to perform the digital filtering functions of digital filter 40, including the function of the switch mode integrator 18, are illustrated in FIGS. 6 and 7. Performance of the routine starts at the beginning of every sample period with step 51. Execution of the filter algorithm continues sequentially with steps 57, 59, 61, etc. of FIGS. 6 and 7. The entire algorithm is computed within the sample period. The algorithm is performed autonomously by each digital filter 40 on each card 34, 36 . . . 38 for the respective dimension whose path data is supplied to that card through the associated buffer 32. In an embodiment employing additional cards 36 . . . 38, each additional card is synchronized to the same sample period. 
     The actual position of the motor 46 is indicated over line 24 and is read at step 57. In step 58, the position error, E k  =X k  -Y k , is determined in accordance with the summing junction 19 in FIG. 1. Then, in steps 59 and 60, the actual velocity and velocity error are determined in accordance with the equations Y.sub.(k) =Y k  -1 and V er (k) =X k  -Y k , respectively. In step 61, the feedback control D(z) is computed. This term D(z) is computed by multiplying a constant A 1  times the error signal E k  and adding to it a quantity denoted Prec 1, which is a precalculated quantity, as described hereafter. 
     The flow then proceeds to a number of steps 62, 64, 66, 68, 70, which implement the switch mode integrator. At test 62, the value of X k  is tested to ascertain whether it is zero, i.e., whether the apparatus is tracking or path holding. If X k  =0, the flow proceeds to step 64, where the integrator error values are set to E k  and E k-1 , respectively. If X k  ≠0, the flow proceeds to step 65, where the integrator error values are set to V er (k) and V er (k-1). Upon exiting either step 64 or 65, the flow proceeds to calculate the lead lag integrator function U integrator (k) at step 68. After calculating the lead lag integrator function, the flow proceeds to step 70, where limits are set on the integrator in order to prevent it from growing unbounded in the event of hardware malfunctions such as amplifier shutdown. According to step 70, if the value of U integrator  is greater than a limit I max  or less than a limit I min , U integrator  is set to the I.sub. max or I min  value, respectively. 
     In step 63, the next target, X k+1  is read from the buffer 32 into the processor 40. As illustrated in step 65, the open loop control parameter is calculated by multiplying a constant B 1  times X k+1  and adding to it a second precalculated value Prec 2 as described hereafter. In the next step 67, the total control signal U total (k) is determined in accordance with the summing junction 29 in FIG. 1. 
     In step 69, a limit test is performed on the total control signal U total . The signal, U total , is compared to parameters denoted DACMAX and DACMIN and clipped if necessary. These parameters are selected to limit the input voltage signal within a range matched to the amplifier 42. The control signal U total  is then sent to the DAC 41 and then to the amp 42 and motor 46 in step 71. The parameters determined during this servo update are saved in step 73 for the next servo update 51. The routine then proceeds to block 75, where all possible terms for the next iteration of servo update 51 are calculated to minimize the time between the beginning of the sample period, step 51, and the output of U total  to the DAC/amp/motor, step 71. In this step 75, the two parameters previously referred to as Prec 1 and Prec 2 are calculated according to the equations established. 
     In a preferred embodiment, all history terms are initially set to zero. The sample period is 1 millisecond and generates a high priority interrupt to the processor 40 so that the beginning of the digital servo algorithm is synchronized to the beginning of every sample period. The path generator 13 can be run before starting the move or concurrently therewith. The path generator 13 may read out previously-stored path information, or may be a computer which calculates detailed trajectory data for a target path in response to general position commands, as desired. 
     With respect to the switch mode integrator 18 shown in FIG. 3, it may be observed that the velocity error, V ek  =X.sub.(k) -Y.sub.(k) =(X k  -X k-1 )-(Y k  -Y k-1 ) =(X k  -Y k )-(X k-1  -Y k-1 ), could be written as V ek  =E k  -E k-1 . This leads to the switch mode integrator configuration of FIG. 8, which does not require the explicit computations of the commanded and actual velocities V cmd (k) and V act (k). In this embodiment, a switch 103 switches the error signal either to position &#34;D,&#34; the input of a differentiator 105, or to position &#34;U&#34; connected to the input 45 of the integrator 49. The conditions governing switching by the switch 103 are as follows: 
     If (X k  =X k-1 ) 
     switch to &#34;U&#34; for holding and set 
     I ek  =E k   
     I ek-1  =E k-1   
     else 
     switch to &#34;D&#34; for pathtracking and set 
     I ek  =E k  E k-1   
     I ek-1  =E k-1  -E k-2   
     The system of FIG. 8 does not show open loop control, which may or may not be added. 
     According to the embodiment of FIG. 8, the integrator 49 may be viewed as having an input of position error during holding and the derivative of position error during pathtracking. Thus, the integrator 49 tries to drive the position error to zero during holding while trying to keep the position error from changing (growing) during pathtracking. This combination works very well for reducing the sensitivity of the system to external disturbances. The implementation of the switch mode integrator 18 in FIG. 8 is not used in the actual embodiment, since the commanded velocity is required for the other parts of the control logic for the system. 
     One feature of the switch mode integrator equation, U integrator (k) =C 1  I ek  -C 2  I e (k-1) +U integrator (k-1), may be noted. According to this equation, the new integrator value is always some value which is dependent on the input plus the old integrator value. This feature provided by the last integrator control value, U integrator (k-1), helps smooth the integrator perturbations introduced by switching the input between position and velocity error. 
     As will be apparent to those skilled in the art, various modifications and adaptations of the preferred embodiment may be made without departing from the scope and spirit of the invention. It is therefore to be understood that, within the scope of the appended claims, the invention may be practiced other than as specifically described herein.