Method And Apparatus Using A Force Sensor To Provide Load

Compensation For A Robot

1. Field of the Invention

This invention relates to force controlled robots and more particularly to compensation of the effect of the load on the readings of a force sensor mounted on the robot .

2. Description of the Prior Art

As compared to a position-controlled robot, a force- controlled robot opens a new horizon of applications for a robot because of the controlled contact of the robot with the working environment . A force sensor is the essential part of a force-controlled robot for sensing the contact force.

Figure 1 shows a typical setup of a force sensor 13 on an articulated robot 11. The articulated robot 11 is secured on a mounting base 10. Usually as is shown in Fig. 1, the force sensor 13 is sandwiched between the load 14 and the robot faceplate 12. As is shown in Fig. 1 , an adapter plate 15 may in some cases be used between force sensor 13 and robot faceplate 12. Another adapter plate (not shown) may also be used between force sensor 13 and the load 14. The load 14 is called the sensor load or payload mass and is either the tool or the work object which is to be worked on by a tool located elsewhere or to be mated with another work object.

With such a configuration, gravitational and inertial forces of the load 14 become a part of the force sensor readings. The task of load compensation is to remove the effects of the gravitational and inertial forces of the load 14 in order to obtain the actual contact force between the robot 11 and its working environment .

Fig. 1 shows the various coordinate systems used in load compensation. The direction of gravitational acceleration (Gdir) could be arbitrarily located with

respect to the robot base frame X _ba s _e - The base frame (or base coordinate system) X _base is attached to the robot base plate 10. As is also shown in Fig. 1, coordinate systems are setup for the faceplate 12, force sensor 13, load 14 as X _p i _at e, Xsensor, and Xioad respectively.

Load compensation commonly uses a simplified model such as Equations 1 and 2 below to calculate the contact force between the robot 11 and its working environment based on a prediction of gravity force F ^pral and torque M ^pred of the sensor load:

^•contact j _j meas τjpred poff jQ contact _ tø meas _ jQ pred _ ^ off

F ^pre " = mg _sensor

M ^pred =c _smsor xmg _sensor (1)

o — \RP ^tate \fl )j? ^senior I n { 9 "1 b sensor ^~ L ⁿ base \ ^σ Opiate J 6 base ^ ^Z '

where p ^off Mf Mf] is the offset of the force sensor measurement, g _bme is the direction of gravitational acceleration expressed in the robot base frame X _base/ g _semor is the direction of gravitational acceleration expressed in the force sensor frame X _sensor , m is the mass of the load 14,

^ _sensor — \ ^x _c 3V ^Z λ i ^s the center of gravity of load 14, expressed in the force sensor frame X _sensor ,

R _p ^e i _a i° ^r is the rotation matrix from the robot faceplate

frame X _p i _a te to the force sensor frame Xsensor,

R^f'ψ) is the rotation matrix from the robot base frame X _base to the faceplate frame Xpiate/ which is a function of robot joint angles θ ≡[θ _γ θ ₂ ... O _n^1 θ _n ] , where n is the number of robot joints.

Equations (1) and (2) contain 16 unknown parameters: m "' / ^x rc I J vc > Z 7-c i F x ^off ' F \ ^off i F z ^off ' M x ^off ' ¹ M ^V1 \ ^off i ¹ M ^Y1 z° ^M i a gbase a ^d n ^li d ^u -

R _p ^S ,™ ^r . These parameters have to be identified before load compensation can be applied. The conventional load compensation method described above assumes g _bme a.nά R^,/ ⁰ are known beforehand, and therefore that method does not include any procedure to identify them. The user has to find applicable techniques and/or devices to measure them. In a typical mounting configuration of robot 11 on the floor, g _base is simply

0 assumed to be 0 which is vertically downwards. In

-9.81 many cases, the force sensor frame X _se nsor is carefully aligned with the robot faceplate frame Xpiate through mechanical means such as a location pin and positioning screws.

A flowchart description of the conventional, that is, prior art, procedure for identifying payload mass and center of gravity to compensate for load is shown in Figure 7. In that compensation the robot 11 is moved to a set of predefined postures. At each posture, the force sensor measurement is recorded after the robot 11 stands still. Because there is no contact involved during the motion and the force measurement is taken only after the robot 11 becomes still, the contact force and torque in equations (1) and (2) are all zeros. Applying standard least square techniques with the collected measurement

data, the mass and center of gravity of load 14 can be solved from equations (1) and (2) .

As can be appreciated, the conventional load compensation method is good for less demanding applications where the requirement on the contact force accuracy is low. One exemplary application with such a requirement is heavy part assembly, where contact force is usually large and is required to be controlled only within a limit. The conventional load compensation method is costly and not easy to apply, as it requires an extra amount of effort and equipment to find Gdir and the force sensor coordinate system (FCS) . Therefore, the conventional load compensation method cannot be used to increase the accuracy of load compensation for demanding applications such as polishing where a very small contact force has to be accurately maintained.

In contrast to the conventional load compensation method, the present invention can provide the accurate load compensation for demanding applications and reduce the equipment and effort required to apply the desired load compensation. Further the present invention provides a means to easily verify the installation and integrity of a force sensor 13.

Summary of the Invention

A system for providing load compensation. The system has a robot. The robot has a force sensor, a faceplate, a load and a baseplate. The force sensor is mounted between the faceplate and the load. The faceplate, force sensor, load and baseplate each having a predefined and different coordinate system. The system also has apparatus connected to the robot for controlling motion of the robot. The robot motion controlling apparatus is responsive to readings from the force sensor to: define the relative position of the force sensor coordinate system with respect to the faceplate

coordinate system; determine the direction of gravitational acceleration in the baseplate coordinate system; determine the mass and center of gravity of the load.

The apparatus subtracts from the force sensor reading a predicted gravity force of the load on the force sensor calculated using the defined relative position of the force sensor coordinate system with respect to the faceplate coordinate system, the determined direction of gravitational acceleration in the baseplate coordinate system and the determined mass and center of gravity of the load to thereby compensate readings of the force sensor for the effect of the load so that the robot can bring the tool in contact with another object with a predetermined contact force.

In a robot having a force sensor, a faceplate, a load and a baseplate, the force sensor mounted between the faceplate and the load where the faceplate, force sensor, load and baseplate each have a known coordinate system which are different from each other, a method for providing compensation for the effect of the load on the readings of the force sensor so that the robot can bring the tool in contact with another object with a predetermined contact force with the method: using readings from the force sensor to define the relative position of the force sensor coordinate system with respect to the faceplate coordinate system; using readings from the force sensor to determine the direction of gravitational acceleration in the baseplate coordinate system; using readings from the force sensor to determine the mass and center of gravity of the load; subtracting from the force sensor readings a predicted gravity force of the load on the force sensor calculated using the defined relative position of the

force sensor coordinate system with respect to the faceplate coordinate system, the determined direction of gravitational acceleration in the baseplate coordinate system and the determined mass and center of gravity of the load to obtain force sensor readings compensated for th effect of the load on the force sensor reading; and using the compensated force sensor readings to control the robot so that the robot can bring the tool in contact with another object with a predetermined contact force.

A system for providing compensation for the effect of a load held by a robot on readings of a force sensor mounted on the robot so that the robot can bring the tool into contact with another object with a predetermined contact force. The robot also has a faceplate and a baseplate. The force sensor is mounted between the faceplate and the load. The faceplate, force sensor, load and baseplate each having a known coordinate system which are different from each other. The load compensation providing system has: a computing device having therein program code usable by the computing device. The program code has: code configured to use readings from the force sensor to define the relative position of the force sensor coordinate system with respect to the faceplate coordinate system; code configured to use readings from the force sensor to determine the direction of gravitational acceleration in the baseplate coordinate system,- code configured to use readings from the force sensor to determine the mass and center of gravity of the load; code configured to subtract from the force sensor readings a predicted gravity force of the load on the force sensor calculated using the defined relative position of the force sensor coordinate system with

respect to the faceplate coordinate system, the determined direction of gravitational acceleration in the baseplate coordinate system and the determined mass and center of gravity of the load to obtain force sensor readings compensated for the effect of the load on the force sensor reading; code configured to use the compensated force sensor readings to control the robot so that the robot can bring the tool into contact with the another object with the predetermined contact force.

Description of the Drawing

Fig. 1 shows a typical setup of a force sensor on a articulated robot.

Fig. 2 shows the robot faceplate, the force sensor, the load and the adapter plate for the robot shown in Fig. 1.

Fig. 3 shows the technique for identifying the faceplate X-axis in the force sensor frame.

Fig. 4 is a flowchart showing the steps in the load compensation method of the present invention.

Fig. 5 is a flowchart showing the procedure for identifying the force sensor rotation matrix.

Fig. 6 is a flowchart showing the procedure for identifying the direction of gravity.

Fig. 7 is a flowchart showing the conventional procedure of providing load compensation.

Fig. 8 shows a system which may be used to implement the load compensation method of the present invention.

Detailed Description The present invention provides an accurate load compensation of a force sensor 13. Fig. 2 is an enlargement of the robot 11 of Fig. 1 and shows the robot faceplate 12, the force sensor 13, the load 14 and the adapter plate 15. As is shown in Fig. 2, the faceplate 12 has a frame X _p i _a te with Cartesian axes identified in Fig. 2 by the axes labeled x _p , y _p , z _p .

The load compensation of the present invention enhances the conventional load compensation method described above by identifying two new parameters, namely the gravitational acceleration Gdir direction and the force sensor frame Xsensor, both of which are shown in Fig. 1. Both translational and rotational offsets exist between the force sensor frame Xsensor shown in Fig. 1 and the faceplate frame Xplate shown in Fig. 2. One aspect of the present invention is finding these offsets and the mathematical principles associated with the present invention are explained with the aid of the equations described below and Figs. 2 and 3.

As will be appreciated by one of skill in the art, the present invention may be embodied as a method, system, or computer program product. Accordingly, the present invention may take the form of an entirely hardware embodiment, an entirely software embodiment (including firmware, resident software, micro-code, etc.) or an embodiment combining software and hardware aspects that may all generally be referred to herein as a "circuit," "module" or "system."

Furthermore, the present invention may take the form of a computer program product on a computer-usable or computer- readable medium having computer-usable program code embodied in the medium. The computer-usable or computer-readable medium may be any medium that can contain, store, communicate, propagate, or transport the program for use by or in connection with the instruction execution system, apparatus, or device and may by way of example but without limitation, be an electronic, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, device, or propagation medium or even be paper or other suitable medium upon which the program is printed. More specific examples (a non- exhaustive list) of the computer- readable medium would include: an electrical connection having one or

more wires, a portable computer diskette, a hard disk, a random access memory (RAM) , a read-only memory (ROM) , an erasable programmable read-only memory (EPROM or Flash memory) , an optical fiber, a portable compact disc readonly memory (CD-ROM) , an optical storage device, a transmission media such as those supporting the Internet or an intranet, or a magnetic storage device.

Computer program code for carrying out operations of the present invention may be written in an object oriented programming language such as Java, Smalltalk, C++ or the like, or may also be written in conventional procedural programming languages, such as the "C" programming language . The program code may execute entirely on the user's computer, partly on the user's computer, as a stand-alone software package, partly on the user's computer and partly on a remote computer or entirely on the remote computer or server. In the latter scenario, the remote computer may be connected to the user's computer through a local area network (LAN) or a wide area network (WAN) , or the connection may be made to an external computer (for example, through the Internet using an Internet Service Provider) .

The present invention for providing accurate load compensation for a force sensor mounted on a robot comprises the following four steps, all of which are shown in flowchart 20 of Fig. 4. These steps are performed in a software program operating in a computing device, which may be a robot controller or a laptop or desktop computer attached to the robot controller. The steps are:

1) identifying in step 22 the force sensor rotation matrix by commanding the robot 11 to rotate a number of different angles along each Cartesian axis (x _p , y _p , Zp) of the faceplate frame Xplate, recording the force sensor 13 reading at each rotation, then finding the force sensor coordinate system (FCS) with respect to the

robot faceplate frame by solving a set of mathematical equations, described below in the section headed "(D Identification of force sensor rotation matrix", formed based on the principle that the force component along the rotation axis is invariant to the rotation angle,

2) identifying in step 24 the gravity direction by- commanding the robot 11 to a set of different postures, recording the force sensor 13 reading at each posture, and then solving a set of mathematical equations, described below in the section headed "(2) Identification of gravity direction", for the direction of gravitational acceleration (Gdir) in the robot base frame Xbase,

3) identifying in step 26 the payload mass, that is, the load 14 mounted on the force sensor 13, and the center of gravity by commanding the robot 11 to a set of different postures, recording the force sensor 13 reading at each posture, and solving, as is well known to those skilled in the art, a set of mathematical equations which are equations (1) and (2) shown above with the contact force F ^Contact and the torque M ^contaa set to zero, for the mass and center of gravity of the load 14 on force sensor 13 , and

4) calculating in step 28 the predicted gravity force of the sensor load using a model containing Gdir, FCS, load mass and center of gravity, the model described in equations (1) and (2), and then subtracting this prediction from the actual force sensor 13 reading.

In the above description, steps 22, 24 and 26 of Fig. 4, respectively, all require the robot 11 to perform a set of motions. Step 22 has a specific requirement on the motion pattern, while steps 24 and 26 do not. This means in practice that steps 24 and 26 can each use the same motion as is used in step 22, or steps 24 and 26 can each use the same motion which is different from the motion used in step 22 or steps 24 and 26 can each use different motions. For example, step 24 can use the

motion used in step 22 and step 26 can use another motion.

The identification of the force sensor rotation matrix is described below and is followed by the identification of the gravity direction. (1) Identification of force sensor rotation matrix

The relative position of the force sensor frame with respect to the robot faceplate 12 of Fig. 1 is defined by a translational vector and the rotation matrix R _p ^S ZT described above. The present invention can only identify the rotational part. Other methods, for instance direct measurement of the distance in the three directions x, y, z, using for example a measurement tape, have to be used to identify the translational part.

Identifying R _p i", ^s ° ^r is equivalent to finding the directional cosine of each faceplate axis (x _p , y _p , z _p ) in the force sensor frame (Fig. 3) . As is well known, for a given robot setup the gravity force vector, G , of the sensor load is a constant in the robot base frame. When the robot 11 rotates 360 degrees along a negative axis of the faceplate frame, for example the X-axis, the projection Gx 18a, as shown in Fig. 3, of the gravity force vector G onto the rotation axis is also constant. But as is shown in Fig. 3, on a plane perpendicular to the rotation axis, the projection of the gravity force vector forms a swirling circle 18b as the robot 11 rotates the 360 degrees. The swirling direction and X- axis follows the right hand rule, from which r _x , the directional cosine of the faceplate X-axis in the force sensor frame and described in more detail below, which has three components [rxl rx2 rx3] , can be calculated.

Mathematically, the above description can be written as :

(FγX (P ^mea \ ^ x 'ι (FγX

(FγX = c _r -^- /-* r, ^■ (Fr ^a x = c. ( 3 ) (f ^mea \ tpmea ^

where c _r , c. and c. are constants, F" FT and F ^fflM are the force sensor reading, r _x , r _λ and r. are the directional cosine of the faceplate coordinate axes represented in the force sensor frame whose relation with the rotation matrix R p ^' IZlatTe is given as: r] opiate L _r 1 _ ( D sensor y _ nsemυr Vx '\ 'z \ \ ⁿ plate I (4:

T,

Applying a standard 3D plane fitting procedure to fit all the force sensor measurement during the rotation along the X axis of the faceplate frame, r _x will be obtained as the norm of the plane according to equation (3) . r _v and r. can be obtained using exactly the same method as that described above for obtaining r _x but rotating the robot along the Y axis and Z axis of the faceplate frame respectively.

Following the above described mathematical principles, to identify the force sensor frame Xsensor, the robot 11 needs to incrementally rotate along each axis of the faceplate frame Xplate. At each rotation the force sensor measurement is recorded only when the robot 11 becomes still. After incrementally rotating along each axis of the faceplate frame 12 to thereby collect all of the needed measurement data, the measurement data is used in equation (3) for a least squares solution of the force sensor rotation matrix. Figure 5 is a flowchart for the procedure described above for identifying the force sensor rotation matrix.

(2) Identification of gravity direction

When there is no contact force present, equations (I) - (2) can be rewritten as:

I nplate [Q Xnsensor Y mea

Equation (5) is used to identify g _base and F ^ojl if

R _p ^s _late is already known. The method described in (1) can be applied to identify R^' if needed. A linear least squares solution for equation (5) gives:

ms{gχ ) _ba se

from which g _base can be easily calculated as the normalized

mg{g > xλ ' bbaassee vector of ^m g(8Xse _ ^m s{gXase.

A flowchart description of the above identification procedure is shown in Figure 6. First, a set of arbitrarily selected postures are defined. Then the robot is commanded to move to each defined posture. At each posture, the force sensor measurement is recorded only after the motion is completed and the robot becomes still. At the end, a number of measurement data is collected and entered into equation (7) for the solution of the gravity direction.

The procedure described in Figure 6 is very similar to the one in Figure 5 for the identification of the force sensor rotation matrix. The major difference is that in gravity direction identification the robot can take arbitrary motions.

Referring now to Fig. 8, there is shown a system 100 which may be used to implement the load compensation method of the present invention described above. The system 100 includes the method 102 in the form of software that is on a suitable media in a form that can be loaded into the robot controller 104 for execution. Alternatively, the method can be loaded into the controller 104 or may be downloaded into the controller 104, as described above, by well known means from the same site where controller 104 is located or at another site that is remote from the site where controller 104 is located. As another alternative, the method 102 may be resident in controller 104 or the method 102 may installed or loaded into a computing device (not shown in Fig. 8) which is connected to controller 104 to send commands to the controller.

As can be appreciated by those of ordinary skill in the art, when the method is implemented in software in controller 104, the controller functions as a computing

device to execute the method 102. The controller 104 is connected to robot 106 which is used to perform the process 108 that uses the load 14. Thus, if the method 102 is executed by controller 104 or if the controller 104 receives commands from a computing device that executes the method 102 the robot 106 is controlled to perform the process 108 in accordance with the present invention. It should be appreciated that the adaptive PI control method 102 can be implemented on the robot controller 104 as a software product, or implemented partly or entirely on a remote computer, which communicates with the robot controller 104 via a communication network, such as, but not limited to, the Internet .

It is to be understood that the description of the foregoing exemplary embodiment ( s ) is (are) intended to be only illustrative, rather than exhaustive, of the present invention. Those of ordinary skill will be able to make certain additions, deletions, and/or modifications to the embodiment (s) of the disclosed subject matter without departing from the spirit of the invention or its scope, as defined by the appended claims.