Underwater cooperative navigation positioning method based on inertia / ultra-short baseline alternative navigation

Abstract

The invention discloses an underwater collaborative navigation positioning method based on inertia / ultra-short baseline alternative navigation. The method comprises the following steps: firstly, establishing a navigator motion model; a navigator carries out calculation navigation by using INS, and when the navigator does not receive a prior topographic map, INS / DVL integrated navigation is carried out by using Kalman filtering. When a navigator receives a prior topographic map, INS / DVL / TAN integrated navigation is carried out by using Kalman filtering. And then establishing a follower collaborative navigation model. Then, the navigator carries out distance measurement and angle measurement on the follower through the ultra-short base line, position information, slant distance information and azimuth angle information of the follower are sent to the follower at fixed time intervals, and the follower carries out tight combination based on Kalman filtering through the information; and finally, when the USBL carries out distance measurement and angle measurement, the single navigator carries out tight combination positioning on the follower. When the USBL only carries out distance measurement, simultaneous navigation is set to be alternate navigation, namely, the navigator 1 broadcasts own position and distance information at the moment, and the navigator 2 broadcasts own position and distance information at the next moment.

Description

Technical Field

[0001] The invention belongs to the technical field of navigation and positioning, and specifically relates to an underwater collaborative navigation and positioning method based on inertial / ultra-short baseline alternating piloting. Background Art

[0002] With the increasing development and utilization of marine resources, underwater vehicles are increasingly used in the fields of ocean exploration, seabed resource exploration, underwater rescue and underwater relay communication. However, due to the complexity and uncertainty of the underwater environment, the navigation positioning and information fusion technology of underwater vehicles have become the key to improving their positioning accuracy.

[0003] The underwater vehicle navigation and positioning technology mainly focuses on performance indicators such as accuracy, concealment, continuity, reliability, real-time and sea area applicability. Taking inertial navigation technology as the main body, combined with auxiliary navigation technologies such as acoustic positioning and terrain matching, the multi-sensor fusion positioning of single underwater vehicles is realized, which is the main method of underwater vehicle navigation at present. Acoustic navigation solves the problem of short propagation distance and inability to lurk underwater for a long time due to the rapid attenuation of electromagnetic waves underwater.

[0004] High-precision underwater navigation and positioning technology is very important for underwater unmanned system clusters to achieve many complex tasks (such as intelligence collection, coastal anti-submarine, mine detection, etc.). At present, it is difficult to complete large-scale, multi-target perception underwater tasks relying solely on the navigation and positioning of a single underwater unmanned system. The collaborative navigation and positioning of multiple underwater unmanned systems can improve the intelligence level and efficiency of the entire task system, effectively improve the navigation accuracy of a single navigation system, reduce positioning errors, and complete tasks that are difficult for a single unmanned system to complete. The collaborative navigation and positioning technology of underwater unmanned systems is mainly divided into distributed and master-slave. In distributed collaborative navigation, each aircraft performs navigation and positioning independently, and then processes the navigation information of each aircraft through a data fusion algorithm to achieve collaborative positioning. However, this method requires each aircraft to be equipped with high-precision navigation and positioning equipment, which cannot balance the relationship between low cost and high precision. In master-slave collaborative navigation, the aircraft equipped with high-precision navigation equipment serves as the navigator, and other aircraft equipped with low-precision navigation equipment serve as followers. The follower requests the navigator to measure distance and communicate at fixed intervals. The follower uses the position information and distance information of the navigator for data fusion processing to correct the position of the follower, thereby achieving collaborative navigation and positioning.

[0005] At present, most studies assume that the position of the navigator is accurate, and the follower uses a compass, a speedometer and an acoustic communication device to measure distance to establish a two-dimensional collaborative navigation model. In order to be more in line with the actual situation, the present invention provides a navigator motion model based on a high-precision inertial navigation system (INS), a Doppler velocity log (DVL), and a terrain-aided navigation (TAN) and a follower collaborative navigation model based on a low-precision inertial sensor (IMU). Both perform ranging and angle measurement through an ultra-short baseline (USBL) to achieve collaborative navigation positioning. When USBL performs ranging and angle measurement, a single navigator performs a tight combination positioning on the follower. When USBL only performs ranging, in order to avoid the signal interference problem caused by the dual navigators to the follower communication at the same time, the present invention sets the simultaneous navigation to alternating navigation, thereby improving the positioning accuracy of the follower.

[0006] The present application is different from the prior art as follows:

[0007] Technical comparison with patent CN118293924A "A multi-UUV collaborative navigation method and system based on USBL distance measurement information"

[0008] Patent CN118293924A uses a compass (provides heading) and an odometer (provides speed) to establish a two-dimensional motion model of the follower vehicle. We use a low-precision IMU to establish a 15-dimensional three-dimensional motion model of the follower vehicle, and at the same time establish a motion model of the navigator. When the navigator sails to a sea area without a priori map, the navigator performs inertial / Doppler odometer combined navigation. When the navigator sails to a sea area with a priori map, the navigator performs inertial / Doppler odometer / terrain-assisted combined navigation.

[0009] Patent CN118293924A uses an ultra-short baseline (USBL) to measure only the distance. However, the USBL we use measures both the distance and the angle. In order to improve the positioning accuracy of the follower based on USBL, the present invention also derives the arm error between the USBL carried by the leader and the inertial navigation system, as well as the arm error between the transponder carried by the follower and the IMU.

[0010] Technical comparison with patent CN117739978B "A multi-AUV parallel collaborative navigation and positioning method and system based on factor graph"

[0011] In patent CN117739978B, a parallel collaborative navigation and positioning model was established using factor graphs. In the parallel collaborative navigation system, each vehicle is equipped with a strapdown inertial navigation system SINS, a Doppler velocimeter DVL, a magnetic compass MCP, and a terrain-aided positioning system TAN, and an acoustic detection device is used to measure the relative distance and relative orientation information between AUVs. However, we proposed to establish a master-slave collaborative navigation and positioning model using a filtering method. In the master-slave collaborative navigation system, the navigator is equipped with SINS / DVL / TAN, and the follower is equipped with a low-precision IMU, which effectively balances the relationship between high precision and low cost. In addition, since the SINS and USBL carried by the navigator and the IMU and transponder carried by the follower all have lever arm errors that cannot be ignored, we also considered the influence of the lever arm error while using USBL to measure distance and direction at the same time.

[0012] Technical comparison with patent CN111595348A "A master-slave collaborative positioning method for an autonomous underwater vehicle integrated navigation system"

[0013] Patent CN103245360A uses dead reckoning navigation system to establish the follower AUV motion equation with state quantities of position, velocity and attitude, and uses acoustic communication to measure distance to establish the observation equation with observation quantities of depth, attitude, velocity and distance. We use IMU to establish the follower motion equation with state quantities of position error, velocity error, attitude error, gyro bias and accelerometer bias, and use USBL to measure distance and angle to establish the observation equation with observation quantities of slant range and azimuth.

[0014] Technical comparison with patent CN114440869A "A collaborative navigation and positioning method for AUV cluster deep water operation with dual master AUV switching"

[0015] Patent CN114440869A constructs a dual-master AUV working mode, that is, after working underwater for a period of time, the master AUV1 switches to the follower mode and surfaces, performs GPS calibration after surfacing and switches to the navigator mode, while the master AUV2 switches to the follower mode. The master AUV2 and the slave AUV perform position error calibration based on the position and ranging information of the master AUV1. The alternating navigation we proposed is that the navigator 1 broadcasts its own position, slant range and azimuth information at a certain moment, and the navigator 2 broadcasts its own position, slant range and azimuth information at the next moment, and the follower uses the navigator's position information, slant range and azimuth information to calibrate the error.

[0016] Patent CN114440869A uses the main AUV to periodically surface to receive GNSS signals for position calibration. However, when there is no prior terrain map, we use SINS / DVL combined navigation to calibrate the position of the navigator, and when there is a prior terrain map, we use SINS / DVL / TAN combined navigation to calibrate the position of the navigator. Summary of the invention

[0017] In view of the above problems, the present invention proposes an underwater collaborative navigation and positioning method based on alternating pilotage of inertial / ultra-short baseline. The leader is equipped with high-precision navigation equipment, and USBL is used to perform distance and angle measurement to improve the navigation and positioning accuracy of the follower.

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

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

[0020] Step 1: Autonomous navigation and positioning of the pilot based on INS / DVL / TAN combination:

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

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

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

[0024] Step 4: When USBL performs distance measurement and angle measurement, a single pilot performs tight combination positioning on multiple followers. When USBL only performs distance measurement, the simultaneous pilot is set to alternate pilot, i.e., t k At this moment, navigator 1 broadcasts its position and distance information. k+1 The Navigator 2 always broadcasts its position and distance information.

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

[0026]

[0027] in, Indicates the attitude error, velocity error, position error, gyro bias and accelerometer bias of INS;

[0028] When the navigator does not receive a priori terrain map, the navigator's observation model is:

[0029]

[0030] in, is the velocity of INS in the carrier coordinate system, h INS is the height information solved by INS, v DVL is the carrier velocity measured by DVL, h ALT is the depth measured by the pressure gauge;

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

[0032]

[0033] Among them, p INS is the position information solved by INS, p TAN is the location of the prior topographic map.

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

[0035]

[0036] in They represent attitude error, velocity error, position error, gyro bias and accelerometer bias respectively.

[0037] As a further improvement of the present invention, the observation model of the follower in step 3 is:

[0038]

[0039] Among them, r USBL ,α USBL ,β USBL They are the slant range and azimuth measured by USBL respectively. They are the slant range and azimuth derived from IMU respectively.

[0040] As a further improvement of the present invention, it is deduced Assuming that the USBL carried by the leader is in the u system, the slant range and azimuth of the transponder carried by the follower in the u system are:

[0041]

[0042] Among them, r u =[x u y u z u ] represents the coordinates of the transponder, and the partial differentiation of (1.10) is

[0043]

[0044] in,

[0045]

[0046] INS position of the pilot INS position with followers The relative position in the earth coordinate system e is p e :

[0047]

[0048] The above formula is Taking partial differentials we get:

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

[0050] in,

[0051]

[0052] The arm error between the INS carried by the follower and the transponder carried by it is Therefore, the relative position of the leader's INS and the follower's transponder is:

[0053]

[0054] The relative position between the USBL in the leader and the follower transponder is:

[0055]

[0056] in

[0057]

[0058] The matrix In [L f ,λ f ] T The error matrix expression can be obtained by Taylor expansion. For the convenience of expression, S a =sina,C a =cosa,a=L f or f .

[0059]

[0060] Considering the attitude error between the follower IMU and the transponder,

[0061]

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

[0063]

[0064] Ignoring the small amount of high-order errors, we can get:

[0065]

[0066] From the second term on the right side of formula (1.19) and formula (1.22), we can get

[0067]

[0068] in

[0069]

[0070] According to formula (1.22) and formula (1.24), we can get

[0071]

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

[0073]

[0074] Among them, α true ,β true ,r true are the true values ​​of azimuth and slant range,

[0075] The azimuth and slant range information measured by the ultra-short baseline system is:

[0076]

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

[0078]

[0079] 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 present invention has the following beneficial effects:

[0081] The present invention provides a method suitable for underwater vehicle cluster navigation and positioning. The present invention first establishes a navigator motion model based on an inertial navigation system (INS), a Doppler velocity log (DVL) and a terrain aided navigation (TAN). The navigator uses INS for dead reckoning navigation. When the navigator does not receive a priori terrain map, Kalman filtering is used for INS / DVL combined navigation. When the navigator receives a priori terrain map, Kalman filtering is used for INS / DVL / TAN combined navigation. Then a follower collaborative navigation model based on a low-precision inertial measurement unit (IMU) is established. Next, the navigator uses the Ultra Short Baseline (USBL) to measure the distance and angle of the follower, and sends its position information, slant range and azimuth information to the follower at fixed intervals. The follower uses its own inertial navigation information and slant range and azimuth information to perform a tight combination based on Kalman filtering, thereby improving the positioning accuracy of the follower. Finally, when USBL performs distance measurement and angle measurement, the single navigator performs a tight combination positioning on the follower. When USBL only performs distance measurement, in order to avoid the signal interference problem caused by the dual navigators to the follower communication at the same time, the present invention sets the simultaneous navigation to alternating navigation, that is, at this moment, navigator 1 broadcasts its own position and distance information, and at the next moment, navigator 2 broadcasts its own position and distance information. BRIEF DESCRIPTION OF THE DRAWINGS

[0082] Figure 1 Positioning flow chart for underwater collaborative navigation;

[0083] Figure 2 Schematic diagram of cooperative positioning of a single leader (blue) and a follower (red) in the case of USBL distance and angle measurement;

[0084] Figure 3 Schematic diagram of the collaborative positioning of the follower (red) with two navigators (blue) taking turns to navigate in the USBL ranging-only case. DETAILED DESCRIPTION

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

[0086] Flowchart as Figure 1 As shown in the figure, the schematic diagram of the cooperative positioning of a single leader (upper) and a follower (lower) under USBL distance and angle measurement is as follows Figure 2 As shown in the figure, in the case of USBL only ranging, the schematic diagram of the collaborative positioning of the two navigators (upper) and the follower (lower) is as follows Figure 3As shown, the present invention proposes an underwater collaborative navigation and positioning method based on inertial / ultra-short baseline alternating piloting, comprising the following steps:

[0087] The technical solution of the present invention is:

[0088] 1) Autonomous navigation and positioning of the pilot based on INS / DVL / TAN combination:

[0089] The navigator is equipped with high-precision INS, DVL, USBL and prior terrain map. The motion model of the navigator is:

[0090]

[0091] in, It represents the attitude error, velocity error, position error, gyro bias and accelerometer bias of INS.

[0092] When the navigator does not receive a priori terrain map, the navigator's observation model is:

[0093]

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

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

[0096]

[0097] Among them, p INS is the position information solved by INS, p TAN is the location of the prior topographic map.

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

[0099]

[0100] in They represent attitude error, velocity error, position error, gyro bias and accelerometer bias respectively.

[0101]

[0102] in, R M is the principal radius of curvature of the meridian, R Nis the main curvature radius of the apogee, and L is the latitude of the spacecraft.

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

[0104]

[0105] Among them, r USBL ,α USBL ,β USBL They are the true values ​​of slant range and azimuth measured by USBL respectively.

[0106] The following will focus on the derivation Assuming that the USBL carried by the leader is in the u system, the slant range and azimuth of the transponder carried by the follower in the u system are:

[0107]

[0108] Among them, r u =[x u y u z u ] represents the coordinates of the transponder, and the partial differentiation of (1.10) is

[0109]

[0110] in,

[0111]

[0112] INS position of the pilot INS position with followers The relative position in the earth coordinate system e is p e :

[0113]

[0114] The above formula is Taking partial differentials we get:

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

[0116] in,

[0117]

[0118] The arm error between the INS carried by the follower and the transponder carried by it is Therefore, the relative position of the leader's INS and the follower's transponder is:

[0119]

[0120] The relative position between the USBL in the leader and the follower transponder is:

[0121]

[0122] in

[0123]

[0124] The matrix In [L f ,λ f ] T The error matrix expression can be obtained by Taylor expansion. For the convenience of expression, S a =sina,C a =cosa,a=L f or f .

[0125]

[0126] Considering the attitude error between the follower IMU and the transponder,

[0127]

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

[0129]

[0130] Ignoring the small amount of high-order errors, we can get:

[0131]

[0132] From the second term on the right side of formula (1.19) and formula (1.22), we can get

[0133]

[0134] in

[0135]

[0136] According to formula (1.22) and formula (1.24), we can get

[0137]

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

[0139]

[0140] Among them, α true ,β true ,r true are the true values ​​of azimuth and slant range,

[0141] The azimuth and slant range information measured by the ultra-short baseline system is:

[0142]

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

[0144]

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

[0146] 4) Follower collaborative navigation and positioning based on alternating piloting: When USBL performs ranging and angle measurement, a single pilot performs tight combined positioning on multiple followers. When USBL only performs ranging, in order to avoid the signal interference problem caused by the dual pilots to the follower communication at the same time, the present invention sets the simultaneous piloting to alternating piloting, that is, t k At this moment, navigator 1 broadcasts its position and distance information. k+1 The Navigator 2 always broadcasts its position and distance information.

[0147] In order to verify the effectiveness of the present invention, the algorithm is simulated and verified using the MATLAB platform. The simulation parameter settings of the collaborative navigation system are shown in Table 1.

[0148] Table 1 Simulation condition parameter settings

[0149]

[0150] 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. An underwater collaborative navigation and positioning method based on alternating piloting of inertial / ultra-short baselines, characterized by: The steps include: Step 1: Autonomous navigation and positioning of the pilot based on INS / DVL / TAN combination: The navigator is equipped with high-precision INS, DVL, USBL and prior terrain maps; Step 2: Establish a follower motion model based on IMU error: The follower relies on low-precision IMU for dead reckoning; Step 3: Establish a follower observation model based on USBL: The leader sends its position and the measured distance and azimuth to the follower every 2 seconds through USBL. The follower uses the distance and azimuth of USBL and the distance and azimuth derived from IMU to perform Kalman filter tight combination; Step 4: When USBL performs distance measurement and angle measurement, a single leader performs tight combination positioning on multiple followers. When USBL only performs distance measurement, the simultaneous piloting is set to alternate piloting, i.e., t k At this moment, Navigator 1 broadcasts its position and distance information. k+1 The Navigator 2 always broadcasts its position and distance information.

2. The underwater collaborative navigation and positioning method based on inertial / ultra-short baseline alternating pilotage according to claim 1 is characterized by: The motion model of the navigator in step 1 is: in, Indicates the attitude error, velocity error, position error, gyro bias and accelerometer bias of INS; When the navigator does not receive a priori terrain map, the navigator's observation model is: in, is the velocity of INS in the carrier coordinate system, h INS is the height information solved by INS, v DVL is the carrier velocity measured by DVL, h ALT is the depth measured by the pressure gauge; When the navigator receives the prior terrain map, the navigator's observation model is: Among them, p INS is the position information solved by INS, p TAN is the location of the prior topographic map.

3. The underwater collaborative navigation and positioning method based on inertial / ultra-short baseline alternating pilotage according to claim 1 is characterized by: Step 2 Follower motion model The motion model is: in They represent attitude error, velocity error, position error, gyro bias and accelerometer bias respectively.

4. The underwater collaborative navigation and positioning method based on inertial / ultra-short baseline alternating pilotage according to claim 1 is characterized by: The observation model of the follower in step 3 is: Among them, r USBL ,α USBL ,β USBL are the slant range and azimuth measured by USBL, They are the slant range and azimuth derived from IMU respectively.

5. The underwater collaborative navigation and positioning method based on inertial / ultra-short baseline alternating pilotage according to claim 4 is characterized in that: Derivation Assuming that the USBL carried by the leader is in the u system, the slant range and azimuth of the transponder carried by the follower in the u system are: Among them, r u =[x u y u z u ] represents the coordinates of the transponder, and the partial differentiation of (1.10) is in, INS position of the pilot INS position with followers The relative position in the earth coordinate system e is p e : The above formula is Taking partial differentials we get: δp e =C e δp n (1.14) in, The arm error between the INS carried by the follower and the transponder carried by it is Therefore, the relative position of the leader's INS and the follower's transponder is: The relative position between the USBL in the leader and the follower transponder is: in The matrix In [L f ,λ f ] T The error matrix expression can be obtained by Taylor expansion. For the convenience of expression, S a =sina,C a =cosa,a=L f or f ; Considering the attitude error between the follower IMU and the transponder, Introducing the error, substituting (1.18) into (1.17) yields: Ignoring the small amount of high-order errors, we can get: From the second term on the right side of formula (1.19) and formula (1.22), we can get in According to formula (1.22) and formula (1.24), we can get Substitute formula (1.25) into formula (1.11) Among them, α true ,β true ,r true are the true values ​​of azimuth and slant range, The azimuth and slant range information measured by the ultra-short baseline system is: Subtracting formula (1.26) from formula (1.27) yields: The observation matrix H = [H φ 0 3×3 H p 0 3×6 ], the measurement noise matrix is ​​V = [ΔαΔβΔD] T .

Images