Millimeter wave radar inertial navigation positioning and mapping method, equipment and medium

Abstract

According to the invention, a set of heterogeneous information cross fusion scheme based on rotation is designed, space structure information of a millimeter wave radar inter-frame spectrogram can be enhanced, and position ambiguity brought by limited angle resolution to space landmark feature extraction is relieved. A space consistency landmark extraction and association scheme is designed, a robust and dense landmark feature point cloud is extracted from a millimeter wave radar spectrogram rich in external space information, and the influence of millimeter wave point cloud sparsity and clutter point cloud can be effectively reduced. Differential self-velocity estimation is provided, sub-pixel-level radial velocity is accurately extracted from a Doppler spectrogram, the self-velocity is accurately estimated, a loss function is constructed based on geometry, velocity consistency and Doppler velocity constraint and used for training a landmark extraction network in a self-supervised mode, and therefore accurate positioning and robust mapping are achieved. The method can be seamlessly adapted to a complex and wide traffic driving environment without positioning truth value supervision, and stable and reliable state estimation and landmark extraction are realized.

Description

Technical Field

[0001] The present application relates to the technical field of autonomous navigation and environmental perception of unmanned systems, and more specifically, to a millimeter-wave radar inertial navigation positioning and mapping method, equipment, and medium. Background Art

[0002] With the rapid development of artificial intelligence (AI) technology, autonomous driving has become one of the most promising areas of innovation in the global transportation sector. The core goal of autonomous driving is to enable autonomous vehicle operation through perception, decision-making, and control. High-precision positioning and environmental reconstruction technologies are key cornerstones for achieving this goal. In complex and dynamic traffic scenarios, vehicles must acquire their own position (location and attitude) in real time and construct a three-dimensional map of their surroundings to ensure safe path planning and obstacle avoidance. Traditional positioning systems rely on GNSS, LiDAR, or camera sensors. However, these sensors are susceptible to signal obstruction, lighting changes, and inclement weather, resulting in positioning drift or mapping failure. Therefore, achieving robust, highly reliable positioning and mapping technology around the clock has become a core challenge for the implementation of autonomous driving and a prerequisite for ensuring its safety and reliability.

[0003] With the development of radar imaging technology, the combined positioning system of millimeter-wave radar (mmWave Radar) and inertial navigation system (INS) has shown unique application potential. Compared with the easy obstruction of GNSS signals, the high cost and low penetration ability of lidar, and the light sensitivity of cameras, millimeter-wave radar has become a reliable choice for positioning perception in complex environments due to its all-weather operation ability (penetrating rain, fog, and dust) and low-cost mass production advantages. As a low-cost internal perception sensor, the inertial navigation system is not affected by external climate and light, and provides the vehicle with high-speed acceleration and angular velocity information. This technical route significantly reduces the dependence of autonomous driving on high-cost hardware, while improving the system's adaptability in extreme weather and complex road conditions. It provides an economical and robust solution for the large-scale commercial deployment of autonomous driving, becoming an important driving force for the industry to move towards L4 / L5 high-level autonomous driving.

[0004] Traditional millimeter-wave radar inertial positioning solutions mainly focus on the following directions: (1) One is to use IMU (inertial measurement unit) observations in the state propagation equation and solve the odometer of the radar inertial navigation system through scan matching between millimeter-wave radar frames. However, this solution requires high spatial resolution of millimeter-wave radar and is not suitable for low-cost single-chip radars; (2) Another solution is to use the internal perception IMU to model the state propagation process of the vehicle, use the Doppler velocity of the millimeter-wave radar as the observation constraint, and fuse multi-source information through Kalman filter or probability factor graph optimization model. However, this solution is limited by the quality of millimeter-wave radar feature point cloud. Due to the limited size of commercial radar hardware, the acquired point cloud is usually very sparse and is affected by clutter points caused by multipath effect and mirror reflection effect, resulting in low robustness of this solution and difficulty in applying to a wide range of complex driving scenarios. (3) Another solution is to perform end-to-end training and learning through deep neural networks. However, this method requires the collection of extensive training samples and a large amount of manual annotation, and generalization is difficult to guarantee.

[0005] Therefore, how to achieve more stable and reliable radar inertial navigation positioning and mapping in more complex and extensive environments is a technical issue that urgently needs to be studied in all-weather autonomous driving systems. Summary of the Invention

[0006] In response to at least one defect or improvement need in the prior art, the present application provides a millimeter-wave radar inertial navigation positioning and mapping method, device and medium to achieve more stable and reliable radar inertial navigation positioning and mapping in more complex and extensive environments.

[0007] To achieve the above objectives, in a first aspect, the present application provides a millimeter wave radar inertial navigation positioning and mapping method, comprising:

[0008] By pre-integrating the millimeter-wave radar data frames between frames of the inertial measurement unit, the dot-product attention mechanism is constructed using the rotation angle difference to generate the expectation matrix between adjacent radar frames, realizing the cross-fusion of heterogeneous information to enhance the spatial structure of the radar spectrogram.

[0009] The U-Net backbone network is used to extract multi-scale features from the fused radar spectra. The spatial position, confidence weight, and feature descriptor of sub-pixel landmarks are output through a multi-head decoding structure, and landmark feature matching is completed by combining the cross-frame attention mechanism.

[0010] The radial velocity of candidate landmarks is estimated based on a sub-pixel Doppler velocity soft query algorithm. The instantaneous self-velocity is calculated using a differentiable optimization algorithm, combining the geometric relationship between the radial velocity of static landmarks and the radar self-velocity.

[0011] A joint loss function including geometric consistency constraints, Doppler velocity constraints, and velocity consistency constraints is constructed to train the feature extraction network in a self-supervised manner.

[0012] The spatially consistent radar landmark point cloud is extracted through the trained feature extraction network, and the self-velocity estimation results are integrated to achieve real-time positioning and mapping.

[0013] Furthermore, the inter-frame pre-integration of the inertial measurement unit includes:

[0014] According to the linear acceleration measurement of the inertial measurement unit Angular velocity measurement Linear acceleration measurement bias b a and angular velocity measurement bias b ω , calculate the relative position between adjacent radar frames speed and attitude quaternion The specific formulas include:

[0015]

[0016] in, Represents the rotation matrix in the inertial measurement unit coordinate system; Represents the quaternion multiplication operator; Indicates the relative attitude change of the inertial measurement unit from the kth frame to the current time t.

[0017] Furthermore, the implementation of the dot product attention mechanism includes:

[0018] The k-th frame radar spectrum The azimuth angle representation η=(η1,…,η n ) and the rotation angle The difference between the kth frame and the k+1th frame is used as a metric to generate the expected vector

[0019]

[0020] Among them, κ represents the annealing coefficient, and the expected matrix is aligned by the scale operation Fusion with the original spectrum to form an enhanced spectrum

[0021] Furthermore, the spatial position, confidence weight and feature descriptor of sub-pixel landmarks are output through the multi-head decoding structure, including:

[0022] In the local pixel block of the radar spectrum According to the detection score map Perform Softmax weighted summation. The specific formulas include:

[0023]

[0024] Among them, (u ij , v ij ) represents pixel coordinates; (u k , v k ) represents sub-pixel coordinates;

[0025] Output normalized confidence weights from the confidence header of the multi-head decoding structure To represent the confidence of each pixel being selected as a candidate landmark;

[0026] The encoder backbone block features are collected from the descriptor header of the multi-head decoding structure and used as descriptors to uniquely identify candidate landmark features.

[0027] Furthermore, combining the cross-frame attention mechanism to complete landmark feature matching includes:

[0028] The radar landmark candidate features between frames are matched using the attention mechanism:

[0029]

[0030] Among them, V′ k Represents the position matrix of the candidate landmarks in the target frame; V represents the position matrix containing all pixel coordinates; represents the query matrix consisting of N candidate landmark descriptors of the k-1th frame; represents the key matrix composed of pixel descriptors of the full image of the kth frame; κ represents the annealing coefficient that adjusts the sharpness of the attention weight distribution; H and W represent the distance height and azimuth width of the radar spectrum, respectively.

[0031] Furthermore, estimating the radial velocity of the candidate landmark based on the sub-pixel Doppler velocity soft query algorithm includes:

[0032] According to the sub-pixel coordinates of the candidate landmarks, the radial velocity v is extracted from the Doppler spectrum by weighted soft query. r :

[0033]

[0034] in, The distance matrix representing the position coordinates of the candidate landmarks and the pixel coordinates of the spectrogram; Indicates pixel-level Doppler velocity.

[0035] Furthermore, the instantaneous self-velocity is calculated by a differentiable optimization algorithm including:

[0036] The geometric relationship between the radial velocity of the static feature relative to the radar and the radar self-velocity is modeled as a linear equation GX = B and solved by the weighted least squares method:

[0037] X=(G T G) -1 G T B

[0038] Where X represents the instantaneous self-velocity; G represents the direction vector matrix; and B represents the observed velocity vector.

[0039] Furthermore, the geometric consistency constraint Defined as:

[0040]

[0041] in, represents the number of training batches; N represents the number of landmarks extracted from a single frame of radar point cloud; represents the confidence weight of the i-th landmark in the k-th frame; e i represents the geometric residual; Represents the rotation matrix from the kth frame to the k+1th frame; represents the 3D coordinates of the i-th landmark in the k-th frame in the radar coordinate system; represents the translation vector from the kth frame to the k+1th frame; Indicates the k+1th frame and The 3D coordinates of the corresponding matching landmark in the radar coordinate system;

[0042] Doppler velocity constraint Defined as:

[0043]

[0044] Where G represents the observation matrix of Doppler velocity of static landmarks; I represents the identity matrix, whose dimension is the same as the number of rows of G;

[0045] Speed consistency constraint Defined as:

[0046]

[0047] in, I v k represents the self-motion velocity in the inertial measurement unit coordinate system at the kth frame; Indicates the speed change from the kth frame to the k+1th frame; Represents the rotation matrix from the k+1th frame to the kth frame; I v k+1 Indicates the velocity measured by the inertial measurement unit at the k+1th frame.

[0048] In a second aspect, the present application provides an electronic device comprising at least one processing unit and at least one storage unit, wherein the storage unit stores a computer program, and when the computer program is executed by the processing unit, the processing unit is enabled to perform the steps of any of the aforementioned millimeter-wave radar inertial navigation positioning and mapping methods.

[0049] In a third aspect, the present application provides a storage medium storing a computer program executable by an access authentication device. When the computer program runs on the access authentication device, the access authentication device is enabled to perform the steps of the millimeter-wave radar inertial navigation positioning and mapping method described in any one of the aforementioned items.

[0050] In general, the above technical solutions conceived by this application can achieve the following beneficial effects compared with the existing technology:

[0051] This application designs the first millimeter-wave radar inertial navigation positioning and mapping technology solution based on self-supervised learning. It can achieve more stable and reliable radar inertial navigation positioning and mapping in more complex and extensive environments by using millimeter-wave radar spectra rich in external spatial information and IMU data containing high-frequency inertial navigation information. Specifically, first, this application designs a rotation-based heterogeneous information cross-fusion solution to enhance the spatial structure information of the millimeter-wave radar inter-frame spectra and alleviate the position ambiguity caused by the limited angular resolution to the feature extraction of spatial landmarks. Second, this application designs a spatial consistency landmark extraction and association solution to extract a robust and dense landmark feature point cloud from the millimeter-wave radar spectra rich in external spatial information, thereby effectively reducing the sparsity of the millimeter-wave point cloud and the influence of clutter point clouds. Third, this application proposes a differentiable self-velocity estimation that can accurately extract sub-pixel radial velocity from the Doppler spectrum and accurately estimate the self-velocity. Fourth, this application constructs a loss function based on geometry, velocity consistency, and Doppler velocity constraints to train the landmark extraction network in a self-supervised manner, thereby achieving accurate positioning and robust mapping. Compared with existing millimeter-wave radar inertial navigation positioning and mapping technology solutions, the self-supervised learning radar inertial navigation positioning and mapping solution proposed in this application can seamlessly adapt to complex and extensive traffic driving environments without the need for true positioning supervision, and achieve stable and reliable state estimation and landmark extraction. BRIEF DESCRIPTION OF THE DRAWINGS

[0052] In order to more clearly illustrate the technical solutions in the embodiments of the present application, the following briefly introduces the drawings required for use in the embodiments. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without creative work.

[0053] Figure 1 This is a core flow chart of a millimeter-wave radar inertial navigation positioning and mapping method provided in an embodiment of the present application;

[0054] Figure 2 A schematic block diagram of the three key steps of a millimeter wave radar inertial navigation positioning and mapping method provided in an embodiment of the present application;

[0055] Figure 3 A schematic diagram of the entire process steps of a millimeter wave radar inertial navigation positioning and mapping method provided in an embodiment of the present application;

[0056] Figure 4 A schematic block diagram of the entire process steps of a millimeter wave radar inertial navigation positioning and mapping method provided in an embodiment of the present application;

[0057] Figure 5 A block diagram of an electronic device suitable for implementing the millimeter wave radar inertial navigation positioning and mapping method described above is provided in an embodiment of the present application. DETAILED DESCRIPTION

[0058] In order to make the purpose, technical solutions and advantages of this application more clearly understood, the present application is further described in detail below with reference to the accompanying drawings and examples. It should be understood that the specific embodiments described herein are merely for the purpose of explaining this application and are not intended to limit this application. In addition, the technical features involved in the various embodiments of this application described below may be combined with each other as long as they do not conflict with each other.

[0059] The terms "including" or "having" and any variations thereof in the specification, claims, or drawings of this application are intended to cover non-exclusive inclusions. For example, a process, method, system, product, or apparatus that includes a series of steps or units is not limited to the listed steps or units, but may optionally include steps or units that are not listed, or may optionally include other steps or units inherent to the process, method, product, or apparatus.

[0060] As described in the background technology section of the specification, traditional millimeter-wave radar inertial navigation positioning solutions mainly focus on the following directions: (1) One is to use IMU observations in the state propagation equation and solve the odometer of the radar inertial navigation system through scan matching between millimeter-wave radar frames. However, this solution has high requirements for the spatial resolution of the millimeter-wave radar and is not suitable for low-cost single-chip radars; (2) Another solution is to use the internal perception IMU to model the state propagation process of the vehicle, use the Doppler velocity of the millimeter-wave radar as the observation constraint, and fuse multi-source information through a Kalman filter or a probability factor graph optimization model. However, this solution is limited by the quality of the millimeter-wave radar feature point cloud. Due to the limited size of commercial radar hardware, the acquired point cloud is usually very sparse and is affected by clutter points caused by multipath effects and mirror reflection effects, resulting in low robustness of this solution and difficulty in applying it to a wide range of complex driving scenarios. (3) Another solution is to perform end-to-end training and learning through deep neural networks. However, this method requires the collection of a wide range of training samples and a large amount of manual annotation, and generalization is difficult to guarantee. Therefore, achieving more stable and reliable radar inertial navigation positioning and mapping in more complex and extensive environments is a technical issue that urgently needs to be studied in all-weather autonomous driving systems. In view of this, this application proposes a millimeter-wave radar inertial navigation positioning and mapping method, device, and medium to achieve more stable and reliable radar inertial navigation positioning and mapping in more complex and extensive environments.

[0061] refer to Figure 1-Figure 4 An embodiment of the present application proposes a millimeter wave radar inertial navigation positioning and mapping method based on self-supervised learning, which may specifically include the following steps.

[0062] Step 1: Synchronize the millimeter-wave radar data frames through inter-frame pre-integration of the inertial measurement unit, use the rotation angle difference to build a dot-product attention mechanism, generate the expectation matrix between adjacent radar frames, and realize cross-fusion of heterogeneous information to enhance the spatial structure information of the radar spectrum.

[0063] Commercial single-chip radars are usually equipped with only a limited number of antennas, resulting in low angular resolution of millimeter-wave radars, making it difficult to extract reliable landmarks directly from radar spectra. To this end, this application designs a rotation-based heterogeneous information cross-fusion scheme to enhance the spatial structure information of the millimeter-wave radar inter-frame spectra. Specifically, this application utilizes the linear mapping relationship between the spatial perception spectra of millimeter-wave radar and the IMU inertial navigation data in the rotational component, and performs data enhancement on the target position in the spectra based on the angular information provided by the inertial navigation, thereby alleviating the position ambiguity caused by the limited angular resolution to the feature extraction of spatial landmarks.

[0064] More specifically, considering the different sampling rates of the IMU and radar, before performing rotation-based heterogeneous information cross-fusion, it is necessary to synchronize the millimeter-wave radar data frames through IMU inter-frame pre-integration:

[0065]

[0066] in, and Respectively represent the linear acceleration measurement value and angular velocity measurement value of the inertial measurement unit; b a and b ω Represent the linear acceleration measurement bias and angular velocity measurement bias respectively; Represents the rotation matrix in the inertial measurement unit coordinate system; Represents the quaternion multiplication operator; represents the relative attitude change of the inertial measurement unit from the kth frame to the current time t; and Represent the relative position, velocity and attitude quaternion between adjacent radar frames respectively. Next, let the k-th frame millimeter wave radar energy spectrum be H and W represent the range height and azimuth width of the radar spectrum respectively. Unlike the uniform linear resolution of the range dimension, the angular resolution of the radar spectrum is nonlinear. k The angle representation along the azimuth is denoted as η=(η1,…,η n ). Considering that when the radar rotates in a short time such as 0.1s When the static landmark maintains the direction θ in the radar coordinate system of the kth frame, and maintains the direction θ in the (k+1)th frame. Direction. express The key step in transforming the expected vector from the kth frame to the (k+1)th frame is to construct the expected vector by designing a dot product attention mechanism based on the angle difference:

[0067]

[0068] Among them, κ represents the annealing coefficient. Then, the expected matrix is aligned by scale ratio (abbreviated as scale alignment) operation. Fusion with the original spectrum to form an enhanced spectrum in,

[0069]

[0070] Similarly, the formula for the expected matrix provided by the (k+1)th frame acting on the kth frame is:

[0071] Through the rotation-based cross-fusion operation in step 1 above, on the one hand, the spatial structure information of the millimeter-wave radar inter-frame spectrogram is significantly enhanced, and on the other hand, the position ambiguity caused by the limited angular resolution in the feature extraction of spatial landmarks can be alleviated.

[0072] Step 2: Use the U-Net backbone network to extract multi-scale features from the fused radar spectrogram, output the spatial position, confidence weight and feature descriptor of sub-pixel landmarks through a multi-head decoding structure, and complete landmark feature matching in combination with the cross-frame attention mechanism.

[0073] Traditional radar inertial navigation positioning solutions are based on sparse point clouds processed onboard. However, due to the high clutter content of these point clouds, traditional scan matching or velocity estimation solutions have difficulty operating properly. To this end, this application designs a spatially consistent landmark extraction and association mechanism. It uses a feature extraction network to extract the spatial position, confidence, and descriptor of landmark features from the millimeter-wave radar spatial spectrum, and aligns candidate landmark features through a differentiable feature matching mechanism. Finally, the candidate features are mapped to a Cartesian coordinate system for subsequent velocity estimation.

[0074] Specifically, after rotation-based cross fusion, the spatial radar spectra with enhanced geometric consistency are then subjected to feature extraction and spatial feature point matching for candidate landmark features across adjacent time frames. More specifically, U-Net is first used as the backbone network for feature extraction, the enhanced radar spectra are used as network input, and a multi-head architecture is designed to decode the spatial position of the feature point, the confidence of the candidate landmark feature point, and the descriptor of the point. The position header outputs the detection score L. In order to fully utilize the signal energy sidelobe information in RAS, the M k Extract sub-pixel coordinates in each pixel block of the radar spectrum. According to the detection score map Perform Softmax weighted summation. The specific formulas include:

[0075]

[0076] Among them, (u ij , v ij ) represents pixel coordinates; (u k , v k ) represents sub-pixel coordinates.

[0077] The second tap (the confidence head of the multi-head decoding structure) outputs the normalized confidence weight It represents the confidence level of each pixel being selected as a candidate landmark. The confidence score can be used to eliminate the influence of clutter points with high reflection intensity.

[0078] The third tap (descriptor head of the multi-head decoding structure) collects the encoder backbone block features and uses them as descriptors to uniquely identify candidate landmark features.

[0079] Next, the radar landmark candidate features between frames are matched using the attention mechanism:

[0080]

[0081] Among them, V ′ Represents the position matrix of the candidate landmarks in the target frame; V represents the position matrix containing all pixel coordinates; and They represent the candidate landmarks of the (k-1)th frame and the descriptors of all pixel coordinates of the kth frame, specifically, represents the query matrix consisting of N candidate landmark descriptors of the k-1th frame; represents the key matrix composed of the pixel descriptors of the entire image of the kth frame; κ represents the annealing coefficient (also known as the temperature coefficient) that adjusts the sharpness of the attention weight distribution.

[0082] Through the above step 2, a robust and dense landmark feature point cloud can be extracted from the millimeter-wave radar spectrum rich in external spatial information, thereby effectively reducing the sparsity of the millimeter-wave point cloud and the influence of the clutter point cloud.

[0083] Step 3: Estimate the radial velocity of the candidate landmark based on the sub-pixel Doppler velocity soft query algorithm. Combined with the geometric relationship between the radial velocity of the static landmark and the radar ego velocity, the instantaneous ego velocity is solved through a differentiable optimization algorithm.

[0084] To achieve accurate velocity estimation using radar inertial navigation in a self-supervised manner, this application further designs a differentiable self-velocity estimation method for motion state estimation. Specifically, a differentiable sub-pixel Doppler velocity is first extracted from the radar's Doppler spectrum based on the positions of candidate features. Instantaneous self-velocity estimation is then achieved using a differentiable weighted least squares algorithm. During the inference phase, Doppler velocity constraints are applied to filter out clutter points and dynamic feature point clouds, thereby obtaining robust landmark features.

[0085] Specifically, in order to estimate the motion state of the millimeter-wave radar, it is necessary to first estimate the Doppler velocity of the candidate landmark. Since the candidate coordinates are at the sub-pixel level, it is impossible to directly query the target through the original Doppler spectrum. Therefore, this application designs a sub-pixel Doppler extraction scheme, which is similar to landmark feature extraction from the spectrum. The radial velocity v is extracted from the Doppler spectrum by performing a weighted soft query based on the pixel position of the candidate landmark. r :

[0086]

[0087] in, The distance matrix representing the position coordinates of the candidate landmarks and the pixel coordinates of the spectrogram; Indicates pixel-level Doppler velocity.

[0088] After obtaining the spatial coordinates of the candidate landmarks, the radial velocity of the static features relative to the radar is used Radar ego velocity R v is the target direction vector [cosα i ,sinα i ] T The vertical component of this characteristic, that is:

[0089]

[0090] The above formula can be rewritten as GX = B, and then the instantaneous self-speed X = (G T G) -1 G T B. Where X represents the instantaneous self-velocity; G represents the direction vector matrix; and B represents the observed velocity vector.

[0091] Then, in the inference stage, some clutter points and dynamic feature point clouds are filtered out by Doppler velocity constraints to obtain robust landmark features.

[0092] Step 4: Construct a joint loss function that includes geometric consistency constraints, Doppler velocity constraints, and velocity consistency constraints, and train the feature extraction network in a self-supervised manner.

[0093] This application designs three constraints: geometric consistency constraints, velocity consistency constraints, and Doppler velocity constraints for self-supervised training of spatially consistent landmark extraction networks (feature extraction networks), thereby achieving accurate positioning and robust mapping. Specifically, this application constructs a joint loss function based on geometric consistency constraints, velocity consistency constraints, and Doppler velocity constraints for self-supervised training of spatially consistent landmark extraction networks.

[0094] Geometric consistency constraints Defined as:

[0095]

[0096] in, represents the number of training batches; N represents the number of landmarks extracted from a single frame of radar point cloud; Represents the confidence weight of the i-th landmark in the k-th frame, ranging from [0, 1]. It is usually predicted by the network. The weight of dynamic points (such as moving objects) or mismatched points is close to 0, while the weight of static points is close to 1. iRepresents the geometric residual, which is used to measure the matching error of landmark positions between adjacent frames; Represents the rotation matrix from the kth frame to the k+1th frame, estimated by the network or obtained through IMU pre-integration; represents the 3D coordinates of the i-th landmark in the k-th frame in the radar coordinate system; represents the translation vector from the kth frame to the k+1th frame; Indicates the k+1th frame and The 3D coordinates of the corresponding matching landmark in the radar coordinate system.

[0097] Translational component It can be obtained by integrating the self-velocity solved by the radar: in, Represents the rotation matrix from time t to frame k (used for coordinate system alignment); I v k Indicates the self-motion speed in the IMU coordinate system at the kth frame; Represents the IMU position offset from the k+1th frame to the kth frame (provided by IMU pre-integration).

[0098] Doppler velocity constraint Defined as:

[0099]

[0100] Where G represents the Doppler velocity measurement matrix for static landmarks. This matrix is a design matrix consisting of the Doppler velocity observations of the static landmark point cloud. Each row corresponds to the Doppler velocity and spatial position of a landmark. If the landmark is static, its Doppler velocity should be linearly related to its ego-velocity, satisfying the column space constraints of G. I represents the identity matrix, which has the same dimensions as the number of rows in G.

[0101] Through the projection matrix G(G T G) -1 G T The velocity consistency component of static landmarks is extracted, and the difference from the identity matrix reflects the degree of deviation of dynamic points or mismatched points. Dynamic point clouds are suppressed because they do not meet the constraint. All candidate landmark feature point clouds from static matching results meet the velocity consistency constraint. Using this loss function, dynamic feature point clouds and mismatched point pairs are ignored by the network during the feature extraction stage, thereby improving landmark quality.

[0102] Speed consistency constraint Defined as:

[0103]

[0104] in, I vk represents the self-motion velocity in the inertial measurement unit coordinate system at the kth frame; Indicates the speed change from the kth frame to the k+1th frame; Represents the rotation matrix from the k+1th frame to the kth frame; I v k+1 Indicates the velocity measured by the inertial measurement unit at the k+1th frame.

[0105] Enforcing the velocity changes calculated by the radar to be consistent with the velocity measured by the IMU after rotational alignment eliminates accumulated errors in motion estimation. Aligning the ego velocities of the radar and IMU further optimizes motion speed, given the consistency between the velocity changes provided by the IMU and the ego velocity calculated by the radar.

[0106] Step 5: Extract spatially consistent radar landmark point clouds through the trained feature extraction network and integrate the self-velocity estimation results to achieve real-time positioning and mapping.

[0107] Specifically, the weighted loss function is used for self-supervised training of a spatially consistent landmark extraction network. During the inference phase, spatially consistent landmarks are extracted, while the vehicle's precise motion state is simultaneously captured. This enables precise positioning and robust mapping without the need for ground-truth position supervision, ensuring accurate, real-time, stable, reliable, and all-weather autonomous driving.

[0108] This application designs the first millimeter-wave radar inertial navigation positioning and mapping technology solution based on self-supervised learning. It can achieve more stable and reliable radar inertial navigation positioning and mapping in more complex and extensive environments by using millimeter-wave radar spectra rich in external spatial information and IMU data containing high-frequency inertial navigation information. Specifically, first, this application designs a rotation-based heterogeneous information cross-fusion solution to enhance the spatial structure information of the millimeter-wave radar inter-frame spectra and alleviate the position ambiguity caused by the limited angular resolution to the feature extraction of spatial landmarks. Second, this application designs a spatial consistency landmark extraction and association solution to extract a robust and dense landmark feature point cloud from the millimeter-wave radar spectra rich in external spatial information, thereby effectively reducing the sparsity of the millimeter-wave point cloud and the influence of clutter point clouds. Third, this application proposes a differentiable self-velocity estimation that can accurately extract sub-pixel radial velocity from the Doppler spectrum and accurately estimate the self-velocity. Fourth, this application constructs a loss function based on geometry, velocity consistency, and Doppler velocity constraints to train the landmark extraction network in a self-supervised manner, thereby achieving accurate positioning and robust mapping. Compared with existing millimeter-wave radar inertial navigation positioning and mapping technology solutions, the self-supervised learning radar inertial navigation positioning and mapping solution proposed in this application can seamlessly adapt to complex and extensive traffic driving environments without the need for true positioning supervision, and achieve stable and reliable state estimation and landmark extraction.

[0109] Figure 5 The following schematically shows a block diagram of an electronic device suitable for implementing the millimeter wave radar inertial navigation positioning and mapping method described above according to an embodiment of the present application. Figure 5 The electronic device shown is only an example and should not limit the functions and scope of use of the embodiments of the present application.

[0110] like Figure 5 As shown, the electronic device 1000 described in this embodiment includes: a processor 1001, which can perform various appropriate actions and processes according to the program stored in the read-only memory (ROM) 1002 or the program loaded from the storage part 1008 to the random access memory (RAM) 1003. The processor 1001 may include, for example, a general-purpose microprocessor (such as a CPU), an instruction set processor and / or a related chipset and / or a special-purpose microprocessor (for example, an application-specific integrated circuit (ASIC)), etc. The processor 1001 may also include an on-board memory for caching purposes. The processor 1001 may include a single processing unit or multiple processing units for executing different actions of the millimeter wave radar inertial navigation positioning and mapping method process according to the embodiment of the present application.

[0111] In RAM 1003, various programs and data required for the operation of electronic device 1000 are stored. Processor 1001, ROM 1002 and RAM 1003 are connected to each other via bus 1004. Processor 1001 performs various operations of the millimeter wave radar inertial navigation positioning and mapping method process according to the embodiment of the present application by executing the programs in ROM 1002 and / or RAM 1003. It should be noted that the program can also be stored in one or more memories other than ROM 1002 and RAM 1003. Processor 1001 can also perform various operations of the millimeter wave radar inertial navigation positioning and mapping method process according to the embodiment of the present application by executing the programs stored in the one or more memories.

[0112] According to an embodiment of the present application, electronic device 1000 may further include an input / output (I / O) interface 1005, which is also connected to bus 1004. Electronic device 1000 may further include one or more of the following components connected to I / O interface 1005: an input portion 1006 including a keyboard, mouse, etc.; an output portion 1007 including devices such as a cathode ray tube (CRT), liquid crystal display (LCD), and speakers; a storage portion 1008 including a hard disk; and a communication portion 1009 including a network interface card such as a LAN card or modem. Communication portion 1009 performs communication processing via a network such as the Internet. A drive 1010 is also connected to I / O interface 1005 as needed. Removable media 1011, such as a magnetic disk, optical disk, magneto-optical disk, semiconductor memory, etc., is installed in drive 1010 as needed, so that computer programs read from the removable media can be installed into storage portion 1008 as needed.

[0113] The millimeter wave radar inertial navigation positioning and mapping method process according to the embodiment of the present application can be implemented as a computer software program. For example, an embodiment of the present application includes a computer program product, which includes a computer program carried on a computer-readable storage medium, and the computer program contains program code for executing the millimeter wave radar inertial navigation positioning and mapping method shown in the flowchart. In such an embodiment, the computer program can be downloaded and installed from the network through the communication part 1009, and / or installed from the removable medium 1011. When the computer program is executed by the processor 1001, the above-mentioned functions defined in the system of the embodiment of the present application are executed. According to an embodiment of the present application, the systems, devices, means, modules and / or units described above can be implemented by computer program modules.

[0114] The embodiments of the present application also provide a computer-readable storage medium, which may be included in the device / apparatus / system described in the above embodiments, or may exist independently without being incorporated into the device / apparatus / system. The computer-readable storage medium carries one or more programs, which, when executed, implement the steps of the millimeter-wave radar inertial navigation positioning and mapping method according to the embodiments of the present application.

[0115] According to an embodiment of the present application, the computer-readable storage medium may be a non-volatile computer-readable storage medium, for example, it may include but is not limited to: a portable computer disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination thereof. In an embodiment of the present application, the computer-readable storage medium may be any tangible medium containing or storing a program that can be used by or in combination with an instruction execution system, apparatus, or device. For example, according to an embodiment of the present application, the computer-readable storage medium may include one or more memories other than the ROM 1002 and / or RAM 1003 described above.

[0116] It should be noted that the functional modules in the various embodiments of the present application can be integrated into a processing module, or each module can exist physically separately, or two or more modules can be integrated into one module. The above-mentioned integrated modules can be implemented in the form of hardware or in the form of software functional modules. If the integrated modules are implemented in the form of software functional modules and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present application, in essence, or the part that contributes to the prior art or all or part of the technical solution can be embodied in the form of a software product.

[0117] The flowcharts and / or block diagrams in the accompanying drawings illustrate the possible architecture, functions and operations of the systems, methods and computer program products according to various embodiments of the present application. In this regard, each box in the flowchart and / or block diagram can represent a module, a program segment or a part of code, and the part of the above-mentioned module, program segment or code contains one or more executable instructions for realizing the specified logical function. It should also be noted that in some alternative implementations, the functions marked in the box can also occur in an order different from that marked in the accompanying drawings. It should also be noted that each box in the block diagram or flowchart, and the combination of the boxes in the block diagram or flowchart, can be implemented by a special hardware-based system that performs the specified function or operation, or can be implemented by a combination of special hardware and computer instructions.

[0118] Those skilled in the art will appreciate that the features described in the various embodiments and / or claims of this application may be combined and / or coupled in various ways, even if such combinations and / or couplings are not explicitly described in this application. In particular, without departing from the spirit and teachings of this application, the technical features described in the various embodiments and / or claims of this application may be combined and / or coupled in various ways, and all such combinations and / or couplings fall within the scope of this application.

[0119] Although the present application has been shown and described with reference to certain exemplary embodiments thereof, it will be understood by those skilled in the art that various changes in form and details may be made to the present application without departing from the spirit and scope of the present application as defined by the appended claims and their equivalents. Therefore, the scope of the present application should not be limited to the above-described embodiments, but should be determined not only by the appended claims but also by the equivalents of the appended claims.

Claims

1. A millimeter wave radar inertial navigation positioning and mapping method, characterized in that: include: By pre-integrating the millimeter-wave radar data frames between frames of the inertial measurement unit, the dot-product attention mechanism is constructed using the rotation angle difference to generate the expectation matrix between adjacent radar frames, realizing the cross-fusion of heterogeneous information to enhance the spatial structure of the radar spectrogram. The U-Net backbone network is used to extract multi-scale features from the fused radar spectra. The spatial position, confidence weight, and feature descriptor of sub-pixel landmarks are output through a multi-head decoding structure, and landmark feature matching is completed by combining the cross-frame attention mechanism. The radial velocity of candidate landmarks is estimated based on a sub-pixel Doppler velocity soft query algorithm. The instantaneous self-velocity is calculated using a differentiable optimization algorithm, combining the geometric relationship between the radial velocity of static landmarks and the radar self-velocity. A joint loss function including geometric consistency constraints, Doppler velocity constraints, and velocity consistency constraints is constructed to train the feature extraction network in a self-supervised manner. The spatially consistent radar landmark point cloud is extracted through the trained feature extraction network, and the self-velocity estimation results are integrated to achieve real-time positioning and mapping.

2. The millimeter wave radar inertial navigation positioning and mapping method according to claim 1, wherein: The inter-frame pre-integration of the inertial measurement unit includes: According to the linear acceleration measurement of the inertial measurement unit Angular velocity measurement Linear acceleration measurement bias b a and angular velocity measurement bias b ω , calculate the relative position between adjacent radar frames speed and attitude quaternion The specific formulas include: in, Represents the rotation matrix in the inertial measurement unit coordinate system; Represents the quaternion multiplication operator; Indicates the relative attitude change of the inertial measurement unit from the kth frame to the current time t.

3. The millimeter wave radar inertial navigation positioning and mapping method according to claim 1, wherein: The implementation of the dot product attention mechanism includes: The k-th frame radar spectrum The azimuth angle representation η=(η1,…,η n ) and the difference between the rotation angle θ as a metric to generate the expected vector from the kth frame to the k+1th frame Among them, κ represents the annealing coefficient, and the expected matrix is aligned by the scale operation Fusion with the original spectrum to form an enhanced spectrum 4. The millimeter wave radar inertial navigation positioning and mapping method according to claim 1, wherein: The multi-head decoding structure outputs the spatial position, confidence weight and feature descriptors of sub-pixel landmarks, including: In the local pixel block of the radar spectrum According to the detection score map Perform Softmax weighted summation. The specific formulas include: Among them, (u ij , v ij ) represents pixel coordinates; (u k , v k ) represents sub-pixel coordinates; Output normalized confidence weights from the confidence header of the multi-head decoding structure To represent the confidence of each pixel being selected as a candidate landmark; The encoder backbone block features are collected from the descriptor header of the multi-head decoding structure and used as descriptors to uniquely identify candidate landmark features.

5. The millimeter wave radar inertial navigation positioning and mapping method according to claim 1, wherein: Combining the cross-frame attention mechanism to complete landmark feature matching includes: The radar landmark candidate features between frames are matched using the attention mechanism: Among them, V′ k Represents the position matrix of the candidate landmarks in the target frame; V represents the position matrix containing all pixel coordinates; represents the query matrix consisting of N candidate landmark descriptors of the k-1th frame; represents the key matrix composed of pixel descriptors of the full image of the kth frame; κ represents the annealing coefficient that adjusts the sharpness of the attention weight distribution; H and W represent the distance height and azimuth width of the radar spectrum, respectively.

6. The millimeter wave radar inertial navigation positioning and mapping method according to claim 3, characterized in that: The radial velocity of candidate landmarks is estimated based on the sub-pixel Doppler velocity soft query algorithm, which includes: According to the sub-pixel coordinates of the candidate landmarks, the radial velocity v is extracted from the Doppler spectrum by weighted soft query. r : in, The distance matrix representing the position coordinates of the candidate landmarks and the pixel coordinates of the spectrogram; Indicates pixel-level Doppler velocity.

7. The millimeter wave radar inertial navigation positioning and mapping method according to claim 6, characterized in that: Solving the instantaneous self-velocity through a differentiable optimization algorithm includes: The geometric relationship between the radial velocity of the static feature relative to the radar and the radar self-velocity is modeled as a linear equation GX = B and solved by the weighted least squares method: X=(G T G) -1 G T B Where X represents the instantaneous self-velocity; G represents the direction vector matrix; and B represents the observed velocity vector.

8. The millimeter wave radar inertial navigation positioning and mapping method according to claim 1, wherein: Geometric consistency constraints Defined as: in, represents the number of training batches; N represents the number of landmarks extracted from a single frame of radar point cloud; represents the confidence weight of the i-th landmark in the k-th frame; e i represents the geometric residual; Represents the rotation matrix from the kth frame to the k+1th frame; represents the 3D coordinates of the i-th landmark in the k-th frame in the radar coordinate system; represents the translation vector from the kth frame to the k+1th frame; Indicates the k+1th frame and The 3D coordinates of the corresponding matching landmark in the radar coordinate system; Doppler velocity constraint Defined as: Where G represents the observation matrix of Doppler velocity of static landmarks; I represents the identity matrix, whose dimension is the same as the number of rows of G; Speed consistency constraint Defined as: in, I v k represents the self-motion velocity in the inertial measurement unit coordinate system at the kth frame; Indicates the speed change from the kth frame to the k+1th frame; Represents the rotation matrix from the k+1th frame to the kth frame; I v k+1 Indicates the velocity measured by the inertial measurement unit at the k+1th frame.

9. An electronic device, characterized in that: The invention comprises at least one processing unit and at least one storage unit, wherein the storage unit stores a computer program, and when the computer program is executed by the processing unit, the processing unit is enabled to perform the steps of the millimeter wave radar inertial navigation positioning and mapping method according to any one of claims 1 to 8.

10. A storage medium, characterized in that: It stores a computer program that can be executed by an access authentication device. When the computer program runs on the access authentication device, the access authentication device is able to perform the steps of the millimeter wave radar inertial navigation positioning and mapping method described in any one of claims 1 to 8.

Images