Underwater Cooperative Navigation and Positioning Method Based on Alternating Inertial / Ultra-Short Baseline Piloting

Abstract

The underwater cooperative navigation and positioning method based on alternating inertial / ultra-short baseline (USBL) navigation first establishes a leader motion model. The leader uses inertial navigation system (INS) for extrapolation navigation. When the leader does not receive prior topographic maps, it uses Kalman filtering for INS / DVL combined navigation. When the leader receives prior topographic maps, it uses Kalman filtering for INS / DVL / TAN combined navigation. Then, a follower cooperative navigation model is established. Next, the leader uses ultra-short baseline (USBL) to measure the distance and angle of the follower, and sends its position, slant range, and azimuth information to the follower at fixed intervals. The follower uses this information for a compact combination based on Kalman filtering. Finally, when USBL measures the distance and angle, a single leader performs compact combination positioning of the follower. When USBL only measures the distance, this invention sets simultaneous leadership as alternating leadership, meaning that at one moment leader 1 broadcasts its position and distance information, and at the next moment leader 2 broadcasts its position and distance information.

Description

Technical Field

[0001] This invention belongs to the field of navigation and positioning technology, specifically an underwater cooperative navigation and positioning method based on alternating inertial / ultra-short baseline navigation. Background Technology

[0002] With the increasing development and utilization of marine resources, underwater vehicles are being used more and more widely in fields such as ocean exploration, seabed resource exploration, underwater rescue, and underwater relay communication. However, due to the complexity and uncertainty of the underwater environment, navigation, positioning, and information fusion technologies have become crucial for improving the positioning accuracy of underwater vehicles.

[0003] Underwater vehicle navigation and positioning technology primarily focuses on performance indicators such as accuracy, stealth, continuity, reliability, real-time performance, and sea area applicability. Using inertial navigation technology as the mainstay, combined with acoustic positioning, terrain matching, and other auxiliary navigation technologies, multi-sensor fusion positioning of a single underwater vehicle is currently the main method for underwater vehicle navigation. Acoustic navigation solves the problem of short propagation distances caused by the rapid attenuation of electromagnetic waves underwater, which prevents long-term underwater operations.

[0004] High-precision underwater navigation and positioning technology is crucial for underwater unmanned system (UAV) swarms to accomplish many complex tasks (such as intelligence gathering, coastal anti-submarine warfare, and mine detection). Currently, relying solely on the navigation and positioning of a single UAV system makes it difficult to complete large-scale, multi-target underwater missions. Cooperative navigation and positioning of multiple UAV systems can improve the intelligence and efficiency of the entire mission system, effectively enhance the navigation accuracy of a single navigation system, reduce positioning errors, and accomplish tasks that are difficult for a single UAV system. Underwater UAV cooperative navigation and positioning technologies are mainly divided into distributed and master-slave types. In distributed cooperative navigation, each vessel independently performs navigation and positioning, and then the navigation information of each vessel is processed through data fusion algorithms to achieve cooperative positioning. However, this method requires each vessel to be equipped with high-precision navigation and positioning equipment, making it difficult to balance low cost and high precision. In master-slave cooperative navigation, the vessel equipped with high-precision navigation equipment acts as the navigator, while other vessels equipped with low-precision navigation equipment act as followers. Followers request distance measurement and communication from the navigator at fixed intervals. The followers use the navigator's position and distance information for data fusion processing to correct their own positions, thereby achieving cooperative navigation and positioning.

[0005] Most current studies assume that the navigator's position is accurate, and the follower uses a compass, log, and underwater acoustic communication device for ranging to establish a two-dimensional cooperative navigation model. To better reflect reality, this invention presents a navigator motion model based on a high-precision inertial navigation system (INS), Doppler Velocity Log (DVL), and Terrain-Aided Navigation (TAN), and a follower cooperative navigation model based on a low-precision inertial measurement unit (IMU). Both models achieve cooperative navigation and positioning through ranging and angle measurement using an ultra-short baseline (USBL). When the USBL performs ranging and angle measurement, a single navigator performs tight-fitting positioning of the follower. When the USBL only performs ranging, to avoid signal interference caused by simultaneous communication between two navigators, this invention sets simultaneous navigation to alternating navigation, thereby improving the follower's positioning accuracy.

[0006] The differences between this application and the prior art are as follows:

[0007] Technical Comparison with Patent CN118293924A "A Multi-UUV Cooperative Navigation Method and System Based on USBL Distance Measurement Information"

[0008] Patent CN118293924A uses a compass (providing heading) and a log (providing speed) to establish a two-dimensional motion model of the vehicle. Our model, however, utilizes a low-precision IMU to establish a 15-dimensional three-dimensional motion model of the following vehicle, along with a motion model of the navigator. When the navigator travels to areas without prior maps, it uses a combination of inertial / Doppler log navigation; when it travels to areas with prior maps, it uses a combination of inertial / Doppler log / terrain-aided navigation.

[0009] Patent CN118293924A uses Ultra-Short Baseline (USBL) technology, which only performs ranging. Our USBL technology, however, performs both ranging and angle measurement. Furthermore, to improve the positioning accuracy of the USBL-based follower, this invention also derives the stick arm error of the navigator equipped with USBL and the inertial navigation system, as well as the stick arm error of the follower equipped with a transponder and IMU.

[0010] Technical Comparison with Patent CN117739978B "A Multi-AUV Parallel Cooperative Navigation and Positioning Method and System Based on Factor Graph"

[0011] Patent CN117739978B established a parallel cooperative navigation and positioning model using factor graphs. In this parallel cooperative navigation system, each AUV is equipped with a strapdown inertial navigation system (SINS), a Doppler velocimeter (DVL), a magnetic compass (MCP), and a terrain-assisted positioning system (TAN). Underwater acoustic detection equipment is used to measure the relative distance and relative bearing information between AUVs. However, we propose a master-slave cooperative navigation and positioning model using a filtering method. In this model, the navigator is equipped with SINS / DVL / TAN, while the follower is equipped with a low-precision IMU, effectively balancing high accuracy and low cost. Furthermore, since the SINS and USBL on the navigator, as well as the IMU and transponder on the follower, all have non-negligible lever arm errors, we considered the impact of these errors while simultaneously using USBL for ranging and direction finding.

[0012] Technical Comparison with Patent CN111595348A "A Master-Slave Cooperative Positioning Method for an Autonomous Underwater Vehicle Integrated Navigation System"

[0013] Patent CN103245360A establishes motion equations for an AUV with position, velocity, and attitude as state variables using a calculation navigation system, and establishes observation equations with depth, attitude, velocity, and distance as observables using acoustic distance measurement. Our system, however, establishes follower motion equations with position error, velocity error, attitude error, gyroscope bias, and accelerometer bias as state variables using an IMU, and establishes observation equations with slant range and azimuth as observables using USBL for range and angle measurement.

[0014] Technical Comparison with Patent CN114440869A "A Cooperative Navigation and Positioning Method for AUV Cluster Operations in Deep Water with Dual-Master AUV Switching"

[0015] Patent CN114440869A describes a dual-master AUV operating mode. After operating underwater for a period, master AUV1 switches to follower mode and surfaces. Upon surfacing, it performs GPS calibration and switches back to navigator mode, while master AUV2 switches to follower mode. Both master and follower AUVs perform position error calibration based on the position and ranging information of master AUV1. Our proposed alternating navigator mode involves navigator 1 broadcasting its position, slant range, and azimuth information at a certain moment, and navigator 2 broadcasting the same information at the next moment. The follower then uses the navigator's position, slant range, and azimuth information for error calibration.

[0016] Patent CN114440869A utilizes the main AUV to periodically ascend and receive GNSS signals for position calibration. However, when there is no prior topographic map, we use SINS / DVL integrated navigation to calibrate the navigator's position; when a prior topographic map exists, we use SINS / DVL / TAN integrated navigation to calibrate the navigator's position. Summary of the Invention

[0017] To address the above problems, this invention proposes an underwater cooperative navigation and positioning method based on alternating inertial / ultra-short baseline navigation. The navigator is equipped with high-precision navigation equipment and uses USBL for ranging and angle measurement to improve the navigation and positioning accuracy of the follower.

[0018] To achieve the above objectives, the technical solution adopted by the present invention is as follows:

[0019] The underwater cooperative navigation and positioning method based on alternating inertial / ultra-short baseline navigation includes the following steps:

[0020] Step 1: Navigator autonomous navigation and positioning based on INS / DVL / TAN combination:

[0021] The Navigator is equipped with high-precision INS, DVL, USBL, and prior topographic maps;

[0022] Step 2: Establish a follower motion model based on IMU error: The follower relies on a low-precision IMU for dead reckoning;

[0023] Step 3: Establish a follower observation model based on USBL: The leader sends its own position and the measured distance and azimuth to the follower every 2 seconds through USBL. The follower uses the distance and azimuth from USBL and the distance and azimuth derived from IMU to perform Kalman filtering tight combination.

[0024] Step 4: When USBL performs ranging and angle measurement, a single leader performs tight-group positioning of multiple followers. When USBL only performs ranging, simultaneous leadership is set to alternating leadership, i.e., t k Navigator 1 broadcasts its location and distance information. k+1 Always Navigator 2 broadcasts its location and distance information.

[0025] As a further improvement to the present invention, the motion model of the navigator in step 1 is as follows:

[0026]

[0027] in, This indicates the attitude error, velocity error, position error, gyroscope bias, and accelerometer bias of the INS.

[0028] When the navigator does not receive a prior topographic map, the navigator's observation model is as follows:

[0029]

[0030] in, Let h be the velocity of INS in the carrier coordinate system. INS For the height information calculated by INS, v DVL The carrier velocity measured by DVL, h ALT Depth measured by a pressure gauge;

[0031] When the navigator receives the prior topographic map, the navigator's observation model is:

[0032]

[0033] Where, p INS For the location information calculated by INS, p TAN The location is indicated by the prior topographic map.

[0034] As a further improvement to the present invention, the motion model of the follower motion model in step 2 is as follows:

[0035]

[0036] in These represent attitude error, velocity error, position error, gyroscope bias, and accelerometer bias, respectively.

[0037] As a further improvement to this invention, the observation model for the follower in step 3 is as follows:

[0038]

[0039] Where, r USBL ,α USBL ,β USBL These are the slope distance and azimuth angle measured by USBL, respectively. These are the slant range and azimuth angle derived from the IMU, respectively.

[0040] As a further improvement to the present invention, the derivation Assuming the navigator's USBL is in the u-series, the follower's transponder's slant range and azimuth in the u-series are:

[0041]

[0042] Where, r u =[x u y u z u [] represents the coordinates of the transponder. Partial differentiation is performed on (1.10).

[0043]

[0044] in,

[0045]

[0046] Navigator's Instagram location Instagram location of followers The relative position in the Earth coordinate system e is p. e :

[0047]

[0048] The above formula is correct. Taking the partial differential, we get:

[0049] δp e =C e δp n (1.14)

[0050] in,

[0051]

[0052] The lever arm error between the INS mounted on the follower and its transponder is [missing information]. Therefore, the relative positions of the navigator's INS and the follower's transponder are:

[0053]

[0054] The relative positions of the USBL and the follower transponder in the navigator are as follows:

[0055]

[0056] in

[0057]

[0058] matrix In [L] f ,λ f ] T The Taylor expansion yields the expression for its error matrix, which, for ease of representation, is denoted as S. a =sina,C a =cosa,a=L f or λ f .

[0059]

[0060] Considering the attitude errors of the follower IMU and transponder,

[0061]

[0062] Introducing an error factor, substituting (1.18) into (1.17) yields:

[0063]

[0064] Ignoring small amounts of higher-order errors, we can obtain:

[0065]

[0066] From the second term on the right side of formulas (1.19) and (1.22), we can obtain...

[0067]

[0068] in

[0069]

[0070] According to formulas (1.22) and (1.24), we can obtain...

[0071]

[0072] Substitute equation (1.25) into equation (1.11)

[0073]

[0074] Where, α true ,β true ,r true These are the true values ​​for azimuth and slant range.

[0075] The azimuth and slant range information obtained from the ultra-short baseline system are as follows:

[0076]

[0077] Subtracting formula (1.26) from formula (1.27) gives:

[0078]

[0079] Where the observation matrix H = [H φ 0 3×3 H p 0 3×6 The measurement noise matrix is ​​V = [ΔαΔβΔD]. T .

[0080] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0081] This invention provides a method for navigation and positioning of underwater vehicle swarms. First, a leader motion model is established based on an Inertial Navigation System (INS), a Doppler Velocity Log (DVL), and Terrain-Aided Navigation (TAN). The leader uses INS for extrapolated navigation. When the leader does not receive a prior topographic map, it uses Kalman filtering for INS / DVL combined navigation. When the leader receives a prior topographic map, it uses Kalman filtering for INS / DVL / TAN combined navigation. Then, a follower cooperative navigation model based on a low-precision Inertial Measurement Unit (IMU) is established. Next, the navigator uses an Ultra Short Baseline (USBL) to measure the distance and angle of the follower, and sends its position, slant range, and azimuth information to the follower at fixed intervals. The follower uses its own inertial navigation information to perform a tight combination based on Kalman filtering with the slant range and azimuth information, thereby improving the follower's positioning accuracy. Finally, when the USBL performs distance and angle measurement, a single navigator performs tight combination positioning of the follower. When the USBL only performs distance measurement, to avoid signal interference caused by simultaneous communication between two navigators to the follower, this invention sets simultaneous navigation to alternating navigation; that is, at one moment, navigator 1 broadcasts its own position and distance information, and at the next moment, navigator 2 broadcasts its own position and distance information. Attached Figure Description

[0082] Figure 1 Flowchart of underwater collaborative navigation and positioning;

[0083] Figure 2 This is a schematic diagram of cooperative positioning between a single navigator (blue) and a follower (red) under USBL ranging and angle measurement conditions.

[0084] Figure 3 This is a schematic diagram illustrating the cooperative localization of the follower (red) with two navigators (blue) taking turns leading in the range-only USBL scenario. Detailed Implementation

[0085] The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments:

[0086] Flowchart as follows Figure 1 As shown, the schematic diagram of cooperative positioning between a single navigator (top) and a follower (bottom) under USBL ranging and angle measurement conditions is as follows. Figure 2 As shown, in the case of USBL ranging only, the diagram illustrates the cooperative localization of the follower (bottom) with alternating navigation by two navigators (top) and the follower (bottom). Figure 3As shown, this invention proposes an underwater cooperative navigation and positioning method based on alternating inertial / ultra-short baseline navigation, comprising the following steps:

[0087] The technical solution of this invention is as follows:

[0088] 1) Navigator autonomous navigation and positioning based on INS / DVL / TAN combination:

[0089] The Navigator is equipped with high-precision INS, DVL, USBL, and prior topographic mapping. The Navigator's motion model is as follows:

[0090]

[0091] in, This indicates the attitude error, velocity error, position error, gyroscope bias, and accelerometer bias of the INS.

[0092] When the navigator does not receive a prior topographic map, the navigator's observation model is as follows:

[0093]

[0094] in, Let h be the velocity of INS in the carrier coordinate system. INS For the height information calculated by INS, v DVL The carrier velocity measured by DVL, h ALT The depth measured by the pressure gauge.

[0095] When the navigator receives the prior topographic map, the navigator's observation model is:

[0096]

[0097] Where, p INS For the location information calculated by INS, p TAN The location is indicated by the prior topographic map.

[0098] 2) Establish a follower motion model based on IMU error: The follower relies on a low-precision IMU for dead reckoning. Its motion model is as follows:

[0099]

[0100] in These represent attitude error, velocity error, position error, gyroscope bias, and accelerometer bias, respectively.

[0101]

[0102] in, R M R is the principal curvature radius of the meridian. NLet L be the principal radius of curvature of the zonal circle, and L be the latitude of the spacecraft.

[0103] 3) Establish a follower observation model based on USBL: The leader sends its own position and measured distance and azimuth to the follower every 2 seconds via USBL. The follower's observation model is as follows:

[0104]

[0105] Where, r USBL ,α USBL ,β USBL These are the true values ​​of the slope distance and azimuth angle obtained from USBL measurements, respectively.

[0106] The following section will focus on the derivation. Assuming the navigator's USBL is in the u-series, the follower's transponder's slant range and azimuth in the u-series are:

[0107]

[0108] Where, r u =[x u y u z u [] represents the coordinates of the transponder. Partial differentiation is performed on (1.10).

[0109]

[0110] in,

[0111]

[0112] Navigator's Instagram location Instagram location of followers The relative position in the Earth coordinate system e is p. e :

[0113]

[0114] The above formula is correct. Taking the partial differential, we get:

[0115] δp e =C e δp n (1.14)

[0116] in,

[0117]

[0118] The lever arm error between the INS mounted on the follower and its transponder is [missing information]. Therefore, the relative positions of the navigator's INS and the follower's transponder are:

[0119]

[0120] The relative positions of the USBL and the follower transponder in the navigator are as follows:

[0121]

[0122] in

[0123]

[0124] matrix In [L] f ,λ f ] T The Taylor expansion yields the expression for its error matrix, which, for ease of representation, is denoted as S. a =sina,C a =cosa,a=L f or λ f .

[0125]

[0126] Considering the attitude errors of the follower IMU and transponder,

[0127]

[0128] Introducing an error factor, substituting (1.18) into (1.17) yields:

[0129]

[0130] Ignoring small amounts of higher-order errors, we can obtain:

[0131]

[0132] From the second term on the right side of formulas (1.19) and (1.22), we can obtain...

[0133]

[0134] in

[0135]

[0136] According to formulas (1.22) and (1.24), we can obtain...

[0137]

[0138] Substitute equation (1.25) into equation (1.11)

[0139]

[0140] Where, α true ,β true ,r true These are the true values ​​for azimuth and slant range.

[0141] The azimuth and slant range information obtained from the ultra-short baseline system are as follows:

[0142]

[0143] Subtracting formula (1.26) from formula (1.27) gives:

[0144]

[0145] Where the observation matrix H = [H φ 0 3×3 H p 0 3×6 The measurement noise matrix is ​​V = [ΔαΔβΔD]. T

[0146] 4) Follower Cooperative Navigation and Positioning Based on Alternating Leading: When USBL performs ranging and angle measurement, a single leader performs tight-group positioning of multiple followers. When USBL only performs ranging, to avoid signal interference caused by simultaneous communication between two leaders and followers, this invention sets simultaneous leading to alternating leading, i.e., t k Navigator 1 broadcasts its location and distance information. k+1 Always Navigator 2 broadcasts its location and distance information.

[0147] To verify the effectiveness of this invention, the algorithm was simulated using the MATLAB platform. The simulation parameter settings for the cooperative navigation system are shown in Table 1.

[0148] Table 1 Simulation condition parameter settings

[0149]

[0150] 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. An underwater cooperative navigation and positioning method based on alternating inertial / ultra-short baseline navigation, characterized in that: Includes the following steps: Step 1: Navigator autonomous navigation and positioning based on INS / DVL / TAN combination: The Navigator is equipped with high-precision INS, DVL, USBL, and prior topographic maps; Step 2: Establish a follower motion model based on IMU error: The follower relies on a low-precision IMU for dead reckoning; Step 3: Establish a follower observation model based on USBL: The leader sends its own position and the measured distance and azimuth to the follower every 2 seconds through USBL. The follower uses the distance and azimuth from USBL and the distance and azimuth derived from IMU to perform Kalman filtering tight combination. Step 4: When USBL performs ranging and angle measurement, a single leader performs tight-group positioning of multiple followers. When USBL only performs ranging, simultaneous leadership is set to alternating leadership, i.e. The first navigator should broadcast its location and distance information at all times. The second navigator broadcasts its location and distance information at all times.

2. The underwater cooperative navigation and positioning method based on alternating inertial / ultra-short baseline navigation according to claim 1, characterized in that: The motion model of the navigator in step 1 is as follows: （1.1） in, This indicates the attitude error, velocity error, position error, gyroscope bias, and accelerometer bias of the INS. When the navigator does not receive a prior topographic map, the navigator's observation model is as follows: (1.2) (1.3) in, The velocity of the INS in the carrier coordinate system. The height information is calculated for INS. The carrier velocity measured by DVL Depth measured by a pressure gauge; When the navigator receives the prior topographic map, the navigator's observation model is: (1.4) in, Location information calculated by INS. The location is indicated by the prior topographic map.

3. The underwater cooperative navigation and positioning method based on alternating inertial / ultra-short baseline navigation according to claim 1, characterized in that: Step 2: The follower motion model is as follows: (1.5) in These represent attitude error, velocity error, position error, gyroscope bias, and accelerometer bias, respectively.

4. The underwater cooperative navigation and positioning method based on alternating inertial / ultra-short baseline navigation according to claim 1, characterized in that: The observation model for followers in step 3 is as follows: (1.9) in, These are the slope distance and azimuth angle measured by USBL, respectively. These are the slant range and azimuth angle derived from the IMU, respectively.

5. The underwater cooperative navigation and positioning method based on alternating inertial / ultra-short baseline navigation according to claim 4, characterized in that: Derivation Assuming the Navigator is equipped with USBL The system, the transponder carried by the follower The slant distance and azimuth of the system are: (1.10) in, Let (1.10) represent the coordinates of the transponder. Partial differentiation is performed on (1.10). (1.11) in, (1.12) Navigator's Instagram location Instagram location of followers In Earth coordinate system The relative positions under the system are : (1.13) The above formula is correct. Taking the partial differential, we get: (1.14) in, (1.15) The lever arm error between the INS mounted on the follower and its transponder is [missing information]. Therefore, the relative positions of the navigator's INS and the follower's transponder are: (1.16) The relative positions of the USBL and the follower transponder in the navigator are as follows: (1.17) in (1.18) matrix exist The Taylor expansion yields the expression for its error matrix, which, for ease of representation, is denoted as... or ; (1.19) Considering the attitude errors of the follower IMU and transponder, (1.20) Introducing the error factor, substituting (1.18) into (1.17) yields: (1.21) Ignoring small amounts of higher-order errors, we can obtain: (1.22) From the second term on the right side of formulas (1.19) and (1.22), we can obtain... (1.23) in (1.24) According to formulas (1.22) and (1.24), we can obtain (1.25) Substitute equation (1.25) into equation (1.11) (1.26) in, These are the true values ​​for azimuth and slant range. , ; The azimuth and slant range information obtained from the ultra-short baseline system are as follows: (1.27) Subtracting formula (1.26) from formula (1.27) gives: (1.28) Where the observation matrix The measurement noise matrix is .

Images