JP2023040931A - Position measuring device for moving object - Google Patents

Abstract

To make it possible to accurately measure the position of a moving object in real time by correcting sequential error drift from the moving object in motion while predicting it.SOLUTION: A position measuring device for a moving object for measuring the position of the moving object using a gyro sensor and an acceleration sensor includes: means 3 for sampling signals from the gyro sensor and the acceleration sensor for a predetermined period of time; a complementary filter 8 that directly inputs the signals from the gyro sensor and the acceleration sensor; and means 5 for estimating a function, means 6 for setting a frequency, and means 7 for setting a coefficient value of the complementary filter, based on the sampled data. The position measuring device further includes means 9 for correcting data output from the complementary filter based on a predicted value approximated by the function estimating means and the frequency obtained by the frequency setting means.SELECTED DRAWING: Figure 1

Description

The present invention relates to a position measuring device that uses a gyro sensor and an acceleration sensor to measure the position of a moving object in real time with high accuracy.

For example, the position measuring device of a moving object is used for forming a chemical solution injection hole for strengthening the underground foundation of a building, drilling a hole for pipes such as gas and electricity to the underground, and excavating along a planned route. It is used for the purpose of grasping the tip position of a drilling rod or the like of a drilling rig. In such cases, in order to measure the tip position with high accuracy, noise errors due to drift caused by the gyro sensor and acceleration sensor used and disturbance factors picked up from long cables laid underground should be reduced. There is a need.
Conventionally, a method for removing errors arising from such sensors is to use least-squares methods to extract drift as a function of time from stored sensor data when the vehicle is stationary or in a steady state with little movement. By calculating the error components such as components and subtracting the function component from the data obtained from the sensor, data that does not include error components such as drift is created, and this data is used to calculate the position of the moving object. (See Patent Document 1, for example).

Further, Patent Document 2 discloses that in an acceleration detection device, the output of an acceleration sensor is averaged each time the vehicle travels a predetermined distance to calculate a drift amount, and zero point correction is performed based on this drift amount. It is That is, the drift amount is calculated as a weighted sum of the output value of the acceleration sensor and the previous value of the drift amount, and the sensor output value is obtained by subtracting this drift amount from the output value of the acceleration sensor. Here, predetermined constants are used as weighting coefficients.
In such a method, since the future drift amount is not calculated and predicted sequentially, there is a possibility that the error from the actual drift amount becomes large.

JP 2017-15605 A JP 2008-145152 A

The problem to be solved is that the analysis result of the error component from the non-operating gyro sensor and acceleration sensor is reflected, and the correction value is predetermined based on past results. , the trend of error in real time is not analyzed during operation, and accurate position measurement of the moving object is not performed in accordance with the actual situation.

A gyro sensor or an acceleration sensor is generally used as a device for detecting motion and rotation information of an object, but each has advantages and disadvantages in obtaining posture position information. Since the gyro sensor integrates the detected angular velocity, error components accumulate and drift occurs, and the acceleration sensor adds components other than gravity when there is movement. To remove these error components, the gyro sensor is passed through a high-pass filter (HPF) and the acceleration sensor is passed through a low-pass filter (LPF). Complementary filters such as Kalman filters and Madgwick filters are known as filters that combine these filters. That is, the offset due to the drift from the gyro sensor can be removed below the cutoff frequency fc, and the high frequency component from the acceleration sensor can be removed above the cutoff frequency fc.

According to the present invention, noise from a gyro sensor or an acceleration sensor is canceled in advance using a complementary filter, and then correction is performed while predicting the trend of errors such as drift that occur during the movement of a moving object. The main feature is to be able to measure the position of a moving object in real time with high accuracy.

The position measuring device of the moving object according to the present invention removes noise from the signal from the sensor in advance with a complementary filter during movement, performs function measurement, and corrects while predicting trends such as drift using that function. Therefore, even if the tendency of occurrence of drift or the like changes during the movement of the moving object, prediction can be made with the corresponding function, and there is an advantage that the position can be measured accurately.

1 is a block diagram showing a main part of a position measuring device for a moving object according to the present invention; FIG. FIG. 4 is a diagram for explaining the operation of the function estimation circuit and frequency setting circuit according to the present invention; FIG. 4 is an explanatory diagram showing output timings of a correction circuit according to the present invention;

Accurate position measurement during movement is achieved by repeating prediction and correction in a short time to correct the drift error from the sensor of the moving body.

Hereinafter, the position measuring device of the present invention will be described in detail with reference to the drawings.
FIG. 1 is a block diagram showing the essential parts of a position measuring apparatus for a mobile object according to the present invention. 1 is a sensor comprising a gyro sensor and an acceleration sensor, and 2 is a circuit for sampling signals from the sensor 1 at predetermined time intervals. 3 is a sampling circuit that performs sampling at a predetermined sampling time; 4 is a complementary filter; 5 is a function estimation circuit; 6 is a frequency setting circuit; 9 is a correction circuit for correcting the signal from the complementary filter 8 based on the function estimated from the function circuit 5 and the frequency set by the frequency setting circuit 6; Circuits for measuring and processing the position of the moving object by the calculation of are shown respectively.

First, a signal from the sensor 1, that is, a signal detected by the gyro sensor and the acceleration sensor is input to the sampling circuit 3 via the gate circuit 2. As shown in FIG. The time for connection to the sampling circuit 3 is around 10 seconds, and an arbitrary time required for sampling is set. A signal output from the sensor 1 is alternately input to the sampling circuit 3 and a complementary filter 8 to be described later by the gate circuit 2 .

Next, after the sampling circuit 3 samples the outputs of the gyro sensor and the acceleration sensor, the sampled data is input to the complementary filter 4 to remove noise.
The pitch angle θ(n+1) output by the complementary filter 4 is similar to that of the complementary filter 8 described later.
Formula θ(n+1)=k×(θ(n)+Δθg)+(1−k)×θα(n+1)
required by
When the value of the coefficient k is increased, the effect on the drift amount of the gyro sensor output value becomes greater than the output value of the acceleration sensor.
Note that the coefficient k of the complementary filter 4 is preset to 0.96 as a default.

The output of complementary filter 4 is input to function estimation circuit 5 , frequency setting circuit 6 and coefficient setting circuit 7 . FIG. 2 is a diagram for explaining the operation of the function estimating circuit and frequency setting circuit of the present invention.
The function estimation circuit 5 estimates the tendency of the sampled data output from the complementary filter 4 with respect to time within the sampling time (around 10 seconds). Function approximation is performed by the method of least squares, linear function, quadratic function, etc., and the future error of the error component such as drift is estimated using this function. Therefore, since the trend of errors such as drift is sequentially estimated within a short period of time (about 10 seconds), even if the trend of error changes, that is, even if the approximation function changes, accurate estimation that follows the trend is possible. Become.

The frequency setting circuit 6 regards the error (fluctuation) between the value obtained by the function estimated by the function estimation circuit 5 and the sampled value as a frequency, and determines a predetermined frequency. Calculate the amount of deviation from the estimated function from the angle of inclination, etc., and if the deviation value is greater than a predetermined value, it is assumed that there is fluctuation (waveform portion in FIG. 2). Determine the predetermined frequency. It should be noted that the frequency may be selected from among several depending on the magnitude of the shift.
Figure 2 shows how the estimation and correction are alternately repeated every 10 seconds or so. That is, the signal from the sensor is corrected during the next time t3-t4 by the function (linear function) estimated during the time t2-t3 and the frequency corresponding to the fluctuation.

The coefficient setting circuit 7 determines coefficient values of the complementary filter 8 . Complementary filter 8 has the same configuration as complementary filter 4 described above, and in order to determine the output of the filter, it is necessary to obtain the value of coefficient k using the above equation. When the output data from the complementary filter 4 varies greatly, the value of the coefficient k is decreased to reduce the contribution of the gyro to the pitch angle of the complementary filter 8 . The default is about 0.96, but it is preferable to lower it to about 0.7. A predetermined threshold value is provided for the detection of variation, and the value of the coefficient k of the complementary filter 8 can be changed when the variation is large.

Next, when the signal from the sensor 1 is switched from the sampling circuit 3 to the complementary filter 8 by the gate circuit 2, the noise-reduced signal is output through the complementary filter 8, and the function is estimated by the correction circuit 9. It will be corrected by circuit 5 and frequency determination circuit 6 . That is, in the correction circuit 9, the expected value based on the function estimated by the function estimation circuit 5 is applied for a predetermined period (about 10 seconds), subtracted from the signal, and the low-pass signal of the frequency determined by the frequency setting circuit 6 is applied. Fluctuation components are removed by the filter. In this manner, the output signal of the correction circuit 9 is obtained by removing error components such as drift. be done.

FIG. 3 is an explanatory diagram for explaining the output timing of the correction circuit according to the present invention. The output timing of the aforementioned correction circuit 9 is intermittent, as indicated by the measurement output 1 a in the figure, but another similar sensor is installed to shift the correction timing. If the same circuit is used for the other sensor, it is possible to output at the timing indicated by b of the measurement output 2 from the other sensor. If two sensors are used in this way, the position information can be obtained continuously by combining the measurement output 1 and the measurement output 2, so that the position can be measured in real time. In addition, the measurement output 3 shows the output of each correction circuit when three similar sensors are installed, but the timing of the output c is delayed compared to the case where one or two sensors are installed. , the sampling time can be lengthened, the function approximation and the fluctuation frequency can be obtained from a large amount of sampling data, and more accurate measurement data output can be obtained. These timings can be set by the gate circuit 2 of each sensor.

Signals from the gyro sensor and acceleration sensor can be detected and measured in real time as highly accurate information with drift etc. removed. For example, in civil engineering projects that use excavators, etc., the planned route for excavation can be easily determined by visual inspection. Accurate position measurement is possible even if the size of the error is different between when the signal from the sensor is transmitted via a long cable and when the signal is communicated wirelessly. Moreover, by adopting the technique of the present invention for an accessory provided between a sensor and a measuring instrument or a part integrated with a sensor, it is possible to improve the accuracy of various existing measuring instruments. INDUSTRIAL APPLICABILITY The present invention has high industrial applicability in this field and in fields where error correction is required in real time.

REFERENCE SIGNS LIST 1 sensor 2 gate circuit 3 sampling circuit 4 complementary filter 5 function estimation circuit 6 frequency setting circuit 7 coefficient setting circuit 8 complementary filter 9 correction circuit 10 measurement processing circuit

Claims (6)

A mobile position measuring device for measuring the position of a mobile body using a gyro sensor and an acceleration sensor, comprising: means for sampling signals from the gyro sensor and the acceleration sensor for a predetermined time; a complementary filter for directly inputting a signal; means for estimating a function based on the sampled data; means for setting a frequency; and means for setting a coefficient value of the complementary filter; A position measuring device for a moving object, comprising means for correcting data output from said complementary filter based on the value and the frequency obtained by said frequency setting means. 2. The movement according to claim 1, wherein said sampled data is output to said function estimating means, said frequency setting means and means for setting coefficient values of said complementary filter through a second complementary filter. Body position measuring device. 2. The position measuring device for a moving body according to claim 1, wherein the function estimating means approximates the sampled data by a least squares method or a quadratic function. 2. A position measuring device for a moving body according to claim 1, wherein said sampling means and said complementary filter are switched to each other to receive signals from said gyro sensor and acceleration sensor. 2. The position measuring device according to claim 1, wherein the frequency setting means determines the frequency based on the degree of difference between the value of the function approximated by the function estimating means and the sampling data. . 2. The position measuring apparatus for a moving body according to claim 1, wherein said means for setting the coefficient value of said complementary filter determines the coefficient value according to the degree of variation of said sampled data.

Images