Underwater mobile platform passive navigation positioning method based on underwater acoustic communication network

Abstract

The invention discloses an underwater mobile platform passive navigation positioning method based on an underwater acoustic communication network, and the method comprises the steps: receiving a data packet transmitted by a fixed communication network node through an underwater mobile platform, and obtaining the position of the fixed node and the packet transmitting time; determining the time when the data packet arrives at the underwater mobile platform, and calculating the time when the data packet is transmitted from the fixed node to the underwater mobile platform; calculating the instantaneous position of the mobile platform based on a data packet transmission time measurement value between the plurality of fixed nodes and the underwater mobile platform, inertial navigation output course information and speedometer output navigational speed information; and calculating the real-time position of the underwater mobile platform based on the calculated instantaneous position of the underwater mobile platform, instantaneous course information output by inertial navigation and instantaneous speed information output by a speedometer. According to the method, the position of the underwater mobile platform is calculated by comprehensively utilizing the information transmission time between the underwater mobile platform and the fixed node, the inertial navigation output course information and the tachymeter output navigational speed information, so that the positioning error caused by platform movement is remarkably reduced, and the navigation positioning precision of the mobile platform is improved.

Description

Technical Field

[0001] The present invention belongs to the technical field of underwater acoustic positioning, and in particular relates to a passive navigation and positioning method for an underwater mobile platform based on an underwater acoustic communication network. Background Art

[0002] With breakthroughs in artificial intelligence, robotics, and new materials, unmanned underwater vehicles (UUVs) have entered a period of rapid development. With advantages such as high cost-effectiveness, excellent stealth, a wide range of combat applications, and high intelligence, UUVs have become a crucial component of future underwater offensive and defensive systems. Maintaining accurate positioning for extended periods in complex underwater environments is essential for these underwater mobile platforms to achieve long-term endurance and multi-mission capabilities. The integration of underwater sensor network communication, navigation, and detection technology is a key development direction in marine information technology. Autonomous navigation and positioning of underwater mobile platforms based on underwater acoustic communication networks is a key component and has attracted widespread attention.

[0003] Traditional navigation and positioning methods for underwater mobile platforms primarily include inertial navigation systems, active navigation and positioning technology, and GPS surface correction. Inertial navigation systems rely on internal platform sensors to measure attitude and acceleration. While they offer excellent short-term navigation capabilities, they suffer from cumulative errors and require regular correction. Active navigation and positioning technology, in which a mobile platform actively transmits signals and calculates its position based on received response signals, offers advantages such as real-time performance and high positioning accuracy. However, its significant disadvantage is that active underwater acoustic communication can easily reveal the platform's location. GPS-based surface correction methods are unsuitable for sustained underwater missions.

[0004] This passive navigation and positioning method for underwater mobile platforms based on an underwater acoustic communication network relies on fixed nodes in the underwater acoustic communication network, combined with the mobile platform's own inertial navigation system and velocimeter, to achieve high-precision positioning of the underwater mobile platform. The entire system achieves positioning without actively transmitting signals, offering the advantages of being discreet, secure, and energy-efficient.

[0005] The solutions currently proposed by scholars at home and abroad mainly include: (1) fusion algorithm based on extended Kalman filtering. Although this method can reduce the error that increases over time to a certain extent, it fails to integrate inertial navigation and underwater acoustic data, and its performance is limited in long-term high-dynamic scenarios; (2) underwater acoustic communication network positioning and navigation technology based on multi-point ranging. Due to the slow propagation speed of sound waves underwater, there are problems of non-co-point and non-synchronous signal reception and transmission during positioning, which becomes one of the main factors affecting the positioning accuracy of the system; (3) prediction algorithm based on machine learning. Although it can adapt to some dynamic changes, it relies on a large amount of training data and lacks real-time performance. Summary of the Invention

[0006] The purpose of the present invention is to provide a passive navigation and positioning method for an underwater mobile platform based on an underwater acoustic communication network.

[0007] The technical solution to achieve the purpose of the present invention is: a passive navigation and positioning method for an underwater mobile platform based on an underwater acoustic communication network, the method comprising the following steps:

[0008] Step 1: The underwater mobile platform receives a data packet sent by a fixed communication network node and obtains the location and packet sending time of the fixed communication network node;

[0009] Step 2: Determine the time when the data packet sent by the fixed communication network node arrives at the underwater mobile platform, and calculate the time it takes for the data packet to be transmitted from the fixed communication network node to the underwater mobile platform;

[0010] Step 3: Calculate the instantaneous position of the underwater mobile platform based on the measured values ​​of data packet transmission time between multiple fixed communication network nodes and the underwater mobile platform, the instantaneous heading information output by the inertial navigation system, and the instantaneous speed information output by the speedometer;

[0011] Step 4: Calculate the real-time position of the underwater mobile platform based on the calculated instantaneous position of the underwater mobile platform, the instantaneous heading information output by the inertial navigation, and the instantaneous speed information output by the speedometer.

[0012] Furthermore, in step 1, the method for obtaining the fixed communication network node position A and the packet sending time t1 is as follows:

[0013] (1-1) The underwater mobile platform receives a data packet sent by a fixed communication network node and obtains the fixed communication network node position A and the packet sending time t1

[0014]

[0015] Where i is the fixed communication network node number, I is the number of fixed communication network nodes, (A x [i],A y [i]) is the horizontal coordinate position of the fixed communication network node i recorded before being deployed into the water, A z [i] is the depth information obtained by the fixed communication network node i through the depth sensor, t1[i] is the time when the fixed communication network node i sends the data packet;

[0016] (1-2) Traverse the recorded data packet data to determine whether there is a fixed communication network node with the same source address. If a duplicate node is detected, delete the old node information, replace the duplicate node position with the new node position A, and update the packet sending time t1; if there is no duplicate node, record the fixed communication network node number i, position A and packet sending time t1.

[0017] Furthermore, in step 2, the time t2 at which the data packet arrives at the underwater mobile platform is determined, and the method for calculating the time Δt taken for the data packet to be transmitted from the fixed communication network node to the underwater mobile platform is as follows:

[0018] (2-1) Record the time t2[i] when the underwater mobile platform receives the data packet from the fixed communication network node i, and record the packet receiving time sequence index k;

[0019] (2-2) Calculate the data packet transmission time

[0020] Δt[i]=t2[i]-t1[i]

[0021] (2-3) Check whether the time delay Δt[i] is reasonable. If not,

[0022] 0<Δt[i]<25

[0023] Then discard the fixed communication network node data packet; if it meets the requirements, record the valid data packet information according to the order of packet receiving time index k.

[0024]

[0025] Among them, k is the packet receiving time sequence index, K is the total number of valid data packets recorded, (B x ,B y ) is the horizontal coordinate position of the fixed communication network node corresponding to the kth valid data packet recorded before being deployed into the water, B z is the depth information obtained by the fixed communication network node corresponding to the kth valid data packet through the depth sensor, t B2 [k] is the moment when the underwater mobile platform receives the kth valid data packet, Δt B [k] is the time taken for the kth valid data packet to be transmitted to the underwater mobile platform.

[0026] Furthermore, in step 3, based on the measured value Δt of the data packet transmission time between multiple fixed communication network nodes and the underwater mobile platform, B The method for calculating the instantaneous position of the underwater mobile platform using the heading information θ output by the inertial navigation and the speed information v output by the speedometer is as follows:

[0027] (3-1) Obtain the time t when the underwater mobile platform receives the kth valid data packet from the fixed communication network node B2 [k], the platform information recorded by the underwater mobile platform, including the depth sensor information Z[p], the inertial navigation output heading information θ[p], and the speed meter output speed information v[p], where p = 1,…,P, where p is the sequential index of the platform information recorded by the underwater mobile platform, and P is the total number of platform information recorded by the underwater mobile platform;

[0028] (3-2) The underwater mobile platform's own data is stored every time interval t d Update once and calculate the time t when the underwater mobile platform receives the data packet from the fixed communication network node B2 [k] The amount of underwater mobile platform information itself

[0029]

[0030] Among them, [·] means taking the integer part;

[0031] (3-3) Calculate the packet receiving time t B2 [k] corresponds to the position change of the underwater mobile platform

[0032]

[0033] Where k = 2,…,K, k is the packet receiving time sequence index, K is the total number of valid data packets received, ΔX[1] = 0, ΔY[1] = 0, ΔZ[1] = Z[1];

[0034] (3-4) Group three consecutively received data packets into a group according to the packet receiving time sequence k, and number the positioning group n, where n = 1, ..., N, where N = K-2 is the total number of positioning groups;

[0035] (3-5) Calculate the three-frame time interval within the positioning group

[0036] τ[n]=t B2 [n+2]-t B2 [n]

[0037] Among them, t B2 [n] is the time when the first data packet is received in positioning group n, t B2 [n+2] is the time when the third data packet is received in positioning group n;

[0038] (3-6) If the three-frame time interval does not meet

[0039] 0≤τ[n]≤3600

[0040] If the positioning group is satisfied, the corresponding fixed communication network node data and underwater mobile platform data in the positioning group are saved in the order of packet receiving time.

[0041]

[0042] Where, C[l]=[C x [l],C y [l],C z [l]] T ,(C x [l],C y[l]) is the horizontal coordinate position of the fixed communication network node corresponding to the lth data packet in positioning group n recorded before being deployed into the water, C z [l] is the depth information obtained by the fixed communication network node corresponding to the lth data packet in the positioning group n through the depth sensor, t C2 [k] is the moment when the underwater mobile platform receives the lth data packet in positioning group n, Δt C [k] is the time taken for the lth data packet in positioning group n to be transmitted to the underwater mobile platform, and [ΔX'[l], ΔY'[l], ΔZ'[l]] is the position change of the underwater mobile platform at the moment of receiving the lth data packet in positioning group n;

[0043] (3-7) Calculate the change in the position of the underwater mobile platform in the positioning group when it receives the three fixed communication network node data packets relative to the position of the first fixed communication network node data packet received in the positioning group

[0044]

[0045] Among them, (ΔS x [l],ΔS y [l]) is the horizontal change, ΔS x [l] is the depth change;

[0046] (3-8) The instantaneous position of the underwater mobile platform is calculated using the weighted least squares method, which specifically includes the following steps:

[0047] (3-8-1) Construct coefficient matrix C lsm :

[0048]

[0049] (3-8-2) Construct coefficient matrix D lsm :

[0050]

[0051] Where Δx[j]=(C x [j]-ΔS x [j]) 2 -C x [1] 2 , Δy[j]=(C y [j]-ΔS y [j]) 2 -C y [1] 2 , ΔZ[j]=(C z [j]-ΔS z [j]-ΔZ'[1]) 2 -(C z[j]-ΔZ'[1]) 2 , Δd[j]=c 2 ·(Δt C [j]-Δt C [1]) 2 , j = 2, 3, c is the propagation speed of sound waves in seawater;

[0052] (3-8-3) Calculate the instantaneous position S of the underwater mobile platform

[0053] S=[C lsm T C lsm ] -1 C lsm T D lsm

[0054] Among them, C lsm T C lsm The transposed matrix, [C lsm T C lsm ] -1 C lsm T C lsm The inverse matrix, S = [S x ,S y ,S z ] T It is the instantaneous position coordinate of the underwater mobile platform calculated based on the positioning group data.

[0055] Furthermore, in step 4, the real-time position of the underwater mobile platform is calculated based on the calculated instantaneous position S of the underwater mobile platform, the instantaneous heading information θ output by the inertial navigation system, and the instantaneous speed information V output by the speedometer, including the following steps:

[0056] (4-1) According to the estimated time t, the time t at which the third data packet in the positioning group is received C2 [3] and the underwater mobile platform’s own data update time interval t d , calculate the number of data intervals N between the estimated time and the time when the third data packet in the positioning group is received by the underwater mobile platform itself d :

[0057]

[0058] Among them, [·] means rounding to the nearest integer;

[0059] (4-2) According to the instantaneous heading information θ[p] output by the underwater mobile platform inertial navigation, the instantaneous speed information v[p] output by the speedometer, and the time interval t d, calculate the instantaneous position coordinate of the underwater mobile platform relative to the moment t when the third data packet in the positioning group is received C2 [1] The horizontal position change (ΔM x ,ΔM y ):

[0060]

[0061] (4-3) Calculate the real-time position of the underwater mobile platform based on the calculated instantaneous position S, the instantaneous heading information θ output by the inertial navigation system, and the instantaneous speed information v output by the speedometer.

[0062]

[0063] Among them, t is the real-time position corresponding to the underwater mobile platform, (S x ,S y ) is the calculated horizontal coordinate of the instantaneous position of the underwater mobile platform, and Z(t) is the depth sensor data of the underwater mobile platform at time t.

[0064] A passive navigation and positioning system for an underwater mobile platform based on an underwater acoustic communication network implements the passive navigation and positioning method for an underwater mobile platform based on an underwater acoustic communication network, and realizes the passive navigation and positioning method for an underwater mobile platform based on an underwater acoustic communication network. The system is divided into four modules, and steps 1 to 4 are executed respectively.

[0065] A computer device includes a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, the method for passive navigation and positioning of an underwater mobile platform based on an underwater acoustic communication network is implemented to realize the method for passive navigation and positioning of an underwater mobile platform based on an underwater acoustic communication network.

[0066] A computer-readable storage medium stores a computer program. When the computer program is executed by a processor, the method for passive navigation and positioning of an underwater mobile platform based on an underwater acoustic communication network is implemented to realize the method for passive navigation and positioning of an underwater mobile platform based on an underwater acoustic communication network.

[0067] Compared with the prior art, the present invention has the following significant advantages: the underwater mobile platform of the present invention receives data packets sent by multiple fixed nodes of the underwater acoustic communication network, and uses least squares multilateral positioning based on the acquired fixed node positions and the information transmission time between the fixed nodes and the mobile platform to obtain an underwater acoustic positioning result without time accumulation error. At the same time, the position of the mobile platform is solved by comprehensively utilizing the information transmission time between the mobile platform and the fixed nodes, the heading information output by the inertial navigation, and the speed information output by the speedometer, thereby significantly reducing the positioning error caused by the platform movement and improving the navigation positioning accuracy of the mobile platform. BRIEF DESCRIPTION OF THE DRAWINGS

[0068] Figure 1 Schematic diagram of the process of the present invention;

[0069] Figure 2 The fixed node position distribution of the simulated communication network in Example 1 and Example 2;

[0070] Figure 3 The actual motion trajectory of the simulated underwater mobile platform in Example 1 and Example 2;

[0071] Figure 4 This is a comparison diagram of the estimated trajectory and the actual trajectory of the underwater mobile platform in the case of no error in the simulation in Example 1;

[0072] Figure 5 This is a comparison diagram of the estimated trajectory and the actual trajectory of the underwater mobile platform in the case of simulation error in Example 2;

[0073] Figure 6 This is a diagram showing the error between the estimated position and the actual position of the underwater mobile platform in the simulation with error in Example 2. DETAILED DESCRIPTION

[0074] In order to make the purpose, technical solutions and advantages of this application more clear, the following further describes this application in detail with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain this application and are not intended to limit this application.

[0075] like Figure 1 As shown, a passive navigation and positioning method for an underwater mobile platform based on an underwater acoustic communication network includes the following steps:

[0076] Step 1: The underwater mobile platform receives a data packet sent by a fixed communication network node and obtains the location and packet sending time of the fixed communication network node;

[0077] Step 2: Determine the time when the data packet sent by the fixed communication network node arrives at the underwater mobile platform, and calculate the time it takes for the data packet to be transmitted from the fixed communication network node to the underwater mobile platform;

[0078] Step 3: Calculate the instantaneous position of the underwater mobile platform based on the measured values ​​of data packet transmission time between multiple fixed communication network nodes and the underwater mobile platform, the instantaneous heading information output by the inertial navigation system, and the instantaneous speed information output by the speedometer;

[0079] Step 4: Calculate the real-time position of the underwater mobile platform based on the calculated instantaneous position of the underwater mobile platform, the instantaneous heading information output by the inertial navigation, and the instantaneous speed information output by the speedometer.

[0080] In step 1, the method for obtaining the fixed communication network node position A, the packet sending time t1, and the packet receiving time sequence index k is as follows:

[0081] (1-1) The underwater mobile platform receives a data packet sent by a fixed communication network node and obtains the fixed communication network node position A and the packet sending time t1

[0082]

[0083] Where i is the fixed communication network node number, I is the number of fixed communication network nodes, (A x [i],A y [i]) is the horizontal coordinate position of the fixed communication network node recorded before being deployed in water, A z [i] is the depth information obtained by the fixed communication network node through the depth sensor, t1 is the time when the fixed communication network node sends the data packet;

[0084] (1-2) Traverse the recorded data packet data to determine whether there is a fixed communication network node with the same source address. If a duplicate node is detected, delete the old node information, replace the duplicate node position with the new node position A, and update the packet sending time t1; if there is no duplicate node, record the fixed communication network node number i, position A and packet sending time t1.

[0085] In step (2), the time t2 when the data packet arrives at the underwater mobile platform is determined, and the method for calculating the time Δt taken for the data packet to be transmitted from the fixed communication network node to the underwater mobile platform is as follows:

[0086] (2-1) Record the time t2[i] when the underwater mobile platform receives the data packet from the fixed communication network node i, and record the packet receiving time sequence index k;

[0087] (2-2) Calculate the data packet transmission time

[0088] Δt[i]=t2[i]-t1[i]

[0089] (2-3) Check whether the time delay Δt[i] is reasonable. If not,

[0090] 0<Δt[i]<25

[0091] Then discard the fixed communication network node data packet; if it meets the requirements, record the valid data packet information according to the order of packet receiving time index k.

[0092]

[0093] Among them, k is the packet receiving time sequence index, K is the total number of valid data packets recorded, (B x ,B y) is the horizontal coordinate position of the fixed communication network node corresponding to the kth valid data packet recorded before being deployed into the water, B z is the depth information obtained by the fixed communication network node corresponding to the kth valid data packet through the depth sensor, t B2 [k] is the moment when the underwater mobile platform receives the kth valid data packet, Δt B [k] is the time taken for the kth valid data packet to be transmitted to the underwater mobile platform.

[0094] In step 3, the method for calculating the instantaneous position S of the underwater mobile platform based on the measured value Δt of the data packet transmission time between multiple fixed communication network nodes and the underwater mobile platform, the heading information θ output by the inertial navigation system, and the speed information v output by the speedometer is as follows:

[0095] (3-1) Obtain the time t when the underwater mobile platform receives the kth valid data packet from the fixed communication network node B2 [k], the platform information recorded by the underwater mobile platform, including the depth sensor information Z[p], the inertial navigation output heading information θ[p], and the speed meter output speed information v[p], where p = 1,…,P, where p is the sequential index of the platform information recorded by the underwater mobile platform, and P is the total number of platform information recorded by the underwater mobile platform;

[0096] (3-2) The underwater mobile platform's own data is stored every time interval t d Update once and calculate the time t when the underwater mobile platform receives the data packet from the fixed communication network node B2 [k] The amount of underwater mobile platform information itself

[0097]

[0098] Among them, [·] means taking the integer part;

[0099] (3-3) Calculate the packet receiving time t B2 [k] corresponds to the position change of the underwater mobile platform

[0100]

[0101] Where k = 2,…,K, k is the packet receiving time sequence index, K is the total number of valid data packets received, ΔX[1] = 0, ΔY[1] = 0, ΔZ[1] = Z[1];

[0102] (3-4) Group three consecutively received data packets into a group according to the packet receiving time sequence k, and number the positioning group n, where n = 1, ..., N, where N = K-2 is the total number of positioning groups;

[0103] (3-5) Calculate the three-frame time interval within the positioning group

[0104] τ[n]=t B2 [n+2]-t B2 [n]

[0105] Among them, t B2 [n] is the time when the first data packet is received in positioning group n, t B2 [n+2] is the time when the third data packet is received in positioning group n;

[0106] (3-6) If the three-frame time interval does not meet

[0107] 0≤τ[n]≤3600

[0108] If the positioning group is satisfied, the corresponding fixed communication network node data and underwater mobile platform data in the positioning group are saved in the order of packet receiving time.

[0109]

[0110] Where, C[l]=[C x [l],C y [l],C z [l]] T ,(C x [l],C y [l]) is the horizontal coordinate position of the fixed communication network node corresponding to the lth data packet in positioning group n recorded before being deployed into the water, C z [l] is the depth information obtained by the fixed communication network node corresponding to the lth data packet in the positioning group n through the depth sensor, t C2 [k] is the moment when the underwater mobile platform receives the lth data packet in positioning group n, Δt C [k] is the time taken for the lth data packet in positioning group n to be transmitted to the underwater mobile platform, and [ΔX'[l], ΔY'[l], ΔZ'[l]] is the position change of the underwater mobile platform at the moment of receiving the lth data packet in positioning group n;

[0111] (3-7) Calculate the change in the position of the underwater mobile platform in the positioning group when it receives the three fixed communication network node data packets relative to the position of the first fixed communication network node data packet received in the positioning group

[0112]

[0113] Among them, (ΔS x [l],ΔS y [l]) is the horizontal change, ΔS x [l] is the depth change;

[0114] (3-8) The instantaneous position of the underwater mobile platform is calculated using the weighted least squares method, which specifically includes the following steps:

[0115] (3-8-1) Construct coefficient matrix C lsm :

[0116]

[0117] (3-8-2) Construct coefficient matrix D lsm :

[0118]

[0119] Where Δx[j]=(C x [j]-ΔS x [j]) 2 -C x [1] 2 , Δy[j]=(C y [j]-ΔS y [j]) 2 -C y [1] 2 , Δz[j]=(C z [j]-ΔS z [j]-ΔZ'[1]) 2 -(C z [j]-ΔZ'[1]) 2 , Δd[j]=c 2 ·(Δt C [j]-Δt C [1]) 2 , j = 2, 3, c is the propagation speed of sound waves in seawater;

[0120] (3-8-3) Calculate the instantaneous position S of the underwater mobile platform

[0121] S=[C lsm T C lsm ] -1 C lsm T D lsm

[0122] Among them, C lsm T C lsm The transposed matrix, [C lsm T C lsm ] -1 C lsm T C lsm The inverse matrix, S = [S x ,S y ,S z ] TIt is the instantaneous position coordinate of the underwater mobile platform calculated based on the positioning group data.

[0123] Among them, in step 4, the real-time position of the underwater mobile platform is calculated based on the calculated instantaneous position S of the underwater mobile platform, the instantaneous heading information θ output by the inertial navigation, and the instantaneous speed information v output by the speedometer, including the following steps:

[0124] (4-1) According to the estimated time t, the time t at which the third data packet in the positioning group is received C2 [3] and the underwater mobile platform’s own data update time interval t d , calculate the number of data intervals N between the estimated time and the time when the third data packet in the positioning group is received by the underwater mobile platform itself d :

[0125]

[0126] Among them, [·] means rounding to the nearest integer;

[0127] (4-2) According to the instantaneous heading information θ[p] output by the underwater mobile platform inertial navigation, the instantaneous speed information v[p] output by the speedometer, and the time interval t d , calculate the instantaneous position coordinate of the underwater mobile platform relative to the moment t when the third data packet in the positioning group is received C2 [1] The horizontal position change (ΔM x ,ΔM y ):

[0128]

[0129] (4-3) Calculate the real-time position of the underwater mobile platform based on the calculated instantaneous position S, the instantaneous heading information θ output by the inertial navigation system, and the instantaneous speed information v output by the speedometer.

[0130]

[0131] Among them, t is the real-time position corresponding to the underwater mobile platform, (S x ,S y ) is the calculated horizontal coordinate of the instantaneous position of the underwater mobile platform, and Z(t) is the depth sensor data of the underwater mobile platform at time t.

[0132] The present invention also proposes a passive navigation and positioning system for an underwater mobile platform based on an underwater acoustic communication network, implements the passive navigation and positioning method for an underwater mobile platform based on an underwater acoustic communication network, and realizes the passive navigation and positioning method for an underwater mobile platform based on an underwater acoustic communication network. The system is divided into four modules, and steps 1 to 4 are executed respectively.

[0133] A computer device includes a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, the method for passive navigation and positioning of an underwater mobile platform based on an underwater acoustic communication network is implemented to realize the method for passive navigation and positioning of an underwater mobile platform based on an underwater acoustic communication network.

[0134] A computer-readable storage medium stores a computer program. When the computer program is executed by a processor, the method for passive navigation and positioning of an underwater mobile platform based on an underwater acoustic communication network is implemented to realize the method for passive navigation and positioning of an underwater mobile platform based on an underwater acoustic communication network.

[0135] Two embodiments are set up to analyze the navigation and positioning results of the passive navigation and positioning method of an underwater mobile platform based on an underwater acoustic communication network with and without errors. Example 1 analyzes the correctness and feasibility of the present invention in the absence of error interference, and Example 2 analyzes the navigation and positioning accuracy of the present invention in the presence of error interference.

[0136] Example 1

[0137] In the first embodiment of the present invention, four fixed nodes are set in the simulation, and the underwater mobile platform moves within the range of the communication network composed of the four fixed nodes.

[0138] The simulation signal parameters are set as follows:,The four fixed nodes are located at (-5000,-5000,-1000), (-5000,5000,-1000), (5000,5000,-1000), (5000,-5000,-1000), and (5000,-5000,-1000), and send data packets in sequence at an interval of 30s for 270s, with a total of 9 data packets;

[0139] The motion model L of the underwater mobile platform moving in a uniform linear motion is:

[0140]

[0141] Among them, (x0, y0, z0) is the initial position of the underwater mobile platform, v is the horizontal speed of the underwater mobile platform, θ is the heading angle of the underwater mobile platform, and v z It is the vertical speed of the underwater mobile platform.

[0142] Initial underwater mobile platform position (x0, y0, z0) = (-3000, -4000, -200), underwater mobile platform horizontal speed V = 6kn = 3.08m / s, underwater mobile platform heading angle θ = 53°, underwater mobile platform vertical speed v z =1kn=0.51m / s, the time interval t for the underwater mobile platform to obtain platform information d= 10s, the propagation speed of sound waves underwater is c = 1500m / s; the positions of fixed nodes in the communication network are set as Figure 2 As shown, the actual moving trajectory of the underwater mobile platform is as follows Figure 3 shown.

[0143] In step (1), the obtained fixed communication network node number i, location A, packet sending time t1 and time sequence index k are:

[0144] i <![CDATA[A x [i]]]> <![CDATA[A y [i]]]> <![CDATA[A z [i]]]> <![CDATA[t1[i] <!-- 9 -->]]> 1 -5000 -5000 -1000 30 2 -5000 5000 -1000 60 3 5000 5000 -1000 90 4 5000 -5000 -1000 120 1 -5000 -5000 -1000 150 2 -5000 5000 -1000 180 3 5000 5000 -1000 210 4 5000 -5000 -1000 240 1 -5000 -5000 -1000 270

[0145] In step (2), determine the time t when the data packet arrives at the underwater mobile platform B2 [k], calculate the time Δt taken by the data packet to be transmitted from the fixed node to the underwater mobile platform B2 [k], the obtained packet receiving time sequence index k, the fixed communication network node position B[k] is:

[0146] k <![CDATA[B x [k]]]> <![CDATA[B y [k]]]> <![CDATA[B z [k]]]> <![CDATA[t B2 [k]]]> <![CDATA[Δt B2 [k]]]> 1 -5000 -5000 -1000 31.6323 1.6323 2 -5000 5000 -1000 66.0886 6.0886 3 5000 5000 -1000 97.8592 7.8592 4 5000 -5000 -1000 125.2792 5.2792 5 -5000 -5000 -1000 151.8368 1.8368 6 -5000 5000 -1000 185.9314 5.9314 7 5000 5000 -1000 217.6111 7.6111 8 5000 -5000 -1000 245.1664 5.1664 9 -5000 -5000 -1000 272.0514 2.0514

[0147] In step (3), the time t at which the underwater mobile platform receives the fixed communication network node data packet is obtained. B2 At time [k], the depth sensor information Z[p], the heading information θ[p] output by the inertial navigation system, and the speed information v[p] output by the speedometer recorded by the underwater mobile platform are:

[0148] p Z[p] θ[p] v[p] 1 -210.29 53 3.08 2 -212.86 53 3.08 3 -215.43 53 3.08 4 -220.58 53 3.08 5 -225.72 53 3.08 6 -230.87 53 3.08 7 -236.02 53 3.08 8 -241.16 53 3.08 9 -246.30 53 3.08 10 -251.45 53 3.08 11 -256.59 53 3.08 12 -261.73 53 3.08 13 -266.88 53 3.08 14 -272.02 53 3.08 15 -277.17 53 3.08 16 -282.31 53 3.08 17 -287.45 53 3.08 18 -292.60 53 3.08 19 -297.74 53 3.08 20 -302.88 53 3.08 21 -308.03 53 3.08 22 -313.17 53 3.08 23 -318.32 53 3.08 24 -323.47 53 3.08 25 -328.61 53 3.08 26 -333.75 53 3.08 27 -338.90 53 3.08

[0149] The underwater mobile platform receives the data packet from the fixed communication network node at time t B2 [k] The amount of underwater mobile platform information itself:

[0150] k <![CDATA[t B2 [k]]]> <![CDATA[N k [k]]]> 1 31.6323 3 2 66.0886 6 3 97.8592 9 4 125.2792 12 5 151.8368 15 6 185.9314 18 7 217.6111 21 8 245.1664 24 9 272.0514 27

[0151] Based on the above information, calculate the position change of the underwater mobile platform:

[0152] k ΔX[l] ΔY[l] ΔZ[l] 1 0.00 0.00 -215.43 2 55.73 73.95 -230.87 3 111.46 147.91 -246.30 4 167.18 221.86 -261.73 5 222.91 295.81 -277.17 6 278.64 369.77 -292.60 7 334.37 443.72 -308.03 8 390.10 517.68 -323.47 9 445.82 591.63 -338.90

[0153] The data after positioning grouping is:

[0154]

[0155]

[0156] Calculate the position change of the underwater mobile platform when it receives information from three fixed nodes:

[0157]

[0158]

[0159] The instantaneous position of the underwater mobile platform is calculated using the weighted least squares method:

[0160] n <![CDATA[S x [n]]]> <![CDATA[S y [n]]]> <![CDATA[S z [n]]]> 1 -2944.27 -3926.05 -215.43 2 -2888.54 -3852.09 -230.87 3 -2832.82 -3778.14 -246.30 4 -2777.09 -3704.19 -261.73 5 -2721.36 -3630.23 -277.17 6 -2665.63 -3556.28 -292.60 7 -2609.90 -3482.32 -308.03

[0161] The actual position T is:

[0162]

[0163]

[0164] The root mean square error between the calculated instantaneous position S and the actual position T = (ΔX, ΔY, ΔZ) is:

[0165]

[0166] In step (4), assuming that the estimated time is t=305, according to t C2 [1] = 214.3492, t C2 [3] = 276.3086, find the number of intervals N d =3

[0167] The instantaneous heading information θ output by the inertial navigation system and the instantaneous speed information v output by the speedometer are:

[0168] p θ[p] v[p] 21 53 3.08 22 53 3.08 23 53 3.08 24 53 3.08 25 53 3.08 26 53 3.08 27 53 3.08 28 53 3.08 29 53 3.08 30 53 3.08

[0169] The horizontal position change of the underwater mobile platform is:

[0170] ΔM x =167.18,ΔM y =221.86

[0171] Let the estimated position M of the underwater mobile platform at time t be:

[0172] M(t)=(-2442.72,-3248.13,-356.91)

[0173] The actual position L of the underwater mobile platform at time t is:

[0174] L(t)=(-2442.72,-3248.13,-356.91)

[0175] The root mean square error between the estimated position and the actual position is:

[0176] Error=(ML)=(0,0,0)

[0177] The comparison of the moving trajectory of the estimated position S and the actual position T is as follows: Figure 4As shown in the figure, it can be seen that the passive navigation and positioning method of the underwater mobile platform can correctly calculate the target position, which proves the correctness and feasibility of the algorithm.

[0178] Example 2

[0179] In the second embodiment of the present invention, four fixed nodes are set in the simulation, and the underwater mobile platform moves within the communication network composed of the four fixed nodes.

[0180] The simulation signal parameters are set as follows: four fixed nodes are located at (-5000, -5000, -1000), (-5000, 5000, -1000), (5000, 5000, -1000), and (5000, -5000, -1000), and send data packets in sequence at an interval of 30 seconds for 270 seconds, with a total of 9 data packets.

[0181] The motion model L of the underwater mobile platform moving in a uniform linear motion is:

[0182]

[0183] Among them, (x0, y0, z0) is the initial position of the underwater mobile platform, v is the horizontal speed of the underwater mobile platform, θ is the heading angle of the underwater mobile platform, and v z It is the vertical speed of the underwater mobile platform.

[0184] Initial underwater mobile platform position (x0, y0, z0) = (-3000, -4000, -200), underwater mobile platform horizontal speed V = 6kn = 3.08m / s, underwater mobile platform heading angle θ = 53°, underwater mobile platform vertical speed v z =1kn=0.51m / s, the time interval t for the underwater mobile platform to obtain platform information d = 10s, the speed of sound wave propagation underwater is c = 1500m / s; the circular probability error of the fixed node position is set to 200m, the delay measurement error is a Gaussian distribution with a mean of 0 and a variance of 0.1ms, and the sound speed measurement error is 5m / s; the position of the fixed node in the communication network is set as Figure 2 As shown, the actual moving trajectory of the underwater mobile platform is as follows Figure 3 shown.

[0185] In step (1), the obtained fixed communication network node number i, location A, packet sending time t1 and time sequence index k are:

[0186] i <![CDATA[A x [i]]]> <![CDATA[A y [i]]]> <![CDATA[A z [i]]]> <![CDATA[t1[i]]]> 1 -5144.25 -4929.20 -1000.00 30 2 -5099.40 4964.64 -1000.00 60 3 4983.49 4982.83 -1000.00 90 4 4983.49 -5017.17 -1000.00 120 1 -5016.51 -5017.17 -1000.00 150 2 -5016.51 4982.83 -1000.00 180 3 4983.49 4982.83 -1000.00 210 4 4983.49 -5017.17 -1000.00 240 1 -5016.51 -5017.17 -1000.00 270

[0187] In step (2), determine the time t when the data packet arrives at the underwater mobile platform B2[k], calculate the time Δt taken by the data packet to be transmitted from the fixed node to the underwater mobile platform B2 [k], the obtained packet receiving time sequence index k, the fixed communication network node position B[k] is:

[0188] k <![CDATA[B x [k]]]> <![CDATA[B y [k]]]> <![CDATA[B z [k]]]> <![CDATA[t B2 [k]]]> <![CDATA[Δt B2 [k]]]> 1 -5144.25 -4929.20 -1000.00 31.7006 1.7006 2 -5099.40 4964.64 -1000.00 66.0778 6.0778 3 4983.49 4982.83 -1000.00 97.8463 7.8463 4 4983.49 -5017.17 -1000.00 125.2656 5.2656 5 -5016.51 -5017.17 -1000.00 151.8781 1.8781 6 -5016.51 4982.83 -1000.00 185.9008 5.9008 7 4983.49 4982.83 -1000.00 217.5546 7.5546 8 4983.49 -5017.17 -1000.00 245.1360 5.1360 9 -5016.51 -5017.17 -1000.00 272.1337 2.1337

[0189] In step (3), the time t at which the underwater mobile platform receives the fixed communication network node data packet is obtained. B2 At time [k], the depth sensor information Z[p], the heading information θ[p] output by the inertial navigation system, and the speed information v[p] output by the speedometer recorded by the underwater mobile platform are:

[0190]

[0191]

[0192] The underwater mobile platform receives the data packet from the fixed communication network node at time t B2 [k] The amount of underwater mobile platform information itself:

[0193] k <![CDATA[t B2 [k]]]> <![CDATA[N k [k]]]> 1 31.6323 3 2 66.0886 6 3 97.8592 9 4 125.2792 12 5 151.8368 15 6 185.9314 18 7 217.6111 21 8 245.1664 24 9 272.0514 27

[0194] Based on the above information, calculate the position change of the underwater mobile platform:

[0195]

[0196]

[0197] The data after positioning grouping is:

[0198]

[0199]

[0200] Calculate the position change of the underwater mobile platform when it receives information from three fixed nodes:

[0201]

[0202] The instantaneous position of the underwater mobile platform is calculated using the weighted least squares method:

[0203]

[0204]

[0205] The actual position T is:

[0206] n <![CDATA[T x [n]]]> <![CDATA[T y [n]]]> <![CDATA[T z [n]]]> 1 -2944.27 -3926.05 -215.43 2 -2888.54 -3852.09 -230.87 3 -2832.82 -3778.14 -246.30 4 -2777.09 -3704.19 -261.73 5 -2721.36 -3630.23 -277.17 6 -2665.63 -3556.28 -292.60 7 -2609.90 -3482.32 -308.03

[0207] The root mean square error between the calculated instantaneous position S and the actual position T = (ΔX, ΔY, ΔZ) is:

[0208]

[0209] In step (4), assuming that the estimated time is t=305, according to t C2 [1] = 214.3492, t C2 [3] = 276.3086, find the number of intervals N d =3

[0210] The instantaneous heading information θ output by the inertial navigation system and the instantaneous speed information v output by the speedometer are:

[0211]

[0212]

[0213] The horizontal position change of the underwater mobile platform is:

[0214] ΔM x =167.18,ΔM y =221.86

[0215] Let the estimated position M of the underwater mobile platform at time t be:

[0216] M(t)=(-2379.13,-3167.79,-356.91)

[0217] The actual position L of the underwater mobile platform at time t is:

[0218] L(t)=(-2433.43,-3248.14,-356.91)

[0219] The root mean square error between the estimated position and the actual position is:

[0220] Error=(ML)=(54.30,80.35,0)

[0221] The error between the estimated position and the actual position is as follows Figure 6 As shown in the figure, it can be seen that the passive navigation and positioning method of the underwater mobile platform still has a high navigation and positioning accuracy under error interference, and can accurately calculate the position of the underwater mobile platform in real time based on the underwater acoustic positioning results and the underwater mobile platform's own data.

[0222] The technical features of the above embodiments can be combined arbitrarily. To make the description concise, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0223] The above-described embodiments merely represent several implementation methods of the present application. While the descriptions are relatively specific and detailed, they should not be construed as limiting the scope of the present application. It should be noted that a person of ordinary skill in the art may make various modifications and improvements without departing from the spirit of the present application, and these modifications and improvements fall within the scope of protection of the present application. Therefore, the scope of protection of the present application shall be determined by the appended claims.

Claims

1. A passive navigation and positioning method for an underwater mobile platform based on an underwater acoustic communication network, characterized in that: The method comprises the following steps: Step 1: The underwater mobile platform receives a data packet sent by a fixed communication network node and obtains the location and packet sending time of the fixed communication network node; Step 2: Determine the time when the data packet sent by the fixed communication network node arrives at the underwater mobile platform, and calculate the time it takes for the data packet to be transmitted from the fixed communication network node to the underwater mobile platform; Step 3: Calculate the instantaneous position of the underwater mobile platform based on the measured values ​​of data packet transmission time between multiple fixed communication network nodes and the underwater mobile platform, the instantaneous heading information output by the inertial navigation system, and the instantaneous speed information output by the speedometer; Step 4: Calculate the real-time position of the underwater mobile platform based on the calculated instantaneous position of the underwater mobile platform, the instantaneous heading information output by the inertial navigation, and the instantaneous speed information output by the speedometer.

2. The passive navigation and positioning method for an underwater mobile platform based on an underwater acoustic communication network according to claim 1, characterized in that: In step 1, the method for obtaining the fixed communication network node location A and the packet sending time t1 is as follows: (1-1) The underwater mobile platform receives a data packet sent by a fixed communication network node and obtains the fixed communication network node position A and the packet sending time t1 Where i is the fixed communication network node number, I is the number of fixed communication network nodes, (A x [i],A y [i]) is the horizontal coordinate position of the fixed communication network node i recorded before being deployed into the water, A z [i] is the depth information obtained by the fixed communication network node i through the depth sensor, t1[i] is the time when the fixed communication network node i sends the data packet; (1-2) Traverse the recorded data packet data to determine whether there is a fixed communication network node with the same source address. If a duplicate node is detected, delete the old node information, replace the duplicate node position with the new node position A, and update the packet sending time t1; if there is no duplicate node, record the fixed communication network node number i, position A and packet sending time t1.

3. The passive navigation and positioning method for an underwater mobile platform based on an underwater acoustic communication network according to claim 2, characterized in that: In step 2, the time t2 when the data packet arrives at the underwater mobile platform is determined, and the method for calculating the time Δt taken for the data packet to be transmitted from the fixed communication network node to the underwater mobile platform is as follows: (2-1) Record the time r2[i] when the underwater mobile platform receives the data packet from the fixed communication network node i, and record the time sequence index k of the packet reception; (2-2) Calculate the data packet transmission time Δt[i]=t2[i]-t1[i] (2-3) Check whether the time delay Δt[i] is reasonable. If not, 0<Δt[i]<25 Then discard the fixed communication network node data packet; if it meets the requirements, record the valid data packet information according to the order of packet receiving time index k. Among them, k is the packet receiving time sequence index, K is the total number of valid data packets recorded, (B x ,B y ) is the horizontal coordinate position of the fixed communication network node corresponding to the kth valid data packet recorded before being deployed into the water, B z is the depth information obtained by the fixed communication network node corresponding to the kth valid data packet through the depth sensor, t B2 [k] is the moment when the underwater mobile platform receives the kth valid data packet, Δt B [k] is the time taken for the kth valid data packet to be transmitted to the underwater mobile platform.

4. The passive navigation and positioning method for an underwater mobile platform based on an underwater acoustic communication network according to claim 3, characterized in that: In step 3, based on the measured value Δt of the data packet transmission time between multiple fixed communication network nodes and the underwater mobile platform B The method for calculating the instantaneous position of the underwater mobile platform using the heading information θ output by the inertial navigation and the speed information v output by the speedometer is as follows: (3-1) Obtaining the platform information recorded by the underwater mobile platform when the underwater mobile platform receives the kth valid data packet from the fixed communication network node, including the depth sensor information Z[p], the inertial navigation output heading information θ[p], and the speed meter output speed information v[p], where p = 1,…,P, where p is the sequential index of the platform information recorded by the underwater mobile platform, and P is the total number of platform information recorded by the underwater mobile platform; (3-2) The underwater mobile platform's own data is stored every time interval t d Update once and calculate the time t when the underwater mobile platform receives the data packet from the fixed communication network node B2 [k] The amount of underwater mobile platform's own information Among them, [·] means taking the integer part; (3-3) Calculate the packet receiving time t B2 [k] corresponds to the position change of the underwater mobile platform Where k = 2,…,K, k is the packet receiving time sequence index, K is the total number of valid data packets received, ΔX[1] = 0, ΔY[1] = 0, ΔZ[1] = Z[1]; (3-4) Group three consecutively received data packets into a group according to the packet receiving time sequence k, and number the positioning group n, where n = 1, ..., N, where N = K-2 is the total number of positioning groups; (3-5) Calculate the three-frame time interval within the positioning group τ[n]=t B2 [n+2]-t B2 [n] Among them, t B2 [n] is the time when the first data packet is received in positioning group n, t B2 [n+2] is the time when the third data packet is received in positioning group n; (3-6) If the three-frame time interval does not meet 0≤τ[n]≤3600 If the positioning group is satisfied, the corresponding fixed communication network node data and underwater mobile platform data in the positioning group are saved in the order of packet receiving time. Where, C[l]=[C x [l],C y [l],C z [l]] T ,(C x [l],C y [l]) is the horizontal coordinate position of the fixed communication network node corresponding to the lth data packet in positioning group n recorded before being deployed into the water, C z [l] is the depth information obtained by the fixed communication network node corresponding to the lth data packet in the positioning group n through the depth sensor, t C2 [n] is the moment when the underwater mobile platform receives the lth data packet in positioning group n, Δt C [k] is the time taken for the lth data packet in positioning group n to be transmitted to the underwater mobile platform, and [ΔX'[l], ΔY'[l], ΔZ'[l]] is the position change of the underwater mobile platform at the moment of receiving the lth data packet in positioning group n; (3-7) Calculate the change in the position of the underwater mobile platform in the positioning group when it receives the three fixed communication network node data packets relative to the position of the first fixed communication network node data packet received in the positioning group Among them, (ΔS x [l],ΔS y [l]) is the horizontal change, ΔS x [l] is the depth change; (3-8) The instantaneous position of the underwater mobile platform is calculated using the weighted least squares method, which specifically includes the following steps: (3-8-1) Construct coefficient matrix C lsm : (3-8-2) Construct coefficient matrix D lsm : Where Δx[j]=(C x [j]-ΔS x [j]) 2 -C x [1] 2 , Δy[j]=(C y [j]-ΔS y [j]) 2 -C y [1] 2 , Δz[j]=(C z [j]-ΔS z [j]-ΔZ'[1]) 2 -(C z [j]-ΔZ'[1]) 2 , Δd[j]=c 2 ·(Δt C [j]-Δt C [1]) 2 , j = 2, 3, c is the propagation speed of sound waves in seawater; (3-8-3) Calculate the instantaneous position S of the underwater mobile platform S=[C lsm T C lsm ] -1 C lsm T D lsm Among them, C lsm T C lsm The transposed matrix, [C lsm T C lsm ] -1 C lsm T C lsm The inverse matrix, S = [S x ,S y ,S z ] T It is the instantaneous position coordinate of the underwater mobile platform calculated based on the positioning group data.

5. The passive navigation and positioning method for an underwater mobile platform based on an underwater acoustic communication network according to claim 4, characterized in that: In step 4, the real-time position of the underwater mobile platform is calculated based on the calculated instantaneous position S of the underwater mobile platform, the instantaneous heading information θ output by the inertial navigation, and the instantaneous speed information V output by the speedometer, including the following steps: (4-1) According to the estimated time t, the time t at which the third data packet in the positioning group is received C2 [3] and the underwater mobile platform’s own data update time interval t d , calculate the number of data intervals N between the estimated time and the time when the third data packet in the positioning group is received by the underwater mobile platform itself d : Among them, [·] means rounding to the nearest integer; (4-2) According to the instantaneous heading information θ[p] output by the underwater mobile platform inertial navigation, the instantaneous speed information v[p] output by the speedometer, and the time interval t d , calculate the instantaneous position coordinate of the underwater mobile platform relative to the moment t when the third data packet in the positioning group is received C2 [1] The horizontal position change (ΔM x ,ΔM y ): (4-3) Calculate the real-time position of the underwater mobile platform based on the calculated instantaneous position S, the instantaneous heading information θ output by the inertial navigation system, and the instantaneous speed information v output by the speedometer. Among them, t is the real-time position corresponding to the underwater mobile platform, (S x ,S y ) is the calculated horizontal coordinate of the instantaneous position of the underwater mobile platform, and Z(t) is the depth sensor data of the underwater mobile platform at time t.

6. A passive navigation and positioning system for an underwater mobile platform based on an underwater acoustic communication network, characterized in that: Implement the passive navigation and positioning method for an underwater mobile platform based on an underwater acoustic communication network as described in any one of claims 1-5 to realize the passive navigation and positioning method for an underwater mobile platform based on an underwater acoustic communication network, which is divided into four modules and executes steps 1 to 4 respectively.

7. A computer device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, the method for passive navigation and positioning of an underwater mobile platform based on an underwater acoustic communication network as described in any one of claims 1 to 5 is implemented to realize the method for passive navigation and positioning of an underwater mobile platform based on an underwater acoustic communication network.

8. A computer-readable storage medium having a computer program stored thereon. When the computer program is executed by a processor, the method for passive navigation and positioning of an underwater mobile platform based on an underwater acoustic communication network as described in any one of claims 1 to 5 is implemented to realize the method for passive navigation and positioning of an underwater mobile platform based on an underwater acoustic communication network.

Images