High-precision ground unmanned platform autonomous positioning method and system

Abstract

The invention relates to a high-precision autonomous positioning method and system for a ground unmanned platform, belongs to the technical field of unmanned vehicle positioning, and solves the problem of low positioning precision under the conditions of severe weather and limited satellite positioning in the prior art. The method comprises the following specific steps: collecting laser radar point cloud, 4D millimeter wave radar point cloud and inertial navigation data of a ground unmanned platform to obtain multi-modal data; dynamic point clouds in the 4D millimeter wave radar point clouds are screened; based on the dynamic point cloud, performing secondary noise reduction on the laser radar point cloud after preliminary noise reduction to obtain a laser radar point cloud after noise reduction; and estimating an initial pose of the ground unmanned platform based on the de-noised laser radar point cloud, performing global optimization on the initial pose by using a factor graph in combination with the multi-modal data, and updating the optimized pose to a global map to obtain a positioning result of the ground unmanned platform. And the positioning robustness and precision under the conditions of severe weather or limited satellite positioning are obviously improved.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned vehicle positioning, and particularly to a high-precision autonomous positioning method and system for ground unmanned platforms. Background Art

[0002] With the continuous development of autonomous driving technology, a large number of autonomous driving algorithms have been put into use, and the positioning algorithm in unknown or known environments is a supporting technology for autonomous driving technology. Previous autonomous driving systems relied on the Global Navigation Satellite System (GNSS) to provide absolute positioning information such as longitude, latitude, and altitude of the vehicle on the earth. However, under the condition of GNSS denial, this positioning method is no longer reliable.

[0003] With the development of SLAM (Simultaneous Localization and Mapping) technology, most of the existing positioning technologies rely on sensor-based SLAM algorithms, including visual SLAM and lidar SLAM. The basic principle of lidar (Light Detection and Ranging, LiDAR) is to emit laser light towards the target and measure the reflected echo information. The distance to the target can be calculated using ToF (Time of Flight), thus generating a point cloud measurement. The quality of the point cloud is related to the accuracy and reliability of subsequent positioning and mapping tasks. Most of the existing lidars use lasers with two wavelengths of 905nm and 1550nm. The lasers of these two wavelengths have poor penetration ability for droplets and dust in the air and can only work under good weather conditions such as sunny days. For light rain, light snow, and even more severe heavy rain, heavy snow, haze, sand and dust and other weather conditions, it is difficult for lidar to ensure the quality of point cloud measurement. Therefore, the algorithms for positioning and mapping using lidar and IMU (Inertial Measurement Unit) are difficult to work in bad weather.

[0004] Different from lidar, 4D millimeter wave radar (Millimeter Wave Radar, mmWave Radar) is a relatively new sensor that uses millimeter-wave electromagnetic waves as measurement signals. Since the electromagnetic waves emitted by itself have a longer wavelength, it can effectively penetrate rain, snow, haze, and still maintain a high measurement quality in bad weather. However, the point cloud of 4D millimeter wave radar is sparser than that of lidar. The typical number of point clouds is 2,000 (the typical value of lidar point cloud number is 100,000). And due to the influence of multipath effects, the point cloud of millimeter wave radar is full of noise and is difficult to be directly used for the positioning task. Directly using existing algorithms often brings large positioning errors or even leads to positioning failures. Summary of the Invention

[0005] In view of the above analysis, embodiments of the present invention aim to provide a high-precision autonomous positioning method and system for ground unmanned platforms to solve the problem of low positioning accuracy in the prior art under bad weather and limited satellite positioning conditions.

[0006] The purpose of the present invention is mainly achieved through the following technical solutions:

[0007] On the one hand, embodiments of the present invention provide a high-precision autonomous positioning method for ground unmanned platforms, including the following steps:

[0008] Collect the lidar point cloud, 4D millimeter wave radar point cloud and inertial navigation data of the ground unmanned platform to obtain multi-modal data;

[0009] Screen the dynamic point cloud in the 4D millimeter wave radar point cloud;

[0010] Based on the dynamic point cloud, perform secondary noise reduction on the preliminarily noise-reduced lidar point cloud to obtain a noise-reduced lidar point cloud;

[0011] Estimate the initial pose of the ground unmanned platform based on the noise-reduced lidar point cloud, combine the multi-modal data, use a factor graph to globally optimize the initial pose, and update the optimized pose to the global map to obtain the positioning result of the ground unmanned platform.

[0012] Further, screening the dynamic point cloud includes:

[0013] Based on the Doppler velocity of each point in the current frame of 4D millimeter wave radar point cloud, use the least squares method to obtain the estimated self-velocity of the ground unmanned platform corresponding to the current frame;

[0014] Use an incremental non-convex optimizer to optimize the estimated self-velocity to obtain the optimized self-velocity of the ground unmanned platform corresponding to the current frame;

[0015] For each point in the current frame of the 4D millimeter-wave radar point cloud, if the difference between the estimated self-speed of the ground unmanned platform after optimization and the Doppler speed of this point is greater than a preset threshold, then this point is a dynamic point; otherwise, this point is a static point.

[0016] Further, an incremental non-convex optimizer is used to optimize the estimated self-speed, and the optimization objective is expressed as:

[0017]

[0018] where w i is the corresponding weight of the i-th point; represents the difference between the measured Doppler speed and the predicted Doppler speed of the i-th point; is the measured Doppler speed of the i-th point in the current frame of the 4D millimeter-wave radar point cloud; R v m is the estimated self-speed of the ground unmanned platform corresponding to the current frame; ρ i is the radial direction vector of the i-th point; μ is the non-convexity control parameter, σ r is the measurement accuracy of the Doppler speed.

[0019] Further, the lidar point cloud after preliminary noise reduction is subjected to secondary noise reduction, including:

[0020] Using a bird's-eye view to cluster each point in the lidar point cloud after preliminary noise reduction;

[0021] Fitting a 3D bounding box to the clustering result and projecting the dynamic point cloud into the clustering result;

[0022] Removing the lidar point cloud within the 3D bounding box where the dynamic point cloud is located to obtain the noise-reduced lidar point cloud.

[0023] Further, combining the multi-modal data, a factor graph is used to globally optimize the initial pose, including:

[0024] Using the inertial navigation data to perform motion de-distortion on the noise-reduced lidar point cloud data;

[0025] Based on the de-distorted lidar point cloud, pose estimation is performed to construct the corresponding odometry factor;

[0026] Based on the inertial navigation data, an IMU pre-integration factor is constructed;

[0027] Based on the self-speed estimation of the 4D millimeter-wave radar point cloud, the estimated self-speed of the ground unmanned platform corresponding to the corresponding frame is obtained, and a self-speed estimation factor is constructed;

[0028] Construct a factor graph with the corresponding odometer factor, IMU pre-integration factor, and self-speed estimation factor as constraints to optimize the initial pose of the ground unmanned platform.

[0029] Further, the expression of the self-speed estimation factor is:

[0030]

[0031] Where, is the self-speed estimation factor; is the rotation matrix from the vehicle radar coordinate system to the navigation coordinate system ; is the estimated self-speed at the i-th moment in the vehicle radar coordinate system ; is the self-speed transformed to the navigation coordinate system ; v i is the speed at the i-th moment in the constructed system state measurement; is the covariance of the speed ; is the Mahalanobis norm; is the measurement set at the corresponding moment of the system state to be optimized.

[0032] Further, constructing the corresponding odometer factor based on the undistorted lidar point cloud includes:

[0033] Establish a measurement equation between the undistorted lidar point cloud and the map;

[0034] Use the minimization of the residual of the measurement equation to obtain the estimated pose of the ground unmanned platform corresponding to the current frame after optimization;

[0035] When the residual after optimization is less than the preset threshold, use the relative pose between this frame and the map points as the corresponding odometer factor; otherwise, calculate the corresponding odometer factor using the 4D millimeter-wave radar point cloud.

[0036] Further, the factor graph also includes a loop detection factor, and the optimization objective of the factor graph is expressed as:

[0037]

[0038] Where, is the system state including the pose to be optimized; is the self-speed estimation factor,

[0039] is the corresponding odometer factor, is the IMU pre-integration measurement factor, is the loop detection factor.

[0040] Furthermore, the laser radar point cloud is subjected to preliminary noise reduction, including:

[0041] Perform intensity measurement on the collected LiDAR point cloud to obtain an intensity map;

[0042] Performing Gaussian filtering on the intensity map to filter out pixel points in the filtered intensity map whose pixel values ​​are less than a first threshold and / or whose pixel value changes are greater than a second threshold;

[0043] The screened pixel points are subjected to inverse projection transformation to obtain the corresponding lidar point cloud, and the corresponding lidar point cloud is removed to obtain the lidar point cloud with preliminary noise reduction.

[0044] On the other hand, an embodiment of the present invention provides a high-precision ground unmanned platform autonomous positioning system, including:

[0045] The acquisition module is used to collect the laser radar point cloud, 4D millimeter wave radar point cloud and inertial navigation data of the ground unmanned platform to obtain multi-modal data;

[0046] A preprocessing module is used to screen the dynamic point cloud in the 4D millimeter wave radar point cloud, and based on the dynamic point cloud, perform secondary denoising on the laser radar point cloud after preliminary denoising to obtain the denoised laser radar point cloud;

[0047] A posture estimation module, used for estimating the initial posture of the ground unmanned platform based on the de-noised laser radar point cloud;

[0048] An optimization module, configured to use the initial posture as a starting point, combine the multimodal data, and use a factor graph to globally optimize the initial posture;

[0049] The map update module is used to update the optimized posture to the global map to obtain the positioning result of the ground unmanned platform.

[0050] Compared with the prior art, the present invention can achieve at least one of the following beneficial effects:

[0051] 1. The present invention proposes to utilize the penetration ability of millimeter waves on rain, snow, fog and haze, and after guiding the denoising of the lidar point cloud based on the 4D millimeter wave radar point cloud, the laser lightning point cloud, inertial navigation data and the 4D millimeter wave radar point cloud are integrated to locate the ground unmanned platform. Compared with the positioning method based on pure lidar, the robustness and accuracy of its positioning are significantly improved under the conditions of severe weather or restricted satellite positioning conditions.

[0052] 2. To address the noise and point cloud sparsity issues of 4D millimeter-wave radar, such as the dynamic points caused by the reflection points of moving objects that interfere with the perception of the static environment, a progressively non-convex optimizer is used to remove noise or dynamic points in the point cloud based on the measured Doppler velocity, and obtain the speed estimate of the vehicle's motion, effectively improving the point cloud quality, more accurately estimating the vehicle's speed, and thus improving the SLAM positioning accuracy.

[0053] 3. For the denoised lidar point cloud, a direct method is used to perform frame-map registration on the point cloud to obtain the estimated pose. Based on the odometer factor and IMU pre-integration factor, combined with the vehicle speed estimate provided by 4D millimeter-wave radar observation, the estimated pose of the ground unmanned platform is further optimized as a constraint. This fusion method can significantly improve the robustness of the system, while efficiently utilizing multi-sensor data, avoiding repeated processing of redundant data, and improving the computational efficiency of the system.

[0054] 4. In order to solve the problem of lidar positioning failure caused by a sharp drop in the number of lidar point clouds under extreme conditions, a registration method based on 4D millimeter-wave radar point clouds is provided to calculate the odometer and positioning pose, ensuring the robustness of the algorithm in extreme weather conditions.

[0055] 5. In order to solve the problem of absorption or reflection noise in the point cloud of LiDAR in bad weather, especially in rain, snow and haze, a filtering noise reduction method based on intensity map projection is proposed to improve the positioning performance of LiDAR in high-density noise point cloud conditions.

[0056] In the present invention, the above-mentioned technical solutions can also be combined with each other to achieve more preferred combination solutions. Other features and advantages of the present invention will be described in the subsequent description, and some advantages can become obvious from the description, or can be understood by practicing the present invention. The purpose and other advantages of the present invention can be realized and obtained through the contents particularly pointed out in the description and the drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0057] The drawings are only for the purpose of illustrating specific embodiments and are not to be considered limiting of the present invention. Like reference symbols denote like components throughout the drawings.

[0058] Figure 1 This is a flow chart of a high-precision ground unmanned platform autonomous positioning method according to an embodiment of the present invention;

[0059] Figure 2 This is a basic framework diagram of a laser radar positioning algorithm in severe weather conditions that incorporates a 4D millimeter-wave radar according to an embodiment of the present invention;

[0060] Figure 3 It is a schematic diagram of the projection of the laser radar intensity map according to an embodiment of the present invention;

[0061] Figure 4 This is a flowchart of point cloud preprocessing using 4D millimeter-wave radar and lidar intensity maps in an embodiment of the present invention;

[0062] Figure 5 This is a schematic diagram of a factor graph used in optimizing the pose in an embodiment of the present invention;

[0063] Figure 6 This is a comparison schematic diagram of lidar point clouds before and after noise reduction in an embodiment of the present invention

[0064] Figure 7 This is a test schematic diagram of the main positioning method for a high-precision ground unmanned platform in an embodiment of the present invention;

[0065] Figure 8 This is a schematic diagram of the autonomous positioning of a ground unmanned platform in a rainy test environment in an embodiment of the present invention;

[0066] Figure 9 This is a schematic diagram of the autonomous positioning of a ground unmanned platform in a snowy test environment in an embodiment of the present invention. Detailed implementation manners

[0067] The following will specifically describe the preferred embodiments of the present invention in conjunction with the accompanying drawings. The accompanying drawings form a part of this application and are used together with the embodiments of the present invention to explain the principle of the present invention, rather than to limit the scope of the present invention.

[0068] Embodiment 1

[0069] A specific embodiment of the present invention discloses a high-precision autonomous positioning method for a ground unmanned platform, as Figure 1 shown, including the following steps:

[0070] Step S1, collect lidar point clouds, 4D millimeter-wave radar point clouds and inertial navigation data of the ground unmanned platform to obtain multi-modal data;

[0071] Step S2, screen the dynamic point clouds in the 4D millimeter-wave radar point clouds;

[0072] Step S3, based on the dynamic point clouds, perform secondary noise reduction on the preliminarily noise-reduced lidar point clouds to obtain noise-reduced lidar point clouds;

[0073] Step S4, estimate the initial pose of the ground unmanned platform based on the noise-reduced lidar point clouds, combine the multi-modal data, globally optimize the initial pose using a factor graph, and update the optimized pose to the global map to obtain the positioning result of the ground unmanned platform.

[0074] Through the above method, the 4D millimeter-wave radar point cloud collected is used to guide the noise reduction of the lidar point cloud. Based on the denoised lidar point cloud, the initial pose is estimated, and then the lidar point cloud, inertial navigation data, and 4D millimeter-wave radar point cloud are fused. The factor graph is used to optimize the initial pose of the ground unmanned platform, and the final positioning result is obtained by updating the map according to the optimized pose, avoiding the problem of pure lidar positioning failure in bad weather and improving the robustness of the positioning algorithm.

[0075] Specifically, in step S1, the ground unmanned platform is equipped with a lidar, a 4D millimeter-wave radar, and an IMU to collect lidar point cloud, 4D millimeter-wave radar point cloud, and inertial navigation data to form multi-modal data.

[0076] Among them, the 4D millimeter-wave radar can not only provide a 3D point cloud similar to that of the lidar, but also provide the measurement of Doppler velocity (radial velocity), and the point cloud density and point cloud signal-to-noise ratio (SNR) are greatly improved compared with traditional millimeter-wave radars. When fusing multi-modal data for positioning and mapping, multi-modal data for a period of time will be collected in real time for positioning optimization. Usually, the first frame of lidar point cloud collected is used as the initial key frame, and only the optimized key frames are updated to the global map subsequently.

[0077] Specifically, in step S2, for the collected 4D millimeter-wave radar point cloud, an incremental non-convex optimizer is used to obtain the vehicle speed (ego-speed) corresponding to the current frame and remove the dynamic points in the 4D millimeter-wave radar point cloud. Specifically, it includes:

[0078] S21. Construct an objective function based on the Doppler velocities measured at each point in the current frame of 4D millimeter-wave radar point cloud, and use the least squares method to estimate the ego-speed of the ground unmanned platform corresponding to the current frame; the objective function J is expressed as:

[0079]

[0080] Among them, is the Doppler velocity measured at the i-th point in the current frame of 4D millimeter-wave radar point cloud; n is the total number of measurement points in the 4D millimeter-wave radar point cloud of this frame; R v m is the estimated ego-speed of the ground unmanned platform corresponding to the current frame; ρ i is the radial direction vector of the i-th point, represents the Doppler velocity predicted by the estimated ego-speed.

[0081] S22. Using the estimated ego-speed of the ground unmanned platform in the current frame as the initial value, construct an incremental non-convex optimization problem to further optimize the estimated ego-speed, and the optimization objective is expressed as:

[0082]

[0083] where, w i is the corresponding weight of the i-th point, and its value range is [0, 1], which is used to represent the contribution of different measurement points to the optimization goal. The purpose is to reduce the weight of dynamic points (outliers) and increase the weight of static points (inliers) to obtain the estimated self-speed; represents the residual of the i-th point, which is the difference between the measured Doppler velocity and the predicted Doppler velocity at the i-th point, and μ is the non-convexity control parameter, σ r is the measurement accuracy of the Doppler velocity. Initially, w i = 1, and the optimization problem is convex. As μ is adjusted, w i changes, making the optimization problem gradually lose its convexity.

[0084] S23. By iteratively updating the non-convexity control parameter μ and updating the optimization problem, the optimal estimated self-speed of the ground unmanned platform corresponding to the current frame is finally obtained;

[0085] Exemplarily, initialize μ: is the maximum residual, that is, the maximum value in. Starting from the initial value, gradually decrease μ, such as μ ← μ / 1.4. For each updated μ value, solve it until μ < 1, then the iteration stops, and the self-speed of the ground unmanned platform corresponding to the current frame is obtained.

[0086] S24. Based on the estimated self-speed of the ground unmanned platform corresponding to the current frame, calculate the residual between the measured value and the estimated value of the Doppler velocity of each point in the millimeter-wave radar point cloud, and judge the dynamic points.

[0087] Exemplarily, if the residual corresponding to the point in the 4D millimeter-wave radar point cloud of the current frame, then it is considered that this point is a static point; otherwise, it is considered a dynamic point (i.e., a noise point). Judge the residual values of all points in the current frame in turn to obtain the dynamic point cloud in the 4D millimeter-wave radar point cloud of the current frame.

[0088] Specifically, in step S3, the specific steps for preliminary noise reduction of the lidar point cloud include:

[0089] S311. Perform intensity measurement on the collected lidar point cloud to generate a sparse intensity map and establish the correspondence between points and pixels;

[0090] Exemplarily, project the points in the lidar onto a cylinder with a radius of r in the following way, as Figure 3 shown, and the corresponding intensity map expression is obtained as:

[0091]

[0092] Among them, C p j is the j-th point on the intensity map; is the position coordinate of the lidar point cloud corresponding to point j; c x and c y are the horizontal and vertical coordinates of the center of the pixel coordinate system respectively, φ is the azimuth angle (the angle with the x-axis) of the lidar point cloud corresponding to point j, θ is the elevation angle (the angle with the z-axis) of the lidar point cloud corresponding to point j, f x and f y are the focal lengths in the x and y directions respectively; Θ is the vertical field of view (VFoV) of the lidar, and w and h are the horizontal and vertical resolutions of the lidar respectively; is the pixel coordinate corresponding to point j on the intensity map; r is the distance between the lidar center and the receiver, that is, the distance between the origin of the lidar coordinate system and the lidar receiver.

[0093] S312. Perform Gaussian filtering on the intensity map of the lidar using a Gaussian filter with a kernel of 3×3 to filter the above intensity map;

[0094] S313. Use the first threshold to screen out the pixel points with pixel values less than the first threshold in the filtered intensity map, and use the second threshold to screen out the pixel points with pixel value changes greater than the second threshold;

[0095] Exemplarily, any point on the filtered intensity map is denoted as C p′ j ; Take the difference between the intensity maps before and after filtering to obtain the intensity difference Δ j = C p′ j - C p j ; When Δ j > Δ threshold , and Δ threshold is the preset second threshold, the lidar point corresponding to this pixel needs to be removed. Similarly, set the first threshold. When any point C p′ j is less than the first threshold, the lidar point corresponding to this pixel point needs to be removed.

[0096] Among them, the first threshold and the second threshold are determined according to a large amount of experimental data of lidar point clouds before and after filtering. For points with small pixel values, their reflection ability is weak, and the corresponding point clouds may be unreliable and contribute little to subsequent analysis; while for abnormal points or points affected by noise in the point cloud, the pixel differences before and after Gaussian filtering are large. Therefore, the corresponding pixel points are screened out using the first threshold and the second threshold respectively, and then removed to obtain a more effective lidar point cloud.

[0097] S314. According to the inverse projection transformation of the screened pixel points, the corresponding lidar point cloud is obtained, and the lidar point cloud data corresponding to the pixel points is removed to overcome the high-frequency noise generated in the lidar point cloud due to bad weather. The inverse transformation expression is:

[0098]

[0099] where Π -1 is the inverse projection transformation, and this transformation is a one-to-many mapping, and only the point with the minimum modulus length after the inverse transformation is taken.

[0100] The specific steps of using the 4D millimeter-wave radar point cloud to guide the lidar point cloud data for secondary noise reduction include:

[0101] S321. Use the Bird’s-eye View (BEV) to cluster each point in the preliminarily noise-reduced lidar point cloud;

[0102] S322. Fit a 3D bounding box to the clustering result and project the dynamic 4D millimeter-wave radar point cloud into the clustering result;

[0103] S323. Remove the lidar point cloud within the 3D bounding box where the dynamic point cloud is located to obtain the noise-reduced lidar point cloud. Among them, if there is a 3D bounding box that falls into the 4D millimeter-wave radar point, it means that the corresponding lidar point cloud clustering also belongs to a dynamic object, then the corresponding clustering is marked as a dynamic or noise category, and then the corresponding lidar point cloud data is removed to obtain the noise-reduced lidar point cloud dataset.

[0104] In the above way, for each frame of lidar or 4D millimeter-wave radar measurement collected, the above processing is repeated. As Figure 4 shown, using the dynamic point cloud of the 4D millimeter wave to guide the secondary noise reduction of the lidar improves the point cloud quality and lays a foundation for subsequent accurate positioning.

[0105] Specifically, in step S4, the steps of performing autonomous positioning of the ground unmanned platform based on the processed lidar point cloud include:

[0106] S41. Use the IMU data to perform motion de-distortion on the noise-reduced lidar point cloud to eliminate the geometric distortion caused by the sensor itself;

[0107] Exemplarily, align the timestamps of the lidar point cloud and the IMU data to ensure data synchronization; preprocess the IMU data to remove the influence of gravity and convert its coordinates to the lidar coordinate system; integrate the angular velocity of the IMU to calculate the rotation increment at each moment relative to the starting moment; traverse the lidar point cloud data, calculate the IMU pose information at the corresponding timestamp of each point, and combine the calculated rotation increment to transform each lidar point cloud to the coordinate system at the start of the scan, thereby correcting the motion distortion.

[0108] S42. Estimate the pose of the ground unmanned platform corresponding to the current frame of lidar point cloud after distortion removal by the direct method, and optimize the estimated pose to obtain the initial pose of the estimated ground unmanned platform.

[0109] Specifically, establish a registration equation between the lidar point cloud and the map, that is, the system state measurement equation, to describe the relationship between the lidar point cloud and the existing map.

[0110] Exemplarily, transform the lidar point L p j in the radar coordinate system to the global navigation coordinate system. After transformation, it should match the corresponding point in the global map and satisfy the following equation:

[0111]

[0112] where is the normal vector of the plane fitted by the point in the global map corresponding to the current point; represents the transformation from the current radar coordinate system to the global map coordinate system G; L n j is the noise caused by the inaccurate position of the radar measurement itself; G q j is the point in the global map coordinate system (abbreviated as the global coordinate system) G.

[0113] Map the lidar point to the observation space through the observation model, and establish a connection between the measurement data of the lidar and the system state. Then the above measurement equation can be expressed as:

[0114] 0 = h j (x k , L p j + L n j ),

[0115]

[0116] where h j () represents the observation model function; x kThe system state at time k to be optimized, including: the rotation matrix from coordinate system I to the global coordinate system G The position vector of coordinate system I in the global coordinate system G The velocity vector of coordinate system I in the global coordinate system G The deviation vector of the angular velocity The deviation vector of the acceleration The gravity vector in the global coordinate system G G g T and the rotation matrix from the radar coordinate system L to coordinate system I The position vector of the radar coordinate system L in coordinate system I is a manifold, expressed as:

[0117] Composed of the Cartesian product of 2 SO(3) and two Euclidean spaces and with a total dimension of 24; SO(3) is the special orthogonal group, and the elements in it can represent rotations in three-dimensional space.

[0118] Transform the above measurement equation into an optimization problem to solve the optimized pose. Among them, the optimization objective is:

[0119]

[0120] It should be noted that if the residual is still large after the lidar measurement is optimized by the direct method residual, such as greater than the preset threshold, then perform point cloud registration on the 4D millimeter-wave radar closest to the time of the above two-frame lidar measurement (there is a time difference in radar signals) to obtain the millimeter-wave radar odometer factor. Exemplarily, the preset threshold is taken as 0.5, and the 3-fold static state residual is set as this threshold by testing the registration residual when the vehicle is stationary, and it can be adjusted according to the actual situation specifically.

[0121] Exemplarily, the optimization objective of the 4D millimeter-wave radar point cloud registration is expressed as:

[0122]

[0123] where T ＊ is the relative pose between the two frames of point clouds to be optimized; represents the matching degree between the point i in the source point cloud and the target point cloud under the transformation T; represents the probability distribution of the matching degree, such as calculating this probability using a Gaussian distribution model.

[0124] S43. Taking the initial pose of the estimated ground unmanned platform as the starting point of optimization, combining multi-modal data, and using a factor graph to globally optimize the above estimated initial pose, specifically including:

[0125] S431. Construct the corresponding odometry factor based on the lidar point cloud;

[0126] Exemplarily, according to the initial pose estimated in step S42, when optimizing with the lidar point cloud, obtain the relative pose change between the current frame lidar points and the map points as the corresponding odometry factor; similarly, when registering the 4D millimeter-wave radar point cloud due to large residuals, use the relative pose change of the 4D millimeter-wave as the corresponding odometry factor.

[0127] S432. Construct the IMU pre-integration factor based on the inertial navigation data;

[0128] Exemplarily, accumulate the IMU measurements between two frames of lidar to construct the IMU pre-integration measurement and pre-integration factor. The measurement of the IMU is expressed as:

[0129]

[0130] where ω t and a t are the angular velocity and acceleration at time t respectively, and are the original IMU measurement values in the IMU coordinate system B at time t. ω t and a t are affected by the slowly varying bias and white noise n t . is the rotation matrix from the navigation coordinate system to the IMU coordinate system B. g is the gravity vector in the navigation coordinate system . Using the above results, construct the system motion from the IMU measurements by integration to obtain the following IMU pre-integration factor:

[0131]

[0132] where Δv ij represents the velocity difference between times i and j; Δp ij represents the position difference between times i and j; ΔR ij represents the rotation difference between times i and j; represents the rotation matrix at time i; v i represents the velocity at time i; p i represents the position at time i; Δt ij represents the time interval between times i and j.

[0133] S433. For all 4D millimeter-wave radar measurements between two frames, accumulate the self-velocity estimates obtained in step S2 to construct the self-velocity estimate factor;

[0134] Exemplarily, the self-speed estimation provides an observation of the system speed while providing a constraint on the system state, manifested as the self-speed estimation residual , and the expression is:

[0135]

[0136] where e v is the self-speed estimation factor; represents the covariance corresponding to the speed ; represents the Mahalanobis norm, and is the self-speed transformed to the navigation system ; and is the vehicle radar coordinate system to the navigation (global) coordinate system rotation matrix, and O(3) is the special orthogonal group; is the self-speed estimation at the i-th moment in the vehicle radar coordinate system . If there is no corresponding speed estimation obtained from the 4D millimeter-wave radar point cloud at the i-th moment, the speed estimation closest to the i-th moment in the above cumulative measurements is searched for as the self-speed estimation at that moment; v i is the speed at the i-th moment in the constructed system state measurement; is the set of corresponding moments of the system state to be optimized in the above paragraph.

[0137] S434. Add the corresponding odometer factor, IMU pre-integration factor, and self-speed estimation factor to the factor graph for optimization to obtain the optimized pose.

[0138] Exemplarily, construct a factor graph as shown in Figure 5 , and its optimization objective is:

[0139]

[0140] where is the system state quantity including the pose to be optimized; is the residual corresponding to the self-speed estimation factor, is the radar odometer residual, is the residual corresponding to the IMU pre-integration measurement, Residuals corresponding to loop constraints (which may occur). For loop detection, we can use the Scan-Context based descriptor method. Specifically, for a key frame, calculate its corresponding Scan-Context descriptor, and then add it to the retrieval of global descriptors. Whenever a new key frame appears, use Scan-Context to construct a KD-Tree for nearest neighbor indexing, retain the first 3 candidate key frames of the index, and use a point cloud registration method such as ICP to calculate their matching scores. If the score is small (high degree of matching), then retain the candidate frame closest to the current frame as the loop frame and calculate the loop constraint; otherwise, reject this loop detection.

[0141] S435. Update the global map according to the optimized pose; usually, only update the key frames to the global map.

[0142] Exemplarily, according to the relative displacement and angle reported by the odometer between two frames, determine whether the current frame is a key frame. If so, add the current frame to the global map; wherein, the key frames are preset according to the odometer information, including:

[0143] Set the first frame of lidar point cloud data sampled each time as the initial key frame;

[0144] Use the odometer to measure the relative displacement and relative rotation angle of the lidar point cloud data of the current frame and the previous frame;

[0145] If the relative displacement exceeds the preset displacement threshold, or the relative rotation angle exceeds the preset angle threshold, then the current frame is considered a key frame. For example, if the displacement corresponding to two frames exceeds 1m, or the relative rotation angle exceeds 10°, then the current frame is considered to form a key frame, and the pose of the frame point cloud in the odometer report is added to the global map.

[0146] To verify the effect of this positioning method, the embodiments of the present invention tested the above algorithm in an environment simulating snowfall and foggy weather indoors. As Figure 6 shown, part (a) is a schematic diagram of the cloud-like noise point cloud that appears in front of the lidar before noise reduction, part (b) is a schematic diagram of a large number of artifacts and noises that appear in the lidar point cloud, and part (c) is the lidar point cloud after noise reduction by the present method in the above situation. It can be seen that by fusing the 4D millimeter-wave radar, the noise in the lidar point cloud under bad weather is successfully removed, improving the overall robustness of the positioning.

[0147] As Figure 7 shown, test the autonomous positioning of the ground unmanned platform, set the inspection path points, and record the positioning results and true poses output each time passing through the inspection points when the vehicle is running. The test indicators include positioning error and average positioning accuracy.

[0148] The positioning error Δ ij is calculated as follows:

[0149]

[0150] wherein, represents the pose estimation output by the positioning kit when the unmanned platform travels to the i-th inspection point and the j-th lap, represents the true pose when the unmanned platform travels to the i-th inspection point and the j-th lap.

[0151] The average positioning accuracy is as follows:

[0152]

[0153] The positioning statistical results in bad weather are obtained through experiments, as shown in Table 1:

[0154] Table 1

[0155]

[0156]

[0157] It can be seen that even in bad weather, the present invention can still achieve extremely high positioning accuracy, and the average positioning accuracy can reach about 7 mm, greatly improving the robustness of the lidar positioning method in bad weather, as Figure 8 and Figure 9 shown. Compared with the positioning method of pure lidar, such as LIO-SAM, in this experimental environment, it cannot perform normal positioning.

[0158] Compared with the prior art, a high-precision autonomous positioning method for a ground unmanned platform provided in this embodiment, as Figure 2 shown, uses the intensity map of the lidar to pre-denoise the lidar point cloud in bad weather, extracts the dynamic point cloud in the 4D millimeter-wave radar self-speed estimation, uses the dynamic point cloud to guide the secondary denoising of the pre-denoised lidar point cloud, constructs the self-speed factor using the estimated self-speed, combines the IMU data and the lidar point cloud, optimizes the pose using the factor graph, obtains the optimized pose of the unmanned platform and updates it to the map, and finally realizes the autonomous positioning of the ground unmanned platform. The robustness and accuracy of its positioning are significantly improved under bad weather conditions.

[0159] Embodiment 2

[0160] Another specific embodiment of the present invention discloses a high-precision autonomous positioning system for a ground unmanned platform, including:

[0161] The acquisition module is used to collect the laser radar point cloud, 4D millimeter wave radar point cloud and inertial navigation data of the ground unmanned platform to obtain multi-modal data;

[0162] A preprocessing module is used to screen the dynamic point cloud in the 4D millimeter wave radar point cloud, and based on the dynamic point cloud, perform secondary denoising on the laser radar point cloud after preliminary denoising to obtain the denoised laser radar point cloud;

[0163] A posture estimation module, used for estimating the initial posture of the ground unmanned platform based on the de-noised laser radar point cloud;

[0164] An optimization module, configured to use the initial posture as a starting point, combine the multimodal data, and use a factor graph to globally optimize the initial posture;

[0165] The map update module is used to update the optimized posture to the global map to obtain the positioning result of the ground unmanned platform.

[0166] The system can perform autonomous positioning according to the autonomous positioning method for a ground unmanned platform described in any one of the solutions in Example 1. The relevant parts are referenced from each other and are not described repeatedly in this embodiment.

[0167] Compared with the prior art, this embodiment provides a high-precision ground unmanned platform autonomous positioning system, which is based on an acquisition module, a preprocessing module, a posture estimation module, an optimization module and a map update module. It is a system that performs autonomous positioning in severe weather and under conditions where satellite positioning is restricted, combined with a 4D millimeter-wave radar. The system is suitable for autonomous positioning in complex environments and improves the positioning accuracy of ground unmanned platforms.

[0168] Those skilled in the art will appreciate that all or part of the processes of the above-mentioned embodiments can be implemented by instructing related hardware through a computer program, and the program can be stored in a computer-readable storage medium, wherein the computer-readable storage medium is a disk, an optical disk, a read-only storage memory, or a random access memory, etc.

[0169] The above description is only a preferred specific implementation manner of the present invention, but the protection scope of the present invention is not limited thereto. Any changes or substitutions that can be easily conceived by any technician familiar with the technical field within the technical scope disclosed by the present invention should be covered within the protection scope of the present invention.

Claims

1. A high-precision ground unmanned platform autonomous positioning method, characterized in that: The steps include: Collect laser radar point cloud, 4D millimeter wave radar point cloud and inertial navigation data of ground unmanned platforms to obtain multimodal data; Filter dynamic point clouds in 4D millimeter wave radar point clouds; Based on the dynamic point cloud, performing secondary denoising on the laser radar point cloud after preliminary denoising to obtain a denoised laser radar point cloud; The initial pose of the ground unmanned platform is estimated based on the denoised lidar point cloud, and the initial pose is globally optimized using a factor graph in combination with the multimodal data. The optimized pose is updated to the global map to obtain the positioning result of the ground unmanned platform.

2. A high-precision ground unmanned platform autonomous positioning method according to claim 1, characterized in that: Filtering the dynamic point cloud includes: Based on the Doppler velocity of each point in the 4D millimeter-wave radar point cloud of the current frame, the estimated self-speed of the ground unmanned platform corresponding to the current frame is obtained using the least squares method; The estimated self-speed is optimized using a progressive non-convex optimizer to obtain the ground unmanned platform self-speed corresponding to the current frame after optimization; For each point in the 4D millimeter-wave radar point cloud of the current frame, if the difference between the optimized ground unmanned platform's own speed and the Doppler speed of the point is greater than a preset threshold, the point is a dynamic point; otherwise, the point is a static point.

3. A high-precision ground unmanned platform autonomous positioning method according to claim 2, characterized in that: The estimated self-speed is optimized using a progressive non-convex optimizer, and the optimization objective is expressed as: Among them, w i is the corresponding weight of the i-th point; represents the difference between the Doppler velocity measured at the i-th point and the predicted Doppler velocity; The Doppler velocity measured at the i-th point in the 4D millimeter-wave radar point cloud of the current frame; R v m is the estimated self-speed of the ground unmanned platform corresponding to the current frame; ρ i is the radial direction vector of the i-th point; μ is the non-convexity control parameter, σ r is the measurement accuracy of Doppler velocity.

4. The high-precision autonomous positioning method for a ground unmanned platform according to claim 1, characterized in that: The laser radar point cloud after the preliminary denoising is subjected to secondary denoising, including: Clustering each point in the laser radar point cloud after preliminary noise reduction using a bird's-eye view; Fitting a 3D bounding box to the clustering result and projecting the dynamic point cloud into the clustering result; The lidar point cloud within the 3D bounding box where the dynamic point cloud is located is removed to obtain the denoised lidar point cloud.

5. The high-precision autonomous positioning method for a ground unmanned platform according to claim 1, characterized in that: Combining the multimodal data, the initial pose is globally optimized using a factor graph, including: Using the inertial navigation data to perform motion de-distortion on the de-noised laser radar point cloud data; Perform pose estimation based on the dedistorted LiDAR point cloud and construct the corresponding odometer factor; Construct IMU pre-integration factors based on inertial navigation data; Based on the self-speed estimation of 4D millimeter-wave radar point cloud, the estimated self-speed of the ground unmanned platform in the corresponding frame is obtained, and the self-speed estimation factor is constructed; The corresponding odometer factor, IMU pre-integration factor and self-speed estimation factor are used as constraints to construct a factor graph, and the initial posture of the ground unmanned platform is optimized.

6. A high-precision ground unmanned platform autonomous positioning method according to claim 5, characterized in that: The expression of the self-speed estimation factor is: Among them, e v is the self-speed estimation factor; is the vehicle radar coordinate system To the navigation coordinate system The rotation matrix of is the vehicle radar coordinate system The estimated speed at the next time i; To transform to the navigation coordinate system The speed under v i is the speed at the i-th moment in the constructed system state measurement; For speed The covariance of is the Mahalanobis norm; is the set of measurements corresponding to the system state to be optimized.

7. A high-precision ground unmanned platform autonomous positioning method according to claim 5, characterized in that: The corresponding odometry factor is constructed based on the dedistorted lidar point cloud, including: Establish the measurement equations of the dedistorted LiDAR point cloud and map; By minimizing the residual of the measurement equation, the estimated pose of the ground unmanned platform corresponding to the current frame after optimization is obtained; When the optimized residual is less than the preset threshold, the relative pose between the frame and the map point is used as the corresponding odometer factor; otherwise, the corresponding odometer factor is calculated using the 4D millimeter-wave radar point cloud.

8. The high-precision autonomous positioning method for a ground unmanned platform according to claim 5, characterized in that: The factor graph also includes a loop detection factor, and the optimization objective of the factor graph is expressed as: in, is the system state including the posture to be optimized; is the self-speed estimation factor, is the corresponding odometer factor, is the IMU pre-integration measurement factor, is the loop detection factor.

9. A high-precision ground unmanned platform autonomous positioning method according to any one of claims 1 to 8, characterized in that: The laser radar point cloud is subjected to preliminary denoising, including: Perform intensity measurement on the collected LiDAR point cloud to obtain an intensity map; Performing Gaussian filtering on the intensity map to filter out pixel points in the filtered intensity map whose pixel values ​​are less than a first threshold and / or whose pixel value changes are greater than a second threshold; The screened pixel points are subjected to inverse projection transformation to obtain the corresponding lidar point cloud, and the corresponding lidar point cloud is removed to obtain the lidar point cloud with preliminary noise reduction.

10. A high-precision ground unmanned platform autonomous positioning system, characterized in that: include: The acquisition module is used to collect the laser radar point cloud, 4D millimeter wave radar point cloud and inertial navigation data of the ground unmanned platform to obtain multi-modal data; A preprocessing module is used to screen the dynamic point cloud in the 4D millimeter wave radar point cloud, and based on the dynamic point cloud, perform secondary denoising on the laser radar point cloud after preliminary denoising to obtain the denoised laser radar point cloud; A posture estimation module, used for estimating the initial posture of the ground unmanned platform based on the de-noised laser radar point cloud; An optimization module, configured to use the initial posture as a starting point, combine the multimodal data, and use a factor graph to globally optimize the initial posture; The map update module is used to update the optimized posture to the global map to obtain the positioning result of the ground unmanned platform.

Images