Tightly coupled navigation and positioning method based on 4d millimeter wave radar / inertial sensor

Abstract

The application discloses a kind of based on 4D millimeter wave radar / inertial sensor tightly coupled navigation positioning method, comprising: the original point cloud is based on the dynamic point rejection of Doppler consistency, obtain first point cloud, then carry out noise point rejection and obtain second point cloud;Weighted Doppler velocity factor residual corresponding to single frame point cloud is constructed based on Doppler velocity weight;Adjacent two frames of second point cloud are histogram matched, obtain interframe point pair set, and interframe point pair factor residual is constructed based on interframe point pair set;Joint weighted Doppler velocity factor residual, interframe point pair factor residual and inertial pre-integral factor residual, construct tightly coupled nonlinear factor graph optimization model, and the navigation positioning information of carrier is obtained by executing nonlinear factor graph optimization.The application is applied to navigation positioning field, faces extreme dynamic scene, the characteristics of dynamic, static target are jointly analyzed, and positioning precision and robustness in dynamic scene are significantly improved.

Description

Technical Field

[0001] This invention relates to the field of navigation and positioning technology, specifically a tightly coupled navigation and positioning method based on 4D millimeter-wave radar / inertial sensor. Background Technology

[0002] Autonomous driving requires accurate real-time state estimation (attitude and position) for subsequent navigation tasks. However, in GNSS-constrained environments such as tunnels, urban canyons, and underground parking lots, the error of traditional INS / GNSS combined positioning increases rapidly. While LiDAR and camera-based solutions have demonstrated superior performance, they may fail in adverse weather conditions such as rain, fog, and snow. In contrast, millimeter-wave radar, due to its operating frequency characteristics, is unaffected by these conditions, making it a highly attractive alternative. Emerging 4D millimeter-wave radar, in particular, provides additional elevation information and denser point clouds compared to traditional 3D millimeter-wave radar (hereinafter referred to as 3D radar and 4D radar, respectively), thus possessing three-dimensional imaging capabilities similar to LiDAR. However, 4D radar point clouds are still sparser than LiDAR point clouds and contain more noise and clutter.

[0003] Numerous studies have been conducted on radar odometer technology. Some researchers utilize multi-sensor fusion strategies, combining 4D millimeter-wave radar with cameras. For example, they align radar point clouds with image pixel information using spatiotemporal synchronization algorithms, improving 3D reconstruction accuracy while enhancing the accuracy and stability of target detection in extreme weather conditions such as rain, fog, snow, darkness, and backlighting. Others focus on processing 4D millimeter-wave radar point clouds, employing Pillar coding to structure and encode the radar point clouds, suppressing noise while retaining effective target information, thus addressing the issues of sparse and noisy point clouds. In some 3D road vehicle detection studies based on convolutional neural networks, point clouds are generated using dual low-cost 4D millimeter-wave radars and combined with single-view camera data, using extended Kalman filtering to achieve 3D tracking. Still others utilize clustering algorithms to process 4D millimeter-wave radar detection data, distinguishing between noise and obstacle reflection point clouds to accurately display passable space.

[0004] Currently, most radar odometry algorithms largely follow the methods used in the lidar field, lacking point cloud registration algorithms specifically designed for the characteristics of 4D millimeter-wave radar. Regarding the utilization of Doppler information, existing algorithms are mostly simplistic. Furthermore, they lack an effective dynamic point removal algorithm when facing extreme dynamic conditions.

[0005] Based on this, it is necessary to propose a tightly coupled odometry calculation method for 4D millimeter-wave radar and inertial measurement unit (IMU) based on histogram registration for dynamic scenarios, which addresses the above-mentioned technical problems. This algorithm is designed for extreme dynamic scenarios, jointly analyzes the characteristics of dynamic and static targets, and proposes a dynamic point elimination algorithm based on Doppler consistency and a point cloud registration algorithm suitable for millimeter-wave radar. Summary of the Invention

[0006] To address the problems of sparse, unevenly distributed, and noisy point clouds in existing 4D millimeter-wave radar technologies, poor dynamic point removal performance in dynamic scenes, insufficient adaptability of traditional registration algorithms, and low stability of velocity estimation, this invention provides a tightly coupled navigation and positioning method based on 4D millimeter-wave radar / inertial sensors. This method is designed for extreme dynamic scenes and jointly analyzes the characteristics of dynamic and static targets, significantly improving the positioning accuracy and robustness in dynamic scenes.

[0007] To achieve the above objectives, this invention provides a tightly coupled navigation and positioning method based on 4D millimeter-wave radar / inertial sensors, comprising the following steps:

[0008] Step 1: Rasterize the original point cloud of the 4D millimeter-wave radar into partitions, and perform dynamic point removal based on inertial sensor assistance and / or dynamic point removal based on Doppler consistency on the original point cloud to obtain the first point cloud after dynamic point removal.

[0009] Step 2: Remove noise from the first point cloud to obtain the second point cloud;

[0010] Step 3: Calculate the Doppler velocity weights of the first point cloud or the second point cloud in each directional interval of the rasterized partition, and construct the weighted Doppler velocity factor residual corresponding to the second point cloud in a single frame based on the Doppler velocity weights;

[0011] Step 4: Perform histogram matching on the second point cloud of two adjacent frames to obtain a set of inter-frame point pairs, and construct the inter-frame point pair factor residual based on the set of inter-frame point pairs;

[0012] Step 5: Combine the weighted Doppler velocity factor residual, the inter-frame point pair factor residual, and the inertial pre-integration factor residual to construct a tightly coupled nonlinear factor graph optimization model, and perform nonlinear factor graph optimization to obtain the navigation and positioning information of the carrier.

[0013] Compared with the prior art, the present invention has the following beneficial technical effects:

[0014] 1. This invention uses a dynamic point elimination algorithm based on Doppler consistency to solve the problem of inaccurate dynamic point elimination in extreme dynamic scenarios:

[0015] In existing technologies, the robustness of the RANSAC algorithm drops sharply when the proportion of dynamic points exceeds 50%, and the GNC method, although capable of handling a high proportion of outliers, is computationally complex. This invention utilizes a process of "Doppler velocity clustering - spatial distance clustering - candidate point screening - two-stage RANSAC," leveraging the high Doppler consistency of static points and low consistency of dynamic points within the same directional interval to accurately screen static points. This achieves a 9.6% improvement in precision compared to traditional methods in scenarios where dynamic points account for 90%, while maintaining a 99.9% recall rate. It effectively solves the problem of large velocity estimation errors caused by dynamic point interference in extreme dynamic scenarios, significantly improving the system's robustness in highly dynamic environments.

[0016] 2. This invention effectively improves the stability and accuracy of velocity estimation by setting adaptive Doppler velocity weights:

[0017] In existing technologies, uneven point cloud distribution in millimeter-wave radar (affected by RCS differences and CFAR filtering) leads to a large condition number and poor solution stability in velocity estimation. This invention constructs an adaptive weighting model based on the statistical characteristics of point cloud distribution in a gridded partition: a higher number of points within a directional interval results in a lower weight, and a lower number of points results in a higher weight, balancing velocity constraints in each direction. This reduces the overall root mean square error (RMSE) of velocity estimation from 0.25 m / s to 0.12 m / s, effectively mitigating the impact of uneven point cloud distribution on velocity estimation, improving the stability and accuracy of velocity constraints, and providing a more reliable observational basis for subsequent pose estimation.

[0018] 3. This invention effectively solves the registration problem of sparse noisy point clouds through histogram matching:

[0019] Traditional registration algorithms such as ICP and NDT suffer from a sharp performance drop due to the sparseness and high noise of radar point clouds, and existing improved methods do not adequately consider radar characteristics. This invention designs a two-dimensional histogram (LGC histogram) that integrates local geometric distribution (nearest neighbor distance) and RCS features, and proposes a neighborhood-extended histogram cross-metric. By statistically analyzing the joint distribution of local features, the discriminative power is enhanced, while addressing feature shifts caused by noise and discretization. This method reduces the relative positioning error from 1.2% and 1.7% to 0.8% in two sets of sequences, respectively, and reduces the vertical direction (Z-axis) error by up to 80%. It solves the problem of stable data association in sparse and noisy point clouds and significantly improves point cloud registration accuracy.

[0020] 4. This invention improves the overall positioning accuracy and robustness of the system by proposing a tightly coupled factor graph optimization framework:

[0021] Existing technologies often employ loose coupling or single-sensor constraints, making them ill-suited for complex scenarios. This invention, within a factor graph optimization framework, integrates inertial pre-integration factors, weighted Doppler velocity factors, and point-to-point factors to achieve deep fusion of multi-source information: inertial sensors provide high-frequency motion constraints, weighted Doppler velocity suppresses the effects of uneven point cloud distribution, and LGC histogram matching provides precise spatial constraints. Experimental results demonstrate that this framework maintains stable positioning even in violently shaking scenarios, with a relative positioning error of only 1.3% in an elevated bridge sequence. Furthermore, it outperforms existing radar / inertial odometry and some lidar solutions in multi-sensor comparisons, fully leveraging the environmental adaptability advantages of 4D millimeter-wave radar while compensating for its accuracy shortcomings.

[0022] In summary, this invention comprehensively solves the core problems of 4D millimeter-wave radar in odometry applications by specifically improving dynamic point removal, velocity estimation, point cloud registration, and fusion framework. It significantly improves positioning accuracy, robustness, and environmental adaptability in dynamic scenarios, providing a reliable solution for high-precision navigation in fields such as autonomous driving. Attached Figure Description

[0023] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on the structures shown in these drawings without creative effort.

[0024] Figure 1 This is a flowchart of a tightly coupled navigation and positioning method based on 4D millimeter-wave radar / inertial sensor in an embodiment of the present invention;

[0025] Figure 2 This is a schematic diagram of rasterized partitioning based on a spherical coordinate system in an embodiment of the present invention;

[0026] Figure 3 This is a flowchart of the dynamic point elimination method based on Doppler consistency in an embodiment of the present invention;

[0027] Figure 4 This is a schematic diagram of Doppler velocity clustering in an embodiment of the present invention;

[0028] Figure 5 This is a schematic diagram of Euclidean distance clustering in an embodiment of the present invention;

[0029] Figure 6 This is a schematic diagram of candidate class screening in an embodiment of the present invention.

[0030] The realization of the objective, functional features and advantages of the present invention will be further explained in conjunction with the embodiments and with reference to the accompanying drawings. Detailed Implementation

[0031] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative effort are within the scope of protection of the present invention.

[0032] Furthermore, the technical solutions of the various embodiments of the present invention can be combined with each other, but only if they are based on the ability of those skilled in the art to implement them. When the combination of technical solutions is contradictory or cannot be implemented, it should be considered that such combination of technical solutions does not exist and is not within the scope of protection claimed by the present invention.

[0033] This embodiment discloses a tightly coupled navigation and positioning method based on 4D millimeter-wave radar / inertial sensors. It primarily addresses issues such as sparse and unevenly distributed point clouds in 4D millimeter-wave radar and interference from dynamic scenes. Through a series of steps, a tightly coupled odometry is constructed, significantly improving the positioning accuracy and robustness of the target in dynamic scenes. (Reference) Figure 1 The tightly coupled navigation and positioning method based on 4D millimeter-wave radar / inertial sensor in this embodiment specifically includes the following steps:

[0034] Step 1: Rasterize the original point cloud acquired by the 4D millimeter-wave radar on the carrier, and perform dynamic point removal based on inertial sensor assistance and / or dynamic point removal based on Doppler consistency to obtain the first point cloud after dynamic point removal.

[0035] Step 2: Remove noise from the first point cloud to obtain the second point cloud;

[0036] Step 3: Calculate the Doppler velocity weights of the first or second point cloud in each directional interval of the rasterized partition, and construct the weighted Doppler velocity factor residuals corresponding to the single-frame point cloud based on the Doppler velocity weights.

[0037] Step 4: Perform histogram matching on the second point clouds of two adjacent frames to obtain a set of inter-frame point pairs, and construct the inter-frame point pair factor residual based on the set of inter-frame point pairs;

[0038] Step 5: Combine the weighted Doppler velocity factor residual, the inter-frame point pair factor residual, and the inertial pre-integration factor residual to construct a tightly coupled nonlinear factor graph optimization model, and perform nonlinear factor graph optimization to obtain the navigation and positioning information of the carrier.

[0039] In the specific implementation of rasterization and partitioning of the raw point cloud from a 4D millimeter-wave radar, the raw point cloud is first transformed into a spherical coordinate system, and then rasterized and partitioned. By discretizing the field of view (FOV) range of the elevation and azimuth angles into equally spaced intervals, rapid spatial indexing and statistics of the point cloud, as well as subsequent feature point extraction, are achieved. Partition visualization is as follows: Figure 2 As shown, specifically:

[0040] Assume the elevation angle FOV range of the 4D millimeter-wave radar is... The grid spacing is Then the pitch angle interval number for:

[0041]

[0042] in, , These are the lower and upper limits of the pitch angle FOV range, respectively. It is the floor function;

[0043] No. i pitch angle range Defined as:

[0044]

[0045] Similarly, let the azimuth field of view (FOV) range of the 4D millimeter-wave radar be... The grid spacing is Then the number of azimuth intervals for:

[0046]

[0047] in, , These are the lower and upper limits of the azimuth field of view (FOV), respectively.

[0048] No. j pitch angle range Defined as:

[0049]

[0050] Assuming points outside the FOV have been removed, if the coordinates of a point in the original point cloud in the spherical coordinate system are... Then its partition is .in, , ;

[0051] Based on the grid spacing within the azimuth and elevation FOV ranges, the original point cloud of the 4D millimeter-wave radar is rasterized into K directional intervals. ,in, .

[0052] Specifically, in this embodiment, the parameters for rasterizing the original point cloud of the 4D millimeter-wave radar are kept uniform, namely the elevation angle grid spacing. The azimuth grid spacing is .

[0053] Within the same azimuth range, the Doppler velocities of static targets exhibit high consistency, while the consistency among dynamic targets is weaker. Based on this understanding, this embodiment proposes a dynamic point elimination method based on Doppler consistency, referencing... Figure 3 Specifically, it includes the following steps:

[0054] Extract points from the original point cloud that are located in the same directional interval and add them to the point set of the corresponding directional interval;

[0055] For all points in each direction interval point set, merge those with Doppler velocity difference values ​​less than a first given threshold into a Doppler velocity class, so that each direction interval point set forms at least one Doppler velocity class.

[0056] Perform Euclidean distance-based clustering on the point set corresponding to each Doppler velocity class, and merge points with a distance difference value less than a second given threshold into a distance class, so that each Doppler velocity class forms at least one distance class;

[0057] Filter out Doppler velocity classes whose number of distance classes exceeds the candidate class threshold, and define all distance classes contained in all filtered Doppler velocity classes as candidate classes;

[0058] Within each candidate class, a point is selected to form a candidate point set, and the RANSAC algorithm is executed based on the candidate point set to estimate the coarse velocity of the carrier. The specific implementation process of the RANSAC algorithm is a conventional technical means in this field, and will not be described in detail in this embodiment.

[0059] Back-projecting all points in the original point cloud based on coarse velocity yields the projected Doppler velocity for each point. The error between the projected Doppler velocity and the true Doppler velocity for each point is calculated. Points with errors greater than a third given threshold are removed from the original point cloud to obtain the transition point cloud.

[0060] The RANSAC algorithm is executed again on all points contained in the transition point cloud to obtain the inner point set. The points in the inner point set are treated as static points and retained, while the outer points are removed. That is, all outer points determined by the RANSAC algorithm are removed from the transition point cloud to obtain the first point cloud.

[0061] In practice, the clustering process for Doppler velocity classes is as follows:

[0062] Let the set of Doppler velocities of all points in a certain direction interval be . The first given threshold is ,Will Arranged in descending order Create the first Doppler velocity class traversal , will satisfy All join in The minimum cluster size is t0 (keeping consistent with the candidate class threshold discussed later). ) ;

[0063] Next, select For points that have not been classified, create new Doppler velocity classes. Repeat the above operation to obtain several Doppler velocity classes.

[0064] Doppler velocity clustering is performed on each directional interval to obtain k Doppler velocity classes. ,For example Figure 4 As shown, each box represents a Doppler velocity class. Figure 4 Because the points are too dense, some Doppler velocity classes may appear to have only one point, but in reality, each Doppler velocity class contains more than three points.

[0065] In the specific implementation process, clustering based on Euclidean distance is performed on the point set corresponding to each Doppler velocity class, and points with a distance difference value less than a second given threshold are merged into one class, that is, satisfying the following formula:

[0066]

[0067] in, For a certain Doppler velocity class, the set of point clouds. , For set midpoint p , q The three-dimensional coordinates For the second given threshold, The distance is Euclidean.

[0068] Specifically: given a certain Doppler velocity class Its corresponding point set is .in, These are the three-dimensional coordinates of the point. Select one of the points. Create the first distance class in this Doppler velocity class. traversal , will satisfy All Join In this process, the minimum cluster size is 1. Next, unclassified points are selected to create new distance classes, and the above operation is repeated to obtain s distance classes. .like Figure 5 As shown, each circle represents a distance class, while circles of the same color belong to the same Doppler velocity class. From Figure 5 It can be seen that, except for the black circle, all other colored circles appear only once. This involves traversing the Doppler velocity class across all directional intervals. Subsequently, if a Doppler velocity class contains t (candidate class threshold) or more distance classes, then all distance classes in that Doppler velocity class are considered candidate classes. Figure 6 For example, only the black circles satisfy the above conditions, therefore only Figure 6 The distance classes corresponding to the four black circles inside the large black circle are candidate classes.

[0069] It is worth noting that in specific applications, the method is not limited to using only the Doppler consistency-based dynamic point removal method to dynamically remove points from the original point cloud. An inertial sensor-assisted dynamic point removal method can also be used. Because extreme dynamics do not always exist, to maintain the overall algorithm's efficiency, this embodiment, as a preferred approach, combines inertial sensor-assisted dynamic point removal with Doppler consistency-based dynamic point removal. Specifically: First, inertial sensor-assisted dynamic point removal is used to dynamically remove points from the original point cloud. If the proportion of removed dynamic points is within a set threshold, the dynamic points are directly removed, and a first point cloud is obtained; otherwise, Doppler consistency-based dynamic point removal is performed on the original point cloud to obtain the first point cloud. The inertial sensor-assisted dynamic point removal is a conventional technique in this field, and therefore will not be described in detail in this embodiment.

[0070] In this embodiment, the noise removal process for the first point cloud is as follows:

[0071] Obtain two adjacent point clouds , ,in, For the first The first or second point cloud of the frame, For the first The first point cloud of the frame, , The first Frame and the The radar coordinate system of the frame, where if the first frame... If the frame is the 2nd frame, then Using the first point cloud, the second If the frame is the point cloud frame from frame 2 onwards, then The first point cloud is preferred;

[0072] Based on the Frame and the The pose transformation matrix between the frame radar coordinate system (obtained through inertial sensors or Doppler information) will transform the point cloud. Convert to the The point cloud is obtained from the radar coordinate system of the frame. ;

[0073] For point clouds structure Tree and perform nearest neighbor search with a fixed radius, if point cloud A point in the radius It does not exist within Any neighboring point will be marked as a noise point and removed, in this embodiment. ;

[0074] Remove point clouds After removing all noise, we obtain the first... The second point cloud in the frame.

[0075] In the process of constructing the weighted Doppler velocity factor residual in step 3, considering the impact of uneven point cloud distribution on velocity estimation, this embodiment proposes an adaptive weight calculation method to reduce the impact of uneven distribution. The fundamental reason why uneven point cloud distribution leads to a decrease in velocity estimation accuracy is that there are too many points in some directional intervals and too few points in other directional intervals. The constraints in a directional interval are relatively homogeneous, and a large number of redundant homogeneous observations will dominate the optimization direction, while the constraints in sparse directions are ignored.

[0076] Clearly, treating every point equally is not the optimal strategy when the point cloud distribution is uneven. Lower weights should be assigned to points within a larger directional interval, and higher weights to points within a smaller interval. After rasterization, the original point cloud, the first point cloud, and the second point cloud can all be divided into K directional intervals. Assuming the three-dimensional velocity of the millimeter-wave radar at the current moment is in the inertial coordinate system... A total of N static points were observed, the first of which was the [missing information]. i The three-dimensional coordinates of the static point in the radar coordinate system are: , No. i The Doppler velocities corresponding to the static points are: , i =1~ N If a certain directional interval has If there are n points, then the residual at each point is:

[0077]

[0078] in, for The normalized vector, For normalization parameters, The three-dimensional velocity of the millimeter-wave radar at the current moment. T This is the transpose of the matrix;

[0079] Let the first k ( k =1~ K ) directional intervals have If there are static points, then the direction interval Total constraints for:

[0080]

[0081] Assuming the noise in the point cloud (Doppler velocity noise, angular noise) follows the same distribution, then the residuals of all points are approximately the same (i.e., (where is a constant), then it simplifies to:

[0082]

[0083] In practical applications, it is desirable that the total constraints within each directional interval are equal, that is, satisfying:

[0084]

[0085] Where C is a constant. Then:

[0086]

[0087] Removing the constant term gives:

[0088]

[0089] Therefore, in the specific implementation process of this embodiment, the Doppler velocity weight of each direction interval is obtained according to the number of points contained in each direction interval. , Direction interval Doppler velocity weights, Direction interval The number of static points included. Preferably, for more robust estimation, all Doppler velocity weights are included. Normalization to Ensure that the maximum weight does not exceed ten times the minimum weight.

[0090] Obtain the Doppler velocity weights in each directional interval. Then, the weighted Doppler velocity factor residual corresponding to a single frame point cloud can be constructed. ,for:

[0091]

[0092] in, To optimize the state of the tightly coupled nonlinear factor graph model. For the first point cloud The position vector of a radar point in the radar coordinate system For the first point cloud Doppler velocity measurements at each radar point The external parameter rotation matrix from the carrier coordinate system to the radar coordinate system. for The rotation matrix from the world coordinate system to the carrier coordinate system at any given moment. for The linear velocity of the carrier in the world coordinate system at any given moment; For the first point cloud Doppler velocity weights for each radar point;

[0093] In this embodiment, the first The calculation process for the Doppler velocity weight of each radar point is as follows:

[0094]

[0095] in, For the first Doppler velocity weights for the direction intervals to which each radar point belongs. For the first The elevation angle of a radar point in the point cloud.

[0096] In this embodiment, the process of obtaining the set of inter-frame point pairs based on histogram matching is as follows:

[0097] First, extract the key points from the second point cloud and construct a two-dimensional histogram for each key point;

[0098] The keypoints of the second point cloud in two adjacent frames are paired to obtain several initial point pairs, and the histogram intersection of the neighborhood expansion of the two points in each initial point pair is calculated. ,for

[0099]

[0100]

[0101] in, This represents the matrix corresponding to the two-dimensional histogram of the first point in the initial point pair. A The set of coordinates of non-zero elements. This represents the matrix corresponding to the two-dimensional histogram of the second point in the initial point pair. B The set of coordinates of non-zero elements. The neighborhood search radius is For matrix A The median coordinate is elements, For matrix B The median coordinate is elements, Distance weights Distance to Manhattan;

[0102] Will satisfy The initial point pairs are used as candidate point pairs, where, The similarity threshold is used, and if a point forms a candidate point pair with multiple points, the point with the highest similarity is selected. The highest-ranking point is selected as the candidate point pair corresponding to that point.

[0103] The RANSAC algorithm is executed based on all candidate point pairs, and the set of point pairs with the most inliers is retained as the set of inter-frame point pairs.

[0104] In the specific implementation process, the process of extracting key points from the second point cloud is as follows: all points in the second point cloud are divided into corresponding directional intervals, and the points with the strongest echoes in each directional interval are selected. The principle behind using a few points as keypoints is that points with lower RCS are more likely to be noise, while points with higher RCS are usually more stable, thus making the extracted keypoints more uniform.

[0105] In the construction of a two-dimensional histogram, this embodiment proposes a histogram feature construction method that combines local geometric constraints and physical features (RCS). The specific implementation process is as follows:

[0106] First, all key points are grouped into a key point set. ,in, For the first i One key point, The number of keypoints, each keypoint containing 3D coordinates and RCS:

[0107]

[0108] in, Key point The three-dimensional coordinates Key point RCS value, , For dimensions;

[0109] For point set structure A tree, for each point in it Search for k nearest neighbors, and for each neighbor... Extract the two-dimensional features as follows:

[0110]

[0111]

[0112] in, For point The three-dimensional coordinates For Euclidean distance, Nearest neighbor RCS value;

[0113] Then point Two-dimensional histogram for:

[0114]

[0115] in, For indicator functions, , Binning is done along the distance dimension and the RCS dimension. 、 For indexing, s The number of bins in the distance dimension. t This represents the number of bins in the RCS dimension.

[0116] In practical applications, the resolution setting of histogram binning needs to be carefully balanced. If the bin width is set too large, local features of different points may be grouped into the same bin, thus losing discriminative power. Conversely, if the bin width is set too small, local features of the same target may be grouped into different bins due to sensor noise and binning operations (discretization), resulting in false differences. To maintain discriminative power while reducing false differences, a similarity calculation method considering histogram neighborhood is proposed, namely neighborhood-expanded histogram cross. Neighborhood-expanded histogram cross can effectively cope with the local feature position shift caused by noise and discretization by expanding the neighborhood, and improves the robustness of similarity calculation by introducing weights. Since the LGC histogram is very sparse (most elements are 0), the algorithm complexity is low, and the number of non-zero elements is equal to the number of nearest neighbors. The radius of the neighborhood search is typically 15-30. It is usually sufficient to set it to 1.

[0117] Considering that some incorrect matches may exist among all candidate point pairs, this embodiment uses RANSAC to remove abnormal candidate point pairs. RANSAC's minimum random sampling point set is 3. Assume that a certain iteration selects point pairs... , , The RANSAC model is as follows:

[0118] First, calculate the centroid of the source point. and the centroid of the target point ,for:

[0119]

[0120]

[0121] Then, construct the covariance matrix. ,for:

[0122]

[0123] Then, the rotation matrix is ​​solved using SVD decomposition. ,for:

[0124]

[0125] in, , It is a unitary matrix. It is a singular value matrix. It is a rotation matrix;

[0126] Then solve for the translation vector ,for:

[0127]

[0128] Finally, the interior points of the RANSAC model are evaluated for all candidate point pairs. Calculate the transformation error :

[0129]

[0130] like If a point is identified by a preset threshold, it is marked as an interior point, and the number of interior points is counted.

[0131] Repeat the above steps for iterative optimization, and retain the set of point pairs with the most inliers as the set of inter-frame point pairs.

[0132] The inter-frame point pair set obtained by histogram matching is used to construct the inter-frame point pair factor residual, which can establish the inter-frame pose constraint of the radar. The weighted Doppler velocity factor residual establishes the single-frame pose constraint of the radar. Combined with the inertial sensor, a tightly coupled nonlinear factor graph optimization model is constructed to obtain the precise navigation and positioning information of the carrier. In this embodiment, the state of the tightly coupled nonlinear factor graph optimization model... for:

[0133]

[0134] Among them, among them, For the first The state of the inertial sensor at the moment the frame point cloud was acquired. The position of the carrier in the world coordinate system. Let the velocity of the carrier be in the world coordinate system. Let be the rotation quaternion from the carrier coordinate system to the world coordinate system. For the zero bias of the inertial sensor accelerometer, For the zero bias of the inertial sensor gyroscope, This represents the number of radar frames within the sliding window. The number of landmarks (feature points), For the first The location of a landmark in the world coordinate system ;

[0135] The objective function of the tightly coupled nonlinear factor graph optimization model is:

[0136]

[0137] in, For the inertial pre-integration factor residual, Let covariance matrix be the variance matrix. For the weighted Doppler velocity factor residual, For inter-frame point pair factor residuals, This indicates Huber's losses. I This refers to the set of inertial sensor acquisition frames between adjacent point cloud frames. D The geometry of radar points in the point cloud, The point-to-point residual weights are related to the number of observations at each radar point. Related, which is defined as:

[0138]

[0139] This indicates that points that are observed more times (match successfully more times) are considered more stable and are given higher weight.

[0140] The specific calculation process for the residual of the inertial pre-integration factor is as follows:

[0141]

[0142]

[0143] in, , , Let position error, velocity error, and rotation error represent the estimated relative displacement and the pre-integrated displacement, respectively. The difference between the estimated relative velocity change and the pre-integrated velocity. The difference between the estimated relative rotation and the pre-integrated rotation. difference; , For the zero bias error of the accelerometer and gyroscope, To the world coordinate system The rotation matrix of the carrier coordinate system at any given time. This is the gravity vector in the world coordinate system. For the first Frame to the Frame time interval, , Let the rotation quaternion from the system to the world coordinate system and its inverse be denoted as . The relative velocity change is obtained by pre-integration from the inertial sensor. , To represent the first Frame and the The frame's accelerometer has zero bias. , For the first Frame and the The frame's gyroscope has zero bias. This is the pre-integrated measurement value from the inertial sensor. , , The changes in position, relative velocity, and relative rotation are obtained by pre-integration of the inertial sensor.

[0144] The calculation process for the inter-frame point pair factor residual is as follows:

[0145]

[0146] in, Let be the rotation matrix from the carrier coordinate system to the world coordinate system. The rotation matrix from the radar coordinate system to the vehicle coordinate system is a pre-estimated value. This is the translation vector from the radar coordinate system to the carrier coordinate system.

[0147] It is worth noting that, although this embodiment Figure 1 , Figure 3The steps are shown sequentially as indicated by the arrows, but they are not necessarily executed in the order indicated by the arrows. Unless otherwise specified in this document, there is no strict order in which these steps are performed; they can be executed in other orders. Figure 1 , Figure 3 At least some of the steps in the process may include multiple sub-steps or multiple stages. These sub-steps or stages are not necessarily completed at the same time, but can be executed at different times. The execution order of these sub-steps or stages is not necessarily sequential, but can be executed in turn or alternately with other steps or at least some of the sub-steps or stages of other steps.

[0148] The above description is only a preferred embodiment of the present invention and does not limit the scope of protection of the present invention. All equivalent structural transformations made under the inventive concept of the present invention using the contents of the present invention specification and drawings, or direct / indirect applications in other related technical fields, are included within the scope of protection of the present invention.

Claims

1. A tightly coupled navigation and positioning method based on 4D millimeter-wave radar / inertial sensor, characterized in that, Includes the following steps: Step 1: Rasterize and partition the original point cloud of the 4D millimeter-wave radar, and perform dynamic point removal based on Doppler consistency on the original point cloud to obtain the first point cloud after dynamic point removal. Step 2: Remove noise from the first point cloud to obtain the second point cloud; Step 3: Calculate the Doppler velocity weights of the second point cloud in each directional interval of the rasterized partition, and construct the weighted Doppler velocity factor residual corresponding to the second point cloud in a single frame based on the Doppler velocity weights; Step 4: Perform histogram matching on the second point cloud of two adjacent frames to obtain a set of inter-frame point pairs, and construct the inter-frame point pair factor residual based on the set of inter-frame point pairs; Step 5: Combine the weighted Doppler velocity factor residual, the inter-frame point pair factor residual, and the inertial pre-integration factor residual to construct a tightly coupled nonlinear factor graph optimization model, and perform nonlinear factor graph optimization to obtain the navigation and positioning information of the carrier. Step 1, specifically the rasterization and partitioning of the original point cloud from the 4D millimeter-wave radar, includes: Assume the elevation angle FOV range of the 4D millimeter-wave radar is... The grid spacing is Then the pitch angle interval number for: in, , These are the lower and upper limits of the pitch angle FOV range, respectively. It is the floor function; No. i pitch angle range Defined as: Assume the azimuth field of view (FOV) range of the 4D millimeter-wave radar is... The grid spacing is Then the number of azimuth intervals for: in, , These are the lower and upper limits of the azimuth field of view (FOV), respectively. No. j pitch angle range Defined as: Based on the grid spacing within the azimuth and elevation FOV ranges, the original point cloud of the 4D millimeter-wave radar is rasterized into K directional intervals, where, ; In step 1, the dynamic point elimination based on Doppler consistency specifically includes: Extract points located in the same directional interval from the original point cloud and add them to the point set of the corresponding directional interval; For all points in each of the aforementioned direction interval point sets, those with Doppler velocity difference values ​​less than a first given threshold are merged into a Doppler velocity class, such that each of the aforementioned direction interval point sets forms at least one Doppler velocity class. Clustering based on Euclidean distance is performed on the point set corresponding to each Doppler velocity class, and points with a distance difference value less than a second given threshold are merged into a distance class, so that each Doppler velocity class forms at least one distance class; Filter out Doppler velocity classes whose number of distance classes exceeds the candidate class threshold, and define all distance classes contained in all filtered Doppler velocity classes as candidate classes; Within each candidate class, a point is selected to form a candidate point set, and the RANSAC algorithm is executed based on the candidate point set to estimate the coarse velocity of the carrier. Based on the coarse velocity, back-projection is performed on all points in the original point cloud to obtain the projected Doppler velocity corresponding to each point. The error between the projected Doppler velocity and the true Doppler velocity corresponding to each point is calculated. Points with errors greater than a third given threshold are removed from the original point cloud to obtain a transition point cloud. The RANSAC algorithm is executed again on all points contained in the transition point cloud to obtain an inner point set. The points in the inner point set are treated as static points and retained, while the outer points are removed. That is, all outer points determined by the RANSAC algorithm are removed from the transition point cloud to obtain the first point cloud.

2. The tightly coupled navigation and positioning method based on 4D millimeter-wave radar / inertial sensor according to claim 1, characterized in that, In step 2, the noise removal process for the first point cloud is as follows: Obtain two adjacent point clouds , ,in, For the first The first point cloud of the frame, For the first The first point cloud of the frame, , The first Frame and the The radar coordinate system of the frame; Based on the Frame and the The pose transformation matrix between the frame radar coordinate system will transform the point cloud Convert to the The point cloud is obtained from the radar coordinate system of the frame. ; For point clouds structure Tree and perform nearest neighbor search with a fixed radius, if point cloud A point in the radius It does not exist within Any neighboring point will be marked as a noise point and removed; Remove point clouds After removing all noise, we obtain the first... The second point cloud in the frame.

3. The tightly coupled navigation and positioning method based on 4D millimeter-wave radar / inertial sensor according to claim 1, characterized in that, In step 3, the Doppler velocity weights for each direction interval are obtained based on the number of points contained in each direction interval, i.e.: in, Direction interval Doppler velocity weights, Direction interval The number of static points included. k =1~ K .

4. The tightly coupled navigation and positioning method based on 4D millimeter-wave radar / inertial sensor according to claim 3, characterized in that, In step 3, the weighted Doppler velocity factor residual is: in, For the weighted Doppler velocity factor residual, To optimize the state of the tightly coupled nonlinear factor graph model. For the first point cloud The position vector of a radar point in the radar coordinate system For the first point cloud Doppler velocity measurements at each radar point The external parameter rotation matrix from the carrier coordinate system to the radar coordinate system. for The rotation matrix from the world coordinate system to the carrier coordinate system at any given moment. for The linear velocity of the carrier in the world coordinate system at any given moment; For the first point cloud Doppler velocity weights for each radar point; No. Doppler velocity weights of each radar point The calculation process is as follows: in, For the first Doppler velocity weights for the direction intervals to which each radar point belongs. For the first The elevation angle of a radar point in the point cloud.

5. The tightly coupled navigation and positioning method based on 4D millimeter-wave radar / inertial sensor according to claim 1, characterized in that, In step 4, the process of obtaining the inter-frame point pair set is as follows: Extract the key points from the second point cloud and construct a two-dimensional histogram for each key point; The key points of the second point cloud in two adjacent frames are combined pairwise to obtain several initial point pairs, and the histogram intersection of the neighborhood expansion of the two points in each initial point pair is calculated. ,for in, This represents the matrix corresponding to the two-dimensional histogram of the first point in the initial point pair. A The set of coordinates of non-zero elements. This represents the matrix corresponding to the two-dimensional histogram of the second point in the initial point pair. B The set of coordinates of non-zero elements. The neighborhood search radius is For matrix A The median coordinate is elements, For matrix B The median coordinate is elements, Distance weights Distance to Manhattan; Will satisfy The initial point pairs are used as candidate point pairs, where, The similarity threshold is used, and if a point forms a candidate point pair with multiple points, the point with the highest similarity is selected. The highest-ranking point is selected as the candidate point pair corresponding to that point. The RANSAC algorithm is executed based on all the candidate point pairs, and the set of point pairs with the most interior points is retained as the set of inter-frame point pairs.

6. The tightly coupled navigation and positioning method based on 4D millimeter-wave radar / inertial sensor according to claim 5, characterized in that, The process of extracting key points from the second point cloud is as follows: Divide all points in the second point cloud into corresponding directional intervals, and select the point with the strongest echo in each directional interval. These points are considered key points.

7. The tightly coupled navigation and positioning method based on 4D millimeter-wave radar / inertial sensor according to claim 6, characterized in that, The construction process of the two-dimensional histogram is as follows: All key points are combined into a key point set. ,in, For the first i One key point, The number of keypoints, each keypoint containing 3D coordinates and RCS: in, Key point The three-dimensional coordinates Key point RCS value, , For dimensions; For point set structure A tree, for each point in it Search for k nearest neighbors, and for each neighbor... Extract the two-dimensional features as follows: in, For point The three-dimensional coordinates For Euclidean distance, Nearest neighbor RCS value; Then point Two-dimensional histogram for: in, For indicator functions, , Binning is done along the distance dimension and the RCS dimension. 、 For indexing, s The number of bins in the distance dimension. t This represents the number of bins in the RCS dimension.

8. The tightly coupled navigation and positioning method based on 4D millimeter-wave radar / inertial sensor according to any one of claims 1 to 7, characterized in that, In step 5, the state of the tightly coupled nonlinear factor graph optimization model for: in, For the first The state of the inertial sensor at the moment the frame point cloud was acquired. The position of the carrier in the world coordinate system. Let the velocity of the carrier be in the world coordinate system. Let be the rotation quaternion from the carrier coordinate system to the world coordinate system. For the zero bias of the inertial sensor accelerometer, For the zero bias of the inertial sensor gyroscope, This represents the number of radar frames within the sliding window. For the number of landmarks, For the first The location of each landmark in the world coordinate system ; The objective function of the tightly coupled nonlinear factor graph optimization model is: in, For the inertial pre-integration factor residual, Let covariance matrix be the variance matrix. For the weighted Doppler velocity factor residual, For the first point cloud The position vector of a radar point in the radar coordinate system For the first point cloud Doppler velocity measurements at each radar point For the first point cloud Doppler velocity weights for each radar point For inter-frame point pair factor residuals, This indicates Huber's losses. I This refers to the set of inertial sensor acquisition frames between adjacent point cloud frames. D The geometry of radar points in the point cloud, Point-to-point residual weights; in, , , Let position error, velocity error, and rotation error represent the estimated relative displacement and pre-integrated displacement, respectively. The difference between the estimated relative velocity change and the pre-integrated velocity. The difference between the estimated relative rotation and the pre-integrated rotation. difference; , For the zero bias error of the accelerometer and gyroscope, To the world coordinate system The rotation matrix of the carrier coordinate system at any given time. This is the gravity vector in the world coordinate system. For the first Frame to the Frame time interval, , Let the rotation quaternion from the system to the world coordinate system and its inverse be denoted as . The relative velocity change is obtained by pre-integration from the inertial sensor. , To represent the first Frame and the The frame's accelerometer has zero bias. , For the first Frame and the The frame's gyroscope has zero bias. This is the pre-integrated measurement value from the inertial sensor. , , The changes in position, relative velocity, and relative rotation are obtained by pre-integration of the inertial sensor. in, Let be the rotation matrix from the carrier coordinate system to the world coordinate system. The rotation matrix from the radar coordinate system to the vehicle coordinate system is a pre-estimated value. This is the translation vector from the radar coordinate system to the carrier coordinate system.

Images