# Patent

## Title
System for collision avoidance of rotary atomizer

## Abstract

A monitoring method for a drive system with a motor and a moving part driven by the motor. The movement of the driven part is monitored and the movement of the driving part is monitored and compared to recognize a collision between the driven part and another structure. Measurement of at least one drive-side motion quantity of the motor is taken and measurement of at least one driven-side motion quantity of the moving part is taken. The motion quantity can be a position, velocity or acceleration. A dynamic model is calculated based on predetermined or expected operational data and the drive side measured data. An error signal is generated when the motion quantity measured on the driven side varies from the dynamic model.

## Background
This application is a continuation of U.S. application Ser. No. 10/895,430, filed Sep. 24, 2003 now abandoned.


BACKGROUND OF THE INVENTION
1. The Field of the Invention
The invention concerns a monitoring method for a drive system with a motor and a moving part driven by a motor and more specifically to a system for detecting collisions between a moveable robot and other structure.
2. Related Prior Art
Painting systems for painting vehicle chassis include multi-axis painting robots. The robots include drive systems to control the position of a rotary atomizer. The drive systems include sensors and a controller that emits control signals to motors associated with the robot. The control signals are sent in response to signals received by sensors in accordance with a control program stored in the memory of the controller. The control program is prepared to achieve optimum painting results.
Painting robots can be positioned adjacent other structures. A collision between the painting robot and room boundaries, obstacles, or persons is possible. The collision should be recognized as soon as possible in order to prevent damage to the painting robot, or injury to the persons, or less than desirable painting results. WO98/51453 discloses a monitoring method for a robot that includes collision recognition. In WO98/51453, the reaction of the mechanism to the drive of the robot is evaluated and an error signal is generated as a function of this reaction. For example, if the painting robot bumps against a stationary obstacle like a building wall, then a disturbance force acts on the robot. This force is fed back to the drive so that the drive is halted. For a collision with an elastic obstacle, a disturbance force likewise acts on the robot. Here, however, the force merely leads to slow or inhibit robot motion. However, in each case, the motion quantities of the drive, such as the angular position and the rpm of the motor shaft, deviate at least for a short time from the disturbance-free values.
Thus, known monitoring methods measure the drive-side motion quantities, such as the angular position and rpm of the motor shaft. The error signal generated as a response to collision recognition is calculated from the motion quantities measured on the drive side and the preset regulation or control quantities for controlling the drive and the mechanism.
However, a disadvantage of this known monitoring method for collision recognition is that the mechanical reaction of a collision disturbance force on the drive is strongly reduced by interposed gears. For example, for painting robots, gears with a transmission ratio of 1:100 are used between the drive and the mechanism so that the mechanical reaction of a collision on the drive can be measured only with difficulty. Another disadvantage of known monitoring methods is that incorrect models for the drive lead to large errors, because the reaction of the mechanism to the drive is then set incorrectly. Finally, another disadvantage of known monitoring methods is that the angular position is measured on the drive side, while the acceleration is calculated by differentiating the measured value twice. This second derivative of the measured value leads to a very noisy signal.
Thus, the invention is based on the problem of improving the previously described, known monitoring method for collision recognition so that collision recognition is possible with higher reliability and a quicker reaction time even with interposed gears, for as little measurement expense as possible.
SUMMARY OF THE INVENTION
According to the invention, an error signal that enables collision recognition is calculated from the motion quantities of the driven mechanism measured on the driven side and from the motion quantities measured on the drive side.
Other applications of the present invention will become apparent to those skilled in the art when the following description of the best mode contemplated for practicing the invention is read in conjunction with the accompanying drawings.



BRIEF DESCRIPTION OF THE DRAWINGS
The description herein makes reference to the accompanying drawings wherein like reference numerals refer to like parts throughout the several views, and wherein:
FIG. 1 a physical equivalent circuit diagram of an electromotor with a gear and a pivoting mechanism;
FIG. 2 a regulation-specific equivalent circuit diagram of an electromotor and a pivoting mechanism of a robot; and
FIG. 3 a monitoring device according to the invention.



DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENT
Referring to FIG. 1 and for determining a system error signal, the invention includes the general technical teaching of not only measuring drive-side (as indicated generally by arrow 34) motion quantities 35 of the drive system 30 preferably having a direct current motor 32, but also motion quantities 37 on the driven side (as indicated by arrow 36), i.e., on a driven mechanism 38.
One advantage of measuring motion quantities 35, 37 on the driven side 36 and the drive side 34 is the fact that the determination of the error signal corresponding to a collision is more accurate. For example, gears 4 interposed between the driven side 36 and the drive side 34 can prevent the communication of collision forces to sensors 7.1, 7.2 disposed on the drive side 34. Thus, the monitoring method for collision recognition according to the present invention can also be used for drive systems 30 which have gears 4 with a high transmission factor.
Another advantage of the monitoring method according to the invention is the fact that feedback of control or regulation quantities is not required, so that the monitoring method according to the invention is independent of the type and structure of the drive regulation or control.
In a preferred embodiment of the invention, a comparison value for the drive force and the drive moment of the motor 32 is calculated from the motion quantities 37 measured on the driven side 36 and from the motion quantities 35 measured on the drive side, 32, where preferably a dynamic model of the motor 32 and the mechanism 38 of the drive system 30, respectively, is taken into account. The dynamic model contemplates the inertia of various components of the system 30, the elastic components, and also the frictional forces or moments of the drive system 30. For disturbance-free operation of the drive system 30, the two comparison values calculated and processed by a monitoring device 6 must agree, while a deviation between the two comparison values can indicate a disturbance or even a collision as then processed by an evaluation unit 18.
The calculation of comparison values for the drive force or the drive moment can be implemented on the drive side 34 and/or on the driven side 36 by a recursive computational method, such as that described, e.g., in Roy Featherstone: “Robot Dynamics Algorithms,” Chapter 4, pages 65-79 (Kluwer Academic Publishers, 1987), ISBN # 0898382300.
The motion quantities measured 35 on the drive side 34 preferably include at least one of a position, a velocity, and/or an acceleration of a drive shaft or armature 2 of the DC motor 32. It is sufficient to measure only one of these motion quantities, while the other two motion quantities can be determined through time differentiation or integration of the measured motion quantity. For example, it is possible to measure only the rotational velocity of the motor drive shaft 2, wherein the acceleration of the motor drive shaft 2 is obtained through differentiation of the measured rotational velocity, and the angular position of the motor drive shaft 2 can be calculated through integration of the measured rotational velocity.
The acceleration of the motor 32 can be measured as a drive-side motion quantity 35. Such a direct measurement of acceleration provides higher accuracy compared with differentiating the measured velocity or even the measured position. For example, differentiating the measured angular position of the motor drive shaft 2 twice leads to a very noisy signal.
In contrast, one of the position, the velocity, and/or the acceleration of the driven mechanism 38 can be measured as the driven-side motion quantities 37. Here, fundamentally, it is also sufficient to measure only one of these motion quantities 37, while the other two motion quantities can be obtained through time differentiation or integration of the measured motion quantity. For example, it is possible to measure only the velocity of the mechanism 38, wherein the acceleration of the mechanism 38 is obtained through differentiation of the measured velocity and the position of the mechanism 38 can be calculated through integration of the measured velocity.
However, preferably the acceleration of the driven mechanism 38 is measured as a driven-side motion quantity 37. Such a direct measurement of the acceleration provides higher accuracy compared with an acceleration value derived from differentiating the measured velocity or even the measured position. For example, differentiating the measured position of the mechanism 38 twice leads to a very noisy signal.
For a multi-axis drive system 30, the error signal output 40 of the monitoring device 6 can preferably be determined separately for the individual axes of the drive system. As will be described and illustrated in FIG. 3, the error signals for the individual axes can each form components of an error vector. For the evaluation and determination of the error signal, a scalar error value is then preferably calculated by the evaluation unit 18 in order to also be able to recognize collisions, which effect only certain axes of the drive system 30.
In addition, the two comparison values for the drive force or the drive moment are also determined separately for each axis. However, due to the interaction between the individual axes, the motion quantities measured for the other axes are also taken into account for the calculation of the comparison values in the individual axis.
For suppressing temporary measurement errors, a sliding mean value of the error signal is preferably formed by the evaluation unit 18. The mean value determined in this way is then preferably compared with a predetermined threshold. If the threshold is exceeded, it is then assumed that a collision has occurred, which may initiate an emergency off action 42.
In addition, the monitoring method according to the invention can also recognize creeping disruptions of the drive system 30, such as, when the friction of the drive system increases due to bearing damage. For this purpose, preferably a sliding mean value of the error signal is formed over a long time period. The mean value formed in this way is then compared with a predetermined threshold. If the threshold is exceeded, it is then assumed that bearing damage has occurred, which can be indicated by a warning message 44.
The physical equivalent circuit diagram in FIG. 1 shows a conventional electromechanical drive system for driving a shaft 1 of a painting robot, with additional drive systems being provided for driving the other shafts of the painting robot, which are configured similarly and thus are not described for simplification.
The shaft 1 of the painting robot is simulated in the physical equivalent circuit diagram by a mass m and a spring element c, while the damping of the shaft 1 is ignored for this embodiment.
The drive of the shaft 1 is here realized by the externally excited direct-current motor 32, which is represented in the physical equivalent circuit diagram as a series circuit composed of the Ohmic resistor R of the armature, the inductance L of the armature, and the voltage ui induced by the armature 2 in the armature circuit (or rotor if AC motor). In complex notation, the armature voltage uA is:

uA=R·iA+s·L·iA+ui  (1)

The direct-current motor 32 is connected to shaft 1 via a drive shaft 3 and a gear 4, with the gear 4 converting the rotational motion of the drive shaft 3 into a different rotational motion.
The voltage ui induced in the armature circuit then results from the motor constant KM and the angular velocity ωA of the drive shaft 3 according to the following equation:

ui=KM·ωA  (2)

Furthermore, the drive moment MA of the direct-current motor 32 is the product of the armature current iA and the motor constant KM:

MA=KM·iA  (3)

On the other side, a frictional force FRV acts on the shaft 1. This force is converted by the gear 4 into a frictional moment MRV:




  
    
      
        
          M
          RV
        
        =
        
          
            
              F
              RV
            
            
              i
              G
            
          
          =
          
            
              
                
                  v
                  A
                
                ·
                
                  d
                  G
                
              
              
                i
                G
              
            
            =
            
              
                ω
                A
              
              ·
              
                d
                G
              
            
          
        
      
    
    
      
        (
        4
        )
      
    
  




In addition, a load FL also acts on the shaft 1. This load is converted by the gear 4 into a load moment:




  
    M
    L
  
  =
  
    
      
        F
        L
      
      
        i
        G
      
    
    =
    
      
        
          c
          ·
          
            (
            
              
                x
                A
              
              -
              
                x
                m
              
            
            )
          
        
        +
        
          
            d
            G
          
          ·
          
            (
            
              
                v
                A
              
              -
              
                v
                m
              
            
            )
          
        
      
      
        i
        G
      
    
  




The drive shaft 3 is thus accelerated by the drive moment MA and braked by the frictional moment MRV and also by the load moment ML. Taking into account the inertial moment JA of the drive, the following acceleration dωA/dt of the drive shaft 3 then results:




  
    
      
        
          
            d
            ω
          
          
            d
            t
          
        
        =
        
          
            
              M
              A
            
            -
            
              M
              L
            
            -
            
              M
              RV
            
          
          
            J
            A
          
        
      
    
    
      
        (
        5
        )
      
    
  




In contrast, the block circuit diagram in FIG. 2 shows a regulation-specific equivalent circuit diagram of the armature 2 of the direct-current motor 32 and a mechanism 5, with the mechanism 5 also including a shaft and gear similar to shaft 1 and gear 4.
The calculation of the individual electrical and mechanical quantities is realized corresponding to the previously listed equations, as can be seen directly from FIG. 2.
However, the previously listed equations apply only to the case of undisturbed movement of the shaft 1. In contrast, if the motion of the shaft 1 is disturbed, then additional forces that lead to deviations of the actual system behavior from the ideal model behavior act on the shaft 1, in addition to the frictional force FRV and the load FL.
For example, if a painting robot bumps against the wall of a painting cabin, then the motion of the painting robot is strongly braked. Also, if there is bearing damage, the painting robot does not follow the modeled behavior exactly because the friction due to the bearing damage is strongly increased and is not taken into account in the previously listed equations. In such cases, the error should be recognized as quickly as possible in order to be able to introduce countermeasures.
Referring to FIG. 3, the monitoring device 6 is provided for each shaft of the painting robot. These monitoring devices recognize deviation of the actual behavior of the drive system 30 from the modeled behavior. Here, for simplification, only the monitoring device 6 for the first shaft is shown, but the monitoring devices for the other shafts are configured identically.
The monitoring device 6 is connected on the input side to several sensors 7.1-7.4, with the sensors 7.1 and 7.2 measuring the angular position φA1 and the angular velocity ωA1 of the drive shaft 3, respectively, while the sensors 7.3 and 7.4 detect the position xm of the shaft 1 and the acceleration am of the shaft 1, respectively.
Alternatively, it is also possible to provide only one sensor for measuring one drive-side motion quantity 35 and one sensor for measuring one driven-side motion quantity 37, where additional motion quantities can be determined from the measured values. This can be implemented, e.g., through time differentiation or integration of the measured values or through the use of a so-called observer.
The position xm1 of the shaft 1 is then supplied to a differentiator 8, which calculates the velocity vm1 of the shaft 1 as a time derivative of the position xm1, so that a measurement of the velocity vm1 in this embodiment is not required.
The position xm1, the velocity vm1, and the acceleration am1 of the shaft 1 are then supplied to a computational unit 9, which calculates a model-based value FMODELL,1 for the force acting on the shaft 1 based on a predetermined model and also taking into account the corresponding motion quantities xmi, vmi, and ami of the other shafts. Thus, the calculation of the model-based value FMODELL,1 is here implemented using load-side or driven side measurement data.
In addition, the monitoring device 6 calculates from drive-side measurement data a comparison value FL1 for the force acting on the shaft 1 from the drive-side. For this purpose, the monitoring device 6 has two computational units 10, 11, which convert the measured angular position φA1 and the measured angular velocity ωA1 of the drive shaft 3 into corresponding values XA1 and YA1 while taking into account the gear transmission ratio iG.
The drive-side calculated position path XA1 is then supplied to a subtractor 12, which calculates the difference Δx=XA1−Xm1 between the drive-side calculated position path xA1 and the actually measured position xm1 of the mass m of the shaft 1. This difference Δx is supplied to another computational unit 13, which calculates the elastic percentage Δx·c of the force acting on the shaft 1 while taking into account the spring constant c.
In contrast, the drive-side calculated velocity vA1 is supplied to a subtractor 14, which calculates the difference Δv=vA1−vm1 between the drive-side calculated position velocity vA1 and the actually measured velocity of the mass m of the shaft 1. This difference Δv is then supplied to a computational unit 15, which calculates the percentage of force acting on the shaft 1 due to the gear damping as a product of the damping constant dG and the velocity difference Δv.
On the output side, the two computational units 13, 15 are connected to an adder 16, which calculates the comparison value FL1 for the force acting on the shaft 1 from the elastic percentage c·Δx and the damping percentage dG·Δv.
Furthermore, the monitoring device 6 has a subtractor 17, which is connected on the input side to the adder 16 and the computational unit 9, and calculates the difference between the two comparison values FL1 and FMODELL,1, and outputs an error signal FSTÖR,1(as indicated by element number 40).
For undisturbed motion of the shaft 1, the two comparison values FL1 and FMODELL,1 agree up to an unavoidable measurement error, because the modeling of the dynamic behavior of the shaft 1 reproduces its actual behavior. The calculation of the force FL1 acting on the shaft 1 from the motion quantities measured on the drive side, ωA1 and φA1 then produces the same value as the calculation of the force FMODELL,1 acting on the shaft 1 from the motion quantities xM1 and aM1 measured on the load side.
In contrast, if the motion of the shaft 1 is disturbed, then the comparison values FL1 and FMODELL,1 deviate from each other, with the deviation of these quantities reproducing the severity of the disturbance. Thus, increased bearing friction leads only to a relatively small error signal FSTÖR,1, while the collision of the shaft 1 with a boundary leads to a very large error signal FSTÖR,1.
Furthermore, for evaluating the operating behavior for all of the shafts, there is an evaluation unit 18, which is connected on the input side to the individual monitoring devices 6 for the individual shafts and receives the error signals FSTÖR,i for all of the shafts.
The evaluation unit 18 contains a support element 19, which receives the error signals FSTÖR,i for all of the shafts in parallel and outputs them as a multi-dimensional disturbance force vector FSTÖR to a computational unit 20.
The computational unit 20 then calculates a scalar error value F from the individual components of the disturbance force vector FSTÖR, said scalar error value F reproduces the severity of the disturbance acting on the painting robot.
The error value F of the disturbance force vector FSTÖR is then supplied to a computational unit 21, which calculates the sliding mean value of the error signal F in order to suppress the influence of measurement outliers in the evaluation. The averaging period of the computational unit 21 is relatively short, so that suddenly occurring disturbances, such as a collision of the painting robot with an obstacle, are recognized quickly.
On the output side, the computational unit 21 is connected to a threshold element 22. The emergency-off signal 42 is generated when a predetermined threshold is exceeded, which leads to immediate halting of the painting robot in order to prevent damage to the painting robot and to the surroundings or even injuries to persons in the area.
In addition, the evaluation unit 18 has another branch in order to be able to react to more slowly occurring and smaller disturbances. For this purpose, the computational unit 20 is connected to a computational unit 23, which calculates the sliding mean value of the error signal F, where the computational unit 23 has a greater averaging period than the computational unit 21, so that only changes that take place over a longer time period are taken into account.
On the output side, the computational unit 23 is connected to a threshold element 24, which generates the warning signal 44 when a predetermined threshold is exceeded.
The invention is not limited to the previously described embodiment. Indeed, a plurality of variants and modifications are possible, which likewise make use of the concept of the invention and thus fall within the scope of protection.
Obviously, many modifications and variations of the present invention are possible in light of the above teachings. It is, therefore, to be understood that within the scope of the appended claims, the invention may be practiced otherwise than as specifically described. The invention is defined by the claims.

## Claims

1. Monitoring method for a drive system of a robot, more specifically a painting robot, with a motor and a moving part driven by the motor, with the following steps:
measurement of at least one drive-side motion quantity (φA1, ωA1) of the electric motor,
measurement of at least one driven-side motion quantity (xM1, aM1) of the moving part,
determination of an error signal (FSTÖR) as a function of the motion quantities (φA1, ωA1) of the electric motor measured on the drive side and of the motion quantities (xM1, aM1) of the moving part measured on the driven side;
measuring an armature current in an armature circuit of the electric motor;
measuring a voltage induced n the armature; and
calculating a comparison value (FMODELL,1) from the armature current, the induced voltage and the at least one driven-side motion quantity (xM1, aM1).

2. The monitoring method set forth in claim 1 comprising the further steps of:
calculation of a first comparison value (FL1) for a drive force or a drive moment of the motor with reference to the motion quantities (φA1, ωA1) of the motor measured on the drive side;
comparison of the first comparison value (FL1) with the second comparison value (FMODELL,1); and
generation of the error signal (FSTÖR) as a function of the comparison.

3. A drive system of a robot comprising:
a rotating first shaft;
a rotating second shaft engaged and disposed non-concentrically to the first shaft;
a first sensor constructed and arranged on the first shaft for measuring a first position value;
a second sensor constructed and arranged on the first shaft for measuring a first angular velocity value;
a third sensor constructed and arranged on the second shaft for measuring a second position value;
a fourth sensor constructed and arranged on the second shaft for measuring an acceleration value; and
a monitoring device that receives input from the first, second, third and fourth sensors and outputs an error signal;
a differentiator of the monitoring device for calculating a second angular velocity value of the second shaft as a time derivative of the second position value.

4. The drive system set forth in claim 3 further comprising a comparison force value calculated by the monitoring device from at least the first angular velocity value, the first position value, the second position value, and the second angular velocity.

5. The drive system set forth in claim 3 further comprising a first computational unit of the monitoring device for calculating a model-based force value from the second position value, the second acceleration value and the second angular velocity.

6. The drive system set forth in claim 5 further comprising a subtractor of the monitoring device for calculating the outputted error signal from the comparison force value and the model-based force value.

7. The drive system set forth in claim 6 further comprising an evaluation unit that receives the error signal from the monitoring device, the evaluation unit having an emergency stop output signal.

8. The drive system set forth in claim 7 further comprising:
a second computational unit of the evaluation unit for calculating a sliding mean value over time and associated with an output warning signal; and
a third computational unit of the evaluation unit for calculating the emergency stop output signal; and
wherein the third computational unit has a shorter averaging period than the second computational unit.

9. A drive system of a robot comprising:
an electric motor having a rotating first shaft having an angular first velocity;
a rotating second shaft having an angular second velocity that is different than the first velocity;
a gear connected between the first and second shafts;
the first shaft having at least one drive sensor for measurement of at least one drive-side motion quantity (φA1, ωA1) of the first shaft;
the second shaft having at least one driven sensor for measurement of at least one driven-side motion quantity (xM1, aM1) of the second shaft;
a monitoring device that receives input signals from the at least one drive sensor and the at least one driven sensor; and
an error signal (FSTÖR) calculated and outputted by the monitoring device
an evaluation unit for receipt of the error signal from the monitoring device the evaluation unit having a first computation unit for calculating a sliding mean value over time and associated with an output warning signal and a second computational unit of the evaluation unit for calculating an emergency stop output signal.

10. The drive system set forth in claim 9 wherein the second computational unit has a shorter averaging period than the first computational unit.

11. The drive system set forth in claim 9 wherein the first shaft is an armature.