JP5730614B2 - Torque sensor calibration device, calibration method, and program - Google Patents

Description

  The present invention relates to a torque sensor calibration device, a calibration method, and a program for calibrating a torque sensor mounted on an articulated robot.

  In general, since the torque sensor is affected by environmental changes such as temperature changes, calibration for adjusting the zero point of the torque sensor is required after being mounted on an articulated robot or the like. In particular, in a multi-joint robot having a complicated configuration such as a humanoid robot, a complicated operation may be required to take a fixed posture for calibration.

  On the other hand, for example, the torque of each joint when a predetermined load is applied in a predetermined posture is measured and stored as an initial value, and the initial value and the measured value in the predetermined posture are compared during calibration. A robot control apparatus that performs calibration is known (see Patent Document 1).

Japanese Patent Laid-Open No. 2000-225592 JP 2010-076074 A

  However, since the robot control device measures the torque of each joint in a predetermined posture in advance and calibrates in the predetermined posture even at the time of calibration, it is limited to the calibration in the predetermined posture.

  Further, when a torque sensor mounted on an articulated robot is calibrated, it is necessary to identify mechanical parameters such as the mass, the position of the center of gravity, and the inertia tensor of the articulated robot (see, for example, Patent Document 2). Furthermore, when these dynamic parameters are fluctuating, it is difficult to perform accurate calibration. Therefore, it is necessary to identify these dynamic parameters before calibrating the torque sensor. At this time, the torque value of each joint is required.For example, in the case of an articulated robot that does not have a torque sensor or an articulated robot that does not have an accurate sensor value before calibration even if it has a torque sensor, The torque value is converted using the current value of the actuator.

  In this case, the torque values used for conversion include errors in torque constants and mechanical errors that cannot be completely modeled (gear friction, etc.). Therefore, the mechanical parameters identified using these torque values are also errors. Will be included. Furthermore, identification of dynamic parameters is not easy and requires a certain amount of work time.

  The present invention has been made to solve such problems, and a torque sensor calibration device, a calibration method, and a calibration method that can easily and accurately calibrate a torque sensor mounted on an articulated robot. The main purpose is to provide a program.

  To achieve the above object, one aspect of the present invention is a torque sensor mounted on an articulated robot having a plurality of links and a plurality of joints rotatably connecting the links, and measuring the torque of the joints. A force sensor for measuring an external force acting on a contact point of the articulated robot, a torque of the joint measured by the torque sensor, the external force measured by the force sensor, and a base of the articulated robot A torque sensor calibration device comprising: an identification unit that simultaneously identifies a minimum dynamic parameter of the equation of motion and a torque offset value of the torque sensor based on an equation of motion relating to a link; is there. According to this aspect, calibration of a torque sensor mounted on an articulated robot can be performed with high accuracy and ease.

  In this one aspect, the identification means uses the following equation (7) which is an equation of motion related to the base link of the articulated robot, and uses the minimum dynamic parameter of the equation of motion, the torque offset value of the torque sensor, May be identified simultaneously. Thereby, since the minimum dynamic parameter and the offset value of the torque sensor can be identified at the same time, the calibration of the torque sensor can be performed with high accuracy and easily.

  In this one aspect, the identification means uses the following equation (6), which is a motion equation related to the base link of the articulated robot, and the minimum dynamic parameter of the motion equation, the torque offset value of the torque sensor, The force sensor force offset value may be identified simultaneously. Thereby, since the minimum dynamic parameter, the offset value of the torque sensor, and the offset value of the force sensor can be simultaneously identified, the calibration of the torque sensor and the force sensor can be performed more accurately and easily.

  In this aspect, the minimum dynamic parameter may be a function of a mass, a gravity center position, and an inertia tensor of each link of the articulated robot.

  In this aspect, the apparatus further includes a correction unit that corrects the torque of the torque sensor based on the torque of the joint measured by the torque sensor and the offset value of the torque sensor identified by the identification unit. It may be. Thus, the articulated robot can be controlled with high accuracy using the corrected torque.

  In this aspect, the correction unit may correct the external force of the force sensor based on the external force measured by the force sensor and the offset value of the force sensor identified by the identification unit. Good. Accordingly, the articulated robot can be controlled with high accuracy using the corrected external force.

  In this aspect, the identification unit may identify the minimum dynamic parameter of the equation of motion and the offset value of the torque of the torque sensor using, for example, a least square method.

  In this aspect, the control parameter may be corrected by the minimum dynamic parameter identified by the identification means.

  On the other hand, one aspect of the present invention for achieving the above object is a step of measuring the torque of the joint of an articulated robot having a plurality of links and a plurality of joints rotatably connecting the links; Based on the step of measuring the external force acting on the contact point of the articulated robot, the measured torque of the joint, the measured external force, and the equation of motion related to the base link of the articulated robot, The calibration method may include a step of simultaneously identifying a minimum dynamic parameter of the equation of motion and a torque offset value of the torque sensor.

  According to another aspect of the present invention for achieving the above object, the torque of the joint of an articulated robot having a plurality of links and a plurality of joints rotatably connecting the links, and the articulated robot A process of simultaneously identifying a minimum dynamic parameter of the equation of motion and an offset value of the torque of the torque sensor based on the external force acting on the contact point of the multi-joint robot and the equation of motion related to the base link of the articulated robot. The program may be executed by a computer.

  According to the present invention, it is possible to provide a torque sensor calibration device, a calibration method, and a program that can easily and accurately calibrate a torque sensor mounted on an articulated robot.

1 is a block diagram illustrating a schematic configuration of a torque sensor calibration device according to an embodiment of the present invention. It is a flowchart which shows an example of the processing flow of the torque sensor calibration apparatus which concerns on one Embodiment of this invention.

  Embodiments of the present invention will be described below with reference to the drawings. FIG. 1 is a block diagram showing a schematic configuration of a torque sensor calibration apparatus according to an embodiment of the present invention. A torque sensor calibration device 10 according to the present embodiment includes a torque sensor 1 and a force sensor 2 mounted on an articulated robot 100, and a calibration device 3 that calibrates the torque sensor 1 and the force sensor 2.

  The multi-joint robot 100 is, for example, a humanoid robot, and has a multi-joint link mechanism including a plurality of links and a plurality of joints that rotatably connect the links.

  The torque sensor 1 is mounted on each joint of the articulated robot 100 and measures the rotational torque of each joint. The torque sensor 1 is connected to the calibration device 3 and outputs the measured torque of each joint to the calibration device 3.

  Similarly, the force sensor 2 is mounted on the articulated robot 100 (for example, an ankle or a sole) and measures an external force acting on a contact point of the articulated robot 100. The force sensor 2 is connected to the calibration device 3 and outputs the measured external force to the calibration device 3.

  The calibration device 3 is identified by the identifying unit 31 and the identifying unit 31 that simultaneously identify the mechanical parameters of the articulated robot 100, the torque offset value of the torque sensor 1, and the external force offset value of the force sensor 2. And a correction unit 32 that corrects the measurement value of the torque sensor 1 and the measurement value of the force sensor 2 based on the offset value.

  The calibration device 3 temporarily stores, for example, a central processing unit (CPU) that performs arithmetic processing, a ROM (read only memory) that stores arithmetic programs executed by the CPU, processing data, and the like. A hardware configuration is provided with a microcomputer having a random access memory (RAM) as a center. The CPU, ROM, and RAM are connected to each other by a data bus or the like.

  The identification unit 31 is a specific example of identification means, and the external force acting on the torque of each joint of the articulated robot 100 measured by the torque sensor 1 and the contact point of the articulated robot 100 measured by the force sensor 2. And the minimum dynamic parameter of the equation of motion, the torque offset value of the torque sensor 1 and the offset value of the external force of the force sensor 2 are simultaneously identified based on the equation of motion relating to the base link of the articulated robot 100. To do.

  Here, a method for deriving the equation of motion of the articulated robot 100 used by the identification unit 31 will be described in detail.

The equation of motion related to the base link of the articulated robot 100 can be generally expressed by the following equation (1). In the following equation (1), M is an inertia matrix, B is a bias vector (including gravity, centrifugal force, Coriolis force, etc.), τ is a joint torque, and F is a force acting on an articulated robot (F = M A) and q represent the rotation angle of the joint.

In the above equation (1), the dynamic parameters such as the mass, the position of the center of gravity, and the inertia tensor of each link of the articulated robot 100 are included in the inertia matrix M and the bias vector B and are nonlinear equations. It can also be expressed as a linear form as shown in equation (2).

  However, the base link is a base and can be set to any link. For example, in a humanoid robot, the base link is often a trunk link. The dynamic parameter φ is generally redundant in describing the dynamic model and cannot be uniquely identified. Therefore, it is necessary to formulate the dynamic parameter φ into a minimum parameter (minimum dynamic parameter) necessary for calculation of the dynamic model.

Further, the minimum identifiable mechanical parameter φ _B ∈R ^NB depends on the structure of the dynamic model, and can be obtained by reducing and reconstructing redundant parameters in the equation of motion. The method for calculating this minimum dynamic parameter numerically and analytically are well known, for example, can be represented as following equation (3) using a minimum dynamic parameter phi _B.

Based on the above equation (3), the estimated value φ _B (hat) of the minimum dynamic parameter φ _B can be identified using, for example, the least square method. The method using the least square method and the method for calculating the minimum dynamic parameter are described in, for example, the paper “Identification of humanoid mechanical parameters using an internal sensor, venture, Serizawa, Nakamura, 14th Robotics Symposia 2009”. This is disclosed and can be used.

On the other hand, the torque value measured by the torque sensor 1 includes an offset value and can be expressed as the following equation (4). In the following equation (4), the measured value of the torque sensor 1 is τ _measured , the true value of the torque is τ, and the offset value of the torque sensor 1 is τ _offset .
τ _measured = τ + τ _offset (4)

Further, the measured value (external force) measured by the force sensor 2 also includes an offset value, which can be expressed as the following equation (5). In the following (5), the measurement value of the k-th force sensor 2 is F _{k_measured} , the true value is F _k , and the offset value of the force sensor 2 is F _{k_offset} .
F _{k_measured} = F _k + F _{k_offset} (5)

Furthermore, the following formula (6) can be obtained by substituting the formulas (4) and (5) into the formula (3). In the following equation (6), I is a unit matrix.

In the above equation (6), for example, by using the least square method, the minimum dynamic parameter φ _B (dynamic parameters such as link mass, center of gravity position, inertia tensor) is identified, and the offset value F _{k_offset} of the force sensor 2 and The offset value τ _offset of the torque sensor 1 can be identified at the same time.

When it is not necessary to identify the offset value F _{k_offset} of the force sensor 2, the minimum dynamic parameter φ is obtained by using the following equation (7) obtained by removing the portion corresponding to the offset value F _{k_offset} from the above equation (6). _B and the offset value τ _offset of the torque sensor 1 can be identified simultaneously.

The identification unit 31 measures the torque τ _measured of each joint of the articulated robot 100 measured by the torque sensor 1, the external force F _{k_measured} measured by the force sensor 2, and the base of the articulated robot 100 derived as described above. Based on the equation of motion (7) relating to the link, the minimum dynamic parameter φ _B of this equation of motion and the torque offset value τ _offset of the torque sensor 1 are simultaneously identified.

Similarly, the identification unit 31 uses the torque τ _measured of each joint of the articulated robot 100 measured by the torque sensor 1, the external force F _{k_measured} measured by the force sensor 2, and the articulated robot derived as described above. Based on the equation of motion (6) for 100 base links, the minimum dynamic parameter φ _B of this equation of motion, the torque offset value τ _offset of the torque sensor 1, and the offset value F _{k_offset} of the external force of the force sensor 2 Are simultaneously identified.

Thus, although the articulated robot 100 is operating in an arbitrary posture, the minimum dynamic parameters φ _B such as the mass of the unknown link, the position of the center of gravity, the inertia tensor, and the offset value τ _offset of the torque sensor 1 are calculated. Can be identified at the same time. Therefore, the minimum dynamic parameters can be identified with high accuracy without using a converted torque value including an error, and at the same time, the offset time τ _offset of the torque sensor 1 is identified, thereby greatly reducing the calibration work time. can do.

The identification unit 31 is connected to the correction unit 32, and outputs the identified offset value τ _offset of the torque sensor 1 and the offset value F _{k_offset} of the force sensor 2 to the correction unit 32.

The correction unit 32 is a specific example of the correction unit, and torque based on the offset value τ _offset of the torque sensor 1 from the identification unit 31, the measured value τ _measured of the torque sensor 1, and the above equation (4). A true value τ obtained by correcting the measurement value of the sensor 1 is calculated.

Similarly, the correction unit 32 determines the measurement value of the force sensor 2 based on the offset value F _{k_offset} of the force sensor 2 from the identification unit 31, the measurement value F _{k_measured} of the force sensor 2, and the above equation (5). A true value F _k corrected for is calculated.

For example, the correction unit 32 outputs the corrected true value τ of the torque sensor 1 and the true value F _k of the force sensor 2 to the control device 4 that controls the rotational drive of each joint of the articulated robot 100. (Fig. 1). Accordingly, the articulated robot 100 can be controlled with higher accuracy based on the true value τ and the true value F _{k obtained} by correcting the measurement values of the torque sensor 1 and the force sensor 2 in real time.

  Next, a calibration method by the torque sensor calibration device 10 according to the present embodiment will be described in detail. FIG. 2 is a flowchart illustrating an example of a processing flow of the torque sensor calibration device according to the present embodiment. Note that the processing shown in FIG. 2 is repeatedly executed at predetermined time intervals.

First, the torque sensor 1 measures the joint torque τ _measured of the articulated robot 100 in an arbitrary posture (step S101) and outputs the _measured torque to the calibration device 3. Similarly, the force sensor 2 measures the external force F _{k_measured} that acts on the articulated robot 100 in an arbitrary posture (step S102), and outputs it to the calibration device 3.

The identification unit 31 of the calibration device 3 includes the torque τ _measured of each joint of the multi-joint robot 100 measured by the torque sensor 1, the external force F _{k_measured} measured by the force sensor 2, and the base link of the multi-joint robot 100 described above. The minimum dynamic parameter φ _{B of the} equation of motion, the torque offset value τ _offset of the torque sensor 1 and the offset value F _{k_offset} of the external force of the force sensor 2 are simultaneously identified based on the equation of motion (6) Then, the offset value τ _{offset of the} identified torque sensor 1 and the offset value F _{k_offset} of the force sensor 2 are output to the correction unit 32.

The correcting unit 32 corrects the measured value of the torque sensor 1 based on the offset value τ _offset of the torque sensor 1 from the identifying unit 31, the measured value τ _measured of the torque sensor 1, and the above equation (4). The true value τ is calculated (step S104) and output to the control device 4. Further, the correction unit 32 calculates the measurement value of the force sensor 2 based on the offset value F _{k_offset} of the force sensor 2 from the identification unit 31, the measurement value F _{k_measured} of the force sensor 2, and the above equation (5). The corrected true value F _k is calculated (step S105) and output to the control device 4.

The control device 4, and the true value τ of the torque sensor 1 output from the correction unit 32, the true value F _k of the force sensor 2, based on, for controlling the actuator of the articulated robot 100 (step S106).

As described above, according to the torque sensor calibration apparatus 10 according to the present embodiment, the minimum mechanical parameters φ _B such as the mass, the position of the center of gravity, and the inertia tensor of the articulated robot 100 using the equation of motion related to the base link of the articulated robot 100. By simultaneously identifying the offset value τ _{offset of} the torque sensor 1 mounted on the articulated robot 100, the torque sensor 1 mounted on the articulated robot 100 can be calibrated with high accuracy and easily.

Note that the present invention is not limited to the above-described embodiment, and can be changed as appropriate without departing from the spirit of the present invention. For example, in the above embodiment, the identification unit 31 includes the minimum dynamic parameter φ _{B of the} equation of motion related to the base link of the articulated robot 100, the offset value τ _offset of the torque sensor 1, the offset value F _{k_offset} of the force sensor 2, However, the present invention is not limited to this. The minimum dynamic parameter φ _B , the offset value τ _offset of the torque sensor 1, and the offset value of any sensor that is mounted on the articulated robot 100 and is related to force or torque. And may be identified simultaneously.

  In the above-described embodiments, the present invention has been described as a hardware configuration, but the present invention is not limited to this. The present invention can also be realized by causing the CPU to execute a computer program for the processing (the processing shown in FIG. 2) executed by the identification unit 31 and the correction unit 32 of the calibration apparatus 3.

  The program may be stored using various types of non-transitory computer readable media and supplied to a computer. Non-transitory computer readable media include various types of tangible storage media. Examples of non-transitory computer-readable media include magnetic recording media (for example, flexible disks, magnetic tapes, hard disk drives), magneto-optical recording media (for example, magneto-optical disks), CD-ROMs (Read Only Memory), CD-Rs, CD-R / W and semiconductor memory (for example, mask ROM, PROM (Programmable ROM), EPROM (Erasable PROM), flash ROM, RAM (random access memory)) are included.

  The program may also be supplied to the computer by various types of transitory computer readable media. Examples of transitory computer readable media include electrical signals, optical signals, and electromagnetic waves. The temporary computer-readable medium can supply the program to the computer via a wired communication path such as an electric wire and an optical fiber, or a wireless communication path.

DESCRIPTION OF SYMBOLS 1 Torque sensor 2 Force sensor 3 Calibration apparatus 4 Control apparatus 10 Torque sensor calibration apparatus 31 Identification part 32 Correction part 100 Articulated robot

Claims (9)

A torque sensor mounted on an articulated robot having a plurality of links and a plurality of joints rotatably connecting the links, and measuring the torque of the joints;
A force sensor for measuring an external force acting on a contact point of the articulated robot;
Based on the torque of the joint measured by the torque sensor, the external force measured by the force sensor, and the equation of motion related to the base link of the articulated robot, the minimum dynamic parameters of the equation of motion, A torque sensor calibration device comprising: identification means for simultaneously identifying the torque offset value of the torque sensor. The torque sensor calibration device according to claim 1,
The identification means simultaneously identifies a minimum dynamic parameter of the equation of motion and a torque offset value of the torque sensor using the following equation, which is an equation of motion related to a base link of the articulated robot. Torque sensor calibration device.

In the above equation, φ _B is the minimum mechanical parameter, τ _measured is the torque of the joint measured by the torque sensor, τ _offset is the offset value of the torque sensor, and Y _B1 and Y _B2 are general values of the base link. F _{k_measured} is an external force acting on the contact point k measured by the force sensor, and K _k1 and K _k2 are at the contact point k. A matrix for converting the external force into a generalized force, _Nc represents the total number of contact points k between the articulated robot and the surrounding environment, and I represents a unit matrix. The torque sensor calibration device according to claim 1,
The identification means uses the following equation which is an equation of motion related to the base link of the articulated robot, and uses the following equation to determine the minimum dynamic parameter of the equation of motion, the torque offset value of the torque sensor, and the force offset value of the force sensor: And a torque sensor calibration device characterized by the above.

In the above equation, φ _B is the minimum dynamic parameter, τ _measured is the torque of the joint measured by the torque sensor, τ _offset is the offset value of the torque sensor, and Y _B1 and Y _B2 are generalizations of the base link An observation matrix obtained from coordinates, each joint angle and the velocity and acceleration of each joint angle, F _{k_measured} is the external force acting on the contact point k measured by the force sensor, and K _k1 and K _k2 are at the contact point k. Matrix for converting external force into generalized force, F _{1_offset to} F _{Nc_offset} is the offset value of the external force of the force sensor, N _c is the total number of contact points k between the articulated robot and the surrounding environment, and I is a unit matrix, Represents. The torque sensor calibration device according to any one of claims 1 to 3,
And a correction means for correcting the torque of the torque sensor based on the torque of the joint measured by the torque sensor and the offset value of the torque sensor identified by the identification means. Torque sensor calibration device. The torque sensor calibration device according to claim 4,
The correction means corrects the external force of the force sensor based on the external force measured by the force sensor and the offset value of the force sensor identified by the identification means. Sensor calibration device. The torque sensor calibration device according to any one of claims 1 to 5,
The torque sensor calibration apparatus characterized in that the identification means identifies a minimum dynamic parameter of the equation of motion and a torque offset value of the torque sensor using a least square method. The torque sensor calibration device according to any one of claims 1 to 6,
A torque sensor calibration apparatus, wherein a control parameter is corrected by the minimum dynamic parameter identified by the identification means. Measuring a torque of the joint of an articulated robot having a plurality of links and a plurality of joints rotatably connecting the links;
Measuring an external force acting on a contact point of the articulated robot;
Based on the measured joint torque, the measured external force, and the motion equation relating to the base link of the articulated robot, the minimum dynamic parameter of the motion equation, and the torque offset value of the torque sensor, , And the step of simultaneously identifying the calibration method.   A torque of the joint of an articulated robot having a plurality of links and a plurality of joints rotatably connecting the links; an external force acting on a contact point of the articulated robot; and a base link of the articulated robot A program that causes a computer to execute a process of simultaneously identifying a minimum dynamic parameter of the equation of motion and a torque offset value of the torque sensor based on the equation of motion.

Images