Adaptive multi-sensor fusion method using radar, laser radar, IMU and GPS in complex environment

Abstract

A self-adaptive multi-sensor fusion method using radar, laser radar, IMU and GPS in a complex environment comprises the following steps: receiving radar point clouds, and preliminarily distinguishing dynamic point clouds and static point clouds in a single frame; the dynamic point cloud and the static point cloud are processed respectively, and the absolute speed of the robot is obtained; receiving point cloud data through the laser radar, and performing time synchronization and space calibration on each frame of data of the millimeter wave radar and the laser radar; dynamic points in each frame are removed in real time, and laser radar point cloud data only containing static points and static millimeter wave radar point cloud data are obtained; geometric feature extraction is carried out on the static laser radar point cloud data, and whether the robot enters a degradation area or not is judged; laser radar or millimeter-wave radar point cloud data are selected according to the number of feature points of the laser radar point cloud for matching of scanned sub-maps; the pose and the speed of the robot are obtained through a GPS, the reliability of GPS data is judged through the speed estimated by the millimeter wave radar and the pose obtained by the front-end odometer, the weight of the GPS data in rear-end optimization is determined by calculating the residual error of the speed estimated by the millimeter wave radar and the pose obtained by the front-end odometer, and the weight is added into a factor graph to be jointly optimized with the front-end odometer.

Description

Technical Field

[0001] The present invention belongs to the field of robot simultaneous positioning and mapping, and in particular to an adaptive multi-sensor fusion method using radar, lidar, IMU and GPS in complex environments. Background Art

[0002] Autonomous robots are tasked with navigating in more challenging environments, with increasing demands for robustness and reliability. In complex environments such as smoke-filled areas or narrow tunnels, LiDAR inertial odometry (LIO) algorithms face significant challenges in performing simultaneous localization and mapping (SLAM) tasks. Particles such as smoke and dust can degrade the quality of LiDAR scans, which in turn prevents point cloud matching. In addition, in narrow environments such as tunnels, the lack of obvious geometric features in the scene and the inability of the global positioning system (GPS) to work properly may cause large drifts in the odometry. Since these special environmental features increase the noise and uncertainty of the sensor data, relying solely on LiDAR and inertial measurement units (IMUs) may not provide accurate odometry. Compared with LiDAR, millimeter-wave radar has a longer wavelength and is less affected by adverse weather conditions such as rain, snow, and fog. In addition, detecting a wider range of information and obtaining Doppler information can help provide robust estimates in environments with similar geometric scenes.

[0003] 4D millimeter-wave radar is longer than lidar and can work normally in adverse weather conditions such as rain, snow, and fog. At the same time, it can detect a wider range of information and obtain Doppler information, which is conducive to providing robust position and velocity estimates in environments with similar geometric scenes. However, in environments with rich and clear geometric features, laser-based odometer estimates are often more accurate. At the same time, in areas with good signals, GPS can provide absolute position information, which can limit the cumulative error caused by dead reckoning. However, in narrow or closed environments (such as tunnels, dense buildings, and canyons), it is difficult for the receiver to track reliable GPS signals, and GPS outliers are likely to cause the system to crash. Therefore, a reasonable strategy is needed to consider the complementarity of sensors in different environments and to perform tight coupling to achieve accurate odometers in challenging environments. Summary of the invention

[0004] In view of the above-mentioned deficiencies of the prior art, the present invention provides an adaptive multi-sensor fusion method using radar, lidar, IMU, and GPS in complex environments. The goal is to use different external perception sensors to deal with adverse environmental changes and ensure the robustness and accuracy of the robot's odometer in environments with reduced perception capabilities such as smoke, tunnels, and high dynamics.

[0005] The objective of the present invention is achieved through the following technical solution: an adaptive multi-sensor fusion method using radar, lidar, IMU, and GPS in a complex environment, comprising the following steps:

[0006] S1: Receive radar point clouds through 4D millimeter-wave radar sensors, use the Doppler velocity of the radar point clouds to preliminarily infer the self-velocity, and preliminarily distinguish between dynamic point clouds and static point clouds in a single frame.

[0007] S2: Process the dynamic point cloud and static point cloud in step S1 separately. The dynamic point cloud uses a density-based clustering method (DBSCAN) with region growing to perform secondary extraction on the dynamic point cloud to accurately extract dynamic vehicles and pedestrians. The static points are extracted from the ground through random sample consistency (RANSAC), thereby removing the underground noise points to obtain a usable static point cloud. The extracted static points are re-estimated for the vehicle's self-velocity to obtain the absolute speed of the robot.

[0008] S3: After receiving the point cloud data through the LiDAR, a high-frequency IMU is used to align each frame of data of the millimeter-wave radar and the LiDAR processed in step S1, including time synchronization and spatial calibration.

[0009] S4: The millimeter-wave radar point cloud data aligned in step S3 is used to assist the laser radar in removing dynamic points in each frame in real time, thereby obtaining laser radar point cloud data containing only static points and static millimeter-wave radar point cloud data.

[0010] S5: Extract the geometric features of the static LiDAR point cloud data in step S4. If the number of feature points suddenly decreases and remains at a low number, it is determined that the robot has entered a degraded area.

[0011] S6: Select the LiDAR or MMW radar point cloud data to scan and match to the submap by the number of feature points of the LiDAR point cloud. Use the iterative error state Kalman filter to tightly couple the LiDAR or MMW radar with the IMU. In each iteration, calculate the current pose variables and use them to derive the state vector and covariance matrix for the next iteration, and continue until convergence.

[0012] S7: Obtain the robot's position and speed through GPS, use the speed estimated by the millimeter-wave radar in S3 and the position obtained by the front-end odometer in S6 to determine the reliability of the GPS data, calculate the residual of the two to determine the weight of the GPS data in the back-end optimization, and add it to the factor graph for joint optimization with the front-end odometer.

[0013] Furthermore, the specific methods for processing the dynamic point cloud and the static point cloud in step S2 are as follows:

[0014] In order to further refine the extraction of dynamic point clouds, a density-based spatial clustering algorithm DBSCAN is used. This method identifies clusters based on the density of surrounding data points, effectively filters out noise points and discards low-density point cloud clusters with inconsistent Doppler information. For the initially extracted static points, ground point extraction is first performed, and then the point clouds below the ground points are judged to be noise points generated by the millimeter-wave radar and are classified as noise points. Therefore, after the preprocessing of the millimeter-wave radar point cloud, the millimeter-wave radar point cloud R can be divided into static points R S , dynamic point R D and noise point R N .

[0015] Furthermore, the specific method of aligning the laser radar and millimeter wave radar data in step S3 is:

[0016] First, find the millimeter-wave radar with the closest timestamp before the current frame of the lidar. Then, calculate the pose transformation between the two frames through IMU pre-integration. Finally, apply the pose transformation to the point cloud of the millimeter-wave radar to complete the sensor data alignment.

[0017] Furthermore, the specific method of removing dynamic obstacles by using the millimeter wave radar to assist the laser radar in step S4 is:

[0018] First, the input is a frame of millimeter wave radar point cloud P r and the LiDAR point cloud P l . Among them, the two frames of point cloud have completed data alignment. Secondly, the data of both frames are projected to the xy plane and stored in the kd-tree to speed up the search. Finally, according to the directly calculated Euclidean distance, the laser radar point cloud within a specific distance ∈ is considered to be a dynamic point. In this way, the laser radar point cloud set L after removing the dynamic points is obtained filtered :

[0019]

[0020] Where, d(·) represents the Euclidean distance. L i and Represent the laser radar and millimeter wave radar point cloud sequences respectively.

[0021] Furthermore, the method for extracting geometric features of the laser radar point cloud data in step S5 is specifically as follows:

[0022] First, for each lidar point cloud data frame, the curvature in the local area is calculated. The magnitude of the curvature reflects the geometric complexity of the point relative to its neighborhood. Secondly, a threshold is set based on the calculated curvature value, and points whose curvature exceeds the threshold are extracted as feature points. When the robot enters a degraded area, there are fewer significant geometric structures in the environment, and the number of feature points with large curvature decreases rapidly. Finally, when it is detected that the number of feature points suddenly decreases significantly, and this number continues to remain at a low level, the system can infer that the robot has entered a degraded area, the lidar data is unavailable, and the millimeter-wave radar point cloud data is used for point cloud registration to maintain the odometer.

[0023] Furthermore, the specific steps of tightly coupling the laser radar and millimeter wave radar data with the IMU in step S6 are as follows:

[0024] The robot state is represented as x, and the state at the i-th IMU sampling moment is represented as:

[0025]

[0026] Where R i represents the rotation matrix on SO(3), p i and v i Represents position and velocity respectively. ω and b a Corresponding to the biases of the gyroscope and accelerometer from the IMU, respectively. In addition, g represents the gravity vector in the world coordinate system, which needs to be estimated. The error state can be expressed in the tangent space near the operating point as δx∈R 18 After receiving the high-frequency IMU data, the current state of the robot is estimated by forward propagation to obtain the error state and covariance After determining the point cloud data, the ikd-tree-based scan-to-submap matching is adopted, which is conducive to real-time calculation. When it is judged that the vehicle is in a complex environment such as a tunnel, smoke, or high dynamics, a millimeter-wave radar submap is constructed in advance, and the millimeter-wave radar is used to replace the lidar for scanning to submap matching. Similarly, when the lidar point cloud density and the number of feature points return to normal, the lidar submap is constructed in advance, and the original millimeter-wave radar inertial system falls back to the lidar inertial system. The iterative error state Kalman filter (IESKF) method is used to iteratively update the observation model. In each iteration, the current variable δx is calculated and used to derive the state vector x and covariance matrix P for the next iteration, and this process continues until convergence. In essence, each iteration involves solving a least squares problem with a priori:

[0027]

[0028] Among them, k represents the end time of the jth lidar scan, represents the state prediction error, represents the propagation covariance, is the observation residual, which represents the error between the current state estimate and the observed data. j is the noise covariance of the jth point.

[0029] Furthermore, the specific method of GPS adaptive weight allocation reliability judgment and graph optimization in step S7 is:

[0030] The pose change and speed between two frames are used as the consistency test criteria. Among them, the pose change can come from the front-end odometer, and the speed comes from the self-speed estimation using the filtered millimeter-wave radar static point cloud. Therefore, the residual e k It is expressed as:

[0031]

[0032] If the GPS status is normal, e k It should be a Gaussian distribution with zero mean, otherwise it is biased. The decision variable λ k It is defined as the residual sum of squares weighted by the associated covariance matrix A:

[0033]

[0034] When the outlier test function is less than T min When the test function is greater than T, it means that GPS can be fully trusted. At this time, the back-end optimization can trust GPS data more to reduce the cumulative error. max When λ is not available, the GPS data is unavailable. In this case, the backend optimization will directly remove the GPS factor to eliminate the influence of outliers. k When it is at the middle threshold, it is considered that the GPS data is questionable. If GPS data is not added for a long time, there will be a certain deviation between the GPS data and the front-end odometer data. Directly adding the GPS factor will cause the odometer to jump or even crash the system. Therefore, the GPS data at the middle threshold can play a transitional role in the optimization process, which can ensure the smooth integration of the odometer and gradually approach or move away from the GPS data. When the GPS is in an uncertain area, a smoothing function ρ is used. k The transition is made and the resulting cost function is used instead of minimizing the residual sum of squares:

[0035]

[0036] Where r(·) represents the residual of the sensor measurement factor. This method allows the gradual integration of GPS data, making the GPS data a smooth change in the entire optimization process, thereby maintaining the stability of the entire system.

[0037] The present invention integrates 4D millimeter-wave radar, lidar, inertial measurement unit (IMU) and global positioning system (GPS) to utilize the complementary advantages of these sensors for robust odometer estimation in complex environments. The proposed method consists of three key modules. The preprocessing module uses millimeter-wave radar data to assist the lidar in removing dynamic points and determining whether the vehicle is in a degraded environment. The front-end odometer selects the point cloud data of the millimeter-wave radar or lidar for scanning to the sub-map matching, and uses an iterative error state Kalman filter to tightly couple it with the IMU. The back-end optimization module calculates the weight between the front-end odometer and GPS data, constructs an optimized pose graph to obtain the optimal estimate, and ensures that the robot maintains accurate pose estimation and navigation in environments such as smoke, tunnels and bad weather.

[0038] The beneficial effects of the present invention are:

[0039] 1. Clustering algorithms such as BSCAN are used to identify and filter out noise points and low-density point cloud clusters when extracting dynamic and static point clouds from millimeter-wave radar, and radar data is used to identify and remove dynamic points, assisting the lidar in performing high-precision point cloud registration in dynamic scenes, thereby improving the accuracy of the odometer.

[0040] 2. Use the Doppler information of the millimeter-wave radar to estimate the robot's self-velocity and obtain the robot's absolute speed, which is used as a basis for judging the reliability of GPS in back-end optimization.

[0041] 3. The present invention can adaptively select the sensor fusion mode according to the different characteristics of the environment. When the performance of the lidar is degraded due to factors such as smoke, tunnels or weather, the system will automatically switch to the radar and IMU for tight coupling. After the environment is restored, the system will switch back to the fusion of lidar and IMU. This adaptive switching ensures robust positioning in various complex environments and avoids system crashes caused by failure of a single sensor. Especially in tunnels, urban canyons or inclement weather, the multimodal front-end odometer can ensure the stability and accuracy of the system.

[0042] 4. By detecting changes in the number and density of lidar feature points, the system can determine whether the current environment is suitable for using lidar, and switch to radar when necessary to ensure continuous high-precision odometer estimation. The system uses the scan matching algorithm accelerated by ikd-tree to select the most suitable submap for registration, thereby avoiding the cumulative error caused by frame-by-frame scan matching.

[0043] 5. The present invention designs a residual-based χ2 test method that can detect the credibility of GPS data during the back-end optimization process.

[0044] 6. For GPS data outliers, the system will dynamically adjust their weights during the optimization process, and even remove them when the data is extremely unreliable to avoid negative impact on odometer estimation. This processing method ensures that the system can maintain accurate positioning even when GPS data is unstable. BRIEF DESCRIPTION OF THE DRAWINGS

[0045] Figure 1 It is a framework diagram of the method of the present invention;

[0046] Figure 2 This is a flow chart of a point cloud preprocessing algorithm in an embodiment of the present invention;

[0047] Figure 3 Schematic diagram of a dynamic perception multimodal front-end odometer in an embodiment of the present invention. DETAILED DESCRIPTION

[0048] In order to better understand the technical solution of the present application, the embodiments of the present application are described in detail below with reference to the accompanying drawings.

[0049] It should be clear that the described embodiments are only part of the embodiments of the present application, rather than all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in the field without creative work are within the scope of protection of the present application.

[0050] The terms used in the embodiments of the present application are only for the purpose of describing specific embodiments, and are not intended to limit the present application. The singular forms "a", "an", "the" and "the" used in the embodiments of the present application and the appended claims are also intended to include plural forms, unless the context clearly indicates other meanings.

[0051] like Figure 1 As shown, in this embodiment, an adaptive multi-sensor fusion method of millimeter wave radar, laser radar, IMU, and GPS is provided, and the experimental scene is outdoors in the park. The following steps are included:

[0052] S1, preliminary separation of millimeter wave radar dynamic and static point clouds, specifically:

[0053] First, install the 4D millimeter-wave radar on the robot and successfully receive the point cloud data. The parameters in the point cloud data include the coordinates (x, y, z), azimuth (alpha, beta), Doppler velocity (doppler), distance (range), and point cloud confidence flag (denoiseFlag). Take the point cloud with a point cloud confidence flag of 1 as input, and use the Doppler velocity of the input point cloud to estimate the absolute speed of the robot. Then, the absolute speed of the robot is applied to each point cloud, and the absolute speed of each point cloud can be derived from the original relative Doppler velocity. Although the absolute speed of each point can be used to roughly distinguish between dynamic and static point clouds, the dynamic points and static points obtained contain a lot of noise and still cannot be used directly.

[0054] S2, further process the dynamic and static point clouds to obtain dynamic and static point clouds without noise, specifically:

[0055] First of all, dynamic points are usually generated by moving vehicles or pedestrians, and are characterized by high point cloud density and consistent Doppler information. In order to further refine the extraction of dynamic point clouds, the density-based spatial clustering algorithm DBSCAN is adopted. This method identifies dense areas in the data by defining a neighborhood radius (ε) and a minimum number of neighborhood points (MinPts). First, for each point, the number of points in its neighborhood is calculated. If the number is greater than MinPts, the point is regarded as a core point and connected to the points in the neighborhood into a cluster. Then, the cluster will gradually incorporate adjacent dense points into it by density expansion until it can no longer expand. Noise points are sparse points that cannot be classified into any cluster. This method can accurately extract the dynamic point cloud R of the millimeter wave. D .

[0056] Secondly, the radar ego velocity is further refined using the linear least squares method for the static points, removing false points underground and generating a usable static point cloud R S .

[0057] Finally, the dynamic point R D and static point R S The removed point cloud is classified as noise R N , and the three point cloud sets satisfy:

[0058] R=R S +R D +R N (7)

[0059] in

[0060] S3, aligning the laser radar and millimeter wave radar data is as follows: install the laser radar and IMU on the robot, and use external parameter calibration to make the coordinate systems of the two the same. In the experiment, the operating frequency of the laser radar is 10Hz, the operating frequency of the millimeter wave radar is 15Hz, and the operating frequency of the IMU is 300Hz. Therefore, the IMU has a higher data frequency. Therefore, when processing the data, it is necessary to find the millimeter wave radar data with the closest timestamp before the current frame of the laser radar. Then, the pre-integration method of the IMU is used to calculate the pose transformation between the two frames, and the pose transformation is applied to the point cloud of the millimeter wave radar to achieve sensor data alignment.

[0061] S4, the specific method of removing dynamic obstacles by using millimeter wave radar to assist laser radar is:

[0062] First, the input is a frame of millimeter wave radar point cloud P r LiDAR Point Cloud P l , where the two frames of point cloud have completed data alignment. Secondly, project the data of both frames to the xy plane and store the data in the kd-tree to speed up the search. The uncertainty of each point is the standard deviation σ of the distance, azimuth and elevation. r =0.00215r,σ a = sin(0.5°)r and σ e = sin(1.0°)r. By this method, the covariance matrix of each point can be obtained and the rotation matrix To convert it to the radar frame, we need to multiply the covariance matrix S by the rotation matrix R, so that we can get the covariance matrix C for each point:

[0063] C=RS (8)

[0064] where θ a ,θ e is the azimuth of the point divided by the elevation. However, it is necessary to calculate the covariance in a two-dimensional coordinate system. The 2×2 covariance matrix for the xy plane can be derived by eliminating the third row and third column from the matrix C. In this way, a 2×2 covariance matrix can be obtained for each point in the radar, enabling the Mahalanobis distance between each lidar point and the mmWave radar point to be measured.

[0065] Finally, according to the directly calculated Mahalanobis distance, the lidar point cloud within a specific distance ∈ 1 is considered as a dynamic point. In this way, the lidar point cloud set L after removing the dynamic points is obtained. filtered :

[0066]

[0067] Where, d(·) represents the Mahalanobis distance. L i and R j Represent the laser radar and millimeter wave radar point cloud sequences respectively.

[0068] S5, extraction of geometric features of lidar point cloud data and determination of whether to select lidar or millimeter-wave radar: First, for each lidar point cloud data frame, calculate the curvature in the local area. The magnitude of the curvature reflects the geometric structure complexity of the point relative to its neighborhood. Secondly, based on the calculated curvature value, the threshold is set to 0.01, and the points whose curvature exceeds the threshold are extracted as feature points. When the robot enters a degraded area, there are fewer significant geometric structures in the environment, and the number of feature points with large curvature decreases rapidly. Finally, when it is detected that the number of feature points suddenly drops sharply to below 100, and this number is maintained for 10 seconds, the system can infer that the robot has entered a degraded area, the lidar data is unavailable, and the millimeter-wave radar point cloud data is used for point cloud registration to maintain the odometer. The entire point cloud preprocessing algorithm flow is as follows: Figure 2 shown.

[0069] S6, tightly couples the lidar and millimeter-wave radar data with the IMU, using x to represent the robot state, u to represent the control input (i.e., IMU data), and w to represent the system noise:

[0070]

[0071] Where R i is the rotation matrix on SO(3), p i and v i are position and velocity, b ω and b a are the deviations from the gyroscope and accelerometer of the IMU, respectively, and b ω and b a is the gravity vector in the world coordinate system that needs to be estimated, a m and w m are the acceleration and angular velocity inputs from the IMU, respectively, and g is the Gaussian white noise that affects the measurement value and deviation, respectively.

[0072] In addition, use and To parameterize the state error, it is on the composite manifold M = SO(3) × R n There are the following expressions:

[0073]

[0074] where r∈R3, a, b∈R n , Exp(r) is the exponential mapping, and Log(·) is its inverse mapping.

[0075] When measurements are received from GNSS or LiDAR, the accumulated IMU data is propagated forward until it is synchronized with the measurements. The forward propagation of the IMU can be represented by its discrete-time kinematic model, where the noise is set to zero:

[0076]

[0077] in Yes ρ+1 The predicted state at the moment, R i,ρ , p i,ρ and v i,ρ Yes ρ The rotation, position and velocity of the object in the world coordinate system at the moment. The fusion SLAM framework is completely based on IESKF; therefore, the error state and covariance The kinematic model is used to represent the state estimation process and can be described as:

[0078]

[0079] in and F w is the transfer matrix of error state and noise.

[0080] The schematic diagram of the dynamic perception multimodal front-end odometer is as follows Figure 3 As shown. In a normal environment, the density of the point cloud generated by the lidar is relatively stable, and the point cloud accuracy is high. However, in a tunnel or smoke environment, the density of the lidar point cloud will decrease and it is difficult to extract obvious shape features from the point cloud. Therefore, before tightly coupling with the IMU, it is necessary to determine which perception sensor is more suitable for the current environment. Feature point extraction is performed on the lidar data that has been removed from the dynamic points. When the number of feature points suddenly drops rapidly and remains at a low value, it is determined that the vehicle has entered the degraded area. At this time, the millimeter-wave radar point cloud is used instead of the lidar point cloud to match the scan to the submap. However, since the point cloud resolution and features of a frame scanned by the millimeter-wave radar are inconsistent with the laser, direct matching may result in large errors. Therefore, before switching from the lidar inertial subsystem to the millimeter-wave radar inertial subsystem, the millimeter-wave radar submap will be constructed in advance, and the millimeter-wave radar scan to the millimeter-wave radar submap will be matched after the switch. Similarly, when the lidar inertial subsystem is rolled back, the lidar submap is constructed again in advance, so as to avoid jump errors caused by matching errors in the early stage of switching.

[0081] Performs direct scan-to-map registration without the need for feature extraction and state update via LiDAR. The measurement model is:

[0082]

[0083] where p j,k+1 t k+1 The jth point at time, q j is the centroid of the small patch fitted to the nearest point, G j is the normal vector of the corresponding patch, is the measurement model Jacobian matrix of the error state, T i and T l They are the transformation from IMU frame to world frame and the pre-calibration parameters from LiDAR frame to IMU frame, v l,j ∈N(0,R l ) is p j,k+1 Then, calculate the Kalman gain K l :

[0084]

[0085] and will Perform iterative updates:

[0086]

[0087] The refined state with covariance will be the output of the fused odometry after convergence or after reaching the maximum number of iterations:

[0088]

[0089] In each iteration, the current variable δx is calculated and used to derive the state vector x and covariance matrix P for the next iteration, and this process continues until convergence. Essentially, each iteration involves solving a least squares problem with a priori:

[0090]

[0091] Among them, k represents the end time of the jth lidar scan, represents the state prediction error, represents the propagation covariance, is the observation residual, which represents the error between the current state estimate and the observed data. j is the noise covariance of the jth point.

[0092] S7, the specific method for GPS adaptive weight allocation and graph optimization is:

[0093] The pose change and speed between two frames are used as the consistency test criteria. Among them, the pose change can come from the front-end odometer, and the speed comes from the self-speed estimation using the filtered millimeter-wave radar static point cloud. Therefore, the residual e k It is expressed as:

[0094]

[0095] If the GPS status is normal, e k It should be a Gaussian distribution with zero mean, otherwise it is biased. The decision variable λ k It is defined as the residual sum of squares weighted by the associated covariance matrix A:

[0096]

[0097] Since GPS represents a six-dimensional state, the outlier test function λ k Corresponds to 6-DoFX 2 Distribution. In calculating the test statistic λ of the kth frame k After that, k Perform the following processing:

[0098]

[0099] When the outlier test function λ k Less than T min When the test function exceeds T, it means that the GPS data can be fully trusted. At this time, the back-end optimization can rely more on GPS data to reduce the cumulative error. max , it means that the GPS data is unreliable, and the back-end optimization will directly remove the GPS factor to eliminate the influence of outliers. min Set to 10, T max Set to 10 5 When λ k When it falls within the middle threshold range, the GPS data is considered suspicious. In scenarios where GPS data has not been integrated for a long time, there may be differences between the GPS data and the front-end odometer pose. In this case, directly adding the GPS factor may cause the odometer to jump or even system failure. Therefore, GPS data within the middle threshold can be used as a transition factor in the optimization process, gradually moving closer to or away from the GPS data while ensuring smooth fusion of the odometer. When GPS data falls into the uncertainty area, a smoothing function is applied to facilitate this transition:

[0100] ρ k =αe k +(1-α)ρ k-1 (19)

[0101] The smoothing coefficient α (where 0 < α < 1) is used to adjust the transition and is set to 0.3 in the experiment. When GPS measurements are considered as constraints in the pose optimization process, the result ρ k Instead of simply minimizing the residual sum of squares, we will use this for the cost function:

[0102]

[0103] where r(·) represents the residual of the sensor measurement factor. This approach allows the gradual integration of GPS data, making the optimization process more robust to potential differences between GPS and odometry data, thus maintaining the stability of the entire system.

[0104] It should also be noted that the terms "include", "comprises" or any other variations thereof are intended to cover non-exclusive inclusion, so that a process, method, commodity or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, commodity or device. In the absence of more restrictions, the elements defined by the sentence "comprises a ..." do not exclude the existence of other identical elements in the process, method, commodity or device including the elements.

[0105] The above describes specific embodiments of the present specification. Other embodiments are within the scope of the appended claims. In some cases, the actions or steps recited in the claims can be performed in an order different from that in the embodiments and still achieve the desired results.

[0106] The above description is merely a preferred embodiment of one or more embodiments of the present specification and is not intended to limit one or more embodiments of the present specification. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of one or more embodiments of the present specification shall be included in the scope of protection of one or more embodiments of the present specification.

Claims

1. An adaptive multi-sensor fusion method using radar, lidar, IMU, and GPS in complex environments, characterized by: The steps include: S1: Receive radar point clouds through the 4D millimeter wave radar sensor, use the Doppler velocity of the radar point clouds to preliminarily infer the self-velocity, and preliminarily distinguish between dynamic point clouds and static point clouds in a single frame; S2: Process the dynamic point cloud and static point cloud in step S1 separately. The dynamic point cloud uses a density-based clustering method DBSCAN with regional growth to perform secondary extraction on the dynamic point cloud to accurately extract dynamic vehicles and pedestrians. The static points are extracted from the ground points through random sample consistency RANSAC, thereby removing the underground noise points to obtain a usable static point cloud; the extracted static points are re-estimated for the vehicle's self-speed to obtain the absolute speed of the robot; S3: After receiving the point cloud data through the laser radar, a high-frequency IMU is used to align each frame of data of the millimeter-wave radar and the laser radar processed in step S1, including time synchronization and spatial calibration; S4: using the millimeter-wave radar point cloud data aligned in step S3 to assist the laser radar in removing dynamic points in each frame in real time, thereby obtaining laser radar point cloud data containing only static points and static millimeter-wave radar point cloud data; S5: extracting geometric features from the static laser radar point cloud data in step S4. If the number of feature points suddenly decreases and remains at a low number, it is determined that the robot has entered a degraded area. S6: Select the laser radar or millimeter wave radar point cloud data to scan and match the sub-map according to the number of feature points of the laser radar point cloud. Use the iterative error state Kalman filter to tightly couple the laser radar or millimeter wave radar with the IMU; in each iteration, calculate the current pose variable and use it to derive the state vector and covariance matrix of the next iteration, and continue until convergence; S7: Obtain the robot's position and speed through GPS, use the speed estimated by the millimeter-wave radar in S3 and the position obtained by the front-end odometer in S6 to determine the reliability of the GPS data, calculate the residual of the two to determine the weight of the GPS data in the back-end optimization, and add it to the factor graph for joint optimization with the front-end odometer.

2. The method according to claim 1, characterized in that In step S2, the specific method for processing the dynamic point cloud and the static point cloud separately is as follows: in order to further refine the extraction of the dynamic point cloud, the density-based spatial clustering algorithm DBSCAN is used to identify clusters according to the density of the surrounding data points, effectively filter out noise points and discard low-density point cloud clusters with inconsistent Doppler information; for the initially extracted static points, ground point extraction is first performed, and then the point cloud below the ground point is judged to be the noise point generated by the millimeter-wave radar, and it is classified as a noise point; therefore, after the preprocessing of the millimeter-wave radar point cloud, the millimeter-wave radar point cloud R is divided into static points R S , dynamic point R D and noise point R N .

3. The method according to claim 1, characterized in that In step S3, the specific method for aligning the laser radar and millimeter-wave radar data is as follows: find the millimeter-wave radar with the closest timestamp before the current frame of the laser radar, then calculate the pose transformation between the two frames through IMU pre-integration, and finally, apply the pose transformation to the point cloud of the millimeter-wave radar to complete the data alignment of the sensors.

4. The method according to claim 4, characterized in that In step S4, the specific method of removing dynamic obstacles by using millimeter wave radar to assist laser radar is: First, the input is a frame of millimeter wave radar point cloud P r LiDAR Point Cloud P l , where the two frames of point clouds have completed data alignment; secondly, the data of the two frames are projected onto the xy plane and stored in the kd-tree to speed up the search; finally, according to the directly calculated Euclidean distance, the laser radar point cloud within a specific distance ∈ is considered as a dynamic point; in this way, the laser radar point cloud set L after removing the dynamic points is obtained filtered : Where, d(·) represents the Euclidean distance. L i and R j Represent the laser radar and millimeter wave radar point cloud sequences respectively.

5. The method according to claim 1, characterized in that In step S5, the method for extracting geometric features of laser radar point cloud data is specifically as follows: First, for each lidar point cloud data frame, the curvature in the local area is calculated. The magnitude of the curvature reflects the geometric complexity of the point relative to its neighborhood. Secondly, based on the calculated curvature value, a threshold is set and points whose curvature exceeds the threshold are extracted as feature points. When the robot enters a degraded area, the significant geometric structures in the environment become fewer and the number of feature points with large curvature decreases rapidly. Finally, when it is detected that the number of feature points suddenly decreases significantly and this number continues to remain at a low level, it is inferred that the robot has entered a degraded area and the lidar data is unavailable. Millimeter-wave radar point cloud data is used for point cloud registration to maintain the odometer.

6. The method according to claim 1, characterized in that In step S6, the laser radar and millimeter wave radar data are tightly coupled with the IMU. The specific steps are as follows: the robot state is represented as x, and the state at the i-th IMU sampling time is represented as: Where R i represents the rotation matrix on SO(3), p i and v i Respectively represent position and velocity; b ω and b a They correspond to the biases of the gyroscope and accelerometer from the IMU, respectively; in addition, g represents the gravity vector in the world coordinate system, which needs to be estimated; The error state can be expressed in the tangent space near the operating point as δx∈R 18 After receiving the high-frequency IMU data, the current state of the robot is estimated by forward propagation to obtain the error state and covariance After determining the point cloud data, the scan-to-submap matching based on ikd-tree is adopted, which is conducive to real-time calculation; when it is judged that the vehicle is in a complex environment such as a tunnel, smoke, or high dynamics, the millimeter-wave radar submap is constructed in advance, and the millimeter-wave radar is used to replace the lidar for scanning to the submap matching; similarly, when the lidar point cloud density and the number of feature points return to normal, the lidar submap is constructed in advance, and the original millimeter-wave radar inertial system is rolled back to the lidar inertial system; the iterative error state Kalman filter IESKF method is used to iteratively update the observation model; in each iteration, the current variable δx is calculated and used to derive the state vector x and covariance matrix P of the next iteration, and this process continues until convergence; essentially, each iteration involves solving a least squares problem with a priori: Among them, k represents the end time of the jth lidar scan, represents the state prediction error, represents the propagation covariance, is the observation residual, which represents the error between the current state estimate and the observed data; R j is the noise covariance of the jth point.

7. The method according to claim 1, characterized in that In step S7, the specific method of GPS adaptive weight allocation reliability judgment and graph optimization is: The pose change and speed between two frames are used as the consistency test criteria, where the pose change can come from the front-end odometer, and the speed comes from the self-speed estimation using the filtered millimeter-wave radar static point cloud. Therefore, the residual e k It is expressed as: If the GPS status is normal, e k It should be a Gaussian distribution with zero mean, otherwise it is biased. The decision variable λ k It is defined as the residual sum of squares weighted by the associated covariance matrix A: When the outlier test function is less than T min When the test function is greater than T max When λ is unavailable, the backend optimization will directly remove the GPS factor to eliminate the influence of outliers. k When it is at the middle threshold, it is considered that the GPS data is questionable. If GPS data is not added for a long time, there will be a certain deviation between the GPS data and the front-end odometer data. Directly adding the GPS factor will cause the odometer to jump or even crash the system. Therefore, the GPS data at the middle threshold can play a transitional role in the optimization process, which can ensure the smooth integration of the odometer and gradually approach or move away from the GPS data. When the GPS is in the uncertain area, a smoothing function ρ is used. k The transition is made and the resulting cost function is used instead of minimizing the residual sum of squares: Where r(·) represents the residual of the sensor measurement factor. This method allows the gradual integration of GPS data, making the GPS data a smooth change in the entire optimization process, thereby maintaining the stability of the entire system.

Images