SINS-DVL integrated navigation method based on new SINS mechanical arrangement

Abstract

The invention discloses an SINS-DVL (Strapdown Inertial Navigation System-Digital Video Library) integrated navigation method based on new SINS (Strapdown Inertial Navigation System) mechanical arrangement, which comprises the following steps: S1, giving SINS mechanical arrangement based on DVL dead reckoning; S2, according to the SINS mechanical arrangement based on the DVL dead reckoning; S3, carrying out related discussion about the SINS-DVL integrated navigation method based on the new SINS mechanical arrangement; and S4, evaluating a navigation result of the provided method through a shipborne actual measurement experiment. According to the invention, a linear error state space model is deduced based on new mechanical arrangement, and the provided SINS / DVL integrated navigation method provides better SINS mechanical arrangement for a DVL-based speed measurement mode and a DVL-based dead reckoning position measurement mode; the SINS / DVL integrated navigation method is oriented to reality, the mechanical arrangement of the SINS in the SINS / DVL is ingeniously rewritten by reasonably utilizing the information of the DVL, and the integrated navigation performance of the SINS / DVL is improved on the basis that the method is easy to implement.

Description

Technical Field

[0001] The invention relates to the technical field of inertial base integrated navigation, and in particular to a SINS-DVL integrated navigation method based on new SINS mechanical arrangement. Background Art

[0002] Strapdown Inertial Navigation System (SINS) is a key device in the field of underwater navigation. It can provide continuous navigation information, namely attitude, velocity, position, angular rate and acceleration information. However, due to the characteristics of its dead reckoning, the corresponding navigation error will accumulate over time, which greatly limits the long-term application of SINS. In the field of underwater navigation, it is often combined with Doppler log (DVL) to form SINS / DVL combined navigation, which complements each other and achieves higher precision underwater navigation.

[0003] For SINS / DVL, the speed measurement provided by DVL is used to suppress the accumulated error of SINS. The specific process of SINS / DVL is: first, SINS calculation is performed according to the general SINS mechanical arrangement, the corresponding error state space model is constructed, and the Kalman filter is used to estimate the SINS calculation error and optimize the SINS result. It can be seen that the core of SINS / DVL integrated navigation is the SINS mechanical arrangement and its corresponding error state space model. However, for SINS / DVL, since DVL can only provide speed information, the position of SINS / DVL is usually divergent. There are two main reasons for this. One is that the position information of SINS comes from the calculation of speed, and the other is that there is no additional position information that does not change with time as measurement.

[0004] In order to improve the position accuracy of SINS / DVL, the present invention proposes a novel SINS mechanical arrangement for SINS / DVL integrated navigation. Summary of the invention

[0005] The purpose of the present invention is to solve the above technical problems and to propose a SINS-DVL integrated navigation method based on new SINS mechanical arrangement.

[0006] In order to achieve the above object, the present invention adopts the following technical solutions:

[0007] The SINS-DVL integrated navigation method based on the new SINS mechanical arrangement includes the following steps:

[0008] S1, presents the SINS mechanical arrangement based on DVL dead reckoning, and lays the theoretical foundation for the SINS / DVL integrated navigation method based on the new SINS mechanical arrangement;

[0009] S2, derive the corresponding SINS / DVL state space model according to the SINS mechanical arrangement based on DVL dead reckoning;

[0010] S3, discuss the SINS / DVL integrated navigation method based on the new SINS mechanical arrangement;

[0011] S4, through shipborne field experiments, the navigation results of the proposed method are evaluated to prove the effectiveness and applicability of the proposed method.

[0012] Preferably, the mechanical arrangement of the SINS based on DVL dead reckoning in step S1 is as follows: INS solution can provide carrier navigation parameters, and the navigation parameters include attitude, speed and position information.

[0013] Preferably, all navigation parameters are defined based on a selected reference coordinate system, including the carrier system, the geocentric inertial coordinate system and the geocentric earth fixed coordinate system. According to these reference coordinate systems, the attitude, velocity and position of the carrier are respectively and

[0014] The traditional SINS update equation is given by:

[0015]

[0016] In the formula, and are the carrier angular velocity and specific force measured by the gyroscope and accelerometer, respectively, is the Earth's rotation angular velocity vector, is the gravity vector represented by ECEF, and (·×) represents the antisymmetric matrix of ·.

[0017] Preferably, the dead reckoning of the DVL is used as the position update of the SINS. The new SINS mechanical arrangement is as follows:

[0018]

[0019] Among them, the DVL output is the vehicle's driving speed, that is, It is related to ground speed The relationship is given by the following formula,

[0020]

[0021] In the proposed SISN mechanical choreography, the attitude and velocity differential equations are the same as those of the conventional SINS mechanical choreography.

[0022] Preferably, the derivation of the corresponding SINS / DVL state space model in step 2 is specifically as follows: according to the new SINS mechanical arrangement, the SINS / DVL error model is deduced, and given the navigation error:

[0023]

[0024] In the formula, and The attitude, velocity and position with errors calculated based on the proposed SINS mechanical arrangement, and is the corresponding error.

[0025] Preferably, the attitude and velocity error state models are the same as those of the SINS / DVL integrated navigation based on the traditional SINS mechanical choreography. Since the proposed SINS mechanical choreography position update is different, the corresponding position error model is as follows:

[0026]

[0027] In the formula, Speed ​​provided for DVL.

[0028] Preferably, in step S3, for the conventional SINS / DVL integrated navigation using DVL speed as measurement, the SINS solution is performed based on the following formula:

[0029]

[0030] The measurement model is as follows:

[0031]

[0032] Preferably, the onboard measurement experiment in step S4 specifically comprises the following steps:

[0033] B1. Obtain the original data of SINS and DVL;

[0034] B2. Perform SINS solution according to the new SINS mechanical arrangement;

[0035] B3. Perform integrated navigation based on the derived SINS / DVL error model;

[0036] B4. Experiments,The performance of the proposed SINS / DVL integrated navigation method is evaluated by,comparing it with the above two traditional SINS / DVL.,The traditional integrated navigation with DVL velocity as measurement is denoted,SINS / DVL-V, the traditional SINS / DVL with DVL dead reckoning position as measurement is denoted,SINS / DVL-DR, and the proposed SINS / DVL is denoted,SINS / DVL-Hybrid.

[0037] Compared with the prior art, the present invention has the following beneficial effects:

[0038] In the proposed SINS mechanical arrangement, the position update is based on DVL dead reckoning instead of the traditional SINS speed integration. During Kalman filtering, the carrier system speed of the DVL is still used as the measurement. Then, the linear error state space model is derived based on the new mechanical arrangement. The proposed SINS / DVL combined navigation method provides a better SINS mechanical arrangement for the DVL speed measurement-based and DVL dead reckoning position measurement modes. The present invention is oriented to reality. By reasonably utilizing the information of the DVL, the mechanical arrangement of the SINS in the SINS / DVL is cleverly rewritten. On the basis of being easy to implement, the combined navigation performance of the SINS / DVL is improved. BRIEF DESCRIPTION OF THE DRAWINGS

[0039] Figure 1 The proposed SINS mechanical arrangement based on DVL dead reckoning is drawn.

[0040] Figure 2 The shipboard trajectory of the first sea navigation experiment was mapped.

[0041] Figure 3 The shipboard trajectory of the second sea trial was plotted.

[0042] Figure 4 The shipboard trajectory diagram of the third sea navigation experiment was drawn.

[0043] Figure 5 The latitude errors of the various methods for the first sea trial are plotted.

[0044] Figure 6 The longitude error of each method for the first sea navigation experiment is plotted.

[0045] Figure 7 The latitude errors of the various methods for the second sea trial are plotted.

[0046] Figure 8 The longitude error diagrams of various methods for the second sea navigation experiment are plotted.

[0047] Fig. 9The latitude error graphs of each method for the third sea trial are plotted.

[0048] Fig.10 The longitude error diagrams of various methods for the third sea navigation experiment were plotted.

[0049] Fig.11 The position error of each method for the first sea trial is plotted.

[0050] Fig.12 The position error graphs of each method for the second sea trial are plotted.

[0051] Fig.13 The position error diagrams of each method for the third sea trial are plotted. DETAILED DESCRIPTION

[0052] The technical solutions in the embodiments of the present invention will be described clearly and completely below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, rather than all the embodiments.

[0053] Reference Figure 1-Figure 13 , the SINS-DVL integrated navigation method based on the new SINS mechanical arrangement includes the following steps:

[0054] S1, presents the SINS mechanical arrangement based on DVL dead reckoning, and lays the theoretical foundation for the SINS / DVL integrated navigation method based on the new SINS mechanical arrangement;

[0055] S2, derive the corresponding SINS / DVL state space model according to the SINS mechanical arrangement based on DVL dead reckoning;

[0056] S3, discuss the SINS / DVL integrated navigation method based on the new SINS mechanical arrangement;

[0057] S4, through shipborne field experiments, the navigation results of the proposed method are evaluated to prove the effectiveness and applicability of the proposed method.

[0058] In the present invention, the mechanical arrangement of the SINS based on DVL dead reckoning in step S1 is as follows: INS solution can provide carrier navigation parameters, and the navigation parameters include attitude, speed and position information.

[0059] In the present invention, all navigation parameters are defined based on the selected reference coordinate system, which includes the carrier system, the geocentric inertial coordinate system and the geocentric earth fixed coordinate system. According to these reference coordinate systems, the attitude, velocity and position of the carrier are respectively and

[0060] The traditional SINS update equation is given by:

[0061]

[0062]

[0063] In the formula, and are the carrier angular velocity and specific force measured by the gyroscope and accelerometer, respectively, is the Earth's rotation angular velocity vector, is the gravity vector represented by ECEF, and (·×) represents the antisymmetric matrix of ·.

[0064] In the present invention, the dead reckoning of DVL is used as the position update of SINS, and the new SINS mechanical arrangement is as follows:

[0065]

[0066] Among them, the DVL output is the vehicle's driving speed, that is, It is related to ground speed The relationship is given by the following formula,

[0067]

[0068] In the proposed SISN mechanical choreography, the attitude and velocity differential equations are the same as those of the conventional SINS mechanical choreography.

[0069] In the present invention, the derivation of the corresponding SINS / DVL state space model in step 2 is specifically as follows: according to the new SINS mechanical arrangement, the SINS / DVL error model is deduced, and the navigation error is given:

[0070]

[0071] In the formula, and The attitude, velocity and position with errors calculated based on the proposed SINS mechanical arrangement, and is the corresponding error.

[0072] In the present invention, the attitude and velocity error state models are the same as those of the SINS / DVL integrated navigation based on the traditional SINS mechanical arrangement. Since the proposed SINS mechanical arrangement position update is different, the corresponding position error model is as follows:

[0073]

[0074] In the formula, Speed ​​provided for DVL.

[0075] In the present invention, in step S3, for the traditional SINS / DVL integrated navigation using DVL speed as measurement, the SINS solution is performed based on the following formula:

[0076]

[0077] The measurement model is as follows:

[0078]

[0079] In the present invention, the onboard measurement experiment in step S4 is specifically implemented by the following steps:

[0080] B1. Obtain the original data of SINS and DVL;

[0081] B2. Perform SINS solution according to the new SINS mechanical arrangement;

[0082] B3. Perform integrated navigation based on the derived SINS / DVL error model;

[0083] B4. Experiments,The performance of the proposed SINS / DVL integrated navigation method is evaluated by,comparing it with the above two traditional SINS / DVL.,The traditional integrated navigation with DVL velocity as measurement is denoted,SINS / DVL-V, the traditional SINS / DVL with DVL dead reckoning position as measurement is denoted,SINS / DVL-DR, and the proposed SINS / DVL is denoted,SINS / DVL-Hybrid.

[0084] Further detailed explanation:

[0085] SINS mechanical choreography based on DVL dead reckoning

[0086] SINS solution can provide the attitude, velocity and position information of the carrier. All navigation parameters are defined based on the selected reference coordinate system. The reference system selected by the present invention includes the carrier system (b / system), the Earth-centered inertial (ECI) coordinate system (i / system) and the Earth-centered earth fixed (ECEF) coordinate system (e / system). According to these reference coordinate systems, the attitude, velocity and position of the carrier are respectively and

[0087] The traditional SINS update equation is given by

[0088]

[0089] In the formula, and are the carrier angular velocity and specific force measured by the gyroscope and accelerometer, respectively. is the Earth's rotation angular rate vector. is the gravity vector expressed as ECEF. (·×) represents the antisymmetric matrix of ·.

[0090] The present invention uses the dead reckoning of DVL as the position update of SINS, and the new SINS mechanical arrangement is as follows.

[0091]

[0092] Among them, the DVL output is the vehicle's driving speed, that is, It is related to ground speed The relationship is given by the following formula.

[0093]

[0094] It can be seen that the attitude and velocity differential equations in the proposed SISN mechanical choreography are the same as those in the traditional SINS mechanical choreography. The difference lies in the position differential equation.

[0095] For SINS mechanical arrangement, error propagation is unidirectional, i.e., attitude error leads to velocity error and then position error. Conversely, position error does not lead to velocity error and attitude error (assuming This is the basic principle of the proposed SINS mechanical arrangement. In the conventional SINS mechanical arrangement, the position error is caused not only by the error sources of attitude calculation, but also by the error sources of velocity calculation. In contrast, in the proposed mechanical arrangement, the position error is caused by the error sources of attitude calculation and the DVL measurement error. The block diagram of the proposed mechanical arrangement is shown in Figure 1 shown.

[0096] Derivation of SINS / DVL error state space model based on new SINS mechanical arrangement

[0097] Based on the new SINS mechanical arrangement mentioned above, this section derives the SINS / DVL error model. Given the navigation error

[0098]

[0099] In the formula, and The attitude, velocity and position with errors are calculated based on the proposed SINS mechanical choreography. and is the corresponding error.

[0100] The attitude and velocity error state models are the same as those of the SINS / DVL integrated navigation based on the traditional SINS mechanical choreography. Since the position update of the proposed SINS mechanical choreography is different, the corresponding position error model is as follows.

[0101]

[0102] In the formula, Speed ​​provided for DVL.

[0103] Adding the drift bias of the inertial sensor to this state, the state of the proposed SINS / DVL integrated navigation can be obtained as follows.

[0104]

[0105] Where b g and b a are the drift and bias of the gyroscope and accelerometer respectively. Based on this, the linear absurd space model is as follows.

[0106]

[0107] Wherein, the state transfer matrix F and the noise control matrix G are as follows.

[0108]

[0109] Noise Matrix middle, and represent the random noise of the gyroscope and accelerometer, respectively.

[0110] The measurement model is the difference between the speed based on DVL and the speed solved by SINS, that is,

[0111]

[0112] According to the error state definition in (9), the measurement model is given by the following equation.

[0113]

[0114] In the formula, H is the measurement matrix.

[0115] Based on the state models (10) and (14), Kalman filtering can be used to estimate the error state. These error state estimates can then be used to optimize the SINS calculation results.

[0116] Related Discussions

[0117] For traditional SINS / DVL integrated navigation using DVL speed as measurement, the SINS solution is based on formulas (1), (2) and (3). The measurement model is as follows:

[0118]

[0119] The error state dimension of the conventional SINS / DVL is 15, which is the same as the SINS / DVL integrated navigation proposed in the present invention. The difference lies in the mechanical arrangement of the SINS.

[0120] For SINS / DVL integrated navigation using DVL dead reckoning position as measurement, the SINS status update is performed based on formulas (1) to (3) and (6). The measurement model is as follows.

[0121]

[0122] In the formula, and are the position error of SINS and the position error of DVL dead reckoning, respectively. and are the SINS position and the DVL dead reckoning position respectively.

[0123] The error of the integrated navigation is defined as follows.

[0124]

[0125] The state dimension is 18. In (16), is the position calculated based on formula (7), is the position calculated based on (6).

[0126] Implementation steps of the present invention in shipboard experiments

[0127] The experimental verification method of the SINS / DVL integrated navigation method based on the cumulative DVL velocity measurement model of the present invention specifically comprises the following steps.

[0128] 1) Obtain the original data of SINS and DVL.

[0129] 2) Perform SINS solution based on the new SINS mechanical arrangement.

[0130] 3) Integrated navigation based on the derived SINS / DVL error model

[0131] 4) Experiments. The performance of the proposed SINS / DVL integrated navigation method is evaluated by comparing it with the two conventional SINS / DVL discussed above. The conventional integrated navigation with DVL velocity as the measurement is denoted as SINS / DVL-V. The conventional SINS / DVL with DVL dead reckoning position as the measurement is denoted as SINS / DVL-DR. The proposed SINS / DVL is denoted as SINS / DVL-Hybrid.

[0132] First, 5 hours of data were collected from the shipboard equipment at sea. The experimental platform was equipped with high-precision SINS, DVL and GPS. The results of the SINS / GPS combination were used as a reference for comparison. Table 1 summarizes the technical specifications of the equipped systems. The raw outputs of SINS and DVL were processed by the three SINS / DVL algorithms mentioned above. The reference trajectory and the trajectories of the three algorithms are shown in Figure 2 The latitude error, longitude error and position error of the three algorithms are shown as follows: Figures 3 to 5 As shown in Figure 2. Experimental results show that the proposed method has the best performance. The performance of SINS / DVL-DR is better than that of SINS / DVL-V. Approximately, the proposed method has similar performance to SINS / DVL-DR. In addition, the state dimension of the proposed method is 15, while the state dimension of SINS / DVL-DR is 18. Therefore, the proposed method will be more computationally efficient than SINS / DVL-DR.

[0133] Table 1 Parameters of main sensors:

[0134]

[0135] Then, experiments were conducted on the lake using the above equipment. 2.5 hours of data were collected for performance evaluation. The reference trajectory and the trajectories of the three integration algorithms are shown in Figure 6 The latitude error, longitude error and position error of the three algorithms are shown as follows: Figures 7 to 9 The experimental results show that the performance of SINS / DVL-V is better than that of SINS / DVL-DR, and the performance of the proposed method is similar to that of SINS / DVL-V. Based on the two experimental results, it can be concluded that the proposed method can always achieve the same performance as the better one of SINS / DVD-V and SINS / DVL-DR.

[0136] To further evaluate the performance of the proposed method, the above three integration mechanisms are used for the combination of SINS and Odometer (OD). The output of OD and DVL is the velocity of the vehicle system. Therefore, the combination mechanism of these two sensors is universal. Data for 1 hour was collected from the on-board equipment. There are SINS, OD and GPS on the experimental platform. For consistency, the three algorithms still use the same representation, namely SINS / DVL-V, SINS / DVL-DR and SINS / DVL-Hybrid, although the auxiliary information is provided by OD instead of DVL. The reference trajectory and the trajectories of the three algorithms are shown in Fig.10 The latitude error, longitude error and position error of the three algorithms are shown as follows: Figures 11 to 13 As shown. In this experiment, the three integration algorithms have similar performance. It is further proved that it is difficult to reach a certain conclusion on the issue of which SINS / DVL integrated navigation is better among the above SINS / DVL-V and SINS / DVL-DR. However, it is not denied that the proposed method still has the same performance as the optimal SINS / DVL. Therefore, the proposed method should be the preferred choice in practical applications.

[0137] The above description is only a preferred specific implementation manner of the present invention, but the protection scope of the present invention is not limited thereto. Any technician familiar with the technical field can make equivalent replacements or changes according to the technical scheme and inventive concept of the present invention within the technical scope disclosed by the present invention, which should be covered by the protection scope of the present invention.

Claims

1. The SINS-DVL integrated navigation method based on the new SINS mechanical arrangement is characterized by: The following steps are involved: S1, presents the SINS mechanical arrangement based on DVL dead reckoning, and lays the theoretical foundation for the SINS / DVL integrated navigation method based on the new SINS mechanical arrangement; S2, derive the corresponding SINS / DVL state space model according to the SINS mechanical arrangement based on DVL dead reckoning; S3, discuss the SINS / DVL integrated navigation method based on the new SINS mechanical arrangement; S4, through shipborne field experiments, the navigation results of the proposed method are evaluated to prove the effectiveness and applicability of the proposed method.

2. The SINS-DVL integrated navigation method based on the new SINS mechanical arrangement according to claim 1 is characterized in that: The mechanical arrangement of the SINS based on DVL dead reckoning in step S1 is as follows: INS solution can provide carrier navigation parameters, and the navigation parameters include attitude, speed and position information.

3. The SINS-DVL integrated navigation method based on the new SINS mechanical arrangement according to claim 2 is characterized in that: All navigation parameters are defined based on the selected reference coordinate system, which includes the carrier system, the geocentric inertial coordinate system and the geocentric earth fixed coordinate system. According to these reference coordinate systems, the carrier's attitude, velocity and position are and The traditional SINS update equation is given by: In the formula, and are the carrier angular velocity and specific force measured by the gyroscope and accelerometer, respectively, is the Earth's rotation angular velocity vector, is the gravity vector represented by ECEF, and (·×) represents the antisymmetric matrix of ·.

4. The SINS-DVL integrated navigation method based on the new SINS mechanical arrangement according to claim 2 is characterized in that: Using DVL dead reckoning as the SINS position update, the new SINS mechanical arrangement is as follows; Among them, the DVL output is the vehicle's driving speed, that is, It is related to ground speed The relationship is given by the following formula, In the proposed SISN mechanical choreography, the attitude and velocity differential equations are the same as those of the conventional SINS mechanical choreography.

5. The SINS-DVL integrated navigation method based on the new SINS mechanical arrangement according to claim 1 is characterized in that: The derivation of the corresponding SINS / DVL state space model in step 2 is specifically as follows: based on the new SINS mechanical arrangement, the SINS / DVL error model is deduced, and the navigation error is given: In the formula, and The attitude, velocity and position with errors calculated based on the proposed SINS mechanical choreography, and is the corresponding error.

6. The SINS-DVL integrated navigation method based on the new SINS mechanical arrangement according to claim 5 is characterized in that: The attitude and velocity error state models are the same as those of the SINS / DVL integrated navigation based on the traditional SINS mechanical choreography. Due to the different position updates of the proposed SINS mechanical choreography, the corresponding position error models are as follows: In the formula, Speed ​​provided for DVL.

7. The SINS-DVL integrated navigation method based on new SINS mechanical arrangement according to claim 1 is characterized in that: In step S3, for the traditional SINS / DVL integrated navigation using DVL speed as measurement, the SINS solution is performed based on the following formula: The measurement model is as follows:

8. The SINS-DVL integrated navigation method based on the new SINS mechanical arrangement according to claim 1 is characterized in that: The specific implementation of the shipboard measurement experiment in step S4 includes the following steps: B1. Obtain the original data of SINS and DVL; B2. Perform SINS solution according to the new SINS mechanical arrangement; B3. Perform integrated navigation based on the derived SINS / DVL error model; B4. Experiments,The performance of the proposed SINS / DVL integrated navigation method is evaluated by,comparing it with the above two traditional SINS / DVL.,The traditional integrated navigation with DVL velocity as measurement is denoted,SINS / DVL-V, the traditional SINS / DVL with DVL dead reckoning position as measurement is denoted,SINS / DVL-DR, and the proposed SINS / DVL is denoted,SINS / DVL-Hybrid.

Images