Self-adaptive estimation and compensation method for zero position of inertial device of inertial navigation system

Abstract

The invention relates to a zero-position self-adaptive estimation and compensation method for an inertial device of an inertial navigation system. The method comprises the following steps: 1, completing installation and electrical connection of inertial navigation and a matched integrated navigation system; 2, starting and electrifying the inertial navigation and matched integrated navigation system to finish inertial navigation initial alignment; 3, enabling the inertial navigation to enter a combined navigation state, and enabling the carrier vehicle or ship to enter an advancing or sailing state; according to the information type of the matched integrated navigation system, selecting a corresponding mode to carry out integrated navigation inertial device zero estimation; 4, in the zero position estimation process, performing external reference information validity judgment on the matched integrated navigation system all the time, and if the external reference information validity judgment is invalid, judging that the zero position estimation result of the inertial device is invalid, and not compensating the zero position of the inertial device; otherwise, entering the step 5 to judge the effectiveness of the zero estimation of the accelerometer and entering the step 6 to judge the effectiveness of the zero estimation of the gyroscope. According to the method, the rationality self-adaptive judgment of the zero estimation path of the inertial device and the effectiveness self-adaptive judgment of the zero estimation result can be realized.

Description

Technical Field

[0001] The invention belongs to the field of inertial navigation inertial device zero position estimation, and in particular relates to an inertial navigation system inertial device zero position adaptive estimation and compensation method. Background Art

[0002] The inertial navigation system is an autonomous navigation system whose main error source is the zero position error of the inertial device. The existence of the zero position of the inertial device (including the gyro zero position and the accelerometer zero position) will directly lead to the reduction of the navigation information accuracy of the inertial navigation system over time. Since the zero position of the inertial device is prone to error divergence during long-term operation, compensation for its zero position can effectively suppress this error, thereby improving navigation accuracy; in addition, compensation for the zero position of the inertial device can improve the stability of the system, ensure the stability of the system during long-term operation, and avoid navigation deviations caused by error accumulation.

[0003] The existing inertial navigation system inertial device zero position estimation method requires the operator to manually judge the rationality of the estimated path and the effectiveness of the inertial device zero position estimation, so as to make compensation when the estimation is valid. It requires the operator to have a certain inertial navigation knowledge base and is not very practical. Summary of the invention

[0004] In view of the shortcomings of the prior art, the present invention proposes an inertial navigation system inertial device zero position adaptive estimation and compensation method, which can realize the adaptive judgment of the rationality of the inertial device zero position estimation path and the adaptive judgment of the effectiveness of the zero position estimation result, and perform online compensation for the effectively estimated inertial device zero position, thereby improving the long-term navigation accuracy of the inertial navigation system.

[0005] The above-mentioned object of the present invention is achieved by the following technical solutions:

[0006] A method for adaptively estimating and compensating zero position of an inertial device of an inertial navigation system, characterized in that it comprises the following steps:

[0007] Step 1: Install the inertial navigation system on the vehicle or ship, and install its supporting integrated navigation system on the test vehicle or ship according to its installation method, and complete the electrical connection between the inertial navigation system and the corresponding integrated navigation system;

[0008] Step 2: Power on the INS and the supporting integrated navigation system, complete the initial alignment of the INS, ensure that the INS can output valid speed, position and attitude information, and the supporting integrated navigation system can output valid reference information;

[0009] Step 3: The inertial navigation enters the integrated navigation state, and the vehicle or ship enters the traveling or sailing state; according to the information type of the supporting integrated navigation system, the corresponding mode is selected to perform the zero position estimation of the integrated navigation inertial device;

[0010] Step 4: During the process of estimating the zero position of the inertial device in the integrated navigation state, the validity judgment of the external reference information of the supporting integrated navigation system is always performed. If the validity judgment of the external reference information is invalid, the zero position estimation result of the inertial device of this group is judged to be invalid, and the zero position of the inertial device is not compensated; if the validity judgment of the external reference information is valid, the process proceeds to step 5 to perform the validity judgment of the accelerometer zero position estimation and to step 6 to perform the validity judgment of the gyro zero position estimation;

[0011] Step 5: judging the validity of the accelerometer zero position estimation according to the set criterion;

[0012] Step 6: Determine the validity of the gyro zero position estimation based on the set criteria.

[0013] Moreover, in step 3, if the supporting combined navigation system is a satellite navigation, the position combination mode is entered to perform the inertial device zero position estimation; if the supporting combined navigation system is an odometer or DVL, the speed combination mode is entered to perform the inertial device zero position estimation; the inertial device zero position estimation method of the position combination and speed combination modes establishes the state equation and the measurement equation based on the characteristics of the inertial navigation and combined information, and adopts the Kalman filtering method to perform the inertial device zero position estimation.

[0014] Moreover, in step 4, different validity criteria are designed according to the characteristics of different external reference information as follows:

[0015] c) The validity criteria for the external reference information of the satellite navigation system provided by the integrated navigation system are:

[0016] (3) Judgment of single continuous failure time of external reference satellite guidance data:

[0017] After starting the position observation integrated navigation to estimate the zero position of the inertial device, the single continuous failure time of the satellite navigation data shall not exceed 1 minute, otherwise it is judged that the current estimation fails;

[0018] (4) Determination of the total failure time of external reference satellite guidance data:

[0019] After starting the position observation integrated navigation to estimate the zero position of the inertial device, the total failure time of the satellite navigation data shall not exceed 10 minutes, otherwise it is judged that this estimation has failed;

[0020] The satellite navigation data failure is judged by the satellite navigation data valid bit, the consistency of satellite navigation data before and after, the consistency of satellite navigation position and inertial navigation position, etc. When both conditions (1) and (2) in a) are met at the same time, the satellite navigation external reference information is considered valid. Otherwise, it is judged that the satellite navigation external reference information is invalid, and the zero position estimation result of the inertial device is invalid.

[0021] d) The validity criterion of the external reference information of the DVL provided by the integrated navigation system

[0022] (3) Determination of single continuous failure time by external reference DVL data:

[0023] After starting the velocity observation integrated navigation to estimate the zero position of the inertial device, the single continuous failure time of the DVL data shall not exceed 1 minute, otherwise it is judged as the current estimation failure;

[0024] (4) Determination of total failure time of external reference DVL data:

[0025] After starting the velocity observation integrated navigation to estimate the zero position of the inertial device, the total failure time of the DVL data shall not exceed 10 minutes, otherwise it is considered that the estimation has failed.

[0026] Among them, the failure of DVL data can be judged by the valid bits of DVL data and the consistency of DVL data before and after. When both conditions (1) and (2) in b) are met at the same time, the DVL external reference information is considered valid. Otherwise, it is determined that the DVL external reference information is invalid, and the zero position estimation result of the inertial device is invalid.

[0027] c) Do not set the validity criteria for the external reference of the odometer, and the default odometer information is always valid.

[0028] Moreover, in step 5, the specific criteria are as follows:

[0029] e) Total time to enter the combined navigation state:

[0030] (3) For the position observation mode of the satellite navigation system as a supporting integrated navigation system, the total time of the integrated navigation state is greater than 3600s;

[0031] (4) For the speed observation mode of the integrated navigation system with DVL or odometer, the total time of the integrated navigation state is greater than 5400 seconds;

[0032] f) Judgment of two effective linear motion positions and heading change:

[0033] (4) First position direct flight judgment:

[0034] If the maximum difference of the inertial navigation heading is less than 40° within 20 consecutive minutes, it is judged that the first position direct flight is completed, and the turning judgment from the first position to the second position is turned;

[0035] (5) Determination of turning from the first position to the second position:

[0036] If the difference between the 1Hz heading value of the inertial navigation for 3 consecutive seconds and the average heading value of the first position is greater than 60°, it is judged that the turning state has been entered; if the difference between two adjacent 1Hz heading values ​​of the inertial navigation for 10 consecutive seconds is less than 0.2°, it is judged that the turn from the first position to the second position is completed, and the judgment of direct flight at the second position is entered;

[0037] (6) Second position direct flight judgment:

[0038] After completing the turn from the first position to the second position, for the position observation mode with satellite navigation as the supporting combined navigation system, if the maximum value difference of the inertial navigation heading is less than 120° within 40 consecutive minutes, and the difference between the mean heading of the second position and the mean heading of the first position is greater than 60°, it is judged that the direct flight to the second position is completed; for the speed observation mode with DVL or odometer as the supporting combined navigation system, if the maximum value difference of the inertial navigation heading is less than 120° within 1 consecutive hour, and the difference between the mean heading of the second position and the mean heading of the first position is greater than 60°, it is judged that the direct flight to the second position is completed.

[0039] When the three conditions (1), (2) and (3) in b) are met, it is judged that the two effective linear motion position judgment and heading change judgment criteria are met;

[0040] g) Accelerometer zero position P array judgment:

[0041] Error variance matrix P in integrated navigation Kalman filter 15×15 In the array, the accelerometer x zero position ▽ x and accelerometer y zero position ▽ y If the RMS of the diagonal elements of the corresponding dimension are not greater than 50 μg, it is judged that the accelerometer zero position P matrix criterion is met, as shown in the following formula:

[0042]

[0043] h) Determination of the estimated zero position value of the accelerometer:

[0044] Accelerometer x and accelerometer y zero position estimates and If the absolute values ​​are all less than 5000 μg, it is judged that the accelerometer zero value criterion is met, as shown in the following formula:

[0045]

[0046] If the above four judgment conditions a), b), c), and d) are met at the same time, the accelerometer zero position estimation of this group is determined to be valid, and the accelerometer zero position estimation can be performed. x and y feedback compensation; otherwise, it is determined that the accelerometer zero position estimation is invalid and the accelerometer zero position estimation is not performed. x and y Feedback compensation.

[0047] Moreover, in step 6, the specific criteria are as follows:

[0048] e) Total time to enter the combined navigation state:

[0049] For the position observation mode with satellite navigation as the supporting integrated navigation system or the speed observation mode with DVL / odometer as the supporting integrated navigation system, the total time of the integrated navigation state is greater than 7200s;

[0050] f) Judgment of two effective linear motion positions and heading change:

[0051] (4) First position direct flight judgment:

[0052] If the maximum difference of the inertial navigation heading is less than 40° within 20 consecutive minutes, it is judged that the first position direct flight is completed, and the turning judgment from the first position to the second position is turned;

[0053] (5) Determination of turning from the first position to the second position:

[0054] If the difference between the 1Hz heading value of the inertial navigation for 3 consecutive seconds and the average heading value of the first position is greater than 60°, it is judged that the turning state has been entered; if the difference between two adjacent 1Hz heading values ​​of the inertial navigation for 10 consecutive seconds is less than 0.2°, it is judged that the turn from the first position to the second position is completed, and the judgment of direct flight at the second position is entered;

[0055] (6) Second position direct flight judgment:

[0056] After completing the turn from the first position to the second position, for the position observation mode with satellite navigation as the supporting combined navigation system, within 1 hour, the maximum value difference of the inertial navigation heading is less than 120°, and the difference between the mean heading of the second position and the mean heading of the first position is greater than 60°, then it is judged that the direct flight to the second position is completed; for the speed observation mode with DVL or odometer as the supporting combined navigation system, within 80 minutes, the maximum value difference of the inertial navigation heading is less than 120°, and the difference between the mean heading of the second position and the mean heading of the first position is greater than 60°, then it is judged that the direct flight to the second position is completed.

[0057] When the three conditions (1), (2) and (3) in the above b) are met, it is judged that the two effective linear motion position judgment and heading change judgment criteria are met;

[0058] g) Gyro zero position P array judgment:

[0059] Error variance matrix P in integrated navigation Kalman filter 15×15 In the array, the gyro x zero position ε x and gyro y zero position ε y If the RMS of the diagonal elements of the corresponding dimension are not greater than 0.03° / h, it is judged that the gyro zero position P matrix criterion is met, as shown in the following formula:

[0060]

[0061] h) Determination of gyro estimated zero position value:

[0062] The estimated zero position of gyro x and gyro y and If the absolute values ​​are all less than 0.2° / h, it is judged that the gyro zero position value criterion is met, as shown in the following formula:

[0063]

[0064] If the above four judgment conditions a), b), c) and d) are met at the same time, the accelerometer zero position estimation of this group is determined to be valid and feedback compensation can be performed; otherwise, the gyro zero position estimation is determined to be invalid.

[0065] The advantages and positive effects of the present invention are:

[0066] The present invention aims at the typical combined navigation mode of the inertial navigation system, namely the satellite navigation combined navigation mode and the speed measuring device combined navigation mode, combines the error principle of the inertial navigation system and the characteristics of the combined navigation observation information, and proposes an inertial device zero position adaptive estimation and compensation method compatible with multiple combined modes, realizes the adaptive judgment of the rationality of the inertial device zero position estimation path and the adaptive judgment of the effectiveness of the zero position estimation result, effectively solves the problem of the inertial device zero position adaptive estimation and compensation of the inertial navigation system, can perform online compensation for the effectively estimated inertial device zero position, and improves the long-term navigation accuracy of the inertial navigation system. The method can be applied to multiple application fields such as vehicle-borne and ship-borne, and is applicable to inertial navigation systems of different accuracy levels, and has strong practicality and high engineering application value. BRIEF DESCRIPTION OF THE DRAWINGS

[0067] Figure 1 The invention is a processing flow chart of a method for adaptively estimating and compensating a zero position of an inertial device of an inertial navigation system. DETAILED DESCRIPTION

[0068] The structure of the present invention is further described below with reference to the accompanying drawings and by way of examples. It should be noted that the present examples are descriptive rather than restrictive.

[0069] A method for adaptive zero position estimation and compensation of inertial devices in an inertial navigation system, see Figure 1 , the invention point is that it comprises the following steps:

[0070] Step 1: Install the inertial navigation system (full name: "inertial navigation system") on the vehicle or ship, and install its supporting integrated navigation system (such as satellite navigation, DVL or odometer) on the test vehicle or ship according to its installation method, and complete the electrical connection between the inertial navigation system and the corresponding integrated navigation system;

[0071] Step 2: The inertial navigation system and the supporting integrated navigation system are powered on. The inertial navigation system completes the initial alignment (the initial alignment methods such as vehicle-mounted static, vehicle-mounted dynamic, and ship-mounted mooring are all available), and can output valid speed, position, and attitude information; the supporting integrated navigation system can output valid reference information, and any errors related to the installation arm and time synchronization can be compensated in advance;

[0072] Step 3, the inertial navigation enters the combined navigation state, and the vehicle or ship can enter the moving or sailing state. According to the information type of the supporting combined navigation system, select the corresponding mode to perform the combined navigation inertial device zero position estimation: if the supporting combined navigation system is a satellite navigation, enter the position combination mode to perform the inertial device zero position estimation; if the supporting combined navigation system is an odometer or DVL, enter the speed combination mode to perform the inertial device zero position estimation. The inertial device zero position estimation method of the position combination and speed combination modes establishes the state equation and measurement equation based on the characteristics of the inertial navigation and combined information, and adopts the Kalman filtering method to perform the inertial device zero position estimation. The specific state variable X is defined as shown in the following formula:

[0073]

[0074] in:

[0075] δV E , δV N , δV U Calculate the east, north and celestial velocity errors for inertial navigation respectively;

[0076] δL, δλ, and δh are the latitude, longitude, and altitude errors of the inertial navigation solution, respectively;

[0077] φ E ,φ N ,φ U are attitude misalignment angles;

[0078] ε x , ε y , ε z is the gyro zero position;

[0079] ▽ x ,▽ y ,▽ z is the accelerometer zero position.

[0080] The state equation of the inertial device zero position estimation is as follows:

[0081]

[0082] The inertial device zero position estimation measurement equation is as follows:

[0083] Z p =H p X+V (3)

[0084] Z v =H v X+V

[0085] Wherein: (3) above is the measurement equation for the position observation mode when the integrated navigation system is a satellite navigation system, (3) below is the measurement equation for the speed observation mode when the integrated navigation system is a DVL or odometer.

[0086] The error model of the inertial navigation integrated navigation system established by equations (2) and (3) is a commonly used model in the field of inertial navigation and will not be elaborated here. The Kalman filtering method is used for the above error model to perform real-time online estimation of the zero position of the inertial device.

[0087] Step 4: During the process of estimating the zero position of the inertial device in the integrated navigation state, the validity judgment of the external reference information of the supporting integrated navigation system is always performed. If the validity judgment of the external reference information is invalid, the zero position estimation result of the inertial device of this group is judged to be invalid, and the zero position of the inertial device is not compensated. If the validity judgment of the external reference information is valid, the process proceeds to step 5 to perform the validity judgment of the accelerometer zero position estimation and to step 6 to perform the validity judgment of the gyro zero position estimation. Different validity criteria are designed according to the characteristics of different external reference information as follows:

[0088] e) The validity criterion of the external reference information of the satellite navigation system as the supporting integrated navigation system

[0089] (5) Judgment of single continuous failure time of external reference satellite guidance data:

[0090] After starting the position observation integrated navigation to estimate the zero position of the inertial device, the single continuous failure time of the satellite navigation data shall not exceed 1 minute, otherwise it is judged that the current estimation fails;

[0091] (6) Determination of the total failure time of external reference satellite guidance data:

[0092] After starting the position observation integrated navigation to estimate the zero position of the inertial device, the total failure time of the satellite navigation data shall not exceed 10 minutes, otherwise it is judged that this estimation has failed;

[0093] The satellite navigation data failure can be judged by the satellite navigation data valid bit, the consistency of satellite navigation data, the consistency of satellite navigation position and inertial navigation position, etc. When both conditions (1) and (2) above are met at the same time, the satellite navigation external reference information is considered valid, otherwise it is judged that the satellite navigation external reference information is invalid, and the zero position estimation result of the inertial device is invalid.

[0094] f) The validity criterion of the external reference information of the DVL provided by the integrated navigation system

[0095] (5) Determination of single continuous failure time by external reference DVL data:

[0096] After starting the velocity observation integrated navigation to estimate the zero position of the inertial device, the single continuous failure time of the DVL data shall not exceed 1 minute, otherwise it is judged as the current estimation failure;

[0097] (6) Determination of total failure time of external reference DVL data:

[0098] After starting the velocity observation integrated navigation to estimate the zero position of the inertial device, the total failure time of the DVL data shall not exceed 10 minutes, otherwise it is considered that the estimation has failed.

[0099] The DVL data failure can be judged by the DVL data valid bits, the consistency of the DVL data, etc. When both conditions (1) and (2) above are met, the DVL external reference information is considered valid. Otherwise, the DVL external reference information is considered invalid, and the zero position estimation result of the inertial device is invalid.

[0100] g) Validity criterion of external reference information for odometer in the integrated navigation system

[0101] The odometer is installed on the vehicle's drive shaft to measure the vehicle's mileage and is less susceptible to external interference. Therefore, no odometer external reference validity criterion is set and the odometer information is always valid by default.

[0102] Step 5: If the external reference information validity criterion of step 4 is met during the zero position estimation process of the inertial device in the integrated navigation state, the accelerometer zero position estimation validity judgment is performed. The specific criteria are as follows:

[0103] i) Total time to enter the combined navigation state:

[0104] (5) For the position observation mode of the satellite navigation system, the total time of the integrated navigation state is greater than 3600s

[0105] (1h);

[0106] (6) For the speed observation mode of the integrated navigation system with DVL or odometer as the supporting system, the total time of the integrated navigation state is greater than 5400 seconds (1.5 hours).

[0107] j) Judgment of two effective linear motion positions and heading change:

[0108] (7) First position direct flight judgment:

[0109] If the maximum difference of the inertial navigation heading is less than 40° within 20 consecutive minutes, it is judged that the first position direct flight is completed, and the turning judgment from the first position to the second position is turned;

[0110] (8) Determination of turning from the first position to the second position:

[0111] If the difference between the 1Hz heading value of the inertial navigation for 3 consecutive seconds and the average heading value of the first position is greater than 60°, it is judged that the turning state has been entered; if the difference between two adjacent 1Hz heading values ​​of the inertial navigation for 10 consecutive seconds is less than 0.2°, it is judged that the turn from the first position to the second position is completed, and the judgment of direct flight at the second position is entered;

[0112] (9) Second position direct flight judgment:

[0113] After completing the turn from the first position to the second position, for the position observation mode with satellite navigation as the supporting combined navigation system, if the maximum value difference of the inertial navigation heading is less than 120° within 40 consecutive minutes, and the difference between the mean heading of the second position and the mean heading of the first position is greater than 60°, it is judged that the direct flight to the second position is completed; for the speed observation mode with DVL or odometer as the supporting combined navigation system, if the maximum value difference of the inertial navigation heading is less than 120° within 1 consecutive hour, and the difference between the mean heading of the second position and the mean heading of the first position is greater than 60°, it is judged that the direct flight to the second position is completed.

[0114] When the above three conditions (1), (2) and (3) are met, it is judged that the two effective linear motion position judgment and heading change judgment criteria are met.

[0115] k) Accelerometer zero position P array judgment:

[0116] Error variance matrix P in integrated navigation Kalman filter 15×15 In the array, the accelerometer x zero position ▽ x and accelerometer y zero position ▽ y If the RMS of the diagonal elements of the corresponding dimension are all less than 50 μg, it is judged that the accelerometer zero position P matrix criterion is met, as shown in the following formula. The accelerometer zero position estimation error variance threshold should be adjusted according to the accuracy of the accelerometer of the inertial navigation system used;

[0117]

[0118] l) Determination of the estimated zero position value of the accelerometer:

[0119] Accelerometer x and accelerometer y zero position estimates and If the absolute values ​​are all less than 5000 μg, it is judged that the accelerometer zero position value criterion is met, as shown in the following formula. The absolute value threshold of the accelerometer zero position estimate should be adjusted according to the accuracy of the accelerometer of the inertial navigation system used;

[0120]

[0121] If the above four judgment conditions a), b), c), and d) are met at the same time, the accelerometer zero position estimation of this group is determined to be valid, and the accelerometer zero position estimation can be performed. x and yThe feedback compensation can adopt the common means of error estimation feedback of the inertial navigation system, which will not be elaborated here; otherwise, it is determined that the accelerometer zero position estimation is invalid, and the accelerometer zero position estimation is not performed. x and y Feedback compensation.

[0122] Step 6: If the external reference information validity criterion of step 4 is met during the zero position estimation process of the inertial device in the integrated navigation state, the validity judgment of the gyro zero position estimation is performed. The specific criteria are as follows:

[0123] i) Total time to enter the combined navigation state:

[0124] For the position observation mode with satellite navigation as the supporting integrated navigation system or the speed observation mode with DVL / odometer as the supporting integrated navigation system, the total time of the integrated navigation state is greater than 7200s (2h).

[0125] j) Judgment of two effective linear motion positions and heading change:

[0126] (7) First position direct flight judgment:

[0127] If the maximum difference of the inertial navigation heading is less than 40° within 20 consecutive minutes, it is judged that the first position direct flight is completed, and the turning judgment from the first position to the second position is turned;

[0128] (8) Determination of turning from the first position to the second position:

[0129] If the difference between the 1Hz heading value of the inertial navigation for 3 consecutive seconds and the average heading value of the first position is greater than 60°, it is judged that the turning state has been entered; if the difference between two adjacent 1Hz heading values ​​of the inertial navigation for 10 consecutive seconds is less than 0.2°, it is judged that the turn from the first position to the second position is completed, and the judgment of direct flight at the second position is entered;

[0130] (9) Second position direct flight judgment:

[0131] After completing the turn from the first position to the second position, for the position observation mode with satellite navigation as the supporting combined navigation system, within 1 hour, the maximum value difference of the inertial navigation heading is less than 120°, and the difference between the mean heading of the second position and the mean heading of the first position is greater than 60°, then it is judged that the direct flight to the second position is completed; for the speed observation mode with DVL or odometer as the supporting combined navigation system, within 80 minutes, the maximum value difference of the inertial navigation heading is less than 120°, and the difference between the mean heading of the second position and the mean heading of the first position is greater than 60°, then it is judged that the direct flight to the second position is completed.

[0132] When the above three conditions (1), (2) and (3) are met, it is judged that the two effective linear motion position judgment and heading change judgment criteria are met.

[0133] k) Gyro zero position P array judgment:

[0134] Error variance matrix P in integrated navigation Kalman filter 15×15 In the array, the gyro x zero position ε x and gyro y zero position ε y If the RMS of the diagonal elements of the corresponding dimensions are all less than 0.03° / h, it is judged that the gyro zero position P matrix criterion is met, as shown in the following formula. The gyro zero position estimation error variance threshold should be adjusted according to the gyro accuracy of the inertial navigation system used.

[0135]

[0136] l) Determination of gyro estimated zero position value:

[0137] The estimated zero position of gyro x and gyro y and If the absolute values ​​are all less than 0.2° / h, it is judged that the gyro zero position numerical criterion is met, as shown in the following formula. The absolute value threshold of the gyro zero position estimate should be adjusted according to the gyro accuracy of the inertial navigation system used).

[0138]

[0139] If the corresponding criteria of the above four judgment conditions a), b), c) and d) are met at the same time, it is determined that the zero position estimation of this group of accelerometers is valid and feedback compensation can be performed. The feedback compensation can adopt the general means of error estimation feedback of the inertial navigation system, which will not be elaborated here. Otherwise, it is determined that the gyro zero position estimation is invalid.

[0140] Although the embodiments and drawings of the present invention are disclosed for illustrative purposes, those skilled in the art will appreciate that various substitutions, changes and modifications are possible without departing from the spirit and scope of the present invention and the appended claims. Therefore, the scope of the present invention is not limited to the contents disclosed in the embodiments and drawings.

Claims

1. A method for adaptively estimating and compensating zero position of inertial devices of an inertial navigation system, characterized in that: The steps include: Step 1: Install the inertial navigation system on the vehicle or ship, and install its supporting integrated navigation system on the test vehicle or ship according to its installation method, and complete the electrical connection between the inertial navigation system and the corresponding integrated navigation system; Step 2: Power on the INS and the supporting integrated navigation system, complete the initial alignment of the INS, ensure that the INS can output valid speed, position and attitude information, and the supporting integrated navigation system can output valid reference information; Step 3: The inertial navigation enters the integrated navigation state, and the vehicle or ship enters the traveling or sailing state; according to the information type of the supporting integrated navigation system, the corresponding mode is selected to perform the zero position estimation of the integrated navigation inertial device; Step 4: During the process of estimating the zero position of the inertial device in the integrated navigation state, the validity judgment of the external reference information of the supporting integrated navigation system is always performed. If the validity judgment of the external reference information is invalid, it is judged that the zero position estimation result of the inertial device of this group is invalid, and the zero position of the inertial device is not compensated; If the validity of the external reference information is judged to be valid, then proceed to step 5 to judge the validity of the accelerometer zero position estimation and proceed to step 6 to judge the validity of the gyro zero position estimation; Step 5: judging the validity of the accelerometer zero position estimation according to the set criterion; Step 6: Determine the validity of the gyro zero position estimation based on the set criteria.

2. The method for adaptively estimating and compensating zero position of inertial device of inertial navigation system according to claim 1, characterized in that: In step 3, if the supporting integrated navigation system is satellite navigation, the position combination mode is entered to perform inertial device zero position estimation; if the supporting integrated navigation system is an odometer or DVL, the speed combination mode is entered to perform inertial device zero position estimation; the inertial device zero position estimation method of the position combination and speed combination modes establishes state equations and measurement equations based on the characteristics of inertial navigation and combined information, and uses the Kalman filtering method to perform inertial device zero position estimation.

3. The method for adaptively estimating and compensating zero position of inertial device of inertial navigation system according to claim 2, characterized in that: In step 4, different validity criteria are designed according to the characteristics of different external reference information as follows: a) The validity criteria for the external reference information of the satellite navigation system provided by the integrated navigation system are: (1) Judgment of single continuous failure time of external reference satellite guidance data: After starting the position observation integrated navigation to estimate the zero position of the inertial device, the single continuous failure time of the satellite navigation data shall not exceed 1 minute, otherwise it is judged that the current estimation fails; (2) Determination of the total failure time of external reference satellite guidance data: After starting the position observation integrated navigation to estimate the zero position of the inertial device, the total failure time of the satellite navigation data shall not exceed 10 minutes, otherwise it is judged that this estimation has failed; The satellite navigation data failure is judged by the satellite navigation data valid bit, the consistency of satellite navigation data before and after, the consistency of satellite navigation position and inertial navigation position, etc. When both conditions (1) and (2) in a) are met at the same time, the satellite navigation external reference information is considered valid. Otherwise, it is judged that the satellite navigation external reference information is invalid, and the zero position estimation result of the inertial device is invalid. b) The validity criterion of the external reference information of the DVL provided by the integrated navigation system (1) Determination of single continuous failure time by external reference DVL data: After starting the velocity observation integrated navigation to estimate the zero position of the inertial device, the single continuous failure time of the DVL data shall not exceed 1 minute, otherwise it is judged as the current estimation failure; (2) Determination of total failure time based on external reference DVL data: After starting the velocity observation integrated navigation to estimate the zero position of the inertial device, the total failure time of the DVL data shall not exceed 10 minutes, otherwise it is considered that the estimation has failed. Among them, the failure of DVL data can be judged by the valid bits of DVL data and the consistency of DVL data before and after. When both conditions (1) and (2) in b) are met at the same time, the DVL external reference information is considered valid. Otherwise, it is determined that the DVL external reference information is invalid, and the zero position estimation result of the inertial device is invalid. c) Do not set the validity criteria for the external reference of the odometer, and the default odometer information is always valid.

4. The method for adaptively estimating and compensating zero position of inertial device of inertial navigation system according to claim 1, characterized in that: In step 5, the specific criteria are as follows: a) Total time to enter the combined navigation state: (1) For the position observation mode of the satellite navigation system as a supporting integrated navigation system, the total time of the integrated navigation state is greater than 3600s; (2) For the speed observation mode of the integrated navigation system with DVL or odometer, the total time of the integrated navigation state is greater than 5400 seconds; b) Two effective linear motion position judgments and heading change judgments: (1) First position direct flight judgment: If the maximum difference of the inertial navigation heading is less than 40° within 20 consecutive minutes, it is judged that the first position direct flight is completed, and the turning judgment from the first position to the second position is turned; (2) Determination of turning from the first position to the second position: If the difference between the 1Hz heading value of the inertial navigation for 3 consecutive seconds and the average heading value of the first position is greater than 60°, it is judged that the turning state has been entered; if the difference between two adjacent 1Hz heading values ​​of the inertial navigation for 10 consecutive seconds is less than 0.2°, it is judged that the turn from the first position to the second position is completed, and the judgment of direct flight at the second position is entered; (3) Second position direct flight judgment: After completing the turn from the first position to the second position, for the position observation mode with satellite navigation as the supporting integrated navigation system, within 40 consecutive minutes, the maximum difference of the inertial navigation heading is less than 120°, and the difference between the mean heading of the second position and the mean heading of the first position is greater than 60°, then it is judged that the direct flight to the second position is completed; for the speed observation mode with DVL or odometer as the supporting integrated navigation system, within 1 consecutive hour, the maximum difference of the inertial navigation heading is less than 120°, and the difference between the mean heading of the second position and the mean heading of the first position is greater than 60°, then it is judged that the direct flight to the second position is completed; When the three conditions (1), (2) and (3) in b) are met, it is judged that the two effective linear motion position judgment and heading change judgment criteria are met; c) Accelerometer zero position P array judgment: Error variance matrix P in integrated navigation Kalman filter 15×15 In the array, the accelerometer x zero position and the accelerometer y zero position If the RMS of the diagonal elements of the corresponding dimension are not greater than 50 μg, it is judged that the accelerometer zero position P matrix criterion is met, as shown in the following formula: d) Determination of the estimated zero position value of the accelerometer: Accelerometer x and accelerometer y zero position estimates and If the absolute values ​​are all less than 5000 μg, it is judged that the accelerometer zero value criterion is met, as shown in the following formula: If the above four judgment conditions a), b), c), and d) are met at the same time, the accelerometer zero position estimation of this group is determined to be valid, and the accelerometer zero position estimation can be performed. and Otherwise, the accelerometer zero position estimation is considered invalid and the accelerometer zero position estimation is not performed. and Feedback compensation.

5. The method for adaptively estimating and compensating zero position of inertial device of inertial navigation system according to claim 1, characterized in that: In step 6, the specific criteria are as follows: a) Total time to enter the combined navigation state: For the position observation mode with satellite navigation as the supporting integrated navigation system or the speed observation mode with DVL / odometer as the supporting integrated navigation system, the total time of the integrated navigation state is greater than 7200s; b) Two effective linear motion position judgments and heading change judgments: (1) First position direct flight judgment: If the maximum difference of the inertial navigation heading is less than 40° within 20 consecutive minutes, it is judged that the first position direct flight is completed, and the turning judgment from the first position to the second position is turned; (2) Determination of turning from the first position to the second position: If the difference between the 1Hz heading value of the inertial navigation for 3 consecutive seconds and the average heading value of the first position is greater than 60°, it is judged that the turning state has been entered; if the difference between two adjacent 1Hz heading values ​​of the inertial navigation for 10 consecutive seconds is less than 0.2°, it is judged that the turn from the first position to the second position is completed, and the judgment of direct flight at the second position is entered; (3) Second position direct flight judgment: After completing the turn from the first position to the second position, for the position observation mode with satellite navigation as the supporting integrated navigation system, within 1 hour, the maximum difference of the inertial navigation heading is less than 120°, and the difference between the mean heading of the second position and the mean heading of the first position is greater than 60°, then it is judged that the direct flight to the second position is completed; for the speed observation mode with DVL or odometer as the supporting integrated navigation system, within 80 minutes, the maximum difference of the inertial navigation heading is less than 120°, and the difference between the mean heading of the second position and the mean heading of the first position is greater than 60°, then it is judged that the direct flight to the second position is completed; When the three conditions (1), (2) and (3) in the above b) are met, it is judged that the two effective linear motion position judgment and heading change judgment criteria are met; c) Gyro zero position P array judgment: Error variance matrix P in integrated navigation Kalman filter 15×15 In the array, the gyro x zero position ε x and gyro y zero position ε y If the RMS of the diagonal elements of the corresponding dimension are not greater than 0.03° / h, it is judged that the gyro zero position P matrix criterion is met, as shown in the following formula: d) Determination of gyro estimated zero position value: The estimated zero position of gyro x and gyro y and If the absolute values ​​are all less than 0.2° / h, it is judged that the gyro zero position value criterion is met, as shown in the following formula: If the above four judgment conditions a), b), c) and d) are met at the same time, the accelerometer zero position estimation of this group is determined to be valid and feedback compensation can be performed; otherwise, the gyro zero position estimation is determined to be invalid.

Images