A cooperative positioning compensation method for underwater vehicle swarms

Abstract

This invention relates to the field of cooperative navigation and positioning technology for underwater unmanned systems, and specifically to a cooperative positioning compensation method for an underwater vehicle swarm. The method includes: constructing a swarm consisting of one lead underwater vehicle and at least two follower underwater vehicles; the lead underwater vehicle periodically acquiring GPS correction information and broadcasting a navigation beacon; the follower underwater vehicles establishing a joint state vector and recursively estimating the joint state based on distance, azimuth, elevation, time difference of arrival, velocity measurement, and depth measurement; compensating for clock offset and clock drift rate based on the time difference of arrival residual; compensating for time division multiple access transmission time slots based on clock offset and clock drift rate; compensating for control commands based on ocean current estimation results; increasing the control prior covariance and decreasing the ocean current compensation gain during strong maneuvering phases based on a maneuver perception gating factor; and smoothing the relative positioning results of the follower underwater vehicles through fixed time delay feedback during the output phase.

Description

Technical Field

[0001] This invention relates to the field of cooperative navigation and positioning technology for underwater unmanned systems, specifically to a cooperative positioning compensation method for underwater vehicle swarms. This method is designed for cooperative positioning compensation of swarms of multiple isomorphic underwater vehicles under conditions of limited underwater acoustic communication, clock drift, and water flow disturbance. Background Technology

[0002] Submersible swarms have significant applications in missions such as ocean exploration, environmental monitoring, resource exploration, and collaborative search. For a swarm consisting of a lead submersible and multiple follower submersibles, the follower submersibles not only need to track the course of the course but also need to continuously obtain their relative positions in the lead submersible's coordinate system in order to maintain formation and perform collaborative tasks.

[0003] Some existing methods rely on long-baseline arrays, fixed transponders, or other external infrastructure for positioning. While these methods can provide absolute positioning references, they are costly to deploy, have long deployment cycles, and are limited to certain sea areas, making it difficult to meet the needs of rapid cluster deployment under ordinary hardware conditions.

[0004] Other existing methods rely on high-frequency short-range relative sonar, visual imaging equipment, or close-range optical sensors to directly measure the relative positions between underwater vehicles. These methods typically require close proximity between the vehicles, good visual range, or high water transparency, and often struggle to operate stably in turbid waters, long-range formations, and with standard hardware configurations.

[0005] For cluster positioning methods that mainly rely on underwater acoustic communication and onboard inertial, velocity, and depth sensors, existing solutions generally have the following problems: First, the time of arrival measurement is often used only for distance estimation without considering the impact of local clock offset and clock drift of each submarine on the time of arrival measurement; Second, the time of transmission of underwater acoustic time division multiple access is often regarded as ideal synchronization without using the estimated clock error for communication time slot correction, resulting in a disconnect between the communication layer and the positioning layer.

[0006] Furthermore, in real underwater environments, ocean current disturbances and complex three-dimensional maneuvers further amplify the difficulty of positioning. If the system cannot estimate and compensate for the ocean current based on the difference between the measured speed and the executed command, the following submersible is prone to continuous drift. If the system still relies on control priors during strong maneuvers such as sharp turns, continuous climbs, and continuous dives, model mismatch is likely to occur, leading to abrupt changes, jumps, and increased tail errors in the positioning curve.

[0007] Meanwhile, confined underwater acoustic communication is characterized by low update frequency, large propagation delay, and inability to emit sound simultaneously. If the system directly uses the instantaneous positioning result of each frame as the externally published result, spikes and discontinuities are likely to occur when acoustic updates arrive, making it difficult to use directly in subsequent control and engineering applications.

[0008] Therefore, there is a need for a collaborative positioning compensation method for underwater submersible swarms that does not rely on high-frequency short-range relative sonar and seabed fixed arrays, can operate under ordinary hardware conditions, and can simultaneously solve problems such as clock error compensation, communication time slot correction, water flow compensation, strong maneuvering mismatch suppression, and release result spike suppression. Summary of the Invention

[0009] The purpose of this invention is to provide a collaborative positioning compensation method for underwater submersible swarms, which enables the following submersible to obtain continuous, stable and reproducible relative positioning results in the navigator's coordinate system even under conditions of ordinary hardware, limited underwater acoustic communication, clock drift and water flow disturbance.

[0010] To achieve the above objectives, the present invention adopts the following technical solution: A collaborative positioning compensation method for an underwater vehicle swarm includes: constructing a swarm consisting of one lead underwater vehicle and at least two follower underwater vehicles; the lead underwater vehicle periodically acquiring GPS correction information and broadcasting navigation beacons; the follower underwater vehicles establishing a joint state vector and recursively estimating the joint state based on distance, azimuth, elevation, time difference of arrival, velocity measurement, and depth measurement; compensating for clock offset and clock drift rate based on the time difference of arrival residual; compensating for time division multiple access transmission time slots based on clock offset and clock drift rate; compensating for control commands based on ocean current estimation results; increasing the control prior covariance and decreasing the ocean current compensation gain during strong maneuvering phases based on a maneuver perception gating factor; and smoothing the relative positioning results of the follower underwater vehicles in the lead coordinate system by fixed time delay re-projection during the output phase.

[0011] The specific steps are as follows: Step one involves constructing an underwater swarm consisting of one lead submersible and at least two follower submersibles. The reference coordinate system established by the lead submersible is denoted as the leader coordinate system, and the positioning target of the follower submersibles is defined as their relative position within the leader coordinate system. All submersibles are identically configured, including at least an inertial measurement unit, attitude sensor, Doppler log, depth sensor, underwater acoustic communication device, and GPS interface. Only the lead submersible periodically acquires GPS correction information.

[0012] Step two: The lead submersible estimates its own state using a combined navigation filtering method based on attitude, velocity, depth, and GPS. It determines the attitude transformation relationship between the hull coordinate system and the leader coordinate system using attitude sensors; recursively calculates the lead submersible's position based on Doppler speed observations and discrete sampling periods; corrects its vertical position based on depth sensor observations; and adjusts its horizontal position and velocity when intermittent GPS correction information is obtained. Subsequently, it broadcasts reference information to the follower submersible using a time-division multiple access method prioritizing the leader beacon. The reference information includes the leader coordinate system, the local transmission timestamp of the leader beacon, and the leader's status.

[0013] Step 3: Establish a joint state vector for each following underwater vehicle. .in, These represent the three relative position components of the following underwater vehicle in the navigator's coordinate system; These represent the three relative velocity components of the following underwater vehicle in the navigator's coordinate system; This indicates the clock offset of the following submersible relative to the lead submersible; This represents the clock drift rate of the following submersible relative to the lead submersible. The prediction phase utilizes the relationship between relative motion propagation and clock propagation, i.e., based on... Update the relative position and use Update clock bias, where, This indicates the sequence number of the current discrete sampling time. Indicates the discrete sampling period. This indicates the relative position of the following underwater vehicle in the navigator's coordinate system. This indicates the relative speed of the following submersible in the navigator's coordinate system.

[0014] Step four: After the following underwater vehicle receives the navigation beacon, it obtains the distance measurement value. Azimuth Angle of elevation and arrival time difference measurement .in, This indicates the measured distance between the following submersible and the lead submersible. This represents the measured time difference of arrival, formed by the arrival times recorded by the local clocks of both parties. First, a pseudo-Cartesian observation vector is constructed based on distance and angle information. Then, the observation residual feedback method is adopted, based on the pseudo-Cartesian observation vector. The residual between the predicted and the relative position, for the relative position components Perform compensation updates. Specifically: Let the predicted relative position vector be... The position observation residual is The compensation gain is Then according to Update the relative position. Specifically, Indicates the first The relative positions of discrete sampling times after pseudo-Cartesian observation compensation. Compensation gain. Determined by the predicted relative position uncertainty and pseudo-Cartesian observation noise; let the predicted relative position covariance be... The pseudo-Cartesian observation noise covariance is Then take When the three coordinate components use the same scalar gain, let the predicted position variance be... The variance of the observation noise is Then take .

[0015] Step 5, based on the distance measurement value Speed ​​of sound propagation in water and clock bias Constructing the predicted time difference of arrival , This represents the predicted time difference of arrival under the current conditions. Further, the residual of the time difference of arrival is constructed. , This represents the deviation between the measured and predicted time difference of arrival. A residual feedback correction method is used, utilizing... Correcting clock bias And the clock drift rate is corrected by the residual change. Specifically: Let the clock bias correction gain be... The clock drift rate correction gain is ,but , Among them, the corrected gain and Determined by clock prediction uncertainty and arrival time difference residual noise; let clock bias prediction variance be... The predicted variance of clock drift rate is The variance of arrival time difference, residual noise is The residual rate of change noise variance is Then take , .

[0016] Step six, based on the corrected clock bias obtained in step five. and clock drift rate Predict the expected clock error as the submarine follows the next uplink launch slot. Let the expected clock error be... The predicted time interval until the next uplink transmission slot is Then there is Further, let the time slot correction gain be... The maximum allowable launch time correction is Then the time slot correction amount . use The timing of the next uplink launch of the following underwater vehicle is corrected to reduce time slot misalignment and communication conflicts caused by clock drift.

[0017] Step 7: Estimate the ocean current based on the measured velocity and the command executed at the previous moment, and perform ocean current compensation at the control layer. Let the velocity vector measured by the Doppler log be... The control velocity vector executed in the previous moment is The instantaneous ocean current observation value is denoted as Let the ocean current estimate after low-pass filtering be... The low-pass coefficient is Then the ocean current estimation update formula is: Let the original control command be... The current compensation gain is The compensated control command is: This step can counteract the steady-state drift caused by water flow disturbances.

[0018] Step 8: To suppress the release spikes caused by model mismatch and restricted underwater acoustic updates during the strong maneuver phase, a maneuver perception gating factor is constructed. , By track curvature Intensity of change at reference point and velocity direction deviation At least one of them is determined; when all three are available, they are first determined according to the preset upper limit. , , Normalization yields the corresponding gating factor. , , Take again and satisfy .when An increase indicates enhanced maneuverability. Based on the joint state component arrangement from step three, the relative velocity observation matrix is ​​defined as follows: Then there is ,in, It is a third-order identity matrix. To control the joint state before the prior update, let the control prior basic standard deviation be . The upper limit of the standard deviation of strong maneuverability is ,according to Adjust the control prior standard deviation and construct the control prior covariance. ,in, For the first The standard deviation of the prior relative velocity at each discrete sampling time is used to represent the degree of uncertainty of the relative velocity obtained from the control command. This is the corresponding control prior relative velocity covariance matrix. or The larger the value, the lower the reliability of the control prior. The control prior velocity update gain and the joint state update formula are respectively... and ,in, The predicted covariance before the update. To control the prior velocity update gain, To control the joint state after the prior update, The relative velocity is the control prior calculated from the control command executed at the previous moment. Subsequently, the relative velocity components are updated again using Doppler speed log observations and their noise covariance according to the same recursive relationship. When it increases, Enlarge This reduces the confidence level of the control prior and enhances the effectiveness of measured velocity observations. Let the basic gain for ocean current compensation be... The ocean current compensation gain during the strong maneuvering phase Adjusted to In the output stage, a fixed time delay window length is set to... Utilize time to The relative position and relative velocity of time The relative positions are smoothed by re-projection, and the final relative position of the published data is calculated. , This indicates a short-delay release result. In specific calculations, let the future [number]th [time lag] be [calculated / determined]. The relative positions and relative velocities at each estimated time point are respectively and ,but This step can suppress spikes, spiking, and aberrations.

[0019] The beneficial effects of this invention are: (1) It does not rely on seabed fixed arrays, high-frequency short-range relative sonar or visual imaging devices, and is suitable for underwater cluster deployment under ordinary hardware conditions.

[0020] (2) Simultaneously, compensation is made for the clock offset in the arrival time difference and the clock misalignment in the communication time slot, forming a compensation chain for the positioning layer and the communication layer to work together.

[0021] (3) Reduce the continuous drift caused by water flow through ocean current estimation and ocean current compensation, and reduce model mismatch in complex three-dimensional maneuvering stages through maneuver perception gating.

[0022] (4) By using fixed time delay feedback to smooth the output, the spikes and abnormal jumps caused by restricted underwater acoustic updates are suppressed, thereby improving the continuity and availability of the results released to the public. Attached Figure Description

[0023] Figure 1 This is a schematic diagram of the overall structure of the cluster cooperative positioning system in this invention, used to illustrate the composition and relationship between the lead submersible, the follow submersible, the onboard sensors, the underwater acoustic communication link, and the various compensation modules.

[0024] Figure 2 This is a flowchart of the cluster cooperative positioning compensation method in this invention, used to illustrate the processing sequence from the state estimation of the lead submersible, the broadcasting of the lead beacon, the joint state estimation of the follower submersible, to the arrival time compensation, time slot compensation, ocean current compensation, maneuver perception compensation, and short time delay release.

[0025] Figure 3 This is a schematic diagram of the joint state, observation and compensation relationship in this invention, used to illustrate the compensation and update relationship between the joint state of the following underwater vehicle, such as relative position, relative speed, clock offset and clock drift rate, and observations such as distance, azimuth, elevation angle and time difference of arrival.

[0026] Figure 4 This is a timing diagram illustrating the navigation broadcast, arrival time processing, and time division multiple access time slot compensation in this invention. It is used to explain the time relationship between the navigation beacon transmission, the follower underwater vehicle reception, the arrival time difference residual calculation, the expected clock error prediction, and the correction of the next uplink transmission time slot.

[0027] Figure 5 This is a schematic diagram of the clock compensation, ocean current compensation, maneuver perception compensation and release compensation link in this invention, used to illustrate the continuous compensation link between clock error correction, control command ocean current compensation, weight adjustment during strong maneuvering phases and smooth release with fixed time delay.

[0028] Figure 6 This is an example diagram of a three-dimensional flight path in the embodiment.

[0029] Figure 7 This is a relative positioning error curve diagram from the example.

[0030] Figure 8 This is a statistical chart of relative positioning errors in three scenarios in the embodiment. Detailed Implementation

[0031] The specific embodiments of the present invention will be further described below with reference to the accompanying drawings and technical solutions.

[0032] like Figure 1As shown, this embodiment uses a cluster of 5 isomorphic underwater vehicles, including 1 lead vehicle and 4 follower vehicles. Each vehicle is equipped with an inertial measurement unit (IMU), attitude sensor (AHRS), Doppler log (DVL), depth sensor and underwater acoustic communication device, with only the lead vehicle receiving intermittent GPS correction information.

[0033] like Figure 2 (In the figure, TOA represents the time of arrival; the time difference of arrival measurement is formed by using the TOA recorded by both parties.) As shown, the positioning compensation process in this embodiment includes: step two, the lead submersible's own state estimation and lead beacon broadcast; step three, the follower submersible's joint state prediction; step four, the pseudo-Cartesian observation vector compensation update; step five, the time difference of arrival residual compensation; step six, the time division multiple access (TDMA) transmission slot correction; step seven, the ocean current estimation and control command ocean current compensation; and step eight, the maneuvering perception gating adjustment and fixed time delay return smoothing release. Each step is executed according to steps one to eight above.

[0034] like Figure 3 As shown, a joint state vector is established following the underwater vehicle. .in Represents the relative position components in the navigator's coordinate system. Represents the relative velocity component. Indicates clock bias. This represents the clock drift rate. Within each sampling period, the relative position is first propagated based on the relative velocity. Then, the observation residual feedback method described in step four is used to compensate and update the relative position based on the distance measurement, azimuth, and elevation angle. Finally, the residual feedback correction method described in step five is used to compensate for the clock offset and clock drift rate using the arrival time difference residual.

[0035] like Figure 4 As shown in the diagram, TOA represents the Time of Arrival. The lead submersible sends a navigation beacon at its local launch time, and the follow submersible receives the beacon at its local reception time. The TOA recorded by both submersibles forms the time difference of arrival measurement. Then, based on the distance measurement values... Speed ​​of sound and clock bias Calculate the predicted time difference of arrival Through residuals Correcting clock bias Subsequently, using The expected clock error for the next uplink transmission time slot is predicted, and the time slot correction is calculated accordingly. This allows the uplink launch following the submarine to return to the target time slot.

[0036] like Figure 5 As shown, the ocean current compensation chain first is based on Instantaneous ocean current observations are obtained, and then a low-pass update formula is used to obtain an estimated ocean current value. During the stable navigation phase, ocean current compensation gain A larger value is chosen to improve resistance to current; during the high-maneuver phase, the maneuver perception gating factor is used. Increase the control prior covariance and decrease the ocean current compensation gain After reduction The calculation of ocean current compensation control commands is initiated to reduce model mismatch during sharp turns, continuous ascents, and continuous descents. In this embodiment, the prior baseline standard deviation is controlled. Take 0.105 m / s as the upper limit of the standard deviation for strong maneuvering. Take 7.0 m / s; control for prior covariance. When it increases, its update gain The reduction allows Doppler log velocity observations to play a greater role in joint state updates.

[0037] In this embodiment, a fixed time delay window length is used. Eleven sampling periods are used, corresponding to a publication delay of approximately 5.5 seconds. The system does not directly publish the instantaneous filtering results; instead, it performs back-projection smoothing on a finite number of subsequent estimated times, as described in step eight. Get the relative position of the post This suppresses spikes and anomalous jumps caused by restricted underwater acoustic updates.

[0038] To further illustrate the effectiveness of this method, this embodiment uses a white-background simulation result image directly output by the program as an example verification result. Figures 6 to 8 All images are original output images from the simulation program; the image content has not been recolored, cropped, or redrawn.

[0039] like Figure 6 As shown, the lead submersible and four follower submersibles complete formation navigation in three-dimensional space. The figure also shows the actual trajectory and the estimated trajectory. This figure is used to illustrate that, under given conditions of water flow disturbance, limited underwater acoustic communication, and clock drift, this method can continuously generate relative positioning results consistent with the actual motion trend, and provide a continuous position reference in the lead coordinate system for subsequent formation control.

[0040] like Figure 7As shown, the relative positioning errors of the four follow-up underwater vehicles remained generally within a low range throughout the mission. Acoustic updates, clock compensation, ocean current compensation, and short-delay release compensation collectively suppressed error spikes. This figure illustrates that this method does not only provide positioning results at the final moment, but also maintains usable relative positioning accuracy during continuous operation.

[0041] like Figure 8 As shown, the complete method performed multi-random seed statistics in three scenarios: straight navigation, turning and climbing, and continuous S-shaped turning and diving, and the root mean square error and 95th percentile error of relative positioning are given. This figure is used to illustrate the overall positioning accuracy of the method under different maneuver intensities. Among them, the error is relatively higher in the continuous S-shaped turning and diving scenario due to the more complex maneuver, but it can still maintain stable and continuous relative positioning results.

[0042] Simulation results show that, under the complete method conditions, the root mean square error (RMSE) of relative positioning in the straight-line navigation scenario is approximately 0.184 meters, and the 95th percentile error is approximately 0.329 meters; the RMSE of relative positioning in the turning and climbing scenario is approximately 0.262 meters, and the 95th percentile error is approximately 0.452 meters; and the RMSE of relative positioning in the continuous S-shaped turning and diving scenario is approximately 0.558 meters, and the 95th percentile error is approximately 0.857 meters. These statistical results demonstrate that the present invention can achieve stable cluster relative positioning compensation even under conditions of ordinary hardware, clock drift, water flow disturbance, and limited underwater acoustic communication.

Claims

1. A method for cooperative positioning compensation of underwater vehicle swarms, characterized in that, include: A cluster consisting of one lead submersible and at least two follower submersibles is constructed. The lead submersible periodically acquires GPS correction information and broadcasts navigation beacons. The follower submersibles establish a joint state vector and recursively estimate the joint state based on distance, azimuth, elevation, time difference of arrival, velocity measurement, and depth measurement. Clock offset and clock drift rate are compensated based on the time difference of arrival residual. Time division multiple access transmission time slots are compensated based on the clock offset and clock drift rate. Control commands are compensated for based on ocean current estimation results. The control prior covariance during strong maneuvering phases is increased and the ocean current compensation gain is decreased based on the maneuver perception gating factor. In the output phase, the relative positioning results of the follower submersibles in the lead coordinate system are smoothly output through fixed time delay return.

2. The underwater vehicle swarm cooperative positioning compensation method according to claim 1, characterized in that, The specific steps are as follows: Step 1: Construct an underwater cluster consisting of one lead submersible and at least two follower submersibles; the reference coordinate system established by the lead submersible is denoted as the leader coordinate system, and the positioning target of the follower submersibles is defined as the relative position of the follower submersibles in the leader coordinate system. Step two: The pilot submersible estimates its own state using a combination of attitude, speed, depth, and GPS navigation filtering. Step 3: Establish a joint state vector for each following underwater vehicle. ;in, These represent the three relative position components of the following underwater vehicle in the navigator's coordinate system; These represent the three relative velocity components of the following underwater vehicle in the navigator's coordinate system; This indicates the clock offset of the following submersible relative to the lead submersible; This represents the clock drift rate of the following submersible relative to the lead submersible; the prediction phase uses the relationship between relative motion propagation and clock propagation, i.e., based on... Update the relative position and use Update clock bias, where, This indicates the sequence number of the current discrete sampling time. Indicates the discrete sampling period. This indicates the relative position of the following underwater vehicle in the navigator's coordinate system. This indicates the relative velocity of the following submersible in the navigator's coordinate system; Step four: After the following underwater vehicle receives the navigation beacon, it obtains the distance measurement value. Azimuth Angle of elevation and arrival time difference measurement First, a pseudo-Cartesian observation vector is constructed based on the distance and angle information. Then, the observation residual feedback method is adopted, based on the pseudo-Cartesian observation vector. The residual between the predicted and the relative position, for the relative position components Perform a compensation update; Step 5, based on the distance measurement value Speed ​​of sound propagation in water and clock bias Constructing the predicted time difference of arrival , This represents the predicted time difference of arrival under the current conditions; further, the time difference of arrival residual is constructed. , This indicates the deviation between the measured and predicted time difference of arrival; a residual feedback correction method is used, utilizing... Correcting clock bias And the clock drift rate is corrected by the residual change. ; Step six, based on the corrected clock bias obtained in step five. and clock drift rate Predict the expected clock error as the submarine follows the next uplink launch slot; let the expected clock error be... The predicted time interval until the next uplink transmission slot is Then there is Further, let the time slot correction gain be... The maximum allowable launch time correction is Then the time slot correction amount ;use The timing of the next uplink launch of the following underwater vehicle is corrected to reduce time slot misalignment and communication conflicts caused by clock drift; Step 7: Estimate the ocean current based on the measured velocity and the command executed at the previous moment, and perform ocean current compensation at the control layer; let the velocity vector measured by the Doppler log be... The control velocity vector executed in the previous moment is The instantaneous ocean current observation value is denoted as Let the ocean current estimate after low-pass filtering be... The low-pass coefficient is Then the ocean current estimation update formula is: Let the original control command be... The current compensation gain is The compensated control command is: ; Step 8: To suppress the release spikes caused by model mismatch and restricted underwater acoustic updates during the strong maneuver phase, a maneuver perception gating factor is constructed. Based on the joint state component arrangement in step three, the relative velocity observation matrix is ​​defined as follows: Then there is ,in, It is a third-order identity matrix. To control the joint state before the prior update; let the control prior standard deviation be . The upper limit of the standard deviation of strong maneuverability is ,according to Adjust the control prior standard deviation and construct the control prior covariance. ,in, For the first The standard deviation of the prior relative velocity is controlled by a discrete sampling time. Here is the corresponding control prior relative velocity covariance matrix; the control prior velocity update gain and joint state update formulas are respectively... and ,in, The predicted covariance before the update. To control the prior velocity update gain, To control the joint state after the prior update, The control prior relative velocity is calculated from the control command executed at the previous moment; subsequently, the relative velocity components are updated again using Doppler speed log velocity observations and their noise covariance according to the same recursive relationship; let the ocean current compensation base gain be... The ocean current compensation gain during the strong maneuvering phase Adjusted to In the output stage, a fixed time delay window length is set as follows: Utilize time to The relative position and relative velocity of time The relative positions are smoothed by re-projection, and the final relative position of the published data is calculated. .

3. The underwater vehicle swarm cooperative positioning compensation method according to claim 1, characterized in that, In step one, all submersibles are configured identically, including at least an inertial measurement unit, attitude sensor, Doppler log, depth sensor, underwater acoustic communication device, and GPS interface, with only the lead submersible periodically acquiring GPS correction information.

4. The underwater vehicle swarm cooperative positioning compensation method according to claim 1, characterized in that, In step two, the attitude transformation relationship between the hull coordinate system and the navigator coordinate system is determined based on the attitude sensor. The position of the navigator is recursively calculated based on the Doppler log velocity observation and discrete sampling period. The vertical position is corrected based on the depth sensor observation. The horizontal position and velocity are corrected when intermittent GPS correction information is obtained. Then, reference information is broadcast to the follower submersible in a time-division multiple access manner with the navigator beacon as the priority. The reference information includes the navigator coordinate system, the navigator beacon local transmission timestamp, and the navigator status.

5. The underwater vehicle swarm cooperative positioning compensation method according to claim 1, characterized in that, Step four specifically involves: Let the predicted relative position vector be... The position observation residual is The compensation gain is Then according to Update the relative position; where, Indicates the first The relative positions of discrete sampling times after pseudo-Cartesian observation compensation; compensation gain Determined by the predicted relative position uncertainty and pseudo-Cartesian observation noise; let the predicted relative position covariance be... The pseudo-Cartesian observation noise covariance is Then take When the three coordinate components use the same scalar gain, let the predicted position variance be... The variance of the observation noise is Then take .

6. The underwater vehicle swarm cooperative positioning compensation method according to claim 1, characterized in that, Step five specifically involves: setting the clock bias correction gain to... The clock drift rate correction gain is ,but , Among them, the corrected gain and Determined by clock prediction uncertainty and arrival time difference residual noise; let clock bias prediction variance be... The predicted variance of clock drift rate is The variance of arrival time difference, residual noise is The residual rate of change noise variance is Then take , .

7. The underwater vehicle swarm cooperative positioning compensation method according to claim 1, characterized in that, In step eight, the motorized perception gating factor , By track curvature Intensity of change at reference point and velocity direction deviation At least one of them is determined; when all three are available, they are first determined according to the preset upper limit. , , Normalization yields the corresponding gating factor. , , Take again and satisfy .

8. The underwater vehicle swarm cooperative positioning compensation method according to claim 1, characterized in that, In step eight, let the future number be... The relative positions and relative velocities at each estimated time point are respectively and ,but .

Images