JP5512406B2 - Fault detection method for external force detection interface - Google Patents

Description

  The present invention relates to a failure detection method for an external force detection interface that detects an external force acting on a robot, a manipulator, etc., and detects a failure of a sensor of the external force detection interface so that a correct external force can be estimated.

  Human support robots are expected to be applied in many fields such as medical and welfare sites and home work. In such an environment where humans come into contact, it is necessary to take sufficient safety measures so that the movement of the robot does not endanger humans. To that end, a means to accurately detect contact with humans is indispensable. .

As shown in FIG. 14, the present inventors fixed a rigid shell-type cover 21 to a movable body 20 such as a robot or a manipulator via force sensors 22, 23, 24, and attached to the shell-type cover 21. An external force detection interface that detects the magnitude and point of action of external force by force sensors 22, 23, and 24 has been developed and is shown in Patent Document 1 below.
In this interface, for example, when the crustacean cover 21 is supported by one 6-axis force sensor (force components in x, y, and z directions and moment components in x, y, and z directions can be measured), If the geometric shape of the crustacean cover 21 and the position information of the force sensor are known, the magnitude and point of action of the external force acting on the crustacean cover 21 using five types of measurement data among the six types of measurement data. Can be detected.

14D, when the crustacean cover 21 is supported by the three force sensors 22, 23, 24, the geometric shape of the crustacean cover 21 and the force sensors 22, 23, 24 are used. If the total number of axes measurable by the force sensors 22, 23, 24 is 5 or more when the position information is known, the magnitude of the external force acting on any position of the crustacean cover 21 and the point of action Can be requested. For example, if the force sensor 22 and the force sensor 23 have a biaxial measuring function and the force sensor 23 has a monoaxial measuring function, a total of five types of measurement data can be obtained. The magnitude and point of action of the external force can be detected.
As described above, the external force detection interface in which the crustacean cover is fixed to the movable body using the force sensor can detect the external force with a simple configuration, and by covering the whole body of the robot or manipulator with the crustacean cover, Robots and manipulators can be easily tactile.

However, the sensor may fail due to an impact or the like.
Japanese Patent Application Laid-Open No. 2004-228561 discloses a configuration for detecting such a failure by making a sensor redundant. The apparatus described in the document includes a base member, an end effector movable with respect to the base member, a plurality of adjustable links connecting the end effector to the base member, and a state of each adjustable link. And a sensor associated with each adjustable link for detecting a sensor, wherein one additional sensor is connected between the base member and the end effector, thereby detecting a sensor failure. .

JP 2009-162599 A JP 2007-520361 A

Even in the case of an external force detection interface in which the crustacean cover (end effector) is fixed to a movable body with a force sensor, if an erroneous signal is output from the failed sensor, the risk of the control system becoming unstable or running away is sufficient. Have.
The present invention was devised in view of such circumstances, and provides a method of detecting an external force by detecting a failure of a force sensor of an external force detection interface in which an end effector is fixed to a movable body by a force sensor. It is an object.

  The present invention is a force sensor failure detection method for an external force detection interface in which a rigid end effector is constrained to a movable body by a force sensor, and an external force acting on the end effector is calculated from a response value of the force sensor. In addition, the force sensor having a redundant number of response channels constrains the end effector to the movable body, and the response value vector of the force sensor when detecting an external force on a hyperplane that can be taken by the response value vector of a normal force sensor. The deviation between the response value vector and the theoretical value on the hyperplane when the force is constrained is obtained, and the channel of the force sensor that responds to the incorrect response value is specified based on the magnitude of this deviation. And

The response value vector of the sensor can take any value on the vector space depending on the external force value. However, like this external force detection interface, a plurality of sensors have a rigid body end with a lower degree of freedom than the number of sensor channels. When the effector is supported, the end effector mechanically restrains the force sensor, and the possible value of the sensor response value vector is limited to a lower dimension. For example, when the end effector is supported by m three-axis force sensors, when the sensor response value is determined in a one-to-one correspondence with a six-dimensional external force pattern generated in the end effector, a set of sensor response value vectors Exists on the 6-dimensional hyperplane in the 3 × m-dimensional space. If the sensor response value vector F ^d contains significant errors, since the sensor response value vector F ^d leaves the six-dimensional hyperplane, the theoretical value of the hyperplane corresponding to the sensor response value vector F ^d a (F ^d deviation F ^f between the ultrasonic theoretical value in the plane) and F ^d when the constrained hyperplane is greater. Therefore, based on the magnitude of the deviation F ^f , the force sensor channel that responds with an incorrect response value can be specified.

Further, the failure detection method of the present invention applies a known external force to the end effector constrained to the movable body by the force sensor, and the inverse transformation matrix T ^inv for constraining the response value vector of the force sensor to the hyperplane. And a second step of ^obtaining a theoretical value F ⁱ on the hyperplane by constraining the response value vector F ^d of the force sensor at the time of detecting an external force to the hyperplane using the first transformation matrix T ^inv . , A third step for obtaining a deviation F ^f between the response value vector F ^d and the theoretical value F ^i, and an element q _{j of} Q represented by F ^f = QF ^d and the deviation for each channel j a fourth step of calculating the inner product F ^f · q _j with F ^f, characterized in that the inner product F ^f · q _j comprises a fifth step of determining a failure of channel j exceeds the threshold value, the .

In principle, the sensor response value vector F ^d is determined in a one-to-one correspondence with the pattern of the six-dimensional external force Γ by the following (Equation 1).
The inverse transformation matrix T ^inv is a matrix that performs an inverse transformation of the transformation matrix T that transforms the response value vector F ^d into the external force Γ, and has an effect of constraining F ^d to a six-dimensional hyperplane. However, since the number of dimensions of the vector F ^d is larger than 6 and the number of dimensions of the external force Γ is 6, T does not become a regular matrix, so that T ^inv is not uniquely determined. Therefore, a known external force is applied to the end effector, and an inverse transformation matrix T ^inv is derived based on the response value vector of the force sensor at that time. Further, when the j-th channel shows an incorrect value due to a sensor failure, the deviation F ^f takes a value substantially proportional to q _j . Therefore, by calculating the inner product F ^f · q _j for each channel and determining whether or not the value exceeds the threshold value, the channel at the failure location can be specified.

Further, in the failure detection method of the present invention, the correct response value is estimated from the response value vector of the force sensor including the response value of the channel j determined to be a failure, so that the sensor response value of the channel j is an element of the theoretical value F ⁱ . The value is replaced with the value of j and the other elements are theorized by using the sixth step for obtaining the estimated value F ^c set with the elements of the response value vector F ^d as it is, the estimated value F ^c and the inverse transformation matrix T ^inv. a seventh step of obtaining the value F ^i, as provided an eighth step of replacing the value of the element j in the estimate F ^c by the value of the element j in the seventh theoretical F ⁱ calculated in step, the May be.
The theoretical value F ⁱ for obtaining the alternative value of the sensor response value in the sixth step is not accurate because it is calculated based on F ^d including an error caused by the failure. Therefore, the theoretical value F ⁱ is recalculated based on the estimated value F ^c, and the calculated value is substituted into the response value of the failure location as the estimated value, so that a more accurate estimated value F ^c is newly obtained. In addition, by repeating this process, the sensor response value at the failure location is more accurately derived.

  According to the present invention, it is possible to detect a failure occurring in a force sensor of an external force detection interface in which an end effector is fixed to a movable body using a force sensor, and correct from a sensor response value including an error caused by the failure. External force can be estimated.

Diagram showing the external force detection interface attached to the robot arm The figure which shows the internal structure of the external force detection interface of FIG. The figure which shows the structure of the control part which performs the failure detection method of this invention Illustration showing hyperplane Flow chart showing the procedure for acquiring calibration data in the failure detection method of the present invention The flowchart which shows the procedure which specifies the failure channel in the failure detection method of this invention The flowchart which shows the procedure of the external force estimation method of this invention The figure which shows the simulation result (the 1) Diagram showing simulation results (Part 2) Diagram showing simulation results (Part 3) The figure which shows a simulation result (the 4) The figure which shows a simulation result (the 5) Diagram showing model used for simulation A diagram showing a conventional external force detection interface

FIG. 1 shows a state where an external force detection interface of the present invention is mounted on a robot arm. As shown in FIG. 2, the external force detection interface includes a rigid end effector 10 and three three-axis force sensors 25, 26, and 27 that support the end effector 10 on the robot arm 30 ( The force sensor 27 is attached to the surface opposite to the surface to which the force sensor 26 of the robot arm 30 is attached, and thus cannot be seen in the drawing. The end effector 10 is a crustace type cover made of an acrylic plate.
When an external force is applied to the end effector 10, the sensor response values of 9 (= 3 axes × 3) channels of the force sensors 25, 26, 27 are sent to the control unit 40 as shown in FIG. 3. The failure detection unit 41 of the control unit 40 detects a channel that has output an incorrect sensor response value due to a failure from the sensor response value vector. The external force estimation unit 42 of the control unit 40 estimates a correct external force based on the sensor response value vector.
The three triaxial force sensors 25, 26, and 27 measure the three-axis external force and the three-axis external force moment generated in the end effector 10 when an external force is applied to the end effector 10. Since the total number of channels of the force sensors 25, 26, and 27 is 9, the force sensors 25, 26, and 27 have redundant channels.

Summing each F ^e of the external force and moment generated in the end effector 10 of the external force detecting interface, placing the M ^e, the equilibrium of forces and moments acting on the end effector 10 in the following respective equation (2) (Equation 3) expressed.
Here, F ^s _i represents the force generated at the i-th support point, and ideally F ^s _i matches the sensor output. However, the vector of the sensor output value is opposite to F ^s _i due to the relationship of action and reaction. P ^o represents the reference coordinate of the external force moment, and P ^o _i represents the coordinate of the i-th support point.
(Equation 2) and (Equation 3) are uniformly expressed by the following equation (Equation 4).
Γ, T, and F ^d are expressed by (Equation 5), (Equation 6), and (Equation 7).
Here, t ^p _i is a distortion target matrix for calculating the outer product with the vector (P ^s _i −P ^o ), and is expressed by the following (Equation 8).
M represents the number of triaxial force sensors that support the end effector 10, and is 3 in this external force detection interface.
(Equation 4) is an expression for linearly mapping a 3 × m-dimensional vector space formed by the sensor response value vector F ^d into a 6-dimensional force / moment space.

  The response value vector of the sensor can take any value on the vector space according to the external force value. However, when the plurality of sensors 25, 26, 27 support the rigid end effector 10 having a lower degree of freedom than the number of channels of the sensor as in the external force detection interface, the end effector 10 is the force sensor 25, 26. , 27 are mechanically constrained, and the possible values of the sensor response value vector are limited to lower dimensions. When sensor response values are determined in a one-to-one correspondence with 6-dimensional external force patterns generated in the end effector 10, a set of sensor response value vectors exists on a 6-dimensional hyperplane in a 3 × m-dimensional space.

The hyperplane is an (n−1) -dimensional figure that divides an n-dimensional space into two parts, and the size in one direction is zero.
FIG. 4 shows a response value that is displayed only in the 3 × m dimensional space for the sake of simplification and is constrained by the 2D hyperplane. When there is no sensor response value vector on this hyperplane, a value that does not satisfy the mechanical constraint condition of the end effector 10 is output, so that any one of the force sensors 25, 26, and 27 has an incorrect value. Is determined to be output.

The failure detection unit 41 of the control unit 40 pays attention to this characteristic and performs failure detection and failure location identification.
First, the sensor response value vector F ^d is determined in a one-to-one correspondence with a six-dimensional external force pattern by (Equation 1) in principle.
T ^inv is a matrix for performing an inverse transformation of the transformation matrix T, and has an effect of constraining F ^d to a 6-dimensional hyperplane. Since the number of dimensions of the vector F ^d is greater than 6, and T does not become a regular matrix, T ^inv is not uniquely determined. Further, the value of T ^inv is determined by an internal force or the like when the end effector 10 is fixed by the force sensors 25, 26, and 27. Therefore, in order to derive T ^inv , it is necessary to obtain experimental data for calibration after the end effector 10 is fixed.

Therefore, in order to secure calibration data, an experiment is performed in which n different known external forces are applied. If the i-th external force vector at that time is represented by Γ _i , the external force data for calibration is represented by the following equation (Equation 9).
Then, the following equations (Equation 10) and (Equation 11) hold.
Here, F ^d _i represents the output of the force sensor 25, 26, 27 when the i-th external force is applied.
When six or more types of external forces are applied, the inverse transformation matrix T ^inv is derived by the following equations (Equation 12) and (Equation 13) based on the data.
Due to the nature of the pseudo inverse matrix, the matrix obtained by (Equation 12) is a least-squares approximate solution for the measured value. Therefore, the calculation accuracy of T ^inv can be improved by increasing the number of patterns of external force and acquiring a lot of calibration data.
A calibration experiment is performed based on the above theory, and failure detection is performed using an inverse transformation matrix T ^inv derived in advance. As described above, F ^d is theoretically constrained to a six-dimensional hyperplane, and its ideal value (theoretical value) F ⁱ is derived by the following equation (Equation 14).

However, in actuality, due to various types of errors, there is a deviation F ^{f from the} ideal value (theoretical value) as shown in the following equation (Equation 15).
Here, when (Equation 14) is substituted into (Equation 15), the following equation (Equation 16) is obtained.
However, Q is expressed by the following equation (Equation 17).
When F ^d contains a large error, it is away from the 6-dimensional hyperplane, so F ^f becomes large. In particular, when the j-th channel shows an incorrect value due to a sensor failure, F ^f takes a value substantially proportional to q _i .
Therefore, by calculating F ^f · q _j for each channel and determining whether or not the value exceeds the threshold value, it is possible to determine the sensor failure and specify the failure location.

The procedure in which the failure detection unit 41 detects a sensor failure is shown in the flowcharts of FIGS.
FIG. 5 shows a procedure for ^obtaining the inverse transformation matrix T ^inv for calibration as advance preparation.
The first known external force Γ _j is applied to the end effector 10 (step 1, step 2), and the sensor output F ^d _j at that time is acquired (step 3). Various known external forces Γ _j are applied to the end effector 10 so as to obtain calibration data with sufficient accuracy (steps 4, 5 and 2), and sensor outputs F ^d _j corresponding to the various known external forces Γ _j are obtained. Obtain (step 3). When the input of the known external force Γ _j is completed (Yes in Step 4), an inverse transformation matrix T ^inv is derived from ( ^Equation 12) ( ^Equation 13) (Step 6).

FIG. 6 shows a procedure for detecting a failure of each force sensor 25, 26, 27 when detecting an external force with the external force detection interface.
The response values of the respective channels of the force sensors 25, 26 and 27 are acquired (step 11), and the theoretical value F ⁱ on the hyperplane is calculated by (Equation 14) (step 12). Next, the deviation F ^f between the theoretical value F ⁱ and the actual sensor response value F ^d is obtained by (Equation 15) (Step 13), and each channel of Q represented by the deviation F ^f and (Equation 17) is obtained. the inner product of the corresponding component q _j to calculate (step 15), and compares the value with a threshold value (step 16). ^If the value of F ^f · q _j exceeds the threshold value, the channel is determined to be a failed channel (step 17).
Such processing is performed for all channels (step 18, step 19, and steps 15 to 17), and when detection of all failed channels is completed (Yes in step 18), the external force estimation unit 42 of the control unit 40 is correct. External force is estimated (step 20).

The flowchart of FIG. 7 shows the processing procedure of the external force estimation unit 42.
The sensor response value of the failure channel j is replaced with the theoretical value F ⁱ , and the estimated value F ^c is set as the actual sensor response value for the other elements. The theoretical value of channel j is obtained by (Equation 18) from (Equation 15) and the value of F ^f · q _j (step 21).
The estimated value F ^c is as shown in (Equation 19).
Since the theoretical value F ⁱ used at this time is obtained based on F ^d including an error caused by a failure, it is not necessarily accurate. Therefore, using the estimated value F ^c , the theoretical value F ⁱ is calculated again according to (Equation 14) (step 22), and the element corresponding to the channel j of the estimated value F ^c is re-calculated as the corresponding value of the theoretical value F ⁱ . (Step 23).
Further, such a process is repeated a required number of times (step 24), and the accuracy of the estimated value F ^c is increased.

FIGS. 8-12 has shown about the result of having simulated the process of the failure detection part 41 and the external force estimation part 42. FIG. This simulation was performed on a two-dimensional plane for simplification.
FIG. 13 shows a model of an external force detection interface used in the simulation, and has a structure in which a circular end effector 11 is supported by three biaxial force sensors 28. The three biaxial force sensors 28 measure forces in the x-axis and y-axis directions, and are fixed to the center base 31.
The end effector 11 slightly fluctuates in the x axis, the y axis, and the rotation direction. The rotation angle is μ. On the other hand, the number of channels to be measured is 6 in total with 2 axes × 3, and has redundancy for 3 degrees of freedom. The end effector 11 is a rigid body, and the biaxial force sensor 28 that supports the end effector 11 is regarded as a highly rigid spring. However, it is assumed that the spring has rigidity k in the x-axis and y-axis directions, and no moment is generated around the contact point between the spring and the end effector 11.

External force,
F ^e _x = 1.0 × sin (t)
The result when given by is shown in FIG.
F ^e _y = 1.0 × cos (1.3t)
The result when given by is shown in FIG.
M ^e _θ = 0.1 × sin (0.1t)
FIG. 10 shows the result when given by.
8 to 10, the external force response value (response value) obtained from the sensor 28, the estimated external force value (estimated value) estimated by the external force estimation unit 42, and the actual external force value (true value) are superimposed. Show. However, in order to simulate a failure, various erroneous sensor response values are output as external force response values every 5 seconds for 2 seconds.
When no failure occurs, both the external force response value and the estimated external force value match the actual external force value. However, when a failure occurs, the deviation between the external force response value and the actual external force value increases. At that time, the estimated external force tracked the actual external force value with almost no error, so it was confirmed that the external force estimation functioned correctly when a failure occurred.
As an exception, it was confirmed that the estimated external force includes an error of about 0.1 N between 20 seconds and 23 seconds. The failure input given at this time was not significantly different from the actual response value of the sensor and was not recognized as a failure. Therefore, the response value of the sensor was directly applied to the estimated value. In other words, a small error proportional to the error that remains unrecognizable will remain.

Further, FIG. 11 shows that, as an external force, a 1 kg weight is placed three times at different positions on the end effector, and at the same time, a failure is simulated for 15-17 seconds, 20-22 seconds, and 25-27 seconds. The external force response value (a) and the moment response value (b) in a state where the external force estimation is not performed when a signal is given to one channel are shown.
On the other hand, FIG. 12 shows a state in which the external force is estimated in the case of FIG.
Also from the results of FIGS. 11 and 12, it can be confirmed that the external force estimation at the time of the failure is functioning correctly.

  The present invention can perform failure detection and external force estimation of an external force detection interface that can be widely used for robots, manipulators, machine tools, wheelchairs, vehicles, and the like, and can further enhance the usefulness of the external force detection interface.

DESCRIPTION OF SYMBOLS 10 End effector 11 End effector 20 Movable body 21 Shell-type cover 22 Force sensor 23 Force sensor 24 Force sensor 25 3-axis force sensor 26 3-axis force sensor 27 3-axis force sensor 28 2-axis force sensor 30 Robot arm 31 Base 40 Control unit 41 Failure detection unit 42 External force estimation unit

Claims (3)

A force sensor failure detection method of an external force detection interface in which a rigid end effector is restrained by a movable body by a force sensor, and an external force acting on the end effector is calculated from a response value of the force sensor,
Restrain the end effector to the movable body with a force sensor having a redundant number of response channels,
Obtain a deviation between the theoretical value on the hyperplane when the response value vector of the force sensor at the time of external force detection is constrained to the hyperplane that can be taken by the response value vector of the normal force sensor, and the response value vector,
A force sensor failure detection method for an external force detection interface, wherein a channel of a force sensor that responds to an erroneous response value is specified based on the magnitude of the deviation. The failure detection method according to claim 1,
Applying a known external force to the end effector to obtain an inverse transformation matrix T ^inv for constraining a response value vector of the force sensor to the hyperplane;
A second step of obtaining a theoretical value F ⁱ on the hyperplane by constraining a response value vector F ^d of the force sensor at the time of detecting an external force to the hyperplane using the inverse transformation matrix T ^inv ;
A third step for obtaining a deviation F ^f between the response value vector F ^d and the theoretical value F ⁱ ;
A fourth step of calculating, for each channel j, an inner product F ^f · q _j of an element q _{j of} Q represented by F ^f = QF ^d and a deviation F ^f ;
A fifth step of determining as failure a channel j in which the inner product F ^f · q _j exceeds a threshold;
A failure detection method for a force sensor of an external force detection interface, comprising: The failure detection method according to claim 2, wherein a correct response value is estimated from a response value vector of the force sensor including a response value of channel j determined to be a failure.
A sixth step of obtaining an estimated value F ^{c in} which the sensor response value of the channel j is replaced with the value of the element j of the theoretical value F ⁱ , and the other elements are left as elements of the response value vector F ^d ;
A seventh step of ^obtaining a theoretical value F ⁱ using the estimated value F ^c and the inverse transformation matrix T ^inv ;
An eighth step of substituting the value of the element j of the estimated value F ^c with the value of the element j of the theoretical value F ⁱ obtained in the seventh step;
A failure detection method for a force sensor of an external force detection interface, comprising: