JP2015148450A - Sensor error correction device, imu calibration system, imu calibration method, and imu calibration program - Google Patents

Abstract

PROBLEM TO BE SOLVED: To calibrate an IMU to be calibrated with high accuracy by using only IMU output.SOLUTION: An IMU calibration system 1 is provided with an IMU error calculation unit 31 and an error correction unit comprising a bias error correction unit 212, a scale factor error correction unit 213, and a misalignment error correction unit 214. The IMU error calculation unit 31 successively estimates and calculates, in a first period prior to separation, a bias error, a scale factor error, and a misalignment error included in a measured value to be calibrated, by using a reference measured value outputted by a high-accuracy IMU and a measured value to be calibrated that is outputted by an IMU to be calibrated. The error correction unit, installed in a first mobile entity, successively corrects the output value of a sensor provided in the IMU to be calibrated, by using each of the errors estimated and calculated by the IMU error calculation unit in the first period. In a second period after a separation performed, the error correction unit maintains the correction that is effective at the time of separation.

Description

  The present invention relates to a sensor error correction device that corrects an error in measurement data of an inertial sensor provided in an inertial posture measurement device (Intermediate Measurement Unit), and an IMU calibration system including the sensor error correction device.

  Conventionally, various inertial posture measuring devices that detect the posture of a moving body using an inertial sensor such as an angular velocity sensor (gyro sensor) or an acceleration sensor have been devised. The inertial posture measuring device is an International Measurement Unit, and is generally represented by an abbreviation of IMU.

  In such an IMU, it is known that an error is caused in the posture to be measured due to an error included in measurement data (angular velocity or acceleration) of the inertial sensor, an error due to an attachment angle of the inertial sensor with respect to the IMU, or the like.

  In order to eliminate the influence of such errors, posture measurement was performed after the output value of the inertial sensor was calibrated. For example, in Patent Document 1, a physical angle difference between a gyro sensor attached to a moving body and a gyro sensor attached to a moving base body that fires the moving body is detected. Based on the physical angle difference, the bias error of the gyro sensor mounted on the moving body is calculated.

Japanese Patent Laid-Open No. 6-26877

  However, in the calibration method shown in Patent Document 1 described above, only the bias error in the IMU can be calibrated, and it is difficult to obtain a highly accurate attitude measurement result. In addition to the gyro sensor of the moving body, the gyro sensor of the moving base, and the apparatus for performing the calibration process, a means for detecting the physical angle must be provided separately.

  Accordingly, an object of the present invention is to provide a sensor error correction apparatus capable of correcting an error of a sensor to be calibrated with higher accuracy using only an IMU output, and an IMU calibration system including the sensor error correction apparatus. There is.

  In the sensor error correction apparatus according to the present invention, the first moving body on which the high-precision IMU is mounted and the second moving body on which the calibration target IMU having a sensor with lower accuracy than the sensor of the high-precision IMU moves integrally. And a second movement mode that is a movement mode following the first movement mode in which the first moving body and the second moving body move separately, and is provided in the calibration object IMU. An error correction unit for correcting measurement data of the obtained sensor. In the first movement mode, the error correction unit includes a bias error and a scale factor included in the measurement data of the sensor provided in the calibration target IMU continuously calculated from the measurement data of the high-precision IMU and the measurement data of the calibration target IMU. The measurement data of the sensor provided in the calibration target IMU is continuously corrected using the error and the misalignment error. In the second movement mode, the error correction unit uses the bias error, the scale factor error, and the misalignment error obtained at the timing of shifting from the first movement mode to the second movement mode. Correct the measured data.

  In this configuration, the measurement data of the sensor of the calibration target IMU can be corrected with high accuracy using the reference measurement value of the high accuracy IMU during the period in which the high accuracy IMU and the calibration target IMU are in the same movement mode. And even if the 2nd mobile body with which the calibration object IMU was mounted | worn becomes another movement mode with the 1st mobile body with which the high precision IMU was mounted | worn, the calibration object IMU can maintain high precision.

  In the sensor error correction apparatus of the present invention, the measurement data includes acceleration and angular velocity. The error correction unit includes a lever arm effect correction unit that corrects an error included in the acceleration of the calibration target IMU caused by a difference in relative position between the measurement position of the measurement data of the high-precision IMU and the measurement position of the measurement data of the calibration target IMU. Prepare.

  In this configuration, even if there is a distance between the high accuracy IMU and the calibration target IMU, the acceleration of the sensor provided in the calibration target IMU can be corrected with high accuracy.

  According to the present invention, the calibration of the calibration target IMU can be performed with higher accuracy by using only the output of the reference IMU and the calibration target IMU. Then, it is possible to perform posture measurement with high accuracy using an IMU calibrated with high accuracy.

1 is a schematic configuration diagram of an IMU calibration system according to a first embodiment of the present invention. It is a block diagram which shows the structure of the IMU calibration system which concerns on the 1st Embodiment of this invention. It is a flowchart which shows the IMU calibration method which concerns on 1st Embodiment of this invention. It is a block diagram which shows the structure of an IMU calibration system when a measured value is angular velocity, and a block diagram which shows the structure of an IMU calibration system when a measured value is acceleration. It is a block diagram which shows the structure of the IMU calibration system which concerns on the 2nd Embodiment of this invention. It is a flowchart which shows the IMU calibration method which concerns on 2nd Embodiment of this invention. It is a block diagram which shows the structure of the IMU calibration system which concerns on the 3rd Embodiment of this invention. It is a flowchart which shows the IMU calibration method which concerns on 3rd Embodiment of this invention.

  An IMU calibration system and an IMU calibration method according to a first embodiment of the present invention will be described. FIG. 1 is a schematic configuration diagram of an IMU calibration system according to the first embodiment of the present invention.

  The IMU calibration system 1 includes a high-precision IMU 11R serving as a reference IMU, a calibration target IMU 21 and an IMU error calculation unit 31. The high precision IMU 11R and the IMU error calculation unit 31 are installed in the airplane 910. The calibration target IMU 21 is mounted on the rocket 920.

  The rocket 920 is attached to the airplane 910 in the initial state, and is transported to a predetermined altitude by the airplane 910. The airplane 910 corresponds to the “first moving body” of the present invention. The rocket 920 corresponds to the “second moving body” of the present invention. A mode in which the rocket 920 and the airplane 910 move together corresponds to the “first movement mode” of the present invention. When the rocket 920 reaches a predetermined altitude, the rocket 920 is disconnected from the airplane 910, and thereafter reaches the outer space by the propulsive force generated by itself. The mode in which the rocket 920 and the airplane 910 move separately corresponds to the “second movement mode” of the present invention.

  The inertial sensor provided in the high-accuracy IMU 11R can measure with higher accuracy than the inertial sensor provided in the calibration object IMU 21. Therefore, the high accuracy IMU 11R can perform posture measurement with higher accuracy than the calibration target IMU 21. For example, the high-precision IMU 11R can perform high-accuracy posture measurement including angular velocity and acceleration due to the rotation of the earth.

  The high-precision IMU 11R and the calibration object IMU 21 can measure a measurement value based on the same inertial force. For example, both the high-precision IMU 11R and the calibration object IMU 21 can detect at least acceleration. For example, both the high-precision IMU 11R and the calibration object IMU 21 can detect at least the angular velocity. For example, both the high-precision IMU 11R and the calibration object IMU 21 can detect both acceleration and angular velocity.

  In a state where the rocket 920 is mounted on the airplane 910, the high-precision IMU 11R and the calibration target IMU 21 are connected to the IMU error calculation unit 31. The high accuracy IMU 11R and the calibration target IMU 21 output measurement values (angular velocity and acceleration) to the IMU error calculation unit 31. Hereinafter, the measurement value from the high-precision IMU 11R is referred to as a reference measurement value. This reference measurement value corresponds to “measurement data of high-precision IMU” of the present invention. A measurement value from the calibration target IMU 21 is referred to as a calibration target measurement value. This calibration target measurement value corresponds to “measurement data of the calibration target IMU” of the present invention.

  The IMU error calculation unit 31 sequentially calculates a misalignment error, a bias error, and a scale factor error for the calibration target IMU 21 at predetermined sampling timings using the calibration target measurement value and the reference measurement value. A more specific IMU calibration method will be described later. The IMU error calculation unit 31 sequentially gives the calculated misalignment error, bias error, and scale factor error to the calibration target IMU 21.

  FIG. 2 is a block diagram showing the configuration of the IMU calibration system according to the first embodiment of the present invention. As shown in FIG. 2, the calibration object IMU 21 includes an inertial sensor 211, a bias error correction unit 212, a scale factor error correction unit 213, and a misalignment error correction unit 214. A portion including the bias error correction unit 212, the scale factor error correction unit 213, and the misalignment error correction unit 214 corresponds to the “error correction unit” of the present invention. The inertial sensor 211 includes at least one of an acceleration sensor and an angular velocity sensor. The output terminal of the inertial sensor 211 is connected to the input terminal of the bias error correction unit 212. The output terminal of the bias error correction unit 212 is connected to the input terminal of the scale factor error correction unit 213. The output terminal of the scale factor error correction unit 213 is connected to the input terminal of the misalignment error correction unit 214. The output end of the misalignment error correction unit 214 is connected to the IMU error calculation unit 31.

  The calibration target IMU 21 uses a misalignment error, a bias error, and a scale factor error sequentially given from the IMU error calculation unit 31, and uses a bias error correction unit 212, a scale factor error correction unit 213, and a misalignment error correction unit. In 214, the output values of the inertial sensor 211 are sequentially corrected. This correction is continuously performed during the first movement mode. Thus, by correcting the error included in the measurement value of the inertial sensor 211 to be calibrated using the three types of errors constituting the posture error, the measurement value output from the calibration target IMU 21 is corrected after correction. It becomes a highly accurate value. That is, the calibration target IMU 21 is calibrated so that highly accurate posture detection is possible.

  When the rocket 920 is disconnected from the airplane 910 and the calibration target IMU 21 is disconnected from the IMU error calculation unit 31, the misalignment error, the bias error, and the scale factor error are not input to the calibration target IMU 21 from the IMU error calculation unit 31. The calibration target IMU 21 stores a correction value based on the finally input error in a period after the timing when the rocket 920 is disconnected from the airplane 910 (period of the second movement mode), and uses the stored correction value. The measurement value of the inertial sensor 211 is continuously corrected. Thereby, even after the rocket 920 is disconnected from the airplane 910, the calibration target IMU 21 can be continuously calibrated with high accuracy.

  Thus, since the attitude measurement value output from the calibration target IMU 21 after calibration is highly accurate, even if the rocket 920 is disconnected from the airplane 910, the rocket 920 can be propelled in a highly accurate orbit. .

  Further, in this IMU calibration system 1, an expensive high-precision IMU 11R is mounted on the airplane 910, and if not calibrated, the rocket 920 is mounted with an inexpensive calibration target IMU 21 that is less accurate than the high-precision IMU 11R. Therefore, the expensive high-accuracy IMU 11R can be used many times, and the inexpensive calibration target IMU 21 can be used for the IMU that is released to an unrecoverable place such as outer space.

  Here, specific processing of the IMU calibration method in the IMU calibration system according to the present embodiment will be described with reference to FIG. FIG. 3 is a flowchart illustrating the IMU calibration method according to the first embodiment of the present invention.

  The IMU error calculation unit 31 sequentially acquires the calibration target measurement value and the reference measurement value while the calibration target IMU 21 and the high accuracy IMU 11R are moving together (S101).

  The IMU error calculation unit 31 sets an equation in which the three-axis component of misalignment error that does not change with time, the three-axis component of bias error, and the three-axis component of scale factor error are unknowns. The IMU error calculation unit 31 sets an equation with the measurement value to be calibrated and the reference measurement value as observation values (known numbers). The IMU error calculation unit 31 substitutes the calibration target measurement value and the reference measurement value that can be sequentially acquired with respect to this equation as observation values (known numbers), so that the misalignment error triaxial component, bias The triaxial component of the error and the triaxial component of the scale factor error are estimated and calculated (S102). For example, the IMU error calculation unit 31 calculates a three-axis component of a misalignment error, a three-axis component of a bias error, and a three-axis component of a scale factor error using state estimation processing such as a Kalman filter or the least square method.

  The IMU error calculation unit 31 feeds back the calculated three types of errors to the calibration target IMU 21 and the calibration target IMU 21 is calibrated using the fed back error (S103).

  By executing such processing, even if the measurement accuracy of the inertial sensor used for the calibration target IMU is low and inexpensive, it can be used in a state where calibration is performed with high accuracy as described above. it can. That is, it can be used as an IMU that is inexpensive and compensates for measurement accuracy with high accuracy.

  Next, a specific theory of IMU calibration in this embodiment will be described. In the following, a case where a state estimation equation is used, particularly a case where a Kalman filter is used will be described. In the following, in the three orthogonal axes, the X axis is an axis parallel to a specific movement direction (for example, the longitudinal direction of the airplane 910), and the Y axis is an axis parallel to a direction orthogonal to the movement direction (X axis) (for example, an airplane) 910 horizontal direction), the Z axis is the axis perpendicular to the moving direction (X axis) and the Y axis. Further, the X axis has the forward direction as the positive direction, the Y axis has the right direction as the positive direction, and the Z axis has the downward direction as the positive direction.

(A) Gyro sensor (angular velocity sensor) First, an observation model is derived from the error model of the calibration target IMU 21. Errors included in the angular velocity ω _mes measured by the calibration target IMU 21 are mainly misalignment errors, bias errors, and scale factor errors. Accordingly, when the bias error in the uniaxial direction is ΔBi _ω and the scale factor error in the uniaxial direction is Δsf _ω at the angular velocity ω1 _mes in one of the three orthogonal axes, the following equation can be expressed. Here, ω _real is a true angular velocity.

This equation can be approximated as follows because the product of the bias error ΔBi _ω and the scale factor error Δsf _ω is sufficiently smaller than the other terms.

The transformation matrix Ctr _mes ^ref when the misalignment angle between the coordinate system of the high precision IMU 11R and the coordinate system of the calibration target IMU 21 is Θ can be expressed by the following equation. Note that C _mes ^ref is a coordinate conversion matrix of a known attachment angle, that is, a coordinate conversion matrix of an attachment angle with respect to the airplane 910 of the high-precision IMU 11R.

  Here, I is a unit matrix. Also, S (Θ) is obtained from the following equation, where the misalignment angle of the X axis is Δψ, the misalignment angle of the Y axis is Δθ, and the misalignment angle of the Z axis is Δφ.

From (Equation 1) and (Equation 2), an observation model of the angular velocity measurement value ω _{mes in} three orthogonal axes including the above-described misalignment error, bias error, and scale factor error is expressed by the following equation.

  Here, if the value of the second order term or less is very small, it is further expressed by the following equation.

When the angular velocity measured by the high precision IMU 11R is ω _ref and the observation noise of the high precision IMU 11R is ν, the following equation is obtained.

Therefore, when the true angular velocity ω _real that cannot be directly observed is eliminated using (Expression refA) and (Expression 3), the following expression is obtained.

As the observed value z, a value obtained by subtracting the angular velocity ω _mes measured by the calibration target IMU 21 from the angular velocity ω _ref measured by the high-precision IMU 11 is used. This suppresses the influence of observation noise. That is, z = ω _ref −ω _mes . An observation matrix can be set as follows for the observation value z set in this way.

Here, x is a state variable vector, the misalignment angles of the X-axis, Y-axis, and Z-axis are respectively Δψ _ω , Δθ _ω , Δφ _ω, and the bias errors of the X-axis, Y-axis, and Z-axis are respectively ΔBix _ω , DerutaBiy _omega, and ΔBiz _ω, X-axis, Y-axis, respectively the scale factor error in the Z-axis Δsfx _ω, Δsfy _ω, when the Derutasfz _omega, is expressed by the following equation.

If the X-axis component, Y-axis component, and Z-axis component of the angular velocity ω _ref measured by the high-precision IMU 11 are ω _refx , ω _refy , and ω _refz , the transformation matrix H is expressed as follows.

C _mes ^ref is expressed by the following equation.

Next, a system model is derived.
The system model is expressed by the following equation, where w is a system error.

Here, since the state variable x described above includes a misalignment error, a bias error, and a scale factor error, it can be assumed to be constant regardless of time. Therefore, it can be defined that A = 0 and B = 0.
And when (Expression 5) is expressed in a discrete system, the following expression is obtained.

  Thereby, the state model formula becomes the following formula.

  By applying these state model equations to the Kalman filter, the IMU error due to the angular velocity represented by the state variable x can be measured.

  Note that the observation update of the Kalman filter is represented by the following equation, as is known.

  Further, the time update of the Kalman filter is represented by the following equation as is known.

(B) In the case of an acceleration sensor Also in the case of measuring an error in acceleration, a Kalman filter can be applied basically in the same manner as in measuring an error in angular velocity. However, in the case of acceleration, since the lever arm effect is added, it is necessary to add a correction term for that purpose. Therefore, in the following, different points in the error measurement of acceleration and the error measurement of angular velocity will be specifically described, and the description of the portions where the above-described angular velocity error measurement processing can be diverted will be omitted.

First, an observation model is derived from the error model of the calibration target IMU 21. Errors included in the acceleration a _mes measured by the calibration target IMU 21 are mainly misalignment errors, bias errors, and scale factor errors. Therefore, among the three orthogonal axes, angular velocity a1 _mes in uniaxial, the bias error of the uniaxial direction and DerutaBi _a, the scale factor error in the axial direction when the Derutasf _a, can be expressed as follows. Where a _real is the true acceleration.

This expression is the multiplication value of the bias error DerutaBi _a and scale factor error Derutasf _a is, since sufficiently smaller than the other terms can be approximated as follows.

Then, as in the case of the angular velocity described above, the following equation is obtained by performing a process of eliminating the true angular velocity a _real that cannot be observed directly.

Here, when the value obtained by subtracting the acceleration a _mes measured by the calibration target IMU 21 from the acceleration a _ref measured by the reference IMU 11R is used as the observed value z, the lever arm effect is considered.

Specifically, the relative position of the calibration target IMU 21 is set as P _mes with the position of the high-precision IMU 11R as a reference position. The relative position P _mes can be decomposed into an X-axis component, a Y-axis component, and a Z-axis component, which are P _mesx , P _mesy , and P _mesz , respectively.

Here, assuming that the lever arm correction value is AA, the lever arm correction value AA depends on the relative position P _mes and the reference angular velocity ω _ref measured by the high precision IMU 11R.

  Therefore, in the case of acceleration, the observed value z is expressed by the following equation.

  By using the observed value z of (Expression 4 ′), the IMU error due to the acceleration represented by the state variable x can be measured.

  When the position of the high-precision IMU 11R and the position of the calibration target IMU 21 are close to each other, or when the operation that generates the angular velocity is sufficiently slow and the angular velocity at the two points hardly changes depending on the positional relationship, the lever arm Without using the correction value, the IMU error due to acceleration can be measured by the same method as in the case of the angular velocity described above.

  As described above, by using the method of this embodiment, the misalignment error, the bias error, and the scale factor error included in the measurement value are measured with high accuracy using only the output of the angular velocity sensor or the acceleration sensor. be able to. By using the measured error, the calibration target IMU can be calibrated with high accuracy.

  Next, calibration of the IMU calibration system for realizing the above-described IMU calibration method will be described more specifically with reference to FIG.

  The IMU error calculation unit 31 receives the reference measurement value OVr and the calibration target measurement value OVm. The IMU error calculation unit 31 is based on a reference measurement value OVr and a calibration target measurement value OVm after error correction described later, a misalignment error [Δψ, Δθ, Δφ], a bias error [ΔBix, ΔByy, ΔBiz], a scale factor. The error [Δsfx, Δsfy, Δsfz] is estimated and calculated.

  The IMU error calculation unit 31 feeds back the bias error [ΔBix, ΔByy, ΔBiz] to the bias error correction unit 212. The bias error correction unit 212 corrects the output value from the inertial sensor 211 using the fed back bias error, and outputs it to the scale factor error correction unit 213.

  The IMU error calculation unit 31 feeds back the scale factor error [Δsfx, Δsfy, Δsfz] to the scale factor error correction unit 213. The scale factor error correction unit 213 corrects the output value from the bias error correction unit 212 using the fed back scale factor error, and outputs it to the misalignment error correction unit 214.

  The IMU error calculation unit 31 feeds back the misalignment error [Δψ, Δθ, Δφ] to the misalignment error correction unit 214. The misalignment error correction unit 214 corrects the output value from the scale factor error correction unit 213 using the fed back misalignment error, and outputs it as a calibration target measurement value OVm after error correction.

  By repeating such error estimation and error correction loop processing, each error converges, misalignment errors [Δψ, Δθ, Δφ], bias errors [ΔBix, ΔByy, ΔBiz], and scale factor errors [Δsfx, Δsfy]. , Δsfz] can be estimated and calculated with high accuracy. The calibration target measurement value OVm output from the calibration target IMU 21 can be set to the same accuracy as the reference measurement value OVr.

  Next, the case where the inertial sensor is an angular velocity sensor and the case where the inertial sensor is an acceleration sensor will be specifically described.

  FIG. 4A is a block diagram showing the configuration of the IMU calibration apparatus when the measurement value is angular velocity, and FIG. 4B is a block diagram showing the configuration of the IMU calibration apparatus when the measurement value is acceleration. In the mode shown in FIG. 4A, the case where the high-precision IMU and the calibration target IMU output only the angular velocity is shown, and in the mode shown in FIG. 4B, the high-precision IMU and the calibration target IMU output only the acceleration. Show the case.

(A) When Measurement Value is Only Angular Velocity As shown in FIG. 4A, the IMU calibration system 1 includes a high-precision angular velocity sensor 11Rω, a calibration target IMU 21ω, and an IMU error calculation unit 31. The calibration target IMU 21ω includes a calibration target angular velocity sensor 211Tω, a bias error correction unit 212ω, a scale factor error correction unit 213ω, and a misalignment error correction unit 214ω.

The IMU error calculation unit 31 uses the reference angular velocity ω _ref from the high-precision angular velocity sensor 11Rω and the calibration target angular velocity ω _mes from the calibration target IMU 21ω to calculate the misalignment error [Δψ _ω , Δθ _ω , Δφ _ω ], bias error [ΔBix _ω , ΔByy _ω , ΔBiz _ω ], and scale factor error [Δsfx _ω , Δsfy _ω , Δsfz _ω ] are estimated and calculated.

The IMU error calculation unit 31 feeds back the bias error [ΔBix _ω , ΔBiy _ω , ΔBiz _ω ] to the bias error correction unit 212 _ω . The IMU error calculation unit 31 feeds back the scale factor error [Δsfx _ω , Δsfy _ω , Δsfz _ω ] to the scale factor error correction unit 213 _ω . The IMU error calculation unit 31 feeds back the misalignment error [Δψ _ω , Δθ _ω , Δφ _ω ] to the misalignment error correction unit 214 _ω .

The bias error correction unit 212ω corrects the output value from the calibration target angular velocity sensor 211Tω using the fed back bias error [ΔBix _ω , ΔByy _ω , ΔBiz _ω ], and outputs it to the scale factor error correction unit 213ω. The scale factor error correction unit 213ω corrects the output value from the bias error correction unit 212ω using the fed back scale factor errors [Δsfx _ω , Δsfy _ω , Δsfz _ω ], and outputs the corrected value to the misalignment error correction unit 214ω. . The misalignment error correction unit 214ω corrects the output value from the scale factor error correction unit 213ω using the fed back misalignment error [Δψ _ω , Δθ _ω , Δφ _ω ], and the IMU is used as the calibration target angular velocity ω _mes . Output to the error calculation unit 31.

By repeating such error estimation and error correction loop processing, each error converges, misalignment error [Δψ _ω , Δθ _ω , Δφ _ω ], bias error [ΔBix _ω , ΔByy _ω , ΔBiz _ω ], scale Factor errors [Δsfx _ω , Δsfy _ω , Δsfz _ω ] can be estimated and calculated with high accuracy. The calibration target angular velocity ω _mes output from the calibration target IMU 21ω can be set to the same accuracy as the reference angular velocity ω _ref .

(B) Case where Measurement Value is Only Acceleration The configuration shown in FIG. 4B is suitable for a mode in which the lever arm effect does not occur or a mode that can be ignored.

  As shown in FIG. 4B, the IMU calibration system 1 includes a high-precision acceleration sensor 11Ra, a calibration target IMU 21a, and an IMU error calculation unit 31. The calibration target IMU 21a includes a calibration target acceleration sensor 211Ta, a bias error correction unit 212a, a scale factor error correction unit 213a, and a misalignment error correction unit 214a.

The IMU error calculation unit 31 uses the reference acceleration a _ref from the high-precision acceleration sensor 11Ra and the calibration target acceleration a _mes from the calibration target IMU 21a to calculate the misalignment error [Δψ _a , Δθ _a , Δφ _a ], bias error [ΔBix _a , ΔByy _a , ΔBiz _a ], and scale factor error [Δsfx _a , Δsfy _a , Δsfz _a ] are estimated and calculated.

The IMU error calculation unit 31 feeds back the bias error [ΔBix _a , ΔByy _a , ΔBiz _a ] to the bias error correction unit 212 a. The IMU error calculation unit 31 feeds back the scale factor error [Δsfx _a , Δsfy _a , Δsfz _a ] to the scale factor error correction unit 213 a. The IMU error calculation unit 31 feeds back the misalignment error [Δψ _a , Δθ _a , Δφ _a ] to the misalignment error correction unit 214 a.

The bias error correction unit 212a corrects the output value from the calibration target angular velocity sensor 211Ta using the fed back bias error [ΔBix _a , ΔByy _a , ΔBiz _a ], and outputs the corrected value to the scale factor error correction unit 213a. The scale factor error correction unit 213a corrects the output value from the bias error correction unit 212a using the fed back scale factor error [Δsfx _a , Δsfy _a , Δsfz _a ], and outputs the corrected value to the misalignment error correction unit 214a. . The misalignment error correction unit 214a corrects the output value from the scale factor error correction unit 213a using the fed back misalignment error [Δψ _a , Δθ _a , Δφ _a ], and uses the IMU as the calibration target acceleration a _mes . Output to the error calculation unit 31.

By repeating such error estimation and error correction loop processing, each error converges, misalignment error [Δψ _a , Δθ _a , Δφ _a ], bias error [ΔBix _a , ΔByy _a , ΔBiz _a ], scale Factor errors [Δsfx _a , Δsfy _a , Δsfz _a ] can be estimated and calculated with high accuracy. The calibration target acceleration a _mes output from the calibration target IMU 21a can be set to the same accuracy as the reference acceleration a _ref .

  Next, an IMU calibration system and an IMU configuration method according to a second embodiment of the present invention will be described with reference to the drawings. FIG. 5 is a block diagram showing a configuration of an IMU calibration system according to the second embodiment of the present invention. Note that the IMU calibration system of the present embodiment is obtained by adding a lever arm effect correction unit 215a to the IMU calibration system shown in the first embodiment. Therefore, only different parts will be specifically described. The lever arm effect correction unit 215a also corresponds to a component of the “error correction unit” of the present invention.

The IMU calibration system 2 includes a high-precision IMU 11R, a calibration target IMU 21 ′, and an IMU error calculation unit 31. The high precision IMU 11R includes a high precision acceleration sensor 11Ra and a high precision angular velocity sensor 11Rω. The high accuracy acceleration sensor 11Ra outputs a reference acceleration a _ref . The high-accuracy angular velocity sensor 11Rω outputs a reference angular velocity ω _ref .

Based on the reference acceleration a _ref from the high-accuracy IMU 11R and the calibration target acceleration a _mes from the calibration target IMU 21 ′, the IMU error calculation unit 31 performs misalignment errors [Δψ _a , Δθ _a , Δφ _a ], bias error [ ΔBix _a , ΔByy _a , ΔBiz _a ] and a scale factor error [Δsfx _a , Δsfy _a , Δsfz _a ] are estimated and calculated.

  The calibration target IMU 21 'includes a calibration target acceleration sensor 211Ta, a bias error correction unit 212a, a scale factor error correction unit 213a, a misalignment error correction unit 214a, and a lever arm effect correction unit 215a. A portion including the bias error correction unit 212a, the scale factor error correction unit 213a, the misalignment error correction unit 214a, and the lever arm effect correction unit 215a corresponds to the “error correction unit” of the present invention.

A reference angular velocity ω _ref is input from the high-precision IMU 11R to the lever arm effect correction unit 215a. Further, the lever arm effect correction section 215a, the relative position _{P mes} being calibrated IMU21 'relative to the position of the high-precision IMU11R is input. This relative position P _mes may be surveyed separately in advance, for example.

The lever arm effect correction unit 215a corrects the acceleration output from the calibration target acceleration sensor 211Ta using the lever arm effect correction value AA calculated from the relative position P _mes and the high-accuracy angular velocity ω _ref .

The bias error correction unit 212a corrects the acceleration after the lever arm effect correction using the bias error [ΔBix _a , ΔByy _a , ΔBiz _a ] estimated by the IMU error calculation unit 31. The scale factor error correction unit 213a corrects the acceleration after the bias error correction using the scale factor errors [Δsfx _a , Δsfy _a , Δsfz _a ] estimated by the IMU error calculation unit 31. The misalignment error correction unit 214a corrects the acceleration a _mes after the scale factor error correction using the misalignment errors [Δψ _a , Δθ _a , Δφ _a ] estimated by the IMU error calculation unit 31. The acceleration subjected to these error corrections is output to the IMU error calculation unit 31 as the calibration target acceleration a _mes .

By repeating such error estimation and error correction loop processing, each error converges, and misalignment errors [Δψ _a , Δθ _a , Δφ _a ], bias errors [ΔBix _a , ΔBya _a , ΔBiz _a ] The scale factor error [Δsfx _a , Δsfy _a , Δsfz _a ] can be estimated and calculated with high accuracy. In particular, if the configuration of the present embodiment is used, the influence of the lever arm effect can be reduced, and these errors can be estimated and calculated with higher accuracy. The calibration target acceleration a _mes output from the calibration target IMU 21 ′ can be set to the same accuracy as the reference acceleration a _ref .

  Note that the calibration process may be performed by programming the above-described process and executing the program by an arithmetic processor such as a computer. FIG. 6 is a flowchart showing an IMU calibration method according to the second embodiment of the present invention.

The IMU error calculation unit 31 sequentially acquires a calibration target measurement value (calibration target acceleration a _mes ) and a reference target measurement value (reference acceleration a _ref ) (S201).

The IMU error calculation unit 31 sets an equation in which the three-axis component of misalignment error that does not change with time, the three-axis component of bias error, and the three-axis component of scale factor error are unknowns. The IMU error calculation unit 31 sets an equation with the measurement value to be calibrated and the reference measurement value as observation values (known numbers). The IMU error calculation unit 31 applies the lever arm effect correction value AA calculated from the relative position P _mes and the reference angular velocity ω _ref to the equation. The IMU error calculation unit 31 substitutes the calibration target acceleration a _mes and the reference acceleration a _ref that can be sequentially acquired with respect to this equation as observed values (known numbers), so that the three-axis component of the misalignment error Then, the triaxial component of the bias error and the triaxial component of the scale factor error are estimated and calculated (S202).

  The IMU error calculation unit 31 feeds back the calculated error to the calibration target IMU 21 ', and the calibration target IMU 21' performs calibration using the fed back error (S203).

  Next, an IMU calibration system and an IMU configuration method according to a third embodiment of the present invention will be described with reference to the drawings. FIG. 7 is a block diagram showing a configuration of an IMU calibration system according to the third embodiment of the present invention. Note that the IMU calibration system of the present embodiment estimates and calculates each error with respect to acceleration and each error with respect to angular velocity, and uses them for calibration. The estimation calculation of each error with respect to the angular velocity and the use for calibration are the same as in the above-described embodiment. Therefore, only the part peculiar to the configuration of the present embodiment will be specifically described.

The IMU calibration system 3 includes a high accuracy IMU 11R, a calibration object IMU 21 ″, and an IMU error calculation unit 31. The high accuracy IMU 11R includes a high accuracy acceleration sensor 11Ra and a high accuracy angular velocity sensor 11Rω. , and outputs the reference acceleration _{a ref.} precision angular velocity sensor 11Rω outputs the reference angular velocity omega _ref.

The IMU error calculation unit 31 uses the reference angular velocity ω _ref from the high-precision IMU 11R and the calibration target angular velocity ω _mes from the calibration target IMU 21 ″, and misalignment errors [Δψ _ω , Δθ _ω , Δφ _ω ], bias with respect to the angular velocity Errors [ΔBix _ω , ΔByy _ω , ΔBiz _ω ] and scale factor errors [Δsfx _ω , Δsfy _ω , Δsfz _ω ] are estimated and calculated.

  The calibration target IMU 21 ″ includes a calibration target acceleration sensor 211Ta, a calibration target angular velocity sensor 211Tω, a bias error correction unit 212a for acceleration, a scale factor error correction unit 213a for acceleration, a misalignment error correction unit 214a for acceleration, and a lever arm effect. A correction unit 215a, a bias error correction unit 212ω for angular velocity, a scale factor error correction unit 213ω for angular velocity, and a misalignment error correction unit 214ω for angular velocity, a bias error correction unit 212a for acceleration, and a scale for acceleration Factor error correction unit 213a, acceleration misalignment error correction unit 214a, lever arm effect correction unit 215a, bias error correction unit 212ω for angular velocity, scale factor error correction unit 213ω for angular velocity, and misalignment for angular velocity A portion consisting of cement error correction unit 214ω corresponds to "error correction unit" of the present invention.

  The calibration of the calibration target acceleration sensor in the calibration target IMU 21 ″ is the same as that of the calibration target IMU 21 ′ according to the second embodiment described above, and a description thereof will be omitted.

The bias error correction unit 212ω for angular velocity corrects the angular velocity output from the calibration target angular velocity sensor 211Tω using the bias error [ΔBix _ω , ΔByy _ω , ΔBiz _ω ] with respect to the angular velocity estimated by the IMU error calculation unit 31. .

The scale factor error correction unit 213ω for angular velocity corrects the angular velocity after the bias error correction using the scale factor errors [Δsfx _ω , Δsfy _ω , Δsfz _ω ] with respect to the angular velocity estimated by the IMU error calculation unit 31.

The angular velocity misalignment error correction unit 214ω corrects the angular velocity after the scale factor error correction using the misalignment error [Δψ _ω , Δθ _ω , Δφ _ω ] with respect to the angular velocity estimated by the IMU error calculation unit 31. The angular velocity subjected to these error corrections is output to the IMU error calculation unit 31 as the calibration target angular velocity ω _mes .

By repeating such error estimation and error correction loop processing, each error converges, and misalignment errors [Δψ _ω , Δθ _ω , Δφ _ω ] with respect to angular velocities, bias errors [ΔBix _ω , ΔBiy _ω , ΔBiz] _ω ] and scale factor errors [Δsfx _ω , Δsfy _ω , Δsfz _ω ] can be estimated and calculated with high accuracy. The calibration target angular velocity ω _mes output from the calibration target IMU 21 ″ can be set to the same accuracy as the reference angular velocity ω _ref .

  Note that the calibration process may be performed by programming the above-described process and executing the program by an arithmetic processor such as a computer. FIG. 8 is a flowchart showing an IMU calibration method according to the third embodiment of the present invention.

The IMU error calculation unit 31 sequentially acquires a calibration target measurement value (calibration target acceleration a _mes and calibration target angular velocity ω _mes ) and a reference measurement value (reference acceleration a _ref and reference angular velocity ω _ref ) (S301).

The IMU error calculation unit 31 sets an equation with unknown three-axis components of misalignment error, bias error three-axis component, and scale factor error that do not change with respect to the angular velocity. The IMU error calculation unit 31 sets an equation using the calibration target angular velocity ω _mes and the reference angular velocity ω _ref as observed values (known numbers). The IMU error calculation unit 31 substitutes the calibration target angular velocity ω _mes and the reference angular velocity ω _ref that can be sequentially acquired with respect to this equation as observation values (known numbers), thereby obtaining three misalignment errors for the angular velocity. An axial component, a triaxial component of a bias error, and a triaxial component of a scale factor error are estimated and calculated (S302).

The IMU error calculation unit 31 sets equations with unknown three-axis components of misalignment errors that do not change with time, three-axis components of bias errors, and three-axis components of scale factor errors. The IMU error calculation unit 31 sets an equation with the calibration target acceleration a _mes and the reference acceleration a _ref as observation values (known numbers). The IMU error calculation unit 31 applies the lever arm effect correction value AA calculated from the relative position P _mes , the reference angular velocity ω _ref, and the misalignment error [Δψ _ω , Δθ _ω , Δφ _ω ] to the angular velocity to the equation. The IMU error calculation unit 31 substitutes the calibration target acceleration a _mes and the reference acceleration a _ref that can be sequentially acquired with respect to this equation as observation values (known numbers), thereby obtaining three misalignment errors for acceleration. An axial component, a triaxial component of a bias error, and a triaxial component of a scale factor error are estimated and calculated (S303).

  The IMU error calculation unit 31 feeds back the calculated error to the calibration target IMU 21 ″, and the calibration target IMU 21 ″ performs calibration using the fed back error (S304).

In the above description, an example is shown in which each error for acceleration and each error for angular velocity are estimated and calculated by separate processing. However, the acceleration and the angular velocity may be estimated by the same process. That is, the three-axis misalignment error [Δψ _a , Δθ _a , Δφ _a ], the bias error with respect to acceleration using the calibration target acceleration a _mes , the reference acceleration a _ref , the calibration target angular velocity ω _mes , and the reference angular velocity ω _ref as observation values. [ΔBix _a , ΔByy _a , ΔBiz _a ], scale factor error [Δsfx _a , Δsfy _a , Δsfz _a ], triaxial misalignment error [Δψ _ω , Δθ _ω , Δφ _ω ], bias error [ΔBix _ω ] , ΔByy _ω , ΔBiz _ω ] and scale factor errors [Δsfx _ω , Δsfy _ω , Δsfz _ω ] as unknowns, one Kalman filter may be applied.

1,2,3: IMU calibration system 11R: high precision IMU
11Ra: high-precision acceleration sensor 11Rω: high-precision angular velocity sensors 21, 21 ′, 21 ″: calibration target IMU
210, 210 ′, 210 ″: Error correction unit 211Ta: Calibration target acceleration sensor 211Tω: Calibration target angular velocity sensor 31: IMU error calculation unit 212a, 212ω: Bias error correction unit 213a, 213ω: Scale factor error correction unit 214a, 214ω: Misalignment error correction unit 215a: Lever arm effect correction unit 910: Airplane 920: Rocket

Claims (9)

A first moving mode in which a first moving body to which a high-precision IMU is mounted and a second moving body to which a calibration target IMU including a sensor having a lower accuracy than a sensor of the high-precision IMU moves are integrated; It is a movement mode following the first movement mode, and has a second movement mode in which the first moving body and the second moving body move separately,
A sensor error correction apparatus for correcting measurement data of a sensor provided in the calibration object IMU,
In the first movement mode, a bias error and a scale factor error included in the measurement data of the sensor provided in the calibration target IMU, which is continuously calculated from the measurement data of the high precision IMU and the measurement data of the calibration target IMU. And continuously correcting the measurement data of the sensor provided in the calibration target IMU using the misalignment error,
In the second movement mode, the calibration target IMU is provided using the bias error, the scale factor error, and the misalignment error obtained at the timing of transition from the first movement mode to the second movement mode. Correct the measurement data of the sensor
With an error correction unit,
Sensor error correction device. The sensor error correction device according to claim 1,
The error correction unit is
A bias error correction unit for correcting the bias error included in the measurement data of the calibration target IMU;
A scale factor error correction unit that corrects the scale factor error included in the measurement data of the calibration target IMU after the bias error correction;
A misalignment error correction unit that corrects the misalignment error included in the measurement data of the calibration target IMU after the scale factor error correction.
Sensor error correction device. The sensor error correction device according to claim 1 or 2,
The measurement data includes acceleration and angular velocity,
The error correction unit is
A lever arm effect correction unit that corrects an error included in the acceleration of the calibration target IMU caused by a difference in relative position between the measurement position of the measurement data of the high-precision IMU and the measurement position of the measurement data of the calibration target IMU; ,
Sensor error correction device. The sensor error correction device according to any one of claims 1 to 3,
The error correction unit is
The bias error, the scale factor error, and the misalignment error are corrected with orthogonal three-axis components according to the movement mode of the first moving body and the second moving body.
Sensor error correction device. A sensor error correction device according to any one of claims 1 to 4,
In the first movement mode, the bias error included in the measurement data of the sensor provided in the calibration target IMU continuously calculated from the measurement data of the high precision IMU and the measurement data of the calibration target IMU, An IMU error calculation unit that continuously calculates a scale factor error and the misalignment error;
An IMU calibration system comprising: An IMU calibration system according to claim 5, comprising:
The IMU error calculation unit is attached to the first moving body.
IMU calibration system. A first moving mode in which a first moving body to which a high-precision IMU is mounted and a second moving body to which a calibration target IMU including a sensor having a lower accuracy than a sensor of the high-precision IMU moves are integrated; It is a movement mode following the first movement mode, and has a second movement mode in which the first moving body and the second moving body move separately,
An IMU calibration method for correcting measurement data of a sensor provided in the calibration target IMU,
The bias error, the scale factor error, and the error included in the measurement data of the sensor provided in the calibration target IMU that is continuously calculated from the measurement data of the high-precision IMU and the measurement data of the calibration target IMU An IMU error calculation step of continuously calculating an alignment error;
In the first movement mode, a first error correction step of continuously correcting measurement data of a sensor provided in the calibration target IMU using the bias error, the scale factor error, and the misalignment error;
In the second movement mode, the calibration target IMU is provided using the bias error, the scale factor error, and the misalignment error obtained at the timing of transition from the first movement mode to the second movement mode. A second error correction step for correcting measurement data of the sensor,
IMU calibration method. The IMU calibration method according to claim 7, comprising:
The measurement data of the calibration target IMU includes angular velocity and acceleration,
The first and second error correction steps include
The IMU calibration method further comprising the step of correcting an error included in the acceleration of the calibration target IMU caused by a difference in relative position between the measurement position of the measurement data of the high precision IMU and the measurement position of the measurement data of the calibration target IMU. . A first moving mode in which a first moving body to which a high-precision IMU is mounted and a second moving body to which a calibration target IMU including a sensor having a lower accuracy than a sensor of the high-precision IMU moves are integrated; It is a movement mode following the first movement mode, and has a second movement mode in which the first moving body and the second moving body move separately,
An IMU calibration program for causing a computer to execute a process of correcting measurement data of a sensor provided in the calibration target IMU,
The computer
The bias error, the scale factor error, and the error included in the measurement data of the sensor provided in the calibration target IMU that is continuously calculated from the measurement data of the high-precision IMU and the measurement data of the calibration target IMU An IMU error calculation process for continuously calculating alignment errors;
In the first movement mode, a first error correction process for continuously correcting measurement data of a sensor provided in the calibration target IMU using the bias error, the scale factor error, and the misalignment error;
In the second movement mode, the calibration target IMU is provided using the bias error, the scale factor error, and the misalignment error obtained at the timing of transition from the first movement mode to the second movement mode. A second error correction process for correcting measurement data of the selected sensor;
An IMU calibration program.

Images