Forward-looking sonar SLAM (Simultaneous Localization and Mapping) method for multilayer dynamic interference suppression

Abstract

The invention discloses a forward-looking sonar SLAM (Simultaneous Localization and Mapping) method for multilayer dynamic interference suppression, and relates to a forward-looking sonar SLAM method. The invention aims to solve the problems of mismatching of features and trajectory drift caused by interference of ships, fish schools, bubbles and the like in complex dynamic water areas such as reservoirs and channels. In order to solve the problems of feature mismatching and trajectory drift caused by interference of ships, fish schools, bubbles and the like in complex dynamic water areas such as reservoirs, channels and the like, a multi-layer dynamic interference suppression mechanism facing a sonar image and motion consistency constraint in a matching stage are introduced on the basis of a traditional forward-looking sonar SLAM framework; and high-precision and high-robustness autonomous underwater vehicle navigation and map construction can be realized under the condition of an airborne CPU (Central Processing Unit). The invention belongs to the technical field of autonomous navigation and positioning of underwater robots.

Description

Technical Field

[0001] This invention relates to a forward-looking sonar SLAM method, belonging to the field of underwater robot autonomous navigation and positioning technology. Background Technology

[0002] With the continuous advancement of marine resource development and marine scientific research, autonomous underwater vehicles (AUVs) are increasingly widely used in tasks such as seabed exploration, environmental monitoring, and underwater search and rescue. However, due to the ineffective coverage of GPS signals in the underwater environment, and the limitations of optical imaging caused by water turbidity and lighting conditions (resulting in a very short usable range), coupled with factors such as poor visibility, complex underwater structures, and drastic water flow changes, autonomous navigation and high-precision positioning of underwater robots remain significant challenges.

[0003] Underwater SLAM in complex waters typically uses forward-looking sonar as the primary sensing method, relying on sonar images for environmental mapping and track estimation. However, a series of problems remain in practical operation. First, sonar images are often affected by speckle noise and multipath echoes, resulting in low image contrast and blurred edges. Traditional extraction methods relying on geometric features struggle to obtain stable and reliable feature points. Second, in scenarios such as reservoirs and waterways, frequent ship traffic, fish activity, and propeller bubble bands constantly enter the field of view. Existing SLAM methods still rely on static environment assumptions for scan matching and loop closure detection, lacking specific suppression mechanisms for dynamic scenes. False features generated by dynamic targets are easily incorporated into the matching and optimization process, leading to the continuous accumulation of pose errors and the appearance of ghosting and blurring in the map. Finally, while some works attempt to reduce noise through image preprocessing or learning models, these often only apply to a single processing step, making it difficult to form a complete interference suppression chain. Furthermore, the high model complexity makes it difficult to maintain real-time performance.

[0004] This invention addresses the high-precision autonomous navigation requirements of AUVs in complex and dynamic underwater environments by proposing a multi-layered dynamic interference suppression forward-looking sonar SLAM method. Building upon the traditional framework, this method constructs a multi-level interference suppression mechanism encompassing image enhancement, dynamic region identification and filtering, and matching optimization constraints. This aims to effectively mitigate the impact of dynamic targets such as ships, schools of fish, bubbles, and noise, thereby improving the system's positioning accuracy and robustness in dynamic environments while ensuring real-time performance. Summary of the Invention

[0005] This invention addresses the problem of feature mismatch and trajectory drift caused by interference from ships, fish schools, and bubbles in complex and dynamic waters such as reservoirs and waterways, and proposes a forward-looking sonar SLAM method with multi-layer dynamic interference suppression.

[0006] The technical solution adopted by the present invention to solve the above problems is as follows: The steps of the present invention include: Step 1: Acquire sonar intensity images in polar coordinates using a multibeam forward-looking sonar at a fixed frequency, while simultaneously acquiring motion information such as forward, lateral, and angular velocities output by the DVL and IMU. After intensity normalization preprocessing of the sonar images, an ordered constant false alarm rate (CFAR) detection algorithm is used to extract significant echo cells from the images. The detection results are then transformed from polar coordinates to Cartesian coordinates to form a sparse two-dimensional feature point cloud, providing input features for subsequent dynamic interference suppression and SLAM front-end matching. Step 2: Perform multi-layer dynamic interference suppression within the sonar image domain; First, a lightweight YOLOv8n network trained on a sonar dataset is used to detect typical dynamic targets such as ships, schools of fish, and bubbles. A segmentation mask is generated for each target, and the mask range is expanded by morphological dilation. Feature points within the mask area are removed during the feature extraction stage. Subsequently, for environmental noise and weak dynamic interference that are not explicitly identified by the network, Gaussian mixture modeling is performed on the historical intensity changes of each pixel position. Components that appear stably in the long term are regarded as background echoes, and components that appear in a short time or have abnormally high intensity are regarded as suspicious interference echoes. Pixels and nearby feature points in the corresponding area are marked as interference and removed from the feature set, thus obtaining a candidate feature set mainly composed of static structures. Step 3: Obtain the pose prior of the current keyframe based on the odometry information provided by DVL and IMU, and input the feature point cloud after multi-layer dynamic suppression into the ICP-based scanning matching module. Motion consistency constraints are introduced during the matching process: by dividing the space into grids, the motion patterns of the matching pairs in the local area are statistically analyzed, and matching pairs that do not conform to the overall motion trend are identified and eliminated to suppress the influence of residual dynamic targets and mismatches. Finally, robust relative pose estimation between adjacent keyframes is obtained and a sequential constraint relationship is established. Step 4: Construct a factor graph by combining the trajectory constraints obtained based on velocity and attitude information with the relative pose constraints obtained by ICP scan matching. Using keyframe pose as nodes, perform incremental optimization using graph optimization tools such as GTSAM to obtain a globally consistent AUV trajectory estimate. By accumulating and fusing the dynamically suppressed sonar point cloud using the optimized pose, an underwater map that can highlight the static environmental structure is generated, enabling high-precision trajectory estimation and environmental modeling of autonomous underwater vehicles in complex dynamic waters.

[0007] Furthermore, step 1 specifically includes: The system first acquires sonar intensity images in polar coordinates using a multibeam forward-looking sonar installed on the autonomous underwater vehicle at a fixed sampling frequency, while simultaneously acquiring motion information such as forward velocity, lateral velocity, and angular velocity output from the DVL and IMU. To reduce image brightness inconsistencies caused by factors such as range switching and gain adjustment, intensity normalization and basic filtering preprocessing are performed on the original sonar image to suppress isolated noise and preserve the environmental boundary structure. Subsequently, the system employs the SO-CFAR detection algorithm in the sonar image domain, adaptively setting the detection threshold by utilizing the statistical characteristics of the reference cells, thereby extracting significant echo cells and forming a sparse echo response set. Finally, based on the measured distance With azimuth The analytical relationship transforms the detected co-wave units from polar coordinates to Cartesian coordinates, resulting in a two-dimensional feature point cloud in meters; this provides a unified input data basis for subsequent dynamic interference suppression and SLAM front-end matching.

[0008] Furthermore, step 2 specifically includes: the system performs multi-layer dynamic interference suppression in the sonar image domain, retaining only the static environmental features used for pose estimation; First, the sonar intensity image that has undergone intensity normalization in step 1 is input into a lightweight YOLOv8n network pre-trained on the sonar dataset to detect typical dynamic targets in the image and obtain deep learning detection results, which include the detected target category and confidence information. The system generates an initial binary mask based on each detection box and performs morphological dilation using a 5-pixel structuring element to obtain a dilated mask. In the feature point filtering stage, all feature points extracted in step 1 are denoted as... It will fall into the expansion mask. The feature points within are denoted as Furthermore, by initially eliminating explicit dynamic targets, the system avoids the participation of obviously moving targets in subsequent matching and mapping. Based on this, for weak dynamic interference and environmental noise not explicitly recognized by the network, the system introduces a Gaussian mixture model for background modeling at each pixel location. For pixels at time... Intensity observation Model it as A weighted mixture of Gaussian distributions; If the current observation does not match any of the background distributions, it is marked as foreground interference, forming a GMM foreground mask, and the corresponding set of feature points is denoted as . Finally, the system combines the two types of masks for the feature set to obtain candidate features with static structure as the main component. After processing in step 2, the sonar feature point cloud is composed of a static environment structure, providing a clean and reliable input for motion consistency matching based on odometry priors and ICP in the subsequent step 3.

[0009] Furthermore, step 3 specifically includes: the system performs scan matching under motion consistency constraints based on the static feature point cloud output in step 2, combined with odometry information from DVL and IMU, to obtain robust relative poses between adjacent keyframes. First, utilizing the time interval Forward and lateral velocities measured by internal DVL and the angular velocity measured by the IMU The current keyframe is obtained by calculating through the motion model. Compared to the previous keyframe Pose prior estimation ; Subsequently, the two frames of static feature point clouds after dynamic suppression were... and Input to the ICP-based scan matching module; A gridding strategy is employed to divide the image plane into non-overlapping grid cells, supporting statistical matching at the cell level. For values ​​falling within a cell... Matching point set in The calculation of its motion support is simplified to examining the units associated with it. The final number of matches in the range, if the match Support meets , where the threshold If the match is correct, it is considered a correct match and retained; otherwise, it is discarded as a false match or dynamic residue. After motion consistency constraint screening, the optimal rigid body transformation matrix is ​​recalculated by minimizing the point-to-point distance using the retained correct matching pairs; thus, the optimized relative pose transformation is obtained. And establish pose constraints between adjacent keyframes.

[0010] Furthermore, in step 4, the system will perform global optimization through factor graph based on the constraints of mileage and relative pose obtained in step 3, and generate a consistent underwater environment map.

[0011] The beneficial effects of this invention are: 1. This invention integrates a three-layer dynamic interference suppression mechanism into the forward-looking sonar SLAM front-end. By comprehensively utilizing lightweight deep learning detection, adaptive background modeling, and geometric consistency constraints, it can effectively distinguish and filter out dynamic targets in underwater environments with strong dynamic interference, including ships, schools of fish, and bubble flows. This method overcomes the limitation of traditional methods in the incomplete removal of dynamic features in low-texture, high-noise sonar scenes, significantly improving the purity and stability of static feature extraction, and providing a reliable data foundation for subsequent pose estimation.

[0012] 2. In the feature matching and state estimation stages, this invention embeds a motion consistency criterion into the scanning matching process. Through local motion pattern analysis, it effectively identifies and eliminates mismatches and residual dynamic points, significantly enhancing the robustness of relative pose estimation. Furthermore, the system tightly integrates odometry and multimodal observation constraints through a factor graph optimization framework, achieving global trajectory and map consistency optimization, effectively suppressing accumulated errors, and improving the system's positioning and mapping accuracy in long-endurance missions.

[0013] 3. This invention achieves efficient collaboration between dynamic perception and SLAM at the system implementation level. Through lightweight model design and accelerated algorithm design, the complete dynamic segmentation and pose estimation process can meet the real-time processing requirements of sonar frame rates on conventional computing units. This system can generate continuous environmental maps dominated by static structures in real-time in real dynamic waters without relying on dedicated hardware acceleration, providing a practical solution for long-term autonomous navigation of AUVs in complex dynamic scenarios.

[0014] 4. This invention enables high-precision positioning and robust mapping of autonomous underwater vehicles in complex scenarios involving moving targets such as ships, schools of fish, and bubble flows. This method effectively overcomes the influence of dynamic targets on the SLAM system through multi-layer dynamic interference suppression, motion consistency constraint matching, and global optimization of factor graphs, achieving the generation of accurate trajectories and environmental maps dominated by static structures in dynamic waters. Attached Figure Description

[0015] Figure 1 This is a schematic diagram of the SLAM algorithm framework of the present invention; Figure 2 This is a schematic diagram of the deep learning results; Figure 3 This is a schematic diagram of the detected target information; Figure 4 This is a schematic diagram of the segmentation mask result; Figure 5 This is a schematic diagram of the Gaussian mixture model for segmenting environmental noise. Figure 6 This is a schematic diagram of the motion core; Figure 7 This is a schematic diagram of correctly matched pairs in adjacent spaces. Detailed Implementation

[0016] Specific implementation method one: as follows Figures 1 to 7 As shown, a forward-looking sonar SLAM method for multi-layer dynamic interference suppression includes the following specific steps: Step 1: Acquire sonar intensity images in polar coordinates using a multibeam forward-looking sonar at a fixed frequency, while simultaneously acquiring motion information such as forward, lateral, and angular velocities output by the DVL and IMU. After preprocessing the sonar images, such as intensity normalization, the ordered constant false alarm rate (SO-CFAR) detection algorithm is used to extract significant echo units from the images. The detection results are then transformed from polar coordinates to Cartesian coordinates to form a sparse two-dimensional feature point cloud, providing input features for subsequent dynamic interference suppression and SLAM front-end matching. Step 2: Perform multi-layer dynamic interference suppression within the sonar image domain; First, a lightweight YOLOv8n network trained on a sonar dataset is used to detect typical dynamic targets such as ships, schools of fish, and bubbles. A segmentation mask is generated for each target, and the mask range is expanded by morphological dilation. Feature points within the mask area are removed during the feature extraction stage. Subsequently, for environmental noise and weak dynamic interference that are not explicitly identified by the network, Gaussian mixture modeling is performed on the historical intensity changes of each pixel position. Components that appear stably in the long term are regarded as background echoes, and components that appear in a short time or have abnormally high intensity are regarded as suspicious interference echoes. Pixels and nearby feature points in the corresponding area are marked as interference and removed from the feature set, thus obtaining a candidate feature set mainly composed of static structures. Step 3: Obtain the pose prior of the current keyframe based on the odometry information provided by DVL and IMU, and input the feature point cloud after multi-layer dynamic suppression into the ICP-based scanning matching module; introduce motion consistency constraints during the matching process: divide the space by gridding, statistically analyze the motion patterns of matching pairs in local areas, identify and eliminate matching pairs that do not conform to the overall motion trend, suppress the influence of residual dynamic targets and mismatches, and finally obtain robust relative pose estimates between adjacent keyframes and establish order constraint relationships; Step 4: The trajectory constraints obtained based on velocity and attitude information and the relative pose constraints obtained by ICP scanning are jointly constructed into a factor graph. Using the keyframe pose as a node, incremental optimization is performed using graph optimization tools such as GTSAM to obtain a globally consistent AUV trajectory estimate. The optimized pose is used to accumulate and fuse the sonar point cloud after dynamic suppression to generate an underwater map that can highlight the static environmental structure, thereby realizing high-precision trajectory estimation and environmental modeling of autonomous underwater vehicles in complex dynamic waters.

[0017] In some embodiments, in step 1, the system first acquires a sonar intensity image in polar coordinate format using a multibeam forward-looking sonar mounted on the autonomous underwater vehicle at a fixed sampling frequency, while simultaneously acquiring motion information such as forward velocity, lateral velocity, and angular velocity output from the DVL and IMU. To reduce image brightness inconsistencies caused by factors such as range switching and gain adjustment, the original sonar image undergoes intensity normalization and basic filtering preprocessing to suppress isolated noise points and preserve environmental boundary structures. Subsequently, the system employs the SO-CFAR detection algorithm in the sonar image domain, adaptively setting the detection threshold using the statistical characteristics of reference cells to extract significant echo cells and form a sparse echo response set. Finally, based on the measured distance... With azimuth The analytical relationship is used to transform the detected co-wave units from polar coordinates to Cartesian coordinates, resulting in a two-dimensional feature point cloud in meters. This provides a unified input data foundation for subsequent dynamic interference suppression and SLAM front-end matching.

[0018] In some embodiments, in step 2, the system performs multi-layer dynamic interference suppression in the sonar image domain, retaining only the static environmental features used for pose estimation. First, the sonar intensity image that has undergone intensity normalization processing in step 1 is input into a lightweight YOLOv8n network pre-trained on a sonar dataset to detect typical dynamic targets in the image, such as boats, schools of fish, and bubble flows, and the results are as follows: Figure 2 The deep learning detection results shown include Figure 3 The system displays the target category and confidence level information. An initial binary mask is generated for each detection box. And uses 5-pixel structural elements Morphological dilation is performed to obtain a dilated mask:

[0019] in This represents the morphological dilation operator, and its dilation effect can be found in [reference needed]. Figure 4 The segmentation mask result is shown. In the feature point filtering stage, all feature points extracted in step one are denoted as... It will fall into the expansion mask. The feature points within are denoted as And through:

[0020] The system first removes explicit dynamic targets to prevent obvious moving targets from participating in subsequent matching and mapping. Building on this, for weak dynamic interference and environmental noise not explicitly recognized by the network, the system introduces a Gaussian Mixture Model (GMM) at each pixel location for background modeling. The framework flow for this background modeling and foreground segmentation is as follows: Figure 5 As shown. For a pixel at time... Intensity observation Model it as A weighted mixture of Gaussian distributions:

[0021] in The probability density function is Gaussian.

[0022] To balance computational efficiency and modeling accuracy, we take A diagonal covariance matrix is ​​used. Model parameters are updated online.

[0023]

[0024]

[0025] in For learning rate, , As an indicator variable. Background determination is made before selection. The most likely Gaussian distribution implementation:

[0026] Where the threshold Distribution according to Sort in descending order. If the current observation does not match any background distribution, it is marked as foreground interference, forming a GMM foreground mask. The corresponding set of feature points is denoted as Finally, the system combines the two types of masks for the feature set to obtain candidate features that are predominantly static in structure:

[0027] After processing in step 2, the sonar feature point cloud is mainly composed of static environmental structures, providing a clean and reliable input for motion consistency matching based on odometry priors and ICP in the subsequent step 3.

[0028] In some embodiments, in step 3, the system is based on the static feature point cloud output in step 2. By combining odometry information from DVL and IMU, scan matching under motion consistency constraints is performed to obtain robust relative poses between adjacent keyframes.

[0029] First, utilizing the time interval Forward and lateral velocities measured by internal DVL and the angular velocity measured by the IMU The current keyframe is obtained by calculating through the motion model. Compared to the previous keyframe Pose prior estimation This prior serves as the initial transformation for ICP, which accelerates convergence and improves robustness in feature-sparse scenarios. Subsequently, the two frames of static feature point clouds after dynamic suppression were... and The input is an ICP-based scanning matching module. To suppress any dynamic points or mismatched points that may remain from step 2, motion consistency constraints are integrated into the matching process. This applies to the initial set of matching pairs obtained through nearest neighbor search. ,in The reliability of the matching is evaluated by analyzing the motion patterns within its local spatial neighborhood; the matching is defined. In point cloud The spatial neighborhood in is:

[0030] in Let be the neighborhood radius. Then, calculate the motion support of this match. That is, its neighborhood Matching points in the point cloud The number of items that still maintain spatial proximity:

[0031] in A threshold is set for judging motion consistency. Correctly matched neighboring points tend to exhibit consistent motion, therefore... The rate is relatively high; however, the movement of mismatched or dynamic feature points is random. Low. The distribution of matching support can be modeled as follows:

[0032] in It is a binomial distribution. The expected probability of a correct match. .

[0033] To accelerate the consistency check, a gridding strategy is employed. The image plane is divided into non-overlapping grid cells, and statistical matching support is achieved at the cell level. This concept of local motion pattern analysis can be implemented through... Figure 6 The diagram shown illustrates the motion kernel. For elements falling within the unit... Matching point set in The calculation of its motion support is simplified to examining the units associated with it. The final number of matches in the range, if the match Support meets , where the threshold If the match is correct, it is considered a correct match and retained; otherwise, it is discarded as a false match or dynamic residue.

[0034] After filtering by motion consistency constraints, the optimal rigid body transformation matrix is ​​recalculated by minimizing the point-to-point distance using the retained correct matching pairs. :

[0035] in This is the set of matches that passed the consistency check. From this, the optimized relative pose transformation is obtained. This step establishes pose constraints between adjacent keyframes. This effectively ensures that even when dynamic interference is not completely filtered out, pose estimation remains dominated by the static environment structure, providing high-precision local constraints for subsequent global optimization. These correctly matched pairs, maintaining motion consistency in adjacent spaces, have geometric relationships as follows: Figure 7 As shown.

[0036] In some embodiments, in step 4, the system performs global optimization using a factor graph based on the constraints of the odometry and the relative pose obtained in step 3, and generates a consistent underwater environment map. This is done using the pose of each keyframe. As variable nodes, two types of constraint factors are introduced. One is the trajectory estimation factor, which is the relative motion observation between adjacent keyframes provided by DVL / IMU. The construction is defined as follows: ,in and These represent pose composition and inverse operation, respectively. The second is the scan matching factor, based on the inter-frame ICP registration results obtained in step three. Its error function is The system integrates the above constraints through a factor graph model and uses the GTSAM library to perform incremental smoothing and graphing (iSAM2) optimization to solve for maximizing the posterior probability estimate:

[0037] in This is the set of poses for all keyframes. After optimization, the optimized poses are used... Coordinate transformation and fusion are performed on the feature point clouds of each frame after dynamic suppression to construct a globally consistent static environment map:

[0038] in and These are the optimized rotation matrix and translation vector, respectively. This step ultimately outputs a high-precision AUV trajectory. underwater maps dominated by static structures It supports reliable navigation and scene awareness in dynamic waters.

[0039] Example A forward-looking sonar SLAM method for multi-layer dynamic interference suppression includes the following steps: Step 1: The system acquires the original intensity image through multibeam forward-looking sonar and simultaneously collects motion information from DVL and IMU. After intensity normalization preprocessing of the sonar image, the SO-CFAR algorithm is used to detect significant echo units and transform them from polar coordinates to Cartesian coordinates to form an initial two-dimensional feature point cloud, which provides input for subsequent processing. Step 2: The system performs multi-layer dynamic interference suppression in the sonar image domain; First, a lightweight YOLOv8n network pre-trained on a sonar dataset is used to detect typical dynamic targets such as ships, schools of fish, and bubble flow. A segmentation mask is generated and morphologically dilated to remove feature points within the mask area. Subsequently, for unidentified weak dynamic interference and environmental noise, a Gaussian mixture model is used to model the background of each pixel. Foreground interference is separated by online updating and threshold judgment, and corresponding feature points are further eliminated, finally obtaining a candidate feature set mainly composed of static structures. Step 3: Based on the odometry priors provided by DVL and IMU, the system performs iterative nearest point matching under motion consistency constraints on the static feature point clouds of adjacent frames after dynamic suppression. By analyzing the motion patterns of matching pairs in the local spatial neighborhood, calculating their motion support, and identifying and eliminating matching pairs with insufficient support, the influence of residual dynamic targets and mismatches is suppressed, thereby obtaining robust relative pose estimation. Step 4: The system constructs a factor graph model to optimize the global trajectory using the aforementioned relative pose constraints and trajectory estimation constraints. Using the optimized pose, the static feature point clouds of each frame are transformed and fused into the global coordinate system to generate a consistent underwater environment map. Finally, a high-precision AUV trajectory and an environment map highlighting static structures are output.

[0040] The above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention in any way. Although the present invention has been disclosed above with reference to preferred embodiments, it is not intended to limit the present invention. Any person skilled in the art can make some modifications or alterations to the above-disclosed technical content to create equivalent embodiments without departing from the scope of the present invention. Any simple modifications, equivalent substitutions, and improvements made to the above embodiments without departing from the scope of the present invention, based on the technical essence of the present invention and within the spirit and principles of the present invention, shall still fall within the protection scope of the present invention.

Claims

1. A forward-looking sonar SLAM method for multi-layer dynamic interference suppression, characterized in that, The specific steps include: Step 1: Acquire sonar intensity images in polar coordinates using a multibeam forward-looking sonar at a fixed frequency, while simultaneously acquiring motion information such as forward, lateral, and angular velocities output by the DVL and IMU. After intensity normalization preprocessing of the sonar images, an ordered constant false alarm rate (CFAR) detection algorithm is used to extract significant echo cells from the images. The detection results are then transformed from polar coordinates to Cartesian coordinates to form a sparse two-dimensional feature point cloud, providing input features for subsequent dynamic interference suppression and SLAM front-end matching. Step 2: Perform multi-layer dynamic interference suppression within the sonar image domain; First, a lightweight YOLOv8n network trained on a sonar dataset is used to detect typical dynamic targets such as ships, schools of fish, and bubbles. A segmentation mask is generated for each target, and the mask range is expanded by morphological dilation. Feature points within the mask area are removed during the feature extraction stage. Subsequently, for environmental noise and weak dynamic interference that are not explicitly identified by the network, Gaussian mixture modeling is performed on the historical intensity changes of each pixel position. Components that appear stably in the long term are regarded as background echoes, and components that appear in a short time or have abnormally high intensity are regarded as suspicious interference echoes. Pixels and nearby feature points in the corresponding area are marked as interference and removed from the feature set, thus obtaining a candidate feature set mainly composed of static structures. Step 3: Obtain the pose prior of the current keyframe based on the odometry information provided by DVL and IMU, and input the feature point cloud after multi-layer dynamic suppression into the ICP-based scanning matching module. Motion consistency constraints are introduced during the matching process: by dividing the space into grids, the motion patterns of matching pairs in local areas are statistically analyzed, and matching pairs that do not conform to the overall motion trend are identified and eliminated to suppress the influence of residual dynamic targets and mismatches. Finally, robust relative pose estimation between adjacent keyframes is obtained and a sequence constraint relationship is established. Step 4: Construct a factor graph by combining the trajectory constraints obtained based on velocity and attitude information with the relative pose constraints obtained by ICP scan matching. Using keyframe pose as nodes, perform incremental optimization using graph optimization tools such as GTSAM to obtain a globally consistent AUV trajectory estimate. By accumulating and fusing the dynamically suppressed sonar point cloud using the optimized pose, an underwater map that can highlight the static environmental structure is generated, enabling high-precision trajectory estimation and environmental modeling of autonomous underwater vehicles in complex dynamic waters.

2. The forward-looking sonar SLAM method for multi-layer dynamic interference suppression according to claim 1, characterized in that, Step 1 specifically includes: The system first acquires sonar intensity images in polar coordinates using a multibeam forward-looking sonar installed on the autonomous underwater vehicle at a fixed sampling frequency, while simultaneously acquiring motion information such as forward velocity, lateral velocity, and angular velocity output from the DVL and IMU. To reduce image brightness inconsistencies caused by factors such as range switching and gain adjustment, intensity normalization and basic filtering preprocessing are performed on the original sonar image to suppress isolated noise and preserve the environmental boundary structure. Subsequently, the system employs the SO-CFAR detection algorithm in the sonar image domain, adaptively setting the detection threshold by utilizing the statistical characteristics of the reference cells, thereby extracting significant echo cells and forming a sparse echo response set. Finally, based on the measured distance With azimuth The analytical relationship transforms the detected co-wave units from polar coordinates to Cartesian coordinates, resulting in a two-dimensional feature point cloud in meters; this provides a unified input data basis for subsequent dynamic interference suppression and SLAM front-end matching.

3. The forward-looking sonar SLAM method for multi-layer dynamic interference suppression according to claim 1, characterized in that, Step 2 specifically includes: the system performs multi-layer dynamic interference suppression in the sonar image domain, retaining only the static environmental features used for pose estimation; First, the sonar intensity image that has undergone intensity normalization in step 1 is input into a lightweight YOLOv8n network pre-trained on the sonar dataset to detect typical dynamic targets in the image and obtain deep learning detection results, which include the detected target category and confidence information. The system generates an initial binary mask based on each detection box and performs morphological dilation using a 5-pixel structuring element to obtain a dilated mask. In the feature point filtering stage, all feature points extracted in step 1 are denoted as... It will fall into the expansion mask. The feature points within are denoted as Furthermore, by initially eliminating explicit dynamic targets, the system avoids obvious moving targets from participating in subsequent matching and mapping. Based on this, for weak dynamic interference and environmental noise not explicitly recognized by the network, the system introduces a Gaussian mixture model for background modeling at each pixel location. For pixels at time... Intensity observation Model it as A weighted mixture of Gaussian distributions; If the current observation does not match any of the background distributions, it is marked as foreground interference, forming a GMM foreground mask, and the corresponding set of feature points is denoted as . Finally, the system combines the two types of masks for the feature set to obtain candidate features with static structure as the main component. After processing in step 2, the sonar feature point cloud is composed of a static environment structure, providing a clean and reliable input for motion consistency matching based on odometry priors and ICP in the subsequent step 3.

4. The forward-looking sonar SLAM method for multi-layer dynamic interference suppression according to claim 1, characterized in that, Step 3 specifically includes: Based on the static feature point cloud output in Step 2, the system combines odometry information from DVL and IMU to perform scan matching under motion consistency constraints in order to obtain robust relative poses between adjacent keyframes. First, utilizing the time interval Forward and lateral velocities measured by internal DVL and the angular velocity measured by the IMU The current keyframe is obtained by calculating through the motion model. Compared to the previous keyframe Pose prior estimation ; Subsequently, the two frames of static feature point clouds after dynamic suppression were... and Input to the ICP-based scan matching module; A gridding strategy is employed to divide the image plane into non-overlapping grid cells, supporting statistical matching at the cell level. For values ​​falling within a cell... Matching point set in The calculation of its motion support is simplified to examining the units associated with it. The final number of matches in the range, if the match Support meets , where the threshold If the match is correct, it is considered a correct match and retained; otherwise, it is discarded as a false match or dynamic residue. After motion consistency constraint screening, the optimal rigid body transformation matrix is ​​recalculated by minimizing the point-to-point distance using the retained correct matching pairs; thus, the optimized relative pose transformation is obtained. And establish pose constraints between adjacent keyframes.

5. The forward-looking sonar SLAM method for multi-layer dynamic interference suppression according to claim 1, characterized in that, In step 4, the system will perform global optimization based on the constraints of the mileage and the relative pose obtained in step 3 through factor graph, and generate a consistent underwater environment map.

Images