JP2023101310A - Method for calculating calibration value - Google Patents

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

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

In construction work using a swinging construction machine, two targets installed on the swinging construction machine are automatically tracked by two total stations to detect the turning direction of the swinging construction machine and In some cases, construction management is performed by detecting the tilt angle with an inclinometer attached to the boom, arm, or head of the machine.
On the other hand, in automatic tracking by a total station, the target may not be collimated correctly when the swing-type construction machine turns.
However, if a high-precision inertial sensor is installed in the swing section of a swing-type construction machine, collimation errors by the total station can be detected and the total station can be automatically reset by comparing the measured values with the inertial sensor.
The inertial sensor is a sensor composed of a 3-axis gyro and a 3-axis acceleration sensor, and is mainly used for inertial navigation and the like for self-diagnosis of a three-dimensional position.
Inertial sensors accumulate errors due to gyro drift. To cope with such errors, methods for calibrating inertial sensors are disclosed, for example, as shown in Patent Document 1 and Patent Document 2. The calibration methods described in Patent Literature 1 and Patent Literature 2 are parameter setting methods using least squares for optimizing the temperature dependence characteristics of the zero-point bias and scale factor.
Here, in a swing-type construction machine, the swing part moves in both the plus side and the minus side, so that gyro errors occur in both the plus and minus directions. Therefore, it has been impossible to uniformly calibrate the inertial sensors used in swing-type construction machines.

Japanese Patent Application Laid-Open No. 2011-209000 Japanese Unexamined Patent Application Publication No. 2011-209001

SUMMARY OF THE INVENTION An object of the present invention is to propose a calibration value calculation method for calibrating an error occurring in a gyro of an inertial sensor used in a swing-type construction machine.

A gyro calibration method according to the present invention for solving the above-mentioned problems comprises the steps of acquiring a turning angular velocity by an inertial sensor, calculating a turning velocity N from the turning angular velocity, and calculating an error ratio SF of the turning velocity N. a step of detecting an interval of the turning speed N to be calibrated; a step of obtaining an interpolation function in the interval from the error ratio SF; a step of calculating a correction coefficient using the interpolation function; and calculating a calibration value from the coefficients.
According to such a gyro calibration method, the gyro (inertial sensor) can be calibrated with high accuracy because the gyro can be calibrated after grasping the individual characteristics of the gyro. Since calibration is not performed uniformly, but according to the turning speed, it is possible to calibrate for errors that occur on both the plus side and the minus side like construction machinery.

The turning speed N is desirably calculated from the ratio between the integral value of the turning angle ω of the inertial sensor (turning angular speed integrated value) θ 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, the error rate SF is calculated by using the equation 2 and the turning angular velocity integrated value θ and the encoder. It is desirable to calculate from the detected rotation angle (encoder rotation angle) θe of the turning test table.

By using the calibration value calculation method of the present invention, it becomes possible to calibrate gyro errors that cannot be uniformly calibrated, and by extension, it is possible to accurately grasp the movement of the swing-type construction machine through the gyro. This makes it possible to carry out more accurate construction.

It is a schematic diagram showing a construction situation by the turning type construction machine of the present embodiment. 1 is a block diagram showing an excavation assistance system of this embodiment; FIG. 4 is a graph showing the rate of error of an inertial sensor with respect to turning angle; 1 is a schematic diagram of a test apparatus used in Examples. FIG. 4 is a flow chart showing the procedure of a gyro calibration method; 7 is a graph showing an example of a relationship between a turning angle and an error rate; 7 is a graph showing an example of a relationship between a turning angle and a sensor correction value; 7 is a graph showing an example of a correction function for each section; It is a graph which shows the reproduction experiment result. It is a graph which shows the relationship between the time of a reproduction experiment result, and the error of a turning angle.

In this embodiment, a case of constructing a mountain tunnel using the swing-type construction machine 1 will be described. FIG. 1 shows a construction situation by a swing-type construction machine 1. As shown in FIG. As shown in FIG. 1, the slewing construction machine 1 is used to remove loose stones remaining on the face (tunnel tip) after blasting and on the face and tunnel walls immediately after excavation.
The slewing construction machine 1 of this embodiment is a so-called backhoe, and includes a traveling body 11, a revolving body 12 provided on the traveling body 11 so as to be rotatable about a vertical axis, and a horizontal axis on the revolving body 12. A boom 13 rotatably attached to the center, an arm 14 attached to the tip of the boom 13 rotatably about the horizontal axis, and a tip of the arm 14 rotatably about the horizontal axis. and a head portion 15 attached thereto. The head portion 15 is a so-called attachment, and the head portion 15 of this embodiment is a hydraulic breaker provided with a chisel 16 .

The excavation assistance system 2 is used to carry out the excavation work with the revolving construction machine 1 while grasping the tip position of the chisel 16 . If the position of the tip of the chisel 16 can be accurately grasped, the swing type construction machine 1 can be operated appropriately, which enables efficient and highly accurate tunnel construction. 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 swing construction machine 1. Inability to precisely control position. Therefore, the auxiliary excavation system 2 of this embodiment automatically compensates for the collimation error and controls the tip position of the chisel 16 .
FIG. 2 shows an outline of the auxiliary excavation system 2. As shown in FIG. As shown in FIG. 2, the auxiliary excavation 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. .

The target 3 is fixed to the rear part of the revolving body 12 of the revolving construction machine 1, as shown in FIG. In this embodiment, two targets 3, 3 are arranged on the upper surface of the revolving body 12 with a gap therebetween. The target 3 has a reflecting prism and reflects the light wave emitted from the total station 4 toward the total station 4 . In this embodiment, a target prism that can be measured from all circumferential directions (360° directions centering on the target 3) is used.

The total station 4, as shown in FIG. 1, is installed behind the rotary construction machine 1 and automatically tracks the target 3. As shown in FIG. In this embodiment, two total stations 4 are installed, and each total station 4 surveys a different target 3 . That is, one target 3 is automatically tracked by one total station 4 and the other target 3 is automatically tracked by the other total station 4 .
The total station 4 measures the position of the target 3 and transmits the measurement results to the computer 7 via communication means (not shown).

The three-axis acceleration sensor 5 is attached to each of the boom 13, the arm 14, and the head portion 15, as shown in FIG. Each triaxial acceleration sensor 5 detects triaxial acceleration at the mounting position. Triaxial acceleration is acceleration in directions of three axes (X-axis, Y-axis, and Z-axis) perpendicular to each other. The X-axis and the Y-axis are axes 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).

The inertial sensor 6 is provided on the revolving body 12 . The inertial sensor 6 detects triaxial acceleration and triaxial rotational angular velocity of the revolving body 12 . The triaxial rotational angular velocity is angular acceleration around three mutually orthogonal axes (X-axis, Y-axis, and Z-axis). The inertial sensor 6 includes a gyro (three-axis gyro) and an accelerometer (three-axis accelerometer). The gyro detects triaxial rotational angular velocities of the revolving body 12 . The accelerometer senses three-axis acceleration at the mounting position. A measurement result of the inertial sensor 6 is transmitted to the computer 7 .

The computer 7 comprises 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 chisel 16 provided on the head portion 15 of the swing-type construction machine 1 . Also, the collimation error determination means 72 detects an error in measurement by the total station 4 . Further, the storage means 73 stores the measurement results transmitted from the total station 4, the three-axis acceleration sensor 5, and the inertia sensor 6, the calculation results of the calculation means 71 and the collimation error determination means 72, and the like.

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 swing construction machine 1 from the coordinates of both targets 3,3. The total station 4 mainly obtains the three-dimensional coordinates of the swing construction machine 1 in the site coordinate system, obtains the pitching angle and rolling angle of the swing body 12 with the inertial sensor 6, and as a result obtains the three-dimensional coordinates of the swing construction machine 1. and direction angle and tilt are detected.
Further, the calculation means 71 calculates the mounting position of the triaxial acceleration sensor 5 (boom 13, arm 14 or The pitching angle and rolling angle of the head portion 15) are calculated. After calculating the pitching angle and rolling angle, the tip position of the chisel 16 is calculated based on the calculation result (the pitching angle and rolling angle at the mounting position of the triaxial acceleration sensor 5) and the measurement result of the total station 4. FIG.
Further, when the collimation error of one total station 4 is detected, the calculation means 71 of the present embodiment uses the measurement result of the other total station 4 and the measurement result of the inertial sensor 6 to Calculate the tip position. That is, the calculation means 71 compensates for the collimation error by the following procedure. First, the yaw angle is calculated from the measured coordinates of the target 3, and the difference from the yaw angle calculated based on the measurement result of the inertial sensor 6 is obtained. Next, using the yaw angle calculated from the measurement result of the inertial sensor 6 and the difference value, the actual yaw angle is calculated. Then, the tip position of the chisel 16 is calculated using the measurement result of the total station 4 which is normally measured and the actual yaw angle.

The collimation error determination means 72 compares the actual positional relationship, which is a prestored positional relationship between the targets 3 (the distance between the targets 3, the position of one target 3 with respect to the other target 3, etc.), and the actual positional relationship measured by the total station 4. The measured positional relationship, which is the positional relationship between the targets 3 determined from the coordinates of the targets 3 thus obtained, is compared. 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 collimation error determination means 72 determines that either one or both of the total stations 4 made an error in the measurement. Send a signal if there is. The signal is transmitted to an operator, work place, etc. via communication means.
The collimation error determination means 72 uses the yaw angle of the revolving body calculated from the three-axis acceleration and the three-axis rotation angular velocity measured by the inertial sensor 6 to calculate the measured positional relationship. Thereby, the position of the other target 3 with respect to the one target 3 can be grasped. Therefore, when both total stations 4, 4 are measuring a target 3 different from the target 3 to be measured (one total station 4 is measuring the other target 3, and the other total station 4 is measuring one target 3). collimation error by the total station 4 is detected when the distance between the targets 3 is a correct value.

Here, in the inertial sensor 6, an error occurs according to the turning angular velocity. FIG. 3 shows an example of the rate of error of the inertial sensor 82 (the rate of error between the turning angle of the inertial sensor and the actual turning angle) SF. FIG. 3 shows error ratios SF for four types of inertial sensors (A to D). As shown in FIG. 3, the error ratio SF does not increase or decrease constantly, but changes with the turning angular velocity. Further, the error generated in the inertial sensor 6 has unique characteristics depending on the type of the inertial sensor 6, and changes depending on the magnitude of the turning angular velocity. Therefore, the error of the inertial sensor 6 should not be calibrated uniformly, but should be appropriately calibrated for each predetermined section corresponding to the magnitude of the swing angular velocity of the swing type construction machine 1 .
The calculation means 71 performs gyro calibration using a predefined gyro correction function f(x). The correction function f(x) is defined after grasping the gyroscopic characteristics of the inertial sensor 6 using the test device 8 . FIG. 4 shows an outline of the test device 8. As shown in FIG. As shown in FIG. 4, the test apparatus 8 includes a turning table (turning test table) 81 on which the inertia sensor 6 is installed, a servomotor 82 for turning the turning table 81, and a rotating table 81 (servomotor 82). It is composed of an encoder 83 for detecting rotation data (angle and movement amount). The test device 8 has a function of freely setting the rotation speed of the swivel 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.

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 section selection step S4, a correction function definition step S5, and a correction coefficient calculation step. It includes a step S6 and a calibration value calculation step S7.
The turning angle acquisition step S<b>1 is a step of acquiring a turning angular velocity by the inertial sensor 6 . The inertia sensor 6 is set in the test device 8 to collect data when the turning table 81 is turned at low speed. The slewing speed is assumed to be the slewing speed of the slewing construction machine 1 .

The turning speed calculation step S2 is a step of calculating the turning speed N from the turning angular speed. The turning speed N is calculated by dividing the integral value (turning angular speed integrated value) θ of the turning angular speed ω obtained by the inertial sensor 6 at each time Δt within the turning time T by the turning time T (Equation 1). .

The error ratio calculation step S3 is a step of calculating an error ratio SF of the turning speed N. FIG. The error ratio SF is calculated using Equation 2 from the encoder rotation angle θe obtained by the encoder 83 and the integrated value of the turning angular velocity ω (turning angular velocity integrated value) θ obtained by the inertial sensor 6 .
If the error rate SF < 0, it means that the inertial sensor measurement value is output larger than the actual turning speed. It means less output. FIG. 6 shows an example of the relationship between the turning speed and the error rate SF.

The calibration section selection step S4 is a step of selecting a turning speed section (selection section) to be calibrated. First, a sensor correction value HSF (=1+SF) for correcting the actual measurement value of the inertial sensor 6 is obtained from the error rate SF obtained by the test device 8 . FIG. 7 shows the relationship between the turning speed and the sensor correction value HSF. Next, a selection interval that defines a correction function is selected.

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 section based on the sensor correction value HSF (error ratio SF). Continuity is ensured by converting the selected section into a function (that is, defining a correction function f(x) for each selected section). As shown in FIG. 8, for example, a correction function f(x1) is defined in a section (selected section) A from measurement P1 to measurement point P2. Similarly, section B from measuring point P2 to measuring point P3, section C from measuring point P3 to measuring point P4, section D from measuring point P4 to measuring point P5, section E from measuring point P5 to measuring point P6 Define the correction functions f(x2) to f(x6) in .

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 detects which of the sections A to E the output (angular velocity) from the inertial sensor 6 is included in, and calculates the correction coefficient k using the correction function f(x) corresponding to that section (equation 3 reference).
k=f(ω) Equation 3

The calibration value calculation step S7 is a step of calculating a calibration value ω0 of the turning speed N from the correction coefficient k.
The calibration value ω0 is calculated by multiplying the correction coefficient k and the measured angular velocity (sensor output value ω) as shown in Equation 4.
ω0=k×ω Expression 4

As described above, according to the construction method using the auxiliary excavation system 2 of the present embodiment, collimation errors of the total station 4 can be appropriately detected, and construction errors in excavation work by the construction machine can be minimized. Compensation for the collimation error of the total station 4 is automatically executed by the auxiliary excavation system 2, which saves the operator the trouble of performing complicated calculations and operations. At this time, the gyro (inertial sensor 6) can be calibrated with high precision after grasping the individual characteristics of the gyro. The gyro is not calibrated uniformly, but is calibrated using the correction coefficient k according to the turning speed, so it is possible to calibrate for errors that occur on both the positive and negative sides like construction machinery. is possible. Therefore, it is possible to improve the accuracy of the gyro. Note that in the present embodiment, the correction function f(x) is defined in advance before the actual construction, and is used for calibrating the gyro during the actual construction.

The results of comparing the performance of the inertial sensor 6 before and after calibration by reproducing the turning motion with the test device 8 are shown below. The true turning angle is the encoder rotation angle θe detected by the encoder 83 .
FIG. 9 shows the results of the reproduction test. During the turning with a working time of about 320 seconds, the final turning angle after calibration overlaps with the encoder rotation angle θe. On the other hand, the final swing angle of the inertial sensor 6 before calibration is detected to be slightly larger than the encoder rotation angle θe, as shown in FIG. FIG. 10 shows the difference between the turning angle and the encoder rotation angle θe before and after calibration. As shown in FIG. 10, there was a large error in the encoder rotation angle θe before calibration, but the error after calibration was small.
Therefore, it was confirmed that the inertial sensor 6 can be calibrated with high accuracy according to the inertial sensor calibration method of the present embodiment. In addition, each inertial sensor 6 has its own unique characteristics, and it is necessary to comprehend each characteristic and configure it. However, by using the inertial sensor calibration method of the present embodiment, calibration can be performed relatively easily.

Although the embodiments of the present invention have been described above, the present invention is not limited to the above-described embodiments, and each of the constituent elements described above can be modified as appropriate without departing from the scope of the present invention.
For example, in the above embodiment, the case where the swing type construction machine 1 is a backhoe has been described, but the machine that constitutes the swing type construction machine 1 is not limited as long as it has a swing structure.

REFERENCE SIGNS LIST 1 swing-type construction machine 2 excavation auxiliary system 3 target 4 total station 5 triaxial acceleration sensor 6 inertia sensor 7 computer 71 calculation means 72 collimation error determination means 8 test apparatus 81 swivel table (swivel test table)
82 Servo motor 83 Encoder S1 Turning angle acquisition process S2 Turning speed calculation process S3 Error ratio calculation process S4 Calibration section selection process S5 Correction function definition process S6 Correction coefficient calculation process S7 Calibration value calculation process

Claims (3)

A method for calculating a calibration value used for calibrating a gyro,
obtaining a turning angular velocity with an inertial sensor;
a step of calculating a turning speed N from the turning angular speed;
a step of calculating an error ratio SF of the turning speed N;
a step of selecting a turning speed section to be calibrated;
obtaining a correction function for calibrating the turning speed N included in the section based on the error ratio SF;
calculating a correction factor using the correction function;
and calculating a calibration value of the turning speed N from the correction coefficient. The turning speed N is calculated by dividing an integrated value θ of the turning angular velocity ω obtained by the inertial sensor every time Δt within the turning time T by the turning time T. Calculation method of the calibration value described in . The inertial sensor is attached to a turning test table of a test device having an encoder,
3. The method according to claim 2, wherein the error ratio SF is calculated from the rotation angle θe of the turning test table obtained by the encoder and the integrated value θ using Equation 1. How to calculate the calibration value of

Images