A forward-looking sonar SLAM method and system based on deep Fourier registration

Abstract

This invention relates to a forward-looking sonar SLAM method and system based on deep Fourier registration. The method includes the following steps: S1: acquiring two sonar images; S2: performing feature extraction, Fourier transform, and Radon transform on the two sonar images to calculate their angular correlation curves; S3: performing rotation correction on the two sonar images and performing feature extraction and phase correlation calculation to obtain a phase correlation heatmap; S4: calculating the rotation variance and displacement variance of the two sonar images; S5: obtaining the covariance matrix by weighted correction of the variance based on the SSIM correlation of the two images; S6: constructing a noise model and explicitly specifying the noise model parameters for each observation factor. This invention introduces Fourier registration technology and deep learning technology into the front end of the SLAM system, enabling the SLAM system to effectively eliminate interference caused by complex seabed environments and better adapt to complex underwater environments.

Description

Technical Field

[0001] This invention belongs to the field of autonomous navigation and perception technology for underwater robots, and particularly relates to a forward-looking sonar SLAM method and system based on depth Fourier registration. Background Technology

[0002] With the rapid development of fields such as marine resource exploration, seabed mapping, pipeline inspection, and underwater archaeology, the application value of Autonomous Underwater Vehicles (AUVs) has significantly increased. AUVs can autonomously perform tasks in high-risk or sparsely populated environments such as the deep sea. The key to their long-term, wide-range, and high-precision autonomous operation lies in their accurate positioning and environmental perception capabilities. Because seawater blocks GPS signals in the underwater environment, optical sensors are limited by water turbidity and lighting conditions, and electromagnetic waves are difficult to use due to severe attenuation. Therefore, acoustic sensing technology, especially sonar, has become the main basis for AUVs to achieve underwater navigation, positioning, and environmental perception.

[0003] Currently, underwater positioning for AUVs generally employs a combination of inertial navigation systems (INS) and Doppler logs (DVL), with intermittent corrections made using external acoustic positioning systems such as ultra-short baseline (USBL). While this method can achieve underwater navigation, positioning, and environmental awareness to a certain extent, it still has inherent limitations. For example, INS positioning errors accumulate over time; DVL reliability decreases in complex seabed topography or near-bottom operations; and USBL systems rely on costly and pre-deployed acoustic beacon base stations, resulting in insufficient deployment flexibility and limited application in unknown or vast sea areas.

[0004] To overcome the aforementioned limitations and improve the robustness and long-endurance accuracy of autonomous navigation for AUVs in complex, unknown, or vast sea areas, Simultaneous Localization and Mapping (SLAM) technology has become an important research direction. In existing technologies, SLAM systems can effectively utilize environmental observation information to correct and suppress the accumulated errors caused by long-term operation of dead reckoning systems such as INS, achieving autonomous localization and map building without relying on external infrastructure. However, in practical applications, especially in the stability of feature extraction, the accuracy of data association, computational efficiency, and the overall robustness of the system, significant improvements are still needed. Summary of the Invention

[0005] The purpose of this invention is to solve one of the aforementioned technical problems by providing a forward-looking sonar SLAM method and system based on deep Fourier registration. This method uses a deep learning network model based on the TransUNet architecture to extract features from sonar images and extracts amplitude spectrum information through Fourier transform for image calibration. This significantly improves the accuracy and robustness of traditional Fourier registration techniques, enabling them to play a good role in forward-looking sonar image registration tasks with high signal-to-noise ratios. As a result, the SLAM system can better adapt to complex underwater environments.

[0006] To achieve the above objectives, the technical solution adopted by the present invention is as follows:

[0007] A forward-looking sonar SLAM method based on deep Fourier registration includes the following steps:

[0008] S1: Acquire two sonar images using forward-looking sonar, namely image [image name missing]. and images ;

[0009] S2: Extract features from the two sonar images to obtain two rotational registration feature maps; perform Fourier transform on the two rotational registration feature maps to extract their amplitude spectrum information, and obtain two amplitude spectra; perform Radon transform on the two amplitude spectra to obtain two sets of angle slices, and calculate the cross-correlation similarity of the two sets of angle slices to obtain the angle correlation curve.

[0010] S3: Rotation correction is performed on two sonar images based on the angle correlation curve. Feature extraction is performed on the corrected images to obtain two displacement registration feature maps. Phase correlation calculation is performed on the two displacement registration feature maps to obtain a phase correlation heatmap.

[0011] S4: Extract the relative rotation angle of the two sonar images from the angle correlation curve and calculate the corresponding rotation variance; extract the relative displacement of the two sonar images from the phase correlation heatmap and calculate the corresponding displacement variance.

[0012] S5: Image based on relative rotation angle and relative displacement Perform rotation and translation corrections, and calculate the image. and the image after rotation and translation correction The SSIM correlation is used to weight and correct the rotation variance and displacement variance to obtain the corrected variance and the corresponding covariance matrix.

[0013] S6: Based on the dynamic construction and / or configuration of the noise model acting on the observation factors based on the covariance matrix; based on this noise model, the noise model parameters are explicitly specified for each observation factor within the GTSAM framework of the SLAM system backend.

[0014] In some embodiments of the present invention, step S2 further includes the following steps:

[0015] Construct a deep learning network model based on the TransUNet architecture;

[0016] The deep learning network model is improved to obtain the rotation registration network model, and the expression of the rotation registration network model is:

[0017] ;

[0018] in, Indicates the first Image number 1 in encoder Intermediate results of the layer's output, Indicates the first The first convolutional kernel in the encoder of the layer, Indicates the first The second convolutional kernel in the encoder of the layer, Indicates the first The third convolutional kernel in the encoder of the layer, This means that the GeLU function is used to activate the system before Batch Norm is performed. This indicates the convolutional downsampling module. This indicates the convolutional upsampling module. This indicates that the input feature maps are stacked and then processed by the efficient self-attention model EMA. This represents the output of the feature extraction part of the rotation registration network. Indicates the first The first convolutional kernel in the decoder of the layer, Indicates the first The second convolutional kernel in the decoder of the layer, This indicates that the i-th image is in the decoder at the th... Feature maps output by the layer;

[0019] The rotation registration network model was used to extract features from the two sonar images to obtain two rotation registration feature maps.

[0020] In some embodiments of the present invention, step S2, the method for improving the deep learning network model specifically includes the following steps:

[0021] The TransUNet architecture is improved into a Siamese-TransUNet structure with a shared codec;

[0022] An efficient multi-scale attention mechanism, EMA, is embedded at the jump connection to interact with the features of two images.

[0023] In some embodiments of the present invention, the formula for calculating the Radon transform in step S2 is as follows:

[0024] ;

[0025] in, The Fourier transform result of the sonar image is a two-dimensional spectrum containing phase and amplitude information. This is the line integral distance. For rotation angle, For impulse response, u and v All are frequencies.

[0026] In some embodiments of the present invention, in step S2, the formula for calculating the cross-correlation similarity between the two sets of angle slices is as follows:

[0027] ;

[0028] Where X and Y are vectors involved in the relevant operations. For expectation operator, Let X be the variance of the elements of vector X. Let Y be the variance of the elements of vector Y. Let X be the i-th element. Let be the i-th element of vector Y, and n be the dimension of the vector.

[0029] In some embodiments of the present invention, step S3 further includes the following steps:

[0030] Construct a deep learning network model based on the TransUNet architecture;

[0031] The deep learning network model is improved to obtain the displacement registration network model, and the expression of the displacement registration network model is:

[0032]

[0033] in, Indicates the first Image number 1 in encoder Intermediate results from the layer output. Indicates the first The first convolutional kernel in the encoder of the layer, Indicates the first The second convolutional kernel in the encoder of the layer, Indicates the first The third convolutional kernel in the encoder of the layer, This means that the GeLU function is used to activate the system before Batch Norm is performed. This indicates activation via the GeLU function. This indicates the convolutional downsampling module. This indicates the convolutional upsampling module. This indicates that the input feature maps are stacked and then processed by the efficient self-attention model EMA. This indicates that the Dysample upsampling module can be learned. This represents the output of the feature extraction network for displacement registration. Indicates the first The first convolutional kernel in the decoder of the layer, Indicates the first The second convolutional kernel in the decoder of the layer, This indicates that the i-th image is in the decoder at the th... Feature maps output by the layer;

[0034] The displacement registration network model is used to extract features from the corrected image to obtain two displacement registration feature maps.

[0035] In some embodiments of the present invention, step S3, the method for improving the deep learning network model specifically includes the following steps:

[0036] The TransUNet architecture is improved into a Siamese-TransUNet structure with a shared codec;

[0037] An efficient multi-scale attention mechanism, EMA, is embedded at the skip connections of the Siamese-TransUNet structure to interact with the features of two images, and the difference results of the two feature maps are added to the input of the EMA module.

[0038] A lightweight, learnable upsampling Dysample module is introduced into the Siamese-TransUNet architecture.

[0039] In some embodiments of the present invention, the calculation formula for phase correlation calculation in step S3 is as follows:

[0040] ;

[0041] in, For phase correlation heatmap, For image Two-dimensional spectrum, For image Two-dimensional spectrum, image Two-dimensional spectrum and image The relationship of the two-dimensional spectrum in the frequency domain is as follows: .

[0042] In some embodiments of the present invention, the formula for calculating SSIM correlation in step S5 is as follows:

[0043]

[0044] in, The original variance, This is the corrected variance.

[0045] Some embodiments of the present invention further provide a forward-looking sonar SLAM system based on deep Fourier registration, for implementing the above-mentioned forward-looking sonar SLAM method based on deep Fourier registration, including a front-end module and a back-end module;

[0046] The front-end module includes a rotation registration network model, a displacement registration network model, and an inter-frame motion estimation unit. The rotation registration network model is used to extract features from two sonar images to obtain two rotation registration feature maps. The displacement registration network model is used to extract features from the corrected image to obtain two displacement registration feature maps. The inter-frame motion estimation unit is used to perform calculations on the feature extraction results of the rotation registration network model and the displacement registration network model through motion calculation algorithms to output the pose transformation and covariance matrix at adjacent time points.

[0047] The backend module adopts the GTSAM framework, which includes a noise model constructed based on the covariance matrix. During the nonlinear optimization process, the GTSAM framework automatically adjusts the contribution weights of different observation factors in the objective function using the parameters provided by the noise model.

[0048] The beneficial effects of this invention are as follows:

[0049] 1. This invention simultaneously introduces Fourier registration technology and deep learning technology. It extracts amplitude spectrum information through Fourier transform, calculates angular correlation curves using Radon transform and cross-correlation similarity, and then achieves rotation correction; furthermore, it obtains a phase correlation heatmap through phase correlation calculation to achieve displacement correction. Simultaneously, it uses a deep learning network model based on an improved TransUNet architecture to extract features from sonar images, obtaining rotation registration feature maps and displacement registration feature maps. This significantly improves the accuracy and robustness of traditional Fourier registration technology, enabling it to perform well in high signal-to-noise ratio forward-looking sonar image registration tasks, thus allowing SLAM systems to better adapt to complex underwater environments.

[0050] 2. The rotation registration network model and displacement registration network model provided by this invention improve the TransUNet architecture to a Siamese-TransUNet structure with a shared encoder and decoder, and embed an efficient multi-scale attention mechanism (EMA) module at the skip connection to interact with the features of the two images, which can extract image features more effectively and improve the accuracy and efficiency of registration.

[0051] 3. This invention is based on the dynamic construction and configuration of noise models of observation factors acting on the GTSAM framework of the back-end module of the SLAM system using the covariance matrix. During its nonlinear optimization solution, the contribution weights of different observation factors in the objective function are automatically adjusted by the parameters provided by the noise model, so that the SLAM system can dynamically filter observation information, that is, actively weaken the constraint of high uncertainty observations, normally fuse low uncertainty data, effectively eliminate the interference caused by complex seabed environment, and thus significantly enhance the robustness of the system.

[0052] Other features and advantages of the invention will be set forth in the description which follows, and will be apparent in part from the description, or may be learned by practicing the invention. The objects and other advantages of the invention may be realized and obtained by means of the structures pointed out in the description, claims and drawings. Attached Figure Description

[0053] To more clearly illustrate the technical solutions in the embodiments of this application or the prior art, the specific embodiments of the present invention will be described in detail below with reference to the accompanying drawings. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0054] Figure 1 A flowchart of a forward-looking sonar SLAM method based on deep Fourier registration;

[0055] Figure 2 This is a schematic diagram of the forward-looking sonar imaging principle provided in an embodiment of the present invention;

[0056] Figure 3 A schematic diagram of the Radon transform of the amplitude spectrum provided in an embodiment of the present invention;

[0057] Figure 4 This is a schematic diagram of the Radon domain image provided in an embodiment of the present invention;

[0058] Figure 5 A schematic diagram of angle-related curves provided for embodiments of the present invention;

[0059] Figure 6 This is a schematic diagram of the rotation registration network model provided in an embodiment of the present invention;

[0060] Figure 7 Phase-correlation heatmaps provided for embodiments of the present invention;

[0061] Among them, (a) is a two-dimensional display diagram of phase correlation; (b) is a three-dimensional visualization diagram of phase correlation.

[0062] Figure 8 This is a schematic diagram of the projection result of the phase correlation thermogram in the direction of motion provided in an embodiment of the present invention;

[0063] Among them, (a) is the X-axis projection diagram; (b) is the Y-axis projection diagram;

[0064] Figure 9 This is an error-variance scatter plot provided in an embodiment of the present invention;

[0065] Figure 10 This is a schematic diagram of the factor graph connection structure provided in an embodiment of the present invention;

[0066] Figure 11 A schematic diagram of SLAM experimental results provided in an embodiment of the present invention;

[0067] Figure 12 This is a schematic diagram of the displacement registration network model provided in an embodiment of the present invention. Detailed Implementation

[0068] To make the objectives, technical solutions, and advantages of this application clearer, the application is described and illustrated below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the scope of this application. All other embodiments obtained by those skilled in the art based on the embodiments provided in this application without inventive effort are within the scope of protection of this application.

[0069] It should be noted that the terminology used herein is for the purpose of describing particular implementations only and is not intended to limit the exemplary implementations according to this application. As used herein, unless the context clearly indicates otherwise, the singular form is also intended to include the plural form. Furthermore, it should be understood that the terms “comprising” and “having”, and any variations thereof, are intended to cover non-exclusive inclusion, for example, a process, method, system, product, or apparatus that comprises a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units not explicitly listed or inherent to such process, method, product, or apparatus.

[0070] Where there is no conflict, the embodiments and features in the embodiments of the present invention can be combined with each other.

[0071] The technical solution of the present invention will be described in detail below with reference to specific embodiments and accompanying drawings.

[0072] As attached Figure 1 -Appendix Figure 12 As shown, in an illustrative embodiment of a forward-looking sonar SLAM method based on deep Fourier registration according to the present invention, the method includes the following steps.

[0073] S1: Acquire two sonar images using forward-looking sonar, namely image [image name missing]. and images .

[0074] It should be noted that forward-looking sonar emits sound beams directionally through its probes. The sound waves reflect off the target, forming echoes that are received by the probes and converted into electrical signals. The sonar imaging system processes these signals, precisely measuring the time delay of the echoes in each direction to calculate the target distance, and combining this with the direction of the received beam to determine the target's azimuth. Simultaneously, the system records echo intensity information. Finally, the forward-looking sonar maps all detected target distance, azimuth, and echo intensity data onto a two-dimensional plane with the sonar as the origin, generating an acoustic image similar to a top-down view. The imaging process is as follows: Figure 2 As shown. This image is essentially an orthogonal projection of a three-dimensional target onto a horizontal plane, losing the target's height information in the vertical direction (Z-axis). Therefore, for environmental perception and navigation based on such images, the state space is usually constrained to the position and orientation in a two-dimensional plane, i.e., SE(2). The image intuitively displays the horizontal distribution of underwater obstacles in the area directly in front of the sonar (represented by pixel coordinates) and their reflectivity (represented by pixel grayscale or color), thus outlining the environmental contours and structure.

[0075] For the forward-looking sonar used in this embodiment, its time-domain and frequency-domain images can be represented by binary functions. When the sonar's carrier frame (i.e., the underwater robot) undergoes a transformation on SE(2), it can be regarded as an image. and images Rotation and translation have occurred. The result of rotation and translation between the two images can be expressed by the following formula:

[0076] .

[0077] in, Representing an image Compared to displacement, express Compared to The rotation angle. Definition and It is the temporal representation of the image. and It is the Fourier transform result of two sonar images, that is, a two-dimensional spectrum containing both phase and amplitude information, where | | and | | represents the amplitude spectrum. Then and The relationship between their spectra can be expressed by the following formula:

[0078] .

[0079] S2: Feature extraction is performed on the two sonar images to obtain two rotational registration feature maps. Fourier transform is then performed on the two rotational registration feature maps to extract their amplitude spectrum information, resulting in two amplitude spectra. Radon transform is then performed on the two amplitude spectra to obtain two sets of angular slices. The cross-correlation similarity between the two sets of angular slices is calculated to obtain the angular correlation curve.

[0080] Since the raw images acquired by forward-looking sonar are often high signal-to-noise ratio and susceptible to interference, the peak value of the calculated angle correlation curve is not obvious, thus affecting the registration effect.

[0081] Therefore, in order to overcome the registration difficulties caused by high noise interference in the original forward-looking sonar image, in some embodiments of the present invention, step S2 further includes the following steps.

[0082] Construct a deep learning network model based on the TransUNet architecture.

[0083] The deep learning network model is improved to obtain a rotation registration network model for robust feature extraction. The structure of the rotation registration network model is shown in the attached figure. Figure 6 As shown, it combines the global context modeling capability of Transformer with the multi-scale local detail preservation feature of UNet, and improves feature quality through targeted optimization.

[0084] The expression for the rotation registration network model is:

[0085]

[0086] in, Indicates the first Image number 1 in encoder Intermediate results from the layer output. Indicates the first The first convolutional kernel in the encoder of the layer, Indicates the first The second convolutional kernel in the encoder of the layer, Indicates the first The third convolutional kernel in the encoder of the layer, This means that the GeLU function is used to activate the system before Batch Norm is performed. This indicates the convolutional downsampling module. This indicates the convolutional upsampling module. This indicates that the input feature maps are stacked and then processed by the efficient self-attention model EMA. This represents the output of the feature extraction part of the rotation registration network. Indicates the first The first convolutional kernel in the decoder of the layer, Indicates the first The second convolutional kernel in the decoder of the layer, This represents the feature map output by the i-th image at the l-th layer of the decoder.

[0087] The rotation registration network model was used to extract features from the two sonar images to obtain two rotation registration feature maps.

[0088] In some embodiments of the present invention, step S2, the method for improving the deep learning network model specifically includes the following steps:

[0089] The TransUNet architecture is improved to a Siamese-TransUNet structure with a shared codec, thereby supporting input of a pair of sonar images.

[0090] An efficient multi-scale attention mechanism (EMA) module is embedded at the skip connection to interact with the features of two images, increasing the model’s spatial correspondence capability and enhancing the cross-regional correlation of effective acoustic targets.

[0091] To extract the rotation angle from the amplitude spectrum, traditional methods typically employ the Log-Polar coordinate transformation. The expression for the Log-Polar coordinate transformation is:

[0092] .

[0093] Log-Polar coordinate transformation transforms the frequency domain image in Cartesian coordinates. Mapping to polar coordinates This allows the rotation transformation between images to be represented as a translation relationship in polar coordinates, with the rotation angle displayed as an independent variable. Subsequently, Fourier registration is applied to the transformed polar coordinate images to detect the translation amount, thus directly deriving the rotation angle between the original images.

[0094] However, the traditional Log-Polar transform applied to discrete amplitude spectra produces large interpolation errors, especially in Cartesian to logarithmic polar coordinate transformation grids, which may even require separate filters to mitigate the effects.

[0095] Therefore, in some embodiments of the present invention, in step S2, the Radon transform is used as an alternative, and the calculation formula for the Radon transform is as follows:

[0096] .

[0097] in, The Fourier transform result of the sonar image is a two-dimensional spectrum that simultaneously contains phase and amplitude information. This is the line integral distance. For rotation angle, For the impulse response, u and v both represent frequencies.

[0098] The Radon transform rotates an image by a set angle and then calculates the line integral along a fixed direction. The line integrals obtained at multiple set angles constitute the Radon domain result of the image. The Radon transform also maps rotation to translation, and the error of rotation interpolation transformation of digital images is much smaller than that of the traditional Log-Polar transform.

[0099] In this embodiment, the Radon transform of the discrete amplitude spectrum is illustrated as follows: Figure 3 As shown, after each fixed rotation, the values ​​are summed along the vertical direction to obtain the image projection data at the current angle. Multiple sets of projection data can be combined to form a composite image. Figure 4 The Radon domain image shown.

[0100] Projected data in a Radon domain image can be treated as vectors. In some embodiments of the present invention, in step S2, normalized cross-correlation (NCC) can be used to calculate the similarity between two vectors. The formula for calculating the cross-correlation similarity of two sets of angle slices is as follows:

[0101] .

[0102] Where X and Y are vectors involved in the relevant operations. For expectation operator, Let X be the variance of the elements of vector X. Let Y be the variance of the elements of vector Y. Let X be the i-th element. Let be the i-th element of vector Y, and n be the dimension of the vector.

[0103] Based on the properties of image amplitude spectra, by sequentially calculating the cross-correlation similarity between a portion of the amplitude spectrum of image I1 and the entire amplitude spectrum of image I2, an angular correlation curve representing the correlation and angular relationship can be obtained. In this embodiment, the angular correlation curve is as follows: Figure 5 As shown.

[0104] S3: Rotation correction is performed on the two sonar images based on the angle correlation curve. Feature extraction is then performed on the corrected images to obtain two displacement registration feature maps. Phase correlation calculation is then performed on the two displacement registration feature maps to obtain a phase correlation heatmap.

[0105] The extraction of phase correlation peaks can also be performed differently, thereby achieving end-to-end training of displacement registration. Therefore, in some embodiments of the present invention, step S3 further includes the following steps.

[0106] Construct a deep learning network model based on the TransUNet architecture.

[0107] The deep learning network model is improved to obtain the displacement registration network model, and the expression of the displacement registration network model is:

[0108]

[0109] in, Indicates the first Image number 1 in encoder Intermediate results from the layer output. Indicates the first The first convolutional kernel in the encoder of the layer, Indicates the first The second convolutional kernel in the encoder of the layer, Indicates the first The third convolutional kernel in the encoder of the layer, This means that the GeLU function is used to activate the system before Batch Norm is performed. This indicates activation via the GeLU function. This indicates the convolutional downsampling module. This indicates the convolutional upsampling module. This indicates that the input feature maps are stacked and then processed by the efficient self-attention model EMA. This indicates that the Dysample upsampling module can be learned. This represents the output of the feature extraction network for displacement registration. Indicates the first The first convolutional kernel in the decoder of the layer, Indicates the first The second convolutional kernel in the decoder of the layer, This represents the feature map output by the i-th image at the l-th layer of the decoder.

[0110] The structure of the displacement registration network model is as follows: Figure 11 As shown, the displacement registration network model is used to extract features from the corrected image, resulting in two displacement registration feature maps.

[0111] In some embodiments of the present invention, step S3, the method for improving the deep learning network model specifically includes the following steps.

[0112] The TransUNet architecture was improved into a Siamese-TransUNet structure with a shared codec.

[0113] An efficient multi-scale attention mechanism, EMA, is embedded at the skip connections of the Siamese-TransUNet structure to interact with the features of two images. The input of the EMA module is then supplemented with the difference between the two feature maps. Experiments show that introducing feature map differencing reduces the average error by 35% in the translation registration task.

[0114] Introducing a lightweight, learnable upsampling Dysample module into the Siamese-TransUNet architecture can effectively avoid the high memory requirements of UNet for large-size image processing, while reducing the theoretical distance resolution of the translation registration network to three times that of the original.

[0115] The maximum value of the angle correlation curve obtained in step S1 represents the rotation angle with the highest confidence. Using this angle information to correct sonar image I2 yields a pair of sonar images with only a translational relationship. Ideally, if sonar image I2 has only a translational relationship with sonar image I1, and no additional objects enter, its spatial domain expression is related to its spectrum as follows:

[0116] .

[0117] make The cross-correlation spectrum between sonar image I1 and sonar image I2. For phase correlation results, considering that new objects may enter the field of view when the forward-looking sonar frame moves, in this case, the traditional The calculation formula is:

[0118] ;

[0119] The above formula needs to take into account the normalization of the two-dimensional spectrum. Will A distinct peak is generated at the point, but due to the high signal-to-noise ratio of the forward-looking sonar image, the high-frequency noise is obvious, which will cause the phase-correlated heatmap to produce many noise peaks due to noise.

[0120] Given that forward-looking sonar images often have a large detection range (typically 30-50 meters) while the speed of an AUV is generally 1-5 knots (0.5-2.5 m / s), the displacement difference between two frames of images obtained by sonar odometer is small. Therefore, in this embodiment, it is assumed that the two frames of images have a frequency domain difference. This allows the normalization operation to be discarded, making the cross-correlation spectrum influenced by the image's amplitude spectrum, i.e., the amplitude gradually attenuates from low to high frequencies. Therefore, in some embodiments of the present invention, the calculation formula for phase correlation calculation in step S3 is:

[0121] .

[0122] in, For phase correlation heatmap, For image Two-dimensional spectrum, For image Two-dimensional spectrum, image Two-dimensional spectrum and image The relationship of the two-dimensional spectrum in the frequency domain is as follows: .

[0123] The improved phase correlation calculation formula increases the contribution of mid-to-low frequencies to the phase correlation peak and reduces the impact of high-frequency noise. Experiments verify that non-normalization can better facilitate training convergence and significantly improve the gradient vanishing problem during training.

[0124] It should be noted that in the two-dimensional Fourier transform of an image, the amplitude spectrum preserves the rotation information between images, while the phase spectrum contains the displacement information. Based on this characteristic, the method provided in this invention first uses the amplitude spectra of two consecutive images for rotation correction; after correction, the translation amount between images is accurately calculated using a phase correlation algorithm. Thus, the pose transformation of the forward-looking sonar between capturing these two images can be deduced.

[0125] In this embodiment, the calculated phase correlation heatmap is as follows: Figure 7 As shown, the deviation between its peak coordinates and the image's center represents the value calculated by the algorithm. , where s is related to the sonar detection range resolution and is a constant. Furthermore, the dispersion of the peak value in the longitudinal and lateral directions represents the value estimated by the algorithm. and The projection results of this phase-correlated heatmap in two directions are as follows: Figure 8 As shown.

[0126] S4: Extract the relative rotation angle between two sonar images from the angle correlation curve. And calculate its corresponding rotational variance. Extracting the relative displacement between two sonar images from the phase correlation heatmap. And calculate the corresponding displacement variance. and .

[0127] S5: Image based on relative rotation angle and relative displacement Perform rotation and translation corrections, and calculate the image. and the image after rotation and translation correction SSIM correlation.

[0128] By combining the similarity index of two registered sonar images, and then using a simple transformation to scale the three variances to a physically meaningful scale, the sigmoid function is used to map them to a specified interval. Finally, multiplying this by the original variance var yields the corrected variance. :

[0129] .

[0130] And, the corrected variance and the corresponding covariance matrix:

[0131] .

[0132] in, This represents the improved variance. S represents a collection of different sensors. In this embodiment, sonar and inertial navigation sensors are considered.

[0133] The corrected error-variance scatter plot is as follows: Figure 9 As shown.

[0134] In this embodiment, steps S2 to S5 together constitute the front-end module of the underwater sonar SLAM system. It has the function of synchronously outputting pose transformation and the uncertainty of the estimation. Ideally, the larger the error between the estimated pose and the true value, the larger the variance will be. Since in practice, it is directly used... , and The obtained error-variance significance level is not excellent. Therefore, this invention proposes a method to enhance the error-variance significance by calculating the similarity of the registered images. After applying the calculated rotation and translation transformations to sonar image I2, ideally, sonar image I2 and sonar image I1 will overlap. At this point, the higher the similarity between the two images, the more reliable the estimated pose transformation is, and the smaller the corresponding variance should be.

[0135] In some embodiments of the present invention, the formula for calculating SSIM correlation in step S5 is as follows:

[0136]

[0137] in, The original variance, This is the corrected variance.

[0138] S6: Based on the dynamic construction and / or configuration of the noise model acting on the observation factors using the covariance matrix. Based on this noise model, the noise model parameters are explicitly specified for each observation factor within the GTSAM framework of the SLAM system backend.

[0139] Some embodiments of the present invention further provide a forward-looking sonar SLAM system based on deep Fourier registration for implementing the above-mentioned forward-looking sonar SLAM method based on deep Fourier registration, including a front-end module and a back-end module.

[0140] The front-end module includes a rotation registration network model, a displacement registration network model, and an inter-frame motion estimation unit.

[0141] The rotation registration network model is used to extract features from two sonar images to obtain two rotation registration feature maps. The displacement registration network model is used to extract features from the corrected image to obtain two displacement registration feature maps. The inter-frame motion estimation unit is used to perform motion calculations on the feature extraction results of the rotation registration network model and the displacement registration network model to output the pose transformation and covariance matrix of adjacent time steps.

[0142] The backend module uses the GTSAM framework, which includes a noise model constructed based on the covariance matrix. During the nonlinear optimization process, the GTSAM framework automatically adjusts the contribution weights of different observation factors in the objective function using parameters provided by the noise model.

[0143] The SLAM system provided in this embodiment achieves incremental pose estimation and simultaneous environmental map construction for a vehicle (AUV) in an unknown environment by fusing multi-source sensor data streams. Its core processing flow is divided into two stages: front-end data association and back-end optimization.

[0144] The front-end processing stage is used for data association and motion estimation. In this stage, the front-end module performs low-latency processing on the raw sensor observations. The core tasks of the front-end processing stage include the following two points.

[0145] (1) Feature-level data extraction: Using the rotation registration network model and displacement registration network model in steps S3 and S4, the sonar image features associated with motion are extracted.

[0146] (2) Inter-frame motion estimation: The pose transformation between adjacent time steps is output in real time using a motion calculation algorithm, namely, an improved Fourier registration method. Rotation is defined as... The displacement is Where s is a constant related to the sonar, the incremental motion between frames can be expressed as: Expanded as follows:

[0147] .

[0148] The aforementioned inter-frame motion estimation provides constraints for the back-end optimization layer. In the SLAM system, this layer has lightweight processing characteristics and directly determines the system's real-time performance, but it will accumulate errors due to sensor noise and dynamic interference.

[0149] The backend optimization phase is used to maintain global consistency. In this phase, the backend module uses a graph optimization framework to implement global state correction. This layer of processing trades high computational cost for a leap in navigation accuracy and is the core guarantee of system robustness. The core tasks of the backend optimization phase include the following three points.

[0150] (1) Topology construction: The pose nodes and feature observation constraints output by the front end are modeled as probabilistic graphical models, such as pose graphs, factor graphs, etc.

[0151] (2) Nonlinear optimization: All historical state nodes are jointly optimized using sparse optimization algorithms (such as Levenberg-Marquardt) to reduce or eliminate the accumulated error at the front end. The cost function of the pose error between any two times i and j is:

[0152] .

[0153] in, This represents the relative pose error. Let represent the pose transformation matrices at times i and j, respectively. This represents the relative pose transformation matrix from time i to j obtained from measurements (such as sonar, inertial navigation, etc.).

[0154] In this embodiment, the backend optimization is implemented using the GTSAM (Georgia Tech Smoothing and Mapping) probabilistic graphical framework. The specific application method is as follows.

[0155] 1) State and Constraint Modeling: The system state variables to be estimated (such as vehicle pose and environmental feature point positions) are defined as variable nodes in the GTSAM factor graph. At the same time, the observation data of various sensors (including IMU and sonar odometry outputs) are transformed into corresponding constraint factors and associated with the corresponding state variable nodes.

[0156] 2) Embedded Uncertainty-Driven Noise Model: In this embodiment, the uncertainty information of the observations estimated by the front-end processing stage (specifically manifested as the covariance matrix) is used to dynamically construct or configure a noise model acting on the observation factors. Within the GTSAM framework, this invention explicitly specifies the noise model parameters for each observation factor, thereby accurately quantifying the confidence level of different sensor measurements and their impact weight on the overall optimization.

[0157] 3) Optimization and Uncertainty Suppression: During the nonlinear optimization process performed by GTSAM, a configured noise model reflecting real-time uncertainty automatically adjusts the contribution weights of different observation factors in the objective function. This mechanism effectively reduces the weight of anomalous observations that may occur in sensors such as sonar, suppressing their interference. This method of dynamically adjusting confidence levels based on physical sensor characteristics ensures more robust and reliable optimization results and is naturally compatible with the uncertainty estimation information provided by the front end of this invention.

[0158] The optimization objective under the influence of the noise model is:

[0159]

[0160] in, This represents the pose change estimation result (sonar odometry) or inertial navigation integral result between two time points. This represents the uncertainty in pose transformation estimation (sonar odometry) or inertial navigation noise. Represents the pose to be optimized at time i:

[0161] Through the above methods, this invention effectively utilizes the noise model configuration interface and optimization capabilities provided by the GTSAM framework to achieve backend state estimation that integrates multi-source heterogeneous sensor data and exhibits adaptive robustness to uncertainties. The pose transformation results (IMU Pose) directly provided by the inertial navigation system on the AUV, and the pose transformation results (Sonar Odom) provided by the sonar odometry (composed of steps S1-S5), are then analyzed according to... Figure 10 By connecting the components into a factor graph and optimizing it, a high-precision forward-looking sonar SLAM system can be obtained.

[0162] To verify the practicality and robustness of this patented technical solution, a SLAM experiment was conducted using a data sequence of approximately 15 minutes from a publicly available dataset. The results are as follows: Figure 11 As shown, the sequence is 131.9m long. The first half of the trajectory is located along the port shore, where the sonar images contain rich textures. The second half moves away from the port, where the sonar images have sparse textures, posing a significant challenge to the SLAM system. Based on this sequence data, we used the evo tool to quantitatively evaluate the trajectory accuracy output by the system. The evaluation results show that the root mean square error (RMSE) of its absolute pose error (APE) is 1.301 meters, and the maximum error is only 3.111 meters. The comprehensive evaluation indicates that the positioning accuracy of this patented technology is excellent during long-term operation in real-world scenarios, effectively verifying the performance of the SLAM system provided by this invention.

[0163] Finally, it should be noted that the various embodiments in this specification are described in a progressive manner, with each embodiment focusing on the differences from other embodiments. The same or similar parts between the various embodiments can be referred to each other.

[0164] The above embodiments are only used to illustrate the technical solutions of the present invention and not to limit them; although the present invention has been described in detail with reference to preferred embodiments, those skilled in the art should understand that modifications can still be made to the specific implementation of the present invention or equivalent substitutions can be made to some technical features without departing from the spirit of the technical solutions of the present invention, and all such modifications and substitutions should be covered within the scope of the technical solutions claimed in the present invention.

Claims

1. A forward-looking sonar SLAM method based on deep Fourier registration, characterized in that, Includes the following steps: S1: Acquire two sonar images using forward-looking sonar, namely image [image name missing]. and images ; S2: Extract features from the two sonar images to obtain two rotational registration feature maps; perform Fourier transform on the two rotational registration feature maps to extract their amplitude spectrum information, and obtain two amplitude spectra; perform Radon transform on the two amplitude spectra to obtain two sets of angle slices, and calculate the cross-correlation similarity of the two sets of angle slices to obtain the angle correlation curve. S3: Based on the angle correlation curve, perform rotation correction on the two sonar images, and extract features from the corrected images to obtain two displacement registration feature maps; Phase correlation heatmaps are obtained by performing phase correlation calculations on two displacement registration feature maps. S4: Extract the relative rotation angle of the two sonar images from the angle correlation curve and calculate the corresponding rotation variance; extract the relative displacement of the two sonar images from the phase correlation heatmap and calculate the corresponding displacement variance; S5: Based on the relative rotation angle and the relative displacement, adjust the image... Perform rotation and translation corrections, and calculate the image. and the image after rotation and translation correction The SSIM correlation is used to weight and correct the rotation variance and the displacement variance to obtain the corrected variance and the corresponding covariance matrix. S6: Dynamically construct and / or configure a noise model acting on the observation factors based on the covariance matrix; based on the noise model, explicitly specify the noise model parameters for each observation factor within the GTSAM framework of the SLAM system backend.

2. The forward-looking sonar SLAM method based on deep Fourier registration according to claim 1, characterized in that, Step S2 further includes the following steps: Construct a deep learning network model based on the TransUNet architecture; The deep learning network model is improved to obtain a rotation registration network model, the expression of which is: in, Indicates the first Image number 1 in encoder Intermediate results from the layer output, Indicates the first The first convolutional kernel in the encoder of the layer, Indicates the first The second convolutional kernel in the encoder of the layer, Indicates the first The third convolutional kernel in the encoder of the layer, This means that the GeLU function is used to activate the system before Batch Norm is performed. This indicates the convolutional downsampling module. This indicates the convolutional upsampling module. This indicates that the input feature maps are stacked and then processed by the efficient self-attention model EMA. This represents the output of the feature extraction part of the rotation registration network. Indicates the first The first convolutional kernel in the decoder of the layer, Indicates the first The second convolutional kernel in the decoder of the layer, This indicates that the i-th image is in the decoder at the th . Feature maps output by the layer; The rotation registration network model is used to extract features from the two sonar images to obtain two rotation registration feature maps.

3. The forward-looking sonar SLAM method based on deep Fourier registration according to claim 2, characterized in that, Step S2, the method for improving the deep learning network model specifically includes the following steps: The TransUNet architecture is improved into a Siamese-TransUNet structure with a shared codec; An efficient multi-scale attention mechanism, EMA, is embedded at the jump connection to interact with the features of two images.

4. The forward-looking sonar SLAM method based on deep Fourier registration according to claim 1, characterized in that, In step S2, the formula for calculating the Radon transform is: ； in, The Fourier transform result of the sonar image is a two-dimensional spectrum containing phase and amplitude information. The distance is the line integral. For rotation angle, For the impulse response, u and v are both frequencies.

5. The forward-looking sonar SLAM method based on deep Fourier registration according to claim 4, characterized in that, In step S2, the formula for calculating the cross-correlation similarity between the two sets of angle slices is: ； Where X and Y are vectors involved in the relevant operations. For expectation operator, Let X be the variance of the elements of vector X. Let Y be the variance of the elements of vector Y. Let X be the i-th element. Let be the i-th element of vector Y, and n be the dimension of the vector.

6. The forward-looking sonar SLAM method based on deep Fourier registration according to claim 1, characterized in that, Step S3 further includes the following steps: Construct a deep learning network model based on the TransUNet architecture; The deep learning network model is improved to obtain a displacement registration network model, the expression of which is: in, Indicates the first The image is in the encoder. Intermediate results from the layer output, Indicates the first The first convolutional kernel in the encoder of the layer, Indicates the first The second convolutional kernel in the encoder of the layer, Indicates the first The third convolutional kernel in the encoder of the layer, This means that the GeLU function is used to activate the system before Batch Norm is performed. This indicates activation via the GeLU function. This indicates the convolutional downsampling module. This indicates the convolutional upsampling module. This indicates that the input feature maps are stacked and then processed by the efficient self-attention model EMA. This indicates that the Dysample upsampling module can be learned. This represents the output of the feature extraction network for displacement registration. Indicates the first The first convolutional kernel in the decoder of the layer, Indicates the first The second convolutional kernel in the decoder of the layer, This indicates that the i-th image is in the decoder at the th . Feature maps output by the layer; The displacement registration network model is used to extract features from the corrected image to obtain two displacement registration feature maps.

7. The forward-looking sonar SLAM method based on deep Fourier registration according to claim 6, characterized in that, Step S3, the method for improving the deep learning network model specifically includes the following steps: The TransUNet architecture is improved into a Siamese-TransUNet structure with a shared codec; An efficient multi-scale attention mechanism (EMA) module is embedded at the skip connection of the Siamese-TransUNet structure to interact with the features of two images, and the difference results of the two feature maps are added to the input of the EMA module. A lightweight, learnable upsampling Dysample module is introduced into the Siamese-TransUNet architecture.

8. The forward-looking sonar SLAM method based on deep Fourier registration according to claim 1, characterized in that, In step S3, the formula for calculating phase correlation is as follows: ； in, For phase correlation heatmap, For image Two-dimensional spectrum, For image Two-dimensional spectrum, image Two-dimensional spectrum and image The relationship of the two-dimensional spectrum in the frequency domain is as follows: .

9. The forward-looking sonar SLAM method based on deep Fourier registration according to claim 1, characterized in that, In step S5, the formula for calculating SSIM correlation is: in, The original variance, This is the corrected variance.

10. A forward-looking sonar SLAM system based on deep Fourier registration, used to implement the forward-looking sonar SLAM method based on deep Fourier registration as described in any one of claims 1-9, characterized in that, Includes front-end and back-end modules; The front-end module includes a rotation registration network model, a displacement registration network model, and an inter-frame motion estimation unit. The rotation registration network model is used to extract features from two sonar images to obtain two rotation registration feature maps. The displacement registration network model is used to extract features from the corrected image to obtain two displacement registration feature maps. The inter-frame motion estimation unit is used to perform calculations on the feature extraction results of the rotation registration network model and the displacement registration network model using a motion calculation algorithm to output the pose transformation and covariance matrix at adjacent time points. The backend module adopts the GTSAM framework, which includes a noise model constructed based on the covariance matrix. During the nonlinear optimization process, the GTSAM framework automatically adjusts the contribution weights of different observation factors in the objective function using parameters provided by the noise model.

Images