Acoustic Doppler delay information assisted integrated navigation method

Abstract

The invention discloses an acoustic Doppler delay information-assisted integrated navigation method, and belongs to the technical field of underwater acoustic velocity measurement and integrated navigation. According to the method, the problems that the SINS / DVL integrated navigation measurement is biased and the navigation position error is rapidly increased along with time due to the fact that the DVL acoustic delay cannot be accurately compensated by the existing method are solved. The method comprises the following steps: establishing a carrier coordinate system and a navigation coordinate system, and defining a delay compensation algorithm to filter related variable initial values; obtaining a DVL speed measurement signal transmitting moment and a DVL speed measurement signal receiving moment according to the synchronization signal; respectively calculating a state transition matrix and a measurement noise matrix; at the DVL data updating moment, measurement of a DVL transmitting moment and a DVL receiving moment is constructed by utilizing SINS resolving historical information and DVL speed measurement information, optimal estimation is carried out on the system error amount at the current moment, the system error obtained by a delay compensation algorithm is fed back and corrected to the integrated navigation system, and acoustic Doppler delay information aided navigation is completed. The method can be applied to the technical field of underwater acoustic velocity measurement and integrated navigation.

Description

Technical Field

[0001] The invention belongs to the technical field of underwater acoustic velocity measurement and combined navigation, and in particular relates to an acoustic Doppler delay information assisted combined navigation method. Background Art

[0002] Due to its concealment and autonomy, SINS / DVL combined navigation is the most commonly used navigation method for AUVs. However, with the continuous deepening of ocean exploration, underwater vehicles are facing a breakthrough development from short-distance to long-distance voyages, from nearshore to deep sea, and from automation to intelligence. Faced with the demand for precise autonomous navigation in unknown and complex environments in the deep sea, new requirements are put forward for the robust navigation capabilities of the platform under maneuvering conditions.

[0003] The time synchronization accuracy of SINS / DVL integrated navigation is a key factor affecting the navigation performance of the platform under maneuvering. The DVL acoustic information has a certain delay due to the influence of underwater acoustic channel transmission, echo signal processing and serial communication output, such as Figure 1As shown in the literature 1 (Jalving B, Gade K, Svartveit K, et al. DVL Velocity Aiding in the HUGIN 1000 Integrated Inertial Navigation System [J]. Modeling, Identification and Control (MIC), 2004, 25 (4).) causes asynchrony of the combined system data, causing serious navigation errors. There are currently three commonly used delay compensation methods. The first is to use the delay time as a state quantity for filtering estimation and compensation. For example, literature 2 (Skog, I, Handel, et al. Time Synchronization Errors in Loosely Coupled GPS-Aided Inertial Navigation Systems [J]. IEEE transactions on intelligent transportation systems, 2011, 12 (4): 1014-1023.) achieves better compensation effects in a shorter delay time (about 100ms). However, for the longer delay time and large maneuvering in reference 3 (Hansen JM, Fossen TI, Johansen TA. Nonlinear observer design for GNSS-aided inertial navigation systems with time-delayed GNSS measurements [J]. Control Engineering Practice, 2017, 60: 39-50.), due to the long delay time and the large amount of filtered state, the KF state estimation requirements are no longer met and other methods are required for compensation. The second method is to use pulse triggering (such as GNSS Pulse Per Second, PPS) between sensors for timing, and then compensate for the known time delay through data latching and interpolation, such as reference 4 (Xiao Jinli, Pan Zhengfeng, Huang Shengxiang. Research on Data Synchronization Processing Method of GPS / INS Integrated Navigation System [J]. Journal of Wuhan University (Information Science Edition), 2008, 33 (7): 715-717.), but due to the influence of device noise, the accuracy of the method of compensating data delay through data interpolation is limited.The last one is a navigation method that uses the recursive relationship between the past and current state quantities and the measurement information at the past moment to complete the current state estimation, such as the delayed Kalman filter algorithm in reference 5 (Xu B, Wang X, Zhang J, et al. Maximum Correntropy delay Kalman filter for SINS / USBL integrated navigation [J]. ISA Transactions, 2021.), which is mainly used in the field of collaborative navigation. In addition, the compensation of DVL delay needs to consider the Doppler speed measurement principle. The DVL Doppler information is modulated by the carrier speed at the time of transmitting and receiving the speed measurement signal. The acoustic delay time is related to the time of sending and receiving the acoustic signal, which brings difficulties to the use of DVL information and the compensation of delay. Reference 6 (P.Liu, B.Wang, Z.Deng, and M.Fu, “A Correction Method for DVL Measurement Errors by Attitude Dynamics,” IEEE Sensors J., vol.17, no.14, pp.4628–4638) is based on the assumption that the horizontal velocity of the carrier is constant. According to the lever arm effect, the carrier velocity at the DVL reception time under attitude angle dynamics is obtained, and the simulation verifies the effectiveness of the algorithm under trajectories such as circles, spirals, and lawn mowers; Reference 7 (Y.Yao, X.Xu, L.Hou, K.Deng, and X.Xu, “A Simple and Precise Correction Method for DVL Measurements Under the Dynamic Environment,” IEEE Trans.Veh.Technol., vol.69, no.10, pp.10750–10758) uses inertial information to separate the velocity at the DVL reception time, and verifies the algorithm's navigation capability under large maneuvers through simulation experiments.

[0004] In summary, although the existing methods have achieved certain achievements in DVL acoustic delay compensation, the existing methods are still unable to accurately compensate for the DVL acoustic delay, which leads to biased SINS / DVL combined navigation measurements under maneuvers and rapid growth of navigation position errors over time. Summary of the invention

[0005] The purpose of the present invention is to solve the problem that the existing methods cannot accurately compensate for the DVL acoustic delay, resulting in the biased SINS / DVL combined navigation measurement under maneuvering and the rapid growth of the navigation position error over time. An acoustic Doppler delay information-assisted combined navigation method is proposed.

[0006] The technical solution adopted by the present invention to solve the above technical problems is: an acoustic Doppler delay information assisted integrated navigation method, the method specifically comprising the following steps:

[0007] Step 1: Establish the carrier coordinate system b and the navigation coordinate system n, obtain the initial values ​​of the carrier attitude, velocity and position, define the initial value of the state covariance matrix P0, the system noise covariance matrix Q1, and the initial value of the state recursion matrix Initial value of state recursion matrix Initial value of measurement noise matrix And the initial value of the measurement noise matrix

[0008] Among them, the initial value of the state recursion matrix I 15×15 is a 15×15 dimensional identity matrix, measuring the initial value of the noise matrix 0 3×3 is a 3×3 dimensional zero matrix;

[0009] And initialize time k = 0;

[0010] Step 2: Determine whether the DVL triggers the synchronization signal at time k:

[0011] If the DVL triggers the synchronization signal at time k, the transmission time k of the DVL speed measurement signal is updated T and receiving time k R Then proceed to step 4;

[0012] If the DVL does not trigger the synchronization signal at time k, execute step 3;

[0013] Step 3: SINS data is solved and the time update result of the current state quantity is calculated, and then k=k+1 is set, and the execution of step 2 is returned;

[0014] Step 4: Compare the current time k with k T , k R and DVL data update time k D size;

[0015] If k = k T , then execute step five;

[0016] If k T <k<k R , then execute step 6;

[0017] If k = k R , then execute step seven;

[0018] If k R <k<k D , then execute step eight;

[0019] If k = k D , then execute step nine;

[0020] Step 5: Solve the SINS data and calculate the current state time update result, and calculate the carrier speed in the carrier coordinate system b solved by SINS and measurement matrix And let k=k+1, and return to execute step 4;

[0021] Step 6: Solve the SINS data and calculate the current state time update result, and calculate the state recursive matrix F at time k k and the measurement noise matrix R k , and set k=k+1, and return to step 4;

[0022] Step 7: Solve the SINS data and calculate the current state time update result, and calculate the carrier speed in the carrier coordinate system b solved by SINS and measurement matrix And let k=k+1, and return to execute step 4;

[0023] Step 8: Solve the SINS data and calculate the current state time update result, and calculate the state recursive matrix F at time k k , measurement noise matrix R k And the state recursive matrix F′ at time k k , measurement noise matrix R′ k , and set k=k+1, and return to step 4;

[0024] Step 9: Solve the SINS data and calculate the current state time update result, and calculate k D The state recursion matrix at time Measurement Noise Matrix State recursion matrix Measurement Noise Matrix

[0025] According to And Doppler velocity information output by DVL Calculation measurement According to State recursion matrix Measurement Noise Matrix State recursion matrix and the measurement noise matrix To process the current state quantity time update result, obtain the state quantity and covariance matrix after the optimal estimation at the current moment, use the optimal estimated state quantity to feedback correct the navigation solution value, and then execute step ten;

[0026] Step 10: Set k=k+1 and return to step 2.

[0027] Furthermore, the establishment of the carrier coordinate system b and the navigation coordinate system n is specifically as follows:

[0028] Carrier coordinate system b:

[0029] The center of gravity of the platform is taken as the origin o of the carrier coordinate system b b , the bow direction along the platform cross section pointing to the platform is taken as y b The positive direction of the axis is the direction perpendicular to the deck upward as z b Positive axis direction, x b Axis, y b Axis and z b The axes form a right-handed coordinate system;

[0030] Navigation coordinate system n:

[0031] The center of gravity of the platform is taken as the origin o of the navigation coordinate system n, the geographic east direction is taken as the positive direction of the x-axis, the geographic north direction is taken as the positive direction of the y-axis, and the positive direction of the z-axis is perpendicular to the xoy plane and points to the sky, that is, the x-axis, y-axis and z-axis constitute a right-handed coordinate system.

[0032] Furthermore, the emission time k of the DVL speed measurement signal is T and receiving time k R The update method is:

[0033] The DVL speed measurement signal transmission time k T Update to time k, and then update the receiving time k of the DVL speed measurement signal according to the updated transmission time of the DVL speed measurement signal R :

[0034] k R =k T +2h / cT SINS cosα

[0035] Where h is the water depth, α is the DVL beam inclination angle, c is the speed of sound, and T SINS It is the update period of the inertial device.

[0036] Furthermore, in step 5, the carrier velocity in the carrier coordinate system b solved by SINS is calculated. and measurement matrix The specific process is:

[0037] Step 51: Calculate the carrier velocity in the carrier coordinate system b solved by SINS

[0038]

[0039] in, is k calculated by SINS T The moment-to-moment attitude transformation matrix, is the posture transformation matrix The transpose of k is the k calculated by SINS T The carrier speed in the navigation coordinate system n at the moment, for The x-axis component of the navigation coordinate system n, for The y-axis component of the navigation coordinate system n, for The z-axis direction component in the navigation coordinate system n;

[0040] Step 52: Calculate the measurement matrix

[0041]

[0042] in, express The cross product matrix, 0 3×9 represents a 3×9 zero matrix;

[0043]

[0044] Furthermore, in step 6, the state recursive matrix F at time k is calculated k and the measurement noise matrix R k The specific process is:

[0045] Step 6. Calculate the state recursion matrix F at the current k moment k :

[0046] F k =F k,k-1 F k-1

[0047] Among them, F k,k-1 is the state transfer matrix at time k, F k-1 is the state recursion matrix at time k-1;

[0048] Step 6.2: Calculate the measurement noise matrix R at the current time k k :

[0049]

[0050] Among them, τ k is the system noise distribution matrix at time k, Indicates F k The inverse matrix, Rk-1 represents the measurement noise matrix at time k-1.

[0051] Furthermore, in step 7, the carrier velocity in the carrier coordinate system b solved by SINS is calculated. and measurement matrix The specific process is:

[0052] Step 7.1 Calculate the carrier velocity in the carrier coordinate system b solved by SINS

[0053]

[0054] in, is k calculated by SINS R The moment-to-moment attitude transformation matrix, is the posture transformation matrix The transpose of k is the k calculated by SINS R The carrier speed in the navigation coordinate system n at the moment, for The x-axis component of the navigation coordinate system n, for The y-axis component of the navigation coordinate system n, for The z-axis direction component in the navigation coordinate system n;

[0055] Step 72: Calculate the measurement matrix

[0056]

[0057] in, express The cross product matrix, 0 3×9 represents a 3×9 zero matrix;

[0058]

[0059] Furthermore, in step eight, the state recursive matrix F′ at time k is calculated k and the measurement noise matrix R′ k The specific process is:

[0060] Step 81: Calculate the state recursive matrix F' at time k k :

[0061] F′ k =F k,k-1 F′ k-1

[0062] Among them, F′k-1 is the state recursion matrix at time k-1;

[0063] Step 82: Calculate the measurement noise matrix R' at time k k :

[0064]

[0065] in, F′ k The inverse matrix, R′ k-1 represents the measurement noise matrix at time k-1.

[0066] Furthermore, the calculation method of the current state quantity time update result is:

[0067]

[0068] in, x k-1 is the state quantity at time k-1, φ k-1 is the SINS attitude solution error at time k-1, Represents φ k-1 x in the vector coordinate system b b Axial component, Represents φ k-1 y in the carrier coordinate system b b Axial component, Represents φ k-1 z in the carrier coordinate system b b Axial component, is the velocity solution error at time k-1, express The x-axis component of the navigation coordinate system n, express The y-axis component in the navigation coordinate system n, express The z-axis component in the navigation coordinate system n, δp k-1 is the position solution error at time k-1, is the latitude error at time k-1, δλ k-1 is the longitude error at time k-1, δh k-1 is the height error at time k-1, is the gyro bias at time k-1, express x in the vector coordinate system b b Axial component, express y in the carrier coordinate system b b Axial component, express z in the carrier coordinate system b b Axial component, is the added table drift at time k-1, express x in the vector coordinate system b b Axial component, express y in the carrier coordinate system b b Axial component, express z in the carrier coordinate system b b Axis component, x k,k-1 is the time update result of the state quantity at time k, F k,k-1 is the state transfer matrix, P k-1 is the error covariance matrix at time k-1, τ k is the system noise distribution matrix at time k, Q k is the system noise covariance matrix at time k, P k,k-1 is the prior estimation result of the error covariance matrix at time k, and the superscript T represents the transpose of the matrix.

[0069] Furthermore, in step nine, according to And Doppler velocity information output by DVL Calculation measurement According to State recursion matrix Measurement Noise Matrix State recursion matrix and the measurement noise matrix To process the current state quantity time update result, obtain the state quantity and covariance matrix after the current optimal estimate; the specific process is:

[0070] Step 91: Calculation and Measurement

[0071]

[0072] in, It is the velocity information of the carrier in the carrier coordinate system output by DVL;

[0073] Step 92: Calculate the measurement matrix

[0074]

[0075] Step 93. Calculate the measurement noise matrix

[0076]

[0077] Among them, R DVL It is the measurement noise matrix determined according to the DVL speed measurement noise variance characteristics;

[0078] Step 94: Filter gain To update:

[0079]

[0080] Step 95: Update the state quantity to obtain the optimal estimated state quantity:

[0081]

[0082] Among them, x k is the state quantity after optimal estimation at time k;

[0083] Step 96: Update the covariance matrix:

[0084]

[0085] Among them, I 15×15 is a 15×15 dimensional unit matrix, P k is the error covariance matrix after optimal estimation at time k.

[0086] The beneficial effects of the present invention are:

[0087] The invention compensates for the DVL Doppler acoustic delay, constructs a new measurement through the DVL speed measurement information and the SINS history information, and derives the acoustic delay compensation navigation algorithm filter equation, so as to use the measurement at the past time to complete the estimation of the current state quantity, solves the problem that the measurement speed deviation caused by the acoustic Doppler delay leads to the rapid increase of the navigation position error, and further improves the precision and robustness of the combined navigation; the invention converts the data fusion between the SINS and the DVL into the carrier coordinate system, omitting the step of converting the coordinate system of the DVL speed measurement information, thereby avoiding the error of the speed measurement result coupled with the DVL sending and receiving time in the coordinate conversion process; the invention constructs the measurement of the DVL sending and receiving time by using the SINS solution history information and the DVL speed measurement information, reduces the measurement deviation caused by time asynchrony, and then the extended DKF filter performs the optimal estimation of the current state quantity through the measurement of two past times, accurately compensates the DVL acoustic delay based on the recursive relationship between the DVL sending and receiving time state quantity and the current state quantity, and solves the problem that the existing method cannot accurately compensate the DVL acoustic delay. BRIEF DESCRIPTION OF THE DRAWINGS

[0088] Figure 1 Schematic diagram of the DVL acoustic delay generation process;

[0089] Figure 2 Schematic diagram of the method of the present invention;

[0090] Figure 3 Flowchart of an acoustic Doppler delay information assisted integrated navigation method of the present invention. Specific embodiments

[0091] Specific embodiment 1: In combination with Figure 2 and Figure 3 This embodiment is described. An acoustic Doppler delay information assisted integrated navigation method described in this embodiment specifically includes the following steps:

[0092] Step 1: Establish a vehicle coordinate system b and a navigation coordinate system n, obtain the initial values of the vehicle attitude, speed, and position (obtained through alignment, calibration, and GPS respectively), define the initial value P0 of the state covariance matrix (determined according to the device accuracy), the system noise covariance matrix Q1 (the system noise covariance matrix is a constant matrix determined by the inertial device noise, and the system noise covariance matrices at each moment are the same), and the initial value of the state transition matrix Initial value of the state transition matrix Initial value of the measurement noise matrix And the initial value of the measurement noise matrix

[0093] Among them, the state transition matrix F k and F k ' means the product of the state transition matrix F k,k-1 , that is The relationship between i and j satisfies i < j, and i and j respectively represent the start and end times of the product. For the convenience of performing the product, it is split into step-by-step multiplications in a loop. Let the initial value of the state transition matrix I 15×15 be a 15×15 dimensional identity matrix, and the initial value of the measurement noise matrix 0 3×3 be a 3×3 dimensional 0 matrix;

[0094] And initialize the time k = 0;

[0095] Step 2: Determine whether the DVL triggers a synchronization signal at time k:

[0096] If the DVL triggers a synchronization signal at time k, then update the transmission time k T and the reception time k R of the DVL speed measurement signal, and then continue to execute Step 4;

[0097] If the DVL does not trigger a synchronization signal at time k, then execute Step 3;

[0098] Step 3: SINS data is solved and the time update result of the current state quantity is calculated, and then k=k+1 is set, and the execution of step 2 is returned;

[0099] Step 4: Compare the current time k with k T , k R and DVL data update time k D size;

[0100] If k = k T , then execute step five;

[0101] If k T <k<k R , then execute step 6;

[0102] If k = k R , then execute step seven;

[0103] If k R <k<k D , then execute step eight;

[0104] If k = k D , then execute step nine;

[0105] The time relationship in the present invention satisfies: k D >k R >k T , and in k D Before the moment, the synchronization signal will no longer be triggered;

[0106] Step 5: Solve the SINS data and calculate the current state time update result, and calculate the carrier speed in the carrier coordinate system b solved by SINS and measurement matrix And let k=k+1, and return to execute step 4;

[0107] Step 6: Solve the SINS data and calculate the current state time update result, and calculate the state recursive matrix F at time k k and the measurement noise matrix R k , and set k=k+1, and return to step 4;

[0108] Step 7: Solve the SINS data and calculate the current state time update result, and calculate the carrier speed in the carrier coordinate system b solved by SINS and measurement matrix And let k=k+1, and return to execute step 4;

[0109] Step 8: Solve the SINS data and calculate the current state time update result, and calculate the state recursive matrix F at time kk , measurement noise matrix R k And the state recursive matrix F′ at time k k , measurement noise matrix R′ k , and set k=k+1, and return to step 4;

[0110] Step 9: Solve the SINS data and calculate the current state time update result, and calculate k D The state recursion matrix at time Measurement Noise Matrix State recursion matrix Measurement Noise Matrix

[0111] According to And Doppler velocity information output by DVL Calculation measurement According to State recursion matrix Measurement Noise Matrix State recursion matrix and the measurement noise matrix To process the current state quantity time update result, obtain the state quantity and covariance matrix after the optimal estimation at the current moment, use the optimal estimated state quantity to feedback correct the navigation solution value, and then execute step ten;

[0112] Step 10: Set k=k+1 and return to step 2.

[0113] The invention updates the transmitting time and receiving time of a DVL speed measurement signal through a synchronization signal trigger time, and performs subsequent processing based on the updating result, thereby obtaining accurate DVL acoustic Doppler delay information and SINS solution information at the time of DVL transmitting and receiving signals, and then uses the obtained acoustic Doppler delay information, the SINS solution information at the time of DVL transmitting and receiving signals, and the recursive relationship of the navigation filter state quantity at different time to compensate for the acoustic delay of the DVL, so as to assist the combined navigation.

[0114] Specific implementation method 2: This implementation method is different from the specific implementation method 1 in that the carrier coordinate system b and the navigation coordinate system n are established as follows:

[0115] Carrier coordinate system b:

[0116] The center of gravity of the platform is taken as the origin o of the carrier coordinate system b b , the bow direction along the platform cross section pointing to the platform is taken as y b The positive direction of the axis is the direction perpendicular to the deck upward as z b Positive axis direction, x bAxis, y b Axis and z b The axes form a right-handed coordinate system;

[0117] Navigation coordinate system n:

[0118] The center of gravity of the platform is taken as the origin o of the navigation coordinate system n, the geographic east direction is taken as the positive direction of the x-axis, the geographic north direction is taken as the positive direction of the y-axis, and the positive direction of the z-axis is perpendicular to the xoy plane and points to the sky, that is, the x-axis, y-axis and z-axis constitute a right-handed coordinate system.

[0119] The other steps and parameters are the same as those in the first embodiment.

[0120] Specific implementation method three: This implementation method is different from specific implementation methods one or two in that the emission time k of the DVL speed measurement signal T and receiving time k R The update method is:

[0121] The DVL speed measurement signal transmission time k T Update to time k, and then update the receiving time k of the DVL speed measurement signal according to the updated transmission time of the DVL speed measurement signal R :

[0122] k R =k T +2h / cT SINS cosα

[0123] Where h is the water depth, α is the DVL beam inclination angle, c is the speed of sound, and T SINS It is the update period of the inertial device.

[0124] The other steps and parameters are the same as those in the first or second embodiment.

[0125] Specific implementation method 4: This implementation method is different from any one of the specific implementation methods 1 to 3 in that in step 5, the carrier velocity in the carrier coordinate system b solved by SINS is calculated. and measurement matrix The specific process is:

[0126] Step 51: Calculate the carrier velocity in the carrier coordinate system b solved by SINS

[0127]

[0128] in, is k calculated by SINS T The moment-to-moment attitude transformation matrix, is the posture transformation matrix The transpose of k is the k calculated by SINST The carrier speed in the navigation coordinate system n at the moment, for The x-axis component of the navigation coordinate system n, for The y-axis component in the navigation coordinate system n, for The z-axis direction component in the navigation coordinate system n;

[0129] Step 52: Calculate the measurement matrix

[0130]

[0131] in, express The cross product matrix, 0 3×9 represents a 3×9 zero matrix;

[0132]

[0133] The other steps and parameters are the same as those in Specific Embodiments 1 to 3.

[0134] It should be noted that, in theory, the attitude conversion matrix and carrier velocity in step 52 should be substituted with true values ​​for calculation. However, since the true values ​​cannot be obtained, the values ​​calculated by SINS are directly substituted for the calculation.

[0135] Specific implementation method 5: This implementation method is different from any one of the specific implementation methods 1 to 4 in that, in step 6, the state recursive matrix F at time k is calculated. k and the measurement noise matrix R k The specific process is:

[0136] Step 6. Calculate the state recursion matrix F at the current k moment k :

[0137] F k =F k,k-1 F k-1

[0138] Among them, F k,k-1 is the state transfer matrix at time k, F k-1 is the state recursive matrix at time k-1. If k=k at the current time T +1, then F k-1 Substitute the initial value of the state recursion matrix

[0139] Step 6.2: Calculate the measurement noise matrix R at the current time k k :

[0140]

[0141] Among them, τ k is the system noise distribution matrix at time k, Indicates F k The inverse matrix, R k-1 represents the measurement noise matrix at time k-1. If k=k at the current time T +1, then R k-1 Substitute the initial value of the measurement noise matrix

[0142] The other steps and parameters are the same as those in Specific Embodiments 1 to 4.

[0143] Specific implementation method 6: This implementation method is different from the specific implementation methods 1 to 5 in that in step 7, the carrier velocity in the carrier coordinate system b solved by SINS is calculated. and measurement matrix The specific process is:

[0144] Step 7.1 Calculate the carrier velocity in the carrier coordinate system b solved by SINS

[0145]

[0146] in, is k calculated by SINS (inertial navigation) R The moment-to-moment attitude transformation matrix, is the posture transformation matrix The transpose of k is the k calculated by SINS R The carrier speed in the navigation coordinate system n at the moment, for The x-axis component of the navigation coordinate system n, for The y-axis component in the navigation coordinate system n, for The z-axis direction component in the navigation coordinate system n;

[0147] Step 72: Calculate the measurement matrix

[0148]

[0149] in, express The cross product matrix, 0 3×9 represents a 3×9 zero matrix;

[0150]

[0151] The other steps and parameters are the same as those in Specific Implementation Methods 1 to 5.

[0152] It should be noted that, in theory, the attitude conversion matrix and carrier velocity in step 72 should be calculated with true values. However, since the true values ​​cannot be obtained, the values ​​solved by SINS are directly used for calculation.

[0153] Specific implementation method 7: This implementation method is different from any one of the specific implementation methods 1 to 6 in that, in step 8, the state recursive matrix F′ at time k is calculated. k and the measurement noise matrix R′ k The specific process is:

[0154] Step 81: Calculate the state recursive matrix F' at time k k :

[0155] F′ k =F k,k-1 F′ k-1

[0156] Among them, F′ k-1 is the state recursive matrix at time k-1. If k=k at the current time R +1, then F′ k-1 Substitute the initial value of the state recursion matrix

[0157] Step 82: Calculate the measurement noise matrix R' at time k k :

[0158]

[0159] in, F′ k The inverse matrix, R′ k-1 represents the measurement noise matrix at time k-1. If k=k at the current time R +1, then R′ k-1 Substitute the initial value of the measurement noise matrix

[0160] The other steps and parameters are the same as those in Specific Embodiments 1 to 6.

[0161] Specific implementation eight: This implementation differs from any one of specific implementations one to seven in that the calculation method of the current state quantity time update result is:

[0162]

[0163] in, x k-1 is the state quantity at time k-1, φ k-1 is the SINS attitude solution error at time k-1, Represents φ k-1 x in the vector coordinate system b b Axial component, Represents φ k-1 y in the carrier coordinate system b b Axial component, Represents φ k-1 z in the carrier coordinate system b b Axial component, is the velocity solution error at time k-1, express The x-axis component of the navigation coordinate system n, express The y-axis component in the navigation coordinate system n, express The z-axis component in the navigation coordinate system n, δp k-1 is the position solution error at time k-1, is the latitude error at time k-1, δλ k-1 is the longitude error at time k-1, δh k-1 is the height error at time k-1, is the gyro bias at time k-1, express x in the vector coordinate system b b Axial component, express y in the carrier coordinate system b b Axial component, express z in the carrier coordinate system b b Axial component, is the added table drift at time k-1, express x in the vector coordinate system b b Axial component, express y in the carrier coordinate system b b Axial component, express z in the carrier coordinate system b b Axis component, x k,k-1 is the time update result of the state quantity at time k, F k,k-1 is the state transfer matrix, P k-1 is the error covariance matrix at time k-1, τ kis the system noise distribution matrix at time k, Q k is the system noise covariance matrix at time k, P k,k-1 is the prior estimation result of the error covariance matrix at time k, and the superscript T represents the transpose of the matrix.

[0164] The other steps and parameters are the same as those in Specific Embodiments 1 to 7.

[0165] When k<k D When x is calculated at the current time k,k-1 As x at the next moment k-1 To participate in the operation.

[0166] Specific implementation method 9: This implementation method is different from the specific implementation methods 1 to 8 in that, in step 9, according to And Doppler velocity information output by DVL Calculation measurement According to State recursion matrix Measurement Noise Matrix State recursion matrix and the measurement noise matrix To process the current state quantity time update result, obtain the state quantity and covariance matrix after the current optimal estimate; the specific process is:

[0167] Step 91: Calculation and Measurement

[0168]

[0169] in, The velocity information of the carrier in the carrier coordinate system output by DVL (DVL outputs once, Update once);

[0170] Step 92: Calculate the measurement matrix

[0171]

[0172] Step 93. Calculate the measurement noise matrix

[0173]

[0174] Among them, R DVL is the measurement noise matrix determined according to the DVL speed measurement noise variance characteristics and is a constant value matrix;

[0175] Step 94: Filter gain To update:

[0176]

[0177] Step 95: Update the state quantity to obtain the optimal estimated state quantity:

[0178]

[0179] Among them, x k is the state quantity after optimal estimation at time k;

[0180] Step 96: Update the covariance matrix:

[0181]

[0182] Among them, I 15×15 is a 15×15 dimensional unit matrix, P k is the error covariance matrix after optimal estimation at time k.

[0183] The other steps and parameters are the same as those in Specific Embodiments 1 to 8.

[0184] x k and P k Used to update the state quantity time at the next moment.

[0185] The above calculation examples of the present invention are only used to explain the calculation model and calculation process of the present invention in detail, and are not intended to limit the implementation methods of the present invention. For ordinary technicians in the relevant field, other different forms of changes or modifications can be made based on the above description. It is impossible to list all the implementation methods here. All obvious changes or modifications derived from the technical solution of the present invention are still within the scope of protection of the present invention.

Claims

1. An acoustic Doppler delay information assisted integrated navigation method, characterized in that: The method specifically comprises the following steps: Step 1: Establish the carrier coordinate system b and the navigation coordinate system n, obtain the initial values ​​of the carrier attitude, velocity and position, define the initial value of the state covariance matrix P0, the system noise covariance matrix Q1, and the initial value of the state recursion matrix Initial value of state recursion matrix Initial value of measurement noise matrix And the initial value of the measurement noise matrix Among them, the initial value of the state recursion matrix I 15×15 is a 15×15 dimensional identity matrix, measuring the initial value of the noise matrix 0 3×3 is a 3×3 dimensional zero matrix; And initialize time k = 0; Step 2: Determine whether the DVL triggers the synchronization signal at time k: If the DVL triggers the synchronization signal at time k, the transmission time k of the DVL speed measurement signal is updated T and receiving time k R Then proceed to step 4; If the DVL does not trigger the synchronization signal at time k, execute step 3; Step 3: SINS data is solved and the time update result of the current state quantity is calculated, and then k=k+1 is set, and the execution of step 2 is returned; Step 4: Compare the current time k with k T , k R and DVL data update time k D size; If k = k T , then execute step five; If k T <k<k R , then execute step 6; If k = k R , then execute step seven; If k R <k<k D , then execute step eight; If k = k D , then execute step nine; Step 5: Solve the SINS data and calculate the current state time update result, and calculate the carrier speed in the carrier coordinate system b solved by SINS and measurement matrix And let k=k+1, and return to execute step 4; Step 6: Solve the SINS data and calculate the current state time update result, and calculate the state recursive matrix F at time k k and the measurement noise matrix R k , and set k=k+1, and return to step 4; Step 7: Solve the SINS data and calculate the current state time update result, and calculate the carrier speed in the carrier coordinate system b solved by SINS and measurement matrix And let k=k+1, and return to execute step 4; Step 8: Solve the SINS data and calculate the current state time update result, and calculate the state recursive matrix F at time k k , measurement noise matrix R k And the state recursive matrix F at time k k ′, measurement noise matrix R′ k , and set k=k+1, and return to step 4; Step 9: Solve the SINS data and calculate the current state time update result, and calculate k D The state recursion matrix at time Measurement Noise Matrix State recursion matrix Measurement Noise Matrix According to And Doppler velocity information output by DVL Calculation measurement According to State recursion matrix Measurement Noise Matrix State recursion matrix and the measurement noise matrix To process the current state quantity time update result, obtain the state quantity and covariance matrix after the optimal estimation at the current moment, use the optimal estimated state quantity to feedback correct the navigation solution value, and then execute step ten; Step 10: Set k=k+1 and return to step 2.

2. The acoustic Doppler delay information assisted integrated navigation method according to claim 1, characterized in that: The establishment of the carrier coordinate system b and the navigation coordinate system n is specifically as follows: Carrier coordinate system b: The center of gravity of the platform is taken as the origin o of the carrier coordinate system b b , the bow direction along the platform cross section pointing to the platform is taken as y b The positive direction of the axis is the direction perpendicular to the deck upward as z b Positive axis direction, x b Axis, y b Axis and z b The axes form a right-handed coordinate system; Navigation coordinate system n: The center of gravity of the platform is taken as the origin o of the navigation coordinate system n, the geographic east direction is taken as the positive direction of the x-axis, the geographic north direction is taken as the positive direction of the y-axis, and the positive direction of the z-axis is perpendicular to the xoy plane and points to the sky, that is, the x-axis, y-axis and z-axis constitute a right-handed coordinate system.

3. The acoustic Doppler delay information assisted integrated navigation method according to claim 2, characterized in that: The emission time k of the DVL speed measurement signal T and receiving time k R The update method is: The DVL speed measurement signal transmission time k T Update to time k, and then update the receiving time k of the DVL speed measurement signal according to the updated transmission time of the DVL speed measurement signal R : k R =k T +2h / cT SINS cosα Where h is the water depth, α is the DVL beam inclination angle, c is the speed of sound, T SINS It is the update period of the inertial device.

4. The acoustic Doppler delay information assisted integrated navigation method according to claim 3, characterized in that: In step 5, the carrier velocity in the carrier coordinate system b solved by SINS is calculated. and measurement matrix The specific process is: Step 51: Calculate the carrier velocity in the carrier coordinate system b solved by SINS in, is k calculated by SINS T The moment-to-moment attitude transformation matrix, is the posture transformation matrix The transpose of k is the k calculated by SINS T The carrier speed in the navigation coordinate system n at the moment, for The x-axis component of the navigation coordinate system n, for The y-axis component in the navigation coordinate system n, for The z-axis direction component in the navigation coordinate system n; Step 52: Calculate the measurement matrix in, × means The cross product matrix, 0 3×9 represents a 3×9 zero matrix; 5. The acoustic Doppler delay information assisted integrated navigation method according to claim 4, characterized in that: In step 6, the state recursive matrix F at time k is calculated k and the measurement noise matrix R k The specific process is: Step 6. Calculate the state recursion matrix F at the current k moment k : F k =F k,k-1 F k-1 Among them, F k,k-1 is the state transfer matrix at time k, F k-1 is the state recursion matrix at time k-1; Step 6.2: Calculate the measurement noise matrix R at the current time k k : Among them, τ k is the system noise distribution matrix at time k, Indicates F k The inverse matrix, R k-1 represents the measurement noise matrix at time k-1.

6. The acoustic Doppler delay information assisted integrated navigation method according to claim 5, characterized in that: In step 7, the carrier velocity in the carrier coordinate system b solved by SINS is calculated. and measurement matrix The specific process is: Step 7.1 Calculate the carrier velocity in the carrier coordinate system b solved by SINS in, is k calculated by SINS R The moment-to-moment attitude transformation matrix, is the posture transformation matrix The transpose of k is the k calculated by SINS R The carrier speed in the navigation coordinate system n at the moment, for The x-axis component of the navigation coordinate system n, for The y-axis component in the navigation coordinate system n, for The z-axis direction component in the navigation coordinate system n; Step 72: Calculate the measurement matrix in, × means The cross product matrix, 0 3×9 represents a 3×9 zero matrix; 7. The acoustic Doppler delay information assisted integrated navigation method according to claim 6, characterized in that: In step eight, the state recursive matrix F at time k is calculated. k ′ and the measurement noise matrix R′ k The specific process is: Step 81: Calculate the state recursive matrix F at time k k ′: F k ′=F k,k-1 F k ′ -1 Among them, F k ' -1 is the state recursion matrix at time k-1; Step 82: Calculate the measurement noise matrix R' at time k k : in, Indicates F k ′, R′ k-1 represents the measurement noise matrix at time k-1.

8. The acoustic Doppler delay information assisted integrated navigation method according to claim 7, characterized in that: The calculation method of the current state quantity time update result is: in, x k-1 is the state quantity at time k-1, φ k-1 is the SINS attitude solution error at time k-1, Represents φ k-1 x in the vector coordinate system b b Axial component, Represents φ k-1 y in the carrier coordinate system b b Axial component, Represents φ k-1 z in the carrier coordinate system b b Axial component, is the velocity solution error at time k-1, express The x-axis component of the navigation coordinate system n, express The y-axis component in the navigation coordinate system n, express The z-axis component in the navigation coordinate system n, δp k-1 is the position solution error at time k-1, is the latitude error at time k-1, δλ k-1 is the longitude error at time k-1, δh k-1 is the height error at time k-1, is the gyro bias at time k-1, express x in the vector coordinate system b b Axial component, express y in the carrier coordinate system b b Axial component, express z in the carrier coordinate system b b Axial component, is the added table drift at time k-1, express x in the vector coordinate system b b Axial component, express y in the carrier coordinate system b b Axial component, express z in the carrier coordinate system b b Axis component, x k,k-1 is the time update result of the state quantity at time k, F k,k-1 is the state transfer matrix, P k-1 is the error covariance matrix at time k-1, τ k is the system noise distribution matrix at time k, Q k is the system noise covariance matrix at time k, P k,k-1 is the prior estimation result of the error covariance matrix at time k, and the superscript T represents the transpose of the matrix.

9. The acoustic Doppler delay information assisted integrated navigation method according to claim 8, characterized in that: In the step nine, according to And Doppler velocity information output by DVL Calculation measurement According to State recursion matrix Measurement Noise Matrix State recursion matrix and the measurement noise matrix To process the time update result of the current state quantity, and obtain the state quantity and covariance matrix after the optimal estimation at the current moment; The specific process is: Step 91: Calculation and Measurement in, It is the velocity information of the carrier in the carrier coordinate system output by DVL; Step 92: Calculate the measurement matrix Step 93. Calculate the measurement noise matrix Among them, R DVL It is the measurement noise matrix determined according to the DVL speed measurement noise variance characteristics; Step 94: Filter gain To update: Step 95: Update the state quantity to obtain the optimal estimated state quantity: Among them, x k is the state quantity after optimal estimation at time k; Step 96: Update the covariance matrix: Among them, I 15×15 is a 15×15 dimensional unit matrix, P k is the error covariance matrix after optimal estimation at time k.

Images