JP2011058820A - Method for correcting angular velocity sensor - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To provide a method for correcting an angular velocity sensor which enables correct attainment of a detection value of the angular velocity sensor by performing correction without requiring time and effort for performing calibration. <P>SOLUTION: In a series of motions of a link 3, the method includes a rotation angle detecting process wherein an angle sensor 6 detects a rotation angle of an actuator 4, an operation process of a reference angular velocity wherein an operation part 15 differentiates the rotation angle after the rotation angle detecting process and calculates the reference angular velocity, a detecting process of an angular velocity of a link wherein the angular velocity sensor 12 detects the angular velocity of the link in relation to a base body 1 in the same process as the rotation angle detecting process, and a correcting process of the angular velocity of the link wherein correction is performed so that the angular velocity of the link may approach the angular velocity of reference after the operation process of the angular velocity of reference. <P>COPYRIGHT: (C)2011,JPO&INPIT

Description

  The present invention relates to a correction method for an angular velocity sensor.

  2. Description of the Related Art Conventionally, a technique has been developed that uses an inertial sensor such as an acceleration sensor or an angular velocity sensor (for example, a rate gyro) to perform highly accurate positioning by suppressing vibration of a robot arm (arm) having a multi-joint structure.

For example, Patent Document 1 uses a low-pass filter that removes high-frequency components of the secondary mode or higher and passes the primary-mode frequency component and the DC component. That is, the acceleration signal in the secondary mode or higher is cut by the low-pass filter, and the arm is vibrated by suppressing the vibration in the secondary mode or higher and exciting the arm.
Further, in Patent Document 2, an inertia sensor is provided in the vicinity of a workpiece gripping portion (chuck portion) to detect the operation (swing state) of the chuck portion. A command signal is input to an inertial sensor and a control circuit connected to a driving body serving as a joint of each arm, thereby controlling the driving of each arm and controlling vibration of the chuck portion.

JP-A-1-173116 Japanese Patent Laid-Open No. 7-9374

  When an actuator (for example, a motor) that drives the arm is stopped while the arm is being driven, a slight vibration (fluctuation) occurs in the arm. As a method of suppressing this fluctuation, for example, there is a method of giving a drive signal that cancels the fluctuation of the arm to the control circuit of the actuator. In order to employ this method, the detection value of the inertial sensor is required to be accurate.

  However, in Patent Documents 1 and 2, since the vibration of the arm and the chuck part is controlled by a control means (for example, a low-pass filter or a control circuit) separate from the inertial sensor, the variation in the detection value of the inertial sensor itself is corrected. It is not possible. In general, since an inertial sensor has an output component (bias) irrelevant to an input component from an actuator and a scale factor having temperature characteristics, a zero point (reference point) at rest changes with time. In particular, when integrating the detection value of the inertial sensor to obtain the required value (for example, when integrating the detection value of the angular velocity sensor to obtain the angle, or integrating the detection value of the acceleration sensor to obtain the velocity or position), There is a problem that required values include errors due to bias and scale factors.

  Usually, when the inertial sensor is shipped from the factory, calibration is performed so that the reading (detected value) of the inertial sensor does not deviate from the reference point. For example, an angular velocity sensor is calibrated so that an error due to a bias and a scale factor is not included in a detection value of the angular velocity sensor at the time of shipment from a factory. However, the bias changes each time the angular velocity sensor is turned on / off, and the scale factor changes due to the influence of the ambient temperature in which the angular velocity sensor is used. For this reason, it is necessary to perform calibration every time the angular velocity sensor is turned on, which is troublesome.

  The present invention has been made in view of such circumstances, and a correction method for an angular velocity sensor that can accurately obtain a detection value of the angular velocity sensor by performing correction without taking the effort of calibration. The purpose is to provide.

  In order to solve the above-described problem, a method for correcting an angular velocity sensor according to the present invention includes a link that can rotate with respect to a base, an actuator that drives the link, an angle sensor that detects a rotation angle of the actuator, An angular velocity sensor correction method for a robot, comprising: an angular velocity sensor that detects an angular velocity of the link relative to a base; and a calculation unit that calculates a reference angular velocity based on the rotation angle, wherein the link starts to drive In a series of movements from stop to stop, a rotation angle detection step in which the angle sensor detects a rotation angle of the actuator, and after the rotation angle detection step, the calculation unit differentiates the rotation angle to obtain the reference angular velocity. The reference angular velocity calculation step for calculating the rotation angle and the rotation angle detection step are the same as the angular velocity sensor. A link angular velocity detecting step for detecting the angular velocity of the link with respect to the link, and a link angular velocity correcting step for correcting the angular velocity of the link so as to approach the reference angle after the reference angular velocity calculating step. Features.

  According to the method for correcting an angular velocity sensor of the present invention, during the driving of the link, the step of detecting the angular velocity of the link by the angular velocity sensor and the step of detecting the rotation angle of the actuator by the angle sensor are performed in the same step. After the step of calculating the reference angular velocity by differentiating the rotation angle of the actuator, that is, after the link angular velocity and the reference angular velocity are obtained, correction is performed so that the link angular velocity approaches the reference angular velocity. . That is, in the present invention, unlike the methods of controlling arm vibrations by control means separate from the inertial sensor as in Patent Documents 1 and 2, variations in the detected value of the angular velocity sensor itself are corrected during link driving. . Therefore, it is possible to accurately obtain the detection value of the angular velocity sensor by performing the correction without taking the trouble of performing calibration.

  In this correction method, the link angular velocity correction step includes a bias correction step of correcting a bias that is an output component unrelated to the input component from the actuator included in the link angular velocity, and the link angular velocity. And a scale factor correction step for correcting a scale factor having a temperature characteristic included in the link, and in the angular velocity correction step of the link, at least one of the bias correction step and the scale factor correction step is performed. Good.

  According to this correction method, at least one of a bias correction step and a scale factor correction step is performed in the link angular velocity correction step. For this reason, the random time fluctuation of the bias existing in the angular velocity sensor is reduced, and the bias stability is improved. Accordingly, when a necessary value (for example, an angle obtained by integrating the angular velocity of the link) is obtained from the detected value of the angular velocity sensor, it is possible to suppress the necessary value from including errors due to a bias or a scale factor. In addition, since the bias correction and the scale factor correction existing in the angular velocity sensor are corrected while the link is driven, it is not necessary to perform calibration every time the angular velocity sensor is turned on. Therefore, it is possible to accurately obtain the detection value of the angular velocity sensor by performing bias correction and scale factor correction without taking the effort of calibration.

  Further, in this correction method, a cycle in which three motion sections of an acceleration motion section, a constant speed motion section, and a deceleration motion section are included in one cycle for a series of motions from when the link starts driving to when it stops. The section may include at least one cycle.

  According to this correction method, the timing for performing correction can be appropriately selected from each motion section during link driving. For example, the correction may be performed by selecting at least one of the three motion sections of the acceleration motion section, the constant speed motion section, and the deceleration motion section, or may be performed in a cycle section of a predetermined cycle. it can. Therefore, it is possible to increase the degree of freedom of timing for performing correction.

  Further, in this correction method, a step of calculating a difference between the angular velocity of the link and the reference angular velocity in the cycle section having a predetermined number of cycles between the reference angular velocity calculation step and the bias correction step. You may have.

  According to this correction method, after the angular velocity of the link and the reference angular velocity are obtained, the difference between the angular velocity of the link and the reference angular velocity in a predetermined cycle section can be obtained. Therefore, the correction timing is shifted as necessary. be able to. For example, if the difference between the angular velocity of the link and the reference angular velocity is within a predetermined range, the correction is not performed. On the other hand, if the difference is outside the predetermined range, the correction is performed. it can. Therefore, the accuracy of the detection value of the angular velocity sensor can be selectively increased as appropriate.

  In this correction method, the bias correction step and the scale factor correction step may be performed after the link passes through the acceleration motion section.

  According to this correction method, the bias correction and the scale are based on the angular velocity of the link detected in the acceleration motion section in the cycle section of one cycle and the reference angular speed calculated by differentiating the rotation angle of the actuator. Factor correction is performed. Then, these corrections are reflected in the constant speed motion section and the deceleration motion section after the acceleration motion section. Therefore, the accuracy of the detection value of the angular velocity sensor can be made much more smoothly than when correction is performed all at once after the passage of one cycle or a plurality of cycles.

  In this correction method, the bias correction step and the scale factor correction step may be performed after the link has passed the constant velocity motion section.

  According to this correction method, bias correction is performed based on the angular velocity of the link detected in the constant velocity motion section in the cycle section of one cycle and the reference angular speed calculated by differentiating the rotation angle of the actuator. Scale factor correction is performed. Then, these corrections are reflected in the deceleration motion section after the constant speed motion section, and further in the subsequent acceleration motion section. Therefore, the accuracy of the detection value of the angular velocity sensor can be made much more smoothly than when correction is performed all at once after the passage of one cycle or a plurality of cycles.

  In this correction method, the bias correction step and the scale factor correction step may be performed after the link passes through the deceleration motion section.

  According to this correction method, the bias correction and the scale are performed based on the angular velocity of the link detected in the deceleration motion section in the cycle section of one cycle and the reference angular speed calculated by differentiating the rotation angle of the actuator. Factor correction is performed. Then, these corrections are reflected in the acceleration motion section and the constant speed motion section after the deceleration motion section. Therefore, the accuracy of the detection value of the angular velocity sensor can be made much more smoothly than when correction is performed all at once after the passage of one cycle or a plurality of cycles.

It is a figure which shows typically the robot provided with the angular velocity sensor which concerns on this invention. It is a flowchart figure of the correction method of the angular velocity sensor which concerns on this invention. It is a graph which shows the detected value of the angular velocity sensor which concerns on this invention, and a reference | standard angular velocity.

  Embodiments of the present invention will be described below with reference to the drawings. This embodiment shows one aspect of the present invention, and does not limit the present invention, and can be arbitrarily changed within the scope of the technical idea of the present invention. Moreover, in the following drawings, in order to make each structure easy to understand, an actual structure and a scale, a number, and the like in each structure are different.

  FIG. 1 is a schematic cross-sectional view of a robot 100 provided with angular velocity sensors 12 and 13 according to an embodiment of the present invention. As shown in FIG. 1, the robot 100 includes a gantry (base) 1, links 3 and 8, motors (actuators) 4 and 9, speed reducers 5 and 11, angle sensors 6 and 10, and an angular velocity sensor 12. , 13 and a calculation unit 15.

  One end of the link 3 is attached to the upper part of the gantry 1, and one end of the link 8 is attached to the other end of the link 3. The link 3 is rotatable relative to the gantry 1 about the shaft 2. The link 8 is rotatable relative to the link 3 about the shaft 7.

  The motor 4 drives the link 3 via the speed reducer 5. The motor 9 drives the link 8 via the speed reducer 11.

  The angle sensor 6 is provided on the top of the motor 4. The angle sensor 6 has a function of detecting the rotation angle Θ <b> 1 of the motor 4 around the axis 2. The angle sensor 10 is provided on the top of the motor 9. The angle sensor 10 has a function of detecting the rotation angle Θ <b> 2 of the motor 9 around the axis 7.

  The angular velocity sensor 12 is provided at the other end of the link 3. The angular velocity sensor 12 has a function of detecting an angular velocity g1 of the link 3 with respect to the gantry 1. The angular velocity sensor 13 is provided at the other end of the link 8. The angular velocity sensor 13 has a function of detecting the angular velocity g2 of the link 8 with respect to the link 3. The angular velocity g1 of the link 3 with respect to the gantry 1 is an angular velocity component in the rotation plane of the link 3. The angular velocity g2 of the link 8 with respect to the link 3 is an angular velocity component in the rotation plane of the link 8.

  The calculation unit 15 is provided in the vicinity of the gantry 1. The calculation unit 15 has a function of differentiating the rotation angles Θ1 and Θ2 of the motors 4 and 9 detected by the angle sensors 6 and 10 to calculate reference angular velocities r1 and r2 of the motors 4 and 9, respectively. The calculation unit 15 also has a function as a storage unit that stores the rotation angles Θ1 and Θ2 detected by the angle sensors 6 and 10 and the angular velocities g1 and g2 of the links 3 and 8 detected by the angular velocity sensors 12 and 13. Have.

  In this embodiment, two links, a motor, a speed reducer, an angle sensor, and an angular velocity sensor are provided, but the present invention is not limited to this. For example, one link, one motor, one speed reducer, one angle sensor, and one angular velocity sensor may be provided, or any number such as three or more may be provided.

  Moreover, although the angular velocity sensors 12 and 13 are arrange | positioned at the front-end | tip of the links 3 and 8, respectively, it is not restricted to this. For example, the angular velocity sensors 12 and 13 may be arranged at arbitrary positions on the links 3 and 8, respectively. Moreover, although the calculating part 15 is arrange | positioned in the vicinity of the mount frame 1, it is not restricted to this. For example, the calculation unit 15 may be arranged at an arbitrary position such as inside the angle sensors 6 and 10.

  The calculation unit 15 has a function of differentiating the rotation angles Θ1 and Θ2 to calculate reference angular velocities r1 and r2, and a function of storing the rotation angles Θ1 and Θ2 and the link angular velocities g1 and g2. However, it is not limited to this. For example, a storage unit that stores the rotation angles Θ <b> 1 and Θ <b> 2 and the link angular velocities g <b> 1 and g <b> 2 may be provided separately from the calculation unit 15.

(Correcting method of angular velocity sensor)
Next, a correction method for the angular velocity sensors 12 and 13 in the present embodiment will be described. FIG. 2 is a flowchart showing a process of correcting the detection value g of the angular velocity sensor (the angular velocity of the link detected by the angular velocity sensor) in a series of movements from when the link starts to the stop. Note that the storage processes in steps S2 and S5 and the calculation processes in steps S6 to S8 may be executed by the calculation unit 15 disposed in the vicinity of the gantry 1 of the robot 100, or a CPU provided outside the robot 100. (Not shown) may be executed.

  First, the angular velocity g1 of the link 3 with respect to the gantry 1 is detected by the angular velocity sensor 12. Further, the angular velocity g2 of the link 8 with respect to the link 3 is detected by the angular velocity sensor 13 (link angular velocity detecting step (S1)). The angular velocities g1 and g2 of the links 3 and 8 detected by the angular velocity sensors 12 and 13 are input and stored in the calculation unit 15 (S2).

  On the other hand, the rotation angle Θ1 of the motor 4 around the axis 2 is detected by the angle sensor 6 in the same step as the angular velocity detection step (S1) of the link. Further, the rotation angle Θ2 of the motor 9 around the shaft 7 is detected by the angle sensor 10 (rotation angle detection step (S3)). The rotation angles Θ 1 and Θ 2 of the motors 4 and 9 detected by the angle sensors 6 and 10 are input to the calculation unit 15. Then, the calculation unit 15 differentiates the rotation angles Θ1 and Θ2 of the motors 4 and 9 detected by the angle sensors 6 and 10 to calculate the reference angular velocities r1 and r2 (reference angular velocity calculation step (S4)). ). These reference angular velocities r1 and r2 are stored in the calculation unit 15 (S5).

  By the way, if the motor that drives the link via the speed reducer is stopped while the link is being driven, a slight vibration (fluctuation) occurs in the link. As a method of suppressing this fluctuation, for example, there is a method of giving a drive signal that cancels the fluctuation of the link to the motor control circuit. In order to employ this method, the detection value of the angular velocity sensor is required to be accurate.

  In general, an angular velocity sensor has an output component (bias) that is unrelated to the input component from the motor. For this reason, the zero point (reference point) at rest changes with time. The angular velocity sensor also has a scale factor having temperature characteristics. This scale factor varies under the influence of the temperature of the atmosphere in which the angular velocity sensor is used.

  Here, with respect to the detected value g of the angular velocity sensor (the angular velocities g1, g2 of the links detected by the angular velocity sensors 12, 13), the reference angular velocity r (the reference angular velocity r1, obtained by differentiating the rotation angles Θ1, Θ2). When expressed by the relationship between r2), the bias B, and the scale factor SF, the following equation (1) is established.

  g = SF · r + B (1)

  From the equation (1), it is confirmed that there is a difference between the detected value (angular velocity of the link) g of the angular velocity sensor and the reference angular velocity r due to the bias B and the scale factor SF. Hereinafter, a process for reducing the difference between the detected value g of the angular velocity sensor caused by the bias B and the scale factor SF and the reference angular velocity r will be described.

  After the steps S2 and S5, that is, after the angular velocities g1 and g2 and the reference angular velocities r1 and r2 are stored in the arithmetic unit 15, the arithmetic unit 15 detects the links 3 and 8 detected by the angular velocity sensors 12 and 13. Differences (g1-r1) and (g2-r2) between the angular velocities g1, g2 and the reference angular velocities r1, r2 are calculated (S6).

  Next, the calculation unit 15 causes the link angular velocities g1 and g2 and the reference angular velocities r1 and r2 in a predetermined cycle section (for example, one cycle section through the acceleration motion section L1, the constant speed motion section L2, and the deceleration motion section L3). It is calculated whether or not the differences (g1-r1) and (g2-r2) are within a predetermined range (within ± α) (S7). The value of α can be determined from an index representing the linear performance of the angular velocity sensors 12 and 13 (for example, α = 0.5).

  When the differences (g1-r1) and (g2-r2) between the angular velocities g1, g2 of the links and the reference angular velocities r1, r2 are within the predetermined range, the process returns to step S6. On the other hand, when the differences (g1-r1) and (g2-r2) between the angular velocities g1, g2 of the links and the reference angular velocities r1, r2 are not within the predetermined range, the process proceeds to the next step 7. That is, if the difference between the angular velocity of the link and the reference angular velocity is within a predetermined range, the correction is not performed. On the other hand, if the difference is outside the predetermined range, the correction is performed. it can.

  Next, the least square method is applied to the data plot P (see FIG. 3) of the angular velocities of the plurality of links detected by the angular velocity sensor by the calculation unit 15 (S8). Thereby, the data plot P of the angular velocities of the plurality of links is approximated (H1 shown in FIG. 3). Note that the method of approximating a plurality of data plots P detected by the angular velocity sensor is not limited to the least square method, and for example, a Kalman filter method can also be used.

  Next, the bias and the scale factor existing in the angular velocity of the link in the predetermined cycle section are estimated by the calculation unit 15 (S9).

  Then, the bias correction for correcting the difference between the first bias and the second bias to be small by the arithmetic unit 15 and the difference between the first scale factor and the second scale factor are small. At least one of the scale factor correction and the correction to be performed is performed (S10). Hereinafter, steps S9 and S10 will be described with reference to FIG.

  In general, a series of movements from the start of driving of the links 3 and 8 to the stop thereof includes an acceleration motion section from the start of driving of the links 3 and 8 to a constant speed, and the links 3 and 8. It includes three motion sections, a constant speed motion section that continues to drive at a constant speed, and a deceleration motion section until the links 3 and 8 decelerate and stop.

  FIG. 3 shows a difference between the angular velocity g1 of the link 3 detected by the angular velocity sensor 12 and the rotation angle Θ1 of the motor 4 detected by the angle sensor 6 in a series of movements from when the link 3 starts to stop. It is the graph which showed the reference | standard angular velocity r1 calculated | required by this. FIG. 3 is a graph showing time t on the horizontal axis and angular velocity g1 on the vertical axis.

  In FIG. 3, symbol L1 represents an acceleration motion section (time range 0 <t <t1), symbol L2 represents a constant velocity motion segment (time range t1 <t <t2), and symbol L3 represents a deceleration motion segment (time range t2 <t <t <t2). te). Reference symbol P is a plurality of data plots (angular velocity g1 of the link 3) detected by the angular velocity sensor 12. Reference numeral H1 is a graph showing the angular velocity g1 of the link 3 detected by the angular velocity sensor 12 in a series of movements from the start of driving of the link 3 to the stop thereof. On the other hand, symbol H2 is a graph showing a reference angular velocity r1 obtained by differentiating the rotation angle Θ1 of the motor 4 detected by the angle sensor 6.

  In the drawing, an example is shown in which one cycle is included in which the three exercise sections are defined as one cycle. Further, although the angular velocity g1 of the link 3 detected by the angular velocity sensor 12 is given as an example, the angular velocity g2 of the link 8 detected by the angular velocity sensor 13 can be similarly applied. The reference angular velocity r1 obtained by differentiating the rotation angle Θ1 of the motor 4 detected by the angle sensor 6 is taken as an example, but the rotation angle Θ2 of the motor 9 detected by the angle sensor 10 is differentiated. The same applies to the reference angular velocity r2 obtained in this way.

  As shown in FIG. 3, when the least square method is applied to a plurality of angular velocity data plots P in the acceleration motion section L1, the trajectory of the relationship between time and angular velocity becomes a straight line that does not pass through the origin. The angular velocity g11 of the link in the acceleration motion section L1 is expressed by the following equation (2), where the slope is a1 (positive integer) and the intercept is b1 (positive integer).

  g11 = a1 · t + b1 (2)

  Further, when the least square method is applied to a plurality of angular velocity data plots P in the constant velocity motion section L2, the trajectory of the relationship between time and angular velocity becomes a straight line that does not pass through the origin. The angular velocity g12 of the link in the constant velocity motion section L2 is expressed by the following equation (3), where the slope is a2 (positive integer) and the intercept is b2 (positive integer).

  g12 = a2 · t + b2 (3)

  Further, when the least square method is applied to a plurality of angular velocity data plots P in the deceleration motion section L3, the trajectory of the relationship between time and angular velocity is a straight line that does not pass through the origin. The angular velocity g13 of the link in the deceleration motion section L3 is expressed by the following equation (4), where the slope is a3 (negative integer) and the intercept is b3 (positive integer).

  g13 = a3 · t + b3 (4)

  From these equations (1) to (4), the bias B and the scale factor SF can be estimated. Specifically, from the relationship between Expression (1) and Expression (2), it is possible to estimate the slope a1 as the scale factor SF and the intercept b1 as the bias B in the acceleration motion section L1. Further, from the relationship between Expression (1) and Expression (3), it is possible to estimate the slope a2 as the scale factor SF and the intercept b2 as the bias B in the constant velocity motion section L2. Further, from the relationship between Expression (1) and Expression (4), it is possible to estimate the slope a3 as the scale factor SF and the intercept b3 as the bias B in the deceleration motion section L3.

  Therefore, the scale factor SF and the bias B existing in the angular velocity of the link in the predetermined cycle section are estimated by the equations (1) to (4) (S9).

  On the other hand, when looking at the reference angular velocity, in the acceleration motion section L1, the locus of the relationship between time and the reference angular velocity is a straight line going up to the right passing through the origin. The reference angular velocity r11 in the acceleration motion section L1 is expressed by the following equation (5), where the slope is c1 (positive integer) and the intercept is d1 (d1 = 0).

  r11 = c1 · t (5)

  In the constant velocity motion section L2, the locus of the relationship between time and the reference angular velocity is a straight line parallel to the horizontal axis. The reference angular velocity r12 in the constant velocity motion section L2 is expressed by the following equation (6), where the slope is c2 (c2 = 0) and the intercept is d2 (positive integer).

  r12 = d2 (6)

  In the deceleration motion section L3, the locus of the relationship between time and the reference angular velocity is a straight line that goes downward to the right and does not pass through the origin. The reference angular velocity r13 in the deceleration motion section L3 is expressed by the following equation (7), where the slope is c3 (negative integer) and the intercept is d3 (positive integer).

  r13 = c3 · t + d3 (7)

  From the relationship between the equations (2) and (5), the equation (2) is multiplied by the ratio (c1 / a1) of the inclinations a1 and c1 to replace the scale factor SF in the angular velocity of the link, and the reference angular velocity. The reference value c1 can be used. In addition, by multiplying the expression (2) by the ratio (d1 / b1 = 0) between the intercept b1 and d1 (d1 = 0), the reference value d1 at the reference angular velocity is substituted for the bias B at the angular velocity of the link. d1 = 0).

  Further, from the relationship between the expressions (3) and (6), the ratio (c2 / a2 = 0) of the inclination a2 and c2 (c2 = 0) is multiplied with respect to the expression (3). Instead of the scale factor SF, the reference value c2 (c2 = 0) at the reference angular velocity can be used. Further, by multiplying the expression (3) by the ratio (d2 / b2) between the intercepts b2 and d2, the reference value d2 at the reference angular velocity can be obtained instead of the bias B at the angular velocity of the link.

  In addition, from the relationship between the equations (4) and (7), the equation (4) is multiplied by the ratio (c3 / a3) of the inclinations a3 and c3 to replace the scale factor SF at the angular velocity of the link. The reference value c3 in the angular velocity can be set. Further, by multiplying the expression (4) by the ratio (d3 / b3) between the intercepts b3 and d3, the reference value d3 at the reference angular velocity can be obtained instead of the bias B at the angular velocity of the link.

  Therefore, the bias correction for correcting the difference between the bias B at the link angular velocity and the reference value at the reference angular velocity to be small, and the scale factor SF at the angular velocity and the reference angular velocity by the equations (2) to (7). The scale factor correction is performed so that the difference between the reference value and the reference value becomes small (S10). Through the above steps, the corrected bias and scale factor are rewritten to the data before correction in the calculation unit 15. And it returns to the angular velocity detection process of step S1, and the rotation angle detection process of step S3, the process of S1-S10 is repeated, and correction | amendment with bias B and scale factor SF is performed.

  According to the correction method of the angular velocity sensor of the present invention, the steps of the angular velocity sensors 12 and 13 detecting the angular velocities g1 and g2 of the links 3 and 8 during the driving of the links 3 and 8, and the angular sensors 6 and 10 are the rotation angle Θ1 of the motor. , Θ2 are performed in the same process. Then, after the step of calculating the reference angular velocities r1 and r2 by differentiating the motor rotation angles Θ1 and Θ2, that is, after the link angular velocities g1 and g2 and the reference angular velocities r1 and r2 are obtained, the link angular velocities are calculated. Correction is performed so that g1 and g2 approach the reference angular velocities r1 and r2. That is, in the present invention, unlike the methods of controlling the arm vibration by the control means separate from the inertial sensor as in Patent Documents 1 and 2, the detected values of the angular velocity sensors 12 and 13 themselves during the driving of the links 3 and 8. Variations in are corrected. Therefore, it is possible to accurately obtain the detection values of the angular velocity sensors 12 and 13 by performing the correction without taking the trouble of performing calibration.

  According to this correction method, at least one of a bias correction step and a scale factor correction step is performed in the link angular velocity correction step. For this reason, random time fluctuations of the bias B existing in the angular velocity sensors 12 and 13 are reduced, and the bias stability is improved. Accordingly, when a necessary value (for example, an angle obtained by integrating the angular velocity of the link) is obtained from the detection values of the angular velocity sensors 12 and 13, it is possible to prevent the necessary value from including an error due to the bias B or the scale factor SF. it can. Since the bias B existing in the angular velocity sensors 12 and 13 and the scale factor SF are corrected while the links 3 and 8 are driven, calibration is performed every time the angular velocity sensors 12 and 13 are turned on. There is no need. Therefore, the detection values of the angular velocity sensors 12 and 13 can be accurately obtained by performing the bias correction and the scale factor correction without taking time for calibration.

  Further, according to this correction method, the timing for correcting the angular velocity sensors 12 and 13 can be appropriately selected from the respective motion sections during the driving of the links 3 and 8. For example, correction may be performed by selecting at least one of the three motion sections of the acceleration motion section L1, the constant speed motion section L2, and the deceleration motion section L3, or the correction may be performed in a cycle section of a predetermined cycle. It can also be done. Therefore, the degree of freedom of timing for correcting the angular velocity sensors 12 and 13 can be increased.

  Further, according to this correction method, after the angular velocities g1 and g2 of the links 3 and 8 and the reference angular velocities r1 and r2 are obtained, the difference between the angular velocities g1 and g2 and the reference angular velocities r1 and r2 in a predetermined cycle section. Since (g1-r1) and (g2-r2) are obtained, the timing for correcting the angular velocity sensors 12, 13 can be shifted as necessary. For example, if the differences (g1-r1) and (g2-r2) between the angular velocities g1 and g2 and the reference angular velocities r1 and r2 are within a predetermined range, no correction is performed. If it is outside the predetermined range, correction can be performed. Therefore, the accuracy of the detection values of the angular velocity sensors 12 and 13 can be selectively increased as appropriate.

  Note that, according to the correction method of the angular velocity sensor of the present embodiment, the motion in the acceleration motion section L1, the constant speed motion section L2, and the deceleration motion section L3 in a series of motions from when the link 3 starts driving to when it stops. Although an example in which a cycle section with one section is included has been described, the present invention is not limited to this. For example, a plurality of cycle sections in which these three motion sections are one cycle may be included.

  In this correction method, the bias correction and the scale factor correction may be performed after the links 3 and 8 have passed the acceleration motion section L1. Thereby, the angular velocities g1 and g2 of the link detected in the acceleration motion section L1 in the cycle section of one cycle and the reference angular velocities r1 and r2 calculated by differentiating the rotation angles Θ1 and Θ2 of the motor are obtained. Based on this, bias correction and scale factor correction are performed. Then, this correction is reflected in the constant velocity motion section L2 and the deceleration motion section L3 after the acceleration motion section L1.

  In this correction method, the bias correction and the scale factor correction may be performed after the links 3 and 8 have passed the constant velocity motion section L2. Thereby, the angular velocities g1 and g2 of the links detected in the constant velocity motion section L2 in the cycle section of one cycle, and the reference angular velocities r1 and r2 calculated by differentiating the rotation angles Θ1 and Θ2 of the motor, Based on this, bias correction and scale factor correction are performed. Then, this correction is reflected in the deceleration motion section L3 after the constant speed motion section L2, and further in the subsequent acceleration motion section L1.

  In this correction method, the bias correction and the scale factor correction may be performed after the links 3 and 8 have passed through the deceleration motion section L3. Thus, based on the angular velocities g1 and g2 detected in the deceleration motion section L3 in the cycle section of one cycle and the reference angular velocities r1 and r2 calculated by differentiating the rotation angles Θ1 and Θ2 of the motor. Bias correction and scale factor correction are performed. Then, these corrections are reflected in the acceleration motion section L1 and the constant speed motion section L2 after the deceleration motion section L3.

  As described above, by correcting the bias correction and the scale factor correction after each motion section has passed, the angular velocity sensors 12, 13 can be corrected rather than being collectively corrected after one cycle or a plurality of cycles. The accuracy of the detected value can be improved remarkably smoothly.

DESCRIPTION OF SYMBOLS 1 ... Base (base | substrate), 3,8 ... Link, 4,9 ... Motor (actuator), 6,10 ... Angle sensor, 12, 13 ... Angular velocity sensor, 100 ... Robot, g ... Angular velocity, r ... Standard angular velocity, B: bias, L1: acceleration motion section, L2: constant speed motion section, L3: deceleration motion section, SF: scale factor, Θ1, Θ2: rotation angle

Claims (7)

Based on the rotation angle, a link capable of rotating relative to the base, an actuator that drives the link, an angle sensor that detects a rotation angle of the actuator, an angular velocity sensor that detects an angular speed of the link relative to the base, and A calculation unit for calculating a reference angular velocity, and a correction method for an angular velocity sensor in a robot having:
In a series of movements from when the link starts driving to when it stops,
A rotation angle detecting step in which the angle sensor detects a rotation angle of the actuator;
A reference angular velocity calculation step in which the calculation unit differentiates the rotation angle and calculates the reference angular velocity after the rotation angle detection step;
In the same step as the rotation angle detection step, the angular velocity detection step of the link in which the angular velocity sensor detects the angular velocity of the link with respect to the base body,
An angular velocity sensor correction method comprising: a link angular velocity correction step of correcting the link so that the angular velocity of the link approaches the reference angle after the reference angular velocity calculation step. The link angular velocity correction step includes a bias correction step of correcting a bias which is an output component irrelevant to an input component from the actuator included in the angular velocity of the link, and a scale having a temperature characteristic included in the angular velocity of the link. A scale factor correction step for correcting the factor,
The angular velocity sensor correction method according to claim 1, wherein at least one of the bias correction step and the scale factor correction step is performed in the link angular velocity correction step.   The series of movements from the start of driving to the stop of the link includes at least one cycle section in which three motion sections of an acceleration motion section, a constant speed motion section, and a deceleration motion section are defined as one cycle. The method of correcting an angular velocity sensor according to claim 1, wherein the angular velocity sensor is corrected.   Between the reference angular velocity calculation step and the angular velocity correction step of the link, there is a step of calculating a difference between the angular velocity of the link and the reference angular velocity in the cycle section having a predetermined number of cycles. The method of correcting an angular velocity sensor according to claim 3.   5. The method of correcting an angular velocity sensor according to claim 3, wherein the bias correction step and the scale factor correction step are performed after the link passes the acceleration motion section. 6.   6. The method of correcting an angular velocity sensor according to claim 3, wherein the bias correction step and the scale factor correction step are performed after the link has passed through the constant velocity motion section. .   The method of correcting an angular velocity sensor according to claim 3, wherein the bias correction step and the scale factor correction step are performed after the link has passed through the deceleration motion section.

Images