Mobile object posture measurement device and mobile object posture measurement program

Abstract

To reduce an estimation error of a posture value of a mobile object in a GNSS compass, even if bias of a gyro sensor drifts due to a change in temperature of the gyro sensor over time when GNSS positioning navigation is disabled for a long period of time.SOLUTION: Based on state equations of a state-space model for bias temperature gradient r and bias prescribed temperature intercept b0 of a gyro sensor G and observation equations of the state-space model for bias b, bias variance σb, temperature T, the bias temperature gradient r and the bias prescribed temperature intercept b0 of the gyro sensor G, the bias temperature gradient r of the gyro sensor G is estimated.SELECTED DRAWING: Figure 4

Description

[Technical field]

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

[0002] Patent Documents 1 and 2 disclose techniques for calculating or estimating attitude information of a mobile object equipped with a GNSS receiver and a gyro sensor based on observation information from a GNSS receiver or angular velocity information from a gyro sensor.

[0003] The configuration of a conventional mobile body attitude measuring device is shown in Figures 1 and 2. A conventional mobile body attitude measuring device D is mounted on a ship or the like, connected to receiving antennas A1-A3, GNSS receivers R1-R3, a gyro sensor G and a temperature sensor T, and includes an attitude calculation unit 1, an attitude estimation unit 2 and an attitude switching unit 3. Figures 1 and 2 show the conventional technology disclosed in Patent Documents 1 and 2.

[0004] The attitude calculation unit 1 executes GNSS positioning navigation when GNSS positioning navigation is possible (satellite signals are received and the number of captured satellites is sufficient). That is, the attitude calculation unit 1 calculates the attitude value a^ of a moving object equipped with a GNSS receiver based on the observation information of the GNSS receiver. Then, the attitude calculation unit 1 calculates the bias b^ of the gyro sensor G based on the observation information of the GNSS receiver and the angular velocity ω of the gyro sensor G.

[0005] The attitude estimation unit 2 executes dead reckoning navigation in a GNSS positioning navigation unavailable state (a state where satellite signals are blocked or the number of captured satellites is insufficient). That is, the attitude estimation unit 2 estimates the attitude value a' of the moving body equipped with the gyro sensor G based on the angular velocity ω of the gyro sensor G. Here, the attitude estimation unit 2 estimates the attitude value a' of the moving body in the current GNSS positioning navigation unavailable state based on the angular velocity ω and bias b^ (most recently calculated by the attitude calculation unit 1) of the gyro sensor G and the attitude value a^ of the moving body in the most recent GNSS positioning navigation available state.

[0006] In a state where GNSS positioning navigation is possible, the attitude switching unit 3 outputs the attitude value a^ of the moving body as the attitude value a of the moving body. In a state where GNSS positioning navigation is not possible, the attitude switching unit 3 outputs the attitude value a' of the moving body as the attitude value a of the moving body. [Prior art documents] [Patent documents]

[0007] [Patent Document 1] Japanese Patent Application Publication No. 04-364422 [Patent Document 2] JP 2002-303533 A [Patent Document 3] JP 2023-136095 A [Patent Document 4] Patent No. 3635145 Summary of the Invention [Problem to be solved by the invention]

[0008] 3, when the temperature T of the gyro sensor G changes over time, the bias b of the gyro sensor G drifts. Therefore, if the attitude estimation unit 2 does not correct the drift of the bias b of the gyro sensor G with respect to the time integration of the angular velocity ω of the gyro sensor G when the GNSS positioning navigation unavailable state continues for a long period of time, the estimation error of the attitude value a' of the moving body will increase.

[0009] Therefore, in FIG. 1, the attitude estimation unit 2 uses a temperature characteristic map to obtain the drift of the bias b of the gyro sensor G and correct the angular velocity ω. Here, the temperature characteristic map takes the temperature T of the gyro sensor G as an input and the drift of the bias b of the gyro sensor G as an output. However, the temperature characteristic map is created or updated based on the equivalence of the angular velocity ω (originally 0) and bias b of the gyro sensor G (see the lower part of FIG. 3) when the moving object is in a stationary state. Therefore, the temperature characteristic map cannot be created or updated when the moving object is in a non-stationary state, and when the stationary time is short, the granularity of the temperature characteristic map becomes coarse, causing a correction error. In addition, the temperature characteristic map superimposes noise on the output of the drift of the bias b of the gyro sensor G due to noise superimposed on the input of the temperature T of the gyro sensor G. Therefore, when the stationary time is short or the temperature error is large, an error occurs in the correction of the angular velocity ω, which increases the estimation error of the attitude value a' of the moving object.

[0010] On the other hand, in FIG. 2, the attitude estimation unit 2 estimates the temperature gradient of the bias b of the gyro sensor G using a Kalman filter, and corrects the angular velocity ω with the drift of the bias b calculated based on the estimated temperature gradient. Here, the Kalman filter receives the temperature T of the gyro sensor G and the difference amount (amount of change over a short time) of the bias b. Then, the temperature gradient of the bias b is predicted and updated by the Kalman filter based on the angular velocity ω (originally 0) of the gyro sensor G and the equivalence of the bias b (see the lower part of FIG. 3) when the moving object is in a stationary state. Therefore, the temperature gradient of the bias b cannot be predicted and updated when the moving object is in a non-stationary state, and when the stationary time is short, the temperature gradient of the bias b cannot be obtained with sufficient accuracy. Furthermore, due to noise superimposed on the temperature T and bias b of the gyro sensor G, the difference amount (amount of change over a short time) or the ratio between the difference amounts (temperature gradient of the bias b) set in the observation equation of the state space model is superimposed with noise based on the additivity of variance, and the error of the estimated temperature gradient increases. Therefore, when the rest time is short or when the errors in the temperature T and bias b are large, a correction error occurs in the angular velocity ω, and the estimation error in the attitude value a′ of the moving body increases.

[0011] Therefore, in order to solve the above problem, the present disclosure aims to reduce the estimation error of the attitude value of a moving body in a GNSS compass that complies with the performance requirements for a heading transmission device (THD) required by the International Maritime Organization (IMO), even if the bias of the gyro sensor drifts due to a change in temperature of the gyro sensor over time when GNSS positioning navigation is unavailable for a long period of time, regardless of whether the moving body is in a non-stationary or stationary state. [Means for solving the problem]

[0012] In order to solve the above problem, the bias temperature gradient of the gyro sensor is estimated based on a state equation of a state space model relating to the bias temperature gradient and bias specified temperature intercept of the gyro sensor, and an observation equation of a state space model relating to the bias, bias variance, temperature, bias temperature gradient, and bias specified temperature intercept of the gyro sensor.

[0013] Specifically, the present disclosure provides a method and device for detecting a GNSS positioning navigation error by using a GNSS receiver and a GNSS receiver-mounted mobile station, comprising: an attitude calculation unit that calculates attitude information of a mobile station equipped with a GNSS receiver based on observation information of the GNSS receiver when GNSS positioning navigation is possible; an attitude estimation unit that estimates attitude information of the mobile station equipped with the gyro sensor based on angular velocity information of the gyro sensor when GNSS positioning navigation is not possible; and a state equation of a state space model relating to a bias temperature gradient and a bias predetermined temperature intercept of the gyro sensor, and a state equation of a state space model relating to a bias, bias variance, temperature, bias temperature gradient, and bias predetermined temperature intercept of the gyro sensor. and a bias time change estimation unit that estimates a bias time change of the gyro sensor based on the bias temperature gradient and temperature time gradient of the gyro sensor, wherein the attitude estimation unit estimates attitude information of the moving body in the current GNSS positioning navigation unavailable state based on angular velocity information, bias, and bias time change of the gyro sensor, and attitude information of the moving body in the most recent GNSS positioning navigation available state.

[0014] According to this configuration, the temperature and bias of the gyro sensor are set in the observation equation of the state space model for estimating the bias temperature gradient, and the bias variance of the gyro sensor is used as the prior information of the observation noise. Therefore, in the GNSS compass, when the GNSS positioning navigation is unavailable for a long period of time, even if the temperature of the gyro sensor changes over time, the bias temperature gradient of the gyro sensor can be estimated with high accuracy. And even if the bias of the gyro sensor drifts, the estimation error of the attitude value of the moving body can be reduced.

[0015] The present disclosure also relates to a mobile body attitude measuring device, characterized in that the attitude calculation unit calculates the bias and bias variance of the gyro sensor based on the observation information of the GNSS receiver and the angular velocity information of the gyro sensor, and further includes a bias switching unit that outputs the bias and bias variance of the gyro sensor calculated by the attitude calculation unit to the bias temperature gradient estimation unit as the bias and bias variance of the gyro sensor, respectively, in an angular velocity change state of the angular velocity information of the gyro sensor.

[0016] According to this configuration, the observation equation of the state space model for bias temperature gradient estimation is switched to and set to the bias of the gyro sensor calculated in a state where GNSS positioning navigation is possible when the moving object is in a non-stationary state, and further, the prior information of the observation noise is switched to and used to the bias variance of the gyro sensor calculated in a state where GNSS positioning navigation is possible. Therefore, even if the bias of the gyro sensor drifts due to a change in the temperature of the gyro sensor over time when the GNSS positioning navigation is unavailable for a long period of time in the GNSS compass, it is possible to reduce the estimation error of the attitude value of the moving object when the moving object is in a non-stationary state.

[0017] The present disclosure also relates to a moving body attitude measuring device, further comprising a bias switching unit that outputs the angular velocity information of the gyro sensor and the bias variance in a stationary state to the bias temperature gradient estimation unit as the bias and bias variance of the gyro sensor, respectively, when the angular velocity of the angular velocity information of the gyro sensor is in a constant angular velocity state.

[0018] According to this configuration, the observation equation of the state space model for bias temperature gradient estimation is switched to and set to the angular velocity of the gyro sensor (equivalent to the bias of the gyro sensor) when the mobile body is stationary or moving with a constant attitude angle, and further the prior information of the observation noise is switched to and used to the angular velocity variance of the gyro sensor (equivalent to the bias variance of the gyro sensor). Therefore, in the GNSS compass, even if the bias of the gyro sensor drifts due to a change in the temperature of the gyro sensor over time when the GNSS positioning navigation is unavailable for a long period of time, the estimation error of the attitude value of the mobile body can be reduced when the mobile body is stationary or moving with a constant attitude angle.

[0019] The present disclosure also relates to a moving body attitude measuring device, further comprising a temperature-time gradient estimation unit that estimates the temperature-time gradient of the gyro sensor based on a state equation of a state space model relating to the temperature of the gyro sensor, a temperature-time gradient, and process noise with respect to the temperature-time gradient, and an observation equation of a state space model relating to the temperature of the gyro sensor and observation noise with respect to the temperature, and outputs the temperature-time gradient to the bias time change estimation unit.

[0020] According to this configuration, the temperature of the gyro sensor is set in the observation equation of the state space model for temperature time gradient estimation, and the prior information of the observation noise uses noise information for the temperature of the gyro sensor. Therefore, even if the temperature of the gyro sensor changes over time, the temperature time gradient of the gyro sensor can be estimated with high accuracy.

[0021] The present disclosure also relates to a moving body attitude measuring device, characterized in that the bias temperature gradient estimation unit estimates the bias temperature gradient of the gyro sensor based on a state equation of a state space model that takes into account the forgetting effect over time regarding the bias temperature gradient and bias specified temperature intercept of the gyro sensor.

[0022] According to this configuration, even if the relationship between the temperature and bias of the gyro sensor gradually changes nonlinearly over time, the bias temperature gradient can be estimated with high accuracy.

[0023] The present disclosure also relates to a moving body attitude measuring device, characterized in that the bias temperature gradient estimation unit determines that the bias of the gyro sensor is normal (abnormal) when the observation residual normalized by the covariance of the observation residual is less than (or greater than) a predetermined threshold, and performs (cancels) estimation of the bias temperature gradient of the gyro sensor.

[0024] According to this configuration, even if the bias of the gyro sensor temporarily becomes abnormal due to the influence of external disturbances or the like, it is possible to determine whether the bias of the gyro sensor is normal or abnormal, and to prevent the transmission of adverse effects to the estimation of the bias temperature gradient in advance.

[0025] To solve further problems, the gyro sensitivity of the gyro sensor is estimated by dividing 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 by the calculated value (which is closer to the true value) of the "rotation speed" of the moving body using the observation information of the GNSS receiver as well.

[0026] Specifically, the present disclosure provides a moving body attitude measuring device characterized in that the attitude calculation unit calculates a predicted value of the 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 the attitude information of the moving body, 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, 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 moving body, and calculates a rotational speed of the moving body, a second rotational speed calculation unit calculates the rotational speed of the moving body based on the 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, and a gyro sensitivity estimation unit divides the rotational speed of the moving body calculated by the first rotational speed calculation unit by the rotational speed of the moving body calculated by the second rotational speed calculation unit, and estimates a gyro sensitivity of the gyro sensor (the ratio between the measured value and true value of angular velocity minus 1).

[0027] According to this configuration, the calculated value of the "rotation speed" of the moving body using the angular velocity information of the gyro sensor is almost not superimposed with the bias error of the gyro sensor, and therefore the gyro sensitivity of the gyro sensor is also almost not superimposed with the bias error of the gyro sensor. Therefore, in the GNSS compass, the influence of the bias error superimposition of the gyro sensor can be reduced, and the gyro sensitivity of the gyro sensor can be estimated in real time, robustly, and with high accuracy.

[0028] 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 gyro sensitivity estimation unit performs low-pass filter processing on the gyro sensitivity of the gyro sensor to suppress sudden high-frequency noise.

[0029] According to this configuration, taking into consideration that the gyro sensitivity of the gyro sensor changes slowly over time, the gyro sensitivity of the gyro sensor can be estimated in real time, robustly, and with high accuracy.

[0030] The present disclosure also relates to a moving body attitude measuring device, characterized in that the attitude estimation unit corrects the attitude information of the moving body based on the angular velocity information and gyro sensitivity of the gyro sensor when GNSS positioning navigation is unavailable.

[0031] According to this configuration, the calculated value of the "rotation speed" of the moving object using the angular velocity information of the gyro sensor is almost not superimposed with the bias error of the gyro sensor, and the gyro sensitivity of the gyro sensor is also almost not superimposed with the bias error of the gyro sensor. Therefore, in the GNSS compass, the influence of the bias error superimposition of the gyro sensor is reduced, and the attitude information of the moving object can be calculated in real time, robustly, and with high accuracy.

[0032] The present disclosure also provides a moving object attitude measuring program for causing a computer to execute each processing step performed by each processing unit included in the moving object attitude measuring device described above.

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

[0034] The above disclosed inventions can be combined to the greatest extent possible. Effect of the Invention

[0035] In this way, in a GNSS compass that complies with the performance requirements for a heading transmission device (THD) required by the International Maritime Organization (IMO), the present disclosure can reduce the estimation error of the attitude value of a moving body, regardless of whether the moving body is in a stationary or non-stationary state, even if the bias of the gyro sensor drifts due to a change in temperature of the gyro sensor over time when GNSS positioning navigation is unavailable for a long period of time. [Brief description of the drawings]

[0036] [Figure 1] FIG. 1 is a diagram showing a configuration of a moving object attitude measuring device according to a conventional technique. [Diagram 2] FIG. 1 is a diagram showing a configuration of a moving object attitude measuring device according to a conventional technique. [Diagram 3] FIG. 1 is a diagram showing the temperature change of the bias of a gyro sensor of the prior art. [Figure 4] 1 is a diagram showing a configuration of a moving object posture measuring device according to the present disclosure. [Diagram 5] FIG. 2 is a diagram showing a procedure of a moving object attitude measurement process according to the present disclosure. [Figure 6] FIG. 4 is a diagram showing a procedure of bias switching processing according to the present disclosure. [Figure 7] FIG. 11 is a diagram showing a procedure of a bias temperature gradient estimation process according to the present disclosure. [Figure 8] FIG. 11 is a diagram showing the procedure of a temperature-time gradient estimation process according to the present disclosure. [Figure 9]FIG. 11 is a diagram showing a procedure of bias time change estimation processing and attitude estimation processing according to the present disclosure. [Figure 10] FIG. 13 is a diagram showing a procedure of a process according to the present disclosure that takes into account the forgetting effect over time. [Figure 11] FIG. 1 is a diagram showing the procedure of a process for determining whether a bias of the present disclosure is normal or abnormal. [Figure 12] FIG. 1 illustrates a problem with the gyro sensitivity calibration process of the prior art. [Figure 13] 1A and 1B are diagrams illustrating problems with the gyro sensitivity correction process of the conventional technology. [Figure 14] FIG. 2 is a diagram showing an overview of a gyro sensitivity estimation process according to the present disclosure. [Figure 15] 1 is a diagram showing a configuration of a moving object posture measuring device according to the present disclosure. [Figure 16] FIG. 13 is a diagram showing a procedure of the attitude calculation process of the present disclosure. [Figure 17] FIG. 4 is a diagram showing a procedure of a first rotation speed calculation process according to the present disclosure. [Figure 18] FIG. 11 is a diagram showing a procedure of a second rotation speed calculation process according to the present disclosure. [Figure 19] FIG. 4 is a diagram showing a procedure of a gyro sensitivity estimation process according to the present disclosure. [Figure 20] FIG. 4 is a diagram showing a procedure of a gyro sensitivity correction process according to the present disclosure. DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

[0037] DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS The present disclosure will be described with reference to the accompanying drawings. The embodiments described below are examples of the present disclosure, and the present disclosure is not limited to the following embodiments.

[0038] (Configuration of moving object attitude measuring device according to the present disclosure) The configuration of a moving body attitude measuring device according to the present disclosure is shown in Fig. 4. A moving body attitude measuring device D according to the present disclosure is mounted on a ship or the like, and is connected to receiving antennas A1-A3, GNSS receivers R1-R3, a gyro sensor G and a temperature sensor T, and includes an attitude calculation unit 1, an attitude estimation unit 2, an attitude switching unit 3, a bias switching unit 4, a bias temperature gradient estimator 5, a temperature time gradient estimator 6 and a bias time change estimator 7. The moving body attitude measuring device D according to the present disclosure can be realized by installing the moving body attitude measuring programs shown in Figs. 5-11 on a computer.

[0039] First, the bias temperature gradient estimation unit 5 receives the temperature T of the gyro sensor G and the bias b output by the bias switching unit 4, and calculates the bias variance σ b Since a state space model is constructed with the above as prior information of the observation noise, the bias temperature gradient r of the gyro sensor G can be estimated with high accuracy. Therefore, by using the highly accurate bias temperature gradient r, even if the temperature T of the gyro sensor G changes over time and the bias b of the gyro sensor G drifts when the GNSS positioning navigation is unavailable for a long period of time, the attitude value a of the moving object can be estimated with high accuracy. ~ This can reduce the estimation error.

[0040] Here, the bias switching unit 4 switches to output the bias b^ of the gyro sensor G calculated in the GNSS positioning navigation available state when the moving body is not stationary, and outputs the bias variance σ of the gyro sensor G calculated in the GNSS positioning navigation available state. b ^ is switched to output as prior information of the observation noise. Therefore, even when the moving object is not stationary, the bias temperature gradient estimation unit 5 can estimate the bias temperature gradient r, and when the GNSS positioning navigation becomes unavailable after that, the bias temperature gradient r most recently estimated can be used. Therefore, the attitude value a ~ This can reduce the estimation error.

[0041] On the other hand, the bias switching unit 4 switches to output the angular velocity ω of the gyro sensor G (equivalent to the bias b of the gyro sensor G) when the moving object is in a stationary state or in a moving state with a constant attitude angle, and outputs the angular velocity variance σ of the gyro sensor G. ω (Gyro sensor G bias variance σ b Therefore, even when the GNSS positioning and navigation is unavailable for a long period of time, if the moving object is stationary or moving with a constant attitude angle, the bias temperature gradient estimator 5 can estimate the bias temperature gradient r, and the attitude value a ~ This can reduce the estimation error.

[0042] Next, the temperature-time gradient estimation unit 6 constructs a state space model using the temperature T of the gyro sensor G as input and noise information R for the temperature T of the gyro sensor G as prior information of the observation noise, so that the temperature-time gradient ΔT of the gyro sensor G can be estimated with high accuracy. Therefore, by using the highly accurate temperature-time gradient ΔT, even if the temperature T of the gyro sensor G changes over time and the bias b of the gyro sensor G drifts when the GNSS positioning navigation is unavailable for a long period of time, the attitude value a of the moving body can be estimated with high accuracy. ~ This can reduce the estimation error.

[0043] Below, the procedures of the present disclosure will be described, including a moving object attitude measurement process, a bias switching process, a bias temperature gradient estimation process, a temperature time gradient estimation process, and a bias time change estimation process.

[0044] (Procedure of moving object attitude measurement process according to the present disclosure) The procedure of the mobile object attitude measurement process of the present disclosure is shown in Fig. 5. In a state where GNSS positioning navigation is possible (step S1, YES), the attitude calculation unit 1 executes GNSS positioning navigation. That is, the attitude calculation unit 1 calculates the attitude value a^ of the mobile object equipped with the GNSS receivers R1 to R3 based on the observation information of the GNSS receivers R1 to R3 (step S2). Then, the attitude calculation unit 1 calculates the bias b^ and bias variance σ of the gyro sensor G based on the observation information of the GNSS receivers R1 to R3 and the angular velocity ω of the gyro sensor G.b ^ is calculated (step S3, see Patent Document 3).

[0045] The attitude estimation unit 2 performs dead reckoning navigation when GNSS positioning navigation is unavailable (step S1, NO). That is, the attitude estimation unit 2 estimates the attitude value a ~ Here, the attitude estimation unit 2 estimates the attitude value a^ of the moving body in the current GNSS positioning navigation unavailable state based on the angular velocity ω of the gyro sensor G, the bias b (switched by the bias switching unit 4 from time to time), the bias time change Δb (estimated by the bias time change estimation unit 7 from time to time), and the attitude value a^ of the moving body in the most recent GNSS positioning navigation available state. ~ is estimated (step S4).

[0046] In a state where GNSS positioning navigation is possible, the attitude switching unit 3 outputs the attitude value a^ of the moving body as the attitude value a of the moving body. ~ is output as the attitude value a of the moving object.

[0047] (Procedure of Bias Switching Process of the Present Disclosure) The procedure of the bias switching process of the present disclosure is shown in Fig. 6. The bias switching unit 4 detects the angular velocity change state of the angular velocity ω of the gyro sensor G (step S11, YES), as well as the bias b^ and bias variance σ of the gyro sensor G calculated by the attitude calculation unit 1 in the GNSS positioning navigation possible state (step S12, YES) and in the converged high accuracy state of the bias b^ of the gyro sensor G (step S13, YES). b ^ are the bias b and bias variance σ of the gyro sensor G, respectively. b and outputs it to the bias temperature gradient estimation unit 5 (step S14).

[0048] The bias switching unit 4 is in an angular velocity change state of the angular velocity ω of the gyro sensor G (step S11, YES), but in a GNSS positioning navigation impossible state (step S12, NO) or in a non-converged low-accuracy state of the bias b^ of the gyro sensor G (step S13, NO), the bias b^ and bias variance σ of the gyro sensor G calculated by the attitude calculation unit 1 are b ^ are the bias b and bias variance σ of the gyro sensor G, respectively. b As a result, the bias temperature gradient estimating unit 5 does not output the bias b of the low-accuracy gyro sensor G to the bias temperature gradient estimating unit 5 (step S15). By the bias switching unit 4 not outputting the bias b of the low-accuracy gyro sensor G, it is possible to prevent the bias temperature gradient estimating unit 5 from estimating a low-reliability bias temperature gradient r using the bias b of the low-accuracy gyro sensor G.

[0049] In a state where the angular velocity ω of the gyro sensor G is constant (step S11, NO), the bias switching unit 4 calculates the angular velocity ω of the gyro sensor G and the bias variance σ in a stationary state of the moving body. b_fix are the bias b and bias variance σ of the gyro sensor G, respectively. b to the bias temperature gradient estimation unit 5 (step S16). In other words, if step S11 is NO, even if the output of the attitude calculation unit 1 is in an unconverged, low-precision state or the start of output is delayed immediately after the moving body attitude measurement device D is turned on or the satellite signal reception environment is poor, step S16 is executed, and the bias b and bias variance σ of the gyro sensor G are quickly stabilized. b Output.

[0050] In other words, the bias switching unit 4 controls the bias b and bias variance σ of the gyro sensor G regardless of whether the moving object is in a non-stationary state, a stationary state, or a moving state with a constant attitude angle. b Furthermore, even if GNSS positioning and navigation becomes unavailable, if the moving object is stationary and moving with a constant attitude angle, the bias b and bias variance σ of the gyro sensor G can be b6 is satisfied, the bias temperature gradient estimation unit 5 can estimate the bias temperature gradient r in all states of the moving object (non-stationary state, stationary state, GNSS positioning navigation possible state, GNSS positioning navigation impossible state), so even if the bias b of the gyro sensor G drifts, the attitude value a of the moving object can be ~ This can reduce the estimation error.

[0051] (Procedure of bias temperature gradient estimation process of the present disclosure) The procedure of the bias temperature gradient estimation process of the present disclosure is shown in Fig. 7. The bias temperature gradient estimation unit 5 processes the input information sequentially instead of storing the input information and processing it collectively using the least squares method or the like, so that the amount of data to be handled, the size of the matrix to be handled, the required memory capacity, and the required CPU power can be reduced compared to bulk processing.

[0052] The bias temperature gradient estimation unit 5 estimates the bias b and bias variance σ of the gyro sensor G. b and temperature T are input (step S21), and then steps S22 to S24 are executed.

[0053] The bias temperature gradient estimator 5 sets a state equation of a state space model relating to the bias temperature gradient r and the bias predetermined temperature intercept b0 of the gyro sensor G (step S22, Equation 1). Here, the state vector x n is said to follow a random walk model, but since it is not thought to change actively over time, the process noise is set to 0.

number

[0054] The bias temperature gradient estimation unit 5 estimates the bias b and bias variance σ of the gyro sensor G. b , temperature T, bias temperature gradient r, and bias predetermined temperature intercept b0 are set (step S23, Equation 2). Here, the observation vector z nThe observation noise R in steps S14 and S16 (bias variance σ b ) 2 In addition, the observation matrix H n has the temperature T of the temperature sensor T as a constituent element.

number

[0055] The bias temperature gradient estimator 5 uses a Kalman filter to estimate the bias temperature gradient r of the gyro sensor G (step S24, Equations 3 and 4). Here, Equation 3 indicates the prediction step of the Kalman filter, and x n - is the predicted state vector at time n, and P n - is the prediction error covariance matrix at timing n, and P n-1 is the update error covariance matrix at timing n-1. And, equation 4 shows the update stage of the Kalman filter, and e n is the observation error vector at timing n, and S n is the observation error covariance matrix at timing n, and K n is the Kalman gain at time n, and P n is the update error covariance matrix at timing n.

number

number

[0056] The state vector x0 and the error covariance matrix P0 in the initial state are set according to the usage status of the mobile object orientation measurement device D. In addition, as a condition for initializing the Kalman filter, when the mobile object orientation measurement device D is turned on or when the observation error vector e n In addition, in order to determine the reliability of the estimation of the bias temperature gradient r, the update error covariance matrix P nIt is confirmed that the variance element for the bias temperature gradient r is below a predetermined value, or the temperature change during estimation is above a predetermined value (the change range of the temperature T is sufficiently large and the correlation between the bias b and the temperature T is significant), and if it is determined that the estimation result is unreliable, it is acceptable not to use it.

[0057] In other words, the bias temperature gradient estimation unit 5 receives the temperature T of the gyro sensor G and the bias b output by the bias switching unit 4, and calculates the bias variance σ b is used as the prior information of the observation noise. Therefore, in the observation equation of the state space model of the bias temperature gradient estimation unit 5, the difference amount or the ratio between the difference amounts of the temperature T and the bias b of the gyro sensor G is not set, but the observation value is set, and further, the bias variance σ b Since the bias temperature gradient r is used, the bias temperature gradient estimator 5 can estimate the bias temperature gradient r with high accuracy. By using the highly accurate bias temperature gradient r, even if the bias b of the gyro sensor G drifts when the GNSS positioning navigation is unavailable for a long period of time, the attitude value a of the moving body can be estimated with high accuracy. ~ This can reduce the estimation error.

[0058] (Procedure for temperature-time gradient estimation process according to the present disclosure) The procedure of the temperature-time gradient estimation process of the present disclosure is shown in FIG.

[0059] The temperature time gradient estimation unit 6 receives the temperature T of the gyro sensor G (step S31, without calculating the difference amount over a short period of time), and then executes steps S32 to S .

[0060] The temperature-time gradient estimator 6 sets a state equation of a state space model relating to the temperature (here, the estimated temperature) T^ of the gyro sensor G, the temperature-time gradient ΔT, and the process noise Q with respect to the temperature-time gradient ΔT (step S32, equation 5). Here, the state vector x n is assumed to follow a first-order Markov process model, dt is the update interval of the data from the temperature sensor T, and σ ais the amount of noise that is set according to the location where the temperature sensor T is used, etc.

number

[0061] The temperature time gradient estimation unit 6 sets an observation equation of a state space model relating to the temperature (here, the observation temperature) T of the gyro sensor G and the observation noise R with respect to the temperature T (step S33, Equation 6). Here, the observation vector z n The observation noise R for the temperature T of the gyro sensor G is set to a noise amount that is set according to the sensor performance of the temperature sensor T, etc.

number

[0062] The temperature-time gradient estimator 6 estimates the temperature-time gradient ΔT of the gyro sensor G using a Kalman filter (step S34, Equations 7 and 8). Here, Equation 7 indicates the prediction step of the Kalman filter, and x n - is the predicted state vector at time n, and P n - is the prediction error covariance matrix at timing n, and P n-1 is the update error covariance matrix at timing n-1. And, equation 8 shows the update step of the Kalman filter, and e n is the observation error vector at timing n, and S n is the observation error covariance matrix at timing n, and K n is the Kalman gain at time n, and P n is the update error covariance matrix at timing n.

number

number

[0063] The state vector x0 and the error covariance matrix P0 in the initial state are set according to the usage status or sensor performance of the temperature sensor T. The condition for initializing the Kalman filter includes turning on the power of the mobile attitude measurement device D. In addition, in order to determine whether the estimation of the temperature time gradient ΔT is valid, the updated error covariance matrix P n It is confirmed that the variance element for the temperature time gradient ΔT is less than a predetermined value, or the temperature change during estimation is greater than a predetermined value (the change range of temperature T is sufficiently large and the correlation between bias b and temperature T is significant), and if it is determined that the estimation is invalid, the estimation result may not be used.

[0064] That is, the temperature-time gradient estimator 6 receives the temperature T of the gyro sensor G as input, and the noise information R for the temperature T of the gyro sensor G as prior information of the observed noise. Therefore, the temperature-time gradient estimator 6 does not set the difference amount or the ratio between the difference amounts of the temperature T of the gyro sensor G in the observation equation of the state space model, but sets the observed value, so that the temperature-time gradient estimator 6 can estimate the temperature-time gradient ΔT of the gyro sensor G with high accuracy. Furthermore, by using the highly accurate temperature-time gradient ΔT of the gyro sensor G, even if the bias b of the gyro sensor G drifts when the GNSS positioning navigation is unavailable for a long period of time, the attitude value a of the moving body can be estimated with high accuracy. ~ This can reduce the estimation error.

[0065] (Procedures for bias time change estimation processing and attitude estimation processing according to the present disclosure) The procedure of the bias time change estimation process and the attitude estimation process of the present disclosure is shown in Fig. 9. The bias time change estimation unit 7 estimates the bias time change Δb of the gyro sensor G within a predetermined time dt based on the bias temperature gradient r and the temperature time gradient ΔT of the gyro sensor G (step S41, Equation 9). Here, dt is the update interval of the data of the gyro sensor G.

number

[0066] The posture estimation unit 2 estimates the angular velocity ω i , bias b0 and bias time change Δb i Based on the attitude value a0^ of the moving object in the most recent GNSS positioning and navigation available state, the attitude value a of the moving object in the current GNSS positioning and navigation unavailable state is calculated. n ~ is estimated (step S42, equation 10). Here, the subscript n indicates the timing n, the subscript i indicates the timing i, the subscript 0 indicates the timing immediately before switching to the dead reckoning navigation, M is a coordinate transformation matrix from the sensor coordinate system to the Earth coordinate system, and dt is the update interval of the data of the gyro sensor G.

number

[0067] (Procedure for processing considering the forgetting effect over time according to the present disclosure) The procedure of the process considering the forgetting effect over time of the present disclosure is shown in Fig. 10. The bias temperature gradient estimator 5 estimates the bias temperature gradient r of the gyro sensor G based on the state equation of the state space model regarding the bias temperature gradient r of the gyro sensor G and the bias predetermined temperature intercept b0, which considers the forgetting effect over time (steps S51 and S52).

[0068] Specifically, the bias temperature gradient estimator 5 sets a forgetting coefficient β according to the passage of time in the state equation of the state space model related to the bias temperature gradient r and the bias predetermined temperature intercept b0 of the gyro sensor G (step S51, equation 11). Here, β>0 is the state vector x n is the forgetting factor over time, and Δt is the update interval of the bias temperature gradient r. In other words, when β is small, the diagonal elements of F are small compared to 1, so the state vector x n The degree of forgetting over time is large. On the other hand, when β is large, the diagonal elements of F are close to 1 compared to 1, so the state vector x n The degree of forgetting over time is small.

number

[0069] Then, the bias temperature gradient estimator 5 sets a process noise Q according to the passage of time in the state equation of the state space model related to the bias temperature gradient r and the bias predetermined temperature intercept b0 of the gyro sensor G (step S52, equation 12). n-1 -x n-1 - is the state residual vector at timing n-1, and α is a gain between 0 and 1. When β is small, current information is given more importance than past information, so α is increased to increase the process noise Q according to the state residual amount. On the other hand, when β is large, past information is given more importance than current information, so α is decreased to decrease the process noise Q according to the state residual amount. However, when an upper or lower limit is set on the state residual vector, a limit must be imposed.

number

[0070] In other words, when creating the state equation of the state space model, the bias temperature gradient estimator 5 adds a process that considers the forgetting effect according to the passage of time, so that the bias temperature gradient r can be estimated with high accuracy even if the relationship between the temperature T and the bias b of the gyro sensor G gradually changes nonlinearly with the passage of time. Therefore, even if the bias b of the gyro sensor G drifts, the attitude value a of the moving body ~ This can reduce the estimation error.

[0071] (Procedure for determining whether the bias in this disclosure is normal or abnormal) The procedure of the process of determining whether the bias is normal or abnormal according to the present disclosure is shown in Fig. 11. When the observation residual normalized by the covariance of the observation residual is less than (or greater than) a predetermined threshold, the bias temperature gradient estimator 5 determines that the bias b of the gyro sensor G is normal (abnormal) and executes (stops) estimation of the bias temperature gradient r of the gyro sensor G (steps S61 to S65).

[0072] Specifically, the bias temperature gradient estimation unit 5 uses a Kalman filter or the like to calculate the observation residual vector e n and the observation error covariance matrix S n (Step S61, Equation 13). Then, the bias temperature gradient estimator 5 calculates the observation error covariance matrix S n The normalized observation residual vector v n (Step S62, Equation 14). Here, e nj e n is the jth vector element of v nj is v n is the jth vector element of S njj is S n is the j-th diagonal element of

number

number

[0073] where v nj has a mean of 0 and a variance of 1 as a statistic. And v nj For this, if a predetermined threshold n (=5, etc.) is set, then nσ (5σ, etc.) testing will be performed.

[0074] Therefore, the bias temperature gradient estimation unit 5 calculates v nj When v is less than a predetermined threshold n (e.g., 5) (step S63, YES), it is determined that the bias b of the gyro sensor G is normal (within the confidence interval), and the bias temperature gradient r of the gyro sensor G is estimated (step S64). nj is equal to or greater than a predetermined threshold value n (e.g., 5) (step S63, NO), it is determined that the bias b of the gyro sensor G is abnormal (outside the confidence interval), the estimation of the bias temperature gradient r of the gyro sensor G is stopped, and the most recent normal bias temperature gradient r is output to the bias time change estimation unit 7 (step S65).

[0075] In other words, by providing the bias temperature gradient estimation unit 5 with a process for determining whether the bias b of the gyro sensor G is normal or abnormal, even if the gyro sensor G or the attitude calculation unit 1 is affected by disturbances or the like and the bias b becomes abnormal temporarily, it is possible to prevent the transmission of adverse effects to the estimation of the bias temperature gradient r in advance. Therefore, the process for determining whether the bias b of the gyro sensor G is normal or abnormal increases the reliability of the estimation of the bias temperature gradient r of the gyro sensor G, so that the attitude value a of the moving body can be calculated. ~ This can reduce the estimation error.

[0076] (1. Procedure of 3D posture measurement processing in this disclosure) First, in order to simplify the explanation of the formulas, this disclosure has shown the procedure of one-dimensional attitude measurement processing. When the mobile body attitude measurement device D is mounted on a ship or the like, if it is assumed that the ship or the like does not have much rocking, only the yaw angle ψ is an important parameter among the yaw angle ψ, pitch angle θ, and roll angle φ, and the angular velocity ω x , angular velocity ω y and angular velocity ω z Of these, angular velocity ω x and angular velocity ω y is negligible, and the angular velocity ω z is the only valid parameter, and it can be treated as a one-dimensional attitude measurement process.

[0077] The coordinate transformation matrix M from the sensor coordinate system to the Earth coordinate system is expressed as the first equation of equation 15. The time evolution of the yaw angle ψ is expressed as the second equation of equation 15, similarly to equation 10. Here, in equations 1 and 2 of equation 15, ψ0, θ0, and φ0 indicate approximate attitude values ​​(e.g., previous attitude values, etc.). In equation 2 of equation 15, subscript i indicates timing i, subscript 0 indicates the timing immediately before switching to dead reckoning, and subscript z indicates the z-axis direction.

number

[0078] Next, the present disclosure can also be applied to a three-dimensional attitude measurement process, and a procedure for the three-dimensional attitude measurement process will be described. When the moving body attitude measurement device D is mounted on a ship or the like, if the ship or the like is subject to a lot of rocking, all of the attitude angles among the yaw angle ψ, the pitch angle θ, and the roll angle φ are important parameters, and the angular velocity ω x , angular velocity ω y and angular velocity ω z Among these, all angular velocities are valid parameters and can be used for three-dimensional attitude measurement processing. In the case of three-dimensional attitude measurement processing, the bias b and bias variance σ of the gyro sensor G are b , the bias temperature gradient r, the bias time change Δb, etc. are also treated as three-dimensional vectors.

[0079] The coordinate transformation matrix M from the sensor coordinate system to the Earth coordinate system is expressed as the first equation of equation 16. The time evolution of the yaw angle ψ, pitch angle θ, and roll angle φ is expressed as the second equation of equation 16, similarly to equation 10. Here, in equations 1 and 2 of equation 16, ψ0, θ0, and φ0 indicate approximate attitude values ​​(e.g., previous attitude values, etc.). In equation 2 of equation 16, subscript i indicates timing i, subscript 0 indicates the timing immediately before switching to dead reckoning, and subscripts x, y, and z indicate the x-axis, y-axis, and z-axis directions.

number

[0080] (Issues with the prior art gyro sensitivity calibration process) The problem with the gyro sensitivity calibration process of Patent Document 3 is shown in FIG. gyrо is the true value of the angular velocity information of the gyro sensor, ω true However, it may deviate by the amount of the gyro sensitivity r of the gyro sensor.

[0081] Therefore, when manufacturing the GNSS compass, the GNSS compass is placed on a rotating table and rotated at a specified speed, and ω gyrо =(1+r)ω trueHowever, during operation of the GNSS compass, 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 Even if the gyro sensitivity r of the gyro sensor is calibrated, a calibration error r'-r of the gyro sensor occurs. Therefore, Patent Document 4 and the like disclose a technology for estimating and correcting the gyro sensitivity r of the gyro sensor in real time during operation of the GNSS compass.

[0082] The problem with the gyro sensitivity correction process of Patent Document 4 is shown in FIG. 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 the predetermined times t1 and t2, and the time change Δa GPS Next, calculate the time change in the attitude information of the moving object Δa gyrо (deviation from the true value), the time change in the attitude information of the moving object Δa GPS (closest to the true value) is subtracted to estimate and correct the gyro sensitivity r of the gyro sensor.

[0083] However, the time change in the attitude 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, and the gyro sensitivity r of the gyro sensor is erroneously estimated and corrected.

[0084] (Overview of the gyro sensitivity estimation process of the present disclosure) An outline of the gyro sensitivity estimation process of the present disclosure is shown in Fig. 14. In Fig. 14, it is assumed that the aircraft coordinate system and the navigation coordinate system are the same, and the pitch angle and roll angle are zero. First, the angular velocity information ω gyrо Calculated value of "rotational speed" of the moving object using r gyrо (True value ω trueThe rotation speed of the moving object is calculated by using the observation information of the GNSS receiver. a (True value ω true (closest to ) to estimate the gyro sensitivity r of the gyro sensor.

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

[0086] Next, in a state where GNSS positioning and navigation is unavailable, the angular velocity information ω gyrо Based on the bias b and the gyro sensitivity r, the attitude information a of the moving body is corrected.

[0087] Here, the angular velocity information ω gyrо Calculated value of "rotational speed" of the moving object using r gyrо Since the bias error of the gyro sensor is hardly superimposed on the gyro sensitivity r of the gyro sensor, the bias error of the gyro sensor is hardly superimposed on the gyro sensitivity r of the gyro sensor. Therefore, in the GNSS compass, the influence of the bias error superimposition of the gyro sensor can be reduced, and the attitude information a of the moving object can be calculated in real time, robustly, and with high accuracy.

[0088] The configuration of a moving object attitude measuring device according to the present disclosure is shown in Fig. 15. Moving object attitude measuring device D is mounted on a ship or the like, connected to receiving antennas A1-A3, GNSS receivers R1-R3 and a gyro sensor G, and includes an attitude calculation unit 1, an attitude estimation unit 2 and an attitude switching unit 3 similar to the configuration shown in Fig. 4, and also includes a first rotation speed calculation unit 8, a second rotation speed calculation unit 9 and a gyro sensitivity estimation unit 10 as newly configured as shown in Fig. 15. Moving object attitude measuring device D can be realized by installing the moving object attitude measuring programs shown in Figs. 16-20 in a computer.

[0089] In the following, as procedures of the present disclosure, an attitude calculation process, a first rotation speed calculation process, a second rotation speed calculation process, a gyro sensitivity estimation process, and a gyro sensitivity correction process will be described.

[0090] (Procedure of attitude calculation process of the present disclosure) The procedure of the attitude calculation process of the present disclosure is shown in Fig. 16. 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 S71, see Patent Document 3). x , ω y , ω z is the angular velocity information of the gyro sensor G in the x-, y-, and z-axis directions of the aircraft body coordinate system of the gyro sensor G.

[0091] 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 S72, see Patent Document 3). ψ, θ, and φ are the yaw angle, pitch angle, and roll angle of the navigation coordinate system of the moving body, and are attitude information of the moving body.

[0092] 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=[b x b y b z ] T Calculate [dps] (step S73, see the configuration in Figure 4). x , b y , b z are the biases of the gyro sensor G in the x, y, and z axis directions of the aircraft coordinate system of the gyro sensor G.

[0093] (Procedure of the first rotation speed calculation process of the present disclosure) 17 shows the procedure of the first rotation speed calculation process of the present disclosure. First, the first rotation speed calculation unit 8 calculates, in the coordinate conversion unit 81, angular velocity 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 S81, see equations 17 and 18). 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

[0094] Next, the first rotation speed calculation unit 8 calculates the rotation speed r gyrо A low-pass filter is applied to suppress sudden high-frequency noise, and the rotation speed r gyrо_LPF [dps] is output (step S82, see formula 19). LPF_gyrо is, for example, a first-order low-pass filter function, and the r gyrо_LPF is the current value, and r on the right hand side 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

[0095] However, the first rotation speed calculation unit 8 uses the angular velocity information ω gyrо If any one of the attitude value a^ of the attitude information of the moving object and the bias b of the gyro sensor G cannot be input, the rotation speed r of the moving object will be gyrо_LPF Do not output.

[0096] (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. 18. First, the second rotation speed calculation unit 9 calculates the most recent attitude value a t ^ and the previous attitude value a t-Δt ^ and, based on the rotational speed r of the moving body a [dps] is calculated (step S91, see formula 20). Δt [s] is the calculation interval for the attitude information of the moving object.

number

[0097] However, the second rotation speed calculation unit 9 calculates the error σ of the attitude value a^ of the attitude information of the moving object in the discrete differentiation unit 91. 2 =[σ ψ 2 σ θ 2 σ φ 2 ] T [deg 2 ] is below the threshold value, the rotation speed r a In other words, the error σ of the attitude value a^ of the attitude information of the moving object is output (step S91). 2 If is small, the attitude value a^ of the attitude information of the moving object is almost accurate, and the rotation speed r a is also almost accurate. Here, the error σ 2 The magnitude is large immediately after the power is turned on, immediately after the GNSS signal is recovered, and in poor GNSS observation environments.

[0098] Next, the second rotation speed calculation unit 9 calculates the rotation speed r a A low-pass filter is applied to suppress sudden high-frequency noise, and the rotation speed r a_LPF [dps] is output (step S92, see formula 21). LPF_a is, for example, a first-order low-pass filter function, and the r a_LPF is the current value, and r on the right hand side a_LPF is the previous value, and Ka is the gain based on the filter time constant. f LPF_a may apply a high-order low-pass filter function.

number

[0099] However, the second rotation speed calculation unit 9 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 attitude information of the moving object 2 If you cannot input any one of the following, the rotation speed r a_LPF Do not output.

[0100] (Procedure of gyro sensitivity estimation process according to the present disclosure) The procedure of the gyro sensitivity estimation process of the present disclosure is shown in Fig. 19. First, the gyro sensitivity estimation unit 10 uses the reciprocal calculation unit 101 and the multiplication unit 102 to calculate the rotation speed r gyrо_LPF The rotation speed r of the moving body calculated by the second rotation speed calculation unit 9 is a_LPF Divide by the gyro sensitivity r of the gyro sensor G. ~ (Step S101, see Equation 22). ~ is the estimated gyro sensitivity of the gyro sensor G.

number

[0101] Here, the gyro sensitivity estimation unit 10 calculates the rotation speed r of the moving body in the reciprocal calculation unit 101 and the multiplication unit 102. gyrо_LPF and the rotational speed of the moving object r a_LPF In this case, if the denominator of the ratio calculation is close to 0, the ratio may suddenly become an abnormal value when noise is superimposed on the denominator. Therefore, the gyro sensitivity estimation unit 10 calculates the "ratio" between the rotation speed r of the moving body in the reciprocal calculation unit 101 and the multiplication unit 102. a_LPF If the rotation speed r of the moving object is faster, the calculation result of the ratio can be updated.a_LPF If is slow (close to 0), the ratio calculation result should be maintained at the previously calculated value.

[0102] Next, the gyro sensitivity estimation unit 10 calculates the gyro sensitivity r ~ , low-pass filtering is performed to suppress sudden high-frequency noise, and the gyro sensitivity r of the gyro sensor G is LPF (Step S102, see Equation 23). LPF_r is, for example, a first-order low-pass filter function, and the r LPF is the current value, and r on the right hand side LPF is the previous value, and K r is the gain based on the filter time constant. f LPF_r may apply a high-order low-pass filter function.

number

[0103] Here, the gyro sensitivity estimation unit 10 calculates the gyro sensitivity r of the gyro sensor G in the reciprocal calculation unit 101 and the multiplication unit 102. ~ The noise suppression unit 103 estimates the gyro sensitivity r ~ On the other hand, the gyro sensitivity estimation unit 10 performs low-pass filtering on the gyro sensitivity r ~ +1, and the noise suppression unit 103 estimates the gyro sensitivity r ~ You can perform a low pass filter on the +1 and subtract 1 from it.

[0104] However, the gyro sensitivity estimation unit 10 uses the rotation speed r of the moving body calculated by the first rotation speed calculation unit 8 as the gyrо_LPF and the rotation speed r of the moving body calculated by the second rotation speed calculation unit 9. a_LPF If any one of the above cannot be input, the gyro sensitivity r of the gyro sensor G will be LPF can be maintained at the previously calculated value.

[0105] Angular velocity information ω of gyro sensor G gyrо Calculated value of rotational speed of moving object using r gyrо_LPF Since the bias b of the gyro sensor G is not superimposed, the gyro sensitivity r of the gyro sensor G LPF In the GNSS compass, the bias b of the gyro sensor G is hardly superimposed. Therefore, the influence of the bias b of the gyro sensor G is reduced, and the gyro sensitivity r of the gyro sensor G is LPF can be estimated in real time, robustly and with high accuracy.

[0106] Considering that the time change of the gyro sensitivity of the gyro sensor G is slow, the gyro sensitivity r LPF can be estimated in real time, robustly and with high accuracy.

[0107] (Procedure of gyro sensitivity correction process of the present disclosure) The procedure of the gyro sensitivity correction process of the present disclosure is shown in Fig. 20. In a state where GNSS positioning navigation is unavailable, the attitude estimation unit 2 estimates the angular velocity information ω gyrо , bias b and gyro sensitivity r LPF Based on this, the posture information of the moving object a ~ is corrected (step S111).

[0108] Specifically, in the one-dimensional attitude measurement process, when GNSS positioning and navigation is unavailable, the attitude estimation unit 2 estimates the attitude information a of the moving object as follows: ~ is corrected (step S112, see Equation 24). LPF、zi is the gyro sensitivity of the gyro sensor G at time i in the z-axis direction of the aircraft coordinate system of the gyro sensor G. The difference between Equation 24 and Equation 15 is that the angular velocity information ω gyrо But 1 / (1+r LPF、zi However, the bias b of the gyro sensor G is so small that it is almost not affected by the gyro sensitivity of the gyro sensor G, so it is corrected by a factor of 1 / (1+r LPF、zi ) factor.

number

[0109] Alternatively, in the three-dimensional attitude measurement process, when GNSS positioning and navigation is unavailable, the attitude estimation unit 2 estimates the attitude information a of the moving object as follows: ~ (Step S113, see Equation 25). LPF、xi , r LPF、yi , r LPF、zi is the gyro sensitivity of the gyro sensor G at time i in the x, y, and z axis directions of the aircraft coordinate system of the gyro sensor G. The difference between Equation 25 and Equation 16 is that the angular velocity information ω gyrо But 1 / (1+r LPF、xi ), 1 / (1+r LPF、yi ), 1 / (1+r LPF、zi However, the bias b of the gyro sensor G is so small that it is almost not affected by the gyro sensitivity of the gyro sensor G, so it is corrected by a factor of 1 / (1+r LPF、xi ), 1 / (1+r LPF、yi ), 1 / (1+r LPF、zi ) factor.

number

[0110] Angular velocity information ω of gyro sensor G gyrо Calculated value of rotational speed of moving object using r gyrо_LPF Since the bias b of the gyro sensor G is not superimposed, the gyro sensitivity r of the gyro sensor G LPF In the GNSS compass, the bias b of the gyro sensor G is hardly superimposed. Therefore, the influence of the bias b of the gyro sensor G is reduced, and the attitude information a of the moving object is ~ can be calculated in real time, robustly and with high accuracy. [Industrial Applicability]

[0111] The moving body attitude measuring device and moving body attitude measuring program disclosed herein can reduce the estimation error of the moving body's attitude value, regardless of whether the moving body is in a non-stationary or stationary state, even if the bias of the gyro sensor drifts due to a change in the temperature of the gyro sensor over time when GNSS positioning navigation is unavailable for a long period of time. [Explanation of symbols]

[0112] D: Moving object attitude measurement device A1~A3: Receiving antennas R1~R3: GNSS receiver G: Gyro sensor T: Temperature sensor 1: Posture calculation section 2: Posture estimation part 3: Posture change section 4: Bias switching section 5: Bias temperature gradient estimation part 6: Temperature-time gradient estimation part 7: Bias time change estimator 8: First rotation speed calculation section 9: Second rotation speed calculation section 10: Gyro sensitivity estimation unit 81: Coordinate conversion section 82: Noise suppression section 91: Discrete differential part 92: Noise suppression section 101: Reciprocal calculation section 102: Multiplication section 103: Noise suppression section

Claims

1. an attitude calculation unit that calculates attitude information of a moving body equipped with the GNSS receiver based on observation information of the GNSS receiver in a state where GNSS positioning navigation is possible; an attitude estimation unit that estimates attitude information of the moving body equipped with the gyro sensor based on angular velocity information of the gyro sensor in a state in which GNSS positioning navigation is unavailable; a bias temperature gradient estimating unit that estimates a bias temperature gradient of the gyro sensor based on a state equation of a state space model related to a bias temperature gradient and a bias predetermined temperature intercept of the gyro sensor, and an observation equation of a state space model related to a bias, bias variance, temperature, bias temperature gradient, and bias predetermined temperature intercept of the gyro sensor; a bias time change estimation unit that estimates a bias time change of the gyro sensor based on a bias temperature gradient and a temperature time gradient of the gyro sensor, The attitude estimation unit estimates attitude information of the moving body in the current GNSS positioning navigation unavailable state based on angular velocity information, bias, and bias time change of the gyro sensor, and attitude information of the moving body in the most recent GNSS positioning navigation available state. A moving object attitude measuring device comprising:

2. The attitude calculation unit calculates a bias and a bias variance of the gyro sensor based on observation information of the GNSS receiver and angular velocity information of the gyro sensor, a bias switching unit that outputs the bias and bias variance of the gyro sensor calculated by the attitude calculation unit in an angular velocity change state of the angular velocity information of the gyro sensor to the bias temperature gradient estimation unit as the bias and bias variance of the gyro sensor, respectively; The moving object attitude measuring apparatus according to claim 1 , further comprising:

3. a bias switching unit that outputs the angular velocity information of the gyro sensor and a bias variance in a stationary state as a bias and a bias variance of the gyro sensor, respectively, to the bias temperature gradient estimation unit when the angular velocity information of the gyro sensor is in a constant angular velocity state; The moving object attitude measuring apparatus according to claim 1 , further comprising:

4. a temperature time gradient estimating unit that estimates a temperature time gradient of the gyro sensor based on a state equation of a state space model related to the temperature of the gyro sensor, a temperature time gradient, and process noise with respect to the temperature time gradient, and an observation equation of a state space model related to the temperature of the gyro sensor and observation noise with respect to the temperature, and outputs the temperature time gradient to the bias time change estimating unit; 4. The moving object attitude measuring apparatus according to claim 1, further comprising:

5. The bias temperature gradient estimation unit estimates a bias temperature gradient of the gyro sensor based on a state equation of a state space model that takes into account a forgetting effect over time with respect to a bias temperature gradient and a bias predetermined temperature intercept of the gyro sensor.

4. The moving object attitude measuring device according to claim 1, wherein the moving object attitude measuring device is a moving object attitude measuring device.

6. The bias temperature gradient estimator determines that the bias of the gyro sensor is normal (abnormal) when the observation residual normalized by the covariance of the observation residual is less than (greater than) a predetermined threshold, and executes (stops) estimation of the bias temperature gradient of the gyro sensor.

4. The moving object attitude measuring device according to claim 1, wherein the moving object attitude measuring device is a moving object attitude measuring device.

7. the attitude calculation unit calculates a predicted value of the 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 the 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 velocity information of the gyro sensor into angular velocity 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 gyro sensitivity estimation unit that divides the rotation speed of the moving body calculated by the first rotation speed calculation unit by the rotation speed of the moving body calculated by the second rotation speed calculation unit to estimate a gyro sensitivity of the gyro sensor (a ratio between a measured value of an angular velocity and a true value - 1); 2. The moving object attitude measuring apparatus according to claim 1, further comprising:

8. The first rotation speed calculation unit and the second rotation speed calculation unit perform low-pass filter processing on the rotation speed of the moving body to suppress sudden high-frequency noise, and / or the gyro sensitivity estimation unit performs low-pass filter processing on the gyro sensitivity of the gyro sensor to suppress sudden high-frequency noise.

8. The moving object attitude measuring device according to claim 7.

9. The attitude estimation unit corrects attitude information of the moving body based on angular velocity information and gyro sensitivity of the gyro sensor when GNSS positioning navigation is unavailable.

9. The moving object attitude measuring device according to claim 7 or 8.

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

Images