Abstract:
A control system in which an end effector of a robot is mechanically compliant for a particular object. The robot has multiple degrees of freedom and movement and includes a sensor which senses a velocity nd a sensor which senses an external force. A virtual compliance control method is applied to the robot. Influence of the weight and gyro moment of the end effector is subtracted from a value obtained from a mechanism which senses the external force to set a term of the external force in a mathematic model of the control system constructed during control. By such arrangement, when a rotational body having a mechanically large rotational momentum is set as the end effector of the robot, the control system is mechanically compliant with the object.

Description:
BACKGROUND OF THE INVENTION 
     The present invention relates to methods of controlling an industrial robot compensate for a disturbance in the control of the robot&#39;s motion due to a mechanism of the robot&#39;s effector. 
     In order to cause a robot to perform operations such as boring, deburring, and grinding a manufactured site, the robot is required to have high performance which the conventional positioning robot does not have. One of the required performance items is to control a force applied to an object. To this end, the conventional techniques have proposed a method of controlling such force, using hardware, that is, a mechanism which sets rigidity in a robot&#39;s effector and a method of controlling such force, using software, that is, a technique which provides mechanical compliance for a robot by control software. Namely, any one of those methods is capable of providing compliance for the robot&#39;s effector. The latter method is capable of providing contact force control over the motion of the robot as opposed to the former method. In particular, a method of realizing a mechanical compliance mechanism of multiple degrees of freedom in the robot&#39;s effector by utilizing software is described as &#34;virtual compliance control of multiple degree of freedom robot&#34; in Society of Instrument and Control Engineers, Collection of Papers, Vol. 22, No. 3, pp.343-350, March 1986. 
     The virtual compliance control is characterized in that the mechanical compliance of the robot&#39;s effector is set at a desired value for an external force and hence the robot has mechanical compliance for an object. 
     It is impossible to directly measure the value of an external force required for actual construction of this control system between the robot&#39;s effector and the object. Thus, it is usually measured at a robot&#39;s wrist between the robot&#39;s body and its end effector. It is to be noted that the value of a measured external force to be described in more detail later includes an external force involving a physical parameter of the end effector. It is impossible for the robot to have any mechanical compliance for the object unless (1) the external force involving a physical parameter of the end effector is accurately calculated, and (2) the calculated external force is subtracted from a measured value of the external force to obtain the value of the external force from the genuine object. 
     In order that the robot is provided mechanical compliance for an object, the measured value of the external force conventionally was taken apart from the weight of the end effector itself as an external force due to the physical parameter of the robot&#39;s end effector on the external force. 
     However, when a rotational body having a mechanically large rotational momentum is set as an end effector, the weight of the above-mentioned end effector itself as well as a gyro moment occurring due to interference of the rotational momentum of the end effector and the robot&#39;s momentum must be reflected on the external force in order that the robot has any mechanical compliance for the object. 
     In the conventional virtual compliance control, no gyro moment has been reflected on the external force. Thus, when a rotational body having a mechanically large rotational momentum is set as an end effector, an external force component of a gyro moment occurring due to interference of the rotational motion and the robot&#39;s motion prevails among the external force components, so that an effective control system cannot be constructed as the case may be. 
     SUMMARY OF THE INVENTION 
     It is an object of the present invention to provide a method of controlling a robot to provide mechanical compliance for an object even when a rotational body is set as the robot&#39;s end effector. 
     In order to achieve the above object, the present invention applies a virtual compliance control method (to be described later) to a multiple-degrees-of-freedom robot including means for sensing a velocity and means for sensing an external force to thereby set the external force accurately for a kinematic model of a control system constructed during control. That is, a term of an external force of a kinematic model in the control system is set by subtracting a term influenced by the weight of the end effector from a value provided by a mechanism which detects the external force (conventional method) and further by subtracting a term influenced by a gyro moment of the end effector from the result of the subtraction. 
     According to the present invention, when a rotational body having a mechanically large rotational momentum is set as the end effector, the weight of the end effector itself and a gyro moment occurring due to interference of the rotational momentum of the end effector and the momentum of the robot are reflected on a value provided by the mechanism which detects the external force to thereby construct a control system providing mechanical compliance for an object. 
     In addition, according to the present invention, a point of contact of an object and a robot&#39;s rotational body having a mechanically large rotational momentum set as the robot&#39;s end effector is presumed with higher accuracy, if desired without influence of the gyro moment occurring due to interference of the rotational momentum of the end effector and the momentum of the robot. 
    
    
     BRIEF DESCRIPTION OF THE DRAWINGS 
     FIG. 1 shows an illustrative structure of a device which carries out the present invention. 
     FIG. 2 is a block diagram of a control system of the device. 
     FIG. 3 shows an illustrative structure of a kinematic model of an end effector constructed in a controller to explain an inventive method. 
     FIG. 4 shows an specific example of a gyro effect related to the present invention. 
    
    
     DESCRIPTION OF THE PREFERRED EMBODIMENTS 
     Referring to FIGS. 1-4, an embodiment of the present invention will be described. 
     Referring to FIG. 1, a 6-degrees-of-freedom articulated robot 2 is provided, at its wrist, with a sensor 1, which senses a 6-degrees-of-freedom force and a torque, and with a grinder 3 attached to an end effector thereof. The robot 2 has attached a sensor which senses the rotational angle or rotational angular velocity at each joint thereof. The robot is controlled by a controller 4 which provides mechanical compliance to the end effector utilizing software. The sensor 1 is not necessarily required to be provided between the robot 2 and the end effector and may be provided between the grinder 3 and a grindstone (not shown). Apart from the sensor 1, a sensor which senses the gyro moment appropriately may be provided on a member which constitutes a part of the robot. 
     Referring to FIG. 2, the operator sets in the control system a conditional value called a virtual coefficient to be described later in more detail. The controller extracts thereinto data on the force sensed from the force sensor and position sensed from the joints of the robot and performs an operation on the data. On the basis of the result of this operation, the controller gives a velocity command to the robot to control the robot. 
     Referring to FIG. 3, the following expression is shown as representing the motion of the end effector: ##EQU1## where q is an external force deviation to the reference external force (a translational force and a torque) applied to the grinder; 
     v is the motion velocity (translating velocity and rotational velocity) of the grinder; 
     e is an error of the grinder from a target position and a target attitude (positional error and attitude error) of the grinder; 
     M is a virtual mass of the grinder; 
     k is a virtual spring constant of the grinder; and 
     C is a virtual viscous constant of the grinder. 
     The external force q of the above expression may be obtained from the sensor 1 which senses the force and torque. If the tool is required to contact the workpiece with a finite force, the term q of the expression (1) is replaced with q-qp. 
     The value of v in the expression (1) may be obtained from a sensor which senses the rotational angle or rotational angular velocity of the joints of robot. When the sensor which senses the rotational angle of the joints of the robot is installed, the rotational angular velocity is obtained by differentiation of the sensed value with respect to time. 
     The value of e in the expression (1) may be obtained from the sensor which senses the rotational angle or the rotational angular velocity of the joints of the robot. When the sensor which senses the rotational angular velocity is installed, the rotational angular velocity is obtained by integration of the sensed values with respect to time. 
     By appropriate setting of M, K and C of expression (1), any mechanical compliance is obtained for the external force q applied to the end effector. The M, K and C are called virtual coefficients. 
     Since the value of the external force q required for actually constructing this control system is measured mechanically at the robot&#39;s wrist between the body of the robot 2 and the grinder 3, the obtained value includes an external force due to the physical parameters of the end effector. Thus, the external force q is calculated as follows: 
     
         q=q.sub.m -(q.sub.g +q.sub.i)                              (2) 
    
     where 
     q is a calculated external force (translational force and torque); 
     q m   is a measured external force (translational force and torque); 
     q g   is an external force (translational force and torque) due to the weight of the end effector; and 
     q i   is an external force (translational force and torque) due to a gyro moment acting on a point of grinding in the end effector. 
     In the conventional virtual compliance control, the term of q i   in expression (2) has not been introduced. Thus, when a rotational body having a mechanically large rotational momentum is installed as the end effector, an external force due to a gyro moment caused by interference of the rotational motion of the end effector and the motion of the robot prevails among the components of the calculated external force q. Thus, no effective control system can be constructed as the case may be. 
     For example, when an object is ground by a grinder provided in the end effector while being supplied with a constant pressing force, the rotation of the grinder and the motion of the robot combine to generate a gyro moment. Thus, the object cannot be conventionally ground while being supplied with the constant pressing force. In contrast, according to the present invention, the object is worked upon while being supplied with a constant pressing force by removal of an external force q i  due to the gyro moment from the external force q, as shown in expression (2). 
     A method of presuming the external force from the measured value of a force sensor will be described next as a specific example. In the following description, a 6-dimensional vector of a force and a moment is referred to as a wrench. First, in order to advance the description, the definitions of variables of FIG. 4 are shown as follows: 
     Σ s  is the origin of a coordinate system as the point of measurement of the force sensor; 
     P G  is the point of center of gravity of a tool (including a rotational body); 
     P C  is the point of center of gravity of a rotational body of the tool; 
     P B  is the point of contact of the tool and a workpiece; 
       s  λ G  is a position vector at P G  expressed by Σ s  ; 
       s  λ C  is a position vector at the origin of P C  expressed by Σ s  ; 
       s  λ B  is a position vector at the origin of P B  expressed by Σ s  ; 
     E G  is a moment of inertia caused at P G  by the motion of the tool (including the rotational body); 
     K is an inertial force caused at P G  by the motion of the tool (including the rotational body); 
     M is gravity (including a rotational body); 
     W is the rotational speed of a change in the attitude of the tool; 
     V G  is a translational velocity at P G  ; 
     T is a force applied from the workpiece due to the contact of the tool and the workpiece; 
     Q B  is a moment applied to P B  due to the contact of the tool and the workpiece; 
     N C  is a moment applied to P C  due to the gyro moment of the rotational body; 
     R* is the rotational speed of the rotational body . . . (since it is a physical quantity of another rigid body, it is discriminated by *); 
     I* is a tensor of inertia of the rotational body . . . (since it is a physical amount of another rigid body, it is discriminated by *); 
     ε is a dual number; 
     e is a strain gauge voltage of the force sensor (6 channels); 
     Γ is a voltage conversion matrix of the force sensor (a 6×6 matrix which converts a voltage to a wrench); 
     f s  is a force sensed directly from the force sensor; 
     m s  is a moment sensed directly from the force sensor; and 
     x is an outer product operator. 
     Since the force sensor is composed of the 6-element strain gauge, a wrench q sens  measured from the force sensor is calculated from the following expression (3): 
     
         q.sub.sens =Γe                                       (3) 
    
     Thus, a presumed term of the external force q is calculated from the following expression (4): 
     
         q=q.sub.sens -(q.sub.GRAV +q.sub.tool)                     (4) 
    
     where 
     q GRAV  is a wrench due to the weight of the tool; and 
     q tool  is a wrench due to the mechanical characteristic of the tool. 
     The wrench q tool  includes a gyro moment due to a change in the rotational momentum of the tool and an inertial wrench due to an acceleration/deceleration of the operation of the robot. The expression (4) exhibits that the term of the external force is the wrench measured by the force sensor minus the wrench due to the tool. 
     The wrench  s  F acting on the tool whose reference is the origin of Σ s  will be calculated next. The moment N C  acting on the tool on the basis of the gyro moment is represented by the following expression (5): 
     
         N.sub.C =W×I*R*                                      (5) 
    
     The wrench produced at P B  is represented by the following wrench  S  F B  whose reference is the origin of Σ s  : 
     
         .sub.S F.sub.B =T+ε(Q.sub.B +.sub.S λ.sub.B ×T) (6) 
    
     The wrench produced at P C  is represented by the wrench  S  F C  whose reference is the origin of Σ s  : 
     
         .sub.S F.sub.C =εN.sub.C                           (7) 
    
     The wrench produced at P G  is represented by the wrench  S  F G  whose reference is the origin of Σ s  : 
     
         .sub.S F.sub.G =K+M+ε{E.sub.G +.sub.S λ.sub.G ×(K+M)}(8) 
    
     Thus, the wrench acting on the tool whose reference is the origin of Σ s  is represented by a wrench  S  F whose reference is the origin of Σ s  and consisting of the sum of expressions (6), (7) and (8) as follows: ##EQU2## 
     The wrench  S  F acting on the tool and the wrench q sens  measured from the force sensor are of the same kind of physical quantity. The relationship between the value measured from the force sensor and the expression (9) is sought. The gravity M and the position vector  S  λ G  of P G  are measured beforehand and known. In the motion of the tool, a change with time in each of the translational velocity V G  at the point of center of gravity of the tool and the rotational speed W of a change in the attitude of the tool is small. Thus, K and E G  which are the inertial wrenches of the tool are sufficiently small compared to other terms and, therefore, negligible. The wrench q sens  measured directly from the force sensor is converted to a force f S  and a moment m S  as shown by the following expression (10): 
     
         q.sub.sens =f.sub.s +εm.sub.S                      (10) 
    
     Thus,  S  F is expressed as follows: 
     
         T+M+ε{Q.sub.B +N.sub.C +E.sub.G +.sub.S λ.sub.B ×T+.sub.S λ.sub.G ×M}=f.sub.s +εm.sub.S (11) 
    
     Thus, when the point of contact  S  λ B  is known, the external force is presumed from the measured value of the force sensor, as shown in the following expressions (12) and (13): 
     
         T=f.sub.S -M                                               (12) 
    
     
         Q.sub.B =m.sub.S -(N.sub.C +E.sub.G +.sub.S λ.sub.B ×T+.sub.S λ.sub.G ×M)                                  (13) 
    
     When the external force is known, the point of contact  S  λ B  may be presumed from the expression (11) as follows. When a grinding operation by the grinder is assumed, the contact of the tool and the workpiece is considered as the contact of a convex curve and a curved surface and hence approximated as point contact. This dynamically implies that a force is transmitted but a moment is not at the point contact. Thus, the moment Q B  due to contact is omittable. Accordingly, when δ is any constant, the point contact  S  λ B  is represented by the following expression (14): ##EQU3## Since the expression (14) contains any constant δ, the point contact  S  λ B  cannot be obtained uniquely. Thus, the shape of the tool which is capable of coming into contact with the workpiece as restrictive conditions is used, as shown in the following expression (15): 
     
         J(.sub.S λ.sub.B)=0                                 (15) 
    
     The expression (15) represents the shape of the contactable portion of the tool represented by any vector  S  λ which is in turn represented by Σ s . As an example, when a grinder is used as the tool, it is considered that the grindstone of the grinder is represented by Σ s  as a disc in a space. The line of action of the pressing force intersects with the disc only once except when it is contained in the disc. At this time, δ is obtained and the contact point  S  λ B  is presumed. 
     The end effector in the invention is not limited to the grinder. Any means which machines a workpiece by rotation and moves the end effector may be used. 
     According to the present invention, when a rotational body having a mechanically large rotational momentum is set as an end effector of a robot, a control system providing any mechanical compliance for an object is constructed. 
     Many different embodiments of the present invention may be constructed without departing from the spirit and scope of the invention. It should be understood that the present invention is not limited to the specific embodiments described in this specification. To the contrary, the present invention is intended to cover various modifications and equivalent arrangements included within the spirit and scope of the claims.