A method for underwater simultaneous positioning and mapping based on multi-beam sonar

Abstract

The present invention belongs to the technical field of underwater synchronous positioning and navigation, and discloses an underwater synchronous positioning and map construction method based on multi-beam sonar, comprising the following steps: S1, constructing a system model of underwater synchronous positioning and map, including a motion model, an observation model and a sensor model; S2, accurately extracting and matching target features based on an improved sonar feature matching algorithm; S3, establishing a sonar SLAM framework; S4, underwater SLAM simulation and experimental results and analysis. The present invention adopts the above-mentioned underwater synchronous positioning and map construction method based on multi-beam sonar, improves the accuracy and reliability of target feature extraction, improves matching progress, and has strong robustness.

Description

Technical Field

[0001] The present invention relates to the field of underwater synchronous positioning and navigation technology, and in particular to an underwater synchronous positioning and map building method based on multi-beam sonar. Background Art

[0002] In recent years, autonomous underwater robots have been widely used in pipeline inspection, geological structure mapping, rescue, resource exploration, etc. Autonomous positioning, navigation and control of underwater robots in complex environments require environmental modeling and robot state estimation as input. Especially in previously unmapped marine environments, where communication and remote control operations are limited or unavailable, autonomous exploration capabilities are even more important. To achieve autonomy, accurate environmental modeling and robot state estimation are important prerequisites. How to solve the underwater perception problem has become an important challenge, which has led to the research direction of underwater simultaneous localization and mapping (SLAM).

[0003] In order to obtain information about the surrounding environment and achieve underwater positioning and mapping, optical sensors (cameras) or acoustic sensors (sonar) were usually used in the past. Although cameras can capture the detailed texture of the underwater environment very well, they are often limited by the turbidity of the water and the light. In addition, cameras are very dependent on light, and different changes in light intensity make the data obtained by the camera unreliable. On the other hand, sound waves are an important alternative method for underwater perception systems. For example, by using a Doppler velocimeter, the speed underwater can be measured, and by using imaging sonar, the intensity of the reflected sound waves can be measured to determine whether there is an obstacle in front of the robot. However, since the resolution of the sonar sensor is severely affected by the wavelength of the sound and other factors, its resolution is relatively low, and there are also refraction, multipath effects, etc., which restrict the development of underwater robots.

[0004] In the process of underwater robots achieving autonomy, underwater navigation and other technologies are involved. It usually includes two points. Compared with land, navigation usually uses inertial navigation. It combines Doppler velocimeters and inertial measurement units, measures angular velocity and linear acceleration through high-precision inertial measurement units (Inertial Measurement Uni, IMU), and improves accuracy with the assistance of Doppler velocimeters (Doppler velocity log, DVL), thereby achieving local positioning and navigation. However, as time accumulates, it will produce cumulative errors. Therefore, GPS needs to be corrected at a certain time. Another is to use geomagnetic navigation. This system has no error accumulation and can provide a global heading reference. But the disadvantage is that satellite signals cannot be received when the water depth exceeds a certain range, and it will be affected by changes in the geomagnetic field.

[0005] Therefore, the main problems of sonar SLAM are poor image quality, low feature accuracy, missing elevation angle, and complex matching calculation, that is, unclear information and inaccurate distance measurement. Summary of the invention

[0006] The purpose of the present invention is to provide an underwater synchronous positioning and mapping method based on multi-beam sonar, which improves the accuracy and reliability of target feature extraction, improves the matching progress, and has strong robustness.

[0007] To achieve the above object, the present invention provides an underwater synchronous positioning and map construction method based on multi-beam sonar, comprising the following steps:

[0008] S1. Construct a system model for underwater synchronous positioning and mapping, including motion model, observation model and sensor model;

[0009] S2, based on the improved sonar feature matching algorithm, accurately extract and match the target features;

[0010] S3, establishment of sonar SLAM framework;

[0011] S4. Underwater SLAM simulation and experimental results and analysis.

[0012] Preferably, in step S2, based on the improved sonar feature matching algorithm, the target features are accurately extracted and matched, and the specific process is as follows:

[0013] S21. Based on the improved CFAR filtering algorithm, accurate extraction of target features is performed;

[0014] S22, cluster segmentation based on Dbscan;

[0015] S23, establishment of graphs and descriptors;

[0016] S24, matching constraints of descriptors.

[0017] Preferably, in step S21, a detection technology based on a constant false alarm rate (CFAR) is used, and the relationship between the detection unit and its surroundings is determined by using a sliding window. The reference units in the window are averaged after eliminating cells close to the detection unit, and then compared with the detection unit.

[0018] Preferably, in step S22, clustering is performed using a Dbscan algorithm to find high-density areas in a given data set and divide them into different clusters.

[0019] Preferably, in step S23, after obtaining a plurality of clusters, the number of point clouds contained in each cluster is screened to remove discrete noise points, while retaining the number of point clouds greater than Ptstheshold Clusters; traverse the cluster labels [label}, for each cluster label i The point cloud cluster is used to generate Gaussian blocks.

[0020] Preferably, in step S24, after the Gaussian blocks are generated, a graph describing the Gaussian information and adjacency relationship of a picture is established, the descriptors are further optimized and screened by searching for the largest cluster, and the descriptors are aligned based on the GP algorithm.

[0021] Preferably, in step S3, the establishment of the sonar SLAM framework includes the following steps:

[0022] S31, front-end frame matching and dead reckoning;

[0023] S32, gtsam backend optimization;

[0024] S33, loop detection based on PCM;

[0025] S34, occupation map algorithm based on sub-map, occupation map construction.

[0026] Preferably, in step S31, the front-end frame matching and dead reckoning include the following processes:

[0027] S311, based on the improved GP and Akaze algorithms, frame matching is performed;

[0028] S312, inter-frame matching method based on parallel Akaze algorithm and GP algorithm;

[0029] S313, based on the front-end frame matching, establishing dead reckoning by using sensor measurement data and motion model;

[0030] S314. Relationship between coupled frame matching and dead reckoning.

[0031] Preferably, in step S311, performing inter-frame matching specifically includes:

[0032] (1) Globally initialize ICP;

[0033] (2) Derivation of probability model;

[0034] (3) Inter-frame matching constraints.

[0035] Therefore, the present invention adopts the above-mentioned underwater synchronous positioning and map construction method based on multi-beam sonar, extracts target features from the sonar original image through a filtering algorithm to establish constraints; obtains non-sequential factors by establishing continuous inter-frame matching order factors and loop detection and adds them to the back-end for optimization; at the same time, in order to solve the problem of falling into a local optimal solution in the ICP solution process, a GP graph matching algorithm is proposed to provide initial values ​​to solve this problem; the Akaze algorithm is used to solve the long straight line environment matching degradation problem in parallel; the consistency measurement set is maximized to eliminate outliers in loop detection; finally, a trajectory is established through the estimated posture, and a map based on a subgraph is established, which improves the accuracy and reliability of target feature extraction, improves the matching progress, and has strong robustness.

[0036] The technical solution of the present invention is further described in detail below through the accompanying drawings and embodiments. BRIEF DESCRIPTION OF THE DRAWINGS

[0037] Figure 1 The figure is a schematic diagram of sonar imaging; (a) is the sonar three-dimensional coordinate system; (b) is the sonar two-dimensional plane projection effect;

[0038] Figure 2 Schematic diagram of the imaging sonar geometric model; (a) is the sonar geometric model; (b) is the multi-beam sonar geometric model;

[0039] Figure 3 It is the general process of CF-CFAR of the present invention;

[0040] Figure 4 : is the effect diagram of clustering using Dbscan in the present invention; wherein, (a) is the point cloud after voxel filtering; (b) is the effect of clustering using Dbscan; (c) is the effect of K-means;

[0041] Figure 5 are Gaussian block parameters and adjacency relations of the present invention;

[0042] Figure 6 It is the shape of Gaussian blocks generated by each cluster of the present invention; wherein (a) is the source point cloud after voxel filtering, (b) is the segmentation effect, and the ellipse in (c) is the different Gaussian blocks generated by clustering;

[0043] Figure 7 It is a schematic diagram of coordinate transformation solution;

[0044] Figure 8 It is different R max The impact on the density of the graph; (a) is R max =0.5; (b) is R max =0.7; (c) is R max =1.0; (d) is Rmax =1.3;

[0045] Fig. 9 It is the descriptor for searching the maximum cluster and screening the descriptor of the present invention; wherein, (a) is the descriptor formed without screening; (b) is the maximum cluster of stable structure found by searching;

[0046] Fig.10 is a maximal cluster with 3 nodes; where (a) is the maximal cluster of the origin point cloud; (b) is the maximal cluster of the target point cloud;

[0047] Fig.11 is a descriptor data graph between Gaussian blocks of the present invention;

[0048] Fig.12 is a flow chart of the matching algorithm of the present invention;

[0049] Fig.13 It is a similar algorithm flow chart of the present invention;

[0050] Fig.14 It is the GP matching effect between two frames of the present invention;

[0051] Fig.15 It is the SLAM framework flow chart of the present invention;

[0052] Fig.16 It is a flow chart of inter-frame matching based on the parallel Akaze algorithm and GP algorithm of the present invention;

[0053] Fig.17 It is a dead reckoning principle diagram of the present invention;

[0054] Fig.18 It is the coupling relationship between the front-end inter-frame matching data and the dead reckoning data of the present invention;

[0055] Fig.19 It is composed of the SLAM back-end optimization factor graph of the present invention;

[0056] Fig. 20 The present invention establishes continuous factors from scan matching;

[0057] Fig.21 This is the wrong loop recognition of the present invention;

[0058] Fig. 22 The SLAM of the present invention recognizes the same features or positions to generate loop constraints;

[0059] Fig.23 is the mapping process of the present invention; wherein (a)-(f) are processes 1 to 6;

[0060] Fig.24This is the GPSLAM prediction trajectory analysis in the simulation; (a) is the prediction effect; (b) is the motion analysis in the x, y, and z directions;

[0061] Fig.25 It is the GPSLAM positioning and mapping effect in the simulation environment;

[0062] Fig.26 It is an experimental test obstacle;

[0063] Fig. 27 It is the positioning and occupancy mapping effect of SLAM; (a) is the effect during the mapping process; (b) is the final mapping result;

[0064] Fig.28 This is the effect of Bruce SLAM positioning and mapping. (a) is the effect during the mapping process; (b) is the final mapping result.

[0065] Fig.29 The figure is a comparison between the SLAM prediction trajectory of the present invention and the existing Bruce SLAM algorithm and the true value trajectory of motion capture; wherein, (a) is the trajectory comparison; (b) is the error in the x, y, and z directions;

[0066] Fig.30 is the calculation result of GP-SLAM absolute pose error; (a) is the calculation curve; (b) is the error intensity diagram;

[0067] Fig.31 It is the calculation result of Bruce SLAM absolute pose error; (a) is the calculation curve; (b) is the error intensity diagram. DETAILED DESCRIPTION

[0068] The technical solution of the present invention is further described below through the accompanying drawings and embodiments.

[0069] The present invention provides an underwater synchronous positioning and map construction method based on multi-beam sonar, comprising the following steps:

[0070] S1. Construct a system model for underwater synchronous positioning and mapping, including motion model, observation model and sensor model;

[0071] S2, based on the improved sonar feature matching algorithm, accurately extract and match the target features;

[0072] S3, establishment of sonar SLAM framework;

[0073] S4. Underwater SLAM simulation and experimental results and analysis.

[0074] Example

[0075] S1. Construct a system model for underwater synchronous positioning and mapping, including motion model, observation model and sensor model.

[0076] S11. Construct a robot motion model.

[0077] In the implementation of SLAM algorithm, the robot's motion model is mainly composed of Doppler velocimeter and inertial measurement unit. In the SLAM algorithm based on multi-beam sonar, the sonar image is a two-dimensional plane, so it is assumed that the robot moves in a two-dimensional plane, and its longitudinal movement has little effect, that is, the floating and sinking movement of the robot is ignored. Assume its state is:

[0078] x=[x,y,ψ,y x , v y , r] T (1)

[0079] Its motion model in the horizontal plane is as follows:

[0080]

[0081] Where x is the coordinate of the robot odometer in the x direction; y is the coordinate of the robot odometer in the y direction; v x is the speed of the robot in the x direction; v y is the speed of the robot in the y direction; ψ is the yaw angle of the robot in the two-dimensional plane; t is the unit time; r is the angular velocity in the yaw direction; w conforms to the w~N(0,Q) distribution, and Q is a constant.

[0082] S12. Construct a robot observation model.

[0083] Usually the observation equation of the robot can be expressed as z k =h(X k )+v k , where z and v are the observed quantity and the observed noise respectively. The observed data consists of two parts, the first part is the information obtained by the sonar, and the other part is the measurement information of the measurement sensor. The motion model is composed of the data obtained by the speedometer DVL and the inertial measurement unit IMU, that is, z = [v x , v y ,ψ,r] T , and then establish the observation equation based on the observed quantity, as shown below:

[0084]

[0085] Wherein, v is Gaussian white noise and v obeys v~N(0,R) distribution, and R is a constant.

[0086] S13. Construct a sensor measurement model.

[0087] In order to achieve simultaneous positioning and map construction underwater, the robot generally needs to be equipped with a variety of sensors and add additional constraints to correct the wrong posture. The main perception sensors are sonar, depth meter, IMU, DVL, etc. The present invention uses Oculus m1200d sonar, which has a group of receivers. By collecting echoes from a single transmission pulse, and then using mathematical methods to synthesize the data into images, the image can be generated many times per second with a high refresh rate.

[0088] When the sonar is turned on, the field of view observed by the sonar sensor is a three-dimensional space area. Assume that its parameters are the horizontal beam angle θ and the elevation angle The effective distance radius detected is r, such as Figure 1 As shown in (a), the green color represents the sound waves emitted by each sonar, and the different layers represent the range of elevation angles. Generally, the imaging sonar will project the three-dimensional space it observes into a two-dimensional plane display, such as Figure 1 as shown in (b).

[0089] (1) Field of view and imaging geometry model of multi-beam sonar.

[0090] When the sonar is working, first define the sonar coordinate system as follows Figure 1 As shown in (a), for the convenience of description, the origin of the sonar coordinate system is usually set to be consistent with the origin of the robot. The origin of the sonar coordinate system is denoted as O. The three axis directions are defined as follows: the x-axis is perpendicular to the sonar array, the z-axis is vertically downward, and the y-axis is horizontal to the right. Assume that the coordinates of a certain echo point on the sonar beam in the Cartesian coordinates in three-dimensional space are p = [x, y, z] T , and by default it is within the field of view of the sonar, so if you use a spherical coordinate system to represent this point, you can use To express:

[0091]

[0092] In real data, imaging sonar sensors generally do not provide The image provided is a two-dimensional image, so the vertical projection point of point p on the horizontal plane is It can be obtained by Q = [r, θ] T To approximate, Figure 2 As shown in (a) in the figure, it is transformed by the following formula:

[0093]

[0094] Furthermore, after receiving the echo point information of the same azimuth distance on each transducer, the coordinates are transformed and interpolated back to the Cartesian coordinate system by sampling and interpolation, and the echo intensity of the point can be displayed on the image.

[0095] (2) Data format and 2D geometric model of the Oculus m1200d sonar.

[0096] The present invention creates an imaging matrix for the data information collected by the sensor, and its structure is L rows and 512 columns. In order to generate the geometric model of the image, the present invention assumes that the fan-shaped field of view of the sensor is α, the angle between the beams is θ, and T is any echo point in the field of view, the distance from T to the origin O is r, and the corresponding image matrix is ​​assumed to be D, and any value in it is represented by D t (a,b) means that a is the row and b is the column. For multi-beam sonar, a is between 1 and L, and b is between 1 and 512, that is, D t The original echo point of the pixel is T, and the origin of the Cartesian coordinate system and the polar coordinate system of the sonar are assumed to be the same, such as Figure 2 As shown in (b) in .

[0097] Since the number of beams of m1200d is 512, the angle between beams can be calculated using the formula:

[0098]

[0099] And through calculation, its distance resolution is 2.5mm, then:

[0100] r=2.5×(La) (8)

[0101] Then the coordinates of the original echo point T corresponding to the pixel point D(a,b) in the polar coordinate system are as follows:

[0102]

[0103] Then the Cartesian coordinates of point T are transformed into the following formula:

[0104]

[0105] In order to display the multi-beam sonar data on a two-dimensional plane, after the coordinate system conversion, it is necessary to interpolate before it can be displayed in the image.

[0106] S2. Based on the improved sonar feature matching algorithm, the target features are accurately extracted and matched.

[0107] S21. Based on the improved CFAR filtering algorithm, accurate target features are extracted.

[0108] In order to reliably extract features in environments with various noise powers, a detection technology based on constant false alarm rate (CFAR) is adopted. The relationship between the detection unit and its surroundings is determined in the form of a sliding window. The reference units in the window are averaged after eliminating the cells close to the detection unit, and then compared with the detection unit.

[0109] like Figure 3 As shown, it is composed of a one-dimensional (1×n) sample vector, and each sonar intensity sample is associated with a detection unit x i Related, each cell represents a pixel, which represents an image coordinate (x, y) of the image. Using CA-CFAR, the detection threshold T is estimated based on the environment of the detection unit and the constant α. This constant is a false alarm probability P fa The specific principle is as follows:

[0110] The total number of sliding windows N used to detect target units c , as shown below:

[0111] N c =2N+2G+1 (11)

[0112] Among them, N is the reference unit and G is the protection unit;

[0113] As protection unit G c for:

[0114] G c =2G+1 (12)

[0115] Assuming that clutter and ambient noise are independent of each other, and that they satisfy exponential distribution after being squared, the probability density function of the surrounding reference cells is:

[0116]

[0117] Among them, β 2 is the average value of the acoustic reverberation power.

[0118] Then the adjacent unit N c The vector is

[0119]

[0120] To maximize the likelihood function above, we can convert it into a summation form after taking the logarithm, which is convenient for solving:

[0121]

[0122] Detection Threshold is a random variable, where α>0, Different from β 2 .

[0123] When the constant false alarm rate value P f a does not depend on the current value of β 2 If the detector is CFAR, the expression of the estimated threshold is:

[0124]

[0125] Let z i =(α / N c )x i ,but Substituting into formula (13) and using the standard probability result, we get:

[0126]

[0127] By N c and N c / αβ 2 , The probability is:

[0128]

[0129] P fa The expected value is as follows:

[0130]

[0131] Through integral calculation and iteration, we can get:

[0132]

[0133] Furthermore, the constant values ​​are as follows:

[0134]

[0135] in, is determined by the number of phase-link units N c , independent of the acoustic reverberation intensity β 2 , in this filtering algorithm, only P fa , G, and N need to be given in advance.

[0136] S22, based on Dbsca n Cluster segmentation.

[0137] S221. Clustering.

[0138] The present invention uses the Dbscan algorithm for clustering to find high-density areas in a given data set and divide them into different clusters. It can effectively handle noise points and data with uneven density and is not affected by initialization parameters and is insensitive to parameter selection, which has obvious advantages.

[0139] After downsampling the feature point cloud and removing discrete points using the edge radius point removal method, Dbscan is used for clustering, ε = 0.6, points min =4, the effect is as follows Figure 4 As shown in the figure, different colors represent different classes. Comparing the effects of K-means and Dbscan, it can be clearly seen that the Dbscan algorithm has a better effect in distinguishing obstacles with different characteristics. The K-means algorithm cannot distinguish the boundaries of different feature blocks well when clustering, and is prone to incorrect clustering. For example, Figure 4 In (b) and (c), the Dbscan algorithm can well identify the pool wall as a whole class of purple, while K-means cannot distinguish the boundary when identifying the boundary. This shows that Dbscan has a significant effect on boundary distinction, while K-means has the best clustering effect.

[0140] S222: segmentation and generation of Gaussians.

[0141] After obtaining multiple clusters, the number of point clouds contained in each cluster is screened to remove discrete noise points, while retaining the number of point clouds greater than Pts thresold Clusters. Traverse the cluster labels {label}, for each cluster label i The point cloud clusters are generated by Gaussian blocks. Gaussian blocks are used to describe the shape of each cluster, and the parameters of Gaussian are defined as follows:

[0142] GA={μX, μY, μI, σB, σS, σI, θ, N} (23)

[0143] Among them, μX, μY are the center points of the Gaussian block in the image; μI is the average sonar image intensity of the Gaussian block; σB is the long axis of the Gaussian block; σ, are the wide axes of the Gaussian block, through the covariance matrix The eigenvalue and eigenvector of σB and σS are calculated; σI is the standard deviation of the sonar image intensity of the Gaussian block; N is the number of point clouds; θ is the angle between the long axis of the Gaussian block and the x-axis direction to the right underwater, and the range is

[0144] Calculation using point cloud principal component analysis:

[0145] (1) Calculate the point cloud center μX, μY of the cluster as follows:

[0146]

[0147] (2) Centralized data processing:

[0148]

[0149] (3) Calculate the covariance matrix and get:

[0150]

[0151] (4) Solving the covariance matrix The eigenvalues ​​and eigenvectors in can be solved by using QR decomposition or Jacobi method. max The corresponding eigenvector v max That is the direction vector of the major axis of the Gaussian block; the lambda with the smallest eigenvalue min The corresponding eigenvector v min That is the direction vector of the minor axis of the Gaussian block, such as Figure 5 As shown. The line segment axes of σS and σB in the figure are represented by orange and orange respectively. The long axis of the Gaussian block on GA1 is defined with the positive direction of the x-axis as the reference, the direction is counterclockwise, and the angle is positive, θ e is the angle between the Gaussian block adjacencies, and the green lines represent the adjacency distance and angle.

[0152] (5) Solve for the angle θ and the intercept b as follows:

[0153]

[0154] θ=arctank (29)

[0155] Finally, the clusters obtained by clustering are further generated into Gaussians and given relevant parameter attributes, such as Figure 6 As shown, the Gaussian block shapes generated for each cluster, and different colors represent different Gaussian blocks. It can be seen that the algorithm proposed in the present invention can adapt to the features of various shapes to generate Gaussian blocks.

[0156] S23. Establishment of graphs and descriptors.

[0157] S231. Create a chart.

[0158] After generating Gaussian blocks, an undirected graph G is established, and G = (V, E) is defined, where V is a set of vertices, that is, a set of Gaussian center points in an image V = {GA 1 , G A2 , GA 3 ,...,GA n}, and each Gaussian vertex is constrained by an edge E, that is, a set of edges is used to describe a set of Gaussian direct relationships E = {E1 t 2 , E, ... E m}, define the content of edge attribute storage as E i ={θ e1 ,θ e2 , ρ e ,GA src , G.A. dest}, θ e1 is the angle of the target Gaussian point relative to the positive direction of the long axis of the source Gaussian point cloud, θ e2 is the angle of the source Gaussian point cloud relative to the positive direction of the long axis of the target Gaussian point cloud. It is obtained by substituting the information of the two Gaussian blocks into the following formula, limiting θ e The range is in (0, 2π].

[0159] θ e =atan(μY t -μY s ,μX t -μX s )-θ (30)

[0160] In order to solve the quadrant unification problem in the actual solution process, such as Figure 7 As shown, the solution is obtained by coordinate transformation as follows:

[0161] x q =x t -x s ,y q =y t -y s (31)

[0162]

[0163] When θ e When θ e =θ e +360, which is convenient for θ e Unified to the coordinates (0,2π].

[0164] In the edge constraint, ρ e is the Euclidean distance between the two Gaussian center points, as shown below:

[0165]

[0166] In order to establish effective constraint edges of the graph, a simple rule is established if the Euclidean distance between the centers of two Gaussian blocks is ρ e Less than the given parameter R max , creating an edge constraint connecting the Gaussian blocks. R max The parameter is related to the information density of the descriptor. maxThe parameters are used to adjust the trade-off between reality and accuracy. Describing the relationship between adjacent feature blocks, solving the rotation invariance during image motion, because the edge between two vertices stores θ e is the major axis angle of the segmentation-dependent Gaussian shape, and this angle θ is generated based on the eigenvector of the covariance of the Gaussian point cloud.

[0167] like Figure 8 Shown are different R max The first column shows the source point cloud after voxel filtering, the second column shows the segmentation effect, and the third column shows the generation of Gaussian blocks and descriptors. The lines represent the relationship between them. max The larger the value, the denser the Gaussian descriptor generation, but the relative calculation time will be longer. Through test comparison, it is found that the present invention uses R max =1.0 achieves best results.

[0168] S232, search for the largest clique and filter the descriptor.

[0169] According to the above, a graph describing the Gaussian information and adjacency relationship of an image is created, and the maximum cluster is searched to further optimize the selection of descriptors, so as to facilitate the subsequent matching constraints. The maximum cluster is searched by using the find_maximal_cliques function of the igraph library in the python programming language. It is found through testing that when the number of nodes is 3, the structure of the cluster is the most stable. The present invention obtains a stable structure by searching for the maximum cluster with 3 nodes, such as Fig. 9 As shown, stable descriptors can be preserved through maximal cliques.

[0170] like Fig.10 As shown, there is a maximal cluster of 3 connected nodes, where blue represents the length of the adjacent edges of the Gaussian block, red represents the angle of the surrounding Gaussian blocks relative to it, and different colors represent different Gaussian blocks.

[0171] After searching for the maximum cluster, we use the maximum cluster as a guide to obtain the vertices contained in the maximum cluster and obtain the edge information stored at the vertex. e and angle θ e Store the descriptor, define the descriptor of the i-th vertex in the maximal clique, and store the data in the descriptor in order from small to large angles, as shown below:

[0172] descriptor i ={[ρ 1 ,θ 1 ],[ρ 2 ,θ 2 ],...,[ρ m ,θ m]} (35)

[0173] Finally, the descriptor is obtained 1 ,descriptor 2 ,...,descriptor m Stored in descriptors and assigned to the corresponding Vertex i The attribute of is convenient for indexing the Gaussian center point. Through such processing, descriptors1 and descriptors2 generated by the maximum cluster of the two images before and after can be obtained, preparing for the next target feature matching, such as Fig.11 The data between the generated descriptors are displayed. The red numbers are the two angles of the edge, and the blue numbers are the length of the edge. It well reflects the stable feature structure in a graph, and can provide good constraints for matching.

[0174] S24, matching constraints of descriptors.

[0175] After obtaining the descriptors of the two images before and after descriptors 1 and descriptors 2 in step S23, an algorithm is proposed to align the two descriptors, referred to as the GP (Graph Matching) algorithm, such as Fig.12 As shown. When traversing descriptors1, the i-th descriptor 1，i and descriptors2 j-th descriptor 2，j 0, define similarity score, best matching vertex index Match best , the best matching distance ρ best , through the description of the violent matching, find Figure 1 VertexVertxt i The corresponding best matching vertex Vertxt j , the specific process of the algorithm is as follows:

[0176] S241, first use the similarity algorithm to calculate the descriptors of the two vertices i and descriptor j The similarity of , get the score, and calculate it by formula (21), (22), where m, n are descriptors respectively. i and descriptor j The mth and nth data sets, m = 1, 2, ..., m and n = 1, 2, ..., n, θ diff is the given parameter angle threshold, ρ diff is the distance threshold for a given parameter.

[0177] The score calculation rules are as follows: First, compare the descriptor i，1 With descriptor j，1 If the angle error of the edge satisfies formula (21), then determine whether the distance error of the corresponding edge satisfies (36). If so, the score is increased by 1, and the next group of data is determined. If the formula (37) is not satisfied during the traversal process, then compare θ i，m With θ j，n If the size of θ j，n If it is larger, m moves the next set of data and continues to compare with the current n set of data until the array of descriptors is traversed and the score is obtained. The specific process is as follows Fig.13 shown.

[0178] |θ i，m -θ j，n |≤θ diff (36)

[0179] |ρ i，m -ρ j，n |<ρ diff (37)

[0180] By adjusting θ diff , diff The two parameters can well control the similarity of the adjacency relationship between Gaussian blocks, and make a trade-off between accuracy and time. When the surrounding environment is far away, the threshold can be adjusted to be larger to better search for descriptors; when the robot's motion space is relatively small and the environmental obstacles are dense, the threshold can be set to be smaller to improve accuracy and select the best aligned vertex, because the sonar field of view is relatively small at this time and the detected feature obstacles are relatively dense.

[0181] S242, completing the first descriptor 1 in descriptors 1 1 and the jth descriptor2 in descriptors2 j After the scores of the descriptors, if the similarity score is greater than the score best , then further calculate whether the direct distance between the two vertices satisfies formula (37). If so, update Match best If the similarity scores are equal, the distances of the vertices are further compared and the smaller vertex is selected. Through such steps, descriptor1 is found 1 The corresponding descriptor2 k, that is, at the paired point (1, k) in the two images, steps S251 and S252 are repeated to find paired points such as (2, q) and store them in the connections container.

[0182] In addition, in formula (38), pos is the Euclidean distance error of the matching vertices. In order to improve the robustness, by adjusting ξ pos The parameters are adjusted according to the robot's movement speed to keep consistent with the keyframe distance generation conditions on the dead reckoning side, preventing the distance of the matching point from exceeding the distance of the dead reckoning keyframe movement.

[0183]

[0184] Through the above steps, a set of rough matching points SourcePts is obtained 0 ,TargetPts 0 , use the ICP algorithm to iteratively solve this set of rough matching points to solve the prediction transformation matrix T 0 , T 0 It can well predict the direction of ICP solution and solve the problem of ICP falling into local optimal solution.

[0185] like Fig.14 As shown in the figure, the matching result of the source point cloud and the target point cloud of the 5th frame is shown. The blue and orange are the source point cloud and the target point cloud, and the green line represents the successfully matched paired points. It can be seen that the matching accuracy is high, and the Gaussian blocks can be well registered, providing the initial points, and then solving the predicted transformation matrix T 0 .

[0186] S3. Establishment of sonar SLAM framework.

[0187] S31, front-end frame matching and track calculation.

[0188] The front end consists of two parts, one is the dead reckoning composed of motion measurement sensors, and the other is the sonar image inter-frame matching to estimate the position and posture. It estimates the motion trajectory of the underwater robot by inter-frame matching based on the data provided by the sonar sensor. Dead reckoning can provide more accurate constraints and correct the wrong inter-frame matching estimated position and posture. The two constrain each other to correct the data. Fig.15 As shown in the figure, through the inter-frame matching of sonar images and the front-end part of the track-reckoning SLAM framework composed of DVL and IMU, preparations are made for subsequent positioning and mapping.

[0189] S311, improved inter-frame matching algorithm based on GP graph algorithm and Akaze.

[0190] 1. Globally initialize ICP;

[0191] In order to solve the local minimum problem, the match is globally initialized before performing local optimization, that is, the prediction transformation matrix is ​​first calculated. Then, the ICP algorithm is used to further refine the globally initialized estimate locally.

[0192] definition is the globally initialized source pose, T 0 The prediction transformation matrix for ICP is initialized globally, which is transformed into the problem of solving the formula:

[0193]

[0194] Among them, ε is the convergence threshold.

[0195] When II is equal to 1, the solution is successful, and when II is equal to 0, the solution fails. ε is the given reprojection error distance, and d is the sum of the distances of the nearest point searched through the last iterative transformation.

[0196]

[0197] 2. Derivation of probability model;

[0198] Apply Bayes’ theorem to find the posterior distribution of the robot’s source pose:

[0199] p(x s |x t , p t , p s ,u)∝p(xs|x t ,u)p(P s |x s , x t , P t ) (41)

[0200] The first term can be obtained from the odometry integral and is usually modeled as a multivariate Gaussian distribution. However, it is often simply assumed that the initial predicted poses provided by the odometry are uniformly distributed, which translates to:

[0201] p(x j |x t , p i , p s ,u)∝(P s |x s , x t , P t ) (42)

[0202] The second term describes the t In the case of x t Point cloud P is observed at sThe probability of , if we assume that individual points are measured independently, is expressed as:

[0203]

[0204] The measurement model is as follows:

[0205]

[0206] Because features are more likely to be detected near existing measurement points, P 1 >P 2 , we can derive the same set consistency maximization problem as equation (39):

[0207]

[0208] 3. Inter-frame matching constraints;

[0209] The present invention proposes a GP algorithm based on graph matching to establish descriptors and then calculate the similarity of descriptors to find matching points, and then use ICP optimization to obtain the initial prediction transformation matrix T 0 .position is the optimized pose, Next, use the initialized T 0 Initialize ICP and get the exact solution

[0210] After the front end obtains the key frame information, it initializes the point cloud waiting for pairing and initializes P s Point cloud is the current frame, P t The point cloud is the point cloud of the previous frame. After successful initialization, the GP algorithm is used to extract valid pairing points, and the ICP algorithm is used for registration estimation to calculate the transformation matrix T s , T 0 , P s , P t Substitute it into ICP again to solve the problem and get the exact solution

[0211] There may be some outliers in the scanning and matching process. Use the following method to remove outliers:

[0212] (1) The source point cloud and the target point cloud must have enough point clouds:

[0213] n src >points min , n tar > points min (46)

[0214] Among them, n src is the number of source point clouds; ntar is the number of target point clouds; points min is the minimum number of point clouds.

[0215] (2) The estimated transformation matrix cannot deviate significantly from the initial transformation matrix:

[0216]

[0217] Where χ is the deviation matrix threshold.

[0218] (3) During the initialization process, the source point cloud and the target point cloud must have enough overlap, which can be determined using the following formula:

[0219]

[0220] Among them, ε is the convergence threshold; η owrlap is the overlap factor.

[0221] S312, Akaze inter-frame algorithm parallelism.

[0222] In the front-end matching algorithm for calculating sonar features, although the proposed GP algorithm can solve the large-scale rotation transformation of the robot very well, it is found that when the surrounding environment is simple and has few features, or when the feature environment is a long straight line and the robot's walking path is parallel to this straight line, the GP algorithm and the ICP algorithm will degenerate and fail to solve. At this time, some bright echo points will generally appear at the end of the long straight line feature, and the Akaze (Accelerated-KAZE) algorithm can track them very well. And if the robot's walking direction is perpendicular to the environmental feature line, the Akaze algorithm also has good robustness and high accuracy in tracking and matching. Although Akaze has the above advantages, the Akaze algorithm cannot match well for multi-frame rotational motion matching and often loses tracking.

[0223] To this end, the present invention uses an inter-frame matching method based on the parallel Akaze algorithm and the GP algorithm, such as Fig.16 As shown, it can perform sonar feature matching well, adapt to rotation and long straight line scenes, and then accurately estimate the motion state of the robot. The two algorithms complement each other's disadvantages.

[0224] The GP algorithm is used to extract matching points, and the Akaze algorithm is used to extract matching points. The two systems calculate the relative pose between the two key frames in parallel. Through testing, it is found that the improved method can significantly eliminate the false matches caused by the sonar echo characteristics, thereby significantly improving the accuracy of the registration and improving the robustness effect.

[0225] S313: Track calculation is established.

[0226] Dead reckoning is based on front-end frame matching. It uses sensor measurement data and motion models to predict the next position of the underwater robot. Dead reckoning can more accurately estimate the robot's motion trajectory and provide constraints, thereby improving positioning accuracy and stability. The IMU inertial measurement unit and DVL Doppler velocity meter, depth meter, and three sensors are used to estimate the robot's motion posture in water and provide initial value constraints as tight coupling.

[0227] In the dead reckoning algorithm, the DVL v is used x , v y Information, specific flow chart as follows Fig.17 As shown, the specific algorithm is as follows:

[0228] (1) Preprocess the sensor data to obtain the roll, pitch, and yaw attitude information of the IMU and the speed v of the DVL x , v y , v z Information, depth information z .

[0229] (2) Formula (49) is used to determine whether the speed of the DVL exceeds the maximum speed threshold. If it exceeds, it will not participate in subsequent operations. Formula (50) is used to determine whether the time interval between two messages is greater than the maximum time. If it exceeds, it is also considered as erroneous delay information, and the data is discarded and does not participate in subsequent operations.

[0230]

[0231] |t tmp -t prev |>t error (50)

[0232] (3) Perform velocity integration, use the time difference δt between two frames, and take the average value of the velocity information of the two frames, integrate, and obtain the relative micro-posture transformation δx, as shown below:

[0233]

[0234] (4) Update the global pose of the odom coordinate system and use the yaw information of the current frame IMU to update P 0 The posture of , and update p as follows:

[0235]

[0236] (5) Determine whether the pose Pose is a key frame. Obtain information from the poses of the two frames, and use formula (53) to determine whether the current frame is a key frame. If satisfied, set the data as a key frame.

[0237]

[0238] Among them, t duration is the minimum time difference between the leading key frames; is the translation parameter; ψ reck is the heading angle parameter.

[0239] (6) Publish the odom information trajectory corresponding to the robot's track estimation key frame.

[0240] S314, coupling relationship between frame matching and dead reckoning.

[0241] In the front end, the sonar node is responsible for receiving the raw data from the sonar sensor, parsing and processing it, and extracting the characteristic point cloud information of the underwater environment. The dead reckoning node receives the course information of the inertial measurement unit, Doppler velocimeter, and depth meter to estimate the position and posture in the odometer coordinate system. Fig.18 As shown in the figure, firstly, the timestamps of the sonar and dead reckoning key frame nodes are unified, allowing a certain delay error. Then, the sonar feature point cloud and the dead reckoning key frame are obtained from the sonar node to obtain the estimated pose. The key frame is initialized by continuous scanning matching. Then, Akaze, ICP, GP algorithm and ICP algorithm are used in parallel to determine whether the error converges. If it converges, the pose information estimated between the key frames is added to the factor graph for optimization. If it does not converge, the pose estimated by dead reckoning is used as a substitute to add to the factor graph to optimize the pose information of this frame.

[0242] S32, gtsam backend optimization.

[0243] The idea of ​​backend optimization is to select some frames from the global (whole process) to establish global constraints with larger spatial and temporal spans and satisfied at the same time to re-optimize the previously obtained pose. In the SLAM backend optimization, the present invention uses the factor graph of gtsam for backend optimization.

[0244] like Fig.19 As shown in the figure, the pose of each keyframe is represented as a black square, and each keyframe is associated with a feature point detected in the sonar image. The factor graph consists of two factors, namely sequential scan matching SSM and non-sequential NSSM. (Colored in green and orange respectively). The sequential factor represents the transformation relative to the previous pose, and the error will inevitably accumulate. On the contrary, the non-sequential factor is a loop constraint that connects two separate poses, thereby correcting the accumulated drift. The graph optimization calculation formula is:

[0245]

[0246] The sequential scan matching factor starts from the second frame. If the number of point cloud overlaps between two frames meets the minimum number within a certain period of time and the displacement between frames does not exceed the preset rotation and translation, it is considered a sequential factor. k-1 and x k The order factor between can be calculated by scanning and matching the two point clouds collected at these two moments. In order to enhance robustness, the order factor between ssm (>1) The features extracted from the frames are calculated together, assuming that the drift is negligible in a short time. Fig. 20 As shown, let T k-1，-i P k-i is the transformed point cloud of pose k-1 in Cartesian coordinates in the ki-th frame, then the target point cloud can be obtained by formula (55). As described in the previous section, if the scan matching calculation fails, the initial transformation of the track calculation will be used for calculation.

[0247]

[0248] The non-sequential scanning matching factor is loop detection. The process is the same as sequential matching, except that the source point cloud is restricted to the earliest time measurement and is obtained by detecting the same features.

[0249] The core idea of ​​the factor graph is to estimate the maximum a posteriori probability of the robot and the surrounding environment based on the measurement information of the odometer and different sensors, as shown below:

[0250]

[0251] Among them, U is the control input of the robot; Z is the measurement information; X is the posture of the robot; and M is the position of the observation point.

[0252] Assume that the SLAM system model and observation model obey the Gaussian noise distribution, that is,

[0253]

[0254] Taking the scaling factor into account, we have:

[0255]

[0256] Its maximum a posteriori probability estimate can be transformed into:

[0257]

[0258] According to the actual situation, due to the similarity of some terrains, it is necessary to consider the matching problem caused by the wrong loop, and it is necessary to introduce switch variables for nonlinear iteration to improve the fault tolerance of the SLAM system. Fig.21As shown, s is an erroneous loop caused by some similar terrains.

[0259] By introducing weight values ​​into the loop, the maximum a posteriori probability estimate can be described as:

[0260]

[0261] The above formula adjusts the weight of the loop detection constraint through the switching factor, allowing it to have a certain degree of fault tolerance.

[0262] S33, loop detection based on PCM.

[0263] Loop detection is an important part of SLAM, which is used to identify when the robot passes through the same place or similar environment, thereby eliminating the accumulated drift between frame matching, reducing positioning errors and improving map consistency.

[0264] Given a set of loop measurement data, a consistency check is performed, and an undirected graph G = (V, E) is finally generated. The vertices carry edge measurement pairs, and the edges represent the consistency relationship between them. In graph theory, although the calculation time to find the largest cluster is very long, fortunately, the scale of the loop graph is small enough, and the cluster in the experiment can be found by exhaustively searching for the largest cluster. In practice, a loop detection queue is maintained according to the arrival time of the loop detection. At each key frame, PCM is executed to find the largest cluster. If the number of clusters is greater than N pcm , then all measurements in this group are considered correct, and those measurements that have not yet been added to the factor graph will be added to the factor graph for optimization.

[0265] like Fig. 22 As shown in the figure, in the experiment, when the robot observes an obstacle that it has observed before, if the loop detection is successful, it will add constraints and correct the posture. The red line in the figure indicates that the two have loop constraints and the observed scenes are similar.

[0266] S34, occupy and build a map.

[0267] There are two methods for mapping. The first is to use point cloud maps for mapping, stitching multiple point cloud data into a more complete point cloud map to provide information for tasks such as environmental perception and robot navigation.

[0268] The second method is to use an occupancy map. The map is discretized into independent grid cells and recursively updated through a Bayesian filter. Since the state estimate obtained using SLAM will have some drift, it is hoped that the updated trajectory can be used to correct the error of the occupancy map when the loop detection corrects the drift. To this end, an occupancy map algorithm based on a sub-map map is used. This algorithm establishes a local map at each key frame with the pose of the key frame as the center point. If a certain section of the trajectory changes, the map can be quickly recalculated.

[0269] Assume that the pose of the key frame is obtained from the SLAM system, denoted as The acquired sonar image features can construct a subgraph in the local coordinate system, denoted as S k ={m ki}, where m ki ∈R 2 represents the two-dimensional grid unit in the kth key frame. Let p(m ki =1) indicates the probability that the cell is occupied, and the corresponding probability value is calculated:

[0270]

[0271] Among them, I ki It indicates the accumulated value of the sonar intensity of the corresponding grid, and n indicates the number of accumulations.

[0272] The superscript k is used to represent the occupancy probability of the cell in the kth keyframe. Then the subgraph of the entire map can be represented as And under the given key frame pose X and sub-graph set M, it is gradually updated through Bayesian filtering to output the expected occupancy map {mi} in the global coordinate system.

[0273]

[0274] Among them, T kg m i It is the operation to transform the global grid cells into the local coordinate system.

[0275] Since loop detection and inter-frame matching will cause the robot's historical posture to be constantly updated and transformed, when the change in its rotation and translation exceeds the threshold set in this paper, the map of the kth key frame is updated using formula (63). The map is not updated all the time to reduce repeated computing resource consumption. If high accuracy is required, the threshold can be set smaller according to actual needs to perform real-time updates to improve accuracy.

[0276] l′(m i )=l(m i )-l k (T kg mi )+l k (T′ kg m i ) (63)

[0277] like Fig.23 As shown in the figure, it is part of the intermediate process of SLAM from the beginning to the end. The darker the black value of the grid, the greater the probability of occupancy, representing obstacles. The gray-white area represents the free area of ​​space, and the dark green area represents the unknown area. From process 1 to 6, the map is gradually updated and the mapping process is finally completed.

[0278] S4. Underwater SLAM simulation and experimental results and analysis.

[0279] In order to verify the perception algorithm proposed in this invention in an unknown and complex underwater environment, simulation experiments and actual experiments will be carried out for verification. In order to facilitate the deployment of the real experimental environment, a simulation environment is first built to simulate the pool environment and robot environment of the laboratory to verify the algorithm of this invention. Then, an experimental platform is built and tested in a real indoor and outdoor environment to analyze the true value trajectory, the trajectory predicted by the existing algorithm, and the algorithm prediction trajectory error to further verify the accuracy and reliability of the algorithm of this invention.

[0280] S41. Underwater simulation experiment verification.

[0281] Control the robot to move at a low speed, set the sonar detection distance to 4m, and make it walk a rectangular trajectory. The SLAM algorithm is referred to as GP SLAM in the following. Through experiments, it can be found that the GP SLAM algorithm has the same motion trajectory and mapping effect as the real one when the sonar image has no multipath reflection and the sonar noise is small. Fig.24 As shown in (b), it can be seen that in the simulation environment, it is relatively easy to control the robot to keep moving at the same height. The robot can well keep moving at the same horizontal plane, and the trajectory effect is as follows Fig.24 As shown in (a) in .

[0282] like Fig.25 As shown in the figure, it can be seen that the obstacles described by the point cloud map are basically consistent with the obstacles in the simulation, and the motion trajectory is consistent with the actual expected walking. The colored points in the figure are point cloud maps, the green ones are robot trajectories, and the red lines are detected loop detections. It can be seen that the map is basically consistent with the real one, and can well describe circular, square obstacles and walls.

[0283] S42. Experimental verification of positioning and mapping algorithm.

[0284] The algorithm of the present invention is referred to as GP SLAM, and is first verified in an indoor 3×8m swimming pool. Fig.26 As shown, the detection distance of the sonar is set to 4m, the gain compensation is set to 15, and the handle is used to control the robot to move at a low speed. In order to keep the robot moving in a plane, the motion capture z-depth information is received and real-time feedback is given. PID control is used to keep the robot moving at a height of 0.55m above the water surface and make the robot travel one circle.

[0285] like Fig. 27 The positioning and mapping results of SLAM. The rectangular black dots represent obstacles, that is, the mapping effect of the pool wall. The black dots above are some of the lens obstacles and water tank obstacles in the laboratory. The closer the color is to black, the greater the reliability of the obstacle. The gray grid is the area that is considered to be free in perception. It can be seen that the mapping effect is close to the true value environment.

[0286] In addition, the algorithm of the present invention is compared with the Bruce SLAM algorithm. Fig.28 As shown in the figure, the mapping effect of the Bruce SLAM algorithm is shown, the colored one is the point cloud map, and the green one is the trajectory. It can be seen that when the environmental features are complex or numerous, when it moves in a small range, the Bruce SLAM frame matching effect is poor, it cannot converge well, and drift occurs. The mapping effect and positioning effect are relatively poor. The occupation mapping and positioning mapping algorithm used in the present invention can well describe the surroundings, and its positioning effect is good and robust.

[0287] The SLAM prediction trajectory of the present invention is compared with the existing Bruce SLAM algorithm and the motion capture true value trajectory. Fig.29 As shown. By using the evo tool, the three tracks are aligned as much as possible for analysis. Fig.29 The blue in (a) indicates the GPSLAM trajectory predicted by the present invention, the orange is the trajectory of the Bruce SLAM algorithm, and the dotted line is the real trajectory of the robot captured by motion capture. It can be seen that the trajectory of the present invention is closer to the motion capture trajectory. The GPSLAM algorithm has fewer inter-frame matching failures and can maintain robustness in a more complex environment, while the Bruce SLAM algorithm has more drift, indicating that its feature tracking fails and more erroneous inter-frame matching occurs, proving that the positioning accuracy of the algorithm of the present invention is higher.

[0288] Fig.29 (a) in the figure is the error in the x, y, and z directions. The robot is basically at a height of 0.58m, and the motion capture measurement accuracy can reach 0.02m. Since it is assumed that the robot moves in a horizontal plane, but in reality it is difficult to control the robot to move at a fixed depth, there will be a certain deviation in the z direction, which will further aggravate the error in the x and y directions of SLAM.

[0289] For trajectory error analysis, the positioning trajectory of GP-SLAM, the true value captured by motion capture, and the Bruce SLAM trajectory are compared with the true value trajectory, such as Fig.30 , Fig.31 The figure shows the calculation result of the absolute position error between the estimated motion trajectory and the true value. By comparing the trajectory errors of the three, it can be seen that the average error mean between the GP SLAM predicted trajectory of the present invention and the true trajectory is 0.118107m, and the root mean square error rmsez is 0.131955m; while the trajectory predicted by the Bruce SLAM algorithm is compared with the true value, and its average error mean is 0.188567m, and the root mean square error rmsez is 0.246543m, and its error is relatively large, and both are greater than the error of the present invention. By comparison, the positioning accuracy of the algorithm of the present invention is very high, and the accuracy is better than that of the Bruce SLAM algorithm; and the standard deviation std of the GP SLAM trajectory of the present invention is 0.058847m, while that of the Bruce SLAM algorithm is 0.158826m, and the errors of other items of the present invention are also basically smaller than the errors of other items of Bruce SLAM, which further illustrates that the positioning algorithm of the present invention has high robustness and is better than the Bruce SLAM algorithm.

[0290] Therefore, the present invention adopts the above-mentioned underwater synchronous positioning and map construction method based on multi-beam sonar, extracts target features from the sonar original image through a filtering algorithm to establish constraints; obtains non-sequential factors by establishing continuous inter-frame matching order factors and loop detection and adds them to the back-end for optimization; at the same time, in order to solve the problem of falling into a local optimal solution in the ICP solution process, a GP graph matching algorithm is proposed to provide initial values ​​to solve this problem; the Akaze algorithm is used to solve the long straight line environment matching degradation problem in parallel; the consistency measurement set is maximized to eliminate outliers in loop detection; finally, a trajectory is established through the estimated posture, and a map based on a subgraph is established, which improves the accuracy and reliability of target feature extraction, improves the matching progress, and has strong robustness.

[0291] Finally, it should be noted that the above embodiments are only used to illustrate the technical solution of the present invention rather than to limit it. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that they can still modify or replace the technical solution of the present invention with equivalents, and these modifications or equivalent replacements cannot cause the modified technical solution to deviate from the spirit and scope of the technical solution of the present invention.

Claims

1. A method for underwater synchronous positioning and mapping based on multi-beam sonar, characterized in that: The following steps are involved: S1. Construct a system model for underwater synchronous positioning and mapping, including motion model, observation model and sensor model; S2. Based on the improved sonar feature matching algorithm, the target features are accurately extracted and matched. The specific process is as follows: S21. Based on the improved CFAR filtering algorithm, accurate extraction of target features is performed; The detection technology based on constant false alarm rate (CFAR) is used. The relationship between the detection unit and its surroundings is determined by using a sliding window. The reference units in the window are averaged after eliminating the cells close to the detection unit, and then compared with the detection unit. S22, cluster segmentation based on Dbscan; Use the Dbscan algorithm for clustering, which is used to find high-density areas in a given data set and divide them into different clusters; S23, establishment of graphs and descriptors; After obtaining multiple clusters, the number of point clouds contained in each cluster is screened to remove discrete noise points, while retaining the number of point clouds greater than Pts threshold Clusters of Traverse the cluster labels {Iabel}, for each cluster label i The point cloud group is used to generate Gaussian blocks; S24, matching constraints of descriptors; After generating Gaussian blocks, a graph describing the Gaussian information and adjacency relationship of an image is established. The descriptors are further optimized and screened by searching for the largest cluster, and the descriptors are aligned based on the GP algorithm. S3, establishment of sonar SLAM framework; S4. Underwater SLAM simulation and experimental results and analysis. The specific process is as follows: First, a simulation environment was built to simulate the pool environment and robot environment of the laboratory to verify the algorithm proposed above. Then, an experimental platform was built and tested in real indoor and outdoor environments to analyze the true value trajectory, the trajectory predicted by the existing algorithm, and the trajectory error predicted by the algorithm to further verify the accuracy and reliability of the algorithm proposed above. S41, conducting underwater simulation experiment verification; S42. Conduct experimental verification of the positioning and mapping algorithm.

2. The underwater synchronous positioning and mapping method based on multi-beam sonar according to claim 1, characterized in that: In step S3, the establishment of the sonar SLAM framework includes the following steps: S31, front-end frame matching and dead reckoning; S32, gtsam backend optimization; S33, loop detection based on PCM; S34, occupation map algorithm based on sub-map, occupation map construction.

3. The underwater synchronous positioning and mapping method based on multi-beam sonar according to claim 2, characterized in that: In step S31, front-end frame matching and dead reckoning include the following processes: S311, based on the improved GP and Akaze algorithms, frame matching is performed; S312, inter-frame matching method based on parallel Akaze algorithm and GP algorithm; S313, based on the front-end frame matching, establishing dead reckoning by using sensor measurement data and motion model; S314. Relationship between coupled frame matching and dead reckoning.

4. The underwater synchronous positioning and mapping method based on multi-beam sonar according to claim 3 is characterized in that: In step S311, performing inter-frame matching specifically includes: (1) Globally initialize ICP; (2) Derivation of probability model; (3) Inter-frame matching constraints.

Images