A SINS / DVL cooperative navigation method based on distance pattern

Abstract

The application provides a SINS / DVL cooperative navigation method based on a distance mode, and belongs to the cooperative navigation technology of multiple underwater vehicles.The method comprises the following steps: 1) obtaining the sensor data of SINS and DVL of an AUV at the current time; 2) obtaining the distance measurement information of the current time and the position information of a master AUV by a distance measurement module; 3) establishing factors of cooperative navigation according to a factor graph model, wherein the factors comprise SINS factors, DVL four-beam factors and distance factors; 4) performing selection on the factors at the historical time by chi-square detection; and 5) establishing an optimal estimation model of the factor graph, performing global estimation on the state of the AUV at the current time, and outputting the optimized result after multi-source information fusion.Under the premise that the SINS / DVL integrated navigation system is carried on the self-integrated navigation system, the application provides a method for completing navigation by using the position information of the master AUV and the distance information of the master AUV, and can realize global estimation of multi-source information by using the plug-and-play characteristics of the factor graph.

Description

Technical Field

[0001] This invention relates to cooperative navigation technology for multiple underwater vehicles, specifically a distance-mode-based SINS / DVL cooperative navigation method. Background Technology

[0002] With the increasing demand for cross-domain collaboration and formation operations in underwater navigation, acoustic ranging-based cooperative navigation technology can improve navigation and positioning accuracy, enhance the complexity of underwater vehicle missions, and increase tolerance to individual vehicle failures without adding its own navigation and positioning sensors. This can be achieved by using a body perception sensor to perform dead reckoning and simultaneously fusing relative position measurement information from underwater acoustic communications of other team members. Summary of the Invention

[0003] This invention proposes a distance-pattern-based SINS / DVL cooperative navigation method. Building upon the SINS / DVL integrated navigation system within its own integrated navigation system, it proposes a method to complete navigation using the position information of the primary AUV and its distance information from the primary AUV. Furthermore, to achieve the fusion of different frequency sensors, the "plug-and-play" nature of factor maps is utilized to achieve global estimation of multi-source information.

[0004] A distance-pattern-based SINS / DVL cooperative navigation method includes:

[0005] Step 1: Obtain the current SINS and DVL sensor data from the AUV;

[0006] The AUV uses its own SINS and DVL navigation sensors to acquire the current values ​​from the SINS accelerometer and gyroscope, updates the inertial navigation system, acquires the DVL four-beam velocity information, and establishes a DVL compact combination model.

[0007] Calculate the velocity in the DVL coordinate system based on the current velocity of SINS:

[0008]

[0009]

[0010] in, SINS indicates that the speed information is displayed in the navigation coordinate system. φ represents the speed error of SINS. n This represents the attitude error of SINS. This represents the attitude transition matrix between the navigation coordinate system and the vehicle coordinate system. This represents the transition matrix between the load cell and the DVL coordinate system due to installation errors. The transfer matrix between the DVL coordinate system and the four-beam coordinate system is typically calculated as follows:

[0011]

[0012] Where α represents the beam angle of DVL.

[0013] The four-beam velocity model of DVL can be expressed as:

[0014]

[0015] Where δK represents the scale factor, w DVL This represents random error.

[0016] Establish a SINS / DVL compact combination model:

[0017]

[0018] Step 2: Obtain the location information of the main AUV and the distance information from the main AUV from the AUV, and establish a distance model:

[0019] Assumption The relative position vector is calculated from the SINS, which provides the relative position information of the slave AUV and the master AUV. The relative position vector is calculated as follows:

[0020]

[0021] Ignoring a small amount of second-order error, the ranging error is:

[0022]

[0023] in This indicates the relative position error.

[0024] Establish a distance model based on the location information of the main AUV and the slave AUV:

[0025]

[0026] in, This indicates the location information of the main AUV. Represents the location information from the AUV, R Nh =R N +h, R Mh =R M +h:

[0027] R e denoted by , and e represents the Earth's semi-major axis radius.

[0028] Will Switch to the ranging coordinate system:

[0029]

[0030] in, The rotation matrix between the vehicle coordinate system and the ranging coordinate system is represented. This indicates the lever arm error between the SINS and the distance sensor. The rotation matrix between the geographic coordinate system and the navigation coordinate system, which is related to the current location:

[0031]

[0032] The error in calculating the rotation matrix caused by the position error is:

[0033]

[0034] calculate

[0035]

[0036] in,

[0037]

[0038] Considering installation errors and lever arm errors:

[0039]

[0040] Ignore higher-order small errors:

[0041]

[0042] in, Simplified to:

[0043]

[0044] but for:

[0045]

[0046] Based on SINS and the distance model, the distance error model is as follows:

[0047]

[0048] in,

[0049] Step 3: Establish the factors for collaborative navigation based on the factor graph model:

[0050] Establish SINS factors:

[0051] r SINS,k =r(X) k|k-1 ,X k ) = F k|k-1 X k-1 -X k

[0052] Among them, F k|k-1 Represents the state transition matrix. φ n ,δV n ,δP,ε, These are attitude error, velocity error, position error, gyro constant drift, and accelerometer bias.

[0053] Establish the DVL compact combination factor:

[0054]

[0055] in,

[0056] Establish distance factor:

[0057]

[0058] Among them, H dis =[H1 0 3×3 H2 0 3×6 ]

[0059] Step 4: Construct a factor graph global optimization function based on the factors from Step 3:

[0060]

[0061] Among them, P m|m-1 is represents the prior covariance at time m, R DVL,m is the noise model of DVL at time m, R dis,m It is the noise model at time m.

[0062] Step 5: Optimize the cost function from Step 4 and output the navigation results.

[0063] The beneficial effects of this invention are as follows:

[0064] 1. Based on the SINS / DVL integrated navigation system built into the self-integrated navigation system, a method is proposed to complete navigation by utilizing the position information of the main AUV and the distance information to the main AUV.

[0065] 2. To achieve the fusion of sensors of different frequencies, the "plug-and-play" nature of factor graphs is utilized to realize global estimation of multi-source information. Attached Figure Description

[0066] Figure 1 This is a flowchart illustrating the workflow of a distance-pattern-based SINS / DVL cooperative navigation method.

[0067] Figure 2 This is a flowchart of a distance-pattern-based SINS / DVL cooperative navigation algorithm. Detailed Implementation

[0068] This invention proposes a distance-pattern-based SINS / DVL cooperative navigation method. Building upon the SINS / DVL integrated navigation system within its own integrated navigation system, it proposes a method to complete navigation using the position information of the primary AUV and its distance information from the primary AUV. Furthermore, to achieve the fusion of different frequency sensors, the "plug-and-play" nature of factor maps is utilized to achieve global estimation of multi-source information.

[0069] Step 1: Obtain the current SINS and DVL sensor data from the AUV;

[0070] The AUV uses its own SINS and DVL navigation sensors to acquire the current values ​​from the SINS accelerometer and gyroscope, updates the inertial navigation system, acquires the DVL four-beam velocity information, and establishes a DVL compact combination model.

[0071] Calculate the velocity in the DVL coordinate system based on the current velocity of SINS:

[0072]

[0073] in, SINS indicates that the speed information is displayed in the navigation coordinate system. φ represents the speed error of SINS. n This represents the attitude error of SINS. This represents the attitude transition matrix between the navigation coordinate system and the vehicle coordinate system. This represents the transition matrix between the load cell and the DVL coordinate system due to installation errors. The transfer matrix between the DVL coordinate system and the four-beam coordinate system is typically calculated as follows:

[0074]

[0075] Where α represents the beam angle of DVL.

[0076] The four-beam velocity model of DVL can be expressed as:

[0077]

[0078] Where δK represents the scale factor, w DVL This represents random error.

[0079] Establish a SINS / DVL compact combination model:

[0080]

[0081] Step 2: Obtain the location information of the main AUV and the distance information from the main AUV from the AUV, and establish a distance model:

[0082] Assumption The relative position vector is calculated from the SINS, which provides the relative position information of the slave AUV and the master AUV. The relative position vector is calculated as follows:

[0083]

[0084] Ignoring a small amount of second-order error, the ranging error is:

[0085]

[0086] in This indicates the relative position error.

[0087] Establish a distance model based on the location information of the main AUV and the slave AUV:

[0088]

[0089] in, This indicates the location information of the main AUV. Represents the location information from the AUV, R Nh =R N +h, R Mh =R M +h:

[0090] R e denoted by , and e represents the Earth's semi-major axis radius.

[0091] Will Switch to the ranging coordinate system:

[0092]

[0093] in, The rotation matrix between the vehicle coordinate system and the ranging coordinate system is represented. This indicates the lever arm error between the SINS and the distance sensor. The rotation matrix between the geographic coordinate system and the navigation coordinate system, which is related to the current location:

[0094]

[0095] The error in calculating the rotation matrix caused by the position error is:

[0096]

[0097]

[0098] calculate

[0099]

[0100] in,

[0101]

[0102] Considering installation errors and lever arm errors:

[0103]

[0104] Ignore higher-order small errors:

[0105]

[0106] in, Simplified to:

[0107]

[0108] but for:

[0109]

[0110] Based on SINS and the distance model, the distance error model is as follows:

[0111]

[0112] in, Factors for collaborative navigation are established based on the factor graph model:

[0113] (1) Establish SINS factors:

[0114] r SINS,k =r(X) k|k-1 ,X k ) = F k|k-1 X k-1 -X k

[0115] Among them, F k|k-1 Represents the state transition matrix. φ n ,δV n ,δP,ε, These are attitude error, velocity error, position error, gyro constant drift, and accelerometer bias.

[0116] (2) Establish the DVL compact combination factor:

[0117]

[0118] in,

[0119] (3) Establish distance factor:

[0120]

[0121] Among them, H dis =[H1 0 3×3 H2 0 3×6 ]

[0122] Step 4: Construct a factor graph global optimization function based on the factors from Step 3:

[0123]

[0124] Among them, P m|m-1 is represents the prior covariance at time m, R DVL,m is the noise model of DVL at time m, R dis,m It is the noise model at time m.

[0125] Step 5: Optimize the cost function from Step 4 and output the navigation results.

[0126] The above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention in any other way. Any modifications or equivalent changes made based on the technical essence of the present invention shall still fall within the scope of protection claimed by the present invention.

Claims

1. A SINS / DVL cooperative navigation method based on distance patterns, characterized in that: include: Step 1: Obtain the current SINS and DVL sensor data from the AUV; The AUV uses its own SINS and DVL navigation sensors to acquire the current values ​​from the SINS accelerometer and gyroscope, updates the inertial navigation system, acquires the DVL four-beam velocity information, and establishes a DVL compact combination model. (1) Calculate the velocity in the DVL coordinate system based on the current velocity of SINS: in, SINS indicates that the speed information is displayed in the navigation coordinate system. This indicates the speed error of SINS. This represents the attitude error of SINS. This represents the attitude transition matrix between the navigation coordinate system and the vehicle coordinate system. This represents the transition matrix between the load cell and the DVL coordinate system due to installation errors. The transfer matrix between the DVL coordinate system and the four-beam coordinate system is typically calculated as follows: in, Indicates the beam angle of DVL; (2) The four-beam velocity model of DVL can be expressed as: in, Indicates the scale factor. Indicates random error; (3) Establish the SINS / DVL compact combination model: Step 2: Obtain the location information of the main AUV and the distance information from the main AUV from the AUV, and establish a distance model: (1) Assumption The relative position vector is calculated from the SINS, which provides the relative position information of the slave AUV and the master AUV. The relative position vector is calculated as follows: Ignoring a small amount of second-order error, the ranging error is: in Indicates relative position error; (2) Establish a distance model based on the location information of the main AUV and the slave AUV: in, This indicates the location information of the main AUV. This indicates the location information from the AUV. , : , , Represents the radius of the Earth's semi-major axis. Indicates the Earth's eccentricity; Will Switch to the ranging coordinate system: in, The rotation matrix representing the vehicle coordinate system and the distance measuring coordinate system. This indicates the lever arm error between the SINS and the distance sensor. The rotation matrix between the geographic coordinate system and the navigation coordinate system, which is related to the current location: The error in calculating the rotation matrix caused by the position error is: (3) Calculation : in, Considering installation errors and lever arm errors: Ignore higher-order small errors: in, Simplified to: but for: (4) Based on SINS and the distance model, the distance error model is as follows: in, , ; Step 3: Establish the factors for collaborative navigation based on the factor graph model: (1) Establishing the SINS factor: in, Represents the state transition matrix. , , , , , These are attitude error, velocity error, position error, gyro constant drift, and accelerometer bias. (2) Establish the DVL compact combination factor: in, (3) Establish distance factor: in, Step 4: Construct a factor graph global optimization function based on the factors from Step 3: in, is represents the prior covariance at time m. is the DVL noise model at time m. This is the noise model at time m; Step 5: Optimize the cost function from Step 4 and output the navigation results.

Images