A SINS / USBL / DVL integrated navigation method based on factor graph optimization

Abstract

The present invention proposes a SINS / USBL / DVL combined navigation method based on factor graph optimization. Firstly, the sliding window length is determined, the SINS factor in the sliding window is constructed, and the factor graph model is established based on the DVL factor of the four-beam velocity and the USBL factor based on the slant range and azimuth; secondly, the chi-square fault detection algorithm is used to perform data fault discrimination on all historical factors in the sliding window except the current moment, and the gross errors of USBL and DVL in the sliding window are accurately identified and eliminated; finally, the global loss function is established using the good USBL and DVL historical observation data in the sliding window and the measurement information at the current moment, and all state information in the sliding window is estimated using the least square method.

Description

Technical Field

[0001] The invention belongs to the SINS / USBL / DVL combined navigation technology of underwater vehicles, in particular to a SINS / USBL / DVL combined navigation method based on factor graph optimization. Background Art

[0002] Traditional combined navigation algorithms have always been based on the Kalman algorithm. The graph optimization algorithm has been widely recognized for its excellent performance in visual positioning and is being extended to traditional combined navigation. Traditional combined navigation based on Kalman filtering adopts the Markov assumption, assuming that the state at the current moment is only related to the state at the previous moment, and requires that the system noise and observation noise are white noise that conforms to the normal distribution. Due to the complexity of the underwater environment, traditional filtering algorithms often have certain defects. The graph optimization algorithm achieves the optimal state estimation by sacrificing a certain amount of computational efficiency for repeated linearization processing. With the development of visual SLAM, graph optimization algorithms have been widely adopted and rapidly evolved, and have gained more and more widespread recognition, which has opened up a new idea for the research of combined navigation algorithms. Summary of the invention

[0003] In order to solve the above problems, the present invention proposes a SINS / USBL / DVL combined navigation method based on factor graph optimization. First, the sliding window length is determined, the SINS factor in the sliding window is constructed, and the factor graph model is established based on the DVL factor based on the four-beam velocity and the USBL factor based on the slant range and azimuth; secondly, the chi-square fault detection algorithm is used to identify data faults for all historical factors in the sliding window except the current moment, and the gross errors of USBL and DVL in the sliding window are accurately identified and eliminated; finally, the global loss function is established using the good USBL and DVL historical observation data in the sliding window and the measurement information at the current moment, and all state information in the sliding window is estimated using the least squares.

[0004] To achieve the above object, the technical solution adopted by the present invention is:

[0005] A SINS / USBL / DVL integrated navigation method based on factor graph optimization is characterized by the following specific steps:

[0006] Step 1: Receive SINS accelerometer and gyroscope data, USBL slant range and azimuth data, DVL four-beam velocity data: X, Y, and Z axis angular velocity information collected by SINS and acceleration information Slope distance of USBL and azimuth DVL four-beam velocity information

[0007] Step 2: Establish the SINS factor, USBL slant range and azimuth tight combination factor and DVL four-beam tight combination factor based on the data received in step 1;

[0008] Step 3: According to the measurement type in the sliding window, combined with the chi-square fault diagnosis algorithm, reconstruct the state matrix and measurement information matrix.

[0009] As a further improvement of the present invention, step 2 is specifically as follows:

[0010] At 2.1k, the integrated navigation system equation is as follows:

[0011]

[0012] in,

[0013] φ n =[φ x φ y φ z ] T Indicates the pitch angle, roll angle, and heading angle errors of the carrier; δV = [δV E δV N δV N ] T Indicates the carrier's eastward, northward, and celestial velocity errors; δP = [δL δλ δh] T Indicates the carrier's geographic longitude, latitude, and altitude errors; Indicates the acceleration zero bias of SINS; [ε x ε y ε z ] T Indicates the gyro bias of SINS, F k-1 represents the state transfer matrix, W k represents the system noise, Z k Represents the system measurement value, H k represents the measurement equation transfer matrix, V k represents the measurement noise;

[0014] According to the system equations, construct the SINS factor:

[0015]

[0016] in, represents a one-step forecast of the state;

[0017] Accordingly, its one-step prediction mean square error is:

[0018]

[0019] Among them, pk-1 represents the mean square error of the previous moment, Q k-1 represents the system noise distribution;

[0020] 2.2 When the USBL slant range and azimuth information is received, the USBL tight combination factor is constructed based on the SINS calculated position and the calibrated transponder position:

[0021] The measurement information received by USBL from transponder A is:

[0022]

[0023] The relative position vector of the transponder in the a system is obtained by using the SINS position calculation: Then the relative position vector of transponder A is The relationship between azimuth and slant range is expressed as:

[0024]

[0025] but:

[0026]

[0027] Among them, vector For the convenience of expression, Then the matrix The expression is:

[0028]

[0029] The relative position vector of the transponder calculated based on the position of the SINS is:

[0030]

[0031] in, represents the transfer matrix corresponding to the installation error angle between SINS and USBL, represents the SINS attitude transfer matrix, The coordinate transformation matrix from the earth coordinate system to the navigation system, is the relative position vector in the earth coordinate system, expressed as:

[0032]

[0033] The slant range azimuth information calculated using the SINS position and the transponder A position is expressed as:

[0034]

[0035] Where [α β R] Tis the true value of azimuth and slant range, and the matrices are

[0036] in,

[0037]

[0038] The system measurement is:

[0039]

[0040] Among them, H u =[0 0 -1], δU=[δα δβ δD] T ;

[0041] Then the USBL tight combination factor at time k is:

[0042] r usbl，k (Z k , X k )=Z usbl，k -H usbl , k X k ;

[0043] 2.3 When the DVL four-beam velocity information is received, the DVL tight combination factor is constructed according to the SINS solution velocity:

[0044] DVL Quad Beam The model is:

[0045]

[0046] Among them, δK D represents the beam channel velocity measurement scale factor error; b D Indicates the zero bias of beam velocity measurement; w D represents the beam velocity measurement noise;

[0047] The projection of the velocity obtained by SINS solution in the DVL system is:

[0048]

[0049] The system measurement is:

[0050]

[0051] in,

[0052] Then the DVL tight combination factor at time k is:

[0053] r dvl，k (Z dvl，k , X k)=Z dvl，k -H dvk，k X k .

[0054] As a further improvement of the present invention, step 3 is specifically as follows:

[0055] 3.1 According to the SINS data in the sliding window, update the i-th state transition probability matrix in the sliding window:

[0056]

[0057] 3.2 Using the chi-square test, the historical measurement information (Z 1 ~Z k-1 Time) to perform selection and calculate the i-th measurement chi-square test value in the sliding window:

[0058]

[0059] in, R i-1 represents the measurement noise matrix.

[0060] If i <T, the measurement is considered normal and does not contain outliers. i ≥T, the measurement is considered to contain outliers.

[0061] 3.3 According to the selection result of step 3.2, combined with the measurement value at the current k moment, reconstruct the state matrix and measurement matrix in the sliding window:

[0062]

[0063]

[0064]

[0065]

[0066] in, Indicates the state prediction information of the rest of the chi-square detections without faults in the sliding window except the current moment, represents the state prediction information at the current moment, p 1：k-1 represents the state prediction probability matrix of the rest of the chi-square detection without fault in the sliding window except the current moment, p k|k-1 Represents the state probability matrix at the current moment, Z 1：k-1 represents the measurement information of the rest of the card-square detection without faults in the sliding window except the current moment, Z k Indicates the measurement information at the current moment, H 1：k-1 represents the measurement transfer matrix of the rest of the chi-square detection fault-free in the sliding window except the current moment, Hk Represents the measurement transfer matrix at the current moment;

[0067] 3.4 According to the factor graph model in step 2, establish the system cost function:

[0068]

[0069] Among them, ∑ pi|i-1 represents the one-step prediction mean square error of the i-th state quantity, R usbl represents the USBL measurement noise matrix, R dvl represents the DVL measurement noise matrix;

[0070] 3.5 Perform the least squares estimation on the cost function in step 3.4 and update the state estimation result X corresponding to all measurements in the sliding window:

[0071] (1) Calculate system gain:

[0072] K=PH T (HPH+R) -1

[0073] (2) Estimate all states in the sliding window:

[0074]

[0075] (3) Update the state transition probability matrix:

[0076] P L =P-KHP

[0077] (4) Feedback all navigation information within the sliding window based on the state estimation results of (2).

[0078] The present invention proposes a SINS / USBL / DVL combined navigation method based on factor graph optimization. Firstly, the sliding window length is determined, the SINS factor in the sliding window is constructed, and the factor graph model is established based on the DVL factor of the four-beam velocity and the USBL factor based on the slant range and azimuth; secondly, the chi-square fault detection algorithm is used to perform data fault discrimination on all historical factors in the sliding window except the current moment, and the gross errors of USBL and DVL in the sliding window are accurately identified and eliminated; finally, the global loss function is established using the good USBL and DVL historical observation data in the sliding window and the measurement information at the current moment, and all state information in the sliding window is estimated using the least square method. BRIEF DESCRIPTION OF THE DRAWINGS

[0079] Figure 1 , overall system flow chart;

[0080] Figure 2 , system factor graph model; DETAILED DESCRIPTION

[0081] The present invention is further described in detail below in conjunction with the accompanying drawings and specific embodiments:

[0082] The present invention proposes a SINS / USBL / DVL combined navigation method based on factor graph optimization. Firstly, the sliding window length is determined, the SINS factor in the sliding window is constructed, and the factor graph model is established based on the DVL factor of the four-beam velocity and the USBL factor based on the slant range and azimuth; secondly, the chi-square fault detection algorithm is used to perform data fault discrimination on all historical factors in the sliding window except the current moment, and the gross errors of USBL and DVL in the sliding window are accurately identified and eliminated; finally, the global loss function is established using the good USBL and DVL historical observation data in the sliding window and the measurement information at the current moment, and all state information in the sliding window is estimated using the least square method.

[0083] The overall flow chart of the system is as follows: Figure 1 shown.

[0084] System factor graph model such as Figure 2 shown.

[0085] A SINS / USBL / DVL integrated navigation method based on factor graph optimization, comprising:

[0086] Step 1: Receive SINS accelerometer and gyroscope data, USBL slant range and azimuth data, DVL four-beam velocity data: X, Y, and Z axis angular velocity information collected by SINS and acceleration information Slope distance of USBL and azimuth DVL four-beam velocity information

[0087] Step 2: Establish SINS factors, USBL slant range and azimuth tight combination factors, and DVL four-beam tight combination factors based on the data received in step 1, including:

[0088] At 2.1k, the integrated navigation system equation is as follows:

[0089]

[0090] in, φ n =[φ x φ y φ z ] T Indicates the pitch angle, roll angle, and heading angle errors of the carrier; δV = [δV E δV NδV N ] T Indicates the carrier's eastward, northward, and celestial velocity errors; δP = [δL δλ δh] T Indicates the carrier's geographic longitude, latitude, and altitude errors; Indicates the acceleration zero bias of SINS; [ε x ε y ε z ] T Indicates the gyro bias of SINS. k-1 represents the state transfer matrix, W k represents the system noise, Z k Represents the system measurement value, H k represents the measurement equation transfer matrix, V k Represents the measurement noise.

[0091] According to the system equations, construct the SINS factor:

[0092]

[0093] in, Represents the one-step forecast of the state.

[0094] Accordingly, its one-step prediction mean square error is:

[0095]

[0096] Among them, p k-1 represents the mean square error of the previous moment, Q k-1 Represents the system noise distribution.

[0097] 2.2 When the USBL slant range and azimuth information is received, the USBL tight combination factor is constructed based on the SINS calculated position and the calibrated transponder position:

[0098] The measurement information received by USBL from transponder A is:

[0099]

[0100] The relative position vector of the transponder in the a system is obtained by using the SINS position calculation: Then the relative position vector of transponder A is The relationship between azimuth and slant range is expressed as:

[0101]

[0102] but:

[0103]

[0104] Among them, vector For the convenience of expression, Then the matrix The expression is:

[0105]

[0106] The relative position vector of the transponder calculated based on the position of the SINS is:

[0107]

[0108] in, represents the transfer matrix corresponding to the installation error angle between SINS and USBL, represents the SINS attitude transfer matrix, The coordinate transformation matrix from the earth coordinate system to the navigation system, is the relative position vector in the earth coordinate system, expressed as:

[0109]

[0110] The slant range azimuth information calculated using the SINS position and the transponder A position is expressed as:

[0111]

[0112] Where [α β R] T is the true value of azimuth and slant range, and the matrices are

[0113] in,

[0114]

[0115] The system measurement is:

[0116]

[0117] Among them, H u =[0 0 -1], δU=[δα δβ δD] T .

[0118] Then the USBL tight combination factor at time k is:

[0119] r usbl，k (Z k , X k )=Z usbl，k -H usbl，k X k

[0120] 2.3 When the DVL four-beam velocity information is received, the DVL tight combination factor is constructed according to the SINS solution velocity:

[0121] DVL Quad Beam The model is:

[0122]

[0123] Among them, δK D represents the beam channel velocity measurement scale factor error; b D Indicates the zero bias of beam velocity measurement; w D Represents the beam velocity measurement noise.

[0124] The projection of the velocity obtained by SINS solution in the DVL system is:

[0125]

[0126] The system measurement is:

[0127]

[0128] in,

[0129] Then the DVL tight combination factor at time k is:

[0130] r dvl，k (Z dvl，k , X k )=Z dvl，k -H dvk，k X k

[0131] Step 3: According to the measurement type in the sliding window, combined with the chi-square fault diagnosis algorithm, reconstruct the state matrix and measurement information matrix, mainly including:

[0132] 3.1 According to the SINS data in the sliding window, update the i-th state transition probability matrix in the sliding window:

[0133]

[0134] 3.2 Using the chi-square test, the historical measurement information (Z 1 ~Z k-1 Time) to perform selection and calculate the i-th measurement chi-square test value in the sliding window:

[0135]

[0136] in, R i-1 represents the measurement noise matrix.

[0137] Ifi <T, the measurement is considered normal and does not contain outliers. i ≥T, the measurement is considered to contain outliers.

[0138] 3.3 According to the selection result of step 3.2, combined with the measurement value at the current k moment, reconstruct the state matrix and measurement matrix in the sliding window:

[0139]

[0140]

[0141]

[0142]

[0143] in, Indicates the state prediction information of the rest of the chi-square detections without faults in the sliding window except the current moment, represents the state prediction information at the current moment, p 1：k-1 represents the state prediction probability matrix of the rest of the chi-square detection without fault in the sliding window except the current moment, p k|k-1 Represents the state probability matrix at the current moment, Z 1：k-1 represents the measurement information of the rest of the card-square detection without faults in the sliding window except the current moment, Z k Indicates the measurement information at the current moment, H 1：k-1 represents the measurement transfer matrix of the rest of the chi-square detection fault-free in the sliding window except the current moment, H k Represents the measurement transfer matrix at the current moment.

[0144] 3.4 According to the factor graph model in step 2, establish the system cost function:

[0145]

[0146] Among them, ∑p i|i-1 represents the one-step prediction mean square error of the i-th state quantity, R usbl represents the USBL measurement noise matrix, R dvl represents the DVL measurement noise matrix.

[0147] 3.5 Perform the least squares estimation on the cost function in step 3.4 and update the state estimation result X corresponding to all measurements in the sliding window:

[0148] (1) Calculate system gain:

[0149] K=PH T (HPH+R) -1

[0150] (2) Estimate all states in the sliding window:

[0151]

[0152] (3) Update the state transition probability matrix:

[0153] P L =P-KHP

[0154] (4) Feedback all navigation information within the sliding window based on the state estimation results of (2).

[0155] The above description is only a preferred embodiment of the present invention and does not constitute any other form of limitation to the present invention. Any modification or equivalent change made based on the technical essence of the present invention still falls within the scope of protection required by the present invention.

Claims

1. A SINS / USBL / DVL integrated navigation method based on factor graph optimization, characterized in that: The specific steps are as follows: Step 1: Receive SINS accelerometer and gyroscope data, USBL slant range and azimuth data, DVL four-beam velocity data: X, Y, and Z axis angular velocity information collected by SINS and acceleration information Slope distance of USBL and azimuth DVL four-beam velocity information Step 2: Establish the SINS factor, USBL slant range and azimuth tight combination factor and DVL four-beam tight combination factor according to the data received in step 1; Step 3: Reconstruct the state matrix and measurement information matrix based on the measurement type in the sliding window and the chi-square fault diagnosis algorithm; Step 3 is as follows: 3.1 According to the SINS data in the sliding window, update the i-th state transition probability matrix in the sliding window: 3.2 Using the chi-square test, the historical measurement information Z1~Z in the sliding window k-1 Delete at every moment and calculate the i-th measurement chi-square test value in the sliding window: in, represents the measurement noise matrix; If i <T, the measurement is considered normal and does not contain outliers. If λ i ≥T, the measurement is considered to contain outliers; 3.3 According to the selection result of step 3.2, combined with the measurement value at the current k moment, reconstruct the state matrix and measurement matrix in the sliding window: in, Indicates the state prediction information of the rest of the chi-square detections without faults in the sliding window except the current moment, represents the state prediction information at the current moment, p 1:k-1 represents the state prediction probability matrix of the rest of the chi-square detection without fault in the sliding window except the current moment, p k|k-1 Represents the state probability matrix at the current moment, Z 1:k-1 represents the measurement information of the rest of the card-square detection without faults in the sliding window except the current moment, Z k Indicates the measurement information at the current moment, H 1:k-1 represents the measurement transfer matrix of the rest of the chi-square detection fault-free in the sliding window except the current moment, H k Represents the measurement transfer matrix at the current moment; 3.4 According to the factor graph model in step 2, establish the system cost function: in, ∑p i|i-1 represents the one-step prediction mean square error of the i-th state quantity, R usbl represents the USBL measurement noise matrix, R dvl represents the DVL measurement noise matrix; 3.5 Perform the least squares estimation on the cost function in step 3.4 and update the state estimation result X corresponding to all measurements in the sliding window: (1) Calculate system gain: K=PH T (HPH+R) -1 (2) Estimate all states in the sliding window: (3) Update the state transition probability matrix: P L =P-KHP (4) Feedback all navigation information within the sliding window based on the state estimation results of (2).

2. A SINS / USBL / DVL integrated navigation method based on factor graph optimization according to claim 1, characterized in that: Step 2 is as follows: At 2.1k, the integrated navigation system equation is as follows: in, φ n =[φ x φ y φz] T Indicates the pitch angle, roll angle, and heading angle errors of the carrier; δV = [δV E δV N δV U ] T Indicates the carrier's eastward, northward, and celestial velocity errors; δP = [δL δλ δh] T Indicates the carrier's geographic longitude, latitude, and altitude errors; Indicates the acceleration zero bias of SINS; [ε x ε y ε z ] T Indicates the gyro bias of SINS, F k-1 represents the state transfer matrix, W k represents the system noise, Z k Represents the system measurement value, H k represents the measurement equation transfer matrix, V k represents the measurement noise; According to the system equations, construct the SINS factor: in, represents a one-step forecast of the state; Accordingly, its one-step prediction mean square error is: Among them, p k-1 represents the mean square error of the previous moment, Q k-1 represents the system noise distribution; 2.2 When the USBL slant range and azimuth information is received, the USBL tight combination factor is constructed based on the SINS calculated position and the calibrated transponder position: The measurement information received by USBL from the transponder is: The relative position vector of the transponder in the a system is obtained by SINS position calculation: Then the relative position vector of the transponder is The relationship between azimuth and slant range is expressed as: but: Among them, vector For the convenience of expression, Then the matrix The expression is: The relative position vector of the transponder calculated based on the position of the SINS is: in, represents the transfer matrix corresponding to the installation error angle between SINS and USBL, represents the SINS attitude transfer matrix, The coordinate transformation matrix from the earth coordinate system to the navigation system, is the relative position vector in the earth coordinate system, expressed as: The slant range azimuth information calculated using the SINS position and the transponder position is expressed as: Where [α β R] T is the true value of azimuth and slant range, and the matrices are in, The system measurement is: Among them, H u =[0 0 -1],δU=[δα δβ δD] T ; Then the USBL tight combination factor at time k is: r usbl,k (Z k ,X k )=Z usbl,k -H usbl,k X k ; 2.3 When receiving the DVL four-beam velocity information, the DVL tight combination factor is constructed according to the SINS solution velocity: DVL Quad Beam The model is: Among them, δK D represents the beam channel velocity measurement scale factor error; b D Indicates the zero bias of beam velocity measurement; w D represents the beam velocity measurement noise; The projection of the velocity obtained by SINS solution in the DVL system is: The system measurement is: in, Then the DVL tight combination factor at time k is: r dvl,k (Z dvl,k ,X k )=Z dvl,k -H dvk,k X k 。

Images