A distributed multi-aquatic robot asynchronous cooperative positioning method and system, medium

Abstract

The application provides a distributed multi-underwater robot asynchronous cooperative positioning method and system and a medium. The application belongs to the field of underwater positioning. The method is designed for the mutual observation data transmission delay or out-of-order problem caused by the sparse network topology and low communication efficiency in the underwater cooperative positioning process. Two key processes and modules, namely, underwater robot local sparse factor graph construction and parallel factor graph reasoning, are designed. A local sparse factor graph is constructed at each underwater robot end to uniformly represent state variables and measurement data. Based on the factor graph structure, real-time estimation and robot inter-observation fusion based on asynchronous communication are decoupled through a parallel reasoning framework to realize real-time asynchronous estimation, thereby realizing distributed and asynchronous updating of multi-underwater robot cooperative positioning.

Description

Technical Field

[0001] This invention relates to the field of underwater positioning technology, and in particular to a distributed asynchronous cooperative positioning method, system, and medium for multiple underwater robots. Background Technology

[0002] Cooperative localization technology improves the localization performance of underwater robots by sharing mutual observation data among them. This enables multiple underwater robots to maintain better localization capabilities in anchorless underwater environments, thereby expanding the underwater operational range and application areas of multi-underwater robots and improving underwater operational accuracy and operating time. However, in complex underwater environments, multi-underwater robot networks often exhibit sparse topology and low communication efficiency, leading to delays or out-of-order transmission of mutual observation data.

[0003] Traditional cooperative localization methods typically neglect network connectivity issues, assuming that relative observation data can be transmitted instantly when available. Some studies have considered the weak connectivity between underwater robot networks, but also take into account probabilistic packet loss and sparse network topology. Furthermore, some optimization-based methods estimate the attitude sequence within a time window by solving optimization problems, effectively addressing the latency and out-of-order issues caused by mutual observation information transmission. However, these methods require solving complex optimization problems at each step, placing high demands on the underwater robot's performance and impacting the real-time performance of localization estimation. Summary of the Invention

[0004] The purpose of this invention is to achieve robust localization of multiple underwater robots in constrained connectivity network conditions, solve the dependence of underwater robots on central nodes, and improve the adaptability of cooperative localization systems to weak underwater connectivity networks. A distributed asynchronous cooperative localization method, system, and medium for multiple underwater robots are proposed.

[0005] The present invention is achieved through the following technical solution: the present invention proposes a distributed asynchronous cooperative localization method for multiple underwater robots, the method including an underwater robot local sparse factor graph construction stage and an underwater robot parallel factor graph inference stage;

[0006] The underwater robot's local sparse factor graph construction stage comprises four parts: creating real-time state variable nodes, creating measurement factor nodes, selecting key frame variable nodes, and communication updates. The underwater robot uses a factor graph model to construct real-time navigation state variable nodes. It acquires body attitude angle, acceleration, angular velocity, and linear velocity measurements through an attitude reference system, inertial navigation system, and Doppler velocimeter. These measurements are then used to add factor node constraints to the underwater robot's navigation state variables, retaining only critical navigation state variable nodes to avoid excessive redundancy in the factor graph. Distance and relative displacement measurements between underwater robots are obtained via an acoustic modem and forward-looking sonar. After establishing communication links between the mutually measuring underwater robots, corresponding mutual measurement factor nodes are added to each, thus realizing the construction of local sparse factor graphs for each underwater robot.

[0007] The underwater robot's parallel factor graph inference stage comprises three parts: real-time single-step inference, interval inference based on communication updates, and callback update synchronization. For a locally sparse factor graph, the underwater robot uses multiple parallel threads to infer the state variables in the graph. The main thread performs only real-time single-step inference, estimating the latest underwater robot state based on the previous state and current measurement information. Other parallel threads are created after the underwater robot establishes a communication link and completes communication transmission. Based on the transmitted mutual measurement information, they insert corresponding variable nodes and factor nodes into the locally sparse factor graph, and then perform interval state inference estimation from the insertion point to the communication time. After the interval inference estimation is completed, a callback update synchronization is inserted in the main thread to synchronize the interval inference estimation result to the latest underwater robot navigation state estimate, achieving the fusion of real-time estimation and asynchronous update thread estimation results.

[0008] Furthermore, in the real-time state variable node creation part of the underwater robot local sparse factor graph construction stage, each underwater robot creates a navigation state variable node containing time attributes at each time step. Then, a composite odometry factor is constructed using dead reckoning odometry information calculated by the attitude reference system, inertial navigation system, and Doppler velocimeter. This composite odometry factor is used to connect the state variable nodes of the current time step and the next time step. The composite odometry factor accumulates the increment between multiple state variable nodes, and the constraint between any two state variable nodes is constructed through this increment.

[0009] Furthermore, in the part of creating measurement factor nodes during the construction phase of the local sparse factor graph of the underwater robot, a one-sided measurement factor is created and added to the latest state variable node based on all the measurement information at the current moment. The measurement factor only constrains the state at the current moment.

[0010] Furthermore, in the selection of key frame variable nodes during the construction stage of the local sparse factor graph of the underwater robot, the aim is to retain state variable nodes that have an important impact on the overall estimation, while discarding historical state variable nodes that are not of interest.

[0011] Furthermore, in the communication update part of the underwater robot's local sparse factor graph construction stage, after the underwater robot obtains mutual observation measurements, it exchanges the measurements and prior state information through the communication channel; the communication update is intermittently triggered, and the communication update is an asynchronous update of the local sparse factor graph.

[0012] Furthermore, in the real-time single-step reasoning part of the parallel factor graph reasoning stage of the underwater robot, this part serves as the main thread for cooperative localization estimation of the underwater robot. It constructs a single-step graph reasoning from the variable node of the previous keyframe to the current variable node. The current state variable node is only constrained by the current composite odometry factor and the current measurement factor.

[0013] Furthermore, in the interval reasoning part based on communication updates during the parallel factor graph reasoning stage of the underwater robot, a parallel thread independent of the main thread is created. This thread is triggered only after the communication interaction of mutual measurement information between underwater robots is completed. The mutual measurement information and the prior state information of neighboring underwater robots are inserted into the local sparse factor graph at the corresponding time through communication updates. Information transmission and variable reasoning estimation are performed in the interval from the insertion time to the current time. This thread is independent of the main thread before completion and does not affect the estimation results of the main thread.

[0014] Furthermore, in the callback update synchronization part of the parallel factor graph inference stage of the underwater robot, this part is only called after any parallel thread ends. It runs on the main thread. Since the interval length of the communication update thread is uncertain, the interval inference time is uncertain. During this process, the main thread may create new keyframe variable nodes, which will cause the interval inference result based on the communication update to be unable to be fed back to the latest time. Therefore, the callback update synchronization can merge the results of the parallel thread inference estimation with the latest estimation of the main thread to achieve asynchronous fusion characteristics.

[0015] The present invention also proposes a distributed multi-underwater robot asynchronous cooperative localization system, the system comprising an underwater robot local sparse factor graph construction module and an underwater robot parallel factor graph inference module;

[0016] The underwater robot local sparse factor graph construction module: The underwater robot uses a factor graph model to construct navigation state variable nodes. It acquires body attitude angle, acceleration, angular velocity, and linear velocity measurement information through an attitude reference system, inertial navigation system, and Doppler velocimeter. It uses this measurement information to add factor node constraints to the underwater robot's navigation state variables, and retains only the key navigation state variable nodes to avoid excessive redundancy in the factor graph. The underwater robots obtain distance and relative displacement measurements between them through an acoustic modem and forward-looking sonar. After establishing a communication link between the underwater robots that measure each other, corresponding mutual measurement factor nodes are added to each of them to realize the construction of local sparse factor graphs for each underwater robot.

[0017] The underwater robot's parallel factor graph inference module works as follows: For a locally sparse factor graph, the underwater robot uses multiple parallel threads to infer the state variables in the graph. The main thread only runs real-time single-step inference, estimating the latest underwater robot state based on the previous state and current measurement information. Other parallel threads are created after the underwater robot establishes a communication link and completes communication transmission. Based on the transmitted mutual measurement information, they insert corresponding variable nodes and factor nodes into the locally sparse factor graph, and then perform interval state inference estimation from the insertion point to the communication time. After the interval inference estimation is completed, a callback update synchronization is inserted in the main thread to synchronize the result of the interval inference estimation to the latest underwater robot navigation state estimation, realizing the fusion of real-time estimation and asynchronous update thread estimation results.

[0018] The present invention also proposes a computer-readable storage medium for storing computer instructions, which, when executed by a processor, implement the steps of the distributed multi-underwater robot asynchronous cooperative localization method.

[0019] The beneficial effects of this invention are:

[0020] 1. This invention can flexibly integrate non-co-frequency sensor measurements during the collaborative positioning process of multiple underwater robots, and through parallel inference structure design, ensure real-time navigation state estimation of underwater robots.

[0021] 2. This invention considers the scalability of multi-underwater robot cooperative navigation systems. By constructing a local sparse factor graph for each robot, it ensures that the robot can flexibly estimate its navigation state based on available measurement information without relying on a centralized data fusion center, thereby improving the system's robustness and scalability.

[0022] 3. This invention addresses the issues of underwater acoustic channel delay, long communication intervals, and low communication rates in multi-underwater robot cooperative localization. After the communication channel is established, historical mutual observation measurement information is asynchronously constructed and added to avoid affecting the real-time estimation of the main thread and improve the stability of the cooperative localization system. Attached Figure Description

[0023] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on the provided drawings without creative effort.

[0024] Figure 1 This is a technical roadmap for the present invention.

[0025] Figure 2 A roadmap for constructing local sparse factor maps for underwater robots.

[0026] Figure 3 A schematic diagram of the construction of a local sparse factor map for an underwater robot.

[0027] Figure 4 This is a parallel factor graph inference route diagram for an underwater robot.

[0028] Figure 5 This is a schematic diagram of parallel factor graph inference for an underwater robot. Detailed Implementation

[0029] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0030] Combination Figures 1-5 This invention proposes a distributed asynchronous cooperative localization method for multiple underwater robots, the method comprising S1, the underwater robot local sparse factor graph construction stage and S2, the underwater robot parallel factor graph inference stage;

[0031] S1, the underwater robot local sparse factor graph construction stage: The underwater robot uses a factor graph model to construct navigation state variable nodes. It obtains body attitude angle, acceleration, angular velocity and linear velocity measurement information through the attitude reference system, inertial navigation system and Doppler velocimeter. It uses these measurement information to add factor node constraints to the underwater robot's navigation state variables, and only retains the key navigation state variable nodes to avoid excessive redundancy in the factor graph. The underwater robots can obtain distance and relative displacement measurements between the robots through acoustic modem and forward-looking sonar. After establishing a communication link between the underwater robots that measure each other, corresponding mutual measurement factor nodes are added to each of them to realize the construction of local sparse factor graphs for each underwater robot.

[0032] S2, the parallel factor graph inference stage for underwater robots: Each underwater robot performs inference on its own local sparse factor graph to obtain the optimal posterior estimate of its own state; a single-step inference estimation module is built through the main thread to ensure the real-time performance of the underwater robot's positioning estimation; after establishing a communication link between the underwater robots that measure each other, an independent parallel thread is created to infer and estimate the distribution of state variables within the historical time window; after the parallel thread ends, a callback is inserted into the main thread to update the synchronous inference module to achieve asynchronous positioning updates.

[0033] Furthermore, in S1, the underwater robot local sparse factor graph construction stage, each robot constructs a local sparse factor graph using its available measurement data to represent the measurement constraints on the navigation state. The frequently updated inertial navigation system measurements constitute the reference time step of the factor graph. This stage comprises four main steps: S1.1, creating real-time state variable nodes; S1.2, creating measurement factor nodes; S1.3, selecting keyframe variable nodes; and S1.4, communication updates.

[0034] Step S1.1, creating real-time state variable nodes, establishes a specific representation of the real-time state of each underwater robot in its own factor graph. At each time step, a navigation state variable node containing time attributes is created and initialized. Then, an odometry factor is constructed using dead reckoning odometry information calculated by the attitude reference system, inertial navigation system, and Doppler velocimeter. This factor is used to connect the state variable nodes of the current time step and the next time step. Since the dead reckoning odometry updates frequently, creating a state variable node for each odometry update would result in an overly dense factor graph. Therefore, a composite odometry factor is designed. This factor accumulates the increments between multiple state variable nodes and uses these increments to construct constraints between any two state variable nodes.

[0035] S1.2, creating measurement factor nodes, involves creating and adding measurement factors to the latest state variable nodes based on all measurement information at the current moment. These measurements are typically robot measurements relative to a reference coordinate system, such as linear velocity, attitude, or position measurements. These measurements only constrain the current state, so these factors can be modeled using one-sided factors. Furthermore, for mutual measurements between underwater robots, since this information needs to be exchanged through the communication channel between the underwater robots for effective fusion, this measurement is not created in this step. Instead, it is backed up and stored by the underwater robot, and then recreated and inserted into the local sparse factor graph after subsequent communication interactions.

[0036] S1.3, selecting keyframe variable nodes, aims to retain state variable nodes that have a significant impact on the overall estimation, while discarding historical state variable nodes that are not of interest, thereby ensuring the sparsity of the local factor graph and improving computational efficiency. The selection of keyframe variable nodes is flexible, essentially a trade-off between computational efficiency and estimation accuracy. On the one hand, this avoids the waste of computational resources caused by an overly dense factor graph; on the other hand, historical keyframes can effectively account for the asynchronous characteristics caused by time delays and out-of-order measurements. In this invention, states with absolute position measurements are set as keyframe variable nodes. Underwater robots typically can only obtain absolute position measurements through beacons or by surfacing and using a global navigation and positioning system. Such measurements are usually sparse, but they can effectively suppress the divergence of dead reckoning. Therefore, the state variable nodes corresponding to these measurements are set as keyframes. The state of mutual measurement between underwater robots is not set as a keyframe because some mutual measurements are usually updated frequently. For example, optical cameras or forward-looking sonar can provide data at tens of hertz. However, the actual underwater communication channel can only exchange some mutual measurement information. Therefore, this mutual measurement information is only stored locally. After subsequent communication is triggered, the mutual measurement factors are inserted into the local factor graph based on the mutual measurement information that was actually triggered for interaction.

[0037] The core of S1.4, communication update, lies in the adaptive factor graph update of the limited connectivity multi-underwater robot communication network. Underwater robots construct their own local sparse factor graph over time using the aforementioned three steps and measurement data. After obtaining mutual observation measurements, the underwater robots need to exchange these measurements with prior state information via a communication channel. However, the underwater communication network cannot meet the requirements for real-time and efficient information exchange between robots. Therefore, communication updates are intermittently triggered. Only after the underwater robots have completed communication and obtained the corresponding information, based on the exchanged mutual observation information and the time of occurrence, the state variable node, mutual observation factor node, neighbor state variable node, and their prior pose factor node are inserted into the local sparse factor graph. This update is an asynchronous update of the local sparse factor graph.

[0038] Furthermore, in S2, the parallel factor graph inference stage for underwater robots, a parallel inference method for locally sparse factor graphs is designed to achieve real-time and asynchronous communication updates during the cooperative localization process of multiple underwater robots. The inference of factor graph state variables is based on Gaussian belief propagation. Each node transmits information to its neighboring nodes, and the posterior distribution of the variable is obtained through belief updates of the variable nodes. Multiple iterations ensure continuous information transmission on the factor graph until the estimation results of the variable nodes converge. This stage comprises three main steps: S2.1, real-time single-step inference; S2.2, interval inference based on communication updates; and S2.3, callback update synchronization.

[0039] S2.1, real-time single-step inference, is the main thread for underwater robot localization estimation. It constructs a single-step graph inference from the variable node of the previous keyframe to the current variable node. The current state variable node is only constrained by the current composite odometry factor and the current measurement factor. This single-step inference estimation process is similar to a one-step update of a Bayesian filter, which can efficiently achieve real-time state estimation.

[0040] S2.2, the interval inference based on communication updates, is a parallel thread independent of the main thread. It is triggered only after the communication exchange of mutual measurement information between underwater robots is completed. The underwater robot will create an independent parallel thread to insert the mutual measurement information and the prior state information of neighboring underwater robots into the local sparse factor graph at the corresponding time point through communication updates. Information transfer and variable inference estimation will be performed within the interval from the insertion time to the current time point. This thread operates independently of the main thread until its completion and will not affect the estimation results of the main thread.

[0041] S2.3, the callback update synchronization, is invoked only after the interval inference based on communication updates is completed, and it runs on the main thread. Since the interval length of the communication update thread is uncertain, the time of interval inference is also uncertain. During this process, the main thread may create new keyframe variable nodes, which may cause the interval inference result based on communication updates to fail to reflect the latest time. Therefore, when the interval inference thread based on communication updates completes, a callback update synchronization from the communication trigger time to the latest state time is inserted on the main thread. This synchronizes the results of the parallel threads with the latest state estimate of the main thread, achieving the fusion of results from multiple threads.

[0042] The present invention also proposes a distributed multi-underwater robot asynchronous cooperative localization system, the system comprising an underwater robot local sparse factor graph construction module and an underwater robot parallel factor graph inference module;

[0043] The underwater robot local sparse factor graph construction module dynamically constructs the underwater robot's local factor graph using measurement information from the attitude reference system, inertial navigation system, Doppler velocimeter, acoustic modem and forward-looking sonar, establishes the temporal relationship between the underwater robot's navigation state and time and measurement, and makes the local factor graph more sparse by using composite odometry factors, thereby improving computational efficiency.

[0044] The underwater robot's parallel factor graph inference module works as follows: For a locally sparse factor graph, the underwater robot uses multiple parallel threads to infer the state variables in the graph. The main thread only runs real-time single-step inference, estimating the latest underwater robot state based on the previous state and current measurement information. Other parallel threads are created after the underwater robot establishes a communication link and completes communication transmission. Based on the transmitted mutual measurement information, they insert corresponding variable nodes and factor nodes into the locally sparse factor graph, and then perform interval state inference estimation from the insertion point to the communication time. After the interval inference estimation is completed, a callback update synchronization is inserted in the main thread to synchronize the result of the interval inference estimation to the latest underwater robot navigation state estimation, realizing the fusion of real-time estimation and asynchronous update thread estimation results.

[0045] Example

[0046] This invention proposes a distributed asynchronous cooperative localization method for multiple underwater robots. For example... Figure 1 As shown, the method addresses the challenges of heterogeneous sensor measurement, time-varying sensor network topology, and high-latency underwater acoustic communication encountered during collaborative localization of multiple underwater robots. It uses factor graph modeling to uniformly represent all available measurement information locally on each underwater robot and designs a parallel inference architecture to achieve real-time and asynchronous localization state estimation. The method consists of two main stages: S1, the underwater robot local sparse factor graph construction stage, and S2, the underwater robot parallel factor graph inference stage.

[0047] The S1 stage, the underwater robot local sparse factor graph construction stage, includes four main steps: S1.1, creating real-time state variable nodes; S1.2, creating measurement factor nodes; S1.3, selecting keyframe variable nodes; and S1.4, communication updates. Figure 2 As shown, the process begins with creating real-time state variable nodes, followed by creating measurement factor nodes and selecting whether to set them as keyframe variable nodes. After the underwater robot transmits mutual measurement information, a communication update is performed, inserting the transmitted mutual measurement factor nodes and corresponding variable nodes into the local sparse factor graph. This process is repeated iteratively, continuously constructing the local sparse factor graph over time with new measurement values.

[0048] S1.1, Create real-time state variable nodes, such as Figure 3 As shown, the underwater robot is in The navigation status at any given time is ,in Let be the rotation matrix of the attitude. Linear velocity, For the location, its estimated value is expressed as ,from At the start of each moment, the underwater robot sequentially creates the current navigation state variable node. , , At each time step, the state variable node is linked to the previous keyframe's state variable using a composite odometry factor. The time-variable node is connected, and the measurement value of the composite odometry is obtained by accumulating the state increment at each time step, for example, in... At any given moment, the underwater robot is in a certain state. Starting from the initial state, the state increment between the two steps is calculated using the current sensor measurements, denoted as . The current composite odometer measurement value can be assigned using this state increment. At any given moment, first create a new navigation state variable. , in state Given the initial state, calculate the new state increment. Then delete the variable node and add a new composite odometer factor connection. and Its measured value is And so on, until... The variable factor is set as a keyframe variable node. Set as and repeat the above from The variable construction process that begins at any given moment.

[0049] S1.2, Create measurement factor nodes. After each navigation state variable node is created, such as... The associated measurements, including attitude rotation matrix, linear velocity, or position, are connected to the variable node via one-sided factor nodes. Because of the presence of these measurement factor nodes, the impact of the measurements on the state is preserved when a non-keyframe variable node is deleted. In the state estimation information, then through The measurements are retained in the composite odometer. In addition, when constructing the composite odometer factor, besides the measured values, the covariance matrix also needs to be calculated. Here, a covariance update method based on extended Kalman filtering is designed:

[0050] ,

[0051] ,

[0052] in and These are the coefficient matrices of the error state equation. Let covariance matrix be the variance matrix. , and These are the Jacobian matrix and covariance matrix of the measured information, respectively. Using these two formulas, the values ​​from any starting time to... The covariance matrix of the time-time composite odometer factor.

[0053] S1.3 Selecting keyframe variable nodes aims to retain variable nodes that have a significant impact on positioning estimation, thereby reducing factor graph redundancy and improving computational efficiency. In this embodiment, only state variable nodes with absolute position measurements are retained, because these variable nodes can obtain absolute position references, thus ensuring the overall trajectory estimation accuracy.

[0054] S1.4 Communication Update aims to update the underwater robots' communication after completing the mutual measurement information transmission. Based on this information, the underwater robots then create and insert their corresponding variable and factor nodes into the local coefficient factor graph. For example, in... The underwater robot completes the task at any time. Real-time communication and transmission of measurement information, underwater robot creation The state variable factor nodes, measurement factor nodes, composite odometry factor nodes on both sides of the insertion point, mutual measurement factor nodes, neighbor state variable factor nodes, and neighbor pose factor nodes corresponding to each time point are identified, and these nodes are inserted into the corresponding time points of the original local sparse factor graph. Figure 3 middle The corresponding dashed box section.

[0055] The S2 parallel factor graph reasoning stage for underwater robots comprises three main steps: S2.1, real-time single-step reasoning; S2.2, interval reasoning based on communication updates; and S2.3, callback update synchronization. Figure 4 As shown, real-time single-step inference runs as the main thread, and at each time step, it checks whether the mutual observation information between the underwater robots has been successfully transmitted. If the transmission is successful, a new parallel thread independent of the main thread is created, and all variables within the interval from the mutual observation time to the thread creation time are inferred on this thread. Simultaneously, the main thread monitors whether the parallel thread has terminated. If it has, a callback update synchronization is used to synchronize the inference results of the parallel thread from the thread creation time to the latest time of the main thread, achieving alignment between the estimation results of the main thread and the parallel thread. Figure 5 (a) is a schematic diagram of the local sparsity factor. Figure 5 The diagram in (b) illustrates the changes in the thread state over time. The following explanation of the three main steps is based on this diagram.

[0056] S2.1 Real-time Single-Step Inference: This step serves as the main thread for factor graph inference, as shown in the green section of the diagram. Each time, only the latest state variable node is estimated. The estimation result is recursively obtained from the initial time step. Therefore, this step is similar to a filter-based estimation method, considering only one update result, thus exhibiting good real-time performance. After creating the state variable node and measurement factor node corresponding to that moment, estimate .

[0057] S2.2, Interval reasoning based on communication updates, assuming the underwater robot is in The mutual observation information communication transmission was completed at a certain time, and the corresponding mutual observation time was [time value missing]. After the communication update in phase S1.4, the local sparse factor graph was inserted. The corresponding variable and factor nodes are observed at each moment. Then, the underwater robot creates an independent parallel thread, as shown in the orange thread in the figure, for inference. to All state variable nodes at any given time.

[0058] S2.3 Callback Update Synchronization: Since parallel threads do not affect the main thread, when the parallel thread completes inference, such as... At that moment, the main thread created a new keyframe state. The inference estimation results of the main thread and the parallel threads were not synchronized. Therefore, a callback update step was inserted in the main thread, as shown in purple in the diagram, to achieve synchronization from... arrive The estimation results are synchronized. Since only keyframe states are retained when creating the factor graph, callback updates only need to handle the synchronization of a few key states, so it will not have a significant impact on real-time performance.

[0059] The present invention also proposes a distributed multi-underwater robot asynchronous cooperative localization system, the system comprising an underwater robot local sparse factor graph construction module and an underwater robot parallel factor graph inference module;

[0060] The underwater robot local sparse factor graph construction module dynamically constructs the underwater robot's local factor graph using measurement information from the attitude reference system, inertial navigation system, Doppler velocimeter, acoustic modem and forward-looking sonar, establishes the temporal relationship between the underwater robot's navigation state and time and measurement, and makes the local factor graph more sparse by using composite odometry factors, thereby improving computational efficiency.

[0061] The underwater robot's parallel factor graph inference module works as follows: For a locally sparse factor graph, the underwater robot uses multiple parallel threads to infer the state variables in the graph. The main thread only runs real-time single-step inference, estimating the latest underwater robot state based on the previous state and current measurement information. Other parallel threads are created after the underwater robot establishes a communication link and completes communication transmission. Based on the transmitted mutual measurement information, they insert corresponding variable nodes and factor nodes into the locally sparse factor graph, and then perform interval state inference estimation from the insertion point to the communication time. After the interval inference estimation is completed, a callback update synchronization is inserted in the main thread to synchronize the result of the interval inference estimation to the latest underwater robot navigation state estimation, realizing the fusion of real-time estimation and asynchronous update thread estimation results.

[0062] The present invention also proposes a computer-readable storage medium for storing computer instructions, which, when executed by a processor, implement the steps of the distributed multi-underwater robot asynchronous cooperative localization method.

[0063] The memory in this application embodiment can be volatile memory or non-volatile memory, or it can include both volatile and non-volatile memory. The non-volatile memory can be read-only memory (ROM), programmable read-only memory (PROM), erasable programmable read-only memory (EPROM), electrically erasable programmable read-only memory (EEPROM), or flash memory. The volatile memory can be random access memory (RAM), which is used as an external cache. By way of example, but not limitation, many forms of RAM are available, such as static random access memory (SRAM), dynamic random access memory (DRAM), synchronous dynamic random access memory (SDRAM), double data rate synchronous dynamic random access memory (DDRSDRAM), enhanced synchronous dynamic random access memory (ESDRAM), synchronous linked dynamic random access memory (SLDRAM), and direct rambus RAM (DR RAM). It should be noted that the memory used in the methods described in this invention is intended to include, but is not limited to, these and any other suitable types of memory.

[0064] In the above embodiments, implementation can be achieved, in whole or in part, through software, hardware, firmware, or any combination thereof. When implemented in software, it can be implemented, in whole or in part, as a computer program product. The computer program product includes one or more computer instructions. When the computer instructions are loaded and executed on a computer, all or part of the processes or functions described in the embodiments of this application are generated. The computer can be a general-purpose computer, a special-purpose computer, a computer network, or other programmable device. The computer instructions can be stored in a computer-readable storage medium or transmitted from one computer-readable storage medium to another. For example, the computer instructions can be transmitted from one website, computer, server, or data center to another via wired (e.g., coaxial cable, fiber optic, digital subscriber line (DSL)) or wireless (e.g., infrared, wireless, microwave, etc.) means. The computer-readable storage medium can be any available medium accessible to a computer or a data storage device such as a server or data center that integrates one or more available media. The available media may be magnetic media (e.g., floppy disks, hard disks, magnetic tapes), optical media (e.g., high-density digital video discs (DVDs)), or semiconductor media (e.g., solid-state disks (SSDs)).

[0065] In implementation, each step of the above method can be completed by integrated logic circuits in the processor's hardware or by instructions in software. The steps of the method disclosed in the embodiments of this application can be directly implemented by a hardware processor, or by a combination of hardware and software modules in the processor. The software modules can reside in random access memory, flash memory, read-only memory, programmable read-only memory, electrically erasable programmable memory, registers, or other mature storage media in the art. This storage medium is located in memory, and the processor reads information from the memory and, in conjunction with its hardware, completes the steps of the above method. To avoid repetition, detailed descriptions are omitted here.

[0066] It should be noted that the processor in the embodiments of this application can be an integrated circuit chip with signal processing capabilities. During implementation, each step of the above method embodiments can be completed by the integrated logic circuitry in the processor's hardware or by instructions in software form. The processor can be a general-purpose processor, a digital signal processor (DSP), an application-specific integrated circuit (ASIC), a field-programmable gate array (FPGA), or other programmable logic devices, discrete gate or transistor logic devices, or discrete hardware components. It can implement or execute the methods, steps, and logic block diagrams disclosed in the embodiments of this application. The general-purpose processor can be a microprocessor or any conventional processor. The steps of the methods disclosed in the embodiments of this application can be directly embodied as execution by a hardware decoding processor, or as a combination of hardware and software modules in the decoding processor. The software modules can be located in random access memory, flash memory, read-only memory, programmable read-only memory, electrically erasable programmable memory, registers, or other mature storage media in the art. This storage medium is located in memory, and the processor reads the information in the memory and, in conjunction with its hardware, completes the steps of the above methods.

[0067] The above provides a detailed description of the distributed multi-underwater robot asynchronous cooperative localization method, system, and medium proposed in this invention. Specific examples have been used to illustrate the principles and implementation methods of this invention. The descriptions of the above embodiments are only for the purpose of helping to understand the method and core ideas of this invention. At the same time, for those skilled in the art, there will be changes in the specific implementation methods and application scope based on the ideas of this invention. Therefore, the content of this specification should not be construed as a limitation of this invention.

Claims

1. A distributed asynchronous cooperative localization method for multiple underwater robots, characterized in that, The method includes a local sparse factor graph construction stage for underwater robots and a parallel factor graph inference stage for underwater robots. The underwater robot's local sparse factor graph construction stage comprises four parts: creating real-time state variable nodes, creating measurement factor nodes, selecting key frame variable nodes, and communication updates. The underwater robot uses a factor graph model to construct real-time navigation state variable nodes. It acquires body attitude angle, acceleration, angular velocity, and linear velocity measurements through an attitude reference system, inertial navigation system, and Doppler velocimeter. These measurements are then used to add factor node constraints to the underwater robot's navigation state variables, retaining only the critical navigation state variable nodes. Distance and relative displacement measurements between underwater robots are obtained via an acoustic modem and forward-looking sonar. After establishing a communication link between the mutually measuring underwater robots, corresponding mutual measurement factor nodes are added to each, thus realizing the construction of the local sparse factor graph for each underwater robot. The underwater robot's parallel factor graph inference stage comprises three parts: real-time single-step inference, interval inference based on communication updates, and callback update synchronization. For a locally sparse factor graph, the underwater robot uses multiple parallel threads to infer the state variables in the graph. The main thread performs only real-time single-step inference, estimating the latest underwater robot state based on the previous state and current measurement information. Other parallel threads are created after the underwater robot establishes a communication link and completes communication transmission. Based on the transmitted mutual measurement information, they insert corresponding variable nodes and factor nodes into the locally sparse factor graph, and then perform interval state inference estimation from the insertion point to the communication time. After the interval inference estimation is completed, a callback update synchronization is inserted in the main thread to synchronize the interval inference estimation result to the latest underwater robot navigation state estimate, achieving the fusion of real-time estimation and asynchronous update thread estimation results.

2. The method according to claim 1, characterized in that, In the real-time state variable node creation stage of the underwater robot local sparse factor graph construction phase, each underwater robot creates a navigation state variable node containing time attributes at each time step. Then, a composite odometry factor is constructed using dead reckoning odometry information calculated by the attitude reference system, inertial navigation system, and Doppler velocimeter. This composite odometry factor is used to connect the state variable nodes of the current time step and the next time step. The composite odometry factor accumulates the increment between multiple state variable nodes, and the constraint between any two state variable nodes is constructed through this increment.

3. The method according to claim 1, characterized in that, In the stage of constructing the local sparse factor graph of the underwater robot, in the part of creating measurement factor nodes, a one-sided measurement factor is created and added to the latest state variable node based on all the measurement information at the current moment. The measurement factor only constrains the state at the current moment.

4. The method according to claim 1, characterized in that, In the communication update part of the underwater robot's local sparse factor graph construction stage, after the underwater robot obtains mutual observation measurements, it exchanges the measurements and prior state information through the communication channel; the communication update is intermittently triggered, and the communication update is an asynchronous update of the local sparse factor graph.

5. The method according to claim 1, characterized in that, In the real-time single-step reasoning part of the parallel factor graph reasoning stage of the underwater robot, this part serves as the main thread for cooperative localization estimation of the underwater robot. It constructs a single-step graph reasoning from the variable node of the previous keyframe to the current variable node. The current state variable node is only constrained by the current composite odometry factor and the current measurement factor.

6. The method according to claim 1, characterized in that, In the interval reasoning part based on communication updates during the parallel factor graph reasoning stage of underwater robots, a parallel thread independent of the main thread is created. This thread is triggered only after the communication interaction of mutual measurement information between underwater robots is completed. The mutual measurement information of the communication interaction and the prior state information of the neighboring underwater robots are inserted into the local sparse factor graph at the corresponding time through communication updates. Information transmission and variable reasoning estimation are performed in the interval from the insertion time to the current time.

7. The method according to claim 1, characterized in that, In the callback update synchronization part of the parallel factor graph inference stage of the underwater robot, this part is called only after any parallel thread ends. It runs on the main thread. The callback update synchronization can merge the results of the inference estimation of the parallel thread with the latest estimation of the main thread to achieve asynchronous fusion characteristics.

8. A distributed asynchronous cooperative localization system for multiple underwater robots, characterized in that, The system includes a local sparse factor graph construction module for underwater robots and a parallel factor graph inference module for underwater robots. The underwater robot local sparse factor graph construction module: The underwater robot uses a factor graph model to construct navigation state variable nodes. It acquires body attitude angle, acceleration, angular velocity, and linear velocity measurement information through an attitude reference system, inertial navigation system, and Doppler velocimeter. It uses this measurement information to add factor node constraints to the underwater robot's navigation state variables, and retains only the key navigation state variable nodes. The underwater robots obtain distance and relative displacement measurements between them through an acoustic modem and forward-looking sonar. After establishing a communication link between the underwater robots that measure each other, corresponding mutual measurement factor nodes are added to each of them to realize the construction of local sparse factor graphs for each underwater robot. The underwater robot's parallel factor graph inference module works as follows: For a locally sparse factor graph, the underwater robot uses multiple parallel threads to infer the state variables in the graph. The main thread only runs real-time single-step inference, estimating the latest underwater robot state based on the previous state and current measurement information. Other parallel threads are created after the underwater robot establishes a communication link and completes communication transmission. Based on the transmitted mutual measurement information, they insert corresponding variable nodes and factor nodes into the locally sparse factor graph, and then perform interval state inference estimation from the insertion point to the communication time. After the interval inference estimation is completed, a callback update synchronization is inserted in the main thread to synchronize the result of the interval inference estimation to the latest underwater robot navigation state estimation, realizing the fusion of real-time estimation and asynchronous update thread estimation results.

9. A computer-readable storage medium for storing computer instructions, characterized in that, When the computer instructions are executed by the processor, they implement the steps of the method according to any one of claims 1-7.

Images