Multi-AUV cooperative SLAM method based on multi-beam water depth measurement data

Abstract

The invention belongs to the technical field of underwater navigation, and particularly relates to a multi-AUV (Autonomous Underwater Vehicle) collaborative SLAM (Simultaneous Localization and Mapping) method based on multi-beam water depth measurement data, which comprises the following steps: a plurality of AUVs synchronously run in a task area, and a multi-beam depth sounding sonar is respectively used for acquiring local water depth data and generating an internal water depth sub-graph set; each AUV performs sparse processing on the internal water depth sub-graph; the AUV receiving the sub-graph performs interpolation reconstruction on the sparse sub-graph by using a GPR algorithm; each AUV constructs a collaborative SLAM factor graph containing a pushing factor, an internal water depth closed-loop factor, an external water depth closed-loop factor and an inter-AUV distance observation factor based on self navigation data, the internal water depth sub-graph and the external water depth sub-graph; and solving the factor graph through a distributed optimization algorithm, and outputting an optimized pose and a global map of each AUV. According to the method, the positioning precision and reliability of the underwater SLAM are remarkably improved.

Description

Technical Field

[0001] The present invention belongs to the field of underwater navigation technology, and in particular relates to a multi-AUV collaborative SLAM method based on multi-beam water depth measurement data. Background Art

[0002] Underwater simultaneous localization and mapping (SLAM) technology is a key means to achieve autonomous detection of unknown marine environments. Existing technologies are mainly based on a single autonomous underwater vehicle (AUV). By carrying sensors such as multi-beam bathymetric sonar, scanning sonar or optical cameras, it collects seabed topography or visual feature data in real time, and combines filtering algorithms (such as extended Kalman filtering) or graph optimization methods to achieve AUV pose estimation and map construction. For example, through feature matching of multi-beam terrain point clouds, or using combined navigation data of inertial navigation systems (INS) and Doppler velocimeters (DVLs), single AUV SLAM can correct cumulative positioning errors to a certain extent. However, such methods still face significant limitations:

[0003] Navigation error accumulation problem: A single AUV relies on an inertial navigation system, and its pose estimation error accumulates over time. Especially in long missions or under complex ocean current interference, the error may increase rapidly, causing SLAM failure.

[0004] Environmental dependence: In areas where the seabed terrain features are sparse or highly repetitive (such as flat sand), the success rate of feature matching is greatly reduced, the SLAM system has difficulty generating effective closed-loop constraints, and the positioning accuracy drops sharply.

[0005] Insufficient efficiency and flexibility: A single AUV needs to repeatedly traverse the same area to obtain sufficient environmental information for trajectory correction, resulting in low mapping efficiency, and path planning is limited by the need to revisit historical trajectories.

[0006] In recent years, collaborative navigation of multiple AUVs has become an important research direction for improving the robustness of underwater positioning. Through information exchange between multiple AUVs (such as distance observation and state sharing), additional constraints can be introduced to suppress individual navigation errors. For example, the ultra-short baseline (USBL) system can obtain the relative distance between AUVs through underwater acoustic communication and combine it with collaborative filtering algorithms to optimize global positioning. However, existing collaborative technologies mostly focus on navigation data fusion and have not fully explored the potential of collaborative environmental perception of multiple AUVs:

[0007] Lack of data association: Traditional methods fail to effectively integrate heterogeneous environmental information collected by multiple AUVs (such as multi-beam terrain data), resulting in low utilization of environmental features.

[0008] Communication bandwidth limitation: The bandwidth of underwater acoustic communication is limited. Direct transmission of raw multi-beam data is unrealistic, and efficient data compression and reconstruction methods need to be designed.

[0009] Distributed optimization challenges: Multi-AUV collaborative SLAM requires a balance between local computation and global consistency. The existing centralized architecture is difficult to meet real-time and fault-tolerance requirements.

[0010] In summary, existing single-AUV SLAM technology is limited by error accumulation and environmental dependence, while multi-AUV collaborative navigation has yet to achieve a deep fusion of environmental perception and navigation constraints. Therefore, a collaborative SLAM method that can efficiently integrate multi-AUV terrain observation and distance constraints is urgently needed to improve positioning accuracy and mapping efficiency in complex marine environments. Summary of the Invention

[0011] In response to the above-mentioned prior art, the present invention provides a multi-AUV collaborative SLAM method based on multi-beam water depth measurement data.

[0012] In order to solve the above technical problems, the present invention proposes a multi-AUV collaborative SLAM method based on multi-beam water depth measurement data, comprising the following steps:

[0013] (1) Multiple autonomous underwater vehicles (AUVs) operate synchronously in the mission area. Each AUV uses a multi-beam bathymetric sonar to collect local water depth data in real time and divides the water depth data into multiple internal water depth sub-graphs according to a preset spatial resolution to form an internal water depth sub-graph set.

[0014] (2) Each AUV performs a sparse process on the internal water depth sub-map, including:

[0015] Construct the Delaunay triangulation of the sub-graph bathymetric points;

[0016] Select key bathymetric points based on slope, aspect characteristics and water depth difference thresholds;

[0017] The sparse sub-image is transmitted to other AUVs via underwater acoustic communication;

[0018] (3) The AUV receiving the sub-map uses the Gaussian process regression (GPR) algorithm to interpolate and reconstruct the external water depth sub-map of the continuous water depth distribution using the plane coordinates and water depth values ​​of the sparse sub-map as training samples to form an external water depth sub-map set

[0019] (4) Each AUV constructs a collaborative SLAM factor graph based on the following constraint factors:

[0020] Position factor: constructed from the motion increment and pose node of the AUV inertial navigation system;

[0021] Internal water depth closed-loop factor: generated by feature matching between sub-images of the same AUV;

[0022] External water depth closed-loop factor: generated by weighted iterative closest point ICP registration between the local sub-image and the external sub-image;

[0023] Inter-AUV distance observation factor: constructed by the relative distance between AUVs obtained by underwater acoustic communication;

[0024] (5) A distributed optimization algorithm is used to solve the factor graph, and the objective function is:

[0025]

[0026] Where, They represent the DR factor generated by the i-th AUV, the internal water depth closed-loop factor, the external water depth closed-loop factor and the distance observation factor between AUVs;

[0027] (6) Output the optimized pose and global map of each AUV to complete multi-AUV collaborative SLAM.

[0028] Preferably, the internal water depth subgraph is divided into:

[0029] The strip data of multibeam bathymetric sonar is divided into sub-graphs according to a fixed grid size or equal time intervals. Each sub-graph contains three-dimensional coordinates and corresponding water depth values.

[0030] Preferably, the specific steps of the sparse processing in step (2) include:

[0031] (a) Construct a Delaunay triangulation for each subgraph and calculate the slope characteristics f of the triangulation around each depth measurement point s and f a ;

[0032] (b) If the difference between the water depth value of a certain sounding point and the average water depth of its surrounding sounding points exceeds the preset water depth threshold, and the variation range of the slope or slope characteristics exceeds the corresponding threshold t b , and the variation range of slope or aspect characteristics exceeds the corresponding threshold t s and t a , then the sounding point is retained; otherwise it is discarded.

[0033] Preferably, the kernel function of the Gaussian process regression (GPR) algorithm in step (3) is defined as:

[0034] Among them, the scale matrix l x , l y is the spatial correlation length, is the kernel function variance; hyperparameter l x , l y , σ h Optimized by maximum likelihood estimation.

[0035] Preferably, the error function of the weighted ICP registration in step (4) is:

[0036]

[0037] Where w i is the weight, is the target sub-graph depth measurement point, The depth measurement point of the source sub-map.

[0038] Preferably, the distributed optimization algorithm in step (5) adopts an incremental least squares method based on QR decomposition. After each AUV independently solves the local factor graph, the global optimization results are synchronized through underwater acoustic communication.

[0039] Preferably, the clock synchronization between the AUVs is achieved by periodic hydroacoustic signal calibration, and the calibration period is consistent with the acquisition frequency of the distance observation factor.

[0040] Preferably, the method is applicable to the following scenarios:

[0041] Large-scale seafloor topography mapping;

[0042] Collaborative search and tracking of underwater targets;

[0043] Dynamic monitoring of marine environmental parameters.

[0044] Compared with the prior art, the present invention has the following beneficial effects:

[0045] The present invention significantly improves the positioning accuracy and reliability of underwater SLAM through the collaborative operation of multiple AUVs and the fusion of multi-source data. Specifically, each AUV, based on the use of its own multi-beam terrain data to construct an internal closed-loop factor to optimize the posture, further integrates the distance observation values ​​of other AUVs and the external closed-loop factor formed by cross-platform sub-graph matching, and realizes the joint solution of multi-dimensional constraints through the factor graph optimization framework. Compared with single-AUV SLAM, this method effectively suppresses the cumulative error of inertial navigation, overcomes the matching failure problem caused by the sparse characteristics of the seabed environment, and can maintain stable positioning performance, especially in areas with low terrain adaptability. Measured data show that the root mean square error of the trajectory after collaborative optimization is reduced by at least 30% compared with autonomous positioning navigation, which significantly improves the positioning accuracy in complex environments.

[0046] In addition, this method is particularly suitable for collaborative observation scenarios of miniaturized AUVs. In order to address the significant problem of navigation system error accumulation caused by cost and load limitations of small AUVs, this method does not require additional high-precision sensors. It can achieve the fusion of terrain data and distance observation values ​​only through existing multi-beam sonar and underwater acoustic communication equipment, greatly improving positioning accuracy without increasing hardware costs. At the same time, the mechanism of synchronous collection and sharing of environmental information by multiple AUVs avoids the inefficient operation mode of a single AUV repeatedly traversing historical trajectories, significantly improving the efficiency of seabed topography mapping and the freedom of path planning, and providing a low-cost, high-efficiency technical solution for large-scale, high-precision underwater terrain detection and safe navigation. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] Figure 1 Schematic diagram of the overall scenario of multi-AUV collaborative SLAM;

[0048] Figure 2 Schematic diagram of water depth point cloud triangulation network construction in the present invention;

[0049] Figure 3 is the collaborative SLAM front-end factor graph in the present invention;

[0050] Figure 4 This is a schematic diagram of the ICP water depth point weight selection in the present invention;

[0051] Figure 5 This is the trajectory optimization for collaborative SLAM in the present invention. DETAILED DESCRIPTION

[0052] The present invention will be further described below with reference to the accompanying drawings and specific embodiments, but the following embodiments are by no means intended to limit the present invention in any way.

[0053] The present invention aims at the positioning and navigation problem of large-scale high-precision underwater terrain mapping in the marine environment, and proposes a collaborative SLAM navigation solution. A single AUV SLAM can only collect water depth environment information at the same place at the same time. When the environmental information needs to be used to correct the navigation data, the AUV needs to return to the historical trajectory point to complete the SLAM trajectory correction, which greatly limits the SLAM mapping efficiency and the freedom of path planning. At the same time, when a single AUV navigates in an area with low terrain adaptability, it may cause the SLAM accuracy to be seriously reduced. Collaborative SLAM increases the flexibility of the SLAM system and is more conducive to completing tasks such as mapping and marine information collection. To this end, a multi-beam SLAM method for multi-AUV collaboration is established. Multiple AUVs simultaneously collect and share environmental information, and at the same time share the distance information between AUVs. A factor graph is established through the association of environmental data and the distance constraint between AUVs, and the navigation trajectory and mapping information of the AUV are synchronously optimized.

[0054] The present invention is further described in conjunction with the accompanying drawings. The present invention proposes a multi-AUV collaborative SLAM method based on multi-beam water depth measurement data, comprising the following steps:

[0055] 1) Construction of internal multi-beam water depth sub-map

[0056] like Figure 1 In a multi-AUV collaborative multibeam SLAM scenario, each AUV uses its own real-time multibeam strip data to create subgraphs. The closed-loop factor obtained by correlating data between subgraphs constructed by the same AUV is called the internal water depth closed loop. The external water depth closed loop is constructed by correlating data between multibeam subgraphs measured by different AUVs. A factor graph is constructed by integrating the AUV navigation system's position factor, internal water depth closed-loop factor, and external water depth closed-loop factor.

[0057] First, the AUV uses its own multi-beam system to measure the strip water depth, divides the multi-beam strip into multiple sub-graphs according to a certain size, and forms an internal water depth sub-graph set. As shown in formula (1).

[0058]

[0059] 2) Sparsification and transmission of multi-beam depth subgraphs

[0060] Each AUV transmits its own internal multibeam depth data submap, which it measures and divides, to other AUVs. The receiving AUV then correlates the received submap with its own submap and uses it in the SLAM solution. However, due to bandwidth limitations in underwater communications, the massive amount of multibeam measurements cannot be transmitted. Therefore, after the internal submaps are generated, they need to be compressed. Here, the Delaunay triangulation sparsification algorithm is used to sparse the internal depth submaps.

[0061] First, a Delaunay triangulation is established using the sounding points in the subgraph, and the key water depth data is extracted using the characteristics of the triangular mesh. If the slope and aspect characteristics of the triangular mesh around a water depth point do not change significantly, the feature information of this hydroacoustic point is insufficient and will be removed. Figure 2 As shown, for the sounding point b G , find the set of other sounding points with it as the vertex in the triangulation network {b A ,b B ,...,b F}. Calculate b G Slope characteristics of each surrounding triangle mesh and aspect characteristics Then, set the water depth threshold t b and feature threshold t sand t a , remove the water depth points with small changes in slope and aspect characteristics. Calculate the average water depth value of the water depth points that constitute the triangular grid At the current sounding point Under the premise that the range of slope characteristics or the range of variation in aspect characteristics If the depth point is not found, the depth point will be retained, otherwise the depth point will be removed. Finally, the depth point set retained in the subgraph is sent to other AUVs using underwater acoustic communication.

[0062] 3) Construction of external water depth subgraph

[0063] When each AUV receives the water depth data sent by other AUVs through the underwater acoustic communication system, it uses the Gaussian Process Regression (GPR) interpolation method to reconstruct the sub-image.

[0064] First, the GPR model is trained using the received key water depth data as training samples, where the sample input is the plane coordinates of the sampling point X = {x i ,y i |b i ∈M s}, the sample output is the water depth value of the sampling point z={z i |b i ∈M s The Gaussian process is a Gaussian probability distribution based on the training sample space function, which can be expressed by the mean function u(x) and the kernel function k(x,x′), namely, Equation (2) and Equation (3).

[0065] h(x)~GP(u(x),k(x,x′))(2)

[0066]

[0067] Among them, the scale matrix Controls the range of the function in the x and y directions; is the maximum feasible value of the kernel function; parameter set Θ={l x ,l y ,σ h} is called the hyperparameter of the kernel function.

[0068] The training process uses sample data X and z to estimate the hyperparameters of the Gaussian process model using the maximum likelihood estimation method, as shown in Equation (4).

[0069]

[0070] Where K Z=k(X,X).

[0071] After training is complete, the GPR model is built using the hyperparameter set Θ. The plane coordinates X of the interpolated point grid are used. * As the prediction point input, the above model is used to estimate the water depth of the prediction point. Under a given prior distribution, the joint distribution of the water depth of the training set and the prediction set can be expressed as Equation (5).

[0072]

[0073] According to the Bayesian formula, the plane coordinates of the predicted point X * The corresponding water depth value output z * It is formula (6).

[0074]

[0075] Water depth value z at the prediction point * The variance of is given by formula (7).

[0076] cov(z * )=k(X * ,X * )-k(X * ,X)k(X,X) -1 k(X,X * ) (7)

[0077] The set of coordinate values ​​and water depth values ​​of the predicted points is used as the reconstructed subgraph to form the external subgraph set. As shown in formula (8).

[0078]

[0079] 4) Factor graph construction

[0080] For each AUV individual, the SLAM front end integrates the navigation factor, the internal water depth closed-loop factor, and the external water depth closed-loop factor generated by its own sub-map and the receiving sub-map to construct a collaborative SLAM factor graph, as shown in the figure. Based on the single SLAM, the collaborative SLAM system adds the external water depth closed-loop factor generated by the association of the internal water depth sub-map and the external water depth sub-map data and the distance measurement factor between the AUVs. The factor graph established is as follows: Figure 3 shown.

[0081] The nodes to be estimated in the factor graph are pose nodes of multiple AUVs, which can be expressed as formula (9).

[0082]

[0083] DR factor Φ r Constructed using formula (10).

[0084]

[0085] Among them, Φ r represents the DR factor, is the motion increment of the AUV between time i and time i+1, is the error covariance matrix of the motion model.

[0086] It is necessary to keep the clocks synchronized between multiple AUVs and perform distance observations at regular intervals. The distance observation factor can be expressed as formula (11).

[0087]

[0088] Among them, Φ d represents the distance observation factor, is the observed distance between AUVs, is the noise covariance matrix of the distance observation model, x k represents the position of the observed AUV at the kth moment.

[0089] The water depth closed-loop factor is calculated by the improved weighted ICP method for the source point cloud M P and target point cloud M Q The target point cloud is formed by interpolation of key water depth data, such as Figure 4 As shown in Figure 1, the key water depth points have higher characteristics and accuracy, so the weight of the key water depth points is increased in the matching process, and the weight calculation is shown in formula (12).

[0090]

[0091] Use equation (13) to match the point cloud.

[0092]

[0093] Where, is the data point of the target point cloud, is the water depth data detected by the current AUV, The data points of the source point cloud are the water depth points received and reconstructed by the AUV.

[0094] If the matching accuracy is higher than the specified threshold, after obtaining the optimal transformation between point clouds, the water depth closed-loop factor is expressed as Equation (14) and Equation (15).

[0095]

[0096] Among them, Φ e represents the external water depth closed-loop factor, is the closed-loop observation value of water depth, f(·) represents the improved two-step registration algorithm model, is the noise covariance matrix of f(·).

[0097] 5) Factor graph solution

[0098] In a multi-AUV collaborative SLAM system, each AUV incorporates both its own and externally observed factors. A factor graph is constructed and solved using internal and external factors generated over time. Internal factors include the DR factor and the internal water depth closed loop generated by all AUVs in the SLAM system. External factors include the distance observations between AUVs and the external water depth closed loop generated by the local subgraph and the external subgraph. Combining these four factors, the objective function for back-end optimization can be expressed as Equation (15).

[0099]

[0100] Where, Represent the DR factor, internal water depth closed-loop factor, external water depth closed-loop factor and distance observation factor generated by the i-th AUV respectively. A least squares solver based on QR decomposition is selected to generate the optimal estimate of each AUV trajectory in the collaborative SLAM system.

[0101] Finally, the effect of two AUVs cooperating with multi-beam SLAM is shown as follows: Figure 5 ,Compared with the autonomous positioning navigation trajectories of the two AUVs, the collaborative SLAM ,trajectory is obviously closer to the real trajectory.

Claims

1. A multi-AUV collaborative SLAM method based on multi-beam bathymetry data, characterized in that: The following steps are involved: (1) Multiple autonomous underwater vehicles (AUVs) operate synchronously in the mission area. Each AUV uses a multi-beam bathymetric sonar to collect local water depth data in real time and divides the water depth data into multiple internal water depth sub-graphs according to a preset spatial resolution to form an internal water depth sub-graph set. (2) Each AUV performs a sparse process on the internal water depth sub-map, including: Construct the Delaunay triangulation of the sub-graph bathymetric points; Select key bathymetric points based on slope, aspect characteristics and water depth difference thresholds; The sparse sub-image is transmitted to other AUVs via underwater acoustic communication; (3) The AUV receiving the sub-map uses the Gaussian process regression (GPR) algorithm to interpolate and reconstruct the external water depth sub-map of the continuous water depth distribution using the plane coordinates and water depth values ​​of the sparse sub-map as training samples to form an external water depth sub-map set (4) Each AUV constructs a collaborative SLAM factor graph based on the following constraint factors: Position factor: constructed from the motion increment and pose node of the AUV inertial navigation system; Internal water depth closed-loop factor: generated by feature matching between sub-images of the same AUV; External water depth closed-loop factor: generated by weighted iterative closest point ICP registration between the local sub-image and the external sub-image; Inter-AUV distance observation factor: constructed by the relative distance between AUVs obtained by underwater acoustic communication; (5) A distributed optimization algorithm is used to solve the factor graph, and the objective function is: Where, They represent the DR factor generated by the i-th AUV, the internal water depth closed-loop factor, the external water depth closed-loop factor and the distance observation factor between AUVs; (6) Output the optimized pose and global map of each AUV to complete multi-AUV collaborative SLAM.

2. The method according to claim 1, characterized in that The division method of the internal water depth subgraph in step (1) is: The strip data of multibeam bathymetric sonar is divided into sub-graphs according to a fixed grid size or equal time intervals. Each sub-graph contains three-dimensional coordinates and corresponding water depth values.

3. The method according to claim 1, characterized in that The specific steps of the sparse processing in step (2) include: (a) Construct a Delaunay triangulation for each subgraph and calculate the slope characteristics f of the triangulation around each depth measurement point s and f a ; (b) If the difference between the water depth value of a certain sounding point and the average water depth of its surrounding sounding points exceeds the preset water depth threshold, and the variation range of the slope or slope characteristics exceeds the corresponding threshold t b , and the variation range of slope or aspect characteristics exceeds the corresponding threshold t s and t a , then the sounding point is retained; otherwise it is discarded.

4. The method according to claim 1, wherein The kernel function of the Gaussian process regression (GPR) algorithm in step (3) is defined as: Among them, the scale matrix l x , l y is the spatial correlation length, is the kernel function variance; hyperparameter l x , l y , σ h Optimized by maximum likelihood estimation.

5. The method according to claim 1, wherein The error function of weighted ICP registration in step (4) is: Where w i is the weight, is the target sub-graph depth measurement point, The depth measurement point of the source sub-map.

6. The method according to claim 1, characterized in that The distributed optimization algorithm in step (5) adopts the incremental least squares method based on QR decomposition. After each AUV independently solves the local factor graph, the global optimization results are synchronized through underwater acoustic communication.

7. The method according to claim 1, characterized in that Clock synchronization between AUVs is achieved through periodic underwater acoustic signal calibration, and the calibration period is consistent with the acquisition frequency of the distance observation factor.

8. The method according to any one of claims 1 to 7, characterized in that The method is applicable to the following scenarios: Large-scale seafloor topography mapping; Collaborative search and tracking of underwater targets; Dynamic monitoring of marine environmental parameters.

Images