Calculation method of correction value

Abstract

To propose a method for calculating a calibration value for calibrating an error occurring in a gyroscope of an inertial sensor used for a slewing construction machine.SOLUTION: A method for calculating a calibration value includes: a slewing angle acquisition step S1 of acquiring a slewing angular velocity by using an inertial sensor provided on a slewing construction machine; a slewing speed calculation step S2 of calculating a slewing speed N from the slewing angular velocity; an error ratio calculation step S3 of calculating a ratio SF of an error in the slewing speed N; a calibration section selection step S4 of selecting a section of the slewing speed to be calibrated; a correction function definition step S5 of determining a correction function for calibrating the slewing speed N included in the section based on the ratio SF of the error; a correction coefficient calculation step S6 of calculating a correction coefficient by using the correction function; and a calibration value calculation step S7 of calculating a calibration value of the slewing speed N from the correction coefficient.SELECTED DRAWING: Figure 5

Description

Technical Field

[0001] The present invention relates to a method for calculating a calibration value used for calibrating a gyro of an inertial sensor used in construction machinery.

Background Art

[0002] In construction work using a swing-type construction machine, by automatically tracking two targets installed on the swing-type construction machine with two total stations, the swing direction of the swing-type construction machine is detected, and at the same time, the inclination angle is detected by inclinometers attached to the boom, arm, and head part of the swing-type construction machine, and construction management may be performed. On the other hand, in automatic tracking by a total station, it may be impossible to correctly sight the target when the swing-type construction machine swings. However, if a high-precision inertial sensor is provided in the swing part of the swing-type construction machine, by comparing with the measurement value by the inertial sensor, it is possible to detect a sighting error by the total station and automatically return the total station. An inertial sensor is a sensor composed of a three-axis gyro and a three-axis acceleration sensor, and is mainly used for inertial navigation and the like and for self-diagnosis of three-dimensional positions. In an inertial sensor, errors due to gyro drift accumulate. For such errors, for example, as shown in Patent Document 1 and Patent Document 2, a calibration method of the inertial sensor is disclosed. The calibration methods described in Patent Document 1 and Patent Document 2 are methods for setting parameters using least squares for optimizing temperature-dependent characteristics of zero-point bias and scale factor. Here, in a swing-type construction machine, since the swing part moves in both the plus side and the minus side, gyro errors also occur in plus and minus. Therefore, in the inertial sensor used for a swing-type construction machine, it cannot be calibrated uniformly.

Prior Art Documents

Patent Documents

[0003]

Patent Document 1

[0004] An object of the present invention is to propose a method for calculating a calibration value for calibrating an error occurring in a gyro of an inertial sensor used in a rotary construction machine. [Means for Solving the Problems]

[0005] The gyro calibration method of the present invention for solving the above problems includes a step of acquiring a turning angular velocity by an inertial sensor, a step of calculating a turning speed N from the turning angular velocity, a step of calculating a ratio SF of an error of the turning speed N, a step of detecting a section of the turning speed N to be calibrated, a step of obtaining an interpolation function in the section from the ratio SF of the error, a step of calculating a correction coefficient using the interpolation function, and a step of calculating a calibration value from the correction coefficient. According to such a gyro calibration method, since calibration can be performed after grasping the individual characteristics of the gyro, the gyro (inertial sensor) can be calibrated with high precision. Instead of calibrating uniformly, calibration is performed according to the turning speed, so it is possible to calibrate errors occurring on both the positive side and the negative side, such as in a construction machine.

[0006] It is desirable that the turning speed N is calculated by the ratio of the integrated value (turning angular velocity integrated value) θ of the turning angle ω of the inertial sensor and the turning time T using Equation 1. Further, when the inertial sensor is attached to a turning test table of a test device having an encoder, it is desirable that the ratio SF of the error is calculated from the turning angular velocity integrated value θ and the rotation angle (encoder rotation angle) θe of the turning test table detected by the encoder using Equation 2.

[0007]

Number

Advantages of the Invention

[0008] By using the method for calculating the calibration value of the present invention, it becomes possible to calibrate the errors of a gyroscope that cannot be uniformly calibrated. As a result, by accurately grasping the movement of the rotary construction machine via the gyroscope, it becomes possible to perform more accurate construction work.

Brief Description of the Drawings

[0009]

Figure 1

Figure 2

Figure 3

Figure 4

Figure 5

Figure 6

Figure 7

Figure 8

Figure 9

Figure 10

Modes for Carrying Out the Invention

[0010] In this embodiment, the case of constructing a mountain tunnel using the rotary construction machine 1 will be described. Fig. 1 shows the construction situation by the rotary construction machine 1. As shown in Fig. 1, the rotary construction machine 1 is used for removing (raking) the loose rocks remaining on the face immediately after excavation or the tunnel wall surface at the face (tunnel tip) after blasting. The rotary construction machine 1 of this embodiment is a so-called backhoe, and includes a traveling body 11, a slewing body 12 rotatably provided on the traveling body 11 about a vertical axis, a boom 13 rotatably attached to the slewing body 12 about a horizontal axis, an arm 14 rotatably attached to the tip of the boom 13 about a horizontal axis, and a head portion 15 rotatably attached to the tip of the arm 14 about a horizontal axis. The head portion 15 is a so-called attachment, and the head portion 15 of this embodiment is a hydraulic breaker equipped with a chisel 16.

[0011] The raking operation by the rotary construction machine 1 is performed using the excavation assistance system 2 while grasping the tip position of the chisel 16. If the tip position of the chisel 16 can be accurately grasped, the rotary construction machine 1 can be appropriately operated, so that efficient and accurate tunnel construction becomes possible. On the other hand, in order to grasp the tip position of the chisel 16, it is necessary to accurately grasp the position and orientation of the rotary construction machine 1. However, if there is a sighting error in the measurement of the rotary construction machine 1, the tip position of the chisel 16 cannot be accurately controlled. Therefore, the excavation assistance system 2 of this embodiment automatically compensates for the sighting error and controls the tip position of the chisel 16. Fig. 2 shows an overview of the excavation assistance system 2. As shown in Fig. 2, the excavation assistance system 2 of this embodiment includes two targets 3, two total stations 4, a three-axis acceleration sensor 5, an inertial sensor 6, and a computer 7.

[0012] As shown in FIG. 1, the target 3 is fixed to the rear part of the upper slewing body 12 of the rotary construction machine 1. In the present embodiment, two targets 3, 3 are arranged on the upper surface of the slewing body 12 with a space therebetween. The target 3 includes a reflecting prism, and reflects the light wave irradiated from the total station 4 toward the total station 4. In the present embodiment, a target prism that can be measured from all circumferential directions (360° directions centered on the target 3) is used.

[0013] As shown in FIG. 1, the total station 4 is installed behind the rotary construction machine 1 and automatically tracks the target 3. In the present embodiment, two total stations 4 are installed, and each total station 4 performs surveying of a different target 3. That is, one total station 4 automatically tracks one target 3, and the other total station 4 automatically tracks the other target 3. The total station 4 measures the position of the target 3 and transmits the measurement result to the computer 7 via communication means (not shown).

[0014] As shown in FIG. 1, the triaxial acceleration sensors 5 are respectively attached to the boom 13, the arm 14, and the head part 15. Each triaxial acceleration sensor 5 detects triaxial acceleration at the attachment position. The triaxial acceleration is the acceleration in the directions of three axes (X-axis, Y-axis, Z-axis) that are orthogonal to each other. The X-axis and the Y-axis are axes that are orthogonal to each other in the horizontal plane, and the Z-axis is an axis (vertical axis) perpendicular to the horizontal plane. The triaxial acceleration measured by the triaxial acceleration sensor 5 is transmitted to the computer 7 via communication means (not shown).

[0015] The inertial sensor 6 is provided on the revolving body 12. The inertial sensor 6 detects the three-axis acceleration and three-axis rotational angular velocity of the revolving body 12. The three-axis rotational angular velocity is the angular acceleration around three axes (X-axis, Y-axis, Z-axis) that are orthogonal to each other. The inertial sensor 6 includes a gyro (three-axis gyro) and an accelerometer (three-axis accelerometer). The gyro detects the three-axis rotational angular velocity of the revolving body 12. The accelerometer detects the three-axis acceleration at the mounting position. The measurement results of the inertial sensor 6 are transmitted to the computer 7.

[0016] The computer 7 includes a calculation means 71, a collimation error determination means 72, and a storage means 73. The calculation means 71 calculates the tip position of the nozzle 16 provided on the head portion 15 of the rotary construction machine 1. Further, the collimation error determination means 72 detects an error in the measurement by the total station 4. Furthermore, the storage means 73 stores the measurement results transmitted from the total station 4, the triaxial acceleration sensor 5, and the inertial sensor 6, and the calculation results of the calculation means 71 and the collimation error determination means 72, etc.

[0017] The calculation means 71 calculates the coordinates of each target 3 based on the measurement results transmitted from the total station 4, and calculates the orientation and inclination of the rotary construction machine 1 from the coordinates of both targets 3, 3. The total station 4 mainly obtains the three-dimensional coordinates of the rotary construction machine 1 in the field coordinate system, obtains the pitching angle and rolling angle of the revolving body 12 with the inertial sensor 6, and as a result, the three-dimensional coordinates, direction angle, and inclination of the rotary construction machine 1 are detected. In addition, the calculation means 71 calculates the pitching angle and rolling angle at the mounting position (boom 13, arm 14, or head portion 15) of the triaxial acceleration sensor 5 based on the data (triaxial acceleration of the boom 13, arm 14, and head portion 15) transmitted from the triaxial acceleration sensor 5. After calculating the pitching angle and rolling angle, the tip position of the nozzle 16 is calculated based on the calculation results (pitching angle and rolling angle at the mounting position of the triaxial acceleration sensor 5) and the measurement results of the total station 4. Furthermore, when a sighting error of one total station 4 is detected, the calculation means 71 of the present embodiment calculates the tip position of the target 16 using the measurement results by the other total station 4 and the measurement results by the inertial sensor 6. That is, the calculation means 71 complements the sighting error according to the following procedure. First, the yaw angle is calculated from the measurement coordinates of the target 3, and the difference from the yaw angle calculated based on the measurement results of the inertial sensor 6 is obtained. Next, the actual yaw angle is calculated using the yaw angle calculated from the measurement results of the inertial sensor 6 and the difference value. Then, the tip position of the target 16 is calculated using the measurement results of the total station 4 measured normally and the actual yaw angle.

[0018] The sighting error determination means 72 compares the actual positional relationship, which is the positional relationship between the targets 3 (the distance between the targets 3, the position of one target 3 relative to the other target 3, etc.) stored in advance, with the measured positional relationship, which is the positional relationship between the targets 3 obtained from the coordinates of the targets 3 measured by the total station 4. As a result of the comparison, if the distance between the targets in the actual positional relationship is different from the distance between the targets 3 in the measured positional relationship, the sighting error determination means 72 transmits a signal assuming that there is an error in the measurement by one or both of the total stations 4. The signal is transmitted to the operator, the workplace, etc. via the communication means. Note that the sighting error determination means 72 calculates the measured positional relationship using the yaw angle of the rotating body calculated from the three-axis acceleration and the three-axis rotational angular velocity measured by the inertial sensor 6. Thereby, the position of one target 3 relative to the other target 3 can be grasped. Therefore, when different targets 3 from the targets 3 to be measured by both total stations 4, 4 are being measured (when one total station 4 measures the other target 3 and the other total station 4 measures one target 3), and the interval between the targets 3 is a correct value, a sighting error by the total station 4 is detected.

[0019] Here, in the inertial sensor 6, an error occurs according to the turning angular velocity. FIG. 3 shows an example of the error ratio SF (the error ratio between the turning angle of the inertial sensor and the actual turning angle) of the inertial sensor 82. FIG. 3 shows the error ratio SF for four types of inertial sensors (A to D). As shown in FIG. 3, the error ratio SF does not increase or decrease linearly, but varies with the turning angular velocity. Also, the error occurring in the inertial sensor 6 has unique characteristics depending on the type of the inertial sensor 6 and also varies with the magnitude of the turning angular velocity. Therefore, the error of the inertial sensor 6 needs to be appropriately calibrated not uniformly but for each predetermined interval corresponding to the magnitude of the turning angular velocity of the swing-type construction machine 1. The calculation means 71 calibrates the gyro using a predefined gyro correction function f(x). The correction function f(x) is defined after grasping the gyro characteristics of the inertial sensor 6 using the test device 8. FIG. 4 shows an overview of the test device 8. As shown in FIG. 4, the test device 8 includes a turning table (turning test table) 81 on which the inertial sensor 6 is installed, a servo motor 82 that turns the turning table 81, and an encoder 83 that detects the rotation data (angle and movement amount) of the turning table 81 (servo motor 82). The test device 8 has a function of freely setting the rotation speed of the turning table 81. Then, the turning table 81 is turned at a constant speed, and the output data of the inertial sensor 6 is collected and analyzed.

[0020] FIG. 5 shows the procedure of the gyro calibration method. As shown in FIG. 5, the gyro calibration method includes a turning angle acquisition step S1, a turning speed calculation step S2, an error ratio calculation step S3, a calibration interval selection step S4, a correction function definition step S5, a correction coefficient calculation step S6, and a calibration value calculation step S7. The turning angle acquisition step S1 is a step of acquiring the turning angular velocity by the inertial sensor 6. The inertial sensor 6 is set on the test device 8, and data when the turning table 81 is turned at a low speed is collected. The turning speed is set to a magnitude assuming the turning speed of the swing-type construction machine 1.

[0021] The turning speed calculation step S2 is a step of calculating the turning speed N from the turning angular velocity. The turning speed N is calculated by dividing the integral value (turning angular velocity integral value) θ of the turning angular velocity ω acquired by the inertial sensor 6 at every time Δt within the turning time T by the turning time T (Equation 1).

[0022]

Equation

[0023] The error ratio calculation step S3 is a step of calculating the ratio SF of the error of the turning speed N. The error ratio SF is calculated from the encoder rotation angle θe acquired by the encoder 83 and the integral value (turning angular velocity integral value) θ of the turning angular velocity ω acquired by the inertial sensor 6 using Equation 2. When the error ratio SF < 0, it means that the inertial sensor measurement value is output larger than the actual turning speed, and when SF > 0, it means that the inertial sensor measurement value is output less than the actual turning speed. An example of the relationship between the turning speed and the error ratio SF is shown in FIG. 6.

[0024]

Equation

[0025] The calibration interval selection step S4 is a step of selecting the interval (selected interval) of the turning speed to be calibrated. First, a sensor correction value HSF (= 1 + SF) for correcting the measured value of the inertial sensor 6 is obtained from the error ratio SF obtained by the test device 8. The relationship between the turning speed and the sensor correction value HSF is shown in FIG. 7. Next, a selected interval for defining the correction function is selected.

[0026] The correction function definition step S5 is a step of obtaining a correction function f(x) for calibrating the turning speed N included in the selected interval based on the sensor correction value HSF (error ratio SF). By functionalizing the selected interval (that is, defining the correction function f(x) for each selected interval), continuity is ensured. As shown in FIG. 8, for example, the correction function f(x1) in the interval (selected interval) A from the measurement P1 to the measurement point P2 is defined. Similarly, the correction functions f(x2) to f(x6) in the interval B from the measurement point P2 to the measurement point P3, the interval C from the measurement point P3 to the measurement point P4, the interval D from the measurement point P4 to the measurement point P5, and the interval E from the measurement point P5 to the measurement point P6 are defined respectively.

[0027] The correction coefficient calculation step S6 is a step of calculating the correction coefficient k using the correction function f(x). The correction coefficient k is calculated by substituting the sensor output value ω to be calibrated into the correction function f(x). That is, it is detected which of the intervals A to E the output (angular velocity) from the inertial sensor 6 is included in, and the correction coefficient k is calculated using the correction function f(x) corresponding to that interval (see Equation 3). k = f(ω) ··· Equation 3

[0028] The calibrated value calculation step S7 is a step of calculating the calibrated value ω0 of the turning speed N from the correction coefficient k. As shown in Equation 4, the calibrated value ω0 is calculated by multiplying the correction coefficient k and the actually measured angular velocity (sensor output value ω). ω0 = k × ω ··· Equation 4

[0029] As described above, according to the construction method using the excavation assistance system 2 of the present embodiment, the sighting error of the total station 4 can be appropriately detected, and the construction error of the excavation work by the construction machine can be minimized. Since the sighting error of the total station 4 is automatically complemented by the excavation assistance system 2, the labor of the surveyor performing complicated calculations and operations can be omitted. At this time, after grasping the individual characteristics of the gyro, the gyro (inertial sensor 6) can be calibrated with high precision. The calibration of the gyro is not performed uniformly, but is calibrated using a correction coefficient k according to the turning speed, so that it is possible to calibrate errors occurring on both the plus side and the minus side, such as in a construction machine. Therefore, it is possible to improve the accuracy of the gyro. In the present embodiment, the correction function f(x) is defined in advance before the actual construction and is used for the calibration of the gyro during the actual construction.

[0030] Hereinafter, the turning operation is reproduced by the test device 8 using the test device 8, and the results of comparing the performance of the inertial sensor 6 before and after calibration are shown. The true turning angle is taken as the encoder rotation angle θe detected by the encoder 83. Fig. 9 shows the results of the reproduction test. In the turning during the working time of about 320 seconds, the final turning angle after calibration overlaps with the encoder rotation angle θe. On the other hand, as shown in Fig. 9, the final turning angle of the inertial sensor 6 before calibration is detected to be slightly larger than the encoder rotation angle θe. Fig. 10 shows the difference between the turning angles before and after calibration and the encoder rotation angle θe. As shown in Fig. 10, a large error occurred between the encoder rotation angle θe before calibration, but the error after calibration was small. Therefore, according to the inertial sensor calibration method of the present embodiment, it was confirmed that the inertial sensor 6 can be calibrated with high precision. In addition, the inertial sensor 6 has individual unique characteristics and needs to be configured by grasping the individual characteristics. However, by using the inertial sensor calibration method of the present embodiment, it can be calibrated relatively easily.

[0031] As described above, the embodiments of the present invention have been described. However, the present invention is not limited to the above-described embodiments, and each of the above-described components can be appropriately changed without departing from the spirit of the present invention. For example, in the above embodiment, the case where the swing-type construction machine 1 is an excavator has been described. However, the machine constituting the swing-type construction machine 1 is not limited as long as it has a swing body.

Explanation of Signs

[0032] 1 Swing-type construction machine 2 Excavation assistance system 3 Target 4 Total station 5 Triaxial acceleration sensor 6 Inertial sensor 7 Computer 71 Calculation means 72 Collimation error determination means 8 Test device 81 Swing table (swing test table) 82 Servo motor 83 Encoder S1 Swing angle acquisition step S2 Swing speed calculation step S3 Error ratio calculation step S4 Calibration section selection step S5 Correction function definition step S6 Correction coefficient calculation step S7 Calibration value calculation step

Claims

1. A method for calculating a calibration value used for calibrating a gyroscope, comprising: a step of obtaining a turning angular velocity by an inertial sensor; a step of calculating a turning speed N from the turning angular velocity; a step of calculating a ratio SF of an error of the turning speed N; a step of selecting an interval of the turning speed to be calibrated; a step of obtaining a correction function for calibrating the turning speed N included in the interval based on the ratio SF of the error; a step of calculating a correction coefficient using the correction function; a step of calculating a calibration value of the turning speed N from the correction coefficient, wherein the method for calculating a calibration value is characterized by comprising the above steps.

2. The method for calculating a calibration value according to claim 1, wherein the turning speed N is calculated by dividing an integral value θ of the turning angular velocity ω obtained by the inertial sensor every time interval Δt within a turning time T by the turning time T.

3. The inertial sensor is attached to a turning test table of a test device having an encoder, The method for calculating a calibration value according to claim 2, wherein the ratio SF of the error is calculated from a rotation angle θe of the turning test table obtained by the encoder and the integral value θ using Equation 1. 【Number 1】

Images