UWB radar SAR-SLAM positioning and mapping method and system oriented to visual degradation environment

Abstract

The invention discloses a UWB radar SAR-SLAM positioning and mapping method and system for a visual degradation environment, and the method comprises the steps: S1, carrying out the UWB radar sensing and environment SAR image construction, carrying out the coherent accumulation of UWB echo signals through a back projection algorithm and odometer pose auxiliary information, and generating a high-resolution environment SAR subgraph; s2, extracting anti-noise environment features, fusing OS-CFAR and SIFT algorithms, adaptively screening high-scattering key points and generating stable feature descriptors; s3, feature matching and relative pose resolving are carried out, and relative pose transformation between the sub-graphs is accurately resolved through bidirectional consistency screening and an RANSAC-Umeyama algorithm; s4, speedometer drift correction and SLAM positioning mapping are carried out, accumulated drift is corrected based on the relative pose, and high-precision self-positioning and global consistent mapping in the visual degradation environment are achieved. According to the invention, high-precision self-positioning and mapping in a complex environment are realized.

Description

Technical Field

[0001] This invention relates to the field of SLAM localization and mapping technology, and in particular to a UWB radar SAR-SLAM localization and mapping method and system for visually degraded environments. Background Technology

[0002] Simultaneous localization and mapping (SLAM) is a core technology for mobile robots to achieve autonomous navigation and task execution in unknown environments. Current mainstream SLAM systems primarily rely on optical sensors such as LiDAR (Lidar Radar) and vision cameras. LiDAR, with its active time-of-flight (ToF) measurement mechanism, provides high-precision, high-density environmental geometry information and is less affected by changes in illumination, making it widely used in various advanced SLAM systems. However, both LiDAR and vision cameras operate in or near the visible light spectrum, making them highly susceptible to failure in degraded environments such as smoke, dense dust, strong reflections, or extremely low light.

[0003] Sonar technology has some potential as a substitute in low-visibility environments (especially underwater), but sound waves are easily affected by refraction effects, and ranging accuracy will significantly decrease in extreme environments with high temperature gradients, such as fire rescue or mining operations. Compared to optical and acoustic sensors, microwave radar, with its longer wavelength, has a natural advantage in penetrating harsh media such as smoke, dust, and rain and snow, making it highly adaptable to various environments. Ultra-wideband (UWB) radar, as a type of microwave radar, has extremely short nanosecond-level pulse characteristics, which not only provides excellent penetration performance but also boasts a high signal-to-noise ratio (SNR), low power consumption, and millimeter-level ranging resolution, making it an important candidate sensor for robust sensing in complex indoor environments.

[0004] In recent years, research on incorporating UWB radar into mobile robot SLAM systems has gradually increased, mainly falling into three technical categories: The first is anchor-based schemes: utilizing the ranging information of UWB sensors relative to preset anchor points and the received signal strength (RSSI), combined with non-line-of-sight (NLOS) recognition algorithms to infer obstacle positions and construct a 2D occupancy grid map; however, this method heavily relies on pre-deployed infrastructure, resulting in high deployment costs and a lack of flexibility, and making it difficult to acquire overall geometric details of the environment. The second is infrastructure-free point cloud schemes: relying solely on monostatic UWB radar, using triangulation to extract environmental feature point clouds for localization; however, limited by the multipath effect and angular resolution of UWB signals, the generated point cloud map has significant noise, making it difficult to finely distinguish minute environmental features such as corners and uneven surfaces, and the system lacks robustness in non-ideal environments. The third is synthetic aperture radar (SAR) imaging schemes: utilizing radar motion to synthesize a virtual large aperture, significantly improving azimuth resolution, enabling high-precision imaging through glass walls and dense smoke, and producing a map quality significantly superior to traditional sparse point cloud methods.

[0005] However, despite the superior performance of UWB-SAR imaging in mapping, high-precision self-localization using the generated SAR images remains a pressing problem. Existing SAR imaging algorithms typically assume a known trajectory or are extremely sensitive to trajectory errors, lacking an effective closed-loop feedback mechanism to correct accumulated odometry drift. Furthermore, the inherent speckle noise in UWB-SAR images poses a significant challenge to traditional image feature extraction and matching.

[0006] Chinese patent document CN110849367B discloses an indoor positioning and navigation method based on visual SLAM fused with UWB. The method includes: establishing a world coordinate system and deploying UWB base stations; a robot equipped with an RGB-D camera and a UWB transceiver to acquire color and depth images during movement, and to obtain the robot's position coordinates in real time based on UWB trilateration; extracting and matching ORB feature points between adjacent images, and calculating the camera projection pose using an epipolar geometry algorithm; constructing a point cloud map by combining pose and depth information, and using UWB positioning information to assist in loop closure detection to correct accumulated errors and optimize pose and map.

[0007] While the aforementioned technical solutions can achieve positioning and mapping in indoor environments, they still suffer from the following problems: The method relies on an RGB-D camera, which fails in visually degraded environments such as smoke, dense dust, and low light; it requires the pre-deployment of three UWB base stations, resulting in poor deployment flexibility; and UWB only assists in loopback detection, failing to effectively correct odometer drift. Therefore, a technical solution capable of achieving high-precision positioning and mapping in visually degraded environments is urgently needed to address the aforementioned problems in existing technologies. Summary of the Invention

[0008] The technical problem solved by this invention is to provide a UWB radar SAR-SLAM localization and mapping method for visually degraded environments, ensuring stable and high-precision operation throughout the entire process from perception, feature extraction, pose estimation to closed-loop mapping in visually degraded environments.

[0009] To address the aforementioned technical issues, the present invention provides a UWB radar SAR-SLAM localization and mapping method for visually degraded environments, comprising the following steps: S1 UWB Radar Sensing and Environmental SAR Image Construction: Utilizing the penetration characteristics of UWB radar in visually degraded environments, combined with the back projection algorithm and odometry pose-assisted information, coherent accumulation of UWB echo signals is performed to invert environmental electromagnetic scattering characteristics and generate high-resolution environmental SAR sub-images. S2 Noise-resistant Environment Feature Extraction: For the inherent speckle noise of high-resolution environmental SAR sub-images, the constant false alarm rate detection OS-CFAR and scale-invariant feature transform SIFT algorithm are fused. First, high scattering key points are adaptively screened by OS-CFAR, and then stable feature descriptors are generated by SIFT to extract robust environmental features. S3 Feature Matching and Relative Pose Calculation: A geometric constraint verification mechanism is introduced in the feature matching stage. Candidate matching pairs are first screened through bidirectional consistency, and then geometric verification is performed using the RANSAC-Umeyama algorithm to accurately calculate the relative pose transformation between high-resolution environmental SAR sub-images. S4 Odometry Drift Correction and SLAM Localization and Mapping: Based on the calculated relative pose, the cumulative drift of the odometry is corrected, enabling mobile robots to achieve high-precision autonomous localization and globally consistent environmental mapping in visually degraded environments such as smoke, dust, and low light.

[0010] The above technical solution, from odometry-assisted UWB-SAR imaging (solving the map source problem), to OS-CFAR and SIFT fusion feature extraction specifically designed for SAR images (solving the feature robustness problem in noisy environments), to high-precision matching and pose calculation with geometric constraint verification (solving the relative measurement accuracy problem), ultimately forms a closed-loop drift correction and SLAM framework (solving the global consistency problem). Each component and technical means is interconnected and works together to ultimately achieve the technical goal of stable and high-precision operation in visually degraded environments.

[0011] Preferably, the specific process of generating a high-resolution environmental SAR sub-map using the back projection algorithm in step S1 is as follows: S11: Discretize the SAR image into a two-dimensional grid, where each pixel represents the backscattering intensity of a local area of ​​the environment; S12: Calculate any pixel point With the Euclidean distance between radar antenna positions at the next observation time The Euclidean distance The calculation formula is: ,in These are the pixel coordinates. For the first Radar antenna position coordinates at the next observation time; S13: Based on the Euclidean distance The sampling index of the echo signal in the fast time domain is calculated using the following formula: ,in At the speed of light, The sampling frequency; S14: Coherently superimpose the signals from all observation locations within the aperture synthesized by the UWB radar, extract the amplitude values ​​at the corresponding range indices from each scan signal, and accumulate them to generate a high-resolution environmental SAR sub-map. The accumulation formula is: ,in, This represents the total number of scans within the synthesis aperture. For the first The data is compressed by pulses after scanning. After traversing all measurement positions and all pixels, the true position of the target will be highlighted due to the coherent superposition of energy, while noise will be suppressed due to incoherent superposition.

[0012] Preferably, the operating parameters of the UWB radar in step S1 are: sampling frequency 23.328 GHz, center frequency 7.29 GHz, bandwidth 1.4 GHz, and pulse amplitude 1.0 V.

[0013] Preferably, the specific process of extracting noise-resistant environmental features in step S2 is as follows: S21: Calculate the modulus square of complex data on high-resolution environmental SAR sub-maps to generate power spectrum maps, enhancing the contrast between high-scattering targets and the background; S22: Execute the OS-CFAR detection algorithm on the power spectrum map. The OS-CFAR detector extracts a set of key points for each SAR image. The specific process is as follows: ; in, Let be the set of pixels to be detected in the SAR image. For the first OS-CFAR detection results for each pixel. This is the OS-CFAR detection operator, with the input parameters in parentheses. The operator performs ordered statistical constant false alarm rate detection on the input pixels. For the first The complex value of the nth pixel in the SAR image, its square represents the nth pixel. The power value of each pixel; The first in the test results The coordinates of the key points Representing the The total number of key points detected corresponding to each pixel; Subsequently, the OS-CFAR detection algorithm is executed on the power spectrum to adaptively select key points with significant scattering characteristics. The judgment rule of the OS-CFAR detection algorithm is as follows: ; in, For pixels The power value, An adaptive detection threshold; This represents the set of all pixels to be detected, covering all locations to be analyzed in the entire SAR image; , This is the threshold scaling factor. The first pixel power value within the background window Order statistics, For the first The set of key points obtained after OS-CFAR detection of a SAR image contains the coordinates of all target points that are determined to be significantly scattering. S23: The Scale Invariant Feature Transform (SIFT) algorithm is used to add feature descriptors to the extracted keypoints, and the gradient magnitude of the pixels is... Used to reflect the intensity of brightness changes in pixels, gradient angle Reflecting the direction of changes in brightness, gradient magnitude and angle are the core components of feature descriptors, which are statistical encodings of these local gradient information. The calculation formula is as follows: ; ; in It is a pixel grayscale value; Store the corresponding feature descriptors of all key points into the corresponding descriptor subsets. In addition, the main direction corresponding to the key point is also stored for subsequent high-resolution environmental SAR sub-map matching.

[0014] Preferably, the specific process of the geometric constraint verification mechanism in step S3 is as follows: S31: Compute from the descriptive subset using the Dual-Softmax matcher Feature descriptors and The similarity between the pairs is used to filter out candidate matching pairs that satisfy the bidirectional consistency constraint; the resulting candidate matching set is then used. Defined as: ; in, For the matching operator, take two sets of keypoints as input and output a set of candidate matching pairs that satisfy the bidirectional consistency constraint; each element Two SAR images to be matched The first in Key points and SAR images The first in The key points are the best match for each other; S32: The RANSAC random sampling consensus algorithm is used to robustly estimate candidate matching pairs. k matching pairs are randomly selected, and the rigid body transformation is calculated using the Umeyama algorithm. The largest consensus set satisfying the residual threshold is then statistically analyzed. The relationship of the rigid body transformation is as follows: This formula describes the spatial relationship between two high-resolution SAR sub-images; it takes the coordinates of key points in the previous frame. Through rotation matrix The translation vector t is mapped to the corresponding coordinates in the current frame. ; where the rotation matrix The calculation process for the translation vector t is as follows: Constructing the covariance matrix , and Let the centroids of the two point sets be denoted by ; then let ... Perform SVD decomposition to obtain , It is a left singular vector matrix; further, while ensuring the right-handed system of the rotation matrix, it is constructed... ,but Translation vector ; The rigid body transformation The calculation formula is: ; ; ; in, For all matching pairs in rigid body transformation The residuals below and These represent the matching key points. and Position vectors and interior point sets in Cartesian coordinate system To meet the residual threshold The largest consistent set; S33: Based on the aforementioned interior point set The Umeyama algorithm was then applied again for a more refined solution, yielding the final relative pose measurement values. .

[0015] The technical problem that this invention also aims to solve is to provide a UWB radar SAR-SLAM positioning and mapping system for visually degraded environments, enabling high-precision autonomous positioning and mapping in complex environments.

[0016] To address the aforementioned technical issues, the present invention provides the following solution: a UWB radar SAR-SLAM localization and mapping system for visually degraded environments, comprising a mapping module and a localization module; the mapping module includes a trajectory estimation unit, a pulse compression unit, and a back projection algorithm unit; the trajectory estimation unit uses the wheel odometer of a mobile robot as a short baseline attitude estimation source to provide pose information for SAR imaging; the pulse compression unit uses matched filtering technology to perform range compression on the UWB radar echo signal, and performs cross-correlation calculations between the received echo signal and the transmitted pulse template to maximize the signal-to-noise ratio; the back projection algorithm unit, based on time-domain radar scan-pixel mapping imaging technology, performs coherent accumulation of radar echo data to invert the electromagnetic scattering characteristics of the environment and generate a high-resolution environmental SAR sub-map.

[0017] Using the above technical solution, the system's mapping module combines odometry pose with UWB echoes, generating a clear SAR environmental image through signal compression and coherent accumulation. The positioning module extracts noise-resistant features from the image and performs geometric verification to calculate the accurate relative pose. The two modules form a closed loop through the SAR sub-map: the mapping module provides environmental characterization for positioning, while the positioning module uses the calculated pose to correct the trajectory estimation in the mapping. This closed-loop mechanism enables the system to continuously achieve high-precision positioning and mapping even in visually impaired environments.

[0018] Preferably, the localization module includes a sub-image coarse matching unit, a feature extraction unit, and a feature matching and relative pose estimation unit. The sub-image coarse matching unit uses a fast screening strategy based on geometric distance to determine potential loop closure candidate sub-images. The feature extraction unit uses a hybrid feature extraction strategy combining ordered statistical OS-CFAR and SIFT to extract noise-resistant features from high-resolution environmental SAR sub-images. The feature matching and relative pose estimation unit uses a coarse matching based on Dual-Softmax and a RANSAC-Umeyama geometric constraint verification mechanism to solve the relative pose transformation between high-resolution environmental SAR sub-images and correct the cumulative odometry drift.

[0019] Preferably, the Gaussian pulse parameters transmitted in the pulse compression unit are: sampling frequency 23.328 GHz, center frequency 7.29 GHz, bandwidth 1.4 GHz, and pulse amplitude 1.0 V.

[0020] Compared with the prior art, the beneficial effects of the present invention are: (1) This invention uses UWB radar as a sensing sensor, making full use of its all-weather penetration characteristics and high resolution advantages in extreme environments. It can work stably in visual degradation environments such as smoke, dust and low light, solving the problem that existing lidar and visual sensors are prone to failure in such environments. (2) The mapping module combines odometry-assisted and back projection algorithms to generate high-quality environmental SAR sub-maps with rich texture information. Compared with the traditional sparse point cloud method, the map construction quality is higher and can capture environmental geometric features in detail. (3) A robust feature extraction strategy combining OS-CFAR and SIFT is proposed. The OS-CFAR detector can adaptively screen high scattering key points and effectively suppress the inherent speckle noise of SAR images. The SIFT descriptor can provide stable feature representation, retain key environmental features while suppressing noise, and improve the reliability of feature extraction. (4) A coarse matching and RANSAC-Umeyama geometric constraint verification mechanism based on Dual-Softmax is designed, which can accurately solve the relative pose transformation between subgraphs, effectively eliminate mismatches, significantly improve the accuracy of relative pose estimation, and thus effectively correct the cumulative drift of odometry and achieve globally consistent trajectory estimation. Attached Figure Description

[0021] Figure 1 A flowchart illustrating the UWB radar SAR-SLAM localization and mapping method; Figure 2The results of pulse compression processing of UWB radar signals based on matched filtering are shown. (a) is the original echo signal containing target echo, multipath clutter and background noise; (b) is the reconstructed transmitted Gaussian pulse reference signal; and (c) is the output signal after pulse compression. It can be seen that the signal energy is coherently accumulated after processing, the noise is significantly suppressed, the waveform peak is sharpened, and the signal-to-noise ratio and range resolution of the signal are effectively improved. Figure 3 The process of feature matching and registration of SAR sub-images is visualized; (a) shows the initial matching result based on SIFT descriptors and the Dual-Softmax algorithm; (b) shows the result after RANSAC geometric verification; and (c) shows the superimposed display effect of transforming the sub-image to be registered into the reference coordinate system using the estimated transformation matrix. Figure 4 The images show the results of imaging the real environment, with the top left corner showing the real environment and the top right corner showing the imaged environment map. Figure 5 The results show the trajectory correction and error analysis for a real-world environment. Detailed Implementation

[0022] The technical solutions in the embodiments of the present invention will now be clearly and completely described with reference to the accompanying drawings.

[0023] Example: Figure 1 As shown, this UWB radar SAR-SLAM localization and mapping method for visually degraded environments includes the following steps: S1 UWB Radar Sensing and Environmental SAR Image Construction: Utilizing the penetration characteristics of UWB radar in visually degraded environments, combined with the back projection algorithm and odometry pose-assisted information, coherent accumulation of UWB echo signals is performed to invert environmental electromagnetic scattering characteristics and generate high-resolution environmental SAR sub-images. The core of this step is to utilize the penetration characteristics of UWB radar, combined with back projection algorithms and odometry assistance, to convert radar echo signals into high-resolution environmental SAR sub-maps. The specific implementation process is as follows: Data Acquisition and Synchronization: The mobile robot is controlled to move at a speed of 0.2 m / s in the target environment. The UWB radar emits pulses at a preset frequency (10 Hz) and receives echo signals. Simultaneously, the odometry outputs robot pose data (x_odom, y_odom, θ_odom) in real time. Hardware trigger signals are used to align the timestamps of the radar echo data and the odometry data, ensuring that each frame of radar data corresponds to a unique robot pose. Echo signal preprocessing: The received UWB echo signal undergoes pulse compression processing. A matched filtering algorithm is used to perform cross-correlation calculation between the echo signal and the transmitted pulse template. The formula is as follows: ,in The original echo signal, This is the template for emitting Gaussian pulses; * indicates cross-correlation operation. The pulse compression processing effect is as follows: Figure 2 As shown, (a) is the original echo signal containing target echo, multipath clutter and background noise; (b) is the reconstructed transmitted Gaussian pulse reference signal; and (c) is the output signal after pulse compression. It can be seen that after processing, the signal energy achieves coherent accumulation, the noise is significantly suppressed, the waveform peak is sharpened, and the signal-to-noise ratio and range resolution of the signal are effectively improved. Back projection imaging: Combining pose-aided information provided by odometry, an environmental SAR sub-map is generated using a back projection algorithm. The specific process is as follows: S11: Discretize the SAR image into a two-dimensional grid, where each pixel represents the backscattering intensity of a local area of ​​the environment; S12: Calculate any pixel point With the Euclidean distance between radar antenna positions at the next observation time The Euclidean distance The calculation formula is: ,in These are the pixel coordinates. For the first Radar antenna position coordinates at the next observation time; S13: Based on the Euclidean distance The sampling index of the echo signal in the fast time domain is calculated using the following formula: ,in At the speed of light, The sampling frequency; S14: Coherently superimpose the signals from all observation locations within the aperture synthesized by the UWB radar, extract the amplitude values ​​at the corresponding range indices from each scan signal, and accumulate them to generate a high-resolution environmental SAR sub-map. The accumulation formula is: ,in, This represents the total number of scans within the synthesis aperture. For the first The data is compressed by pulses after scanning. After traversing all measurement positions and all pixels, the true position of the target will be highlighted due to the coherent superposition of energy, while noise will be suppressed due to incoherent superposition.

[0024] In this embodiment, the specific operation of the back projection algorithm for generating environmental SAR sub-maps is as follows: the area to be imaged is discretized into a 5cm × 5cm two-dimensional grid, each grid corresponding to a pixel in the SAR image, and the pixel value characterizes the electromagnetic backscattering intensity of the area; for any pixel... According to the odometer reading Radar antenna position at the next scan time Calculate the Euclidean distance between the pixel and the antenna position. Then, based on the electromagnetic wave propagation speed c (3×10⁻⁶), 8 (m / s), deriving the sampling index of the echo signal in the fast time domain. The signals from N scans within the synthetic aperture (N=50 in this embodiment) are coherently superimposed, and the corresponding sampling index is extracted from each scan signal. The amplitude values ​​at each point are summed to obtain the pixel value. The grayscale value is calculated using the following formula: ,in This represents the total number of scans within the synthesis aperture. For the first The data from this scan is compressed using pulses. A final environmental SAR sub-map with a size of 600×400 pixels is generated, completing the inversion of the environmental electromagnetic scattering characteristics. The imaging results of the real environment are as follows: Figure 4 As shown in the figure, it can be clearly seen that even in a visually degraded environment, the imaging results can still accurately restore key features such as the outline of obstacles and the structure of walls in the environment, without obvious distortion or noise interference, providing a high-quality environmental characterization basis for subsequent feature extraction.

[0025] S2 Noise-resistant Environment Feature Extraction: To address the inherent speckle noise in SAR images, the constant false alarm rate (CFAR) detection algorithm (OS-CFAR) and the scale-invariant feature transform (SIFT) algorithm are fused. First, OS-CFAR adaptively filters high-scattering key points, and then SIFT generates stable feature descriptors to extract robust environment features. The inherent speckle noise in SAR images can severely affect the robustness of feature extraction. This step achieves noise-resistant feature extraction by fusing the OS-CFAR and SIFT algorithms.

[0026] The specific process of extracting noise-resistant environmental features in step S2 is as follows: S21: Calculate the modulus square of complex data on high-resolution environmental SAR sub-maps to generate power spectrum maps, enhancing the contrast between high-scattering targets and the background; S22: Execute the OS-CFAR detection algorithm on the power spectrum map. The OS-CFAR detector extracts a set of key points for each SAR image. The specific process is as follows: ; in, Let be the set of pixels to be detected in the SAR image. For the first OS-CFAR detection results for each pixel. This is the OS-CFAR detection operator, with the input parameters in parentheses. The operator performs ordered statistical constant false alarm rate detection on the input pixels. For the first The complex value of the nth pixel in the SAR image, its square represents the nth pixel. The power value of each pixel; The first in the test results The coordinates of the key points Representing the The total number of key points detected corresponding to each pixel; Photo Gallery Each subgraph in First, the square of the modulus of its complex data is calculated to generate the power spectrum. This operation can enhance the contrast between highly scattering targets and the background in the sub-image; Subsequently, the OS-CFAR detection algorithm is executed on the power spectrum to adaptively select key points with significant scattering characteristics. The judgment rule of the OS-CFAR detection algorithm is as follows: ; in, For pixels The power value, An adaptive detection threshold; This represents the set of all pixels to be detected, covering all locations to be analyzed in the entire SAR image; , This is the threshold scaling factor. The first pixel power value within the background window Order statistics, For the first The set of key points obtained after OS-CFAR detection of a SAR image contains the coordinates of all target points that are determined to be significantly scattering. Threshold scaling factor Based on the pre-set false alarm probability Through recursion, we obtain: ; Points to be tested Divide the area into a protective window and a background window, centered on the protective window. The number of pixels in the protective window is denoted as [missing information]. This is used to avoid interference from target power leakage on clutter statistics; within the background window The power values ​​of each pixel constitute a set. Sort it in ascending order to obtain an ordered sequence. Select the k-th order statistic As a characterization value of clutter power. Among them for: ; in These are parameters related to the background window; specifically, they can represent the "radius" or "half-side length" of the background window, used to define its size. This ensures a minimum false alarm rate. From this, the adaptive detection threshold can be calculated, and the required set of key points can be extracted. This is used for subsequent feature description; S23: The Scale Invariant Feature Transform (SIFT) algorithm is used to add feature descriptors to the extracted keypoints, and the gradient magnitude of the pixels is... Used to reflect the intensity of brightness changes in pixels, gradient angle Reflecting the direction of changes in brightness, gradient magnitude and angle are the core components of feature descriptors, which are statistical encodings of these local gradient information. The calculation formula is as follows: ; ; in It is a pixel grayscale value; Store the corresponding feature descriptors of all key points into the corresponding descriptor subsets. In addition, the main direction corresponding to the key point is also stored for subsequent subgraph matching.

[0027] The specific operation of extracting noise-resistant environmental features in this embodiment is as follows: The SAR sub-image generated in step S1 is subjected to grayscale normalization, mapping the pixel grayscale values ​​to the [0, 255] interval. Simultaneously, a 5×5 mean filter is used for preliminary noise reduction to suppress high-frequency noise interference. To address the randomness of speckle noise, the Ordered Statistical Constant False Alarm Rate Detection (OS-CFAR) algorithm is used to screen high-scattering key points. Specific parameter settings are: a 3×3 pixel guard window (to avoid target energy diffusion affecting threshold calculation), a 7×7 pixel background window (to cover sufficient background area for statistical noise characteristics), and a threshold scaling factor. =1.2 (calibrated according to ambient noise level). The judgment rule is: if the pixel... power value >Adaptive threshold If it is a high scattering key point, then it is determined to be a key point; where the adaptive threshold , The first pixel power value in the background window, sorted in ascending order. Order statistics.

[0028] After OS-CFAR filtering, high-scattering keypoints were used to generate stable feature descriptors using the SIFT algorithm. A Difference-of-Gaussian (DoG) pyramid was employed, with a Gaussian kernel variance range of 1.6-6.4, 6 pyramid layers, and an image scaling factor of 0.8 for each layer, to achieve scale invariance detection of keypoints. Local extrema in the DoG pyramid were fitted and optimized to remove low-contrast and edge points. The gradient magnitude within the keypoint's neighborhood (16×16 pixels) was calculated. and argument Using the gradient magnitude of pixels Reflects the intensity of brightness changes in a pixel. Uses the pixel's gradient magnitude. Reflects the intensity of brightness changes in a pixel: ; in It is a pixel The grayscale value. The gradient angle reflects the direction of the change in brightness: ; After calculating the gradient magnitude and argument within the neighborhood, a histogram needs to be built to record the argument of each pixel in the neighborhood. The histogram is divided into 36 bars, each representing 10 degrees. All pixels in the neighborhood are classified according to their argument range, and the gradient magnitudes of all pixels within that range are summed to obtain the height of the corresponding bar. After the histogram is built, it needs to be smoothed to prevent noise interference. ; It is the th in the smoothed histogram A new height for the directional pillar. It is the first line in the original histogram before smoothing. The height of each column. Find the highest column in the histogram as the principal direction of the feature point, since this column represents a range. For accurate values, the following fitting is needed: ; ; Determine the principal direction of the feature point. Then, rotate the main direction to horizontal. The coordinate system is aligned with the horizontal axis. Subsequent feature descriptor extraction will be performed in this aligned coordinate system. The neighborhood of each keypoint is divided into 4×4 sub-regions, and gradient histograms in 8 directions are calculated for each sub-region, ultimately generating a 128-dimensional SIFT feature descriptor. The corresponding feature descriptors for all keypoints are stored in the corresponding descriptor subsets. In addition, the main direction corresponding to the key point is also stored for subsequent subgraph matching.

[0029] S3 Feature Matching and Relative Pose Calculation: A geometric constraint verification mechanism is introduced in the feature matching stage. Candidate matching pairs are first screened through bidirectional consistency, and then the RANSAC-Umeyama algorithm is used for geometric verification to accurately calculate the relative pose transformation between subgraphs. This step improves the accuracy of feature matching by introducing a geometric constraint verification mechanism, thereby accurately calculating the relative pose transformation between subgraphs.

[0030] The specific process of the geometric constraint verification mechanism described in step S3 is as follows: S31: Calculate feature descriptors from the descriptor set using the Dual-Softmax matcher. and The similarity between the pairs is used to filter out candidate matching pairs that satisfy the bidirectional consistency constraint; the resulting candidate matching set is then used. Defined as: ; Each element Two SAR images to be matched The first in Key points and SAR images The first in The key points are the best match for each other; S32: The RANSAC random sampling consensus algorithm is used to robustly estimate candidate matching pairs. k matching pairs are randomly selected, and the rigid body transformation is calculated using the Umeyama algorithm. The largest consensus set satisfying the residual threshold is then statistically analyzed. The relationship of the rigid body transformation is as follows: This formula describes the spatial relationship between two high-resolution SAR sub-images; it takes the coordinates of key points in the previous frame. Through rotation matrix The translation vector t is mapped to the corresponding coordinates in the current frame. ; where the rotation matrix The calculation process for the translation vector t is as follows: Constructing the covariance matrix , and Let the centroids of the two point sets be denoted by ; then let ... Perform SVD decomposition to obtain , It is a left singular vector matrix; further, while ensuring the right-handed system of the rotation matrix, it is constructed... ,but Translation vector ; The rigid body transformation The calculation formula is: ; ; ; in, For all matching pairs in rigid body transformation The residuals below and These represent the matching key points. and Position vectors and interior point sets in Cartesian coordinate system To meet the residual threshold The maximum consistent set S33: based on the interior set The Umeyama algorithm was then applied again for a more refined solution, yielding the final relative pose measurement values. .

[0031] The specific operation of the geometric constraint verification mechanism in this embodiment is as follows: Initial matching and bidirectional consistency filtering: The Dual-Softmax matcher is used to calculate the similarity of SIFT feature descriptors in adjacent SAR sub-images (the current sub-image and the previous frame sub-image), and a similarity threshold of 0.7 is set to filter out initial matching pairs; further, a bidirectional consistency constraint is introduced, that is, if key point a in sub-image A matches key point b in sub-image B, and the optimal match of key point b in sub-image B is key point a in sub-image A, then the matching pair is retained, otherwise it is discarded, thus initially eliminating mismatched pairs; RANSAC-Umeyama Geometric Validation: To further eliminate remaining mismatched pairs, a geometric validation process is performed using the RANSAC algorithm combined with the Umeyama algorithm. The specific process is as follows: Random sampling: Randomly select 4 sets of non-collinear matching pairs from the candidate matching pairs (the minimum number of samples that satisfy the rigid body transformation model); Model fitting: The Umeyama algorithm is used to solve for the rigid body transformation matrix T (including rotation matrix R and translation vector t) between adjacent subgraphs. The transformation relationship is as follows: ,in The coordinates of the key points in the current subgraph. Match keypoint coordinates to the previous frame sub-image; Interior point statistics: Calculate the projected residuals of all candidate matching pairs. Set residual threshold =0.05m, and match pairs with residuals less than the threshold are identified as interior points; Iterative optimization: Repeat the above sampling-fitting-statistical process, with 1000 iterations. Select the transformation model with the most inliers as the optimal model. Based on the inlier set of the optimal model, apply the Umeyama algorithm again to refine the solution, obtaining the final relative pose transformation matrix. .

[0032] The visualization results of the SAR sub-image feature matching and registration process are as follows: Figure 3 As shown, (a) is the initial matching result based on the SIFT descriptor and the Dual-Softmax algorithm. Due to the inherent speckle noise and viewpoint differences in SAR images, there are many geometrically inconsistent mismatch pairs. (b) is the result after RANSAC geometric verification. The mismatch pairs are effectively eliminated, and only the interior points that satisfy the rigid body transformation constraints are retained, ensuring the accuracy of pose estimation. (c) is the superimposed display effect of transforming the sub-image to be registered to the reference coordinate system using the estimated transformation matrix. The image features are highly overlapping, verifying the reliability of the relative pose calculation and providing a precise constraint basis for subsequent odometry drift correction.

[0033] S4 Odometry Drift Correction and SLAM Localization and Mapping: Based on the calculated relative pose, the cumulative drift of the odometry is corrected, enabling mobile robots to achieve high-precision autonomous localization and globally consistent environmental mapping in visually degraded environments such as smoke, dust, and low light.

[0034] Based on the relative pose calculated in step S3, the cumulative drift of the odometry is corrected, ultimately achieving high-precision positioning and globally consistent mapping. Specific implementation details are as follows: Odometer drift correction: Odometer original pose As the moving distance increases, cumulative drift will occur, affecting the relative pose transformation matrix obtained in step S3. As a constraint, the original pose is corrected, and the correction formula is as follows: ,in To correct the robot's pose and ensure pose accuracy; SLAM Mapping and Optimization: A graph-based SLAM framework is adopted, using the corrected robot pose as nodes and the relative transformation between adjacent poses as edges to construct a pose graph; at the same time, high scattering key points in the SAR subgraph are used as map feature points and associated with the corresponding pose nodes; finally, bundle adjustment (BA) is used to globally optimize the pose graph, minimize the global pose error, and generate a globally consistent environment map. Real-time output: Location results are displayed in [the format of the output]. The system outputs data in real-time (10Hz update frequency), and the map is stored in raster format, supporting autonomous navigation and obstacle avoidance for the robot.

[0035] The trajectory correction results and error analysis in the real environment are as follows: Figure 5As shown in the figure, the differences between the original odometer trajectory, the trajectory corrected by this invention, and the actual trajectory are clearly compared. The figure also demonstrates the changes in positioning error at different time steps. The trajectory comparison shows that the original odometer trajectory deviates significantly due to accumulated drift, while the corrected trajectory highly overlaps with the actual trajectory. The error curves show that the absolute pose error of the original odometer can reach over 1000mm. After correction using the method of this invention, the error is significantly reduced to within 200mm, fully verifying the effectiveness of this invention in correcting odometer drift and improving positioning accuracy.

[0036] This UWB radar SAR-SLAM localization and mapping system for visually degraded environments includes a mapping module and a localization module. The mapping module includes a trajectory estimation unit, a pulse compression unit, and a back projection algorithm unit. The trajectory estimation unit uses the wheel odometer of a mobile robot as a short baseline attitude estimation source to provide pose information for SAR imaging. The pulse compression unit uses matched filtering technology to perform range compression on the UWB radar echo signal and performs cross-correlation calculations between the received echo signal and the transmitted pulse template to maximize the signal-to-noise ratio. The back projection algorithm unit is based on time-domain "radar scan-pixel mapping" imaging technology. By coherently accumulating radar echo data, it inverts the electromagnetic scattering characteristics of the environment and generates an environmental SAR sub-map. The localization module includes a sub-image coarse matching unit, a feature extraction unit, and a feature matching and relative pose estimation unit. The sub-image coarse matching unit employs a fast screening strategy based on geometric distance to identify potential loop closure candidate sub-images. The feature extraction unit uses a hybrid feature extraction strategy combining ordered statistical OS-CFAR and SIFT to extract noise-resistant features from the SAR sub-images. The feature matching and relative pose estimation unit uses a coarse matching based on Dual-Softmax and a RANSAC-Umeyama geometric constraint verification mechanism to calculate the relative pose transformation between sub-images and correct the cumulative drift of the odometry. The Gaussian pulse parameters transmitted in the pulse compression unit are: sampling frequency 23.328 GHz, center frequency 7.29 GHz, bandwidth 1.4 GHz, and pulse amplitude 1.0 V.

[0037] The UWB radar SAR-SLAM localization and mapping system for visually degraded environments is configured as follows: Mobile robot platform: A wheeled mobile robot is selected, equipped with an incremental photoelectric encoder as a wheeled odometer, with a distance resolution of 0.01m and an angle resolution of 0.1°, which is used to provide an initial pose reference; UWB radar module: It adopts a side-view installation method and is fixed on the right side of the robot 10cm above the ground. The radar parameters are set as follows: center frequency 7.29GHz, bandwidth 1.4GHz, sampling frequency 23.328GHz, transmitted Gaussian pulse amplitude 1.0V, pulse width 1ns, field of view 60°, detection distance range 0.4m-2.6m, used to collect environmental echo signals. Data processing unit: An embedded industrial computer (CPU: Intel Core i5-10300H, 16GB memory, 512GB SSD storage) is used to run core algorithms such as SAR imaging, feature extraction, pose calculation and SLAM mapping. The data transmission interface adopts USB 3.0 to ensure synchronous transmission of radar data and odometer data (timestamp accuracy 1ms).

[0038] For those skilled in the art, the specific embodiments are merely illustrative descriptions of the present invention. Obviously, the specific implementation of the present invention is not limited to the above-described manner. Any non-substantial improvements made using the inventive concept and technical solution of the present invention, or the direct application of the inventive concept and technical solution to other situations without modification, are all within the protection scope of the present invention.

Claims

1. A UWB radar SAR-SLAM localization and mapping method for visually degraded environments, characterized in that, Includes the following steps: S1 UWB Radar Sensing and Environmental SAR Image Construction: Utilizing the penetration characteristics of UWB radar in visually degraded environments, combined with the back projection algorithm and odometry pose-assisted information, coherent accumulation of UWB echo signals is performed to invert environmental electromagnetic scattering characteristics and generate high-resolution environmental SAR sub-images. S2 Noise-resistant Environment Feature Extraction: For the inherent speckle noise of high-resolution environmental SAR sub-images, the constant false alarm rate detection OS-CFAR and scale-invariant feature transform SIFT algorithm are fused. First, high scattering key points are adaptively screened by OS-CFAR, and then stable feature descriptors are generated by SIFT to extract robust environmental features. S3 Feature Matching and Relative Pose Calculation: A geometric constraint verification mechanism is introduced in the feature matching stage. Candidate matching pairs are first screened through bidirectional consistency, and then geometric verification is performed using the RANSAC-Umeyama algorithm to accurately calculate the relative pose transformation between high-resolution environmental SAR sub-images. S4 Odometry Drift Correction and SLAM Localization and Mapping: Based on the calculated relative pose, the cumulative drift of the odometry is corrected, enabling mobile robots to achieve high-precision autonomous localization and globally consistent environmental mapping in visually degraded environments such as smoke, dust, and low light.

2. The UWB radar SAR-SLAM localization and mapping method for visually degraded environments according to claim 1, characterized in that, The specific process of generating a high-resolution environmental SAR sub-map using the back projection algorithm in step S1 is as follows: S11: Discretize the SAR image into a two-dimensional grid, where each pixel represents the backscattering intensity of a local area of ​​the environment; S12: Calculate any pixel point With the Euclidean distance between radar antenna positions at the next observation time The Euclidean distance The calculation formula is: ,in These are the pixel coordinates. For the first Radar antenna position coordinates at the next observation time; S13: Based on the Euclidean distance The sampling index of the echo signal in the fast time domain is calculated using the following formula: ,in At the speed of light, The sampling frequency; S14: Coherently superimpose the signals from all observation locations within the aperture synthesized by the UWB radar, extract the amplitude values ​​at the corresponding range indices from each scan signal, and accumulate them to generate a high-resolution environmental SAR sub-map. The accumulation formula is: ,in, This represents the total number of scans within the synthesis aperture. For the first The data is compressed by pulses after scanning. After traversing all measurement positions and all pixels, the true position of the target will be highlighted due to the coherent superposition of energy, while noise will be suppressed due to incoherent superposition.

3. The UWB radar SAR-SLAM localization and mapping method for visually degraded environments according to claim 1, characterized in that, The operating parameters of the UWB radar in step S1 are: sampling frequency 23.328 GHz, center frequency 7.29 GHz, bandwidth 1.4 GHz, and pulse amplitude 1.0 V.

4. The UWB radar SAR-SLAM localization and mapping method for visually degraded environments according to claim 2, characterized in that, The specific process of extracting noise-resistant environment features in step S2 is as follows: S21: Calculate the modulus square of complex data on high-resolution environmental SAR sub-maps to generate power spectrum maps, enhancing the contrast between high-scattering targets and the background; S22: Execute the OS-CFAR detection algorithm on the power spectrum map. The OS-CFAR detector extracts a set of key points for each SAR image. The specific process is as follows: ； in, Let be the set of pixels to be detected in the SAR image. For the first OS-CFAR detection results for each pixel. This is the OS-CFAR detection operator, with the input parameters in parentheses. The operator performs ordered statistical constant false alarm rate detection on the input pixels. For the first The complex value of the nth pixel in the SAR image, its square represents the nth pixel. The power value of each pixel; The first in the test results The coordinates of the key points Representing the The total number of key points detected corresponding to each pixel; Subsequently, the OS-CFAR detection algorithm is executed on the power spectrum to adaptively select key points with significant scattering characteristics. The judgment rule of the OS-CFAR detection algorithm is as follows: ； in, For pixels The power value, An adaptive detection threshold; This represents the set of all pixels to be detected, covering all locations to be analyzed in the entire SAR image; , This is the threshold scaling factor. The first pixel power value within the background window Order statistics, For the first The set of key points obtained after OS-CFAR detection of a SAR image contains the coordinates of all target points that are determined to be significantly scattering. S23: The Scale Invariant Feature Transform (SIFT) algorithm is used to add feature descriptors to the extracted keypoints, and the gradient magnitude of the pixels is... Used to reflect the intensity of brightness changes in pixels, gradient angle Reflecting the direction of changes in brightness, gradient magnitude and angle are the core components of feature descriptors, which are statistical encodings of these local gradient information. The calculation formula is as follows: ； ； in It is a pixel grayscale value; Store the corresponding feature descriptors of all key points into the corresponding descriptor subsets. In addition, the main direction corresponding to the key point is also stored for subsequent high-resolution environmental SAR sub-map matching.

5. The UWB radar SAR-SLAM localization and mapping method for visually degraded environments according to claim 4, characterized in that, The specific process of the geometric constraint verification mechanism in step S3 is as follows: S31: Compute from the descriptive subset using the Dual-Softmax matcher Feature descriptors and The similarity between the pairs is used to filter out candidate matching pairs that satisfy the bidirectional consistency constraint; the resulting candidate matching set is then used. Defined as: ； in, For the matching operator, take two sets of keypoints as input and output a set of candidate matching pairs that satisfy the bidirectional consistency constraint; each element Two SAR images to be matched The first in Key points and SAR images The first in The key points are the best match for each other; S32: The RANSAC random sampling consensus algorithm is used to robustly estimate candidate matching pairs. k matching pairs are randomly selected, and the rigid body transformation is calculated using the Umeyama algorithm. The largest consensus set satisfying the residual threshold is then statistically analyzed. The relationship of the rigid body transformation is as follows: This formula describes the spatial relationship between two high-resolution SAR sub-images; it takes the coordinates of key points in the previous frame. Through rotation matrix The translation vector t is mapped to the corresponding coordinates in the current frame. ; where the rotation matrix The calculation process for the translation vector t is as follows: Constructing the covariance matrix , and Let the centroids of the two point sets be denoted by ; then let ... Perform SVD decomposition to obtain , It is a left singular vector matrix; further, while ensuring the right-handed system of the rotation matrix, it is constructed... ,but Translation vector ; The rigid body transformation The calculation formula is: ； ； ； in, For all matching pairs in rigid body transformation The residuals below and These represent the matching key points. and Position vectors and interior point sets in Cartesian coordinate system To meet the residual threshold The largest consistent set; S33: Based on the aforementioned interior point set The Umeyama algorithm was then applied again for a more refined solution, yielding the final relative pose measurement values. .

6. A UWB radar SAR-SLAM localization and mapping system for visually degraded environments, characterized in that, The system includes a mapping module and a positioning module. The mapping module comprises a trajectory estimation unit, a pulse compression unit, and a back projection algorithm unit. The trajectory estimation unit uses the wheel odometer of the mobile robot as a short baseline attitude estimation source to provide pose information for SAR imaging. The pulse compression unit uses matched filtering technology to perform range compression on the UWB radar echo signal and performs cross-correlation calculations between the received echo signal and the transmitted pulse template to maximize the signal-to-noise ratio. The back projection algorithm unit is based on time-domain radar scan-pixel mapping imaging technology. By coherently accumulating radar echo data, it inverts the electromagnetic scattering characteristics of the environment and generates a high-resolution environmental SAR sub-map.

7. The UWB radar SAR-SLAM localization and mapping system for visually degraded environments according to claim 6, characterized in that, The localization module includes a sub-image coarse matching unit, a feature extraction unit, and a feature matching and relative pose estimation unit. The sub-image coarse matching unit uses a fast screening strategy based on geometric distance to determine potential loop closure candidate sub-images. The feature extraction unit uses a hybrid feature extraction strategy combining ordered statistical OS-CFAR and SIFT to extract noise-resistant features from high-resolution environmental SAR sub-images. The feature matching and relative pose estimation unit uses a coarse matching based on Dual-Softmax and a RANSAC-Umeyama geometric constraint verification mechanism to solve the relative pose transformation between high-resolution environmental SAR sub-images and correct the cumulative odometry drift.

8. The UWB radar SAR-SLAM localization and mapping system for visually degraded environments according to claim 6, characterized in that, The Gaussian pulse parameters transmitted in the pulse compression unit are: sampling frequency 23.328 GHz, center frequency 7.29 GHz, bandwidth 1.4 GHz, and pulse amplitude 1.0 V.

Images