An acoustic doppler delay information aided integrated navigation method

Abstract

The application discloses an acoustic Doppler delay information auxiliary combination navigation method, and belongs to the technical field of underwater acoustic velocity measurement and combination navigation. The application solves the problem that the existing method cannot accurately compensate for the acoustic delay of a DVL, and leads to the problems of biased SINS / DVL combination navigation measurement under maneuvering and rapidly increasing navigation position error with time. The method comprises the following steps: establishing a carrier coordinate system and a navigation coordinate system, and defining initial values of filter related variables of a delay compensation algorithm; obtaining a DVL velocity measurement signal transmission time and a receiving time according to a synchronization signal; respectively calculating a state transition matrix and a measurement noise matrix; at a DVL data update time, using SINS calculation historical information and DVL velocity measurement information to construct a measurement of two times of DVL transmission and receiving, and performing optimal estimation on a current time system error quantity; feeding back and correcting system errors obtained by the delay compensation algorithm to a combination navigation system, and completing acoustic Doppler delay information auxiliary navigation. The method can be applied to the technical field of underwater acoustic velocity measurement and combination navigation.

Description

Technical Field

[0001] This invention belongs to the field of underwater acoustic velocity measurement and integrated navigation technology, specifically relating to an acoustic Doppler delay information-assisted integrated navigation method. Background Technology

[0002] Due to its stealth and autonomy, SINS / DVL integrated navigation is the most commonly used navigation method for AUVs. However, with the continuous deepening of ocean exploration, underwater vehicles are facing breakthrough developments from short-range to long-range navigation, from coastal waters to deep seas, and from automation to intelligence. Facing the need for precise autonomous navigation in the unknown and complex environment of the deep sea, new requirements are placed on 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 a platform under maneuver. However, DVL acoustic information is subject to a certain delay due to factors such as underwater acoustic channel transmission, echo signal processing, and serial communication output. Figure 1As shown, Reference 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 asynchronous data in the integrated system, resulting in serious navigation errors. Currently, there are three commonly used delay compensation methods. The first method is to use the delay time as a state variable for filtering estimation and compensation. For example, Reference 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 good compensation results in a relatively short delay time (around 100ms). However, for the longer delay time and large maneuvering conditions described in reference 3 (Hansen JM, Fossen TI, Johansen T A. Nonlinear observer design for GNSS-aided inertial navigation systems with time-delayed GNSS measurements[J]. Control Engineering Practice, 2017, 60:39-50.), the KF state estimation requirements are no longer met due to the long delay time and the non-small amount of filtered state variables, and other methods are needed for compensation. The second method is to synchronize the sensors through pulse triggering (such as GNSS Pulse Per Second, PPS), and then compensate for the known time delay through data latching and interpolation, as in 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.). However, due to the influence of device noise, the accuracy of the data delay compensation method through data interpolation is limited.The last type is a navigation method that uses the recursive relationship between past and current state quantities to estimate the current state using measurement information from past moments. Examples include the delayed Kalman filter algorithm in reference 5 (Xu B, Wang X, Zhang J, et al. Maximum Correntropydelay Kalman filter for SINS / USBL integrated navigation[J]. ISA Transactions, 2021.), which is mainly used in the field of cooperative navigation. In addition, compensation for DVL delay needs to consider the Doppler velocity measurement principle. DVL Doppler information is modulated by the carrier velocity at both the time of transmission and reception of the velocity measurement signals. The acoustic delay time is related to both the transmission and reception times of the acoustic signal, which makes the use of DVL information and delay compensation difficult. 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 of constant horizontal velocity of the carrier. It obtains the carrier velocity at the DVL reception moment under dynamic attitude angles based on the lever effect, and verifies the algorithm's effectiveness under circular, spiral, and lawnmower trajectories through simulation. 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 moment and verifies the algorithm's navigation capability under high-maneuverability through simulation experiments.

[0004] In summary, although existing methods have made some progress in DVL acoustic delay compensation, they still cannot accurately compensate for DVL acoustic delay, resulting in biased measurements in SINS / DVL integrated navigation under maneuver and a rapid increase in navigation position error over time. Summary of the Invention

[0005] The purpose of this invention is to solve the problem that existing methods cannot accurately compensate for DVL acoustic delay, which leads to biased measurements and rapid increase in navigation position error in SINS / DVL integrated navigation under maneuver. This invention proposes an acoustic Doppler delay information-assisted integrated navigation method.

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

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

[0008] Wherein, the initial value of the state recursion matrix I 15×15 It is a 15×15 dimensional identity matrix, and the initial value of the measurement noise matrix is... 0 3×3 It is a 3×3 dimensional zero matrix;

[0009] And initialize time k = 0;

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

[0011] If the DVL triggers a synchronization signal at time k, then update the transmission time k of the DVL speed measurement signal. T and receiving time k R Then, proceed to step four;

[0012] If DVL does not trigger the synchronization signal at time k, then proceed to step three;

[0013] Step 3: Solve the SINS data and calculate the time update result of the current state quantity, then let k = k + 1, and return to execute Step 2;

[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 proceed to step five;

[0016] If k T <k<k R Then proceed to step six;

[0017] If k = k R Then proceed to step seven;

[0018] If k R <k<k D Then proceed to step eight;

[0019] If k = k D Then proceed to step nine;

[0020] Step 5: Solve the SINS data and calculate the time update result of the current state quantity. Calculate the carrier velocity in the carrier coordinate system b calculated by SINS. and measurement matrix Then set k = k + 1 and return to step four;

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

[0022] Step 7: Solve the SINS data and calculate the time update result of the current state quantity. Calculate the carrier velocity in the carrier coordinate system b calculated by SINS. and measurement matrix Then set k = k + 1 and return to step four;

[0023] Step 8: Solve the SINS data and calculate the time update result of the state quantity at the current time, and calculate the state recursion matrix F at time k. k Measurement noise matrix R k And the state recurrence matrix F′ at time k k Measurement noise matrix R′ k Then set k = k + 1 and return to step four;

[0024] Step 9: Solve the SINS data and calculate the time update result of the current state quantity, and calculate k. D The state recurrence matrix at each time step Measurement noise matrix State recurrence matrix Measurement noise matrix

[0025] Then according to and the Doppler velocity information output by DVL Calculation measurement Then according to State recurrence matrix Measurement noise matrix State recurrence matrix and measurement noise matrix The current state quantity time update result is processed to obtain the optimal estimated state quantity and covariance matrix at the current time. The optimal estimated state quantity is used to perform feedback correction on the navigation solution value, and then step ten is executed.

[0026] Step 10: Let k = k + 1, then return to step 2.

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

[0028] Carrier coordinate system b:

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

[0030] Navigation coordinate system n:

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

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

[0033] The DVL speed measurement signal transmission time k T Update the time to time k, and then update the receiving time k of the DVL speed measurement signal based on 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 tilt angle, c is the speed of sound, and T is the water depth. SINS This refers to the replacement cycle of inertial devices.

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

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

[0038]

[0039] in, k is calculated by SINS. T Attitude transition matrix at time step, attitude transformation matrix transpose, k calculated for SINS T The velocity of the vehicle in the navigation coordinate system n at any given moment. for In the x-axis direction component of the navigation coordinate system n for In the y-axis direction component of the navigation coordinate system n for The z-axis 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 six, the state recursion matrix F at time k is calculated. k and measurement noise matrix R k The specific process is as follows:

[0045] Step 61: Calculate the state recurrence matrix F at time k. k :

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

[0047] Among them, F k,k-1 Let F be the state transition matrix at time k. k-1 It is the state recurrence matrix at time k-1;

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

[0049]

[0050] Where, τ k Assign a noise matrix to the system at time k. F represents k The inverse matrix, Rk-1 This represents the measurement noise matrix at time k-1.

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

[0052] Step 71: Calculate the carrier velocity in the carrier coordinate system b of the SINS solution.

[0053]

[0054] in, k is calculated by SINS. R Attitude transition matrix at time step, attitude transformation matrix transpose, k calculated for SINS R The velocity of the vehicle in the navigation coordinate system n at any given moment. for In the x-axis direction component of the navigation coordinate system n for In the y-axis direction component of the navigation coordinate system n for The z-axis 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 recursion matrix F′ at time k is calculated. k and measurement noise matrix R′ k The specific process is as follows:

[0060] Step 81: Calculate the state recurrence matrix F′ at time k. k :

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

[0062] Among them, F′k-1 It is the state recurrence 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 This represents the measurement noise matrix at time k-1.

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

[0067]

[0068] in, x k-1 The state variable at time k-1 φ k-1 The SINS attitude calculation error at time k-1 is... Represents φ k-1 In the x-coordinate of the carrier coordinate system b b Axial component, Represents φ k-1 In the carrier coordinate system b, y b Axial component, Represents φ k-1 In the carrier coordinate system b, z b Axial component, The velocity calculation error at time k-1 is... express In the x-axis direction component of the navigation coordinate system n express In the y-axis direction component of the navigation coordinate system n express In the z-axis direction component of the navigation coordinate system n δp k-1 The position calculation error at time k-1, It is the latitude error at time k-1, δλ k-1 It is the longitude error at time k-1, δh k-1 It is the height error at time k-1. The gyroscope has zero bias at time k-1. express In the x-coordinate of the carrier coordinate system b b Axial component, express In the carrier coordinate system b, y b Axial component, express In the carrier coordinate system b, z b Axial component, For the table drift at time k-1, express In the x-coordinate of the carrier coordinate system b b Axial component, express In the carrier coordinate system b, y b Axial component, express In the carrier coordinate system b, z b Axial component, x k,k-1 For the time update result of the state quantity at time k, F k,k-1 Let P be the state transition matrix. k-1 Let τ be the error covariance matrix at time k-1. k Let Q be the system noise assignment matrix at time k. k Let P be the system noise covariance matrix at time k. k,k-1 The prior estimate of the error covariance matrix at time k is given by the superscript T, which represents the transpose of the matrix.

[0069] Furthermore, in step nine, according to and the Doppler velocity information output by DVL Calculation measurement Then according to State recurrence matrix Measurement noise matrix State recurrence matrix and measurement noise matrix The current state quantity time update result is processed to obtain the optimal estimated state quantity and covariance matrix at the current time; the specific process is as follows:

[0070] Step 91: Calculation and Measurement

[0071]

[0072] in, This 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 a measurement noise matrix determined based on the variance characteristics of DVL speed measurement noise;

[0078] Step 94: Adjust the filter gain Update:

[0079]

[0080] Step 95: Update the state variables to obtain the optimally estimated state variables:

[0081]

[0082] Where, x k The state variables are the optimal estimates at time k;

[0083] Step 96: Update the covariance matrix:

[0084]

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

[0086] The beneficial effects of this invention are:

[0087] This invention compensates for DVL Doppler acoustic delay by constructing new measurements using DVL velocity information and SINS history information. It derives the filtering equation for the acoustic delay compensation navigation algorithm, thereby estimating the current state quantity using measurements from past moments. This solves the problem of rapidly increasing navigation position errors caused by velocity deviation due to acoustic Doppler delay, thus improving the accuracy and robustness of integrated navigation. Furthermore, by fusing and converting SINS and DVL data to the carrier coordinate system, this invention eliminates the need for coordinate system conversion of DVL velocity information, avoiding errors in coordinate conversion caused by coupled velocity results from DVL transmission and reception. The invention constructs measurements for both transmission and reception moments using SINS historical information and DVL velocity information, reducing measurement deviations caused by time asynchrony. Then, an extended DKF filter performs optimal estimation of the current state quantity using measurements from two past moments. Based on the recursive relationship between the DVL transmission and reception state quantities and the current state quantity, it accurately compensates for DVL acoustic delay, solving the problem that existing methods cannot accurately compensate for DVL acoustic delay. Attached Figure Description

[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 Flow chart 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 describe this embodiment. 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), the initial value of the state recurrence matrix Initial value of the state recurrence matrix Initial value of the measurement noise matrix and the initial value of the measurement noise matrix

[0093] Among them, the state recurrence 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, 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 recurrence 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 velocity 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: Solve the SINS data and calculate the time update result of the current state quantity, then let k = k + 1, and return to execute Step 2;

[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 proceed to step five;

[0101] If k T <k<k R Then proceed to step six;

[0102] If k = k R Then proceed to step seven;

[0103] If k R <k<k D Then proceed to step eight;

[0104] If k = k D Then proceed to step nine;

[0105] The time relationship in this invention satisfies: k D >k R >k T And in k D Before that time, no more synchronization signals will be triggered;

[0106] Step 5: Solve the SINS data and calculate the time update result of the current state quantity. Calculate the carrier velocity in the carrier coordinate system b calculated by SINS. and measurement matrix Then set k = k + 1 and return to step four;

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

[0108] Step 7: Solve the SINS data and calculate the time update result of the current state quantity. Calculate the carrier velocity in the carrier coordinate system b calculated by SINS. and measurement matrix Then set k = k + 1 and return to step four;

[0109] Step 8: Solve the SINS data and calculate the time update result of the state quantity at the current time, and calculate the state recursion matrix F at time k.k Measurement noise matrix R k And the state recurrence matrix F′ at time k k Measurement noise matrix R′ k Then set k = k + 1 and return to step four;

[0110] Step 9: Solve the SINS data and calculate the time update result of the current state quantity, and calculate k. D The state recurrence matrix at each time step Measurement noise matrix State recurrence matrix Measurement noise matrix

[0111] Then according to and the Doppler velocity information output by DVL Calculation measurement Then according to State recurrence matrix Measurement noise matrix State recurrence matrix and measurement noise matrix The current state quantity time update result is processed to obtain the optimal estimated state quantity and covariance matrix at the current time. The optimal estimated state quantity is used to perform feedback correction on the navigation solution value, and then step ten is executed.

[0112] Step 10: Let k = k + 1, then return to step 2.

[0113] This invention updates the transmission and reception times of the DVL velocity measurement signal by synchronizing the signal trigger time, and performs subsequent processing based on the update results to obtain accurate DVL acoustic Doppler delay information and SINS solution information at the DVL transmission and reception times. Then, it uses the obtained acoustic Doppler delay information, SINS solution information at the DVL transmission and reception times, and the recursive relationship of navigation filter state variables at different times to compensate for the acoustic delay of DVL, so as to assist integrated navigation.

[0114] Specific Implementation Method Two: This implementation method differs from Specific Implementation Method One in that the establishment of the carrier coordinate system b and the navigation coordinate system n is specifically as follows:

[0115] Carrier coordinate system b:

[0116] Taking the platform's center of gravity as the origin of the carrier coordinate system b. b The direction along the platform's cross-section pointing towards the bow of the platform is taken as y. b The positive direction of the axis is defined as the direction perpendicular to the deck and upwards. b Positive direction of the axis, x baxis, y b axis and z b The axes form a right-handed coordinate system;

[0117] Navigation coordinate system n:

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

[0119] The other steps and parameters are the same as in Specific Implementation Method 1.

[0120] Specific Implementation Method Three: This implementation method differs from Specific Implementation Method One or Two in that the transmission time k of the DVL speed measurement signal is... T and receiving time k R The update method is as follows:

[0121] The DVL speed measurement signal transmission time k T Update the time to time k, and then update the receiving time k of the DVL speed measurement signal based on 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 tilt angle, c is the speed of sound, and T is the water depth. SINS This refers to the replacement cycle of inertial devices.

[0124] Other steps and parameters are the same as in specific implementation method one or two.

[0125] Specific Implementation Method Four: This implementation method differs from Specific Implementation Methods One to Three in that, in step five, the carrier velocity in the carrier coordinate system b calculated by SINS is... and measurement matrix The specific process is as follows:

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

[0127]

[0128] in, k is calculated by SINS. T Attitude transition matrix at time step, attitude transformation matrix transpose, k calculated for SINST The velocity of the vehicle in the navigation coordinate system n at any given moment. for In the x-axis direction component of the navigation coordinate system n for In the y-axis direction component of the navigation coordinate system n for The z-axis 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 one of the specific implementation methods one to three.

[0134] It should be noted that, theoretically, the attitude transformation matrix and the carrier velocity in step 52 should be calculated using the true values. However, since the true values ​​are unavailable, the values ​​calculated by SINS are directly used here for the calculation.

[0135] Specific Implementation Method Five: This implementation method differs from Specific Implementation Methods One to Four in that, in step six, the state recursion matrix F at time k is calculated. k and measurement noise matrix R k The specific process is as follows:

[0136] Step 61: Calculate the state recurrence matrix F at time k. k :

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

[0138] Among them, F k,k-1 Let F be the state transition matrix at time k. k-1 It is the state recurrence matrix at time k-1. If the current time k = k T +1, then F k-1 Substitute the initial values ​​of the state recurrence matrix

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

[0140]

[0141] Where, τ k Assign a noise matrix to the system at time k. F represents k The inverse matrix, R k-1 This represents the measurement noise matrix at time k-1, if the current time k = k T +1, then R k-1 Substitute the initial values ​​of the measurement noise matrix

[0142] The other steps and parameters are the same as those in one of the specific implementation methods one to four.

[0143] Specific Implementation Method Six: This implementation method differs from Specific Implementation Methods One to Five in that, in step seven, the carrier velocity in the carrier coordinate system b calculated by SINS is... and measurement matrix The specific process is as follows:

[0144] Step 71: Calculate the carrier velocity in the carrier coordinate system b of the SINS solution.

[0145]

[0146] in, It is the k calculated by SINS (Inertial Navigation System). R Attitude transition matrix at time step, attitude transformation matrix transpose, k calculated for SINS R The velocity of the vehicle in the navigation coordinate system n at any given moment. for In the x-axis direction component of the navigation coordinate system n for In the y-axis direction component of the navigation coordinate system n for The z-axis 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 one of the specific implementation methods one to five.

[0152] It should be noted that, theoretically, the attitude transformation matrix and the carrier velocity in step 72 should be calculated using the true values. However, since the true values ​​are unavailable, the values ​​calculated by SINS are directly used here for the calculation.

[0153] Specific Implementation Method Seven: This implementation method differs from Specific Implementation Methods One through Six in that, in step eight, the state recursion matrix F′ at time k is calculated. k and measurement noise matrix R′ k The specific process is as follows:

[0154] Step 81: Calculate the state recurrence matrix F′ at time k. k :

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

[0156] Among them, F′ k-1 It is the state recurrence matrix at time k-1. If the current time k = k R +1, then F′ k-1 Substitute the initial values ​​of the state recurrence 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 This represents the measurement noise matrix at time k-1, if the current time k = k R +1, then R′ k-1 Substitute the initial values ​​of the measurement noise matrix

[0160] The other steps and parameters are the same as those in one of the specific implementation methods one to six.

[0161] Specific Implementation Method Eight: This implementation method differs from Specific Implementation Methods One to Seven in that the calculation method for the current state quantity time update result is as follows:

[0162]

[0163] in, x k-1 The state variable at time k-1 φ k-1 The SINS attitude calculation error at time k-1 is... Represents φ k-1 In the x-coordinate of the carrier coordinate system b b Axial component, Represents φ k-1 In the carrier coordinate system b, y b Axial component, Represents φ k-1 In the carrier coordinate system b, z b Axial component, The velocity calculation error at time k-1 is... express In the x-axis direction component of the navigation coordinate system n express In the y-axis direction component of the navigation coordinate system n express In the z-axis direction component of the navigation coordinate system n δp k-1 The position calculation error at time k-1, It is the latitude error at time k-1, δλ k-1 It is the longitude error at time k-1, δh k-1 It is the height error at time k-1. The gyroscope has zero bias at time k-1. express In the x-coordinate of the carrier coordinate system b b Axial component, express In the carrier coordinate system b, y b Axial component, express In the carrier coordinate system b, z b Axial component, For the table drift at time k-1, express In the x-coordinate of the carrier coordinate system b b Axial component, express In the carrier coordinate system b, y b Axial component, express In the carrier coordinate system b, z b Axial component, x k,k-1 For the time update result of the state quantity at time k, F k,k-1 Let P be the state transition matrix. k-1 Let τ be the error covariance matrix at time k-1. kLet Q be the system noise assignment matrix at time k. k Let P be the system noise covariance matrix at time k. k,k-1 The prior estimate of the error covariance matrix at time k is given by the superscript T, which represents the transpose of the matrix.

[0164] The other steps and parameters are the same as those in any of the specific implementation methods one to seven.

[0165] When k < k D When, it is the x calculated at the current moment. k,k-1 As x in the next moment k-1 They come to participate in the calculation.

[0166] Specific Implementation Method Nine: This implementation method differs from Specific Implementation Methods One to Eight in that, in step nine, according to... and the Doppler velocity information output by DVL Calculation measurement Then according to State recurrence matrix Measurement noise matrix State recurrence matrix and measurement noise matrix The current state quantity time update result is processed to obtain the optimal estimated state quantity and covariance matrix at the current time; the specific process is as follows:

[0167] Step 91: Calculation and Measurement

[0168]

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

[0170] Step 92: Calculate the measurement matrix

[0171]

[0172] Step 93: Calculate the measurement noise matrix

[0173]

[0174] Among them, R DVL It is a measurement noise matrix determined based on the variance characteristics of DVL speed measurement noise, and it is a constant matrix;

[0175] Step 94: Adjust the filter gain Update:

[0176]

[0177] Step 95: Update the state variables to obtain the optimally estimated state variables:

[0178]

[0179] Where, x k The state variables are the optimal estimates at time k;

[0180] Step 96: Update the covariance matrix:

[0181]

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

[0183] The other steps and parameters are the same as those in one of the specific implementation methods one to eight.

[0184] x k and P k Used for updating the state variables at the next time step.

[0185] The above examples of the present invention are merely illustrative of the computational model and process of the present invention, and are not intended to limit the implementation of the present invention. Those skilled in the art will recognize that other variations or modifications can be made based on the above description. It is impossible to exhaustively list all possible implementations here. Any obvious variations or modifications derived from the technical solutions of the present invention are still within the scope of protection of the present invention.

Claims

1. A method for integrated navigation assisted by acoustic Doppler delay information, characterized in that, The method specifically includes the following steps: Step 1: Establish the vehicle coordinate system b and the navigation coordinate system n, obtain the initial values ​​of the vehicle's attitude, velocity, and position, and define the initial values ​​of the state covariance matrix P0, the system noise covariance matrix Q1, and the initial values ​​of the state recursion matrix. Initial value of the state recurrence matrix Initial values ​​of the measurement noise matrix and initial values ​​of the measurement noise matrix Wherein, the initial value of the state recursion matrix I 15×15 It is a 15×15 dimensional identity matrix, and the initial value of the measurement noise matrix is... 0 3×3 It is a 3×3 dimensional zero matrix; And initialize time k = 0; Step 2: Determine whether the DVL triggers a synchronization signal at time k: If the DVL triggers a synchronization signal at time k, then update the transmission time k of the DVL speed measurement signal. T and receiving time k R Then, proceed to step four; If DVL does not trigger the synchronization signal at time k, then proceed to step three; Step 3: Solve the SINS data and calculate the time update result of the current state quantity, then let k = k + 1, and return to execute Step 2; 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 proceed to step five; If k T <k<k R Then proceed to step six; If k = k R Then proceed to step seven; If k R <k<k D Then proceed to step eight; If k = k D Then proceed to step nine; Step 5: Solve the SINS data and calculate the time update result of the current state quantity. Calculate the carrier velocity in the carrier coordinate system b calculated by SINS. and measurement matrix Then set k = k + 1 and return to step four; Step 6: Solve the SINS data and calculate the time update result of the state quantity at the current time, and calculate the state recursion matrix F at time k. k and measurement noise matrix R k Then set k = k + 1 and return to step four; Step 7: Solve the SINS data and calculate the time update result of the current state quantity. Calculate the carrier velocity in the carrier coordinate system b calculated by SINS. and measurement matrix Then set k = k + 1 and return to step four; Step 8: Solve the SINS data and calculate the time update result of the state quantity at the current time, and calculate the state recursion matrix F at time k. k Measurement noise matrix R k And the state recurrence matrix F at time k k Measurement noise matrix R′ k Then set k = k + 1 and return to step four; Step 9: Solve the SINS data and calculate the time update result of the current state quantity, and calculate k. D The state recurrence matrix at each time step Measurement noise matrix State recurrence matrix Measurement noise matrix Then according to and the Doppler velocity information output by DVL Calculation measurement Then according to State recurrence matrix Measurement noise matrix State recurrence matrix and measurement noise matrix The current state quantity time update result is processed to obtain the optimal estimated state quantity and covariance matrix at the current time. The optimal estimated state quantity is used to perform feedback correction on the navigation solution value, and then step ten is executed. Step 10: Let k = k + 1, then 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 specifically involves: Carrier coordinate system b: Taking the platform's center of gravity as the origin of the carrier coordinate system b. b The direction along the platform's cross-section pointing towards the bow of the platform is taken as y. b The positive direction of the axis is defined as the direction perpendicular to the deck and upwards. b Positive direction of the axis, x b axis, y b axis and z b The axes form a right-handed coordinate system; Navigation coordinate system n: The platform's center of gravity is taken as the origin o of the navigation coordinate system n. The geographical east direction is taken as the positive x-axis, the geographical north direction as the positive y-axis, and the positive z-axis is perpendicular to the xoy plane and points towards the sky. That is, the x-axis, y-axis, and z-axis form a right-handed coordinate system.

3. The acoustic Doppler delay information-assisted integrated navigation method according to claim 2, characterized in that, The transmission time k of the DVL speed measurement signal T and receiving time k R The update method is as follows: The DVL speed measurement signal transmission time k T Update the time to time k, and then update the receiving time k of the DVL speed measurement signal based on 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 tilt angle, c is the speed of sound, and T is the water depth. SINS This refers to the replacement cycle of inertial devices.

4. The acoustic Doppler delay information-assisted integrated navigation method according to claim 3, characterized in that, In step five, the carrier velocity in the carrier coordinate system b calculated by SINS is determined. and measurement matrix The specific process is as follows: Step 51: Calculate the carrier velocity in the carrier coordinate system b calculated by SINS. in, k is calculated by SINS. T Attitude transition matrix at time step, attitude transformation matrix transpose, k calculated for SINS T The velocity of the vehicle in the navigation coordinate system n at any given moment. for In the x-axis direction component of the navigation coordinate system n for In the y-axis direction component of the navigation coordinate system n for The z-axis component in the navigation coordinate system n; Step 52: Calculate the measurement matrix in, × indicates 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 six, the state recursion matrix F at time k is calculated. k and measurement noise matrix R k The specific process is as follows: Step 61: Calculate the state recurrence matrix F at time k. k : F k =F k,k-1 F k-1 Among them, F k,k-1 Let F be the state transition matrix at time k. k-1 It is the state recurrence matrix at time k-1; Step 6.2: Calculate the measurement noise matrix R at the current time k. k : Where, τ k Assign a noise matrix to the system at time k. F represents k The inverse matrix, R k-1 This 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 seven, the carrier velocity in the carrier coordinate system b calculated by SINS is determined. and measurement matrix The specific process is as follows: Step 71: Calculate the carrier velocity in the carrier coordinate system b of the SINS solution. in, k is calculated by SINS. R Attitude transition matrix at time step, attitude transformation matrix transpose, k calculated for SINS R The velocity of the vehicle in the navigation coordinate system n at any given moment. for In the x-axis direction component of the navigation coordinate system n for In the y-axis direction component of the navigation coordinate system n for The z-axis component in the navigation coordinate system n; Step 72: Calculate the measurement matrix in, × indicates 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 recursion matrix F at time k is calculated. k and measurement noise matrix R′ k The specific process is as follows: Step 81: Calculate the state recurrence matrix F at time k. k ′: F k ′=F k,k-1 F k ′ -1 Among them, F k ′ -1 It is the state recurrence matrix at time k-1; Step 82: Calculate the measurement noise matrix R′ at time k. k : in, F represents k The inverse matrix of ', R' k-1 This 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 for the current state quantity time update result is as follows: in, x k-1 The state variable at time k-1 φ k-1 The SINS attitude calculation error at time k-1 is... φ k-1 In the x-coordinate of the carrier coordinate system b b Axial component, φ k-1 In the carrier coordinate system b, y b Axial component, φ k-1 In the carrier coordinate system b, z b Axial component, The velocity calculation error at time k-1 is... express In the x-axis direction component of the navigation coordinate system n express In the y-axis direction component of the navigation coordinate system n express In the z-axis direction component of the navigation coordinate system n δp k-1 The position calculation error at time k-1, It is the latitude error at time k-1, δλ k-1 It is the longitude error at time k-1, δh k-1 It is the height error at time k-1. The gyroscope has zero bias at time k-1. express In the x-coordinate of the carrier coordinate system b b Axial component, express In the carrier coordinate system b, y b Axial component, express In the carrier coordinate system b, z b Axial component, For the table drift at time k-1, express In the x-coordinate of the carrier coordinate system b b Axial component, express In the carrier coordinate system b, y b Axial component, express In the carrier coordinate system b, z b Axial component, x k,k-1 For the time update result of the state quantity at time k, F k,k-1 Let P be the state transition matrix. k-1 Let τ be the error covariance matrix at time k-1. k Let Q be the system noise assignment matrix at time k. k Let P be the system noise covariance matrix at time k. k,k-1 The prior estimate of the error covariance matrix at time k is given by the superscript T, which represents the transpose of the matrix.

9. The acoustic Doppler delay information-assisted integrated navigation method according to claim 8, characterized in that, In step nine, according to and the Doppler velocity information output by DVL Calculation measurement Then according to State recurrence matrix Measurement noise matrix State recurrence matrix and measurement noise matrix The current state variables are processed to obtain the optimally estimated state variables and covariance matrix at the current time. The specific process is as follows: Step 91: Calculation and Measurement in, This 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 a measurement noise matrix determined based on the variance characteristics of DVL speed measurement noise; Step 94: Adjust the filter gain Update: Step 95: Update the state variables to obtain the optimally estimated state variables: Where, x k The state variables are the optimal estimates at time k; Step 96: Update the covariance matrix: Among them, I 15×15 For a 15×15 dimensional identity matrix, P k Let be the error covariance matrix after the optimal estimation at time k.

Images