KR101402767B1 - A rapid alignment mechanism applying an adaptive filter and disturbance detection technique - Google Patents

Abstract

The present invention provides an adaptive filter capable of maintaining the alignment performance and navigation performance of the inertial navigation system and significantly shortening the alignment real time to about 4 minutes even under dynamic ground alignment conditions in which persistent disturbance and single- Speed sorting mechanism for an inertial navigation system employing a disturbance detection technique.
The adaptive filter and the rapid rapid alignment mechanism for an inertial navigation system employing the disturbance detection technique according to the present invention are characterized in that an inertial sensor unit composed of an accelerometer and a gyro and a rough initial posture value using an acceleration value and an angular velocity value input from the inertial sensor unit And a measured shape adaptive filter for calculating a velocity error value caused by the attitude error value when the rough closed loop alignment in the roughly closed loop alignment module is completed. The roughly closed loop sorting module includes a rough horizontal sorting module for calculating a roll angle and a pitch angle by using an acceleration value inputted from the inertia sensor part and a roughly horizontal sorting module for detecting a signal due to the disturbance from the acceleration value and the angular velocity value inputted from the inertia sensor part A disturbance detection module for calculating a disturbance threshold value, and a rough azimuth alignment module for calculating an azimuth angle using an angular velocity value input from the inertia sensor portion.

Description

Technical Field [0001] The present invention relates to an adaptive filter and disturbance detection technique for an inertial navigation system employing an adaptive filter and a disturbance detection technique,

The present invention relates to an alignment mechanism of an inertial navigation device and is capable of maintaining the alignment and navigation performance of an inertial navigation device even under dynamic ground alignment conditions where persistent disturbance, wind buffet, And to a robust alignment mechanism for an inertial navigation system employing an adaptive filter and a disturbance detection technique, which can shorten the alignment actual time to about 4 minutes or less.

Inertial Navigation System (INS), which is used for guided weapons, aircraft, asteroids, submarines and maneuvers that require position and attitude control, basically has an angular velocity and acceleration of "0" , That is, ground alignment is performed on the premise of stationary status.

If this precondition is violated, the estimation performance of the azimuth angle of the inertial navigation system is degraded, resulting in a large navigation error. For example, when the ground alignment is performed under an environment in which the single inertial disturbance, the wind buffet, the persistent disturbance, etc. are applied to the inertial navigation apparatus, the angular velocity and the acceleration are not " 0 & The navigation performance of the navigation system is significantly degraded.

In the case of foreign countries, the ground alignment is performed for 4 to 15 minutes according to the characteristics of the inertial navigation system. When vibration is introduced during the ground alignment, the ground alignment is stopped, and in- (In-Flight Alignment) or the like, and performs alignment during movement. However, such alignment during movement requires an actual alignment time of about 10 minutes and requires a start (including a specific trajectory). Especially, when the disturbance is introduced at the time of 4-minute alignment (rapid alignment), the estimation performance of the azimuth (alignment performance) and the navigation performance are significantly deteriorated.

In Korea, it takes 8 to 15 minutes to arrange the intermediate / precision inertial navigation system on the ground. In case of ground alignment performed on the assumption of a static condition, a disturbance such as a wind buffet is applied to a small amount of disturbance, a notch filter may be used to filter the disturbance (or the influence of the disturbance) The influence of the disturbance (variance) is distributed over the entire alignment time, thereby minimizing the influence on the estimation performance of the azimuth angle.

However, such an alignment method is not suitable for the rapid four-minute alignment in which demand is rapidly increasing.

Alignment performance and navigation performance of the inertial navigation system depend greatly on the operating environment of the carrier or platform (hereinafter referred to as "platform") on which the inertial navigation system is mounted.

For example, when the rotor blade (landing gear) is aligned in the ground, the alignment performance is rapidly deteriorated due to the vibration generated continuously due to the rotation of the rotor and the one-shot impact caused by the engine starting.

On the other hand, in the case of ground alignment in a ship or underwater, rolling and pitching due to waves and tidal currents occur, resulting in poor alignment performance and alignment time delay.

Accordingly, a first object of the present invention is to provide a rapid alignment mechanism that can be effectively applied to rapid alignment (four-minute alignment) of an intermediate / precision inertial navigation device.

It is also a second object of the present invention to provide a highly reliable robust alignment mechanism that can exhibit excellent alignment performance even when disturbance, vibration, or shock is introduced during ground alignment.

According to an aspect of the present invention, there is provided an inertial navigation system including a disturbance detection module, a roughly closed loop alignment module, and a measurement shaping adaptive Kalman filter, The approximate closed loop sorting module calculates the approximate azimuth only when the output value of the inertia sensor portion is smaller than the disturbance threshold by calculating an allowable disturbance threshold value by detecting an applied disturbance during the alignment of the inertial navigation device, And the measurement shape adaptive filter is configured to improve the azimuth estimation performance deterioration with respect to the disturbance introduced.

Particularly, in the present invention, the disturbance detection and the disturbance threshold value for detecting the disturbance from the output value of the inertia sensor section and calculating the allowable disturbance threshold value under the dynamic ground alignment condition to which persistent disturbance, wind buffet, We present estimation method, estimation (shaping) method of adaptive filter, and threshold value calculation method for updating and propagating state variable of adaptive filter. The disturbance threshold value calculation and the measurement shape technique of the adaptive filter, etc. will become clear from the description of the embodiment based on the attached drawings.

The adaptive filter of the present invention which can maintain the alignment performance and the navigation performance of the inertial navigation system and reduce the actual alignment time to about 4 minutes even under the condition that the disturbance is introduced and the solid quick alignment mechanism for the inertial navigation system employing the disturbance detection technique An inertial sensor unit including an accelerometer and a gyroscope; A rough closed loop sorting module for calculating a rough initial posture value using an acceleration value and an angular velocity value input from the inertia sensor; A measured shape ( a) that controls the gain by real-time calculation of the measurement covariance according to the magnitude of the disturbance, using the velocity error value and disturbance caused by the attitude error value when the rough closed loop alignment in the rough closed loop alignment module is completed adaptive filter ; And a control unit.

In one embodiment of the present invention, the roughly closed loop sorting module includes a rough horizontal sorting module for calculating a roll angle and a pitch angle using an acceleration value input from the inertial sensor part, A disturbance detection module for detecting a signal due to a disturbance from an acceleration value and an angular velocity value input from the inertial sensor unit and calculating an allowable disturbance threshold value; And a rough azimuth alignment module that calculates the azimuth using this rotation amount.

In one embodiment of the present invention, the disturbance detecting module is schematically calculated lung maximum value with a standard deviation calculation module and calculating a standard deviation using the moving window average of the module and the inertial sensor of the instantaneous value with the entire length of the average loop depending on the nature of the disturbance to be introduced into the module and an inertial navigation system is mounted platform consists of a disturbance threshold selecting switch for selecting any one of three kinds of these calculation module. In addition to the above three cases, the natural vibration and impact of a specific platform can be measured in advance and used as a disturbance threshold value.

In one embodiment of the present invention, the rough azimuth alignment module calculates an approximate azimuth angle when the angular velocity value input from the inertial sensor unit is smaller than the disturbance threshold value calculated by the disturbance detection module, If the angular velocity input from the inertial sensor unit is equal to or greater than the disturbance threshold value calculated by the disturbance detection module, the alignment real time of the rough azimuth alignment module is initialized to initialize the rough azimuth alignment module, and then the rough azimuth alignment is performed again And when the actual azimuth alignment real time of the rough azimuth alignment module increases to exceed the azimuth alignment reference time, the alignment of the rough azimuth is completed.

In one embodiment of the present invention, the measured shape adaptive filter includes an error state estimation extrapolation module that first updates an error model of a measured shape adaptive filter after performing a Kalman filter, and an error state estimation extrapolation module that realizes a measurement covariance value in real time optimized adaptive measurement covariance update module, an adaptive measurement covariance update is carried out after renewal of the measurement covariance value by the module pre-error and the error covariance extrapolation module to update the covariance value, the error state covariance value and the system processing an error covariance value and the measurement covariance that errors by using a value based on the Kalman gain assessment module, as well as a reference value previously calculated and also by using the Kalman gain updates the error state covariance value (trim value) for calculating a Kalman gain determines the update if the error status value State covariance update module .

In one embodiment of the present invention, the adaptive measurement covariance update module is configured to update the measured covariance update using the total interval cumulative averaging to obtain the covariance by averaging the integrated value of the acceleration output from the inertial sensor unit over the entire section in which the adaptive filter is operated. Module , a measurement covariance update module that uses a moving window average to continuously obtain an average of a moving window by imposing a constant moving window on the velocity measurement value , a measurement value covariance update module using an instantaneous value of the inertial sensor portion , a platform equipped with an inertial navigation device, And a measurement covariance selection switch for selecting any one of the three types of measurement covariance update modules or the multi-covariance update module according to the ground alignment condition of the measurement covariance update module.

In one embodiment of the present invention, the error-state covariance update module includes a standard deviation calculation module using a total interval average of a closed loop, a standard deviation calculation module using a moving window average, and a maximum value calculation module using an instantaneous value of the inertial sensor part. calculation module and the earth angular velocity calculation module and, depending on the disturbance characteristic which weights the gravitational acceleration to calculate the gravitational acceleration applied to calculation module and an inertial navigation unit is introduced in the mounting platform 5 kinds of calculating the earth angular velocity weighted And a reference value selection switch for selecting one or more of these calculation modules.

According to the present invention, when disturbance is applied during ground alignment of the inertial navigation apparatus, the disturbance threshold value is calculated by the disturbance detection module, and only when the output value of the inertia sensor portion is smaller than the disturbance threshold value, The azimuth angle is calculated, and when the vibration is introduced by the measurement shape adaptive filter, the measurement covariance is increased to reduce the reliability of the measurement value, thereby remarkably improving the azimuth estimation performance of the inertial navigation device under the dynamic ground alignment condition.

In addition, when the influence due to the disturbance can be minimized or completely eliminated, the approximate closed loop sorting module is executed to estimate the general azimuth angle, so that the alignment performance and navigation performance of the inertial navigation device can be maintained at a high level .

Further, in the present invention, since the measurement shape adaptive filter configured to be able to remarkably shorten the estimation time of the azimuth angle and improve the estimation accuracy of the azimuth angle by more than 70% is used compared with the case of using the general filter, It provides a solution for rapid alignment of navigation devices and is very effective for general low-level navigation devices.

1 is a block diagram of a robust alignment mechanism for an inertial navigation system employing an adaptive filter and a disturbance detection technique according to the present invention;
FIG. 2 is a block diagram illustrating a configuration and a processing algorithm of an approximate closed loop sorting module of a robust sorting mechanism for an inertial navigation device using an adaptive filter and a disturbance detection technique according to the present invention. FIG.
3 is a block diagram of a disturbance detection module of a robust alignment mechanism for an inertial navigation system using an adaptive filter and a disturbance detection technique according to the present invention.
FIG. 4 is a diagram illustrating a configuration and a processing algorithm of a rough azimuth alignment module for a robust alignment mechanism for an inertial navigation system employing an adaptive filter and a disturbance detection technique according to the present invention; FIG.
5 is a diagram showing a configuration and a processing algorithm of a measured shape adaptive filter of a robust alignment mechanism for an inertial navigation system using an adaptive filter and a disturbance detection technique according to the present invention.
6 is a block diagram of an adaptive measurement covariance update module of a robust alignment mechanism for an inertial navigation system employing an adaptive filter and a disturbance detection technique according to the present invention.
FIG. 7 is a block diagram of an error state update module of a robust alignment mechanism for an inertial navigation system employing an adaptive filter and a disturbance detection technique according to the present invention. FIG.

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS Hereinafter, a preferred embodiment of a robust alignment mechanism for an inertial navigation system according to the present invention will be described in detail with reference to the accompanying drawings.

In this specification, the term "module" refers to a program itself for a specific process, but may also refer to a circuit or an electronic component on which a part of such a program is mounted.

First, as shown in FIG. 1, a solid quick aligning mechanism for an inertial navigation system according to an exemplary embodiment of the present invention is designed to improve performance of an intermediate / precision inertial navigation system due to disturbance (signal) Stage configuration including an inertial sensor unit 101, a roughly closed loop alignment module 102 and a measurement shaping adaptive filter 103 so as to minimize the deterioration of the navigation performance and the navigation performance.

The inertial sensor unit 101 is composed of an accelerometer for acceleration detection, a gyroscope for angular velocity detection, and a block on which an accelerometer and a gyro are mounted. The inertia sensor unit 101 is connected to a cable for outputting an output value (signal) of acceleration and angular velocity detected by the accelerometer and the gyro to the roughly closed loop alignment module 102 described later. The inertial sensor unit 101 outputs acceleration and angular velocity of the x-axis, the y-axis, and the z-axis, respectively.

The output values of the acceleration and the angular velocity output from the inertia sensor unit 101 to the roughly closed loop sorting module 102 are shown in the following Equation (1).

In Equation (1), each symbol is defined as follows.

: x축, y축, z축의 자이로의 각속도 출력값

: Output value of angular velocity of gyros in x, y, z axis

: x축, y축, z축의 각속도 참값

: angular velocity true values in x, y, and z axes

: 외란에 의한 각속도의 변량

: Variation of angular velocity due to disturbance

: x축, y축, z축의 가속도계의 가속도 출력값

: Acceleration output value of accelerometer on x, y, z axis

: x축, y축, z축의 가속도 참값

: Acceleration truth values on x, y, and z axes

: 외란에 의한 가속도의 변량

: Variation of Acceleration by Disturbance

In Equation (1), the output value of the inertia sensor unit 101 includes a variation due to disturbance. In other words, the output value output from the inertia sensor unit 101 to the roughly closed loop sorting module 102 is a true value obtained when measuring in stationary status without disturbance, .

Therefore, when the output value of the inertial sensor unit 101 is used as it is when estimating the azimuth angle, the variation due to the disturbance is reflected in the estimation value of the azimuth angle, resulting in an error, which degrades the performance of the inertial navigation system.

Therefore, in the present invention, the rough closed loop sorting module 102 and the measured shape adaptive filter 103 are used so that the variance due to the disturbance is not reflected in the azimuth estimate. Among them, the configuration of the rough closed loop sorting module 102 And the effect of the action will be described first.

2, the approximate closed loop sorting module 102 according to the present embodiment is a module for calculating a rough initial posture value using the acceleration value input from the inertia sensor unit 101 and the output value of the angular velocity, A rough horizontal alignment module 104 for calculating a roll angle and a pitch angle using an output value of the acceleration, and a rough horizontal alignment module 104 for detecting a signal due to the disturbance from the output values of the acceleration and the angular velocity, A disturbance detection module 105 for calculating a threshold value of a disturbance (hereinafter referred to as a disturbance threshold value), a rough azimuth angle calculation for calculating an azimuth using a horizontal axis roll angle and a control value DCM for obtaining a pitch angle Module 109, as shown in FIG.

The roll angle and the pitch angle can be adjusted to maintain the navigation coordinate system (local east, north, up coordinate system or local local north, east, down coordinate system) 0 " when the posture of the platform is rotated by the attitude conversion matrix.

The disturbance threshold value is provided to the measurement shape adaptive filter 103 and the rough azimuth alignment module 109, respectively.

In addition, the disturbance detecting module 105 is as shown in Fig. 3, calculate the standard deviation using the entire length of the average of a schematic closed loop module 111, the calculated standard deviation with a moving window average module 112, the inertial sensor unit A maximum value calculation module 113 using an instantaneous value , and a disturbance threshold value selection switch 114 .

The standard deviation calculation module 111 using the entire section average calculates an average and a standard deviation by using the following equations (2) and (3), respectively.

In the above equations (2) and (3), the respective symbols are defined as follows.

: 관성센서부에서 출력되는 x축, y축, z축의 각속도의 전체 구간 평균

: Averages of the angular velocity of the x-axis, y-axis, and z-axis output from the inertial sensor unit

: 관성센서부에서 출력되는 ｉ번째의 x축, y축, z축의 각속도

: The angular velocity of the i-th x-axis, y-axis, and z-axis output from the inertia sensor unit

: 관성센서부에서 출력되는 x축, y축, z축의 가속도의 전체 구간 평균

: The average of the acceleration of the x-axis, y-axis, and z-axis output from the inertial sensor unit

: 관성센서부에서 출력되는 ｉ번째의 x축, y축, z축의 가속도

: Acceleration of the i-th x-axis, y-axis, and z-axis output from the inertial sensor unit

: 관성센서부에서 출력되는 x축, y축, z축의 각속도의 ｋ번째 표준편차

: The kth standard deviation of the angular speeds of the x-axis, y-axis, and z-axis output from the inertial sensor unit

: 관성센서부에서 출력되는 x축, y축, z축의 가속도의 ｋ번째 표준편차

: The kth standard deviation of the acceleration in the x, y, and z axes output from the inertia sensor unit

On the other hand, the standard deviation calculation module 112 using the moving window average calculates the average and the standard deviation using the following equations (4) and (5). The size of the window varies depending on the ground alignment operating environment of the platform to which the inertial navigation system is applied.

In the equations (4) and (5), the respective symbols are defined as follows.

: 관성센서부에서 출력되는 x축, y축, z축의 각속도의 이동 윈도우 평균

: Average moving window of angular velocity in x, y, z axis output from inertia sensor

: 관성센서부에서 출력되는 ｉ번째의 x축, y축, z축의 각속도

: The angular velocity of the i-th x-axis, y-axis, and z-axis output from the inertia sensor unit

N: Size of the moving window

: 관성센서부에서 출력되는 x축, y축, z축의 가속도의 이동 윈도우 평균

: The moving window average of the acceleration of the x-axis, y-axis, and z-axis output from the inertial sensor unit

: 관성센서부에서 출력되는 ｉ번째의 x축, y축, z축의 가속도

: Acceleration of the i-th x-axis, y-axis, and z-axis output from the inertial sensor unit

: 관성센서부에서 출력되는 x축, y축, z축의 각속도의 ｋ번째 표준편차

: The kth standard deviation of the angular speeds of the x-axis, y-axis, and z-axis output from the inertial sensor unit

: 관성센서부에서 출력되는 x축, y축, z축의 가속도의 ｋ번째 표준편차

: The kth standard deviation of the acceleration in the x, y, and z axes output from the inertia sensor unit

On the other hand, the maximum value calculation module 113 using the instantaneous value (also referred to as instantaneous value) of the inertial sensor unit 101 calculates the maximum acceleration value and the maximum angular velocity value output from the inertial sensor unit 101 as a disturbance threshold value , And the instantaneous value maximum value may be limited by directly using the instantaneous value maximum value or measuring the vibration characteristic of the platform to which the inertial navigation device is applied in advance.

The disturbance threshold selection switch 114 selects a calculation module that best represents the characteristics of disturbance (vibration, shock, etc.) flowing into the platform on which the inertial navigation device is mounted, among these calculation modules 111 Select.

The disturbance threshold value calculated by the disturbance detection module 105 may be used as a gyroscope reference value and an accelerometer reference value, respectively, according to the vibration characteristics of the platform on which the inertial navigation device is mounted, or these reference values may be used in combination.

When the output value of the inertia sensor unit 101 is smaller than the disturbance threshold value of the disturbance detection module 105, the rough azimuth alignment module 109 is executed to calculate the rough azimuth angle as shown in FIG.

First, after executing the rough horizontal alignment module 104, the disturbance threshold value is calculated in real time by the disturbance detection module 105, and the horizontal alignment real time increase module 106 calculates the disturbance threshold value Horizontal alignment Increases the actual time.

Here, the approximate horizontal alignment module 104 can converge at a time of about 20 seconds although the convergence time of the horizontal axis posture is slightly different depending on the closed loop gain value.

Next, the horizontal alignment time and the horizontal alignment actual time set in accordance with the characteristics of the platform are compared by the horizontal alignment time comparison module 107, and the horizontal alignment real time is roughly horizontally aligned until the horizontal alignment real time becomes longer than the horizontal alignment reference time .

Subsequently, when the output value of the inertia sensor unit 101 is smaller than the disturbance threshold value of the disturbance detection module 105 during the horizontal alignment reference time, the rough azimuth alignment module 109 performs rough azimuth alignment. The comparison of the output value of the inertia sensor section 101 and the disturbance threshold value is performed by the threshold value comparison module 108. [

Next, when the rough azimuth alignment completion module 110 completes the alignment of the azimuth angles, the measurement shape adaptive filter 103 is executed. Otherwise, the approximate azimuth alignment is completed, And continues the above process in the module 102.

In general, when the disturbance continuously flows or the impact is applied on a single occasion, the estimation accuracy of the rough azimuth can be improved by appropriately adjusting the azimuth alignment reference time. In other cases, it is possible to shorten the alignment time of the rough azimuth angle by setting the azimuth alignment reference time as short as possible.

In accurately estimating the rough azimuth angle within a short time, the performance of the rough azimuth alignment module 109 is very important. In particular, since the disturbance introduced during the alignment of the rough azimuth angle greatly affects the estimation accuracy of the rough azimuth angle, it is very important to exclude the influence due to the disturbance.

Therefore, in the rough azimuth alignment module 109 according to the present invention, as shown in FIG. 4, the output value of the inertial sensor unit 101 including the variation due to the disturbance is calculated by the disturbance detection module 105 When the disturbance threshold value is smaller than the threshold value, the rough azimuth calculation module 117 calculates the approximate azimuth angle and increases the azimuth alignment real time of the rough azimuth alignment module 109 by the azimuth alignment real time increasing module 118 .

When the output value of the inertial sensor unit 101 is equal to or greater than the disturbance threshold value, the azimuthal alignment actual time reset module 116 initializes the azimuthal alignment real time of the rough azimuth alignment module 109, And the alignment of the rough azimuth is again performed.

When the actual azimuth alignment real time of the rough azimuth alignment module 109 increases and exceeds the azimuth alignment reference time, the alignment of the rough azimuth is completed. However, if the azimuthal alignment actual time is less than the azimuthal alignment reference time, the output value of the inertial sensor unit 101, for example, the angular velocity / acceleration value, is detected by the disturbance detection module 105 until the azimuthal alignment actual time reaches the azimuthal alignment reference time From the process of comparing whether the calculated disturbance threshold value is smaller or smaller to the process of increasing the azimuth alignment actual time is executed again. Whether or not the azimuth alignment real time of the rough azimuth alignment module 109 exceeds the azimuth alignment reference time is performed by the azimuth alignment time comparison module 119.

The azimuth alignment reference time may be appropriately set in consideration of the characteristics (for example, vibration characteristics) of the platform on which the inertial navigation system is mounted, the performance of the inertial navigation system, the frequency of occurrence of disturbance, etc. However, The time during which the disturbance can be introduced is lengthened so that the estimation accuracy of the rough azimuth angle may be reduced.

The rough azimuth alignment module 109 estimates the rough azimuth angle by the following equation (6).

In Equation (6), each symbol is defined as follows.

: 방위각

: Azimuth angle

: y축 가속도값이 「0」이 나오도록 x축 자이로를 회전시킨 회전량

: The amount of rotation in which the x-axis gyro is rotated so that the y-axis acceleration value becomes "0"

: x축 가속도값이 「0」이 나오도록 y축 자이로를 회전시킨 회전량

: The rotation amount of the y-axis gyro rotated so that the x-axis acceleration value becomes "0"

: 지구 자전 각속도

: Earth rotation angular velocity

: 위도

: Latitude

When the alignment in the rough closed loop alignment module 102 is completed in this manner (referred to as rough closed loop alignment ), the measured shape adaptive filter 103 shown in Fig. 5 is executed, The adaptive measurement covariance shape filter using the induced velocity error value as a measurement is used to update the error model. Adaptive Measurements Covariance shape filters are used to minimize the effects of errors.

In general, the posture error that is not compensated after the rough closed loop alignment is very small.

In addition, the velocity error caused by the position error of the intermediate / precision inertial navigation system is an integral value over time of the acceleration value of 10 to 1000 μg.

In the case of disturbance, the pure velocity error may be smaller than the disturbance. Therefore, the covariance value of the filter should be varied according to the magnitude of the disturbance, and the error model should be updated according to the angular velocity true value and acceleration true value.

That is, when the disturbance is large, the compensation value is kept small so that the velocity error true value is reflected to the error model small by increasing the measurement value covariance. If the disturbance is small, the measurement covariance is decreased, Should be.

For this reason, in the present invention, a measurement shape adaptive filter capable of completing the alignment process in a short time of about 4 minutes regardless of the order of the error model is used. By using such a shape shape adaptive filter, the azimuth estimation time can be shortened and the accuracy of estimation can be improved by 70% or more as compared with the case of using a general filter.

The ground alignment error model is preferably composed of a 12th (order) error model including the velocity error, the attitude error, the bias error of the accelerometer, and the bias error of the gyroscope. However, The degree of the model may be increased.

Generally, the error model of the adaptive filter is expressed by Equation (7) below.

When the measurement value (measurement noise) covariance value R is fixed and used in Equation (7), the output value of the inertia sensor unit 101 is relied on to estimate the error state more accurately as the measurement value covariance value R becomes smaller . However, when the disturbance is introduced, the estimation accuracy of the filter decreases. In addition, when the measurement value covariance value R is large, the output value of the inertia sensor unit is smaller than that of the error state covariance value, and the estimation accuracy is lowered.

Therefore, in a situation where there is vibration and disturbance, alignment performance can be guaranteed only by optimizing the measured value covariance value in real time based on the measured value.

As shown in FIG. 5, the error model is first updated after the Kalman filter is executed by executing the error state estimation extrapolation module 121. [0064] FIG.

The adaptive measurement covariance update module 122 is then executed to update the adaptive measurement covariance update.

Since the ground alignment assumes a stationary status, the true value of speed (true speed value) is "0". Therefore, only the velocity component, which is the integral value of the acceleration output from the inertial sensor unit 101, acts as an error term and uses it as a measurement value of the filter, as shown in the following equation (9).

As shown in FIG. 6, the adaptive measurement covariance update module 122 calculates a cumulative average of the cumulative averages of averages of the acceleration output from the inertial sensor unit 101 over the entire section in which the adaptive filter is operated, A measurement covariance update module 129 using a moving window average that continuously obtains an average of a moving window by covering a constant moving window on a velocity measurement value and a measurement covariance updating module 129 using an instantaneous value of the inertial sensor portion 101 And a measurement covariance selection switch 131. The measurement covariance update module 130 includes a measurement covariance update module 130 and a measurement value covariance selection switch 131. [

The adaptive measurement covariance update module 122 calculates the covariance by using the following equations (8) to (10).

The measurement covariance selection switch 131 selects a module that best reflects the ground alignment condition of the platform equipped with the inertial navigation device among the above three types of measurement value covariance update modules 128, 129 and 130. For example, when the disturbance introduced from the platform is a one-shot vibration or an impact, the measurement covariance update module 129 using the moving window average or the measurement covariance update module 130 using the instantaneous values of the inertial sensor portion 101 is selected, If the disturbance continuously flows, the measurement covariance update module 128 using the total interval cumulative average is selected.

By using the adaptive measurement covariance update module 122, the Kalman gain for disturbance introduced from the outside can be flexibly adjusted. In addition, the measurement covariance update module 128 using the total interval cumulative average and the measurement value covariance update module 129 using the moving window average have an advantage that the estimated value of the filter is kept relatively stable. The inertial sensor unit 101, The measurement covariance update module 130 using the instantaneous value of the instantaneous value is advantageous in that the response speed to disturbance is high.

Returning to FIG. 5, after updating the adaptive measurement covariance value by the adaptive measurement covariance update module 122, the error covariance extrapolation module 123 is executed to update the transparency error covariance value.

The Kalman gain evaluation module 124 is then executed to calculate the Kalman gain.

For calculation of the Kalman gain, an error state covariance value, a processing error covariance value, and a measured value (measurement noise) covariance value are used.

The Kalman gain is determined to minimize the effect of the disturbance due to the influence of the measurement covariance.

Then, the error-state covariance update module 125 is executed to update the error-state covariance value and to determine whether to update the error-state value based on a previously calculated reference value. At this time, the previously calculated Kalman gain is used.

When it is determined to update the error state value based on the reference value, the error state update module 126 is executed to determine whether to update the error state equation or only the error state covariance value.

The reference value for determining whether to update the error state equation or to propagate only the error state covariance value includes standard deviation using the total interval average calculated at the rough closed loop alignment, standard deviation using the moving window average, instantaneous value of the inertia sensor portion , The reference value having the weighted value of the earth's rotational angular velocity, and the reference value having the weighted value of the gravitational acceleration, the disturbance characteristic flowing into the platform equipped with the inertial navigation system is taken into consideration, or a combination thereof is used.

The selection of the reference value is made by the reference value selection switch 134 as shown in Fig.

When the output value of the inertia sensor unit 101 is smaller than the reference value selected by the reference value selection switch 134, the error state equation is updated.

In other cases, only the error state covariance value is propagated.

The earth rotation angular velocity to which the weight is applied is calculated by the earth rotation angular velocity calculation module 132 as shown in Equation (11) below, and the gravity acceleration applied with the weight is calculated by the gravity acceleration calculation module 133 (12). &Quot; (12) "

The weights applied to the earth's rotational angular velocity and gravitational acceleration are set in consideration of the accuracy of the inertial navigation system.

In the above equations (11) and (12), the respective symbols are defined as follows.

: 지구 회전 각속도

: Earth rotation angular velocity

: 중력 가속도

: Gravitational acceleration

As a matter of course, when the disturbance flows into the inertial sensor unit 101, both the earth rotational angular velocity value output from the inertial sensor unit 101 and the gravitational acceleration value are changed.

When the earth rotational angular velocity value output from the inertial sensor unit 101 is compared with a reference value obtained by applying a weight to the ideal angular velocity value at the current alignment position and the earth rotational angular velocity value output from the inertial sensor unit 101 is small, Update the equation.

On the other hand, when the gravity acceleration value output from the inertia sensor unit 101 is compared with the reference value obtained by applying the weight to the ideal acceleration value at the current alignment position and the gravity acceleration value output from the inertia sensor unit 101 is smaller than the reference value The error state is updated, and if it exceeds the reference value, only the error state covariance value is propagated.

101 ... inertial sensor unit
102 ... roughly closed loop alignment module
103 ... Measured shape adaptive filter
104 ... rough horizontal alignment module
105 ... Disturbance detection module
106 ... Horizontal alignment real time increment module
107 ... horizontal alignment time comparison module
108 ... threshold comparison module
109 ... rough azimuth alignment module
110 ... rough azimuth alignment completion module
111 ... Standard Deviation Calculation Module Using Whole Section Average
112 ... Standard deviation calculation module using moving window average
113 ... Maximum value calculation module using instantaneous value of inertia sensor part
114 ... disturbance threshold selection switch
116 ... azimuth alignment real time reset module
117 ... rough azimuth calculation module
118 ... azimuth alignment real time increment module
119 ... azimuth alignment time comparison module
121 ... error state estimation extrapolation module
122 ... adaptive measure covariance update module
123 ... error covariance extrapolation module
124 ... Kalman gain evaluation module
125 ... error state covariance update module
126 ... error state update module
128 ... Measurement covariance update module using cumulative average of all sections
129 ... Measurement covariance update module using moving window average
130 ... measurement value covariance update module using the instantaneous value of the inertia sensor part
131 ... Measurement covariance selection switch
132 ... Earth rotation angular velocity calculation module
133 ... gravity acceleration calculation module
134 ... reference value selection switch

Claims (6)

An approximate closed loop sorting module for calculating a rough initial posture value by using an acceleration value and an angular velocity value input from an inertia sensor part composed of an accelerometer and a gyroscope; And
A shaping adaptive filter that uses the velocity error value caused by the attitude error value and the disturbance value included therein as a measurement value after the approximate closed loop alignment in the approximate closed loop alignment module is completed;
And,
The roughly closed loop sorting module comprises:
A rough horizontal alignment module for calculating a roll angle and a pitch angle using an acceleration value input from the inertial sensor;
A disturbance detection module for detecting a disturbance signal from an acceleration value and an angular velocity value input from the inertia sensor and calculating an allowable disturbance threshold;
A rough azimuth alignment module for calculating an azimuth angle using an acceleration value and an angular velocity value input from the inertial sensor;
A reference value calculation method for varying the measurement covariance in real time according to the velocity error caused by the error of the attitude value calculated in the closed loop sorting module and the disturbance value included therein and determining whether to update the error state equation A fast adaptive mechanism for inertial navigation systems using a measurement adaptive filter and a disturbance detection technique. The method of claim 1,
Wherein the disturbance detecting module comprises:
A standard deviation calculation module using the entire interval average of the closed loop,
A standard deviation calculation module using a moving window average,
A maximum value calculation module using an instantaneous value of the inertia sensor part,
And a disturbance threshold selection switch for selecting any one of the three types of calculation modules and a combination of the disturbance threshold selection switches according to the characteristics of disturbance introduced into the platform equipped with the inertial navigation device. Robust alignment mechanism for inertial navigation systems. The method according to claim 1,
Wherein the rough azimuth alignment module comprises:
When the angular velocity / acceleration value inputted from the inertial sensor unit is smaller than the disturbance threshold value calculated by the disturbance detection module, the approximate azimuth angle is calculated, the alignment real time of the rough azimuth alignment module is increased,
If the angular velocity / acceleration value input from the inertial sensor unit is equal to or greater than the disturbance threshold value calculated by the disturbance detection module, the alignment real time of the general azimuth alignment module is initialized to initialize the general azimuth alignment module, Lt; / RTI &
When the actual azimuth alignment time of the rough azimuth alignment module exceeds the azimuth alignment reference time, the rough azimuth alignment is completed. If the azimuth alignment actual time is less than the azimuth alignment reference time, the azimuth alignment is performed until the actual time reaches the azimuth alignment reference time. And a step of comparing the angular velocity / acceleration value input from the inertial sensor unit and the disturbance threshold value calculated by the disturbance detection module to a process of increasing the azimuth alignment real time. A rapid rapid alignment mechanism for inertial navigation systems with adaptive technique. The method according to claim 1,
Wherein the measurement shape adaptive filter comprises:
An adaptive measurement covariance update module for optimizing a measured value covariance value in real time based on the measured value when disturbance is introduced,
A Kalman gain evaluation module for calculating a Kalman gain using an error state covariance value, a processing error covariance value, and an adaptive measurement value covariance value;
And an error state update module for updating the error state covariance value using the Kalman gain and determining whether to update the error state value based on a previously calculated reference value. Robust Alignment Mechanism for Inertial Navigation System Using Disturbance Detection Technique. 5. The method of claim 4,
Wherein the adaptive measurement covariance update module comprises:
A measurement covariance update module that uses a total interval cumulative average to obtain an covariance by averaging integral values of acceleration output from the inertial sensor unit over the entire section in which the adaptive filter is operated;
A measurement covariance update module using a moving window average that continuously obtains an average of a moving window by covering a constant moving window with a velocity measurement value;
The measurement covariance update module using the instantaneous value of the inertial sensor part (instantaneous value can be limited to the previously measured system disturbance characteristic value) and the ground alignment condition of the platform equipped with inertial navigation device, the three types of measurement covariance update And a measurement value covariance selection switch for selecting any one or a combination of the modules. The adaptive filter and the robust alignment mechanism for the inertial navigation system using the disturbance detection technique. 5. The method of claim 4,
Wherein the error state covariance update module comprises:
A standard deviation calculation module using the entire interval average of the closed loop,
A standard deviation calculation module using a moving window average,
A maximum value calculation module using an instantaneous value of the inertia sensor part,
A earth rotation angular velocity calculation module for calculating the earth rotation angular velocity to which the weight is applied,
A gravity acceleration calculation module for calculating a gravity acceleration to which a weight is applied,
And a reference value selection switch for selecting or combining any one of the five types of calculation modules according to the disturbance characteristic introduced into the platform equipped with the inertial navigation device. The inertial navigation device using the adaptive filter and the disturbance detection technique Fast solid alignment mechanism.

Images