Underwater terrain SLAM robust back-end optimization algorithm based on Fretch distance
Through the use of Fretch -based robust back -end optimization algorithms in the underwater SLAM technology, automatically identify and remove error closed loops, solving the problem of insufficient robustness in the existing technology, and realized long -range, high -precision under unknown environments, high accuracy in unknown environments Reliable and accurate navigation in navigation scenarios.
Patent Information
- Application Number
- CN202411163560.9
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-08-23
- Publication Date
- 2025-01-28
AI Technical Summary
The existing underwater SLAM technology is not robust enough when dealing with errors closed loops, resulting in the accumulation of errors in navigation state estimation and map construction, especially in long-range and high-precision navigation scenarios under unknown environments.
Based on the snack rear optimization algorithm based on the Fretch distance, it automatically recognizes and removes the error closed loop through factor graph construction, Levenberg –Marquardt optimizer, error closed -loop detection and voting algorithm to optimize the AUV's trajectory and topography.
It significantly improves the steelation of the underwater SLAM on the error closed loop, reducing the impact of error closed -loop on navigation status and map construction, and ensuring that reliable and accurate positioning navigation can still be achieved in the unknown terrain.
Smart Images
Figure CN119374587A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the technical field of underwater autonomous navigation, in particular to an underwater terrain SLAM robust back-end optimization algorithm based on Fretch distance. Background Art
[0002] Underwater SLAM technology belongs to the field of underwater robot navigation and refers to the technology used for autonomous positioning, navigation and path planning of robots in underwater environments. These technologies include but are not limited to: (1) Inertial navigation technology: using inertial sensors such as gyroscopes and accelerometers for autonomous positioning; (2) Hydroacoustic positioning technology: using long baseline, ultra-short baseline and other hydroacoustic positioning systems to provide positioning for underwater vehicles; (3) Combined navigation technology: integrating inertial navigation, Doppler velocimetry, magnetometer and other data to estimate navigation status and improve long-term positioning accuracy in underwater environments; (4) Geophysical field-assisted navigation: including matching positioning technology based on terrain, gravity and magnetic maps, as well as technology for synchronous positioning and mapping in unknown environments using real-time measured seabed terrain or gravity and magnetic data.
[0003] According to the different sensors used, underwater SLAM technology can be divided into visual image-based SLAM, sonar image-based SLAM, and distance point cloud-based SLAM. Underwater visual SLAM uses a camera as the main environmental sensor, and completes the perception of the position of the underwater autonomous vehicle (AUV) and the construction of an environmental map by fusing an inertial navigation unit, a compass, a depth meter, and an environmental image. SLAM based on sonar images usually uses side-scan sonar, forward-looking sonar, or mechanical scanning sonar to obtain underwater topographic images, and realizes the association of environmental information through image feature point matching or observation of feature landmarks. SLAM is realized based on the constraints of environmental association information and navigation information. SLAM based on distance point cloud usually uses single-beam depth sounder, lidar, and multi-beam sonar to generate distance information of the AUV's surrounding environment. The distance information at multiple times is combined into a point cloud and the point cloud data is matched to achieve the association of environmental information. Multi-beam sonar can provide topographic maps, scattering intensity and water depth point cloud information, and is widely used in water depth topographic mapping, and is more suitable for large-scale water depth SLAM work.
[0004] The SLAM backend solution framework mainly includes traditional filtering methods and factor graph-based graph optimization methods. The filtering method recursively updates the prior estimate of the robot state and combines the sensor measurement data to obtain the posterior estimate, thereby achieving real-time underwater positioning and map construction. The filtering-based depth SLAM methods mainly include Extended Kalman Filter (EKF) SLAM and Particle Filter (PF) SLAM. Filter-based SLAM methods usually have problems such as high computational complexity when the state vector is high-dimensional, inaccurate noise modeling, and insufficient optimization in the update phase leading to error accumulation. The graph optimization method represents the SLAM problem as a factor graph and uses graph theory methods to obtain the optimal state estimation and map construction. The graph optimization SLAM method can be divided into the front end and the back end. The front end constructs a factor graph and uses the data association method to generate the closed-loop factors in the graph. The back end optimizes the factor graph to achieve accurate pose estimation and map construction. Graph optimization can be viewed as a nonlinear least squares problem that can be solved using a variety of methods, such as the Gaussian-Newtown method, the Levenberg-Marquardt (LM) method, and gradient descent.
[0005] Graph optimization SLAM is often based on the least squares method, which is unable to identify correct closed loops and incorrect closed loops. This results in the SLAM algorithm having poor tolerance for incorrect closed loops. Even when there are a small number of incorrect closed loops, the optimization results will have large errors. Therefore, the robustness of the back-end optimizer to incorrect closed loops is a key factor in determining the accuracy of SLAM. Common robust back-end algorithms include: (1) adding switch variables to the objective function of factor graph optimization to obtain a robust solution for SLAM to incorrect closed loops. (2) Based on the consistency of the constraints between the correct closed loops and the odometry constraints, the χ 2 Threshold detection and deletion of incorrect closed loops. The above solution may lead to local optimization or greatly increase the time consumption of the backend. Summary of the invention
[0006] In view of the shortcomings of the prior art, the present invention provides an underwater terrain SLAM robust back-end optimization algorithm based on Fretch distance, which aims to increase the robustness of depth SLAM to error closed loops and meet the needs of AUV for long-range and high-precision underwater autonomous navigation scenarios in unknown environments.
[0007] To achieve the above objectives, the present invention is implemented by the following technical solutions: a robust back-end optimization algorithm for underwater terrain SLAM based on Fretch distance, comprising the following steps: S1: factor graph construction; the posture of the AUV at all times forms a pose node vector, and during the navigation process of the AUV, whenever a new pose node and factor are generated, they are added to the factor graph;
[0008] S2: AUV trajectory optimization; use the Levenberg–Marquardt optimizer to optimize and solve the constructed front-end factor graph;
[0009] S3: detection error closed loop factor;
[0010] S4: Error closed loop identification; after the error closed loop is detected, the error closed loop is removed through a voting algorithm.
[0011] Preferably, in S1, the posture of the AUV at all times forms a pose node vector represented by X = [x1, x2, ..., x i ]; The navigation factors between the AUV posture nodes constitute a set The data correlation factors between all multi-beam subgraphs constitute a set
[0012] Preferably, an optimized trajectory is generated in S2
[0013] Preferably, in S3, for each pose node associated with the closed-loop factor, 2*n+1 consecutive pose nodes centered thereon constitute a path segment Where k is the serial number of the center pose node, the superscript DR represents the dead reckoning method of the underwater autonomous navigation system, and the corresponding path subsegment after back-end optimization is
[0014] Then, by comparing the Frechet distance between the DR trajectory segment and the SLAM trajectory segment, we can determine whether there is a possible false closed loop. and Decentralize, and then use formula (1) to calculate the Frechet distance D between them Frechet ,
[0015]
[0016] If the closed loop position D Frechet Greater than the threshold t Frechet , then the closed loop will be put into the candidate wrong closed loop set l LC As shown in formula (2),
[0017]
[0018] Preferably, in S4, the voting algorithm comprises the following steps:
[0019] ① Parameter definition and initialization using error closed loop set, pose node X, push navigation factor set Closed-loop factor set As input, the final output of the algorithm is the optimized trajectory X after removing the error closed loop opt , initialize the vote counting variable poll to 0, and initialize the value of the temporary comparison parameter p of the number of votes to infinity;
[0020] ② Establish a temporary factor graph and re-optimize for l LC Each member of It will be separated from Delete, use the pose node X, push navigation factor set New set of closed-loop factors Build the factor graph and reproduce the optimization trajectory using the Levenberg–Marquardt optimizer
[0021] ③ Count the number of wrong closed loops in the re-optimization results as the number of votes. Use the Frechet distance defined in formula ① to detect the distortion of the newly generated optimization trajectory, and add the closed loops in the area where the distortion exceeds the threshold to the candidate wrong closed loop set use The number of incorrect closed loop elements in the collection will be used as The number of votes received;
[0022] ④ Determine the final removed error loop. When a certain loop is removed, the number of error loops in the re-optimized trajectory is reduced or the error loop is empty, which proves that the removed error loop is effective. LC The members in the poll calculate the number of votes, and the candidate with the lowest score is the closed loop It will be considered as an incorrect closed loop and Delete from;
[0023] Repeat the process of ①-④ until The algorithm terminates when the set becomes empty.
[0024] The beneficial effects of the present invention are: compared with the optimizer of the traditional SLAM method, it has the ability to automatically identify the error factors in the front-end factor graph, which greatly reduces the impact of possible error closed loops on navigation state estimation and composition results, so that the underwater multi-beam terrain SLAM method can still ensure reliable and accurate positioning and navigation when the terrain features are not obvious.
[0025] During the AUV underwater navigation and multi-beam terrain measurement process, the factor graph consisting of the AUV posture node, multi-beam data correlation factor, and push factor is input into the back-end optimizer. The optimizer uses a continuously updated closed loop to update the AUV posture state and terrain map. At the same time, the invented robust detection method based on Fretch distance is used to identify and eliminate erroneous closed loops at the back end to avoid problems such as severe trajectory deformation caused by errors, thereby ensuring the accuracy of track estimation. BRIEF DESCRIPTION OF THE DRAWINGS
[0026] Figure 1 It is the AUV water depth SLAM factor map in the present invention;
[0027] Figure 2 Schematic diagram of Frechet distance calculation between optimized trajectory and DR trajectory in the present invention;
[0028] Figure 3a is an error closed-loop graph identified by the robust optimizer in the present invention;
[0029] Figure 3b This is a comparison chart between the robust optimizer and the general optimizer in the present invention. DETAILED DESCRIPTION
[0030] The technical solutions in the embodiments of the present invention will be described clearly and completely below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, rather than all the embodiments.
[0031] Combine the following Figure 1 Describe the algorithm process and principles.
[0032] A robust backend optimization algorithm for underwater terrain SLAM based on Fretch distance includes the following steps:
[0033] 1) Factor graph construction
[0034] The posture of the AUV at all times forms a pose node vector and is expressed as X = [x1, x2, ..., x i The navigation factors between AUV pose nodes constitute a set The data correlation factors between all multi-beam subgraphs constitute a set During the AUV navigation process, whenever a new pose node and factor are generated, they are added to the factor graph. The AUV multi-beam water depth SLAM factor graph is as follows: Figure 1 shown.
[0035] 2) AUV trajectory optimization
[0036] Use the Levenberg–Marquardt optimizer to optimize the constructed front-end factor graph and generate an optimization trajectory Since the underwater navigation system usually has a high heading accuracy, the AUV position cumulative error is small in a short time. Therefore, the optimized AUV trajectory and the push-to-position navigation trajectory should have good consistency in a short time. When the front-end factor graph contains an error closed loop, it will cause obvious deformation of the optimized trajectory.
[0037] 3) Detection of error closed-loop factors
[0038] For each pose node associated with the closed-loop factor, the 2*n+1 consecutive pose nodes centered on it constitute a path segment. Where k is the serial number of the center pose node, and the superscript DR represents the dead reckoning method of the underwater autonomous navigation system. The corresponding path subsegment after back-end optimization is
[0039] Then, by comparing the Frechet distance between the DR trajectory segment and the SLAM trajectory segment, we can determine whether there is a possible false closed loop here, such as Figure 2 As shown. and Decentralize, and then use formula (1) to calculate the Frechet distance D between them Frechet .
[0040]
[0041] If the closed loop position D Frechet Greater than the threshold t Frechet , then the closed loop will be put into the candidate wrong closed loop set l LC As shown in formula (2).
[0042]
[0043] 4) Error closed loop identification
[0044] Since an incorrect closed loop will cause serious distortion of the surrounding trajectory, it may also cause the correct closed loop near the incorrect closed loop to be identified as an incorrect closed loop. A voting algorithm is designed to LC The real error closed loop is identified and removed. The specific process of the algorithm is as follows:
[0045] ①Parameter definition and initialization
[0046] Use the error closed loop set, pose node X, and push navigation factor set Closed-loop factor set As input, the final output of the algorithm is the optimized trajectory X after removing the error closed loopopt Initialize the vote counting variable poll to 0, and initialize the value of the temporary comparison parameter p of the number of votes to infinity.
[0047] ② Establish a temporary factor graph and re-optimize
[0048] For l LC Each member of It will be separated from Delete, use the pose node X, push navigation factor set New set of closed-loop factors Build the factor graph. Reproduce the optimization trajectory using the Levenberg–Marquardt optimizer
[0049] ③ Count the number of wrong closed loops in the re-optimization results as the number of votes
[0050] Use the Frechet distance defined by formula ① to detect the distortion of the newly generated optimization trajectory, and add the closed loops in the area where the distortion exceeds the threshold to the candidate error closed loop set use The number of incorrect closed loop elements in the collection will be used as The number of votes.
[0051] ④ Determine the final error closed loop to be removed
[0052] When a closed loop is removed, the number of error closed loops in the re-optimized trajectory is reduced or the error closed loop is empty, which proves that the removed error closed loop is effective. LC The members in the poll calculate the number of votes, and the candidate with the lowest score is the closed loop It will be considered as an incorrect closed loop and Delete it.
[0053] Repeat the process of ①-④ until The algorithm terminates when the set becomes empty.
[0054] Finally, the AUV multi-beam terrain map optimization SLAM error closed loop example is shown in Figure 3(a), and the trajectories optimized by the proposed robust optimizer and the ordinary optimizer are compared with the real trajectory, as shown in Figure 3b shown.
[0055] The above description is only a preferred specific implementation manner of the present invention, but the protection scope of the present invention is not limited thereto. Any technician familiar with the technical field can make equivalent replacements or changes according to the technical scheme and inventive concept of the present invention within the technical scope disclosed by the present invention, which should be covered by the protection scope of the present invention.
Claims
1. A robust back-end optimization algorithm for underwater terrain SLAM based on Fretch distance, characterized by: The following steps are involved: S1: Factor graph construction; the posture of the AUV at all times forms a pose node vector. During the navigation process of the AUV, whenever a new pose node and factor are generated, they are added to the factor graph; S2: AUV trajectory optimization; use the Levenberg-Marquardt optimizer to optimize and solve the constructed front-end factor graph; S3: detection error closed loop factor; S4: Error closed loop identification; after the error closed loop is detected, the error closed loop is removed through a voting algorithm.
2. The underwater terrain SLAM robust back-end optimization algorithm based on Fretch distance according to claim 1 is characterized in that: In S1, the posture of the AUV at all times forms a pose node vector represented by X = [x1, x2, ..., x i ]; The navigation factors between the AUV posture nodes constitute a set The data correlation factors between all multi-beam subgraphs constitute a set 3. The underwater terrain SLAM robust back-end optimization algorithm based on Fretch distance according to claim 1 is characterized in that: Generate optimized trajectory in S2 4. The underwater terrain SLAM robust back-end optimization algorithm based on Fretch distance according to claim 1 is characterized in that: In S3, for each pose node associated with the closed-loop factor, 2*n+1 consecutive pose nodes centered on it constitute a path segment Where k is the serial number of the center pose node, the superscript DR represents the dead reckoning method of the underwater autonomous navigation system, and the corresponding path subsegment after back-end optimization is Then, by comparing the Frechet distance between the DR trajectory segment and the SLAM trajectory segment, we can determine whether there is a possible false closed loop. and Decentralize, and then use formula (1) to calculate the Frechet distance D between them Frechet , If the closed loop position D Frechet Greater than the threshold t Frechet , then the closed loop will be put into the candidate wrong closed loop set l LC As shown in formula (2), 5. The underwater terrain SLAM robust back-end optimization algorithm based on Fretch distance according to claim 1 is characterized in that: In S4, the voting algorithm comprises the following steps: ①Parameter definition and initialization Use the error closed loop set, pose node X, and push navigation factor set Closed-loop factor set As input, the final output of the algorithm is the optimized trajectory X after removing the error closed loop opt , initialize the vote counting variable poll to 0, and initialize the value of the temporary comparison parameter p of the number of votes to infinity; ② Establish a temporary factor graph and re-optimize For l LC Each member of It will be separated from Delete, use the pose node X, push navigation factor set New set of closed-loop factors Build the factor graph and reproduce the optimization trajectory using the Levenberg-Marquardt optimizer ③ Count the number of wrong closed loops in the re-optimization results as the number of votes Use the Frechet distance defined by formula ① to detect the distortion of the newly generated optimization trajectory, and add the closed loops in the area where the distortion exceeds the threshold to the candidate error closed loop set use The number of incorrect closed loop elements in the collection will be used as The number of votes received; ④ Determine the final error closed loop to be removed When a closed loop is removed, the number of error closed loops in the re-optimized trajectory is reduced or the error closed loop is empty, which proves that the removed error closed loop is effective. LC The members in the poll calculate the number of votes, and the candidate with the lowest score is the closed loop It will be considered as an incorrect closed loop and Delete from; Repeat the process of ①-④ until The algorithm terminates when the set becomes empty.