Moving body attitude measurement device and moving body attitude measurement program

Abstract

To reduce the effects of bias error superposition in gyro sensors and the effects of unexpected disturbances, to estimate an SF error of the gyro sensors in real time, robustly, and with high accuracy, and to calculate attitude information of a moving body in real time, robustly, and with high accuracy.SOLUTION: An SF error estimation unit 4 subtracts a calculated value ra (close to a true value ωtrue) of a "rotation speed" of a moving body using observation information of GNSS receivers R1 to R3, from a calculated value rgyro (which deviates from the true value ωtrue) of a "rotation speed" of the moving body using angular velocity information ωgyro of a gyro sensor G, to estimate an SF error rωtrue of the gyro sensor G. A process noise adjustment unit 5 adjusts a "process noise" of a state equation of a state space model related to attitude information "a" of the moving body to be larger or smaller, as a square value of the estimated SF error rωtrue of the gyro sensor G is larger or smaller, respectively.SELECTED DRAWING: Figure 4

Description

[Technical Field]

[0001] The present disclosure relates to a technique for measuring attitude information of a moving body. [Background technology]

[0002] Patent Document 1 and other publications disclose a technology for calculating the attitude information of a moving object based on angular velocity information from a gyro sensor and observation information from a GNSS receiver in a GNSS compass that complies with the performance requirements of a vessel's heading transmission device (THD) required by the International Maritime Organization (IMO). [Prior art documents] [Patent documents]

[0003] [Patent Document 1] Japanese Patent Application Publication No. 2023-136095 [Patent Document 2] Patent No. 3635145 Summary of the Invention [Problem to be solved by the invention]

[0004] The problem with the SF (Scale Factor) error calibration process in Patent Document 1 is shown in Figure 1. gyrо is the true value of the angular velocity information of the gyro sensor ω true However, there may be a deviation by the amount of the gyro sensitivity r of the gyro sensor.

[0005] Therefore, when manufacturing a GNSS compass, the GNSS compass is placed on a rotating table and rotated at a specified speed. gyrо =(1+r)ω true Measure the SF error rω of the gyro sensor true However, when the GNSS compass is in operation, the gyro sensitivity of the gyro sensor changes from r to r' due to changes in the operating environment such as temperature changes or aging of the GNSS compass, and ω gyrо =(1+r')ωtrue and the SF error of the gyro sensor rω true Even if you calibrate, the calibration error of the gyro sensor (r'-r)ω true Therefore, Patent Document 2 and the like disclose a technique for estimating and correcting the gyro sensitivity r of a gyro sensor in real time during operation of a GNSS compass.

[0006] The problem with the gyro sensitivity correction process of the prior art is shown in Figure 2. First, the measured value ω of angular velocity information of the gyro sensor gyrо is integrated over a predetermined period t1 to t2, and the time change in the attitude information of the moving object Δa gyrо On the other hand, the calculated values ​​a1 and a2 of the attitude information of the moving object using GPS are calculated at predetermined times t1 and t2, and the time change Δa GPS Next, calculate the time change Δa gyrо (deviation from the true value) and the time change in the posture information of the moving object Δa GPS (close to the true value) is subtracted to estimate and correct the gyro sensitivity r of the gyro sensor.

[0007] However, the time change in the posture information of the moving object Δa gyrо Since the bias error of the gyro sensor is superimposed on the gyro sensitivity r of the gyro sensor, the bias error of the gyro sensor is also superimposed on the gyro sensitivity r of the gyro sensor. If there is a bias error of the gyro sensor or the influence of an unexpected disturbance, an error is directly accumulated in the gyro sensitivity r of the estimated gyro sensor, and a negative transmission occurs in which the SF error of the gyro sensor using the estimated value worsens in conjunction with the error.

[0008] Therefore, in order to solve the above-mentioned problems, the present disclosure aims to reduce the influence of bias error superposition of a gyro sensor and the influence of unexpected disturbances in a GNSS compass that complies with the performance requirements for a heading transmission device (THD) required by the International Maritime Organization (IMO), estimate the SF (Scale Factor) error of the gyro sensor in real time, robustly, and with high accuracy, and calculate the attitude information of a moving body in real time, robustly, and with high accuracy. [Means for solving the problem]

[0009] To solve the above problem, the calculated value (close to the true value) of the "rotation speed" of the moving body using the observation information of the GNSS receiver is subtracted from the calculated value (which deviates from the true value) of the "rotation speed" of the moving body using the angular velocity information of the gyro sensor, and the SF error of the gyro sensor is estimated.

[0010] Specifically, the present disclosure provides a mobile body attitude measuring device comprising: an attitude calculation unit that calculates a predicted value of attitude information of the mobile body based on angular velocity information of a gyro sensor and a state equation of a state space model related to the attitude information of the mobile body, and calculates an attitude value of the attitude information of the mobile body based on observation information of a GNSS receiver and an observation equation of a state space model related to the attitude information of the mobile body; a first rotation velocity calculation unit that converts the angular velocity information of the gyro sensor into angular velocity information of a navigation coordinate system based on the attitude value of the attitude information of the mobile body and calculates the rotation velocity of the mobile body; a second rotation velocity calculation unit that calculates the rotation velocity of the mobile body based on the most recent attitude value of the attitude information of the mobile body and an even earlier attitude value of the attitude information of the mobile body; and a scale factor error estimation unit that subtracts the rotation velocity of the mobile body calculated by the second rotation velocity calculation unit from the rotation velocity of the mobile body calculated by the first rotation velocity calculation unit and estimates a scale factor error (the difference between the measured value and the true value of the angular velocity) of the gyro sensor.

[0011] With this configuration, the calculated value of the "rotation speed" of a moving object using angular velocity information from the gyro sensor is almost free of gyro sensor bias error, and the SF error of the gyro sensor is also almost free of gyro sensor bias error. Therefore, in the GNSS compass, the influence of gyro sensor bias error superposition can be reduced, and the SF error of the gyro sensor can be estimated in real time, robustly, and with high accuracy.

[0012] The present disclosure also relates to a moving body attitude measuring device, characterized in that the first rotational speed calculation unit and the second rotational speed calculation unit perform low-pass filter processing on the rotational speed of the moving body to suppress sudden high-frequency noise, and / or the scale factor error estimation unit performs low-pass filter processing on the scale factor error of the gyro sensor to suppress sudden high-frequency noise.

[0013] With this configuration, the SF error of the gyro sensor can be estimated in real time, robustly, and with high accuracy, taking into account that the SF error of the gyro sensor changes slowly over time.

[0014] To solve the above problem, the "process noise" of the state equation of the state space model related to the attitude information of the mobile object is adjusted to be larger or smaller as the squared value of the estimated SF error of the gyro sensor is larger or smaller, respectively. In other words, the larger or smaller the squared value of the estimated SF error of the gyro sensor is, the more importance is placed on the observation information of the GNSS receiver or the angular velocity information of the gyro sensor, respectively.

[0015] Specifically, the present disclosure is a moving body attitude measuring device characterized by further comprising a process noise adjustment unit that adjusts the process noise calculated by the attitude calculation unit to be larger or smaller for the state equation of a state space model related to the attitude information of the moving body, as the squared value of the scale factor error of the gyro sensor is larger or smaller, respectively.

[0016] With this configuration, even when affected by unexpected disturbances, the "process noise" (degree of importance attached to the GNSS receiver's observation information and the gyro sensor's angular velocity information) of the state equation of the state space model related to the mobile object's attitude information is optimally adjusted. Furthermore, because the effects of unexpected disturbances are not directly transmitted to the gyro sensor's gyro sensitivity, there is no negative transmission that results in a linked deterioration of the estimated gyro sensor's SF error. As a result, the GNSS compass can reduce the effects of unexpected disturbances and calculate the mobile object's attitude information in real time, robustly, and with high accuracy.

[0017] The present disclosure also provides a moving body attitude measuring device, characterized in that the process noise adjustment unit adjusts the process noise calculated by the attitude calculation unit to be larger or smaller, respectively, for a state equation of a state space model related to attitude information of the moving body, as the absolute value of the rotational speed of the moving body calculated by the second rotational speed calculation unit is larger or smaller.

[0018] This configuration takes into account changes in the SF error of the gyro sensor according to the rotational speed of the moving object, and enables the attitude information of the moving object to be calculated in real time, robustly, and with high accuracy.

[0019] The present disclosure also relates to a moving body attitude measuring device, characterized in that the second rotational speed calculation unit does not perform low-pass filter processing on the rotational speed of the moving body, does not suppress sudden high-frequency noise, and inputs it to the process noise adjustment unit.

[0020] This configuration makes it possible to detect the switching between the rotating and non-rotating states of the moving body in real time, and to calculate the posture information of the moving body in real time, robustly, and with high accuracy.

[0021] The present disclosure also provides a moving body attitude measurement program for causing a computer to execute the processing steps performed by the processing units included in the moving body attitude measurement device described above.

[0022] According to this configuration, it is possible to provide a program having the above-described effects.

[0023] The above-disclosed inventions can be combined as much as possible. [Effects of the Invention]

[0024] In this way, the present disclosure reduces the effects of gyro sensor bias error superposition and the effects of unexpected disturbances in a GNSS compass that complies with the performance requirements for heading transmission devices (THD) required by the International Maritime Organization (IMO), and enables the SF (Scale Factor) error of the gyro sensor to be estimated in real time, robustly, and with high accuracy, thereby enabling the attitude information of a moving body to be calculated in real time, robustly, and with high accuracy. [Brief explanation of the drawings]

[0025] [Figure 1] FIG. 1 is a diagram illustrating a problem with the SF error calibration process of the prior art. [Figure 2] FIG. 1 is a diagram illustrating a problem with the gyro sensitivity correction process of the prior art. [Figure 3] FIG. 10 is a diagram illustrating an overview of the SF error estimation process of the present disclosure. [Figure 4] 1 is a diagram illustrating a configuration of a moving body posture measurement device according to the present disclosure. [Figure 5] FIG. 10 is a diagram illustrating a procedure of the posture calculation process of the present disclosure. [Figure 6] FIG. 4 is a diagram showing a procedure of a first rotation speed calculation process of the present disclosure. [Figure 7] FIG. 10 is a diagram showing a procedure of a second rotation speed calculation process of the present disclosure. [Figure 8] FIG. 10 is a diagram showing the procedure of an SF error estimation process according to the present disclosure. [Figure 9] FIG. 10 is a diagram showing a procedure of a process noise adjustment process according to the present disclosure. [Figure 10] FIG. 10 is a diagram illustrating the definition of an SF error conversion coefficient according to the present disclosure. DETAILED DESCRIPTION OF THE INVENTION

[0026]

[0023] The following embodiments of the present disclosure will be described with reference to the accompanying drawings. The embodiments described below are examples of implementation of the present disclosure, and the present disclosure is not limited to the following embodiments.

[0027] (Overview of SF error estimation process of the present disclosure) An overview of the SF error estimation process of the present disclosure is shown in Fig. 3. In Fig. 3, it is assumed that the aircraft coordinate system and the navigation coordinate system are the same, and that the pitch angle and roll angle are zero. First, angular velocity information ω gyrо The calculated value of the "rotational speed" of the moving object using gyrо (True value ω true From this, the calculated value r of the "rotation speed" of the moving object using the observation information of the GNSS receiver is a (True value ω true (close to ) and subtract the SF error rω true Estimate.

[0028] Then, the angular velocity information ω of the gyro sensor gyrо The calculated value of the "rotational speed" of the moving object using gyrо Since the bias error of the gyro sensor is not superimposed, the SF error rω of the gyro sensor true Therefore, in the GNSS compass, the influence of the bias error superimposition of the gyro sensor is reduced, and the SF error rω of the gyro sensor is true can be estimated in real time, robustly, and with high accuracy.

[0029] Next, the estimated SF error rω of the gyro sensor true The larger or smaller the squared value of is, the larger or smaller the "process noise" of the state equation of the state space model related to the attitude information a of the moving object is adjusted to be. In other words, the estimated SF error rω of the gyro sensor is true The larger or smaller the squared value of ω, the better the GNSS receiver observation information or the gyro sensor angular velocity information ω gyrо We place importance on:

[0030] Then, even under the influence of unexpected disturbances, the "process noise" of the state equation of the state space model for the attitude information a of the moving object (observation information of the GNSS receiver and angular velocity information ω gyrо The degree of importance of the gyro sensor is optimally adjusted. Since the influence of unexpected disturbances is not directly transmitted to the gyro sensor's gyro sensitivity r, the estimated SF error rωtrue Therefore, the GNSS compass can reduce the influence of unexpected disturbances and calculate the attitude information a of the moving object in real time, robustly, and with high accuracy.

[0031] The configuration of a mobile object attitude measurement device according to the present disclosure is shown in Fig. 4. Mobile object attitude measurement device D is mounted on a ship or the like, and is connected to receiving antennas A1 to A3, GNSS receivers R1 to R3, and a gyro sensor G, and includes an attitude calculation unit 1, a first rotation speed calculation unit 2, a second rotation speed calculation unit 3, an SF error estimator 4, and a process noise adjustment unit 5. Mobile object attitude measurement device D can be realized by installing the mobile object attitude measurement programs shown in Figs. 5 to 9 on a computer.

[0032] Below, the posture calculation process, the first rotation speed calculation process, the second rotation speed calculation process, the SF error estimation process, and the process noise adjustment process will be described as procedures of the present disclosure.

[0033] (Procedure of attitude calculation processing of the present disclosure) The procedure of the attitude calculation process of the present disclosure is shown in Fig. 5. First, the attitude calculation unit 1 calculates the angular velocity information ω gyrо =[ω x ω y ω z ] T A predicted value of the attitude information of the moving object is calculated based on [dps] and a state equation of a state space model related to the attitude information of the moving object (step S1, see Patent Document 1). x , ω y , ω z is the angular velocity information of the gyro sensor G in the x, y, and z directions of the aircraft body coordinate system of the gyro sensor G.

[0034] Next, the attitude calculation unit 1 calculates the attitude value a=[ψ θ φ] of the attitude information of the moving object based on the observation information of the GNSS receivers R1 to R3 and the observation equation of the state space model related to the attitude information of the moving object. T [deg] is calculated (step S2, see Patent Document 1). ψ, θ, and φ are attitude information of the moving body in the yaw, pitch, and roll directions of the navigation coordinate system of the moving body.

[0035] Furthermore, the attitude calculation unit 1 calculates the angular velocity information ω gyrо Based on the observation information of the GNSS receivers R1 to R3, the bias b of the gyro sensor G is calculated as b x b y b z ] T Calculate [dps] (Step S3, see Patent Document 1). x , b y , b z are the biases of the gyro sensor G in the x, y, and z directions of the aircraft coordinate system of the gyro sensor G.

[0036] Here, the attitude calculation unit 1 calculates the SF error rω of the gyro sensor G. true The larger or smaller the squared value of [dps] is, the larger or smaller the calculated process noise is adjusted to be for the state equation of the state space model relating to the attitude information of the moving object (step S41, described later).

[0037] (Procedure of the first rotation speed calculation process of the present disclosure) 6 shows the procedure of the first rotation speed calculation process of the present disclosure. First, the first rotation speed calculation unit 2 calculates, in the coordinate conversion unit 21, angular speed information ω of the gyro sensor G based on the attitude value a of the attitude information of the moving body and the bias b of the gyro sensor G. gyrо is converted into angular velocity information in the navigation coordinate system, and the rotational velocity r gyrо [dps] is calculated (step S11, see equations 1 and 2). G is a transformation matrix from the aircraft coordinate system of the gyro sensor G to the navigation coordinate system of the moving body.

number

number

[0038] Next, the first rotation speed calculation unit 2 calculates the rotation speed r of the moving body in the noise suppression unit 22. gyrоLow-pass filtering is performed on the moving object to suppress sudden high-frequency noise, and the rotation speed r gyrо_LPF [dps] is output (see step S12, equation 3). LPF_gyrо is, for example, a first-order low-pass filter function, and the left side r gyrо_LPF is the current value, and the right-hand side r gyrо_LPF is the previous value, and K gyrо is the gain based on the filter time constant. f LPF_gyrо may apply a high-order low-pass filter function.

number

[0039] However, the first rotation speed calculation unit 2 uses the angular speed information ω gyrо If either the attitude value a of the attitude information of the moving object or the bias b of the gyro sensor G cannot be input, the rotation speed r of the moving object will gyrо_LPF Do not output.

[0040] (Procedure of second rotation speed calculation process of the present disclosure) The procedure of the second rotation speed calculation process of the present disclosure is shown in Fig. 7. First, the second rotation speed calculation unit 3 calculates the most recent attitude value a of the attitude information of the moving body in the discrete differentiation unit 31. t and the previous attitude value a t-Δt Based on this, the rotational speed r of the moving object a [dps] is calculated (step S21, see equation 4). Δt [s] is the calculation interval for the attitude information of the moving object.

number

[0041] However, the second rotation speed calculation unit 3 calculates the error σ of the attitude value a of the attitude information of the moving body in the discrete differentiation unit 31. 2 =[σ ψ 2 σ θ 2 σ φ 2 ]T [deg 2 ] is below the threshold value, the rotation speed r a (Step S21). That is, the error σ of the attitude value a of the attitude information of the moving object is output. 2 If is small, the attitude value a of the attitude information of the moving object is almost accurate, and the rotation speed r of the moving object a The error σ of the attitude value a of the moving object's attitude information is also almost accurate. 2 The values ​​are large immediately after power-on, immediately after GNSS signal recovery, and in poor GNSS observation environments.

[0042] Next, the second rotation speed calculation unit 3 calculates the rotation speed r of the moving body in the noise suppression unit 32. a Low-pass filtering is performed on the moving object to suppress sudden high-frequency noise, and the rotation speed r a_LPF [dps] is output (see step S22, equation 5). LPF_a is, for example, a first-order low-pass filter function, and the left side r a_LPF is the current value, and the right-hand side r a_LPF is the previous value, and K a is the gain based on the filter time constant. f LPF_a may apply a high-order low-pass filter function.

number

[0043] However, the second rotation speed calculation unit 3 uses the most recent attitude value a t and the previous attitude value a t-Δt and the error σ of the attitude value a of the moving object's attitude information 2 If you cannot input any one of the above, the rotation speed r a_LPF Do not output.

[0044] (Procedure of SF error estimation process of the present disclosure) The procedure of the SF error estimation process of the present disclosure is shown in Fig. 8. First, the SF error estimation unit 4 subtracts the rotation speed r of the moving body calculated by the first rotation speed calculation unit 2 in a subtraction unit 41. gyrо_LPFThe rotation speed r of the moving body calculated by the second rotation speed calculation unit 3 is calculated from a_LPF Subtract the SF error r of the gyro sensor G. ~ ω true ~ [dps] is estimated (step S31, see equation 6). ~ is the estimated gyro sensitivity of the gyro sensor G, and ω true ~ is the estimated true value of the rotational speed of the moving object.

number

[0045] Here, the SF error estimation unit 4 subtracts the rotation speed r of the moving body in the subtraction unit 41. gyrо_LPF and the rotational speed r of the moving object a_LPF This is because if the denominator of the ratio calculation is close to 0, the ratio will suddenly become an abnormal value when noise is superimposed on the denominator. Therefore, the SF error estimation unit 4 subtracts the rotation speed r of the moving body in the subtraction unit 41. gyrо_LPF and the rotational speed r of the moving object a_LPF The "difference" between the two is calculated. This means that the difference will not be a sudden outlier and follows a Gaussian distribution, making it easy to handle statistically.

[0046] Next, the SF error estimation unit 4 calculates the SF error r ~ ω true ~ Low-pass filtering is performed on the gyro sensor G to suppress sudden high-frequency noise, and the SF error s LPF [dps] is output (see step S32, equation 7). LPF_s is, for example, a first-order low-pass filter function, and the left side s LPF is the current value, and s on the right side LPF is the previous value, and K s is the gain based on the filter time constant. f LPF_s may apply a high-order low-pass filter function.

number

[0047] However, the SF error estimation unit 4 uses the rotation speed r of the moving body calculated by the first rotation speed calculation unit 2 gyrо_LPF and the rotation speed r of the moving body calculated by the second rotation speed calculation unit 3. a_LPF If any one of these cannot be input, the SF error s of the gyro sensor G LPF Do not output.

[0048] Angular velocity information ω of gyro sensor G gyrо The calculated rotational speed of the moving object using r gyrо_LPF Since the bias b of the gyro sensor G is hardly superimposed, the SF error s of the gyro sensor G LPF Therefore, in the GNSS compass, the influence of the bias b of the gyro sensor G is reduced, and the SF error s of the gyro sensor G is LPF can be estimated in real time, robustly, and with high accuracy.

[0049] Considering that the time change of the SF error of the gyro sensor G is slow, the SF error s LPF can be estimated in real time, robustly, and with high accuracy.

[0050] (Procedure for process noise adjustment processing of the present disclosure) The procedure of the process noise adjustment processing of the present disclosure is shown in Fig. 9. First, the process noise adjustment unit 5 calculates the SF error s LPF The larger or smaller the squared value of , the more the adjustment amount q for the process noise calculated by the attitude calculation unit 1 for the state equation of the state space model related to the attitude information of the moving object becomes. SF [deg 2 ] is adjusted to be larger or smaller (step S41, see equation 8). k is a conversion coefficient of the SF error, and Δt is a calculation interval of the attitude information of the moving object = SF error s LPF is the accumulation time, n is the noise magnification, and k and n will be described later.

number

[0051] The process noise adjustment unit 5 also adjusts the rotation speed r of the moving body calculated by the second rotation speed calculation unit 3. a (r a_LPF The larger or smaller the absolute value of (rather than ) is, the more the adjustment amount q for the process noise calculated by the attitude calculation unit 1 for the state equation of the state space model related to the attitude information of the moving object is. SF are adjusted to be larger or smaller (step S41, see equation 9). max is the upper limit of the SF error conversion coefficient k, and Th [dps] is the rotation speed r a is the threshold value.

number

[0052] The definition of the SF error conversion coefficient in this disclosure is shown in Figure 10. The rotation speed of the moving body |r a When | is low and is less than the threshold Th, the SF error s of the gyro sensor G LPF is considered to be small, the SF error conversion coefficient k is set to a small value of k→0.

[0053] Rotational speed of the moving object |r a When | is fast but is less than the threshold Th, the SF error s of the gyro sensor G LPF is considered to be large and is considered to be small compared to the true value of the SF error of the gyro sensor G, so the conversion coefficient k of the SF error is set to a large value of k>1. Here, the SF error s of the gyro sensor G LPF The reason why it is considered that the SF error of the gyro sensor G is smaller than the true value is as follows: The attitude value a of the attitude information of the moving body includes the angular velocity information ω gyrо is taken into account, the rotation speed r a_LPF is the rotational speed r of the moving object. gyrо_LPF Although not as large as the actual value, the SF error of the gyro sensor G is slightly superimposed, so the result of equation 6 is calculated to be smaller than the actual true value.

[0054] Rotational speed of the moving object |r a When | is high and is equal to or greater than the threshold Th, the rotation speed |r a |(|r a_LPF In order to suppress high frequency noise (rather than |), or to suppress the upper limit of the process noise (even if the process noise is excessive, it does not affect the attitude value a of the attitude information of the moving object), the SF error conversion coefficient k is k = k max is set to

[0055] The noise magnification n is the ratio of the process noise calculated by the process noise adjustment unit 5 to the process noise calculated by the attitude calculation unit 1 (s LPF ·k·Δt) 2 is the degree of importance that the attitude calculation unit 1 should attach to the rotation speed |r a It is sufficient if it is set to a constant value regardless of |.

[0056] In Equation 9, the second rotation speed calculation unit 3 calculates the rotation speed r of the moving body as a is input to the process noise adjustment unit 5 without performing low-pass filtering and without suppressing sudden high-frequency noise. This makes it possible to detect the switching between the rotating state and the non-rotating state of the moving body in real time and to calculate the attitude information of the moving body in real time, robustly, and with high accuracy.

[0057] Unlike Equation 9, the second rotation speed calculation unit 3 calculates the rotation speed r a may be subjected to low-pass filtering to suppress sudden high-frequency noise, and then input to the process noise adjustment unit 5. Then, the rotation speed |r a_LPF |In turn, this makes it possible to suppress sudden changes in process noise and calculate the attitude information of a moving object in real time, robustly, and with high accuracy.

[0058] In Equation 8, the process noise adjustment unit 5 SF =(s LPF ·k·Δt·n) 2 Unlike Equation 8, the process noise adjustment unit 5 calculates qSF =(s LPF ·Δt) 2 , q SF =(s LPF ·k·Δt) 2 , or q SF =(s LPF ·Δt·n) 2 may be calculated.

[0059] Next, the process noise adjustment unit 5 adjusts the state equation of the state space model relating to the attitude information of the moving object by adjusting the process noise calculated by the attitude calculation unit 1 by an adjustment amount q SF Then, the attitude calculation unit 1 adds the amount of adjustment q SF The result is applied in the "next" period of the attitude calculation of the moving body.

[0060] However, the process noise adjustment unit 5 adjusts the SF error s of the gyro sensor G estimated by the SF error estimation unit 4. LPF and the rotation speed r of the moving body calculated by the second rotation speed calculation unit 3. a If any one of these cannot be input, the process noise adjustment amount q SF and outputs 0.

[0061] Even under the influence of unexpected disturbances, the process noise of the state equation of the state space model related to the attitude information of the moving object (the observation information of the GNSS receivers R1 to R3 and the angular velocity information ω gyrо Since the influence of unexpected disturbances is not directly transmitted to the gyro sensitivity r of the gyro sensor G, the estimated SF error s of the gyro sensor G is LPF Therefore, the GNSS compass can reduce the influence of unexpected disturbances and calculate the attitude information of a moving object in real time, robustly, and with high accuracy.

[0062] And the rotational speed of the moving object r a SF error s of gyro sensor G according toLPF This allows us to calculate the attitude information of a moving object in real time, robustly, and with high accuracy by taking into account changes in the rotational and non-rotating states of the moving object. Furthermore, we can detect the transition between the rotating and non-rotating states of the moving object in real time, and calculate the attitude information of the moving object in real time, robustly, and with high accuracy. [Industrial Applicability]

[0063] The mobile body attitude measurement device and mobile body attitude measurement program disclosed herein can reduce the influence of bias error superposition of the gyro sensor and the influence of unexpected disturbances, estimate the SF (Scale Factor) error of the gyro sensor in real time, robustly, and with high accuracy, and calculate the attitude information of the mobile body in real time, robustly, and with high accuracy. [Explanation of symbols]

[0064] D: Mobile object attitude measurement device A1~A3: Receiving antennas R1~R3: GNSS receiver G: Gyro sensor 1: Posture calculation section 2: First rotation speed calculation section 3: Second rotation speed calculation section 4: SF error estimation part 5: Process noise adjustment section 21: Coordinate conversion section 22: Noise suppression section 31: Discrete differential part 32: Noise suppression section 41: Subtraction section 42: Noise suppression section

Claims

1. an attitude calculation unit that calculates a predicted value of attitude information of the moving body based on angular velocity information of the gyro sensor and a state equation of a state space model related to attitude information of the moving body, and calculates an attitude value of the attitude information of the moving body based on observation information of the GNSS receiver and an observation equation of a state space model related to the attitude information of the moving body; a first rotation speed calculation unit that converts angular speed information of the gyro sensor into angular speed information in a navigation coordinate system based on an attitude value of the attitude information of the moving body, and calculates a rotation speed of the moving body; a second rotation speed calculation unit that calculates a rotation speed of the moving body based on a most recent attitude value of the attitude information of the moving body and an even earlier attitude value of the attitude information of the moving body; a scale factor error estimating unit that subtracts the rotational velocity of the moving body calculated by the second rotational velocity calculating unit from the rotational velocity of the moving body calculated by the first rotational velocity calculating unit and estimates a scale factor error of the gyro sensor (a difference between a measured value of an angular velocity and a true value); A moving object attitude measurement device comprising:

2. The first rotation speed calculation unit and the second rotation speed calculation unit perform low-pass filtering on the rotation speed of the moving body to suppress sudden high-frequency noise, and / or the scale factor error estimation unit performs low-pass filtering on the scale factor error of the gyro sensor to suppress sudden high-frequency noise.

2. The moving body attitude measurement device according to claim 1, wherein:

3. a process noise adjustment unit that adjusts the process noise calculated by the attitude calculation unit to be larger or smaller as the squared value of the scale factor error of the gyro sensor becomes larger or smaller, respectively, for a state equation of a state space model related to attitude information of the moving body; 3. The moving body attitude measurement device according to claim 1, further comprising:

4. The process noise adjustment unit adjusts the process noise calculated by the attitude calculation unit to be larger or smaller, respectively, for a state equation of a state space model related to attitude information of the moving body, as the absolute value of the rotation speed of the moving body calculated by the second rotation speed calculation unit is larger or smaller.

4. The moving body attitude measurement device according to claim 3.

5. The second rotation speed calculation unit does not perform low-pass filtering on the rotation speed of the moving body, does not suppress sudden high-frequency noise, and inputs the rotation speed to the process noise adjustment unit.

5. The moving body attitude measurement device according to claim 4.

6. 10. A moving object attitude measurement program for causing a computer to execute each processing step performed by each processing unit included in the moving object attitude measurement device according to claim 1.

Images