Adaptive multi-sensor fusion method for radar, lidar, imu, gps in complex environment

Abstract

The adaptive multi-sensor fusion method for complex environment using radar, lidar, IMU and GPS includes: receiving radar point cloud, preliminarily distinguishing dynamic point cloud and static point cloud in a single frame; processing the dynamic point cloud and the static point cloud respectively to obtain the absolute speed of the robot; time synchronization and space calibration are performed on each frame of data of the millimeter wave radar and the lidar through the point cloud data received by the lidar; dynamic points in each frame are removed in real time to obtain laser radar point cloud data and static millimeter wave radar point cloud data containing only static points; geometric feature extraction is performed on the static laser radar point cloud data to determine whether the robot has entered a degenerative area; the number of feature points of the laser radar point cloud is used to select the laser radar or the millimeter wave radar point cloud data to perform scanning to the matching of the submap; the pose and speed of the robot are obtained through the GPS, the speed estimated by the millimeter wave radar and the pose obtained by the front end odometer are used to determine the reliability of the GPS data, the residual error of the two is calculated to determine the weight of the GPS data in the back end optimization, and the GPS data is added to the factor graph to be optimized together with the front end odometer.

Description

TECHNICAL FIELD

[0001] The present application belongs to the field of robot simultaneous localization and mapping, and particularly relates to a self-adaptive multi-sensor fusion method using radar, lidar, IMU and GPS in complex environments. BACKGROUND

[0002] The task of autonomous robots is to navigate in more challenging environments, and the requirements for robustness and reliability are also increasing. In complex environments such as smoky areas or narrow tunnels, radar inertial odometry (LIO) algorithms encounter major challenges when performing the task of simultaneous localization and mapping (SLAM). Particulate matter such as smoke and dust can cause the quality of lidar scanning to decline, thereby hindering the matching of point clouds. In addition, in narrow environments such as tunnels, the scene lacks obvious geometric features and the global positioning system (GPS) cannot work normally, which can cause large drift of the odometry. Due to these special environmental features, the noise and uncertainty of sensor data are increased, and it may not be possible to provide accurate odometry relying only on lidar and inertial measurement unit (IMU). Compared with lidar, millimeter wave radar has a longer wavelength and is less affected by adverse weather conditions such as rain, snow and fog. And detecting a wider range of information and obtaining Doppler information can be beneficial in providing 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, while it can detect a wider range of information and obtain Doppler information, which is beneficial in providing robust position and velocity estimates in environments with similar geometric scenes. However, in environments with rich and clear geometric features, lidar-based odometry 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 enclosed environments (such as tunnels, dense buildings and canyons), it is difficult for the receiver to track reliable GPS signals, and GPS outliers can likely cause the system to crash. Therefore, a reasonable strategy is needed to consider the complementarity of sensors in different environments and tightly couple them to obtain accurate odometry in challenging environments. SUMMARY

[0004] In view of the deficiencies of the prior art, the present application provides a self-adaptive multi-sensor fusion method using radar, lidar, IMU and GPS in complex environments, which aims to use different external perception sensors to handle adverse environmental changes and ensure the robustness and accuracy of the odometry of the robot in environments with reduced perception capabilities such as smoke, tunnels and high dynamics.

[0005] The application aims to realize the technical scheme of a self-adaptive multi-sensor fusion method for radar, lidar, IMU and GPS in a complex environment, comprising the following steps:

[0006] S1: receiving a radar point cloud through a 4D millimeter wave radar sensor, using the Doppler velocity of the radar point cloud to preliminarily infer the self speed, and preliminarily distinguishing the dynamic point cloud and the static point cloud in a single frame.

[0007] S2: processing the dynamic point cloud and the static point cloud in step S1 respectively. The dynamic point cloud is extracted again by using a region growing density-based clustering method (DBSCAN) to complete the accurate extraction of dynamic vehicles and pedestrians. The ground points are extracted from the static points by using random sample consensus (RANSAC), so as to remove the noise points underground and obtain the available static point cloud. The static points after extraction are re-estimated for the vehicle self speed to obtain the absolute speed of the robot.

[0008] S3: receiving point cloud data through a lidar, and aligning each frame of data of the millimeter wave radar and the lidar after processing in step S1 by using a high-frequency IMU, including time synchronization and space calibration.

[0009] S4: using the millimeter wave radar point cloud data after alignment in step S3 to assist the lidar in removing the dynamic points in each frame in real time, so as to obtain the lidar point cloud data and the static millimeter wave radar point cloud data containing only static points.

[0010] S5: extracting the geometric features of the static lidar point cloud data in step S4, and judging that the robot enters a degenerative area if the number of feature points suddenly decreases and is maintained at a low number.

[0011] S6: selecting the lidar or millimeter wave radar point cloud data to scan and match to a submap by using the number of feature points of the lidar point cloud. The lidar or millimeter wave radar is tightly coupled with the IMU by using an iterative error state Kalman filter. In each iteration, the current pose variable is calculated and used to derive the state vector and covariance matrix of the next iteration, and the process is continued until convergence.

[0012] S7: obtaining the pose and speed of the robot through a GPS, using the speed estimated by the millimeter wave radar in S3 and the pose obtained by the front-end odometer in S6 to judge the reliability of the data of the GPS, calculating the residual error of the two to determine the weight of the GPS data in the back-end optimization, and adding the GPS data to the factor graph to be optimized together with the front-end odometer.

[0013] Further, the specific method for processing the dynamic point cloud and the static point cloud in step S2 is as follows:

[0014] To further refine the extraction of dynamic point cloud, a density-based spatial clustering algorithm DBSCAN is adopted. This method identifies clusters according to the density of surrounding data points, effectively filtering out noise points and discarding low-density point cloud clusters with inconsistent Doppler information. For the preliminary extraction of static points, ground point extraction is first performed, and then points below the ground points are judged as noise points generated by the millimeter wave radar, which are classified as noise points. Therefore, after the preprocessing of the millimeter wave radar point cloud, the millimeter wave radar point cloud R S , dynamic points R D and noise points R N .

[0015] Further, the method for aligning the lidar and millimeter wave radar data in step S3 is as follows:

[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 data alignment of the sensors.

[0017] Further, the method for removing dynamic obstacles by the millimeter wave radar to assist the lidar in step S4 is as follows:

[0018] First, the input is a frame of millimeter wave radar point cloud P r and lidar point cloud P l . Among them, the two frames of point cloud have completed data alignment. Second, project the data of the two frames to the xy plane, and store the data in the kd-tree to speed up the search. Finally, according to the directly calculated Euclidean distance, the lidar point cloud within a certain distance ∈ is considered as a dynamic point. Through this method, the lidar point cloud set L filtered after removing dynamic points is obtained:

[0019]

[0020] wherein, d(·) represents the Euclidean distance. L i and represent the lidar and millimeter wave radar point cloud sequences respectively.

[0021] Further, the method for extracting the geometric features of the lidar point cloud data in step S5 is as follows:

[0022] First, for each frame of lidar point cloud data, the curvature within a local region is computed. The magnitude of the curvature reflects the geometric complexity of the point relative to its neighborhood. Second, a threshold is set according to the computed curvature values, and points with curvature exceeding the threshold are extracted as feature points. When the robot enters a degenerate region, the number of feature points with large curvature decreases rapidly as the significant geometric structures in the environment become less. Finally, when the number of feature points is detected to suddenly decrease substantially and this number is maintained at a low level, the system can infer that the robot enters a degenerate region, and the lidar data is not available, and the millimeter wave radar point cloud data is used for point cloud registration to maintain the odometry.

[0023] Further, the step S6 of tightly coupling the lidar and millimeter wave radar data with the IMU includes the following steps:

[0024] Let x represent the state of the robot, and x

[0025]

[0026] where R i is a rotation matrix on SO(3), p i and v i are the position and velocity, respectively. b ω and b a correspond to the biases of the gyroscope and accelerometer from the IMU, respectively. In addition, g represents the gravity vector in the world frame, which needs to be estimated. The error state can be represented in the tangent space around the working point as δx ∈ R 18 Upon receiving the high-frequency IMU data, the current state of the robot is first estimated by forward propagation, resulting in the error state and the covariance After determining the point cloud data, a scan-to-submap matching based on ikd-tree is used, which is beneficial for real-time computation. When it is determined 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 scan-to-submap matching. Similarly, when the lidar point cloud density and the number of feature points recover to the normal state, a lidar submap is constructed in advance, and the millimeter wave radar inertial system reverts to the lidar inertial system. An 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 the covariance matrix P for the next iteration, which is continued until convergence. Essentially, each iteration involves solving a least squares problem with a priori:

[0027]

[0028] where k denotes the end time of the jth laser radar scan, denotes the state prediction error, denotes the propagation covariance, is the observation residual, which represents the error between the current state estimate and the observation data. R j is the noise covariance of the jth point.

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

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

[0031]

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

[0033]

[0034] When the outlier test function is less than T min , it means that the GPS can be fully trusted, and at this time the back-end optimization can trust the GPS data more to reduce the cumulative error. When the test function is greater than T max , it means that the GPS data is not available, and at this time the back-end optimization will directly remove the GPS factor to exclude the influence of outliers. When λ k is in the middle threshold, it is determined that the GPS data is questionable. If the GPS data is not added for a long time, it will cause a certain deviation between the GPS data and the front-end odometer data pose, and directly adding the GPS factor will cause the odometer to jump or even system crash. Therefore, the GPS data in the middle threshold can play a transitional role in the optimization process, which can ensure the smooth integration of the odometer and gradually move closer to or away from the GPS data. When the GPS is in the uncertain region, a smoothing function ρ k is used for transition, and the obtained cost function is not the minimum residual sum of squares:

[0035]

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

[0037] The present application integrates 4D millimeter wave radar, laser radar, inertial measurement unit (IMU) and global positioning system (GPS) to utilize the complementary advantages of these sensors for robust odometry estimation in complex environments. The proposed method consists of three key modules, the preprocessing module utilizes millimeter wave radar data to assist laser radar in removing dynamic points and determining whether the vehicle is in a degraded environment. The front-end odometry selects either millimeter wave radar or laser radar point cloud data for scan-to-submap matching and tightly couples it with IMU using an iterative error state Kalman filter. The back-end optimization module calculates the weight between front-end odometry and GPS data, constructs an optimized pose graph to obtain the optimal estimate, ensuring 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 application are:

[0039] 1. BSCAN clustering algorithm is used to identify and filter out noise points and low-density point cloud clusters on millimeter wave radar extraction of dynamic and static points, and radar data is used to identify and remove dynamic points, assisting laser radar in high-precision point cloud registration in dynamic scenes, improving the accuracy of odometry.

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

[0041] 3. The present application can adaptively select sensor fusion mode according to different characteristics of the environment. When the performance of laser radar is degraded due to smoke, tunnels or weather, the system will automatically switch to radar and IMU for tight coupling. After the environment recovers, the system will switch back to the fusion of laser radar and IMU. This adaptive switching ensures robust positioning in various complex environments, avoiding system collapse caused by failure of a single sensor. Especially in tunnels, urban canyons or bad weather, the multi-modal front-end odometry can ensure the stability and accuracy of the system.

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

[0043] 5. The present application designs a set of residual-based χ2 test method, which can detect the reliability of GPS data in the back-end optimization process.

[0044] 6、For GPS data outliers, the system will dynamically adjust its weight in the optimization process, and even remove it when the data is extremely unreliable, avoiding negative impact on odometry estimation. This processing method ensures that the system can still maintain accurate positioning in the case of unstable GPS data. BRIEF DESCRIPTION OF DRAWINGS

[0045] Figure 1 is a block diagram of the method of the present application;

[0046] Figure 2 is a flowchart of the point cloud preprocessing algorithm in the embodiment of the present application;

[0047] Figure 3 is a schematic diagram of the dynamic perception multi-modal front-end odometry in the embodiment of the present application. DETAILED DESCRIPTION

[0048] In order to better understand the technical solutions of the present application, the embodiments of the present application will be 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, not all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those of ordinary skill in the art without creative labor belong to 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 the specific embodiments, and are not intended to limit the present application. The singular forms "a", "an" and "the" used in the embodiments of the present application and the appended claims are also intended to include the plural forms, unless the context clearly indicates otherwise.

[0051] As Figure 1 shown, in the present embodiment, an adaptive multi-sensor fusion method of millimeter wave radar, laser radar, IMU, GPS is provided, and the experimental scene is outdoor in a park. It includes the following steps:

[0052] S1, initially separate the millimeter wave radar dynamic and static point cloud, 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 of the points (x, y, z), the azimuth angle (alpha, beta), the Doppler velocity (doppler), the distance (range), and the point cloud confidence flag (denoiseFlag). The point cloud with the point cloud confidence flag being 1 is used as input, and the absolute speed of the robot is estimated using the Doppler velocity of the input point cloud. Then, the absolute speed of the robot is applied to each point cloud, and the absolute speed of each point cloud can be obtained from the original relative Doppler velocity. Although the absolute speed of each point can roughly distinguish dynamic and static point clouds, the dynamic and static points obtained still contain a large amount of noise and cannot be directly used.

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

[0055] First, dynamic points are usually generated by moving vehicles or pedestrians, and the characteristics are high point cloud density and consistent Doppler information. In order to further refine the extraction of dynamic point clouds, the DBSCAN spatial clustering algorithm based on density is used. This method defines a neighborhood radius (ε) and a minimum number of neighborhood points (MinPts) to identify dense regions in the data. First, for each point, calculate the number of points in its neighborhood. If the number is greater than MinPts, the point is considered a core point and is connected with the points in the neighborhood to form a cluster. Then, the cluster will gradually include adjacent dense points through density expansion until it can no longer be expanded. Noise points are those sparse points that cannot be included in any cluster. Through this method, the dynamic point cloud R D .

[0056] Second, for static points, the linear least squares method is used to further refine the radar self-speed, remove false points underground, and generate a usable static point cloud R S .

[0057] Finally, the dynamic point R D and the static point R S after removing the point cloud set are classified as noise points R N , and the three point cloud sets satisfy:

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

[0059] wherein

[0060] S3, the laser radar and millimeter wave radar data alignment operation is: install laser radar and IMU on the robot, and adopt external parameter calibration to make the coordinate systems of the two same. In the experiment, the working frequency of the laser radar is 10 Hz, the working frequency of the millimeter wave radar is 15 Hz, and the working frequency of the IMU is 300 Hz. Therefore, the IMU has a higher data frequency. Therefore, when processing data, the closest millimeter wave radar data to the timestamp of the current frame of the laser radar needs to be found. 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 realize the alignment of the sensor data.

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

[0062] First, the input is a frame of millimeter wave radar point cloud P r Laser radar point cloud P l , where the two frames of point clouds have completed data alignment. Secondly, project the data of the two 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 σ r = 0.00215r, σ a = sin(0.5°)r and σ e = sin(1.0°)r. By this method, the covariance matrix of each point C and the rotation matrix R To convert it to the radar frame, the covariance matrix S needs to be multiplied by the rotation matrix R, so that the covariance matrix C of each point can be obtained:

[0063] C = RS (8)

[0064] where θ a , θ e is the azimuth angle divided by the elevation angle of the point. However, the covariance in the two-dimensional coordinate system needs to be calculated. The 2x2 covariance matrix of the xy plane can be derived by eliminating the third row and the third column from the matrix C. In this way, the 2x2 covariance matrix of each point in the radar can be obtained, which enables the measurement of the Mahalanobis distance between each laser radar point and the millimeter wave radar point.

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

[0066]

[0067] where d(·) denotes the Mahalanobis distance. L i and R j denote the lidar and mmWave radar point cloud sequences, respectively.

[0068] S5, extraction of lidar point cloud data geometric features and judgment of selection of lidar or mmWave radar: first, for each lidar point cloud data frame, the curvature in the local region is calculated. The size of the curvature reflects the geometric structure complexity of the point relative to its neighborhood. Second, according to the calculated curvature value, a threshold of 0.01 is set, and the points with curvature exceeding the threshold are extracted as feature points. When the robot enters the degenerate region, the significant geometric structure in the environment becomes less, and the number of feature points with large curvature decreases rapidly. Finally, when the number of feature points is suddenly reduced to below 100 and this number is maintained for 10 seconds, the system can infer that the robot has entered a degenerate region, the lidar data is unavailable, and the mmWave radar point cloud data is used for point cloud registration to maintain the odometer. The whole point cloud preprocessing algorithm flow is as shown in Figure 2 .

[0069] S6, tightly coupling the lidar and mmWave radar data with the IMU, where x denotes the robot state, u denotes the control input (i.e. the IMU data), and w denotes the system noise:

[0070]

[0071] where R i is a rotation matrix on SO(3), p i and v i are the position and velocity, respectively, b ω and b a are the biases of the gyroscope and accelerometer from the IMU, b ω and b a are the gravity vectors in the world coordinate system that need to be estimated, a m and w m are the inputs of the acceleration and angular velocity from the IMU, and g

[0072] In addition, the state error is parameterized using and , which has the following expression on the complex manifold M = SO(3) x R n :

[0073]

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

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

[0076]

[0077] where is the predicted state at time t ρ+1 , R i,ρ , p i,ρ and v i,ρ are the rotation, position and velocity of the object in the world frame at time t ρ . The fusion SLAM framework is implemented entirely based on IESKF; therefore, the kinematic model of the error state and covariance is used to represent the process of state estimation, which can be described as:

[0078]

[0079] where and F w are the transition matrix of the error state and noise.

[0080] A schematic diagram of the dynamic perception multi-modal front-end odometry is shown in Figure 3 . In normal environments, the point cloud density generated by lidar is relatively stable, and the point cloud accuracy is high. But in the tunnel or smog environment, the lidar point cloud density will decrease while it is difficult to extract obvious shape features from the point cloud. Therefore, before the tight 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 laser radar data after removing the over-dynamic points, and when the number of feature points suddenly decreases 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 perform scan-to-submap matching. However, since the resolution of a frame of scanning point cloud of the millimeter wave radar is inconsistent with the features of the laser, direct matching may result in a large error. Therefore, before switching from the laser radar inertial subsystem to the millimeter wave radar inertial subsystem, the millimeter wave radar submap will be constructed in advance, and after switching, the millimeter wave radar scan-to-millimeter wave radar submap matching is performed. Similarly, when the laser radar inertial subsystem is backed off, the laser radar submap is also constructed in advance, which can avoid the jump error caused by the matching error in the early stage of switching.

[0081] Direct scan-to-map registration is performed without feature extraction and state update by the laser radar. 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 with the nearest point, G j is the normal vector of the corresponding patch, is the Jacobian matrix of the measurement model of the error state, T i and T l They are the pre-calibration parameters for the transformation from IMU frame to world frame and from lidar frame to IMU frame, v l,j ∈N(0,R l ) is p j,k+1 The measurement noise is then calculated as 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] Where 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 j-th point.

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

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

[0094]

[0095] If GPS is normal, e k should be zero-mean Gaussian, otherwise biased. The decision variable λ k is defined as the weighted sum of squared residuals by the correlation covariance matrix A:

[0096]

[0097] Since GPS represents six-dimensional state, the outlier test function λ k corresponds to 6-DoF X 2 distribution. After computing the test statistic λ k for the k-th frame, λ k is processed as follows:

[0098]

[0099] When the outlier test function λ k is less than T min , it means that GPS data is fully reliable, in which case the back-end optimization can rely more on GPS data and reduce the cumulative error; if the test function exceeds T max , it means that GPS data is unreliable, and the back-end optimization will directly remove the GPS factor to eliminate the influence of outliers. Among them, T min is set to 10, and T max is set to 10 5 . When λ k falls within the middle threshold range, GPS data is considered suspicious. In the scenario where GPS data has not been integrated for a long time, there may be a difference between GPS data and front-end odometry pose, and in this case, directly adding the GPS factor may cause odometry to jump or even system failure. Therefore, GPS data within the middle threshold can be used as a transitional factor in the optimization process, gradually approaching or deviating from GPS data while ensuring smooth integration of odometry. When GPS data falls into the uncertain region, a smoothing function will be 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, which is set to 0.3 in the experiment. When GPS measurements are treated as constraints in the pose optimization process, the result ρ k will be used in the cost function, rather than simply minimizing the sum of squared residuals:

[0102]

[0103] where r(·) represents the residual of the sensor measurement factor. This approach allows for the step-wise integration of GPS data, making the optimization process more robust to potential discrepancies between GPS and odometry data, thereby preserving the stability of the overall system.

[0104] It is also to be noted that the terms "comprising", "including", and any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements does not include only those elements but can also include other elements not expressly listed or inherent to such process, method, article, or apparatus. An element proceeded by "comprises a... " does not, without more constraints, exclude the existence of additional identical elements in the process, method, article, or apparatus that comprises the element.

[0105] The above description of specific embodiments of the disclosure has been presented for the purposes of illustration and description. Other embodiments are within the scope of the following claims. In some cases the actions recited in the claims can be performed in a different order and still achieve desirable results.

[0106] The above description of one or more embodiments of the disclosure is not intended to be exhaustive or to limit the disclosure to the precise form disclosed. While specific embodiments of, and examples for, the disclosure are described herein for illustrative purposes only, various equivalent modifications are possible within the spirit and scope of the disclosure, as those skilled in the relevant art will recognize. Any and all modifications or variations of any kind are possible within the scope of the disclosure, which is defined by the following claims.

Claims

1. An adaptive multi-sensor fusion method for complex environments using millimeter wave radar, lidar, IMU, GPS, characterized in that, Comprising the following steps: S1: receiving a radar point cloud by a 4D millimeter wave radar sensor, using the Doppler velocity of the radar point cloud to preliminarily infer the ego velocity, for preliminarily distinguishing the dynamic point cloud and the static point cloud in a single frame; S2: processing the dynamic point cloud and the static point cloud in step S1 respectively; the dynamic point cloud adopts a region growing density-based clustering method DBSCAN to perform secondary extraction on the dynamic point cloud, to complete accurate extraction of dynamic vehicles and pedestrians; the static point is extracted from the ground point by random sample consensus RANSAC, to remove the noise points underground and obtain available static point cloud; the extracted static point is re-estimated for vehicle ego velocity, to obtain the absolute velocity of the robot; S3: receiving point cloud data by a laser radar, using high-frequency IMU to align each frame of data of the millimeter wave radar and the laser radar processed in step S1, including time synchronization and space 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, to obtain laser radar point cloud data and static millimeter wave radar point cloud data containing only static points; S5: performing geometric feature extraction on the static laser radar point cloud data in step S4, if the number of feature points suddenly decreases and is maintained at a relatively low number, it is judged that the robot has entered a degenerative area; S6: selecting laser radar or millimeter wave radar point cloud data for scanning to map matching by the number of feature points of the laser radar point cloud; using an iterative error state Kalman filter to tightly couple the laser radar or the millimeter wave radar with the IMU; in each iteration, the current pose variable is calculated and used to derive the state vector and the covariance matrix of the next iteration, which is continuously performed until convergence; S7: obtaining the pose and velocity of the robot by GPS, using the velocity estimated by the millimeter wave radar in S2 and the pose obtained by the front-end odometer in S6 to judge the reliability of the GPS data, calculating the residual error of the two to determine the weight of the GPS data in the back-end optimization, and adding it to the factor graph for common optimization with the front-end odometer.

2. The method of claim 1, wherein, In step S2, the specific methods for processing dynamic point clouds and static point clouds separately are as follows: in order to further refine the extraction of dynamic point clouds, the density-based spatial clustering algorithm DBSCAN is used to identify clusters according to the density of 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 clouds below the ground points are judged to be noise points generated by 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 is Divided into static points , dynamic point and noise points .

3. The method of claim 1, wherein, In step S3, the specific method for aligning the laser radar and the millimeter wave radar data is: finding the millimeter wave radar with the closest time stamp before the current frame of the laser radar, then calculating the pose transformation between the two frames by IMU pre-integration, and finally applying the pose transformation to the point cloud of the millimeter wave radar to complete the data alignment of the sensors.

4. The method of claim 1, wherein, In step S4, the specific method for the laser radar to remove dynamic points in each frame in real time is: First, the input is a frame of millimeter-wave radar point cloud , laser radar point cloud , where the two frames of point cloud have completed data alignment; second, project the data of both frames to xy the plane and store the data in kd-tree to speed up the search; finally, according to the directly calculated Euclidean distance, the laser radar point cloud within a certain distance is considered as dynamic point; through this method, the laser radar point cloud set after removing dynamic points is obtained : wherein, wherein denotes the Euclidean distance; and denote the lidar and mm-wave radar point cloud sequences, respectively.

5. The method of claim 1, wherein, In step S5, the extraction method of the geometric features of the laser radar point cloud data is specifically: First, for each frame of laser radar point cloud data, the curvature in the local region is calculated, and the size of the curvature reflects the geometric structure complexity of the point relative to its neighborhood; Secondly, according to the calculated curvature value, a threshold is set, and the points whose curvature exceeds the threshold are extracted as feature points. When the robot enters the degenerate region, the number of feature points with large curvature decreases rapidly because the significant geometric structure in the environment becomes less.

6. The method of claim 1, wherein, 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 of the IMU sampling moment is represented as: i ​ where denotes a rotation matrix on SO(3), and denote position and velocity, respectively; and correspond to biases of the gyroscope and accelerometer from the IMU, respectively; furthermore, denotes the gravity vector in the world coordinate frame, which needs to be estimated; The error state is expressed in the tangent space near the operating point as 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, a 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 tunnel, smoke, or a high-dynamic complex environment, a millimeter wave radar submap is constructed in advance, and the millimeter wave radar is used to replace the laser radar to perform scan-to-submap matching; similarly, when the laser radar point cloud density and the number of feature points recover to the normal state, a laser radar submap is constructed in advance, and the original millimeter wave radar inertial system is returned to the laser radar inertial system; an iterative error state Kalman filter (IESKF) method is used to iteratively update an observation model; in each iteration, a current variable is calculated and used to derive a state vector x and a covariance matrix P of the next iteration, and the process is continued until convergence; essentially, each iteration involves solving a least squares problem with a priori: wherein, wherein k denotes the end time of the j laser radar scan, denotes the state prediction error, denotes the propagation covariance, is the observation residual, which represents the error between the current state estimate and the observation data; is the noise covariance of the j point.

7. The method of claim 1, wherein, In step S7, the specific method of GPS adaptive weight distribution reliability judgment and graph optimization is: The pose change between two frames and the velocity are used as the standard for consistency check, where the pose change comes from the front-end odometry and the velocity comes from the self-velocity estimation using the filtered millimeter-wave radar static point cloud, thus the residual is represented as: If the GPS status is normal, should be zero-mean Gaussian, otherwise biased; decision variable is defined as the weighted sum of squared residuals by the correlation covariance matrix A: When the outlier test function is less than it means that GPS can be fully trusted, at this time the back-end optimization is more trusted GPS data to reduce cumulative error; when the test function is greater than it means that GPS data is not available, at this time the back-end optimization will directly remove the GPS factor, excluding the influence of outliers; when in the middle threshold, it is determined that there is doubt about the GPS data; if the GPS data is not added for a long time, it will cause the GPS data to have a certain deviation from the front-end odometry data pose, and directly adding the GPS factor will cause the odometry to jump or even the system to crash; therefore, the GPS data in the middle threshold plays a transitional role in the optimization process, which can ensure the smooth fusion change of the odometry, gradually approaching or moving away from the GPS data; when the GPS is in the uncertain region, a smoothing function is used for transition, which is used for the cost function instead of minimizing the sum of squared residuals: wherein represents the residual of the sensor measurement factor; this approach allows for the step-wise integration of GPS data, which acts as a smooth change to the overall optimization process, thereby maintaining the stability of the overall system.

Images