Cooperative positioning communication method, device and equipment of underwater robot and medium

Abstract

The invention discloses a cooperative positioning communication method, device and equipment for an underwater robot, and a medium, and relates to the technical field of underwater cooperative navigation. In the first batch of diving tasks, the position of the follower is corrected by relying on a high-precision satellite positioning reference provided by a water surface navigator and combining an ultra-short baseline system, so that the positioning precision of the follower when the follower arrives at the seabed is remarkably improved, and accurate initial position information is provided for an integrated navigation system of the follower. On the basis, in a second batch of diving tasks, the pilot uses initial position information of a plurality of followers as known reference points, and realizes inverse solution and correction of the position of the pilot by fusing underwater acoustic positioning data and navigation state information of the pilot. Through a batch diving and multi-round cooperative correction mechanism, positioning error accumulation caused by the fact that the integrated navigation system is in a pure inertial working mode for a long time in the large-depth diving process is effectively inhibited, and the positioning accuracy and the system robustness of underwater formation cooperative operation are remarkably improved.

Description

Technical Field

[0001] This invention relates to the field of underwater cooperative navigation technology, and in particular to a cooperative positioning and communication method, device, equipment and medium for underwater robots. Background Technology

[0002] With the continuous deepening of marine resource development and utilization, tasks such as marine environmental surveys, resource exploration, and marine archaeology place higher demands on exploration efficiency. Although autonomous underwater vehicles (AUVs) have been widely used, the vast operating areas and limited coverage efficiency of single-vessel operations make it difficult to meet the increasing demands for timeliness. Therefore, collaborative operations in multi-AUV swarms have become an important way to improve overall exploration efficiency. Swarm collaboration relies on precise positioning and reliable communication among members to achieve functions such as formation maintenance, status reporting, and task coordination and scheduling.

[0003] Currently, research on autonomous underwater robot formation coordination mainly focuses on positioning information fusion and trajectory calculation. However, systematic research is lacking on underwater acoustic positioning and communication scheduling strategies for critical operational phases such as deep dives from the surface to the seabed and near-bottom navigation. This leads to the accumulation and gradual amplification of positioning and communication errors under long-endurance, highly dynamic, and channel-limited conditions, thereby affecting the stability of formation coordination and overall operational accuracy. Summary of the Invention

[0004] In view of this, the present invention provides a cooperative positioning and communication method, device, electronic device and medium for underwater robots, in order to solve the technical problem of significant underwater acoustic positioning and communication errors in deep-sea diving and near-bottom navigation.

[0005] Firstly, a cooperative localization and communication method for underwater robots is provided, applicable to a single-leader master-slave formation scenario, wherein the formation includes one leader and multiple followers. The method includes: In response to the first dive command, control multiple followers to perform dive operations, and control the navigator to remain floating on the surface; According to the preset time interval, the satellite positioning position information and heading information of the navigator, as well as the first underwater acoustic positioning position information of each follower, are obtained. Based on the satellite positioning position information, heading information and first underwater acoustic positioning position information, multiple rounds of collaborative positioning correction are performed on multiple followers. The first dive operation ended when it was detected that the Doppler logs on each follower were in a valid bottom-alignment state. In response to the second batch of dive commands, control the navigator to perform the dive operation, and control multiple followers to hover and wait in the preset area; According to the preset time interval, the second underwater acoustic positioning position information and the navigation status information of each follower are obtained, and the navigator is corrected in multiple rounds based on the second underwater acoustic positioning position information and the navigation status information. The second dive operation will end when the Doppler log on the navigator is detected to be in a valid bottom-alignment state. According to the preset cycle, the formation will perform near-bottom navigation positioning and communication operations to complete the coordinated position correction of the formation members.

[0006] Secondly, a cooperative positioning and communication device for underwater robots is provided, applicable to a single-leader master-slave formation scenario, wherein the formation includes one leader and multiple followers. The device includes: The control module is used to respond to the first batch of dive commands, control multiple followers to perform dive operations, and control the navigator to remain floating on the surface. The first acquisition module is used to acquire the satellite positioning location information and heading information of the navigator, as well as the first underwater acoustic positioning location information of each follower, according to a preset time interval, and to perform multiple rounds of collaborative positioning correction for multiple followers based on the satellite positioning location information, heading information and first underwater acoustic positioning location information. The control module is also used to terminate the first batch of dive operations when it is detected that the Doppler logs on each follower are in a valid bottom-alignment state. The control module is also used to control the navigator to perform a dive operation in response to the second batch of dive commands, and to control multiple followers to hover and wait in a preset area; The second acquisition module is used to acquire the second underwater acoustic positioning position information and the navigation status information of each follower according to a preset time interval, and to perform multiple rounds of positioning correction on the navigator based on the second underwater acoustic positioning position information and the navigation status information. The control module is also used to terminate the second batch of dive operations when it detects that the Doppler log on the Navigator is in a valid bottom-alignment state. The control module is also used to perform near-bottom navigation positioning and communication operations for the formation according to a preset cycle, and to complete the coordinated position correction of the formation members.

[0007] Thirdly, an electronic device is provided, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the steps of the above-described cooperative positioning and communication method for underwater robots.

[0008] Fourthly, a computer-readable storage medium is provided, which stores a computer program that, when executed by a processor, implements the steps of the above-described cooperative positioning and communication method for underwater robots.

[0009] In the aforementioned scheme implementing the cooperative positioning and communication method, device, electronic equipment, and storage medium for underwater robots, during the first batch of dives, the high-precision satellite positioning benchmark provided by the surface navigator, combined with an ultra-short baseline system, significantly improves the positioning accuracy of the followers upon reaching the seabed, providing precise initial position information for their integrated navigation system. Building upon this, in the second batch of dives, the navigator utilizes the initial position information of multiple followers as known reference points, fusing underwater acoustic positioning data with its own navigation status information to achieve inverse positioning and correction. Through batch dives and multi-round cooperative correction mechanisms, the accumulation of positioning errors caused by the integrated navigation system operating in pure inertial mode for extended periods during deep dives is effectively suppressed, ensuring that the entire formation possesses high-precision navigation capabilities after completing the dive. While maintaining relatively low system costs, this significantly improves the positioning accuracy and system robustness of underwater formation cooperative operations. Attached Figure Description

[0010] Various other advantages and benefits will become apparent to those skilled in the art upon reading the following detailed description of preferred embodiments. The accompanying drawings are for illustrative purposes only and are not intended to limit the scope of this application. Furthermore, the same reference numerals denote the same parts throughout the drawings. In the drawings: Figure 1 This is a flowchart illustrating a collaborative positioning and communication method for an underwater robot in one embodiment of the present invention. Figure 2 This is a schematic diagram of the navigator equipped with sensors in one embodiment of the present invention; Figure 3 This is a schematic diagram of the structure of a follower equipped with a sensor in one embodiment of the present invention; Figure 4 This is a schematic diagram of the formation positioning and communication process during near-bottom navigation operations in one embodiment of the present invention; Figure 5(a) is a flowchart of the surface floating preparation stage during the diving and near-bottom navigation of a multi-underwater robot operation formation in one embodiment of the present invention; Figure 5(b) is a flowchart of the follower diving phase during the multi-underwater robot formation diving and near-bottom navigation process in one embodiment of the present invention; Figure 5(c) is a flowchart of the navigator's diving phase during the multi-underwater robot formation diving and near-bottom navigation process in one embodiment of the present invention; Figure 5(d) is a flowchart of the formation-near-bottom navigation operation phase during the diving and near-bottom navigation of multiple underwater robots in one embodiment of the present invention; Figure 6 This is a schematic diagram of the structure of a cooperative positioning and communication device for an underwater robot in one embodiment of the present invention.

[0011] in, Figure 2 and Figure 3 The correspondence between the reference numerals and component names in the attached drawings is as follows: 1. Laser inertial navigation system; 2. Doppler log; 3. Ultrashort array system; 4. Fiber optic inertial navigation system. Detailed Implementation

[0012] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. It should be understood that the accompanying drawings in the present invention are only for illustrative and descriptive purposes and are not intended to limit the scope of protection of the present invention.

[0013] Furthermore, it should be understood that the schematic drawings are not drawn to scale. The flowcharts used in this invention illustrate operations implemented according to some embodiments of the invention. It should be understood that the operations in the flowcharts may not be implemented in sequence, and steps without logical contextual relationships may be reversed or performed simultaneously. Moreover, those skilled in the art, guided by the content of this invention, may add one or more other operations to the flowcharts, or remove one or more operations from the flowcharts.

[0014] Furthermore, the embodiments described herein are merely some, not all, of the embodiments of the invention. The components of the embodiments of the invention described and illustrated herein can typically be arranged and designed in various different configurations. Therefore, the following detailed description of the embodiments of the invention provided in the accompanying drawings is not intended to limit the scope of the claimed invention, but merely to illustrate selected embodiments of the invention. All other embodiments obtained by those skilled in the art based on the embodiments of the invention without inventive effort are within the scope of protection of the invention.

[0015] It should be noted that the term "comprising" will be used in the embodiments of the present invention to indicate the presence of a feature subsequently declared, but does not exclude the addition of other features. It should also be noted that similar reference numerals and letters in the following figures indicate similar items; therefore, once an item is defined in one figure, it does not need to be further defined and explained in subsequent figures.

[0016] With the continued deepening of global marine resource development and utilization, the importance of marine exploration technology is becoming increasingly prominent. The application of underwater exploration equipment such as autonomous underwater vehicles (AUVs) in marine environmental surveys, resource exploration, and marine archaeology is becoming increasingly mature. However, due to the vastness of marine exploration areas, the operational efficiency of a single underwater robot is relatively low, making it difficult to meet the growing demands of marine applications. Therefore, developing a collaborative exploration model based on multi-underwater robot swarms has become an effective way to improve the timeliness of marine environmental surveys.

[0017] In collaborative underwater robot formation operations, it is necessary to obtain the location information of each member through positioning technology and realize information exchange and task scheduling among formation members according to mission requirements. At present, research on autonomous underwater robot formation collaborative positioning and navigation mainly focuses on positioning information fusion and trajectory prediction, while there is still relatively little discussion on underwater acoustic positioning and communication strategies for deep diving from the surface to the seabed and near-bottom navigation.

[0018] To address the aforementioned issues, this application provides an underwater collaborative positioning and communication method for a single-leader master-slave formation. It employs two-way underwater acoustic ranging and direction finding techniques to organize positioning and information exchange among formation members. By combining batch-dive correction with near-bottom time-slotted communication and positioning timing, it balances positioning accuracy, communication efficiency, and signal collision avoidance reliability requirements during deep-dive and near-bottom navigation operations.

[0019] The following is a detailed description of this case, in conjunction with the relevant accompanying drawings in the instruction manual.

[0020] Please see Figure 1 This specification provides a cooperative localization and communication method for underwater robots, applied to a single-leader master-slave formation scenario, wherein the formation includes one leader and multiple followers, and specifically includes the following steps: S10: In response to the first dive command, controls multiple followers to perform dive operations and controls the navigator to remain floating on the surface.

[0021] It is understood that the executing entity of this invention can be a cooperative positioning and communication device for an underwater robot, or it can be a terminal or a server; no specific limitation is made here. This embodiment of the invention will be described using a server as an example.

[0022] In this step, the descent process is carried out in batches. In response to the first batch of descent commands, multiple followers in the formation are controlled to perform synchronized descent operations, while the navigator is controlled to float at a fixed point on the water surface.

[0023] S20: According to a preset time interval, acquire the satellite positioning information and heading information of the navigator, as well as the first underwater acoustic positioning information of each follower, and perform multiple rounds of collaborative positioning correction for multiple followers based on the satellite positioning information, heading information and first underwater acoustic positioning information.

[0024] In this step, during the initial descent phase, the navigator acquires its surface location via its onboard satellite positioning system. Simultaneously, it obtains its heading information using a high-precision laser inertial navigation system. This satellite positioning and real-time heading information together form the navigator's spatiotemporal reference during the surface phase, providing precise reference for the subsequent positioning of followers during descent.

[0025] During the descent, the Doppler logs of multiple followers were not yet operational at the bottom. The integrated navigation systems on the followers, relying solely on inertial calculations, were prone to cumulative errors, causing their positioning accuracy to decrease over time. Therefore, the navigator needed to periodically perform underwater acoustic positioning on the followers using an ultra-short array (USBL) positioning system and send the calculated accurate positions to each follower to correct their navigation status.

[0026] Specifically, the navigator sends positioning query signals to multiple followers sequentially through an ultra-short array system at preset time intervals, based on the number of each follower, and receives response signals from each follower, thereby obtaining the first underwater acoustic positioning information of each follower. The first underwater acoustic positioning information includes the distance and orientation information of each follower relative to the navigator at the time the positioning query signal is sent.

[0027] Subsequently, the navigator, combining its own satellite positioning information, heading information, and first underwater acoustic positioning information, converts the relative position information of the followers into absolute position information through coordinate transformation and calculation. The navigator then transmits this absolute position information to the corresponding followers via an underwater acoustic communication device. The followers use this information to update their fiber optic inertial navigation systems, complete real-time position calibration, and effectively suppress the accumulation of positioning errors.

[0028] Optionally, a preset time interval can be set in advance based on system performance, operational requirements, and operational environment constraints. The navigator can be set with a metronome to initiate a positioning cycle at the preset time interval (e.g., 10 seconds). Further, the N formation members are sequentially numbered, and the order of positioning and communication is agreed upon accordingly. The navigator is numbered 2, and the followers are numbered 3, 4, ..., N+1; number 1 is reserved for temporary highest communication priority.

[0029] In practical application scenarios, such as Figure 2The diagram shows the structure of the Navigator equipped with sensors. The Navigator features a high-precision laser-inertial navigation system, including a laser inertial navigation system 1 and a Doppler log 2, enabling it to achieve high-precision positioning and calculate the Navigator's coordinates. It also carries a high-precision ultra-short array system 3, which accurately measures the azimuth and distance between the Navigator and its followers. Furthermore, the Navigator is equipped with a BeiDou positioning system (not shown in the diagram) and an underwater acoustic communication device (not shown in the diagram). The BeiDou positioning system is installed directly below the positioning antenna at the stern of the Navigator. Figure 3 The diagram shows the structure of the follower equipped with sensors. Each follower is equipped with a low-cost fiber optic inertial navigation system, including a fiber optic inertial navigation system 4 and a Doppler log 2, which can maintain high-precision positioning and navigation to complete operations in a short time. Simultaneously, the follower is also equipped with an acoustic positioning beacon (not shown) to send information to the formation members and respond to positioning query signals from the navigator's ultra-short array system 3. Furthermore, the follower is also equipped with a BeiDou positioning system (not shown), which is installed directly below the positioning antenna at the stern of the follower for satellite positioning.

[0030] In one embodiment of this application, a specific collaborative positioning correction scheme is provided. In S20, the satellite positioning information and heading information of the navigator, as well as the first underwater acoustic positioning information of each follower, are acquired at preset time intervals. Based on the satellite positioning information, heading information, and first underwater acoustic positioning information, multiple rounds of collaborative positioning correction are performed on the multiple followers. Specifically, this includes the following steps S21-S28: S21: For any round of collaborative positioning correction, the navigator obtains satellite positioning information through the BeiDou positioning system and heading information through the laser inertial navigation system.

[0031] In this step, during each positioning correction cycle, the navigator acquires satellite positioning information via a satellite positioning system. This information represents the navigator's real-time absolute position coordinates during the floating phase on the water. Simultaneously, it obtains its heading information through the onboard laser inertial navigation system.

[0032] S22: Based on satellite positioning information, heading information, and the installation distance between the ultra-short array system and the BeiDou positioning system, the Navigator calculates the first position information of the ultra-short array system in its own coordinate system.

[0033] In this step, based on satellite positioning information, heading information, and the installation distance between the ultra-short array system and the BeiDou positioning system, the absolute position information corresponding to the center of the ultra-short array system on the Navigator is generated through coordinate transformation and lever compensation calculation, and is recorded as the first position information.

[0034] Optionally, the installation distance between the ultra-short array system and the BeiDou positioning system is typically a three-dimensional spatial offset, including horizontal and vertical components. This offset is used to accurately convert the position obtained by the satellite positioning antenna to the array center of the ultra-short array system during collaborative positioning correction, thereby establishing a unified spatial reference benchmark and ensuring the geometric accuracy of subsequent underwater acoustic positioning calculations.

[0035] In the above manner, the satellite positioning position is transformed to the center position of the array along the arm offset vector in the carrier coordinate system according to the real-time heading. This maps the satellite positioning result to the geometric reference point for the actual underwater acoustic positioning and measurement, ensuring that all subsequent relative positioning measurements based on the ultra-short array are calculated based on a unified and accurate absolute spatial reference.

[0036] S23: The navigator sends positioning query signals to each follower sequentially through the ultra-short array system and receives response signals from each follower.

[0037] S24: Based on the response signals from each follower, the navigator generates the first underwater acoustic positioning position information of each follower at the moment the navigator receives the response signal; The first underwater acoustic positioning information includes the distance and orientation of each follower relative to the navigator.

[0038] In steps S23-S24, the navigator sequentially sends positioning query signals to each follower via its onboard ultra-short array system, triggering each follower to promptly respond with a corresponding signal based on the received signal. Based on the response signals returned by each follower, the navigator processes and calculates the signals using the ultra-short array system to generate the first underwater acoustic positioning position information of each follower at the moment the navigator receives the response signal. This first underwater acoustic positioning position information characterizes the spatial relative relationship of each follower to the navigator when the navigator locates the response signal. Specifically, it includes distance information calculated based on sound wave propagation time and a preset sound speed, and azimuth information calculated based on the phase difference or time difference of the received signal from the array, providing the initial observation data basis for subsequent conversion to absolute position.

[0039] S25: The navigator determines the coordinates of each follower in the array body coordinate system based on the first underwater acoustic positioning information.

[0040] In this step, while the relative distance and azimuth information directly output by the ultra-short array system can describe the spatial relative relationships of the target, it cannot be directly geometrically synthesized with the navigator's navigation status. Therefore, the navigator converts this information into three-dimensional Cartesian coordinates in the array's body coordinate system, representing it as a definite spatial vector in the navigator's carrier coordinate system. This provides a standardized mathematical foundation for subsequently calculating the follower's absolute position based on the navigator's absolute position, ensuring the accuracy of the positioning calculation in terms of geometric relationships and the operability of the calculation process.

[0041] S26: The navigator calculates the one-way propagation delay of the response signal from each follower to the navigator based on the preset sound speed and the coordinate values ​​of each follower.

[0042] In this step, the navigator first calculates the straight-line distance between itself and each follower based on the three-dimensional coordinates of each follower in the array's body coordinate system obtained from the ultra-short array system. Then, using a pre-set average speed of sound, the one-way time delay required for sound waves to propagate from the followers to the navigator is calculated using the ratio of distance to speed of sound. This propagation delay is used to subsequently align the underwater acoustic positioning observation time with the signal transmission time, providing a crucial time compensation parameter for time synchronization and position calculation.

[0043] S27: Based on the first position information, coordinate values ​​and one-way propagation delay, the navigator calculates the second position information of each follower at the time when they send a response signal to the navigator, and sends the second position information to each follower through an underwater acoustic communication device.

[0044] In this step, the navigator first uses the one-way propagation delay to trace its position back to the position at the time the response signal was sent. It then combines this position with the coordinates of the followers, transforms the coordinates to a geographic coordinate system, and calculates the absolute position coordinates of each follower at the time the response signal was sent—this is the second position information. Subsequently, the navigator transmits this second position information to the corresponding followers via an underwater acoustic communication device. Upon receiving this information, the followers update their fiber-optic inertial navigation systems based on it, completing real-time position correction and effectively suppressing positioning drift caused by accumulated inertial calculations.

[0045] S28: Each follower determines the navigator's third position information at the current moment based on the received second position information, and performs positioning correction on its own fiber optic inertial navigation system based on the third position information.

[0046] In this step, each follower receives the second position information calculated and transmitted by the navigator via an acoustic positioning beacon. The absolute position of the navigator at the moment each follower sends a response signal to the navigator is used as the positioning reference. From this, the accurate position of the navigator at the current moment is calculated and recorded as the third position information. Subsequently, based on the track changes recorded by their fiber optic inertial navigation system, the followers use the third position information as the initial point for integral calculation, achieving real-time updates of position status and error reset, ensuring that subsequent dead reckoning continues on an accurate spatial reference.

[0047] In one embodiment of this application, a specific follower positioning correction scheme is provided. In S28, each follower determines the current position information of the navigator based on the received second position information, and performs positioning correction on its respective fiber optic inertial navigation system based on the third position information. Specifically, this includes the following steps S281-S284: S281: Each follower receives a second location information sent by the navigator via its onboard acoustic positioning beacon.

[0048] In this step, each follower obtains the second position information sent by the navigator through an acoustic positioning beacon. This second position information is high-precision position data calculated by the navigator based on the response signals fed back by the followers, and includes the precise coordinates of the followers at the time of sending the response signal.

[0049] S282: Each follower obtains its first positioning result from its fiber optic inertial navigation system at the moment it sends a response signal to the navigator, and its second positioning result from its fiber optic inertial navigation system at the current moment, through its own onboard fiber optic inertial navigation system.

[0050] In this step, each follower uses its onboard fiber optic inertial navigation system to obtain the first positioning result of its fiber optic inertial navigation system at the moment it sends a response signal to the navigator, and the second positioning result of its fiber optic inertial navigation system at the current moment. By comparing the positioning results at these two moments, the follower can combine the sound wave propagation delay and its own motion calculations to achieve spatiotemporal alignment between the received information and its own trajectory, providing accurate local state input for subsequent position updates based on the navigator's correction information.

[0051] S283: Each follower determines its navigation data from the time it sent the response signal to the current time based on the first positioning result and the second positioning result.

[0052] In this step, the follower uses the first and second positioning results to calculate the change in its flight trajectory from the time the response signal was sent to the current time through trajectory extrapolation and data interpolation. This includes the cumulative flight distance and heading. This flight data is used to extrapolate the received accurate position of the navigator to the current time, thereby achieving spatiotemporal alignment between the correction information and the follower's real-time navigation status, providing the necessary motion compensation for accurate position updates.

[0053] S284: Each follower uses the second position information received from the navigator as a reference, combines it with navigation data, calculates the navigator's third position information at the current moment, and uses the third position information as the initial value to update the navigation position of the fiber optic inertial navigation system, thus completing the position correction of the fiber optic inertial navigation system.

[0054] In this step, the follower uses the second position information as an initial reference, overlays it with the position change calculated from the navigation data, and obtains the navigator's calculated position at the current moment, i.e., the third position information. Subsequently, the follower uses the third position information as the new starting point of its inertial navigation system, resets the navigation state, and continues dead reckoning based on this reference, thereby completing the real-time calibration and update of position information, effectively eliminating accumulated positioning errors, and ensuring that the subsequent navigation process unfolds on a precise spatial reference.

[0055] S30: When it is detected that the Doppler logs on each follower are in a valid bottom alignment state, the first batch of dive operations will end.

[0056] In this step, during the descent of each follower, the Doppler logs of each follower are continuously monitored to determine whether they have reached an effective seabed depth and stably locked onto the seabed. When it is detected that the Doppler logs of all followers are in a valid seabed-alignment state, the first batch of descent operations is considered complete, and the descent process for this phase ends.

[0057] In practical applications, the first dive involves all followers beginning their descent, while the navigator remains afloat. During the dive, the navigator performs underwater acoustic positioning on each follower every Δt time interval and sends the positioning results back to the corresponding follower in real time. The first dive concludes when all followers have descended to near the seabed, i.e., when the Doppler logs are effectively aligned with the bottom. At the end of this dive, the navigator uses an ultra-short baseline positioning system to centrally correct the positions of the followers, as detailed below: Step 1: The navigator floats on the water, At that moment, the ultra-short array located the numbered [number missing]. The coordinates of the follower in the matrix body coordinate system are The satellite positioning information received at the corresponding time is The distance between the BeiDou positioning system, a leader in navigation, and the ultra-short array is... The navigator's real-time heading .

[0058] Step 2: Using a general method, based on , and Calculate The position of the ultra-short time array is .

[0059] Step 3: Calculate the time it takes for the positioning query signal to travel from the follower to the navigator: ; in, The preset sound velocity can be set to 1500 m / s.

[0060] Step 4: Based on the state of the ultrashort array and positioning distance It can be solved Location information of the constant follower: ; Where g is the coordinate transformation function; The navigator sends this location information to the follower via underwater acoustic communication.

[0061] Step 5: Numbered followers in The follower receives the information sent by the navigator at all times. Location at any moment Followers The positioning result of the time-based integrated navigation is ,exist The result of the time-based combined navigation positioning is .

[0062] Step 6: The follower calculates the distance and flight path between the two points using established methods. to Time of sailing distance and direction : ; Where f is a geodetic calculation function.

[0063] Step 7: with Ultra-short time array positioning point Starting from, calculate towards Direction offset Distance after the current moment The coordinates are the exact coordinates of the current location: ; Where h is the geodetic forward calculation function.

[0064] Step 8: The fiber optic inertial navigation system uses the current position... Starting from this point, dead reckoning continues, allowing navigation and positioning to restart with an accurate position reference. This effectively solves the problem of increased positioning errors during descent when the Doppler log fails to align with the bottom, resulting in the fiber optic inertial navigation system being in a purely inertial navigation state.

[0065] S40: In response to the second batch of dive commands, controls the navigator to perform the dive operation and controls multiple followers to hover and wait in a preset area.

[0066] In this step, in response to the second batch of dive commands, the navigator is controlled to begin the dive operation, while the multiple followers that have completed the first batch of dives are controlled to hover at low speed within a preset level and depth range.

[0067] S50: According to a preset time interval, obtain the second underwater acoustic positioning position information and the navigation status information of each follower, and perform multiple rounds of positioning correction for the navigator based on the second underwater acoustic positioning position information and the navigation status information.

[0068] In this step, during the navigator's descent, it periodically positions each follower using an ultra-short array system at preset time intervals. Within each acoustic positioning cycle, the navigator acquires the second acoustic positioning position information of each follower. Subsequently, it receives navigation status information reported by each follower via an acoustic communication device. Based on this information, the navigator performs spatiotemporal alignment and data fusion of the second acoustic positioning positions of each follower with their reported status information. Using a cooperative positioning inverse kinematics algorithm, it calculates multiple position estimates for the navigator itself and employs a filtering fusion method to obtain the optimal navigator position at the current moment. This allows for real-time correction of the navigator's laser-inertial navigation system, maintaining its high-precision positioning capability even in environments without satellite signals.

[0069] In one embodiment of this application, a specific navigator positioning correction scheme is provided. In S50, the second underwater acoustic positioning position information and the navigation status information of each follower are obtained at preset time intervals. Based on the second underwater acoustic positioning position information and the navigation status information, the navigator is corrected in multiple rounds. Specifically, this includes the following steps S51-S56: S51: For any round of positioning correction, the navigator sends positioning query signals to each follower sequentially through the ultra-short array system and receives response signals from each follower.

[0070] In this step, within each positioning correction cycle, the navigator, through its onboard ultra-short array system, sequentially sends positioning query signals to each follower according to their assigned numbers. Upon receiving the query signal, each follower immediately responds via its onboard acoustic beacon, sending a response signal containing its own identifier and timestamp information. This ordered query-response mechanism allows the navigator to conduct independent underwater acoustic positioning observations of each follower individually, avoiding signal collisions caused by simultaneous responses from multiple nodes. This ensures the reliability and timing controllability of the positioning process, providing ordered observational data input for subsequent distance and azimuth calculations based on the response signals.

[0071] S52: Based on the response signals from each follower, the navigator generates the second underwater acoustic positioning information of each follower at the moment the navigator receives the response signal, and converts the second underwater acoustic positioning information into coordinate values ​​in the matrix coordinate system.

[0072] In this step, the navigator generates second underwater acoustic positioning information based on the response signals from each follower at the moment the navigator receives the response signal. This second underwater acoustic positioning information includes the real-time distance and azimuth observations of the followers relative to the navigator at the moment the response signal is received. Subsequently, the distance and azimuth observations in spherical coordinates are converted into coordinate values ​​in a Cartesian coordinate system according to the geometric model of the array, providing a standardized spatial vector input for subsequent coordinate transformations based on the navigator's own position and attitude.

[0073] S53: The navigator calculates the one-way propagation delay of the response signal from each follower to the navigator based on the preset sound speed and the coordinates of each follower.

[0074] In this step, the navigator calculates the linear geometric distance between each follower and the center of the navigator's ultrashort array based on their coordinates. Then, combining this with a preset average speed of sound, it calculates the one-way time delay required for the sound wave signal to propagate from each follower to the navigator using the ratio of distance to speed of sound. This propagation delay is used in subsequent time synchronization processing to compensate for the positioning observation time to the actual signal transmission time, providing a crucial time compensation parameter for accurately determining the follower's position at the signal transmission time.

[0075] S54: After each follower completes underwater acoustic positioning, each follower obtains its own navigation status information and sends it to the navigator in sequence via acoustic positioning beacons. The navigation status information includes: the time of information transmission, depth information, altitude information, heading information, speed information, pitch angle information, and latitude and longitude information.

[0076] In this step, after each follower responds to the navigator's positioning query signal, each follower acquires its own navigation status information. Each follower sequentially transmits this navigation status information to the navigator via an acoustic positioning beacon. This navigation status information includes: the time of transmission, current depth, altitude above ground, real-time heading, ground speed, pitch angle, and latitude and longitude calculated by its own integrated navigation system. Through this structured reporting of information, the navigator can obtain the real-time motion status and position changes of each follower after the positioning time, providing accurate and complete dynamic data input for subsequent collaborative positioning inverse kinematics, state fusion, and formation scheduling.

[0077] S55: The navigator generates the navigator's accurate position information at the current moment based on the navigation status information and coordinate values ​​of each follower.

[0078] In this step, the navigator first uses the position, velocity, and heading data contained in the navigation status information of each follower, combined with the reported timestamps, to perform motion compensation and calculate the actual position of each follower at the time the positioning signal was transmitted. Then, this calculated position is fused with the relative coordinate values ​​measured by the ultra-short array system. Using a cooperative positioning inverse algorithm, multiple position estimates of the navigator itself at the time of signal reception are derived. Finally, a filtering and fusion method is used to process these estimates, generating the optimal and accurate position information for the current moment, which is used to perform real-time correction of the navigator's own laser-inertial navigation system.

[0079] In one embodiment of this application, a specific scheme for generating the current position information of a navigator is provided. In step S55, the navigator generates the accurate position information of the navigator at the current moment based on the navigation status information and coordinate values ​​of each follower. This specifically includes the following steps S551-S556: S551: The navigator determines the distance traveled by each follower from the moment it sends a response signal to the moment it sends navigation status information, based on the speed information.

[0080] In this step, the navigator multiplies the speed data included in the navigation status information reported by each follower by the time difference between when each follower sends a response signal and when it sends its navigation status information, to calculate the actual navigation distance of each follower relative to the seabed during that time period. This navigation distance is used to subsequently extrapolate the position from the location observation time to the time of status information reporting, so as to achieve spatiotemporal consistency and alignment between the location data and the status information.

[0081] S552: The navigator determines the fourth position information of each follower at the moment of sending a response signal, based on the latitude and longitude information, heading information, and travel distance of each follower.

[0082] In this step, the navigator uses the latitude and longitude information of each follower at the time they send their response signal as a starting point. Based on their heading information and the distance traveled by each follower from the time they sent their response signal to the time they sent their navigation status information, the navigator calculates the estimated position of each follower at the time they sent their response signal, and records this as the fourth position information. This fourth position information reflects the absolute position of the follower at the time the response signal was sent.

[0083] S553: ​​The navigator determines the fifth position information of the ultra-short array system at the moment when each follower sends a response signal, based on the fourth position information and coordinate values.

[0084] In this step, the navigator uses the fourth position information of each follower at the moment of sending the response signal as a known reference point. Combined with the coordinate values ​​of each follower relative to the navigator at that moment, measured by the ultra-short array system, the fifth position information of the ultra-short array system on the navigator is calculated using a cooperative positioning inverse kinematics algorithm. Since the ultra-short array system is fixedly installed on the navigator, and its measured coordinate values ​​are all referenced to the geometric center of the system, the fifth position information calculated by the inverse kinematics algorithm completely coincides with the navigator's position in space. In other words, the fifth position information directly reflects the navigator's accurate position at the moment each follower sends the response signal. Through this method, the navigator can achieve accurate calculation and state correction of its own position.

[0085] S554: The navigator obtains the third positioning result of the laser inertial navigation system at the moment when each follower sends a response signal, and the fourth positioning result of the laser inertial navigation system at the current moment.

[0086] In this step, the navigator obtains the positioning position of the integrated navigation system at the time when each follower sends a response signal, based on its own laser inertial integrated navigation system, and records it as the third positioning result, as well as the fourth positioning result of the laser inertial integrated navigation system at the current time.

[0087] S555: The navigator determines the distance and direction of travel from the moment each follower sends a response signal to the present moment, based on the third and fourth positioning results.

[0088] In this step, the navigator uses the third positioning result from the time each follower sends a response signal, and the fourth positioning result at the current time, obtained through geodetic inverse calculation or planar approximation, to obtain the spatial straight-line distance between the two positions as the navigation distance. It also calculates the direction angle from the time each follower sends a response signal to the current position as the navigation direction. This navigation distance and direction reflect the navigator's own motion state during the response signal processing.

[0089] S556: Based on the fifth position information, the sailing distance and the sailing direction, the navigator determines the sixth position information of the navigator at the current moment, and according to the preset fusion strategy, fuses the sixth position information of multiple followers to obtain the accurate position information of the navigator at the current moment.

[0090] In this step, the navigator takes the fifth position information (i.e., the inverse solution position at the time when each follower sends a response signal) as the starting point, and uses the calculated sailing distance and sailing direction from the time each follower sends a response signal to the current time to convert the sailing distance and direction into position offsets in the geographic coordinate system using the coordinate forward calculation method. This offset is then superimposed on the fifth position information to obtain the navigator's position at the current time, which is recorded as the sixth position information.

[0091] Furthermore, based on the self-position estimates (i.e., multiple sixth position information) obtained by each follower through inverse solving, the navigator uses a preset fusion algorithm to process these estimates. Taking into account the accuracy of each follower's positioning observation, communication delay, and uncertainty of motion state, the navigator generates a globally optimal accurate position information at the current moment by optimizing weight allocation and state covariance management. This information serves as the final correction input for the navigator's integrated navigation system, thereby significantly improving the navigator's positioning accuracy and robustness in underwater environments without satellite signals.

[0092] Optionally, the preset fusion algorithm can be Kalman filtering, weighted average, or covariance cross-fusion, and this application does not make specific limitations.

[0093] S56: The navigator updates the navigation position of the laser inertial navigation system using the accurate position information as the initial value, and completes the position correction of the laser inertial navigation system.

[0094] In this step, the navigator inputs the fused, accurate current position information into its onboard laser-inertial navigation system, updating the position state vector and related covariance matrix in the navigation filter to achieve position correction. Through this reset operation, the navigator's navigation system restarts dead reckoning from this accurate starting point, eliminating accumulated position deviations caused by inertial device drift and acoustic measurement errors. This restores and maintains high accuracy and stability in subsequent navigation processes, providing a reliable positioning benchmark for subsequent coordinated operations within the formation.

[0095] S60: When the Doppler log on the Navigator is detected to be in a valid bottom alignment state, the second batch of dive operations is terminated.

[0096] In this step, during the navigator's descent, the Doppler log's alignment status is continuously monitored to determine if it has reached a valid seabed depth and stably locked onto the seabed. When all Doppler logs on the navigator are detected as being in a valid alignment state, the second descent operation is considered complete, and this phase of the descent process ends. At this point, all members of the entire formation have completed the descent from the surface to near the seabed and completed the corresponding position corrections, laying the foundation for subsequent near-bottom coordinated operations.

[0097] In practical applications, the second dive involves the navigator beginning its descent while the followers maintain a circling, waiting position. During this waiting period, the followers continuously perform autonomous positioning using their fiber-optic inertial navigation system. Throughout the descent, the navigator continues to perform underwater acoustic positioning of the followers at intervals of Δt, while each follower simultaneously transmits its own integrated navigation information to the navigator. Based on the followers' position information and the underwater acoustic positioning results, the navigator inversely calculates multiple estimates of its own position and fuses these estimates to obtain its accurate position information at the current moment. This process continues until the navigator reaches the seabed, where its Doppler log is effectively aligned with the bottom and corrects its position. The specific procedure is as follows: Step 1: The navigator uses an ultra-short baseline positioning system to poll and locate the followers. The time measurement yielded the number as The distance of the follower in the ultra-short array body coordinate system is .

[0098] Step 2: Calculate the time it takes for the positioning query signal to travel from the follower to the navigator: ; in, The preset speed of sound can be set to 1500 m / s; Step 3: The navigator in Received status information from the follower, the status information includes timestamp information. Follower's current depth, altitude, and heading speed Current longitude ,latitude It may also include the target longitude, target latitude, status code, and mission of the near-bottom navigation operation.

[0099] Step 4: Navigator Calculation Always follow the position, the follower is In time, by speed If sailing, the sailing distance is: According to latitude and longitude of the moment Combined with heading and sailing distance It can be solved Location coordinates: ; Step 5: Calculate the navigator's position. Location coordinates at that moment: ; Step 6: Calculate the navigator's... Distance traveled from time to the present and direction of navigation , The current position coordinates of the Navigator integrated navigation system are as follows: and Then it can be calculated. ; Step 7: Calculate the current coordinates of the navigator's position: ; Step 8: Use the Kalman filter fusion method to inversely solve for the coordinates of the leader from each follower. The fusion yields the accurate navigator position coordinates at the current moment. ; Step 9: The laser inertial navigation system uses the corrected current position Starting from this point, dead reckoning continues, allowing navigation and positioning to restart with an accurate position reference. This effectively solves the problem of increased positioning errors during descent when the Doppler log is ineffective at ground level, resulting in the fiber optic inertial navigation system being in a purely inertial navigation state.

[0100] S70: Performs near-bottom navigation positioning and communication operations for the formation according to a preset cycle to complete the coordinated position correction of the formation members.

[0101] In this step, after the formation completes its dive and enters the near-bottom navigation phase, a structured and time-sequential collaborative positioning and information exchange process is executed cyclically according to a pre-set communication cycle. Within each communication cycle, the navigator initiates underwater acoustic positioning for each follower in the order of their formation member numbers. After completing underwater acoustic positioning, it receives navigation status information reported by each follower. Subsequently, based on the acquired positioning observations and status data, the navigator performs formation position fusion correction, generates updated overall formation positioning information, and broadcasts it to all members via underwater acoustic communication. Through this periodic and orderly communication scheduling mechanism, continuous synchronization of positioning information and reliable interaction of mission commands among formation members are achieved, thereby enabling real-time acquisition and maintenance of the formation's overall positioning accuracy and communication connectivity, providing stable information support for collaborative control and mission execution in complex underwater environments.

[0102] In one embodiment of this application, a specific near-bottom navigation positioning and communication scheme is provided. In S70, that is, according to a preset cycle, near-bottom navigation positioning and communication operations are performed on the formation to complete the coordinated position correction of the formation members. Specifically, it includes the following steps S71-S73: S71: During near-bottom navigation operations, the navigator sequentially performs underwater acoustic positioning on each follower based on the number of the multiple followers according to a preset cycle, in order to obtain the underwater acoustic positioning results of the formation.

[0103] In this step, during near-bottom navigation operations, the navigator sequentially performs underwater acoustic positioning on each follower based on their assigned numbers, according to a preset cycle, to obtain the formation's underwater acoustic positioning results. Specifically, at the start of each cycle, the navigator, based on the pre-set follower numbering order, sends positioning query signals to each follower one by one through its onboard ultra-short array system and receives corresponding response signals, thereby sequentially obtaining the real-time distance and bearing information of each follower relative to the navigator. This orderly, time-divisional positioning mechanism effectively avoids overlapping and collisions in the response signals of multiple followers, ensuring that the underwater acoustic positioning observation of each follower is independent and complete, thus forming the overall underwater acoustic positioning results of the formation within that cycle, providing an accurate observational data foundation for subsequent formation position fusion and status updates.

[0104] S72: Determine the communication interval between multiple followers based on the preset underwater acoustic positioning distance threshold, the number of followers, and the preset sound speed.

[0105] In this step, to ensure that the response signals between adjacent followers do not overlap, an underwater acoustic positioning distance threshold (l) is set according to the maximum expected operating range of the formation. Combining the number of followers (N) and the average sound speed in the water (v), the communication interval for each follower to send status information is calculated using the formula 2Nl / v, thereby achieving orderly and collision-free timing scheduling within the formation.

[0106] S73: After underwater acoustic positioning is completed, each follower reports its navigation status information to the navigator in sequence based on its number and communication interval. After receiving the navigation status information reported by each follower, the navigator sends a response confirmation signal to that follower.

[0107] In this step, after underwater acoustic positioning is completed, each follower sequentially reports its navigation status information to the navigator based on its assigned number and calculated communication interval. Each follower, according to its sequence number within the formation, transmits structured data containing its current position, speed, heading, depth, and operational status via acoustic positioning beacons within the corresponding time slot after the positioning phase ends. Upon receiving the navigation status information reported by each follower, the navigator immediately sends a response confirmation signal containing a reception confirmation identifier and a timestamp via underwater acoustic communication to the respective follower, ensuring the reliability and timing control of information transmission. This mechanism not only guarantees the orderly and complete reporting of status information but also provides real-time and reliable data input for subsequent formation status fusion and task scheduling.

[0108] In practical application scenarios, such as Figure 4 The diagram illustrates the formation positioning and communication process during near-bottom navigation operations. The communication sequence is based on a cycle T. At the start of each cycle, the navigator initiates underwater acoustic positioning with each follower sequentially according to preset numbers (1, 2...i...N-1) to obtain the relative position information of each member. After the positioning phase, each follower, according to its number sequence, actively broadcasts its own status information, such as position, speed, heading, and operational status, to the navigator within a designated time slot. If the navigator does not receive status information from a follower within a preset time period, it initiates an active inquiry mechanism, sending an inquiry command to that follower or its neighboring nodes via underwater acoustic communication. The navigator performs task scheduling and path planning based on the acquired overall formation status information, generating corresponding control commands or task adjustment information. The navigator broadcasts the merged and corrected overall formation status, scheduling commands, and other public information to all followers via underwater acoustic communication, achieving information synchronization. Outside of the fixed time sequence, any formation member can initiate point-to-point or broadcast communication at any time based on its own status changes or task requirements, achieving event-driven real-time information interaction.

[0109] In one embodiment of this application, a specific communication relay scheme is provided, which further includes the following steps during the communication phase: During the communication process, if the navigator does not receive the navigation status information reported by any follower within a preset time period, the follower is determined to be in a communication interruption state. Based on the underwater acoustic positioning results, the target follower that is closest to the follower in the communication interruption state is identified as the relay node in the formation; The navigator forwards the scheduling information to the followers that are in a communication interruption state through the target followers.

[0110] In this embodiment, during communication, if the navigator does not receive navigation status information reported by any follower within a preset time period, the follower is determined to be in a communication interruption state. At this time, based on the underwater acoustic positioning results acquired in the current period, the navigator identifies the target follower in the formation that is spatially closest to the follower in the communication interruption state as the target relay node. Subsequently, the navigator sends the scheduling information or query commands that need to be sent to the interrupted follower to the target follower via an acoustic positioning beacon, and then the target follower forwards them to the follower in the communication interruption state. This relay mechanism effectively overcomes direct communication obstacles caused by excessive distance or local channel obstruction, improving the robustness and coverage of formation communication.

[0111] Optionally, if the same follower cannot resume communication with it directly or via relay within two consecutive communication cycles, it is marked as out of formation, triggering a corresponding formation reorganization or task adjustment strategy. For situations where the navigator cannot receive follower information due to members being outside the communication range, an active query strategy is adopted: at time N×2l / v+N×2l / v after the start of each communication round, an active query is initiated for followers who have not reported status information. During the query, the current position is estimated based on the previous round's positioning results, and the nearest target follower is selected as the relay node for forwarding. That is, the navigator first sends the instruction information to the relay follower, which then forwards it to the target follower. The maximum duration of this process is 2N×2l / v.

[0112] Furthermore, after acquiring the status information of all followers, the navigator performs fusion correction on the followers' positions. This correction process is the same as the algorithm and the initial dive phase, the only difference being that the navigator's own position information is replaced by integrated navigation system positioning instead of surface GPS positioning; the remaining steps are completely identical. After correction, the navigator broadcasts the corrected position information and the overall formation status information to all members. After the broadcast ends (i.e., at time 10Nl / v), the active communication process concludes, and the random communication listening phase begins. In this phase, all nodes have the same communication priority and can send information to designated formation members at any time.

[0113] In practical application scenarios, as shown in Figures 5(a) to 5(d), the process flow diagrams for the diving and near-bottom navigation of a multi-underwater robot formation are presented. Specifically, the diving phase refers to the process from the formation descending from the surface to the point where the Doppler log is effectively aligned with the bottom. The diving process adopts a batch diving method. The first batch consists of all followers diving, while the navigator floats on the surface. The followers quickly dive to the seabed. After diving to the seabed, the navigator measures the distance and bearing of all followers using an ultra-short array. Combined with the navigator's satellite positioning position, the absolute position of each follower is calculated and transmitted to the followers via underwater acoustic communication to complete the position correction of the followers. After the follower positions are corrected, the navigator begins its descent, marking the second descent. During the descent, the distance and bearing of the followers are measured again using an ultra-short array. The navigator's position information is then deduced from the position information of each follower. This deduced position information is fused to obtain the accurate navigator position, completing the navigator's position correction. This process continues until the Doppler log on the navigator is effectively aligned with the bottom, completing the second descent of the navigator. During the formation's near-bottom navigation operations, considering both communication and positioning timeliness requirements, an efficient communication and positioning sequence is designed. This enables underwater acoustic communication between the navigator and all followers, and positioning of followers via the ultra-short array, effectively avoiding communication collisions among formation members. Each formation member has a time-keeping function, conducting a round of positioning communication at fixed intervals T within the designed communication sequence framework, based on communication needs. Each communication and positioning cycle consists of two parts: a fixed positioning communication process and random communication monitoring. In the fixed positioning process, the navigator uses an ultra-short array for positioning, followers upload status information, and the navigator broadcasts information about the formation members. Formation members are required to maintain precise timeliness and communicate strictly according to the agreed-upon schedule. During the random monitoring process, any formation member can initiate communication at any time and broadcast status changes to the operational formation in real time.

[0114] As can be seen, in the above scheme, during the first batch of dives, relying on the high-precision satellite positioning benchmark provided by the surface navigator and combining it with the ultra-short baseline system to correct the position of the followers significantly improved the positioning accuracy of the followers upon reaching the seabed, providing accurate initial position information for its integrated navigation system. Building on this, in the second batch of dives, the navigator used the initial position information of multiple followers as known reference points, and by fusing underwater acoustic positioning data with its own navigation status information, achieved inverse positioning and correction. Through the phased dives and multi-round collaborative correction mechanism, the accumulation of positioning errors caused by the integrated navigation system operating in pure inertial mode for extended periods during deep dives was effectively suppressed, ensuring that the entire formation possessed high-precision navigation capabilities after completing the dive. While maintaining relatively low system costs, the positioning accuracy and system robustness of underwater formation collaborative operations were significantly improved.

[0115] In one embodiment, a cooperative positioning and communication device for underwater robots is provided, applied to a single-leader master-slave formation scenario, wherein the formation includes one leader and multiple followers. This cooperative positioning and communication device for underwater robots corresponds one-to-one with the cooperative positioning and communication method for underwater robots described in the above embodiments. Figure 6 As shown, the cooperative positioning and communication device 100 for the underwater robot includes: a control module 101, a first acquisition module 102, and a second acquisition module 103. Detailed descriptions of each functional module are as follows: The control module 101 is used to respond to the first batch of dive commands, control multiple followers to perform dive operations, and control the navigator to remain floating on the water surface. The first acquisition module 102 is used to acquire the satellite positioning location information and heading information of the navigator, as well as the first underwater acoustic positioning location information of each follower, according to a preset time interval, and to perform multiple rounds of collaborative positioning correction for multiple followers based on the satellite positioning location information, heading information and first underwater acoustic positioning location information. The control module 101 is also used to terminate the first batch of dive operations when it is detected that the Doppler logs carried by each follower are in a valid bottom alignment state. The control module 101 is also used to control the navigator to perform a diving operation in response to the second batch of diving instructions, and to control multiple followers to hover and wait in a preset area; The second acquisition module 103 is used to acquire the second underwater acoustic positioning position information and the navigation status information of each follower according to a preset time interval, and to perform multiple rounds of positioning correction on the navigator based on the second underwater acoustic positioning position information and the navigation status information. The control module 101 is also used to terminate the second batch of dive operations when it detects that the Doppler log on the Navigator is in a valid bottom-alignment state. The control module 101 is also used to perform near-bottom navigation positioning and communication operations on the formation according to a preset cycle, and to complete the coordinated position correction of the formation members.

[0116] In one embodiment, the navigator is equipped with a BeiDou positioning system, a laser inertial navigation system, an ultra-short array system, and an underwater acoustic communication device; each follower is equipped with a fiber optic inertial navigation system and an acoustic positioning beacon. The laser inertial navigation system includes a laser inertial navigation system and a Doppler log; the fiber optic inertial navigation system includes a fiber optic inertial navigation system and a Doppler log. The first acquisition module 102 is specifically used for: For any round of collaborative positioning correction, the navigator obtains satellite positioning information through the BeiDou positioning system and heading information through the laser inertial navigation system. Based on satellite positioning information, heading information, and the installation distance between the ultra-short array system and the BeiDou positioning system, the Navigator calculates the first position information of the ultra-short array system in its own coordinate system. The navigator sends positioning query signals to each follower sequentially through an ultra-short array system and receives response signals from each follower. Based on the response signals from each follower, the navigator generates the first underwater acoustic positioning information of each follower at the moment the navigator receives the response signal. The first underwater acoustic positioning information includes the distance and orientation of each follower relative to the navigator. Based on the first underwater acoustic positioning information, the navigator determines the coordinates of each follower in the array body coordinate system; The navigator calculates the one-way propagation delay of the response signal from each follower to the navigator based on the preset speed of sound and the coordinates of each follower. Based on the first position information, coordinate values, and one-way propagation delay, the navigator calculates the second position information of each follower at the time when they send a response signal to the navigator, and sends the second position information to each follower through an underwater acoustic communication device; Each follower determines the navigator's third position information based on the received second position information, and performs positioning correction on its own fiber optic inertial navigation system based on the third position information.

[0117] In one embodiment, the first acquisition module 102 is further configured to: Each follower receives a second location information sent by the navigator via an onboard acoustic positioning beacon; Each follower obtains its first positioning result from its fiber optic inertial navigation system at the moment it sends a response signal to the navigator, and its second positioning result from its fiber optic inertial navigation system at the current moment, through its own onboard fiber optic inertial navigation system. Each follower determines its navigation data from the time it sent the response signal to the current time based on the first and second positioning results; Each follower uses the second position information received from the navigator as a reference, combines it with navigation data, calculates the navigator's third position information at the current moment, and uses the third position information as the initial value to update the navigation position of the fiber optic inertial navigation system, thus completing the position correction of the fiber optic inertial navigation system.

[0118] In one embodiment, the second acquisition module 103 is specifically used for: For any round of positioning correction, the navigator sends positioning query signals to each follower sequentially through the ultra-short array system and receives response signals from each follower. Based on the response signals from each follower, the navigator generates the second underwater acoustic positioning position information of each follower at the moment the navigator receives the response signal, and converts the second underwater acoustic positioning position information into coordinate values ​​in the matrix coordinate system. The navigator calculates the one-way propagation delay of the response signal from each follower to the navigator based on the preset speed of sound and the coordinates of each follower. After each follower completes its underwater acoustic positioning, each follower acquires its own navigation status information and transmits it to the navigator sequentially via an acoustic positioning beacon. The navigation status information includes: the time of transmission, depth, altitude, heading, speed, pitch angle, and latitude and longitude. Based on the navigation status information and coordinate values ​​of each follower, the navigator generates the navigator's accurate position information at the current moment; The navigator uses the accurate location information as the initial value to update the navigation position of the laser inertial navigation system, thus completing the position correction of the laser inertial navigation system.

[0119] In one embodiment, the second acquisition module 103 is further configured to: Based on speed information, the navigator determines the distance traveled by each follower from the moment it sends a response signal to the moment it sends navigation status information. The navigator determines the fourth position information of each follower at the moment of sending a response signal, based on the latitude and longitude information, heading information, and travel distance of each follower. Based on the fourth position information and coordinate values, the navigator determines the fifth position information of the ultra-short array system at the moment when each follower sends a response signal; The navigator obtains the third positioning result of the laser inertial navigation system at the moment when each follower sends a response signal, and the fourth positioning result of the laser inertial navigation system at the current moment; Based on the third and fourth positioning results, the navigator determines the distance and direction of travel from the moment each follower sends a response signal to the current moment. Based on the fifth position information, the distance traveled, and the direction of travel, the navigator determines its sixth position information at the current moment, and merges the sixth position information of multiple followers according to a preset fusion strategy to obtain the accurate position information of the navigator at the current moment.

[0120] In one embodiment, the control module 101 is further configured to: During near-bottom navigation operations, the navigator sequentially performs underwater acoustic positioning on each follower based on the number of the multiple followers according to a preset cycle, in order to obtain the underwater acoustic positioning results of the formation. Based on a preset underwater acoustic positioning distance threshold, the number of followers, and a preset sound speed, the communication interval between multiple followers is determined. Once underwater acoustic positioning is complete, each follower reports its navigation status information to the navigator in sequence, based on its number and communication interval. After receiving the navigation status information reported by each follower, the navigator sends a response confirmation signal back to that follower.

[0121] In one embodiment, the device further includes: The judgment module is used to determine that, during the communication process, if the navigator does not receive the navigation status information reported by the follower within a preset time period, the follower is in a communication interruption state. The determination module is used to identify the target follower that is closest to the follower in the formation that is in a communication interruption state, based on the underwater acoustic positioning results, as the relay node; The sending module is used by the navigator to forward scheduling information to followers that are in a communication interruption state through the target follower.

[0122] This invention provides a cooperative positioning and communication device 100 for underwater robots. In the first batch of dives, relying on the high-precision satellite positioning benchmark provided by the surface navigator, and combined with an ultra-short baseline system, the position of the followers is corrected, significantly improving the positioning accuracy of the followers upon reaching the seabed and providing accurate initial position information for their integrated navigation system. Based on this, in the second batch of dives, the navigator uses the initial position information of multiple followers as known reference points, and achieves inverse positioning and correction of its own position by fusing underwater acoustic positioning data with its own navigation status information. Through batch dives and multi-round cooperative correction mechanisms, the accumulation of positioning errors caused by the integrated navigation system operating in pure inertial mode for extended periods during deep dives is effectively suppressed, ensuring that the entire formation possesses high-precision navigation capabilities after completing the dive. While maintaining relatively low system costs, this significantly improves the positioning accuracy and system robustness of underwater formation cooperative operations.

[0123] Specific limitations regarding the cooperative positioning and communication device for underwater robots can be found in the limitations on the cooperative positioning and communication method for underwater robots described above, and will not be repeated here. Each module in the aforementioned cooperative positioning and communication device for underwater robots can be implemented entirely or partially through software, hardware, or a combination thereof. These modules can be embedded in hardware or independently of the processor in the electronic device, or stored in software in the memory of the electronic device, so that the processor can call and execute the corresponding operations of each module.

[0124] In one embodiment, an electronic device is provided, including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements the above-described cooperative positioning and communication method for an underwater robot.

[0125] In one embodiment, a computer-readable storage medium is provided, which stores a computer program that, when executed by a processor, implements the above-described cooperative positioning and communication method for an underwater robot.

[0126] It should be noted that the functions or steps that can be implemented by the computer-readable storage medium or electronic device described above can be referred to the relevant descriptions on the server side and client side in the foregoing method embodiments. To avoid repetition, they will not be described one by one here.

[0127] Those skilled in the art will understand that all or part of the processes in the methods of the above embodiments can be implemented by a computer program instructing related hardware. The computer program can be stored in a non-volatile computer-readable storage medium, and when executed, it can include the processes of the embodiments of the above methods. Any references to memory, storage, databases, or other media used in the embodiments provided in this application can include non-volatile and / or volatile memory. Non-volatile memory can include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM), or flash memory. Volatile memory can include random access memory (RAM) or external cache memory. By way of illustration and not limitation, RAM is available in various forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), dual data rate SDRAM (DDRSDRAM), enhanced SDRAM (ESDRAM), synchronous link DRAM (SLDRAM), Rambus direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and memory bus dynamic RAM (RDRAM), etc.

[0128] Those skilled in the art will clearly understand that, for the sake of convenience and brevity, the above-described division of functional units and modules is used as an example. In practical applications, the above functions can be assigned to different functional units and modules as needed, that is, the internal structure of the device can be divided into different functional units or modules to complete all or part of the functions described above.

[0129] The above-described embodiments are only used to illustrate the technical solutions of the present invention, and are not intended to limit it. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention, and should all be included within the protection scope of the present invention.

Claims

1. A cooperative localization and communication method for underwater robots, characterized in that, Applied to a single-leader master-slave formation scenario, where the formation includes one leader and multiple followers, the method includes: In response to the first batch of dive commands, the system controls the multiple followers to perform dive operations and controls the navigator to remain floating on the water surface. According to a preset time interval, the satellite positioning location information and heading information of the navigator, as well as the first underwater acoustic positioning location information of each follower, are acquired. Based on the satellite positioning location information, the heading information and the first underwater acoustic positioning location information, multiple rounds of collaborative positioning correction are performed on the multiple followers. The first dive operation ended when it was detected that the Doppler logs on each follower were in a valid bottom-alignment state. In response to the second batch of dive commands, the navigator is controlled to perform a dive operation, and the multiple followers are controlled to hover and wait in a preset area; According to the preset time interval, the second underwater acoustic positioning position information and the navigation status information of each follower are obtained, and the navigator is corrected in multiple rounds based on the second underwater acoustic positioning position information and the navigation status information. The second dive operation will end when the Doppler log on the navigator is detected to be in a valid bottom-alignment state. According to the preset cycle, the formation will perform near-bottom navigation positioning and communication operations to complete the coordinated position correction of the formation members.

2. The cooperative positioning and communication method for underwater robots according to claim 1, characterized in that, The navigator is equipped with a BeiDou positioning system, a laser inertial navigation system, an ultra-short array system, and an underwater acoustic communication device. Each follower is equipped with a fiber optic inertial navigation system and an acoustic positioning beacon. The laser inertial navigation system includes a laser inertial navigation system and a Doppler log, and the fiber optic inertial navigation system includes a fiber optic inertial navigation system and a Doppler log. The step of acquiring the satellite positioning information and heading information of the navigator, as well as the first underwater acoustic positioning information of each follower, at preset time intervals, and performing multi-round collaborative positioning corrections on the multiple followers based on the satellite positioning information, the heading information, and the first underwater acoustic positioning information, specifically includes: For any round of collaborative positioning correction, the navigator obtains satellite positioning information through the BeiDou positioning system and heading information through the laser inertial navigation system. Based on the satellite positioning information, the heading information, and the installation distance between the ultra-short array system and the BeiDou positioning system, the navigator calculates the first position information of the ultra-short array system in its own coordinate system. The navigator sequentially sends positioning query signals to each follower through the ultra-short array system and receives response signals from each follower. The navigator generates first underwater acoustic positioning information for each follower at the moment the navigator receives the response signal, based on the response signals fed back by each follower. The first underwater acoustic positioning information includes the distance and orientation of each follower relative to the navigator. The navigator determines the coordinates of each follower in the array body coordinate system based on the first underwater acoustic positioning information. The navigator calculates the one-way propagation delay of the response signal from each follower to the navigator based on a preset sound speed and the coordinate values ​​of each follower; Based on the first position information, the coordinate value, and the one-way propagation delay, the navigator calculates the second position information of the time when each follower sends a response signal to the navigator, and sends the second position information to each follower through the underwater acoustic communication device. Each follower determines the navigator's third position information at the current moment based on the received second position information, and performs positioning correction on its respective fiber optic inertial navigation system based on the third position information.

3. The cooperative positioning and communication method for underwater robots according to claim 2, characterized in that, The steps of each follower determining the navigator's third position information at the current moment based on the received second position information, and performing positioning correction on its respective fiber optic inertial navigation system based on the third position information, specifically include: Each follower receives the second location information sent by the navigator via the acoustic positioning beacon it carries; Each follower obtains its first positioning result from the fiber optic inertial navigation system at the moment it sends a response signal to the navigator, and its second positioning result from the fiber optic inertial navigation system at the current moment, through its own fiber optic inertial navigation system. Each follower determines its navigation data from the time it sent the response signal to the current time based on the first positioning result and the second positioning result; Each follower uses the second position information received from the navigator as a reference, combines it with the navigation data, calculates the navigator's third position information at the current moment, and uses the third position information as the initial value to update the navigation position of the fiber optic inertial navigation system, thus completing the position correction of the fiber optic inertial navigation system.

4. The cooperative positioning and communication method for underwater robots according to claim 1, characterized in that, The step of obtaining the second underwater acoustic positioning position information and navigation status information of each follower at preset time intervals, and performing multiple rounds of positioning correction for the navigator based on the second underwater acoustic positioning position information and navigation status information, specifically includes: For any round of positioning correction, the navigator sequentially sends positioning query signals to each follower through the ultra-short array system and receives response signals from each follower. Based on the response signals fed back by each follower, the navigator generates the second underwater acoustic positioning position information of each follower at the moment the navigator receives the response signal, and converts the second underwater acoustic positioning position information into coordinate values ​​in the matrix coordinate system. The navigator calculates the one-way propagation delay of the response signal from each follower to the navigator based on a preset sound speed and the coordinate values ​​of each follower; After each follower completes underwater acoustic positioning, each follower acquires its own navigation status information and sends it to the navigator in sequence via acoustic positioning beacons. The navigation status information includes: information transmission time, depth information, altitude information, heading information, speed information, pitch angle information, and latitude and longitude information. The navigator generates its accurate position information at the current moment based on the navigation status information and coordinate values ​​of each follower. The navigator uses the accurate location information as an initial value to update the navigation position of the laser inertial navigation system, thus completing the position correction of the laser inertial navigation system.

5. The cooperative positioning and communication method for underwater robots according to claim 4, characterized in that, The step of generating the navigator's accurate position information at the current moment based on the navigation status information and coordinate values ​​of each follower specifically includes: The navigator determines the distance traveled by each follower from the moment it sends a response signal to the moment it sends navigation status information, based on the speed information. The navigator determines the fourth position information of each follower at the moment of sending the response signal based on the latitude and longitude information, heading information, and travel distance of each follower. The navigator determines the fifth position information of the ultra-short array system at the moment when each follower sends a response signal, based on the fourth position information and coordinate values. The navigator obtains the third positioning result of the laser inertial navigation system at the moment when each follower sends a response signal, and the fourth positioning result of the laser inertial navigation system at the current moment. Based on the third and fourth positioning results, the navigator determines the sailing distance and sailing direction from the moment each follower sends a response signal to the current moment. Based on the fifth position information, the sailing distance, and the sailing direction, the navigator determines its sixth position information at the current moment, and according to a preset fusion strategy, fuses the sixth position information corresponding to multiple followers to obtain the accurate position information of the navigator at the current moment.

6. The cooperative positioning and communication method for underwater robots according to claim 1, characterized in that, The step of performing near-bottom navigation positioning and communication operations on the formation according to a preset cycle to complete the coordinated position correction of the formation members specifically includes: During near-bottom navigation operations, according to the preset cycle, the navigator sequentially performs underwater acoustic positioning on each follower based on the number of the multiple followers to obtain the underwater acoustic positioning results of the formation. Based on a preset underwater acoustic positioning distance threshold, the number of followers, and a preset sound speed, the communication interval between the multiple followers is determined. Once underwater acoustic positioning is complete, each follower reports its navigation status information to the navigator in sequence based on its number and communication interval. After receiving the navigation status information reported by each follower, the navigator sends a response confirmation signal back to that follower.

7. The cooperative positioning and communication method for underwater robots according to claim 6, characterized in that, Also includes: During the communication process, if the navigator does not receive the navigation status information reported by any follower within a preset time period, the follower is determined to be in a communication interruption state. Based on the underwater acoustic positioning results, the target follower that is closest to the follower in the communication interruption state in the formation is identified as the relay node; The navigator forwards the scheduling information to the followers that are in a communication interruption state through the target follower.

8. A cooperative positioning and communication device for an underwater robot, characterized in that, Applied to a single-leader master-slave formation scenario, wherein the formation includes one leader and multiple followers, the device includes: The control module is used to respond to the first batch of dive commands, control the multiple followers to perform dive operations, and control the navigator to remain floating on the water surface. The first acquisition module is used to acquire the satellite positioning location information and heading information of the navigator, as well as the first underwater acoustic positioning location information of each follower, according to a preset time interval, and to perform multiple rounds of collaborative positioning correction on the multiple followers based on the satellite positioning location information, the heading information and the first underwater acoustic positioning location information. The control module is also used to terminate the first batch of dive operations when it is detected that the Doppler logs on each follower are in a valid bottom-alignment state. The control module is also used to, in response to the second batch of dive commands, control the navigator to perform a dive operation and control the multiple followers to hover and wait in a preset area; The second acquisition module is used to acquire the second underwater acoustic positioning position information and the navigation status information of each follower according to a preset time interval, and to perform multiple rounds of positioning correction on the navigator based on the second underwater acoustic positioning position information and the navigation status information. The control module is also used to terminate the second batch of dive operations when it detects that the Doppler log on the Navigator is in a valid bottom-alignment state. The control module is also used to perform near-bottom navigation positioning and communication operations for the formation according to a preset cycle, and to complete the coordinated position correction of the formation members.

9. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the computer program, it implements the steps of the cooperative positioning and communication method for an underwater robot as described in any one of claims 1 to 7.

10. A computer-readable storage medium storing a computer program, characterized in that, When the computer program is executed by the processor, it implements the steps of the cooperative positioning and communication method for an underwater robot as described in any one of claims 1 to 7.

Images