An outdoor cross-modal robust positioning and navigation method for extreme bad weather

Abstract

This invention provides an outdoor cross-modal robust localization and navigation method for extreme weather conditions, relating to the field of online localization and navigation for mobile robots. The method includes the following steps: acquiring and preprocessing data; generating a pathpoint set based on a CMR network, refining the pathpoint set through local trajectory fitting and error optimization, and constructing a hybrid pathpoint relative pose metric-topology map; performing motion prior estimation based on Doppler velocity, combining it with LiDAR odometry information to generate an effective matching prior estimate, performing cross-modal pose estimation through a CMR network, and employing a pure tracking strategy and a PD controller for pathpoint tracking; performing geometric and intensity alignment of the forward 4D millimeter-wave radar point cloud and the omnidirectional 3D lidar point cloud through the CMR network, outputting a rotation matrix and translation vector to achieve accurate localization. This invention overcomes the perception and localization deficiencies of existing technologies under extreme weather conditions, improving localization accuracy and robustness.

Description

Technical Field

[0001] This invention relates to the field of online positioning and navigation for mobile robots, and in particular to an outdoor cross-modal robust positioning and navigation method for extreme weather conditions. Background Technology

[0002] The perception and localization systems of autonomous vehicles face severe challenges in extreme weather conditions such as heavy rain, blizzards, and dense fog. Omnidirectional 3D LiDAR generates significant noise and false point clouds due to atmospheric particulate scattering, while visual sensors suffer from reduced visibility, resulting in a significant degradation in feature extraction capabilities. This leads to a substantial decrease in the reliability of traditional single-sensor-based localization and mapping methods in such environments, posing a risk of fundamental failure. Although forward-facing 4D millimeter-wave radar can maintain high perception capabilities in severe weather, its low point cloud resolution and limited field of view make it difficult to independently achieve high-precision localization and mapping.

[0003] Existing technologies generally employ multimodal sensor fusion strategies to improve robustness, but significant problems remain: different sensor modalities have inherent differences in data alignment, feature complementarity, and system adaptation, especially under adverse conditions of unbalanced perception performance, making data coordination and fusion extremely difficult. Furthermore, limitations in application scenarios, reliance on external facilities, high system costs, or insufficient environmental adaptability also exist, making it difficult to achieve stable and efficient accurate positioning in long-term, large-scale outdoor navigation.

[0004] Therefore, there is an urgent need for a robust positioning and navigation method that can operate stably under extreme weather conditions, enabling effective collaboration among multiple sensors, independent of precise calibration, and possessing environmental adaptability, thereby effectively perceiving surrounding environmental information under extreme weather conditions and ensuring high-precision positioning and accurate navigation performance. Summary of the Invention

[0005] The purpose of this invention is to provide an outdoor cross-modal robust positioning and navigation method for extreme weather conditions, which overcomes the perception and positioning defects of existing technologies in extreme weather conditions. It can operate stably in extreme weather without relying on strict sensor calibration, and has environmental adaptability, thus improving the positioning accuracy, stability and robustness in harsh environments.

[0006] To achieve the above objectives, this invention provides an outdoor cross-modal robust positioning and navigation method for extreme weather conditions, comprising the following steps:

[0007] Step S1: Acquire forward 4D millimeter-wave radar point cloud data and omnidirectional 3D lidar point cloud data and preprocess them;

[0008] Step S2: Generate a path point set based on the CMR network, and improve the path point set through local trajectory fitting and error optimization to construct a hybrid map of path point relative pose measurement and topology.

[0009] Step S3: Motion prior estimation is performed based on the Doppler velocity in the forward 4D millimeter-wave radar point cloud data. This is combined with LiDAR odometry information to generate an effective matching prior estimate. Cross-modal pose estimation is performed through a CMR network, and path point tracking is achieved using a pure tracking strategy and a PD controller.

[0010] Step S4: Perform geometric and intensity alignment on the forward 4D millimeter-wave radar point cloud and the omnidirectional 3D lidar point cloud using the CMR network, and output the rotation matrix and translation vector to complete the precise positioning.

[0011] Preferably, in step S1, the preprocessing process includes the following steps:

[0012] Step S101: Model the raw power data of the forward 4D millimeter-wave radar point cloud and the raw intensity data of the omnidirectional 3D lidar point cloud, respectively. The specific expression for the raw power data of the forward 4D millimeter-wave radar point cloud is as follows:

[0013] ;

[0014] ;

[0015] in, This represents the raw power data of the forward-facing 4D millimeter-wave radar point cloud; Indicates intermediate variables; Indicates the effective amplitude of the target; Indicates the observation angle of the forward-facing 4D millimeter-wave radar; This indicates the transmission distance between the forward-facing 4D millimeter-wave radar and the target; This indicates the transmit power of the forward-facing 4D millimeter-wave radar; Indicates the transmit antenna gain; Indicates the receiver antenna gain; Indicates the system loss factor; Indicates the signal wavelength;

[0016] The specific expression for the point cloud intensity of omnidirectional 3D LiDAR is:

[0017] ;

[0018] ;

[0019] in, This represents the raw data of the point cloud intensity from an omnidirectional 3D lidar system. Indicates intermediate variables; This represents the maximum reflection coefficient when incident perpendicularly; Indicates the observation angle of the omnidirectional 3D lidar; This indicates the transmission distance between the omnidirectional 3D lidar and the target; The atmospheric attenuation factor represents the omnidirectional 3D lidar. Indicates the attenuation coefficient; This indicates the transmit power of the omnidirectional 3D lidar; This represents the effective aperture area of ​​the receiving antenna.

[0020] Step S102: Apply exponential attenuation compensation to the raw power data of the forward 4D millimeter-wave radar point cloud and perform a square root transform to obtain pseudo-millimeter-wave radar intensity data. The specific expression is as follows:

[0021] ;

[0022] in, This represents pseudo-millimeter-wave radar strength data; The atmospheric attenuation factor represents the forward-facing 4D millimeter-wave radar.

[0023] Step S103: Normalize the pseudo millimeter-wave radar intensity data and the original omnidirectional 3D lidar point cloud intensity data by performing maximum and minimum value normalization to obtain normalized pseudo millimeter-wave radar intensity data and normalized omnidirectional 3D lidar point cloud intensity data.

[0024] Preferably, step S2 specifically includes:

[0025] Step S201: Using the starting point and ending point of the teaching path as the initial path points, the CMR network is used to compare the registration loss between the LiDAR point cloud data and the initial forward 4D millimeter-wave radar frame frame by frame. When the registration loss exceeds the preset threshold, a new path point is inserted and the next forward 4D millimeter-wave radar frame is selected for comparison. A series of path points are generated iteratively.

[0026] Step S202: Use the path points generated in step S201 to perform local trajectory fitting on the teaching path, and use the cubic Hermite interpolation method to construct a local interpolation curve within the sliding window;

[0027] Step S203: Calculate the local trajectory fitting error. When the local trajectory fitting error exceeds the preset threshold, calculate the 3D curvature based on the relative pose of three adjacent frames provided by the LiDAR odometry. Based on the calculated 3D curvature, insert additional path points at the locations with larger curvature in the local trajectory, re-execute the local trajectory fitting in step S202, and re-evaluate the local trajectory fitting error until all local trajectory fitting errors are lower than the set threshold.

[0028] Step S204: Using omnidirectional 3D LiDAR point cloud data, construct a path point relative pose metric-topology hybrid map through the generalized iterative nearest point algorithm.

[0029] Preferably, in step S203, the specific expression for the local trajectory fitting error is:

[0030] ;

[0031] ;

[0032] ;

[0033] in, Indicates positional error; Indicates directional error; Indicates the teaching trajectory in The position at that moment; Indicates the teaching trajectory in The direction of time; Indicates the fitted trajectory at The position at that moment; Indicates the fitted trajectory at The direction of time; Indicates the weighting factor. , ; This represents the local trajectory fitting error; Indicates transpose; Indicates rotation; This represents the magnitude of the vector.

[0034] Preferably, in step S203, the specific expression for the 3D curvature is:

[0035] ;

[0036] in, , , Indicates the first The location corresponding to the frame of LiDAR point cloud data; Indicates 3D curvature; This represents the L2 norm.

[0037] Preferably, the specific content of motion prior estimation in step S3 is as follows:

[0038] Based on the Doppler velocity in the forward 4D millimeter-wave radar point cloud data, dynamic outliers are removed using RANSAC, and the static forward 4D millimeter-wave radar inlier set is retained. For each static forward 4D millimeter-wave radar point, its unit direction vector is extracted and stacked into a unit direction vector matrix. The vehicle velocity is solved using least squares and combined with the generation frequency of the forward 4D millimeter-wave radar to obtain the motion prior estimate.

[0039] Preferably, in step S3, the specific expression for the effective matching prior estimate is:

[0040] Calculate the distance between the current position and the target path point. If the distance between the current position and the target path point is less than a set threshold, it indicates that the vehicle is close enough to the target path point, and it needs to switch to the next path point as a reference, triggering the frame-switching logic. The specific expression for effective matching of prior estimation is:

[0041] ;

[0042] in, Indicates forward 4D millimeter-wave radar frame To LiDAR waypoint Valid matching prior estimates; Represents LiDAR waypoints arrive The relative pose; Indicates forward 4D millimeter-wave radar frame To LiDAR waypoint The matching output results; Indicates from time arrive Doppler motion prior estimation;

[0043] If the distance between the current location and the target path point is greater than the set threshold, it indicates that the vehicle has not yet approached the target path point and must continue to use the target path point as a reference. The frame-switching logic will not be triggered. The specific expression for effectively matching the prior estimate is:

[0044] ;

[0045] in, Indicates forward 4D millimeter-wave radar frame To LiDAR waypoint Valid matching prior estimates; Indicates forward 4D millimeter-wave radar frame To LiDAR waypoint The matching output results; Indicates from time arrive Doppler motion prior estimation.

[0046] Preferably, the specific content of step S4 is as follows:

[0047] Step S401: Using the effective matching prior estimate obtained in step S3, project the forward 4D millimeter-wave radar point cloud data onto the omnidirectional 3D lidar and expand the range by 30˚ to deal with disturbances. Then, mask the 3D lidar point cloud outside the 30˚ range to achieve preprocessing.

[0048] Step S402: Perform cylindrical projection on the occluded 3D lidar point cloud and the original point cloud of the forward 4D millimeter-wave radar respectively to obtain pseudo images of geometric features and pseudo images of intensity features.

[0049] Step S403: Use swin-transformer to perform bi-branch feature extraction, extracting multi-scale feature maps from the pseudo-images of geometric features and intensity features.

[0050] Step S404: Based on the bijective correlation transformer, coarse registration is performed using the effective matching prior estimate from step S3 to obtain a low-resolution coarse registration result.

[0051] Step S405: Divide the multi-scale feature map extracted by swin-transformer into multiple feature blocks, and perform cross-iterative optimization of the geometric feature branch and intensity feature branch through a progressive optimization strategy to output the rotation matrix and translation vector.

[0052] Step S406: Construct a loss function based on the translation and rotation components of the output, and train the CMR network by calculating the total loss function through multi-level supervision.

[0053] Preferably, in step S404, the improvement to the cross-attention mechanism of the bijective transformer is as follows:

[0054] Initial alignment results are generated using pose priors; for each forward 4D millimeter-wave radar point Searching for its spatial neighbors in LiDAR point clouds Each point is a LiDAR neighborhood point set. And calculate the forward 4D millimeter-wave radar points With LiDAR neighborhood point set The feature association, specifically expressed as:

[0055] ;

[0056] in, Represents the LiDAR neighborhood point set Features; Represents the weight matrix; This represents the attention mechanism; Indicates forward-facing 4D millimeter-wave radar point Features; This represents the fused forward 4D millimeter-wave radar point features.

[0057] For each LiDAR point Searching for its spatial neighbors in a forward-facing 4D millimeter-wave radar point cloud. Each point is a neighborhood point set of the forward 4D millimeter-wave radar. And calculate LiDAR points Neighborhood point set of forward 4D millimeter-wave radar The feature association, specifically expressed as:

[0058] ;

[0059] in, Represents the neighborhood point set of a forward-facing 4D millimeter-wave radar. Features; Represents LiDAR points Features; This represents the fused LiDAR point features;

[0060] Each forward-facing 4D millimeter-wave radar point coordinates and By stitching together the data, a joint representation of the forward-facing 4D millimeter-wave radar points is obtained. ;in, Indicates the number of forward-facing 4D millimeter-wave radar points; Indicates splicing; combining each LiDAR point coordinates and By splicing, a joint representation of LiDAR points is obtained. ;in, The number of LiDAR points is represented; then, the two joint representations are processed using the method of bijective association with the transformer in the regformer to obtain the coarse registration result.

[0061] Preferably, in step S405, the specific content of the incremental optimization strategy is as follows:

[0062] Using the coarse registration result generated in step S404, a rigid transformation is performed on the forward 4D millimeter-wave radar and LiDAR feature blocks to align them in the spatial coordinate system, resulting in an aligned feature map.

[0063] The aligned feature map is divided into The local blocks are processed independently, using the PWC structure to calculate the local transformation and then output the local transformation parameters of each local block; the PWC structure includes a pyramid feature extraction module, an optical flow deformation field estimation module, and a cost volume module.

[0064] Based on the local transformation parameters of each local block, a multi-head attention mechanism is used to calculate the feature associations between different blocks, capture the global spatial dependencies, and obtain global attention features. The local transformation parameters of each local block are concatenated with the global attention features to obtain joint features. Then, a nonlinear transformation is performed through a fully connected layer to extract high-order features. After dimensionality reduction and aggregation of information from each block through a pooling layer, the residual transformation amount is output. The residual transformation amount is then superimposed using quaternion multiplication, and after iterative optimization, the rotation matrix and translation vector are output.

[0065] Therefore, the present invention employs the above-mentioned outdoor cross-modal robust positioning and navigation method for extreme weather conditions, and the beneficial technical effects are as follows:

[0066] First, by integrating the complementary sensing characteristics of forward-facing 4D millimeter-wave radar and omnidirectional 3D lidar, cross-modal registration of forward-facing 4D millimeter-wave radar and omnidirectional 3D lidar is achieved through CMR network, realizing effective cross-modal data collaboration and effectively overcoming the problem of single sensor failure under extreme weather conditions.

[0067] Secondly, by analyzing the intensity characteristics of both omnidirectional 3D LiDAR and forward 4D millimeter-wave radar, the problem of insufficient geometric features of forward 4D millimeter-wave radar is compensated, improving the robustness of feature matching. Furthermore, by leveraging the Doppler velocity of forward 4D millimeter-wave radar to generate motion prior estimates, the matching conflict between forward 120˚ narrow field of view and omnidirectional 360˚ perception is effectively alleviated. In addition, adaptive topological discretization storage of path points is achieved through a CMR network, significantly reducing path storage requirements while maintaining positioning accuracy, effectively balancing navigation accuracy and storage efficiency. Attached Figure Description

[0068] Figure 1 This invention provides an overall algorithm framework for an outdoor cross-modal robust positioning and navigation method for extreme weather conditions.

[0069] Figure 2 This is a schematic diagram of a mobile robot device deployed using the algorithm of an embodiment of the present invention;

[0070] Figure 3 This is a flowchart of the adaptive topology discretization storage method for path points proposed in this invention;

[0071] Figure 4 This is a flowchart of the efficient matching prior estimation and path point tracking proposed in this invention;

[0072] Figure 5 This is the CMR network flowchart proposed in this invention.

[0073] Figure Labels

[0074] 1. Omnidirectional 3D LiDAR; 2. Forward-facing 4D millimeter-wave radar; 3. Data collection platform; 4. Data processing algorithm platform; 5. Power supply; 6. Load-bearing platform; 7. Mobile robot chassis; 8. Forward-facing 4D millimeter-wave radar field of view. Detailed Implementation

[0075] The technical solution of the present invention will be further described below with reference to the accompanying drawings and embodiments.

[0076] Unless otherwise defined, the technical or scientific terms used in this invention shall have the ordinary meaning as understood by one of ordinary skill in the art to which this invention pertains.

[0077] Example 1

[0078] like Figure 1 As shown, an outdoor cross-modal robust positioning and navigation method for extreme weather conditions includes the following steps:

[0079] Step S1: Acquire and preprocess forward 4D millimeter-wave radar point cloud data and omnidirectional 3D lidar point cloud data. The forward 4D millimeter-wave radar point cloud data includes raw power data and Doppler velocity; the omnidirectional 3D lidar point cloud data includes raw intensity data.

[0080] The specific expression for the raw power data of forward-facing 4D millimeter-wave radar point cloud is as follows:

[0081] ;

[0082] ;

[0083] in, This represents the raw power data of the forward-facing 4D millimeter-wave radar point cloud; Indicates intermediate variables; Indicates the effective amplitude of the target; Indicates the observation angle of the forward-facing 4D millimeter-wave radar; Transmission distance between forward-facing 4D millimeter-wave radar and target; This indicates the transmit power of the forward-facing 4D millimeter-wave radar; Indicates the transmit antenna gain; Indicates the receiver antenna gain; Indicates the system loss factor; Indicates the signal wavelength.

[0084] The specific expression for the point cloud intensity of omnidirectional 3D LiDAR is:

[0085] ;

[0086] ;

[0087] in, This represents the raw data of the point cloud intensity from an omnidirectional 3D lidar system. Indicates intermediate variables; This represents the maximum reflection coefficient when incident perpendicularly; The observation angle of an omnidirectional 3D lidar; This indicates the transmission distance between the omnidirectional 3D lidar and the target; The atmospheric attenuation factor represents the omnidirectional 3D lidar. Indicates the attenuation coefficient; This indicates the transmit power of the omnidirectional 3D lidar; This represents the effective aperture area of ​​the receiving antenna.

[0088] The preprocessing process includes the following steps:

[0089] Step S101: Apply exponential attenuation compensation and perform square root transformation on the raw power data of the forward 4D millimeter-wave radar point cloud to generate pseudo-millimeter-wave radar intensity data. The specific expression is as follows:

[0090] ;

[0091] in, This represents pseudo-millimeter-wave radar strength data; This represents the atmospheric attenuation factor of a forward-facing 4D millimeter-wave radar.

[0092] Step S102: For the pseudo-millimeter-wave radar intensity data and the original omnidirectional 3D lidar point cloud intensity data, the ratio between the two can be simplified to a constant, indicating that the two data modes are theoretically comparable. The specific expression is:

[0093] ;

[0094] Building upon this, to eliminate dimensional differences in the actual data, the pseudo-millimeter-wave radar intensity data and the original omnidirectional 3D lidar point cloud intensity data were further normalized by maximum and minimum values ​​to ensure consistent numerical distribution ranges. This resulted in normalized pseudo-millimeter-wave radar intensity data and normalized omnidirectional 3D lidar point cloud intensity data with similar distribution patterns. These operations eliminated dimensional differences between sensors, achieved cross-modal feature alignment, and provided an interpretable data foundation for subsequent CMR network (cross-modal registration network) processing.

[0095] Step S2: Generate a path point set based on the CMR network, and improve the path point set through local trajectory fitting and error optimization to construct a path point relative pose metric-topology hybrid map.

[0096] Step S201: Fix the start and end points of the teaching path as initial path points; use the initial forward 4D millimeter-wave radar frame as the registration and matching reference; use the CMR network to register and match each frame of LiDAR point cloud data in the teaching path starting from the first frame with the initial forward 4D millimeter-wave radar frame, and compare the registration loss between the LiDAR point cloud data and the initial forward 4D millimeter-wave radar frame frame by frame; when the first frame... When the registration loss between the first frame of LiDAR point cloud data and the initial forward-facing 4D millimeter-wave radar frame exceeds a preset threshold, the first frame will be... The location corresponding to the frame of LiDAR point cloud data is added to the set as a new path point; at the same time, the forward 4D millimeter-wave radar frame closest to the new path point is selected as the next registration and matching reference. A series of path points are generated iteratively according to this step to achieve the optimal registration and matching accuracy under limited storage conditions in cross-modal scenarios.

[0097] Step S202: Use the path points generated in step S201 to perform local trajectory fitting on the teaching path.

[0098] To evaluate the local trajectory fitting error, a sliding window with a size of 10 meters and a step size of 2 meters is used to traverse the LiDAR odometry trajectory. Within each sliding window, based on a given path point, a local interpolation curve is constructed using cubic Hermite interpolation to ensure the pose continuity of the local interpolation curve at each path point. The specific expression is as follows:

[0099] ;

[0100] ;

[0101] ;

[0102] ;

[0103] ;

[0104] in, Indicates the starting point of the sliding window; Indicates the endpoint of the sliding window; Indicates the tangent at the starting point; Indicates the endpoint tangent; This indicates the local interpolation curve at the interpolation parameter. The location coordinates; Basis functions representing the starting position: Basis functions representing the tangent line at the starting point; Basis functions representing the endpoint position: Basis functions representing the endpoint tangent; This represents the interpolation parameter, with a value range of [value range missing]. ,when When, the curve is located at the starting point; when At that time, the curve is located at the endpoint.

[0105] Step S203: Evaluate the local trajectory fitting error between the calculated local interpolation curve and the teaching path within the sliding window range. The specific expression is as follows:

[0106] ;

[0107] ;

[0108] ;

[0109] in, Indicates positional error; Indicates directional error; Indicates the teaching trajectory in The position at that moment; Indicates the teaching trajectory in The direction of time; Indicates the fitted trajectory at The position at that moment; Indicates the fitted trajectory at The direction of time; Indicates the weighting factor. , ; This represents the local trajectory fitting error; Indicates transpose; Indicates rotation; This represents the magnitude of the vector.

[0110] When the local trajectory fitting error exceeds a preset threshold, the 3D curvature is calculated based on the relative pose of three adjacent frames provided by the LiDAR odometry. The specific expression is as follows:

[0111] ;

[0112] in, , , Indicates the first The location corresponding to the frame of LiDAR point cloud data; Indicates 3D curvature; This represents the L2 norm.

[0113] Based on the calculated 3D curvature, additional path points are inserted at locations with high curvature in the local trajectory. Step S202 of local trajectory fitting is then re-executed, and the local trajectory fitting error is evaluated again. This process is repeated until all local trajectory fitting errors are below a set threshold.

[0114] Step S204: Record the "path point LiDAR point cloud in the teaching path" (i.e., the omnidirectional 3D LiDAR point cloud data corresponding to these locations) of a series of path points, and establish the relative pose relationship between them through the generalized iterative nearest point algorithm to construct a path point relative pose metric-topology hybrid map.

[0115] Step S3: Motion prior estimation is performed based on the Doppler velocity in the forward 4D millimeter-wave radar point cloud data. This is combined with LiDAR odometry information to generate an effective matching prior estimate. Cross-modal pose estimation is performed through a CMR network, and path point tracking is achieved using a pure tracking strategy and a PD controller.

[0116] Step S301: Perform RANSAC (Random Sample Consistency) fitting based on the Doppler velocity in the forward 4D millimeter-wave radar point cloud data to estimate the vehicle's driving speed vector, and obtain motion prior estimates using the generation frequency of the forward 4D millimeter-wave radar.

[0117] RANSAC is used to remove dynamic outliers from Doppler velocities, while retaining the static forward-looking 4D millimeter-wave radar inlier set.

[0118] For each static forward-facing 4D millimeter-wave radar point, its unit direction vector is extracted and stacked into a unit direction vector matrix. The specific expression is as follows:

[0119] ;

[0120] ;

[0121] in, Represents the unit direction vector; Represents the unit direction vector matrix; This indicates the position of each static forward-facing 4D millimeter-wave radar point relative to the sensor; Indicates an index variable; This represents the total number of static forward-facing 4D millimeter-wave radar points in the point cloud; This indicates transpose.

[0122] Depend on Solve using least squares. The motion prior estimate is obtained using the generation frequency of the forward-facing 4D millimeter-wave radar, and the specific expression is as follows:

[0123] ;

[0124] ;

[0125] in, Indicates the Doppler velocity of a static forward-looking 4D millimeter-wave radar point; Indicates the vehicle's speed; This represents the prior estimation of motion; This indicates the generation frequency of the forward-facing 4D millimeter-wave radar.

[0126] Step S302: Combine the motion prior estimate with the matching result of the previous frame and the relative pose of the LiDAR odometry to obtain an effective matching prior estimate.

[0127] Motion prior estimation characterizes the vehicle's recent motion, while the matching result of the previous frame indicates the vehicle's pose relative to the target path point at the previous moment. Combining these two factors, the vehicle's position relative to the target path point is predicted. Subsequently, if a path point switch is determined, it needs to be combined with the LiDAR odometry relative pose to ultimately obtain an effective matching prior estimate for the next path point.

[0128] Calculate the distance between the current position and the target path point. If the distance between the current position and the target path point is less than a set threshold, it indicates that the vehicle is close enough to the target path point, and it needs to switch to the next path point as a reference, triggering the frame-switching logic. The specific expression for effective matching of prior estimation is:

[0129] ;

[0130] in, Indicates forward 4D millimeter-wave radar frame To LiDAR waypoint Valid matching prior estimates; Represents LiDAR waypoints arrive The relative pose; Indicates forward 4D millimeter-wave radar frame To LiDAR waypoint The matching output results; Indicates from time arrive Doppler motion prior estimation.

[0131] If the distance between the current position and the target path point is greater than the set threshold, it indicates that the vehicle has not yet approached the target path point. The target path point should continue to be used as a reference, and the frame-switching logic should not be triggered. The tracking should continue. The specific expression for effective matching of the prior estimate is:

[0132] ;

[0133] in, Indicates forward 4D millimeter-wave radar frame To LiDAR waypoint Valid matching prior estimates; Indicates forward 4D millimeter-wave radar frame To LiDAR waypoint The matching output results; Indicates from time arrive Doppler motion prior estimation.

[0134] Step S303: Input the LiDAR point cloud of the path points in the teaching path, the forward 4D millimeter-wave radar point cloud of real-time perception during repetition, and the effective matching prior estimate into the CMR network to perform cross-modal pose estimation, obtain the matching result of the current frame, and finally use the PD controller to track the path points through a pure tracking strategy.

[0135] "Repeated real-time perception of forward 4D millimeter-wave radar point cloud" refers to the data representing the current frontal environment collected in real time by the vehicle through forward 4D millimeter-wave radar.

[0136] The specific expression is:

[0137] ;

[0138] in, Indicates the first Frame forward 4D millimeter-wave radar point cloud to the first Matching results of frame LiDAR point clouds; Indicates the first Frame forward 4D millimeter-wave radar point cloud to the first Matching prior estimation of frame LiDAR point clouds; Represents a cross-modal registration network; Represents the relative pose metric of pathpoints - the first pathpoint stored in the topological hybrid map. LiDAR point cloud of each target point; Indicates the first step in the navigation process Real-time forward-facing 4D millimeter-wave radar point cloud obtained from frame acquisition.

[0139] Then, the desired motion arc is constructed based on the pure tracking strategy, specifically expressed as follows:

[0140] ;

[0141] in, Represents the desired circular arc; Indicates the vehicle's desired linear velocity; This indicates the angular velocity provided to the PD controller; Indicates the yaw angle; This represents a position vector.

[0142] Simultaneously, using a PD controller, the required linear and angular velocities of the mobile robot are calculated and output based on the deviation between the desired motion arc and the actual motion of the robot, thereby dynamically adjusting the mobile robot's trajectory and achieving tracking.

[0143] Step S304: Take the matching result of the current frame as the new "matching result of the previous frame" and re-execute steps S302 and S303. Repeat this process until the robot reaches the final path point.

[0144] Step S4: Align the forward 4D millimeter-wave radar point cloud and the omnidirectional 3D lidar point cloud with the field of view through the CMR network, that is, align them geometrically and in terms of intensity, and output the rotation matrix and translation vector to complete the precise positioning.

[0145] Step S401: Using the effective matching prior estimate obtained in step S3, project the forward 4D millimeter-wave radar point cloud data onto the omnidirectional 3D lidar and expand the range by 30˚ to cope with disturbances. Then, mask the 3D lidar point cloud outside the 30˚ range to achieve preprocessing.

[0146] Step S402: Perform cylindrical projection on the original point cloud of the masked 3D LiDAR and the forward-facing 4D millimeter-wave radar respectively, converting the 3D point cloud data into 2D images. The projection range is centered on either the omnidirectional 3D LiDAR or the forward-facing 4D millimeter-wave radar, covering the area horizontally. The area.

[0147] Specifically, for each point in the point cloud of the pre-processed omnidirectional 3D LiDAR and forward-facing 4D millimeter-wave radar, the horizontal and vertical angles of each point are used as two-dimensional coordinates. The image is projected onto a two-dimensional grid. The resulting two-dimensional image is divided into two branches: a geometric feature branch and an intensity feature branch. In the geometric feature branch, each pixel stores the three-dimensional coordinates of that point. In the intensity feature branch, each pixel stores intensity information after nonlinear mapping alignment. After the operation, four pseudo-images are obtained: a LiDAR geometric feature image, a LiDAR intensity feature image, a forward-facing 4D millimeter-wave radar geometric feature image, and a forward-facing 4D millimeter-wave radar intensity feature image. Then, the pseudo-images of the geometric features (i.e., the LiDAR geometric feature image and the forward-facing 4D millimeter-wave radar geometric feature image) and the pseudo-images of the intensity features (i.e., the LiDAR intensity feature image and the forward-facing 4D millimeter-wave radar intensity feature image) are respectively fed into the geometric feature branch and the intensity feature branch of the CMR network for processing.

[0148] Step S403: Use the swin-transformer (shift-window visual transformer) for bi-branch feature extraction. Input the pseudo-images of geometric features and intensity features into the corresponding branches, and use the mask constructed in step S401 to exclude blank areas from attention calculation. The CMR network contains a three-layer structure. Through layer-by-layer downsampling, three sets of feature maps at different scales are extracted from each branch. Finally, the geometric feature branch outputs the initial rotation matrix and the initial translation matrix, and the intensity feature branch outputs the initial rotation matrix. Together with the initial input pseudo-images of geometric features and intensity features, these four layers together constitute four sets of multi-scale feature maps with decreasing resolution, used for subsequent progressive optimization.

[0149] Step S404: Based on the bijective correlation transformer, using the effective matching prior estimate obtained in step S3, coarse registration is performed on the initial rotation matrix and the initial translation matrix. The cross-attention module can combine the effective matching prior estimate to narrow the search range of point cloud matching from all target points to the nearest neighbor target points of the source point, thereby accelerating the cross-attention process and significantly improving computational efficiency while ensuring registration accuracy. Finally, a low-resolution coarse registration result is obtained, i.e., coarse registration rotation estimate. Coarse registration translation estimation .

[0150] The CMR network used in this embodiment is an end-to-end deep learning model. Its overall architecture is based on the Regformer network (an efficient Transformer network for large-scale point cloud registration) and has been improved to adapt to cross-modal registration tasks under extreme weather conditions. The bijective correlation transformer in the CMR network is the core for feature association and coarse registration. This embodiment improves the cross-attention mechanism of the bijective correlation transformer, as detailed below:

[0151] Initial alignment results are generated using pose priors. For each forward-facing 4D millimeter-wave radar point... Instead of performing calculations with all LiDAR point cloud data, it searches for spatially neighboring points within the LiDAR point cloud. Each point is a LiDAR neighborhood point set. Then calculate the forward 4D millimeter-wave radar points. With LiDAR neighborhood point set The feature association, specifically expressed as:

[0152] ;

[0153] in, Represents the LiDAR neighborhood point set Features; Represents the weight matrix; This represents the attention mechanism; Indicates forward-facing 4D millimeter-wave radar point Features; This represents the fused forward 4D millimeter-wave radar point features.

[0154] For each LiDAR point Searching for its spatial neighbors in a forward-facing 4D millimeter-wave radar point cloud. Each point is a neighborhood point set of the forward 4D millimeter-wave radar. And calculate LiDAR points Neighborhood point set of forward 4D millimeter-wave radar The feature association, specifically expressed as:

[0155] ;

[0156] in, Represents the neighborhood point set of a forward-facing 4D millimeter-wave radar. Features; Represents LiDAR points Features; This represents the fused LiDAR point features.

[0157] Each forward-facing 4D millimeter-wave radar point coordinates and By stitching together the data, a joint representation of the forward-facing 4D millimeter-wave radar points is obtained. ;in, Indicates the number of forward-facing 4D millimeter-wave radar points; This indicates stitching. Each LiDAR point... coordinates and By splicing, a joint representation of LiDAR points is obtained. ;in, This represents the number of LiDAR points. Then, a bijective correlation transformer is used to process the two joint representations to obtain the coarse registration result.

[0158] Step S405: Divide the multi-scale feature map extracted by the Swin-transformer into multiple feature blocks and adopt a progressive optimization strategy of "coarse registration, feature alignment, local optimization, global optimization, cross compensation, and residual stacking". The progressive optimization strategy is based on the coarse registration result of step S404, and iteratively optimizes the multi-scale feature map with higher resolutions step by step, gradually improving the registration accuracy. By refining local feature matching through block processing and fusing multi-scale information using an attention mechanism, after three iterations of the progressive optimization strategy, four different resolutions of registration results from coarse to fine are finally output. The registration results include a rotation matrix. With translation vector .

[0159] The details of the incremental optimization strategy are as follows:

[0160] Using the coarse registration result generated in step S404, i.e., coarse registration rotation estimation Coarse registration translation estimation A rigid transformation is performed on the forward-facing 4D millimeter-wave radar and LiDAR feature blocks to align them in the spatial coordinate system, resulting in an aligned feature map.

[0161] The aligned feature map is divided into The system processes local blocks independently, using a PWC (Precision Waveform Control) architecture to calculate local transformations and output the local transformation parameters for each block. The PWC architecture includes a pyramid feature extraction module, an optical flow deformation field estimation module, and a cost volume module. Therefore, the calculation of local transformations involves: first, extracting multi-scale features from coarse to fine using the pyramid feature extraction module; then, calculating the local offsets between adjacent blocks using the optical flow deformation field estimation module; and finally, evaluating the inter-block matching degree and selecting reliable corresponding points using the cost volume module.

[0162] Based on the local transformation parameters of each local block, a multi-head attention mechanism is used to calculate the feature associations between different blocks, capture global spatial dependencies, and obtain global attention features.

[0163] The local transformation parameters of each local block are concatenated with the global attention features to obtain joint features. Then, a fully connected layer is used for nonlinear transformation to extract high-order features. Finally, a pooling layer is used to reduce the dimensionality of each block and aggregate the information to output the residual transformation. and .

[0164] Bundle and Coarse registration rotation estimation Coarse registration translation estimation The residuals are superimposed using quaternion multiplication, and the superimposed residuals are then fed into the next optimization iteration until the optimization is complete, at which point the rotation matrix is ​​output. With translation vector The specific expression is:

[0165] ;

[0166] .

[0167] in, Indicates the number of levels.

[0168] Step S406, due to the rotation matrix With translation vector Due to the difference in amplitude and unit, the translation and rotation components in the loss function are decoupled, and two learnable parameters are introduced. and .

[0169] The CMR network outputs registration results at four different resolutions, corresponding to four different layers. Each layer has an independent training loss function, the specific expression of which is:

[0170] ;

[0171] in, This represents the training loss function for each layer; This represents the matching rotation matrix of the output of each level of the CMR network; This represents the rotation matrix of the GT at each level; GT represents the cross-modal matching ground truth obtained from the calibration results of LiDAR and forward 4D millimeter-wave radar and the matching results of LiDAR itself. Represents an exponential function; This represents the translation vector of the output at each level of the CMR network. This represents the translation vector of the ground truth at each level; , This represents the learnable parameters of each layer in the CMR network.

[0172] The total loss function is calculated using multi-level supervision, and its specific expression is as follows:

[0173] ;

[0174] in, Represents the total loss function; This indicates the weight of each level.

[0175] Based on the above method, a specific application implementation scheme is provided, which deploys the above method in a mobile robot. A schematic diagram of the mobile robot device is shown below. Figure 2 As shown, it includes an omnidirectional 3D lidar 1, a forward-facing 4D millimeter-wave radar 2, a data collection platform 3, a data processing algorithm platform 4, a power supply 5, a load-bearing platform 6, a mobile robot chassis 7, and a forward-facing 4D millimeter-wave radar field of view 8.

[0176] Among them, the omnidirectional 3D LiDAR 1, the forward 4D millimeter-wave radar 2, the data collection platform 3, the data processing algorithm platform 4, and the power supply 5 are set on the load-bearing platform 6, which is fixed on the mobile robot chassis 7. The omnidirectional 3D LiDAR 1 and the forward 4D millimeter-wave radar 2 are both placed in the center from the forward-looking perspective.

[0177] During the pre-training phase, the mobile robot was manually controlled to simultaneously collect and record omnidirectional 3D LiDAR and forward 4D millimeter-wave radar data in different scenarios. Each frame of omnidirectional 3D LiDAR data was time-aligned with the previous N frames of forward 4D millimeter-wave radar data using timestamp synchronization and known external calibration parameters. To improve generalization ability, random rotational and translational perturbations were applied to the motion prior estimates from the forward 4D millimeter-wave radar Doppler velocity measurements to enhance data diversity. The obtained paired sample data was used for the CMR network.

[0178] During the teaching phase, the mobile robot is manually driven along the desired working path, simultaneously recording data from the omnidirectional 3D LiDAR and the forward-facing 4D millimeter-wave radar. Subsequently, based on this data, the omnidirectional 3D LiDAR pathpoint selection operation is performed using a CMR network, as detailed in the following process: Figure 3 As shown.

[0179] Path point selection is divided into two stages: In the first stage, the start and end points of the teaching path are used as initial path points. Starting from the start point, the CMR network is used frame by frame to evaluate the registration loss between subsequent LiDAR frames and the initial forward 4D millimeter-wave radar frame. If the registration loss exceeds a set threshold, the frame preceding the current frame is set as the new path point, and the registration matching benchmark is updated to the forward 4D millimeter-wave radar frame closest to the new path point. This process is repeated until all data is traversed, ultimately generating a sparse and high-precision matched set of path points.

[0180] The second stage first performs three Hermite interpolations on adjacent path points to generate smooth local interpolation curves. Then, it compares the trajectory fitting errors between the local interpolation curves and the original teaching path, calculates the 3D curvature in the region of maximum error, and inserts a new path point at the location of maximum curvature. This process of "interpolation-evaluation-insertion of new path points" is iteratively optimized until all local trajectory fitting errors are below a set threshold. Finally, based on the LiDAR point clouds corresponding to these path points and their continuous pose relationships, a hybrid map of path point relative pose measurement and topology is constructed.

[0181] During the repetition phase, the mobile robot is first placed at the starting point of the taught trajectory, and then... Figure 4 The process shown is as follows. To reduce data loading latency, the system starts a separate thread before beginning pathpoint tracking to preload the LiDAR point cloud data corresponding to subsequent pathpoints.

[0182] Whenever the mobile robot receives new forward-facing 4D millimeter-wave radar data, it first calculates a motion prior estimate based on the Doppler velocity and the time difference between two frames. This motion prior estimate is then combined with the matching result of the previous frame and the relative pose from the LiDAR odometry to generate an effective matching prior estimate. This estimate is then input into the CMR network for cross-modal pose estimation, and finally outputs an accurate current pose estimate relative to the nearest path point.

[0183] After obtaining the current pose estimate, a circular trajectory connecting the current pose estimate and the target path point is constructed based on a pure tracking strategy. Stable path tracking is achieved by maintaining a fixed ratio between linear velocity and angular velocity. Finally, the linear velocity and angular velocity commands required for actual control of the mobile robot are calculated by the PD controller.

[0184] The specific steps for cross-modal registration of forward-facing 4D millimeter-wave radar and omnidirectional 3D lidar via CMR network are as follows: Figure 5 As shown. During path point tracking, the LiDAR point cloud corresponding to the target path point and the forward 4D millimeter-wave radar cloud acquired in real time are jointly input into the CMR network for field-of-view occlusion and intensity nonlinear mapping preprocessing, generating four pseudo images: LiDAR geometric feature image, LiDAR intensity feature image, forward 4D millimeter-wave radar geometric feature image, and forward 4D millimeter-wave radar intensity feature image.

[0185] Subsequently, the pseudo-images of geometric features (i.e., LiDAR geometric feature images and forward-facing 4D millimeter-wave radar geometric feature images) and pseudo-images of intensity features (i.e., LiDAR intensity feature images and forward-facing 4D millimeter-wave radar intensity feature images) are fed into the geometric feature branch and intensity feature branch of the CMR network for processing, respectively. In each branch, a swin-transformer is first used to extract multi-scale feature maps from the pseudo-images of geometric features and intensity features, and a mask is used to exclude blank areas from attention calculation. Next, coarse registration is performed on the low-resolution pseudo-images based on pose priors. On this basis, high-resolution feature maps are further utilized, and a PWC structure is used to progressively optimize the registration accuracy, finally outputting a refined registration result.

[0186] To verify the comprehensive performance of the method of this invention in long-distance, complex scenarios, seven experimental tracks (tracks A to G) with different environmental challenges were set up on a university campus. LiDAR point cloud data and forward-facing 4D millimeter-wave radar point cloud data were collected for tracks A to G. Track A covers a large area of ​​heterogeneous environment and is used to test long-distance stability. Track B includes subtle dynamic features such as moderate slopes and parking areas, testing adaptability to environments with small changes. Track C circles a teaching building with high pedestrian traffic, highlighting stability under dynamic and partially degraded conditions. Track D travels along a riverbank with sparse geometric features, used to evaluate positioning capabilities in feature-scarce scenarios. Track E passes through a narrow corridor restricting lateral movement, focusing on local accuracy testing. Track F traverses a highly repetitive open scene that easily leads to sensor degradation, testing system stability. Track G includes rugged terrain with rapid elevation changes, testing robustness to physical disturbances.

[0187] For trajectory A, using navigation success rate and positioning accuracy as evaluation benchmarks, four types of methods are evaluated and compared: global mapping methods based on SLAM (Simultaneous Localization and Mapping), LiDAR same-modal registration methods, forward 4D millimeter-wave radar same-modal registration methods, and cross-modal registration methods. Among them, SLAM-based global mapping methods include LIO-SAM (Laser Inertial Odometry Based on Smoothing and Mapping); LiDAR same-modal registration methods include GICP (Inter-Covariance Based ICP) and Regformer (Transformer-based registration network); forward 4D millimeter-wave radar same-modal registration methods include GICP (Generalized Iterative Closest Point), ADPGICP (Adaptive Dynamic Point Cloud Generalized Iterative Closest Point), and NDT (Normal Distribution Transformation); and cross-modal registration methods include CT-ICP, GICP, the method of this invention (without intensity branching), and the method of this invention.

[0188] Table 1 Comparison of navigation performance in trajectory A

[0189]

[0190]

[0191] The comparative data is shown in Table 1. The data excluding certain sections in Table 1 represents navigation task failures of this method. From the data in Table 1, it can be concluded that, in terms of navigation success rate, only the SLAM-based global map method, the LiDAR same-modal registration method, the method of this invention (without intensity branch), and the method of this invention successfully completed all repetitions of trajectory A. Therefore, further evaluation was conducted on trajectories B through G. The forward 4D millimeter-wave radar same-modal registration method and the cross-modal registration method failed to maintain stable registration in long-distance navigation, thus demonstrating the significant superiority of the method of this invention in terms of positioning accuracy.

[0192] For trajectories B to G, the method of this invention is compared with the global map method based on SLAM, the LiDAR same-modal registration method, and the method of this invention (without intensity branch). The comparison data are shown in Tables 2 and 3.

[0193] Table 2. Comparison of Absolute Position Error and Standard Deviation of Absolute Position Error for Different Methods

[0194]

[0195] Table 3. Comparison of Absolute Heading Error and Standard Deviation of Absolute Heading Error for Different Methods

[0196]

[0197] It can be seen that while SLAM-based global map methods possess high global accuracy and consistency, cumulative drift weakens local accuracy, leading to collisions with obstacles in complex and narrow environments (such as narrow passages between poles), whereas the method of this invention can successfully traverse these obstacles. LiDAR co-modal registration methods mitigate drift through discrete pathpoint relocalization, but in highly dynamic scenarios such as parking areas and pedestrian zones, large positioning drift and trajectory inconsistencies occur due to interference from moving targets. In contrast, the method of this invention, leveraging the beyond-line-of-sight perception capabilities of forward-facing 4D millimeter-wave radar, maintains high repeatability and achieves successful navigation. Furthermore, the method of this invention (without intensity branches) performs poorly, even failing on trajectories E and G, confirming the necessity of aligning the nonlinear mapping between the power of the forward-facing 4D millimeter-wave radar and the intensity of the LiDAR. Therefore, the method of this invention, utilizing a CMR network, achieves stable and accurate positioning comparable to the LiDAR co-modal registration method, with optimal performance in some absolute position and absolute heading error indicators, and can handle situations that other methods struggle with, thus improving both stability and accuracy.

[0198] The method of this invention is compared with the traditional fixed threshold strategy on trajectory A, and the results are measured from two dimensions: navigation accuracy and storage efficiency. The specific expressions of the relevant evaluation indicators are as follows:

[0199] ;

[0200] ;

[0201] in, Indicates absolute position error; Indicates absolute heading error; Indicates the number of path points; Evaluation metrics related to absolute position error; This refers to the evaluation index related to absolute heading error.

[0202] The comparative data is shown in Table 4. It can be seen that the method of this invention achieves the best balance between positioning accuracy and storage efficiency, with both evaluation indicators... , All values ​​are the highest, further demonstrating the superiority of the method of the present invention.

[0203] Table 4 Performance Comparison of Path Point Selection Methods on Trajectory A

[0204]

[0205] To demonstrate the advantages of the method of the present invention under conditions of decreased perception, in a Navigation tests were conducted in a closed experimental environment under smog. The experimental procedure was as follows: First, the teaching path was recorded under clear weather conditions; then, in the repetition phase, dense chemical smog was released to simulate an extremely harsh perception degradation environment. Throughout the experiment, the absolute position error of the method of this invention remained within 10 cm, and the absolute navigation error remained within 5°, verifying the strong robustness and anti-interference capability of the method under extremely harsh conditions. The above experimental results show that, in this scenario, the perception capability of the forward-facing 4D millimeter-wave radar remained stable, while the performance of the omnidirectional 3D lidar was significantly reduced due to smoke obstruction and scattering effects. The method of this invention successfully completed the navigation task, further verifying its strong robustness and anti-interference capability under extremely harsh conditions.

[0206] Therefore, the present invention adopts the above-mentioned outdoor cross-modal robust positioning and navigation method for extreme weather conditions, which overcomes the perception and positioning defects of the existing technology under extreme weather conditions. It can operate without relying on strict sensor calibration, has environmental adaptability, and can operate stably in extreme weather, significantly improving the positioning accuracy, stability and robustness in harsh environments.

[0207] Finally, it should be noted that 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 or equivalent substitutions can still be made to the technical solutions of the present invention, and these modifications or equivalent substitutions cannot cause the modified technical solutions to deviate from the spirit and scope of the technical solutions of the present invention.

Claims

1. An outdoor cross-modal robust positioning and navigation method against extreme adverse weather, characterized in that, Includes the following steps: Step S1: Acquire forward 4D millimeter-wave radar point cloud data and omnidirectional 3D lidar point cloud data and preprocess them; Exponential attenuation compensation is applied to the raw power data of the forward 4D millimeter-wave radar point cloud, and square root transformation is performed to obtain pseudo millimeter-wave radar intensity data. Step S2: Generate a path point set based on the CMR network, and improve the path point set through local trajectory fitting and error optimization to construct a hybrid map of path point relative pose measurement and topology. Step S3: Motion prior estimation is performed based on the Doppler velocity in the forward 4D millimeter-wave radar point cloud data. This is combined with LiDAR odometry information to generate an effective matching prior estimate. Cross-modal pose estimation is performed through a CMR network, and path point tracking is achieved using a pure tracking strategy and a PD controller. In step S3, the specific details of motion prior estimation are as follows: Based on the Doppler velocity in the forward 4D millimeter-wave radar point cloud data, dynamic outliers are removed using RANSAC, and the static forward 4D millimeter-wave radar inlier set is retained. For each static forward 4D millimeter-wave radar point, its unit direction vector is extracted and stacked into a unit direction vector matrix. The vehicle velocity is solved using least squares and combined with the generation frequency of the forward 4D millimeter-wave radar to obtain the motion prior estimate. Step S4: Perform geometric and intensity alignment on the forward 4D millimeter-wave radar point cloud and the omnidirectional 3D lidar point cloud using the CMR network, and output the rotation matrix and translation vector to complete the precise positioning.

2. The outdoor cross-modal robust positioning and navigation method against extreme adverse weather according to claim 1, characterized in that, In step S1, the preprocessing process includes the following steps: Step S101: Model the raw power data of the forward 4D millimeter-wave radar point cloud and the raw intensity data of the omnidirectional 3D lidar point cloud, respectively. The specific expression for the raw power data of the forward 4D millimeter-wave radar point cloud is as follows: ； ； in, This represents the raw power data of the forward-facing 4D millimeter-wave radar point cloud; Indicates intermediate variables; Indicates the effective amplitude of the target; Indicates the observation angle of the forward-facing 4D millimeter-wave radar; This indicates the transmission distance between the forward-facing 4D millimeter-wave radar and the target; This indicates the transmit power of the forward-facing 4D millimeter-wave radar; Indicates the transmit antenna gain; Indicates the receiver antenna gain; Indicates the system loss factor; Indicates the signal wavelength; The specific expression for the point cloud intensity of omnidirectional 3D LiDAR is: ； ； in, This represents the raw data of the point cloud intensity from an omnidirectional 3D lidar system. Indicates intermediate variables; This represents the maximum reflection coefficient when incident perpendicularly; Indicates the observation angle of the omnidirectional 3D lidar; This indicates the transmission distance between the omnidirectional 3D lidar and the target; The atmospheric attenuation factor represents the omnidirectional 3D lidar. Indicates the attenuation coefficient; This indicates the transmit power of the omnidirectional 3D lidar; This represents the effective aperture area of ​​the receiving antenna. Step S102, the specific expression for the pseudo-millimeter-wave radar intensity data is as follows: ； in, This represents pseudo-millimeter-wave radar strength data; The atmospheric attenuation factor represents the forward-facing 4D millimeter-wave radar. Step S103: Normalize the pseudo millimeter-wave radar intensity data and the original omnidirectional 3D lidar point cloud intensity data by performing maximum and minimum value normalization to obtain normalized pseudo millimeter-wave radar intensity data and normalized omnidirectional 3D lidar point cloud intensity data.

3. The outdoor cross-modal robust positioning and navigation method for extreme weather conditions according to claim 1, characterized in that, Step S2 specifically includes: Step S201: Using the starting point and ending point of the teaching path as the initial path points, the CMR network is used to compare the registration loss between the LiDAR point cloud data and the initial forward 4D millimeter-wave radar frame frame by frame. When the registration loss exceeds the preset threshold, a new path point is inserted and the next forward 4D millimeter-wave radar frame is selected for comparison. A series of path points are generated iteratively. Step S202: Use the path points generated in step S201 to perform local trajectory fitting on the teaching path, and use the cubic Hermite interpolation method to construct a local interpolation curve within the sliding window; Step S203: Calculate the local trajectory fitting error. When the local trajectory fitting error exceeds the preset threshold, calculate the 3D curvature based on the relative pose of three adjacent frames provided by the LiDAR odometry. Based on the calculated 3D curvature, insert additional path points at the locations with larger curvature in the local trajectory, re-execute the local trajectory fitting in step S202, and re-evaluate the local trajectory fitting error until all local trajectory fitting errors are lower than the set threshold. Step S204: Using omnidirectional 3D LiDAR point cloud data, construct a path point relative pose metric-topology hybrid map through the generalized iterative nearest point algorithm.

4. The outdoor cross-modal robust positioning and navigation method for extreme weather conditions according to claim 3, characterized in that, In step S203, the specific expression for the local trajectory fitting error is as follows: ； ； ； in, Indicates positional error; Indicates directional error; Indicates the teaching trajectory in The position at that moment; Indicates the teaching trajectory in The direction of time; Indicates the fitted trajectory at The position at that moment; Indicates the fitted trajectory at The direction of time; Indicates the weighting factor. , ; This represents the local trajectory fitting error; Indicates transpose; Indicates rotation; This represents the magnitude of the vector.

5. The outdoor cross-modal robust positioning and navigation method for extreme weather conditions according to claim 4, characterized in that, In step S203, the specific expression for 3D curvature is: ； in, , , Indicates the first The location corresponding to the frame of LiDAR point cloud data; Indicates 3D curvature; This represents the L2 norm.

6. The outdoor cross-modal robust positioning and navigation method for extreme weather conditions according to claim 1, characterized in that, In step S3, the specific expression for the effective matching prior estimate is: Calculate the distance between the current position and the target path point. If the distance between the current position and the target path point is less than a set threshold, it indicates that the vehicle is approaching the target path point, and it needs to switch to the next path point as a reference, triggering the frame-switching logic. The specific expression for effective matching of prior estimation is: ； in, Indicates forward 4D millimeter-wave radar frame To LiDAR waypoint Valid matching prior estimates; Represents LiDAR waypoints arrive The relative pose; Indicates forward 4D millimeter-wave radar frame To LiDAR waypoint The matching output results; Indicates from time arrive Doppler motion prior estimation; If the distance between the current location and the target path point is greater than the set threshold, it indicates that the vehicle has not yet approached the target path point and must continue to use the target path point as a reference. The frame-switching logic will not be triggered. The specific expression for effectively matching the prior estimate is: ； in, Indicates forward 4D millimeter-wave radar frame To LiDAR waypoint Valid matching prior estimates; Indicates forward 4D millimeter-wave radar frame To LiDAR waypoint The matching output results; Indicates from time arrive Doppler motion prior estimation.

7. The outdoor cross-modal robust positioning and navigation method for extreme weather conditions according to claim 1, characterized in that, The specific content of step S4 is as follows: Step S401: Using the effective matching prior estimate obtained in step S3, project the forward 4D millimeter-wave radar point cloud data onto the omnidirectional 3D lidar and expand the range by 30˚ to deal with disturbances. Then, mask the 3D lidar point cloud outside the 30˚ range to achieve preprocessing. Step S402: Perform cylindrical projection on the occluded 3D lidar point cloud and the original point cloud of the forward 4D millimeter-wave radar respectively to obtain pseudo images of geometric features and pseudo images of intensity features. Step S403: Use swin-transformer to perform bi-branch feature extraction, extracting multi-scale feature maps from the pseudo-images of geometric features and intensity features. Step S404: Based on the bijective correlation transformer, coarse registration is performed using the effective matching prior estimate from step S3 to obtain a low-resolution coarse registration result. Step S405: Divide the multi-scale feature map extracted by swin-transformer into multiple feature blocks, and perform cross-iterative optimization of the geometric feature branch and intensity feature branch through a progressive optimization strategy to output the rotation matrix and translation vector. Step S406: Construct a loss function based on the translation and rotation components of the output, and train the CMR network by calculating the total loss function through multi-level supervision.

8. The outdoor cross-modal robust positioning and navigation method for extreme weather conditions according to claim 7, characterized in that, In step S404, the improvements to the cross-attention mechanism of the bijective transformer are as follows: Initial alignment results are generated using pose priors; for each forward 4D millimeter-wave radar point Searching for its spatial neighbors in LiDAR point clouds Each point is a LiDAR neighborhood point set. And calculate the forward 4D millimeter-wave radar points With LiDAR neighborhood point set The feature association, specifically expressed as: ； in, Represents the LiDAR neighborhood point set Features; Represents the weight matrix; This represents the attention mechanism; Indicates forward-facing 4D millimeter-wave radar point Features; This represents the fused forward 4D millimeter-wave radar point features. For each LiDAR point Searching for its spatial neighbors in a forward-facing 4D millimeter-wave radar point cloud. Each point is a neighborhood point set of the forward 4D millimeter-wave radar. And calculate LiDAR points Neighborhood point set of forward 4D millimeter-wave radar The feature association, specifically expressed as: ； in, Represents the neighborhood point set of a forward-facing 4D millimeter-wave radar. Features; Represents LiDAR points Features; This represents the fused LiDAR point features; Each forward-facing 4D millimeter-wave radar point coordinates and By stitching together the data, a joint representation of the forward-facing 4D millimeter-wave radar points is obtained. ;in, Indicates the number of forward-facing 4D millimeter-wave radar points; Indicates splicing; combining each LiDAR point coordinates and By splicing, a joint representation of LiDAR points is obtained. ;in, The number of LiDAR points is represented; then, the two joint representations are processed using the method of bijective association with the transformer in the regformer to obtain the coarse registration result.

9. The outdoor cross-modal robust positioning and navigation method for extreme weather conditions according to claim 7, characterized in that, In step S405, the specific details of the incremental optimization strategy are as follows: Using the coarse registration result generated in step S404, a rigid transformation is performed on the forward 4D millimeter-wave radar and LiDAR feature blocks to align them in the spatial coordinate system, resulting in an aligned feature map. The aligned feature map is divided into The local blocks are processed independently, using the PWC structure to calculate the local transformation and then output the local transformation parameters of each local block; the PWC structure includes a pyramid feature extraction module, an optical flow deformation field estimation module, and a cost volume module. Based on the local transformation parameters of each local block, a multi-head attention mechanism is used to calculate the feature associations between different blocks, capture the global spatial dependencies, and obtain global attention features. The local transformation parameters of each local block are concatenated with the global attention features to obtain joint features. Then, a nonlinear transformation is performed through a fully connected layer to extract high-order features. After dimensionality reduction and aggregation of information from each block through a pooling layer, the residual transformation amount is output. The residual transformation amount is then superimposed using quaternion multiplication, and after iterative optimization, the rotation matrix and translation vector are output.

Images