A highly adaptable multi-sensor weighted fusion SLAM system and method

Abstract

The present application discloses a highly adaptable multi-sensor weighted fusion SLAM system and method, belonging to the field of data processing technology. The system includes: a data preprocessing module, a tightly coupled front-end module, and a mapping module. The multi-sensor fusion SLAM system proposed in the present application utilizes the advantages of multiple sensors to make up for their respective shortcomings, and proposes a weight distribution strategy. It implements weight distribution based on initialization detection, degradation detection, and vehicle slip detection, performs weighted joint optimization, improves the effect of nonlinear optimization, has strong adaptability to the environment, can cope with the problems of laser radar degradation environment, vehicle slip conditions, instability in the initial stage of mapping, etc., and has stable positioning and high accuracy.

Description

Technical Field

[0001] The present application relates to the field of visual space positioning technology, and in particular to a highly adaptable multi-sensor weighted fusion SLAM system and method. Background Art

[0002] LiDAR data has a simple format, high positioning accuracy, and small data processing volume (the size of a frame of point cloud is much smaller than that of a frame of image). It is not affected by changes in light intensity. Multi-line LiDAR has richer point cloud information. By collecting geometric structure information of the environment, it is widely used in SLAM and can usually achieve higher positioning accuracy than visual SLAM.

[0003] However, LiDAR can only obtain geometric structure information of the environment. When faced with environments with a single geometric structure, such as tunnels and long corridors, degradation problems will occur in some directions, and it is difficult to provide sufficient constraints, resulting in positioning drift, affecting the mapping effect and having a lasting impact on subsequent positioning. In addition, LiDAR is greatly disturbed by the environment and reflective materials, and there are often some noise points. During the ICP alignment stage at the front end, outliers cause SLAM to have incorrect matches, resulting in reduced positioning accuracy.

[0004] To address the above technical issues, one existing approach is to fuse IMU and LiDAR information, using a loosely coupled approach to obtain a coarse pose for each, then perform an extended Kalman filter to optimize the pose. However, this approach struggles to fully utilize sensor information, resulting in insignificant fusion results. Furthermore, the linearization process of the extended Kalman filter suffers from significant errors. Another existing approach is LiDAR SLAM, which utilizes LiDAR intensity information, constructs an intensity map, and incorporates intensity constraints. However, LiDAR intensity information is significantly affected by noise and, in many operating conditions, is unstable, making it difficult to provide stable constraints. Consequently, this approach has limited adaptability. Summary of the Invention

[0005] The purpose of the embodiments of the present application is to provide a highly adaptable multi-sensor weighted fusion SLAM system and method, which utilizes the high precision of lidar, the IMU attitude measurement that is not affected by the environment, and the wheel speed meter to obtain vehicle speed data, performs weighted joint nonlinear optimization, and proposes a weight distribution strategy including initialization detection, degradation detection, and vehicle slip detection to improve the robustness of the laser SLAM system to adapt to extreme working conditions such as degraded environments, thereby solving at least one technical problem involved in the background technology.

[0006] In order to solve the above technical problems, this application is implemented as follows:

[0007] In a first aspect, an embodiment of the present application provides a highly adaptable multi-sensor weighted fusion SLAM system, comprising:

[0008] The data preprocessing module collects raw point clouds using lidar, IMU, and wheel speedometer data, calculates the IMU pre-integral and wheel speedometer pre-integral between two consecutive frames of lidar point clouds, removes outliers from the original lidar point cloud, performs segmentation and clustering on the removed original point cloud, removes noise points, and finally extracts line feature points and surface feature points from the original point cloud to obtain a feature point cloud.

[0009] The tightly coupled front-end module is responsible for subscribing to the feature point cloud output by the data preprocessing module and the information from the IMU and wheel speedometer. It uses the IMU data to remove the motion distortion of the feature point cloud point by point, calculates the residual constraints of the lidar, IMU, and wheel speedometer, and then performs initialization detection, vehicle slip detection, and degradation detection. It calculates the weights of the residuals of the lidar, IMU, and wheel speedometer, and performs weighted joint nonlinear optimization to obtain the optimized vehicle posture.

[0010] The mapping module performs keyframe determination on the optimized vehicle pose and uses the keyframes to update the point cloud map.

[0011] In a second aspect, the present application further provides a highly adaptable multi-sensor weighted fusion SLAM method based on the system, comprising:

[0012] Step 1: Read the data from the lidar, IMU, and wheel speed meter, and use the external parameters of the offline calibration to unify the data into the first frame coordinate system of the IMU to obtain the original point cloud;

[0013] Step 2: Calculate the pre-integrated quantities of the IMU and wheel speedometer between two consecutive frames of lidar point clouds based on the lidar frequency.

[0014] Step 3: Remove outliers from the original LiDAR point cloud;

[0015] Step 4: Segment and cluster the original point cloud after elimination to eliminate noise points;

[0016] Step 5: Calculate the curvature of all original point clouds in the same line, divide the original point clouds into line feature points and surface feature points according to the curvature, and obtain the feature point cloud;

[0017] Step 6: Use IMU data to remove motion distortion of the feature point cloud point by point, and construct residual constraints for the pre-integrated components of the IMU and wheel speed meter after removing the distortion;

[0018] Step 7: Calculate the multi-sensor weights;

[0019] Step 8: Perform weighted joint nonlinear optimization to obtain the optimized vehicle posture;

[0020] Step 9: Perform keyframe determination on the optimized vehicle posture and use the keyframes to update the point cloud map.

[0021] Optionally, in step 3, removing outliers from the original laser radar point cloud includes:

[0022] The random sampling consensus algorithm is used to remove outliers from the original point cloud of the LiDAR, while discarding the closer and farther points obtained by the LiDAR, and removing points with infinite distance in the original point cloud.

[0023] Optionally, in step 4, segmenting and clustering the original point cloud after elimination to eliminate noise points includes:

[0024] The original point cloud after elimination is segmented and clustered, and the clustered point cloud is given a label of 1. The categories with less than 30 points are judged as noise points and removed. After the elimination of noise points, the feature point matching of the laser point cloud is performed based on the category label.

[0025] Optionally, in step seven, the calculation of multi-sensor weights includes initialization detection, vehicle slip detection, and degraded environment detection.

[0026] Optionally, the initialization detection includes:

[0027] In the initial stage, that is, when the number of key frames is less than ten frames, the weight of the laser residual is set to be small to improve the positioning accuracy in the initial mapping stage.

[0028] Optionally, the vehicle slip detection includes:

[0029] If the vehicle is in a slip condition, the weight α of the wheel speedometer residual between the two frames of laser point cloud is w Set to 0.

[0030] Optionally, the degradation environment detection includes:

[0031] When a degraded environment is detected, the weight of the lidar is reduced, and the constraints of the IMU and wheel speed meter are relied upon to provide stable positioning in a short period of time.

[0032] Optionally, in step nine, performing key frame determination on the optimized vehicle posture includes:

[0033] The vehicle pose outputted by the tightly coupled module is received. If the inter-frame pose calculated by the front-end exceeds the preset translation or rotation, it is determined to be a key frame.

[0034] If it is not a keyframe, the point cloud data is discarded, but the data of the wheel speed meter and IMU are retained, and pre-integration continues to the next keyframe. The residuals of the IMU and wheel speed meter are expressed as the constraints between the two keyframes.

[0035] Optionally, in step nine, the point cloud map is updated using keyframes, including:

[0036] The point cloud of the key frame is transformed into the world coordinate system according to the optimized vehicle pose and updated to the map. At the same time, the map update adopts the incremental IKD-tree data structure to avoid the imbalance of the tree caused by adding new data.

[0037] The beneficial effects of this application are as follows:

[0038] The multi-sensor fusion SLAM system proposed in this application utilizes the advantages of multiple sensors to make up for their respective shortcomings, and proposes a weight distribution strategy. It implements weight distribution according to initialization detection, degradation detection and vehicle slip detection, performs weighted joint optimization, and improves the effect of nonlinear optimization. It has strong adaptability to the environment and can cope with problems such as lidar degradation environment, vehicle slip conditions, and instability in the initial stage of mapping. It has stable positioning and high accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0039] Figure 1 1 is a framework diagram of a highly adaptable multi-sensor weighted fusion SLAM system provided in an embodiment of the present application;

[0040] Figure 2 Schematic diagram of the hardware structure of a highly adaptable multi-sensor weighted fusion SLAM system provided in an embodiment of the present application;

[0041] Figure 3 This is a flow chart of a highly adaptable multi-sensor weighted fusion SLAM method provided by an embodiment of the present application;

[0042] Figure 4 This is a vehicle slip detection flow chart provided in an embodiment of the present application;

[0043] Figure 5 This is a flowchart of the degradation environment detection provided by the embodiment of the present application. DETAILED DESCRIPTION

[0044] The following will be combined with the drawings in the embodiments of this application to clearly and completely describe the technical solutions in the embodiments of this application. Obviously, the embodiments described are part of the embodiments of this application, not all of them. Based on the embodiments in this application, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of this application.

[0045] The terms "first," "second," and the like in the specification and claims of this application are used to distinguish similar objects, and are not used to describe a specific order or precedence. It should be understood that the terms used in this manner are interchangeable where appropriate, so that the embodiments of this application can be implemented in an order other than that illustrated or described herein, and that the objects distinguished by "first," "second," and the like are generally of the same type, and do not limit the number of objects; for example, the first object can be one or more. In addition, the term "and / or" in the specification and claims refers to at least one of the connected objects, and the character " / " generally indicates that the objects connected are in an "or" relationship.

[0046] The highly adaptable multi-sensor weighted fusion SLAM method provided by the embodiments of the present application is described in detail below with reference to the accompanying drawings through specific embodiments and their application scenarios.

[0047] See Figure 1 and 2 As shown, an embodiment of the present application provides a highly adaptable multi-sensor weighted fusion SLAM system, which includes a data preprocessing module 1, a tightly coupled front-end module 2, and a mapping module 3.

[0048] The data preprocessing module 1 obtains the original point cloud through the laser radar 11, IMU 12 and wheel speed meter 13, calculates the IMU pre-integral and wheel speed meter pre-integral between two consecutive frames of laser radar point clouds for IMU 12 and wheel speed meter 13; removes outliers from the laser radar original point cloud from the laser radar 11, segments and clusters the original point cloud after removal, removes noise points, and finally extracts the line feature points and surface feature points of the original point cloud to obtain a feature point cloud.

[0049] It should be noted that the system further includes an industrial computer 14 , and the laser radar 11 , the IMU 12 and the wheel speed meter 13 are respectively connected to the industrial computer 14 .

[0050] The tightly coupled front-end module 2 is responsible for subscribing to the feature point cloud output by the data preprocessing module 1 and the information of the IMU 12 and the wheel speed meter 13, using the IMU data to remove the motion distortion of the feature point cloud point by point, calculating the residual constraints of the laser radar 11, IMU 12 and wheel speed meter 13, and then performing initialization detection, vehicle slip detection, degradation detection, calculating the weights of the residuals of the laser radar 11, IMU 12 and wheel speed meter 13, and performing weighted joint nonlinear optimization to obtain the optimized vehicle posture.

[0051] The mapping module 3 performs key frame determination on the optimized vehicle posture and uses the key frames to update the point cloud map.

[0052] Recombination Figure 3As shown, the present application also provides a highly adaptable multi-sensor weighted fusion SLAM method based on the system, comprising:

[0053] Step 1: Read the data from the lidar, IMU, and wheel speed meter, and use the external parameters of the offline calibration to unify the data into the first frame coordinate system of the IMU to obtain the original point cloud. Among them, the state quantities to be optimized in the highly adaptable multi-sensor weighted fusion SLAM system are as follows:

[0054]

[0055] Among them, the first frame coordinate system of the fixed IMU is the G coordinate system, and the first frame coordinate system of the fixed lidar is the M coordinate system; I is the coordinate system of the IMU at the current moment, All are based on the G coordinate system, and the posture is represented based on the IMU system, corresponding to rotation, position, velocity, b w , b a They are the angular velocity and acceleration bias of the IMU respectively.

[0056] Step 2: Calculate the pre-integrated quantities of the IMU and wheel speedometer between two consecutive frames of lidar point clouds based on the lidar frequency.

[0057] It should be noted that the pre-integration quantity is only related to the IMU measurement value. It directly integrates the IMU data over a period of time to obtain the pre-integration quantity. This application uses the Euler median method for discretization. The IMU pre-integration formula is as follows:

[0058]

[0059]

[0060]

[0061]

[0062]

[0063] Where i and j represent the serial numbers of two consecutive frames of lidar point clouds, w and a are the discretized angular velocity and acceleration, Represent the pre-integrated quantities of position, velocity and rotation respectively, is the acceleration data of the IMU at time k, is the angular velocity data of the IMU at time k, δt is the time interval between two adjacent frames of IMU data, is the zero bias of the angular velocity at time k, is the zero bias of acceleration at time k;

[0064] The wheel speed meter calculates the pre-integrated component based on the vehicle's two-wheel chassis differential model as follows:

[0065]

[0066] Where k is the kth frame of wheel speed meter data in two consecutive frames of laser radar. The wheel speed meter reads the left and right wheel speed data of the vehicle. The pre-integrated quantity is derived based on the two-wheel differential model, x k+1 ,y k+1 ,θ k+1 are the vehicle’s horizontal and vertical positions and yaw angles, v L , v R is the left and right wheel speeds read by the wheel speed meter, d is the vehicle's powered wheel track, and Δt is the interval between two consecutive frames of wheel speed meter data.

[0067] Step 3: Use the random sampling consensus algorithm to remove outliers from the original LiDAR point cloud. At the same time, discard the close and distant points with poor stability and low confidence obtained by the LiDAR, and remove points with infinite distance in the original point cloud.

[0068] Step 4: Segment and cluster the original point cloud after elimination to eliminate noise points;

[0069] Specifically, the original point cloud after elimination is segmented and clustered, and the clustered point cloud is given a label of 1. Categories with less than 30 points are determined to be noise points and removed. After the elimination of noise points, the laser point cloud feature point matching is performed based on the category label to improve efficiency.

[0070] Step 5: Calculate the curvature of all original point clouds in the same line, divide the original point clouds into line feature points and surface feature points according to the curvature, and obtain the feature point cloud;

[0071] Step 6: Use IMU data to remove motion distortion of the feature point cloud point by point, and construct residual constraints for the pre-integrated components of the IMU and wheel speed meter after removing the distortion;

[0072] It should be noted that distortion removal is based on the uniform motion model assumption, that is, it is assumed that the vehicle has a uniform speed between two consecutive frames of laser point cloud.

[0073] The laser point cloud residual is constructed based on the point-line ICP and point-surface ICP as follows:

[0074]

[0075] Where r L is the residual constraint of the laser radar, j is the jth feature point, e is the set of line feature points, p is the set of surface feature points, is the coordinate of the nearest point on the map corresponding to the line feature point of the current frame, is the coordinate of the current j-th feature point in the M coordinate system; norm is the normal vector of the plane formed by the plane points in the map corresponding to the surface feature points of the current frame.

[0076] The residual of the IMU is constructed as follows, with a total of 15 dimensions:

[0077]

[0078] In the above formula, is the IMU pre-integrated quantity between two consecutive frames of laser point cloud, [] .xyz is the vector part of the quaternion, is the rotation matrix of the previous frame relative to the G coordinate system, Δt k is the interval between two consecutive frames of laser point cloud, g G It is the projection of the acceleration due to gravity in the G coordinate system.

[0079] The residuals of the wheel speed meter are constructed as follows, with a total of three dimensions:

[0080]

[0081] In the above formula, i and j represent the serial numbers of the two frames of laser point cloud, x j , x i is the horizontal coordinate of the vehicle at the corresponding moment, y j ,y i is the vertical coordinate of the vehicle at the corresponding moment, θ j ,θ i is the yaw angle of the vehicle at the corresponding moment.

[0082] Step 7: Calculate the multi-sensor weights;

[0083] Specifically, the calculation of multi-sensor weights includes initialization detection, vehicle slip detection, and degraded environment detection.

[0084] The initialization detection includes: in the initial stage, that is, when the number of key frames is less than ten frames, setting the weight of the laser residual to be smaller to improve the positioning accuracy in the initial mapping stage.

[0085] The vehicle slip detection includes: if the vehicle slips, the weight α of the wheel speed meter residual between the two frames of laser point cloud is w Set to 0.

[0086] It should be noted that under certain operating conditions, the vehicle may experience wheel slippage, which can make the data read by the wheel speedometer unreliable. If the wheel speedometer data during vehicle slippage is added to the constraints, system instability will be introduced, reducing the robustness of the system.

[0087] Therefore, this paper proposes a vehicle slip detection strategy. When the vehicle slips, the wheel speedometer data will suddenly increase, indicating a high wheel speed. At this time, the IMU attitude information is not affected by the slip and remains stable, reflecting the advantage of multiple sensors compensating for each other's shortcomings.

[0088] See also Figure 4 The figure shows the vehicle slip detection flow chart of the present invention. Before adding the wheel speedometer constraint to the optimization item, the wheel speedometer data is read and the speed change ΔV between the two frames of wheel speedometer is calculated. This is compared with the change threshold. If the wheel speedometer data fluctuates significantly, the IMU acceleration and angular velocity data of the past N frames are calculated within a sliding window (window size is N). If the acceleration and angular velocity are small, it is determined to be a slip state. When the vehicle slips, although the wheel speed data increases abnormally, its acceleration and angular velocity data should be very small, and the variance of the acceleration is also small. Therefore, the judgment formula is proposed as follows:

[0089]

[0090] In the above formula, a k =[a xk ,a yk ,a zk ] T is the acceleration data at time k, w k =[w xk ,w yk ,w zk ] T is the angular velocity data at time k, threshold m are the combined observation thresholds, ||a|| represents the size of vector a, represents the mean value of acceleration within the sliding window, and are the random noise variances of acceleration and angular velocity respectively, N is the size of the sliding window, and g is the local gravitational acceleration.

[0091] If A is less than the threshold, the vehicle speed has not changed significantly, and the vehicle is judged to be in a slip condition. The weight α of the wheel speedometer residual between the two frames of laser point cloud is w Set to 0.

[0092] The degraded environment detection includes: when a degraded environment is detected, reducing the weight of the laser radar, and relying on the constraints of the IMU and the wheel speed meter to provide stable positioning in a short time.

[0093] It should be noted that the laser radar can only obtain the geometric structure information of the environment. When the vehicle runs into an environment with a single geometric structure, the constraints provided by the point cloud are insufficient. In the direction where there is no structural information, the problem of insufficient constraints will occur, which makes it easy to be affected by noise, causing the optimization to go in the wrong direction. Therefore, the present invention proposes a degradation detection strategy. When a degraded environment is detected, the weight α of the laser radar is adjusted. L Reduce,rely on the constraints of IMU and wheel speedometer to provide stable positioning in a short time.

[0094] like Figure 5 The figure shows the flow chart of the degraded environment detection of the present invention. In the degraded environment detection, the constraints of the laser radar are first linearized to the form of Ax=b. Calculate A T The eigenvalues ​​and corresponding eigenvectors of A, the degradation factor D = λ min +1, compare the degradation factor with the degradation threshold t. The eigenvector corresponding to the eigenvalue smaller than the threshold is the direction of degradation. The degradation threshold is the intermediate value obtained by sampling in degraded and non-degraded environments based on experience.

[0095] In a degraded environment, α is taken according to the relationship between the degradation factor and the degradation threshold. L as follows:

[0096]

[0097] In the above formula, D is the degradation factor and t is the degradation threshold.

[0098] Step 8: Perform weighted joint nonlinear optimization to obtain the optimized vehicle posture;

[0099] Step 9: Perform keyframe determination on the optimized vehicle posture and use the keyframes to update the point cloud map.

[0100] Specifically, if all frame data are added to the map, the computational complexity of the system will increase. Therefore, this application provides a key frame strategy that receives the vehicle pose output by the tightly coupled module optimization. If the inter-frame pose calculated by the front end exceeds the preset translation or rotation amount, it is determined to be a key frame.

[0101] If it is not a keyframe, the point cloud data is discarded, but the data of the wheel speed meter and IMU are retained, and pre-integration continues to the next keyframe. The residuals of the IMU and wheel speed meter are expressed as the constraints between the two keyframes.

[0102] Furthermore, in step nine, the point cloud map is updated using keyframes, including:

[0103] The point cloud of the key frame is transformed into the world coordinate system according to the optimized vehicle pose and updated to the map. At the same time, the map update adopts the incremental IKD-tree data structure to avoid the imbalance of the tree caused by adding new data.

[0104] As the vehicle runs, the scale of the map continues to expand, making the system load increasingly heavier. Therefore, the present invention limits the map to a cube centered on the current vehicle position, so that the scale of the problem is limited to a certain range. The size of the cube can be set according to actual needs.

[0105] It should be noted that, in this document, the terms "comprises," "includes," or any other variations thereof are intended to encompass non-exclusive inclusion, such that a process, method, article, or apparatus comprising a series of elements includes not only those elements but also other elements not explicitly listed, or elements inherent to such process, method, article, or apparatus. In the absence of further limitations, an element defined by the phrase "comprising a ..." does not exclude the presence of other identical elements in the process, method, article, or apparatus comprising the element.

[0106] Furthermore, it should be noted that the scope of the methods and systems in the embodiments of the present application is not limited to performing functions in the order shown or discussed, but may also include performing functions substantially simultaneously or in reverse order depending on the functions involved. For example, the described methods may be performed in an order different from that described, and various steps may be added, omitted, or combined. In addition, features described with reference to certain examples may be combined in other examples.

[0107] The embodiments of the present application are described above in conjunction with the accompanying drawings, but the present application is not limited to the above-mentioned specific implementation methods. The above-mentioned specific implementation methods are merely illustrative and not restrictive. Under the guidance of this application, ordinary technicians in this field can also make many forms without departing from the purpose of this application and the scope of protection of the claims, all of which are within the protection of this application.

Claims

1. A highly adaptable multi-sensor weighted fusion SLAM system, characterized in that: include: The data preprocessing module collects raw point clouds using lidar, IMU, and wheel speedometer data, calculates the IMU pre-integral and wheel speedometer pre-integral between two consecutive frames of lidar point clouds, removes outliers from the original lidar point cloud, performs segmentation and clustering on the removed original point cloud, removes noise points, and finally extracts line feature points and surface feature points from the original point cloud to obtain a feature point cloud. The tightly coupled front-end module is responsible for subscribing to the feature point cloud output by the data preprocessing module and the information from the IMU and wheel speedometer. It uses the IMU data to remove the motion distortion of the feature point cloud point by point, calculates the residual constraints of the lidar, IMU, and wheel speedometer, and then performs initialization detection, vehicle slip detection, and degradation detection. It calculates the weights of the residuals of the lidar, IMU, and wheel speedometer, and performs weighted joint nonlinear optimization to obtain the optimized vehicle posture. The mapping module performs keyframe determination on the optimized vehicle pose and uses the keyframes to update the point cloud map.

2. A highly adaptable multi-sensor weighted fusion SLAM method based on the system according to claim 1, characterized in that: include: Step 1: Read the data from the lidar, IMU, and wheel speed meter, and use the external parameters of the offline calibration to unify the data into the first frame coordinate system of the IMU to obtain the original point cloud; Step 2: Calculate the pre-integrated quantities of the IMU and wheel speedometer between two consecutive frames of lidar point clouds based on the lidar frequency. Step 3: Remove outliers from the original LiDAR point cloud; Step 4: Segment and cluster the original point cloud after elimination to eliminate noise points; Step 5: Calculate the curvature of all original point clouds in the same line, divide the original point clouds into line feature points and surface feature points according to the curvature, and obtain the feature point cloud; Step 6: Use IMU data to remove motion distortion of the feature point cloud point by point, and construct residual constraints for the pre-integrated components of the IMU and wheel speed meter after removing the distortion; Step 7: Calculate the multi-sensor residual weights; Step 8: Perform weighted joint nonlinear optimization to obtain the optimized vehicle posture; Step 9: Perform keyframe determination on the optimized vehicle posture and use the keyframes to update the point cloud map.

3. The method according to claim 2, characterized in that In step 3, the outlier points of the original laser radar point cloud are removed, including: The random sampling consensus algorithm is used to remove outliers from the original point cloud of the LiDAR, while discarding the closer and farther points obtained by the LiDAR, and removing points with infinite distance in the original point cloud.

4. The method according to claim 2, characterized in that In step 4, the original point cloud is segmented and clustered to remove noise points, including: The original point cloud after elimination is segmented and clustered, and the clustered point cloud is labeled. The categories with less than 30 points are determined to be noise points and removed. After the elimination of noise points, the laser point cloud feature point matching is performed based on the category labels.

5. The method according to claim 2, characterized in that In step seven, the calculation of multi-sensor residual weights includes initialization detection, vehicle slip detection, and degraded environment detection.

6. The method according to claim 5, characterized in that The initialization detection includes: In the initial stage, that is, when the number of key frames is less than ten frames, the weight of the laser residual is set to be small to improve the positioning accuracy in the initial mapping stage.

7. The method according to claim 6, characterized in that The vehicle slip detection includes: If the vehicle is in a slip condition, the weight of the wheel speedometer residual between the two frames of laser point cloud is Set to 0.

8. The method according to claim 7, characterized in that The degradation environment detection includes: When a degraded environment is detected, the weight of the lidar is reduced, and the constraints of the IMU and wheel speed meter are relied upon to provide stable positioning in a short period of time.

9. The method according to claim 2, characterized in that In step nine, the key frame determination of the optimized vehicle posture includes: The vehicle pose outputted by the tightly coupled module is received. If the inter-frame pose calculated by the front-end exceeds the preset translation or rotation, it is determined to be a key frame. If it is not a keyframe, the point cloud data is discarded, but the data of the wheel speed meter and IMU are retained, and pre-integration continues to the next keyframe. The residuals of the IMU and wheel speed meter are expressed as the constraints between the two keyframes.

10. The method according to claim 2, characterized in that In step nine, the point cloud map is updated using keyframes, including: The point cloud of the key frame is transformed into the world coordinate system according to the optimized vehicle pose and updated to the map. At the same time, the map update adopts the incremental IKD-tree data structure to avoid the imbalance of the tree caused by adding new data.

Images