Underwater simultaneous localization and mapping method based on key feature extraction of seafloor topography

Abstract

The application discloses a kind of underwater simultaneous localization and mapping methods based on seabed topography key feature extraction, comprising the following methods: step one, determine vehicle motion model, establish motion constraint factor, using motion information to remove the low quality factor of SLAM front end;Step two, in actual navigation process, vehicle carries out real-time observation and data association to environmental feature, simultaneously real-time detection whether there is closed loop, obtains terrain constraint factor;Step three, according to the constraint factor described above, the feature map and motion information are updated constantly, so as to obtain real-time positioning navigation result.This method has certain improvement in positioning accuracy, weakens the influence of environmental noise on SLAM system, so that it realizes higher-precision positioning navigation without prior mapping chart.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of underwater terrain aided navigation, in particular to an underwater simultaneous localization and mapping method based on key feature extraction of seabed terrain. BACKGROUND

[0002] In the current underwater navigation and positioning method, the navigation methods available for underwater vehicles mainly include two categories: non-autonomous navigation and autonomous navigation. Non-autonomous navigation methods, such as Loran, Omega, GPS, etc., can only complete navigation when the receiver can receive navigation signals. The navigation accuracy of Loran and Omega is lower than that of GPS. However, due to the rapid attenuation of electromagnetic waves in water, the use of these navigation methods based on radio waves on underwater vehicles is greatly limited. Compared with electromagnetic signals, acoustic signals can propagate over a long distance underwater, so acoustic transducers can be used as beacons to guide the navigation of the carrier. The currently used acoustic navigation systems mainly include long baseline navigation, short baseline navigation and ultra-short baseline navigation. These three forms all need to place transducers or transducer arrays in the navigation sea area in advance. Since acoustic navigation requires position-known beacons, this method is more suitable for scientific research and other civilian fields. The current geophysical navigation methods mainly include navigation based on geomagnetism, navigation based on gravity field and navigation based on terrain and topography. However, when performing such navigation, the carrier needs to match the measured data with the prior surveying and mapping graph or database, which not only has a high cost and difficulty in generating these prior surveying and mapping graphs, but also the computational complexity of finding the matching peak value increases exponentially with the increase of the spatial dimension. Due to the shortcomings of these navigation methods, people are exploring underwater navigation algorithms without prior graphs, which is also the problem that the SLAM algorithm tries to solve.

[0003] It is a key problem for a robot to perceive the surrounding information by relying on its own sensors in a completely unknown environment and to realize its own positioning and environment map creation at the same time, which is a key problem for the robot to realize autonomous navigation. The initial goal of SLAM technology is to make a mobile robot construct a map by using its own sensors in an unknown position and without prior environmental information, and to estimate its own attitude (position and direction) by using the map.

[0004] In recent years, many technologies for solving indoor, outdoor, even aerial and underwater simultaneous localization and mapping have emerged, however, in underwater applications, the sensor perception ability is weakened, the underwater terrain data is large and the identifiable features are sparse, and the real-time performance and robustness of the system are difficult to guarantee, which makes the underwater SLAM problem one of the most challenging problems. SUMMARY

[0005] In view of the deficiencies of the prior art, the underwater simultaneous localization and mapping method based on seabed terrain key feature extraction is provided, the efficiency of the front end and the rear end of the SLAM system is improved, and the positioning accuracy is improved.

[0006] To achieve the above object, the underwater simultaneous localization and mapping method based on seabed terrain key feature extraction is realized through the following technical scheme: in a two-dimensional plane in the deep sea, an underwater vehicle with a known motion model starts from an unknown initial point and moves in an unknown environment where terrain information can be detected; the vehicle senses the environment information in real time through a sensor and determines its three-dimensional coordinates at the same time, including the following methods: step one, determining the motion model of the vehicle, establishing a motion constraint factor, and using the motion information to remove the low-quality factor of the SLAM front end; step two, in the actual navigation process, the vehicle observes and data associates the environment features in real time, and simultaneously detects whether there is a closed loop to obtain a terrain constraint factor; step three, performing graph optimization according to the above constraint factor, and continuously updating the feature map and the motion information to obtain real-time positioning and navigation results.

[0007] Preferably, in step one, the subgraph length can be selected as 0.5-1 times the strip width.

[0008] Preferably, in step three, the graph optimization SLAM optimal trajectory is converted into a problem of solving the optimal node position, and the optimization objective function is designed as follows:

[0009]

[0010] In the formula, Omega ij is a weight coefficient, representing the weight of the error term e(P SLAMi ,P SLAMj ,Z ij ) in all effective constraint factor corresponding error terms, determined by the feature matching correlation between the subgraphs corresponding to the i-j constraint factor group,

[0011] Optimization function

[0012]

[0013] The underwater simultaneous localization and mapping technology based on multi-beam sonar data has good navigation real-time performance, high robustness, and does not need a prior mapping graph, and is suitable for small underwater mobile vehicles in the deep sea environment. BRIEF DESCRIPTION OF DRAWINGS

[0014] Figure 1 Fig. 8 is a diagram of subgraph length versus overlap ratio;

[0015] Figure 2a Fig. 9 is an original graph of feature regions based on adaptive grid;

[0016] Figure 2b Fig. 10 is a grid graph of feature region division based on adaptive grid;

[0017] Figure 2c Fig. 11 is an extracted feature region graph based on adaptive grid;

[0018] Figure 3 Fig. 12 is a diagram of SLAM system factor graph;

[0019] Figure 4 Fig. 13 is a diagram of SLAM system optimization result based on robust graph optimization;

[0020] Figure 5 Fig. 14 is a diagram of SLAM system trajectory error based on robust graph optimization. DETAILED DESCRIPTION

[0021] The technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the drawings in the embodiments of the present application. Obviously, the described embodiments are only some of the embodiments of the present application, but not all the embodiments of the present application.

[0022] The present application will be further described with reference to the drawings.

[0023] An underwater simultaneous localization and mapping method based on seabed terrain key feature extraction, comprising the following steps:

[0024] (1) Establishing adaptive subgraph

[0025] A plurality of beam pings are combined to form a subgraph. When the subgraph feature or length reaches an adaptive threshold, the current subgraph is stored, and data collection for the next subgraph is started.

[0026] To increase the overlap ratio and matching accuracy, the subgraph length can be selected as 0.5-1 times the strip width, to ensure at least 25% overlap ratio, as shown in Fig. 5. Figure 1

[0027] (2) Grid division

[0028] According to the point cloud density of the subgraph, a grid of appropriate size is established, and the water depth points are distributed to each grid range.

[0029] The point cloud density can be calculated by the following formula:

[0030] ​

[0031] where b i is the i-th water depth point in the point cloud, b i ′ is its nearest neighbor, and n is the number of points in the point cloud.

[0032] As shown in Fig. 2(b), subgraph B is divided into grid regions g ij .

[0033] (3) Feature metrics

[0034] The feature metrics of water depth points within a grid region g ij are calculated. Suppose there are n water depth points b i , i = 1, 2, …, n, within the grid region, and their features f ij can be calculated as:

[0035] Water depth mean

[0036] Water depth standard deviation σ:

[0037] Terrain normal vector

[0038]

[0039] Terrain slope:

[0040] Terrain aspect:

[0041] Terrain curvature:

[0042] (4) Establishing feature vectors

[0043] The features of adjacent 3*3 grid regions G i，j = g i：i+2，j：j+2 are taken in turn to establish feature vectors:

[0044]

[0045] (5) Coarse registration

[0046] The similarity of each two feature vectors is measured using Mahalanobis distance, and the 3*3 grid regions G s1，t1 and G s2，t2 corresponding to the high-similarity feature vector pair are selected. The transformation T between the two regions is calculated, and this transformation is applied to other 3*3 grid regions. The consistency of the transformation is used to score the transformation, and the transformation with the maximum score is selected as the coarse registration transformation.

[0047] Calculate the mean of the feature vector F and its covariance matrix The similarity between two feature vectors can be expressed as:

[0048]

[0049] Calculate the transformation

[0050] T = G s2，s2 - G s1，s1

[0051] Calculate the transformation score

[0052] e k = G si，sj - G si，sj - T

[0053]

[0054] score = ∑ δ k

[0055] Calculate the best transformation

[0056] T best = T score＝ma(score)

[0057] (6) Feature subgraph extraction

[0058] Compare the eigenvalue f ij of each grid with the eigenvalues f i′j′ of the surrounding grids, and record the grid with a larger feature difference d as a feature grid. Determine whether each grid is a feature grid in turn, and the set of all feature grid depth points constitutes a feature subgraph B f , as shown in Figure 2(c).

[0059] Calculate the adjacent grid feature difference:

[0060] d ij = max(f ij - f i′j′ )

[0061] i' = [i-1, i, i+1], j' = [j-1, j, j+1], [i', j'] ≠ [i, j]

[0062] Feature grid determination:

[0063]

[0064] Extract feature points

[0065]

[0066] (7) Fine registration

[0067] For source point cloud and target point cloud On the basis of coarse registration, centroid removal transformation is carried out, and fine registration is carried out on subgraph data using clipping ICP.

[0068] In order to prevent matrix singularity in the calculation process, the target point cloud and the source point cloud are both subtracted by the centroid of the target point cloud:

[0069]

[0070]

[0071]

[0072] Calculate the Euclidean distance of point set and click The two points with the shortest distance are recorded as corresponding points

[0073]

[0074] Remove the points with Euclidean distance greater than threshold ∈, and the clipped point cloud is:

[0075]

[0076] Calculate the target point cloud Normal n j =(n jx ,n jy ,n jz ,0) T ;

[0077] Find the optimal transformation between the two point clouds:

[0078]

[0079] (8) Consistency verification

[0080] The result of fine registration should meet the distance between the nearest neighbor points of the point cloud, the normal angle within a certain range, and the amplitude of translation and rotation transformation should not exceed the uncertainty of navigation.

[0081] Distance and normal constraint:

[0082] ||b′ k -b′ j ||<∈ d

[0083] <n′ k ,n′j ><∈ a

[0084] The rotation matrix R corresponds to a rotation angle a around the z axis, which is smaller than the heading uncertainty, and the translation vector t is smaller than the position uncertainty:

[0085] R→α,α<u h

[0086] t<u p

[0087] The data association that satisfies the consistency check condition is correct.

[0088] (9) Establish a closed loop

[0089] Use the position relationship between the matched subgraphs to establish a closed loop constraint.

[0090]

[0091] (10) Construct a factor graph

[0092] Let P SLAM =[P SLAM ,P SLAM2 ,…,P SLAMN ] represent the preset N pose nodes to be optimized, establish a pose constraint factor to ensure the shape of the trajectory, and establish a terrain constraint factor to correct the trajectory error.

[0093] Pose constraint construction:

[0094]

[0095] Terrain constraint factor construction:

[0096]

[0097] The factor graph is shown in Figure 3 .

[0098] (11) Factor graph optimization

[0099] The problem of solving the optimal trajectory of the graph optimization SLAM is converted into the problem of solving the optimal node position, and the optimization objective function is designed as follows:

[0100]

[0101] In the formula, Ω ij is the weight coefficient, e(P SLAMi ,P SLAMj ,Z ij) the weight of all valid constraint factors in the error term, determined by the feature matching correlation parameter between the sub-graphs corresponding to the i-j group of constraint factors.

[0102] Optimization function

[0103]

[0104] Solved using LM optimization. As shown in Figure 4 , 8 data associations are established on a navigation track, the optimized track is closer to the true track, and the navigation cumulative error is corrected to a certain extent. The comparison of the push bit error before optimization and the SLAM track error after optimization is shown in Figure 5 .

[0105] The above is only the preferred specific embodiment of the present application, but the protection scope of the present application is not limited to this. Any person skilled in the art, according to the technical solution and the inventive concept of the present application, makes equivalent replacement or change within the technical range disclosed by the present application, which should be covered within the protection scope of the present application.

Claims

1. A method for underwater simultaneous localization and mapping based on key feature extraction of seafloor terrain, characterized in that: The method comprises the following steps: (1) Adaptive subgraph establishment Multiple beam pings are combined to form a subgraph, when the subgraph feature or length reaches an adaptive threshold, the current subgraph is stored, and data collection of the next subgraph is started; In order to increase the overlap rate and matching accuracy, the subgraph length can be selected as 0.5-1 times of the swath width, so as to ensure that there is at least a 25% overlap rate; (2) Grid division According to the point cloud density of the subgraph, a grid of appropriate size is established, and the water depth points are distributed in the range of each grid; The point cloud density can be calculated by the following formula: where b i is the i-th water depth point in the point cloud, b i ′ is its nearest neighbor, and n is the number of points in the point cloud; (3) Feature measurement The feature measure f ij for a water depth point within a grid region g i , i = 1, 2, …, n, can be calculated as: ij ​ Mean water depth Water depth standard deviation σ: terrain normal vector Terrain slope: Terrain aspect: Terrain curvature: (4) Establishment of feature vector Take the adjacent 3*3 grid G in turn i,j = g i:i+2,j:j+2 Eigenvalues of the characteristic vector: (5) Coarse registration The similarity of each two feature vectors is measured using Mahalanobis distance, and a 3*3 grid area G corresponding to a high similarity feature vector pair is selected s1,t1 and G s2,t2 The transformation T between the two areas is calculated, the transformation is applied to other 3*3 grid areas, the transformation is scored using the consistency of the transformation, and the transformation with the maximum score is selected as the coarse registration transformation; The mean of the feature vectors F is computed and the covariance matrix thereof The similarity between two feature vectors can be expressed as: Obtaining a transformation T = G s2,s2 - G s1,s1 Obtaining a transformation score e k = G si,sj - G si,sj - T score =∑δ k Obtaining the best transformation T best = T score＝max(scire) (6) Feature subgraph extraction The feature value f of each grid is calculated ij The feature value f of each grid is calculated i′j′ The feature value f of each grid is calculated f The feature value f of each grid is calculated Calculating the feature difference value of adjacent grids: d ij = max(f ij -f i′j′ ) i' = [i-1, i, i+1], j' = [j-1, j, j+1], [i', j'] ≠ [j, j] Feature grid determination: Extracting feature points (7) Fine registration For source point cloud and target point cloud On the basis of rough registration, the centroid removal transformation is carried out, and the subgraph data is precisely registered using the clipping ICP. In order to prevent matrix singularity in the calculation process, the target point cloud and the source point cloud are both subtracted by the centroid of the target point cloud: Compute the set of points and click Euclidean distance, take the two points closest to the corresponding points Points with a Euclidean distance greater than a threshold ∈ are removed, and the clipped point cloud is: Computing target point cloud Normal n j = (n jx , n jy , n jz , 0) T ; Obtaining the optimal transformation between the two point clouds: (8) Consistency verification The result of fine registration should satisfy that the distance between the nearest neighbor points of the point cloud and the normal angle are within a certain range, and the amplitudes of translation and rotation transformation should not exceed the navigation uncertainty; Distance and normal constraint: ||b′ k -b′ j ||<∈ d <n′ k ,n′ j ><∈ a The rotation angle α corresponding to the rotation matrix R around the z-axis is less than the heading uncertainty, and the translation vector t is less than the position uncertainty: R→a,a h ​ t < u p The one that satisfies the consistency verification condition is the correct data association; (9) Closed loop establishment The position relationship between the matching subgraphs is used to establish a closed loop constraint; (10) Construction of factor graph P SLAM = [P SLAM1 0, P SLAM2 1, …, P SLAM N] represents the preset N pose nodes to be optimized, a pose pushing constraint factor is established to ensure the shape of the trajectory, and a terrain constraint factor is established to correct the trajectory error; Pushing the position constraint construction: Constructing the terrain constraint factor: (11) Factor graph optimization The optimal trajectory of graph optimization SLAM is converted into a problem of solving the optimal node position, and the optimization objective function is designed as follows: Where, Ω ij is the weight coefficient, which represents the error term e(P SLAM ,P SLAMj ,Z ij The weight of the error term corresponding to all valid constraint factors is determined by the feature matching correlation judgment parameter between the subgraphs corresponding to the ijth group of constraint factors; Optimization function The LM optimization is used for solving.

2. The method of claim 1, wherein the method is based on key features extraction of seafloor topography. In step one, the subgraph length can be selected as 0.5-1 times of the swath width. 3.The method of claim 1, wherein the method further comprises: In step three, the optimal trajectory of graph optimization SLAM is converted into a problem of solving the optimal node position, and the optimization objective function is designed as follows: wherein Ω ij is a weight coefficient, and e(P SLAMi , P SLAMj , Z ij ) represents the weight of the error term e(P SLAMi , P SLAMj , Z ij ) in all valid constraint factor corresponding error terms, determined by the feature matching correlation parameter between the sub-graphs corresponding to the i-th to j-th constraint factor groups. Optimization function

Images