An improved LeGO-LOAM method considering bumpy road conditions

Abstract

The application relates to the field of real-time accurate positioning, in particular to an improved LeGO-LOAM method considering a bumpy road condition, which comprises the following steps: S1: vehicle bumping state detection; S2: segmenting a complete point cloud sequence in a scene according to a vehicle bumping state period obtained by a bumping state detection algorithm to obtain a plurality of point cloud subsequences in non-bumping states; S3: performing point cloud subsequence recombination operation on the point cloud subsequences in non-bumping states after segmentation according to time sequences; S4: after recombination of the point cloud subsequences in non-bumping states in adjacent time sequences, completing global optimization and outputting a global position and a point cloud map; the positioning accuracy and the mapping quality of a laser radar SLAM in a bumpy road condition can be improved.

Description

Technical Field

[0001] This invention relates to the field of real-time precise positioning, and more specifically to an improved LeGO-LOAM method that takes into account bumpy road conditions. Background Technology

[0002] Real-time and accurate positioning technology is one of the keys to autonomous vehicles, providing crucial information such as the vehicle's position and attitude in a global coordinate system. Currently, real-time positioning typically employs three methods: 1) high-precision integrated navigation equipment utilizing satellite signals and inertial measurement units (IMUs); 2) high-precision map alignment technology; and 3) Simultaneous Localization and Mapping (SLAM) technology.

[0003] While high-precision integrated navigation equipment provides stable positioning results, it suffers from problems such as satellite signal obstruction, high hardware costs, and opaque algorithms. High-precision map alignment methods, on the other hand, rely on pre-drawn high-precision maps and exhibit poor robustness to environmental changes. Therefore, SLAM (Simultaneous Localization and Mapping) has become a research hotspot for meeting the demands of real-time, accurate positioning. Significant progress has been made in SLAM research, including visual SLAM and lidar SLAM positioning schemes, with lidar SLAM offering higher positioning accuracy and robustness.

[0004] In existing LiDAR SLAM work, researchers have improved matching accuracy by studying the correspondence between matching objects during point cloud matching. The earliest methods directly matched points from the original point cloud, followed by matching based on geometric features in the environment, then matching by manually constructing local or global descriptors, and more recently, learning-based abstract feature matching. With the improvement of matching technology, the accuracy of point cloud matching has also been continuously improved.

[0005] However, the accuracy of point cloud matching is also affected by the complexity of the point cloud data itself. Factors such as rain and snow noise, dynamic obstacles in the scene, and rugged or bumpy ground conditions all increase the complexity of the point cloud data, leading to a decrease in matching accuracy. Bumpy road conditions cause significant changes in vehicle attitude within a short period, resulting in large motion distortions in the point cloud data and reducing its accuracy. Simultaneously, the vehicle tilt caused by bumps reduces the vertical field of view of the LiDAR, resulting in uneven distribution of point cloud data on the ground. This leads to discontinuities in the objective function of the point cloud matching stage in the LeGO-LOAM algorithm, causing the iterative solution process to fail to converge correctly, reducing the accuracy of point cloud matching, and increasing the vehicle's vertical positioning error. While the problem of increased vehicle vertical positioning error due to reduced accuracy or uneven / missing distribution of ground point cloud data can be mitigated by adding ground constraints or utilizing additional sensors such as IMUs and GPS for information complementation, these methods rely on the correctness of the ground constraints or the accuracy of other sensors like IMUs and GPS, and cannot guarantee consistently accurate vehicle pose output under bumpy road conditions. Therefore, this invention proposes a LiDAR SLAM algorithm based on the LeGO-LOAM algorithm, with point cloud sequence segmentation and reconnection as its core idea, to solve the problem of increased vehicle vertical positioning error under bumpy road conditions. Summary of the Invention

[0006] The purpose of this invention is to provide an improved LeGO-LOAM method that takes into account bumpy road conditions, which can improve the positioning accuracy and mapping quality of lidar SLAM under bumpy road conditions.

[0007] The objective of this invention is achieved through the following technical solution:

[0008] An improved LeGO-LOAM method that takes into account bumpy road conditions includes the following steps:

[0009] S1: Vehicle bump condition detection;

[0010] S2: The complete point cloud sequence in the scene is segmented according to the vehicle bump state period obtained by the bump state detection algorithm to obtain several point cloud subsequences under non-bump state conditions.

[0011] S3: After segmentation, the point cloud subsequences in the non-bumpy state are reconnected according to the time sequence.

[0012] S4: After reconnecting the point cloud subsequences under adjacent time-series non-turbulent conditions, complete the global optimization and output the global pose and point cloud map;

[0013] The vehicle bump state detection is based on the vertical distance between the nearest planes of the ground point clouds in the complete point cloud sequence of the scene, which reflects the degree of overlap in the vertical direction between the ground point clouds of two adjacent frames. This allows us to determine whether there is a significant change in the vehicle pose in the vertical direction between the two adjacent frames, i.e., to determine whether the current frame point cloud data is point cloud data generated under bump conditions.

[0014] The vehicle bump condition detection includes the following steps:

[0015] S11: Define t i The point cloud data generated at each moment is Ground point set obtained after point cloud data segmentation Non-ground point set In the point cloud data feature extraction stage, the curvature values ​​of all point cloud points in their neighborhoods are sorted from largest to smallest to obtain the corresponding main point set. Secondary Corner Set Subplane point set and principal plane point set All points in the principal plane point set come from the ground point set, which is called the ground feature point set.

[0016] S12: By adjusting t i Each point in the ground feature point set at time t i-1 In the KD-tree constructed from the ground point set at each time step, search for three non-collinear nearest neighbors to form a point-to-plane correspondence, calculate the perpendicular distance between the correspondences, and count the distances exceeding a set threshold d. th The number of correspondences can reflect t i time relative to t i-1 The degree of change in the vehicle's position and orientation in the vertical direction at any given moment;

[0017] The formula for calculating the distance from a point to a surface is:

[0018]

[0019] in For t i The three-dimensional coordinates of the j-th point in the ground feature point set at time step [time]. They are respectively In t i-1 The three-dimensional coordinates of the three nearest neighboring points in the ground point set at any given time;

[0020] The discriminant formula for detecting the bumpy state is:

[0021]

[0022] Where n is t iThe number of points in the ground feature point set at a moment, f(·) is a counting function, c is the count value, and d th is the distance threshold from a point to a plane; when the distance d j > d th the value of f(·) is 1, and when d j ≤ d th the value of f(·) is 0; when the count c ≥ m, it is considered that the vehicle is in a bumpy state at time t i When all the ground feature points of the current frame are processed in a loop and the count c < m, it is considered that the vehicle is in a non-bumpy state at time t i ;

[0023] In S2, the point cloud data in the detected bumpy state is removed and the relevant data of the previous frame before the bumpy state occurs is stored, and after the bumpy state ends, the point cloud sequence is segmented and the displacement obtained by assuming uniform motion during the bumpy period is calculated; the point cloud data in the detected non-bumpy state is optimized within the point cloud subsequence;

[0024] In S3, for the point cloud subsequences before and after the bump, a reconnect operation is performed. For the first reconnect, the transformation relationship between the vehicle pose at the end of the bump and the vehicle pose at the beginning of the bump is obtained by assuming uniform motion during the bump; for subsequent reconnect processes, the pose transformation between the vehicle poses obtained from the adjacent frames within the point cloud subsequence after the bump and the vehicle pose obtained from the previous reconnect is used to complete the reconnect process of the vehicle pose and the corresponding point cloud data from the current point cloud subsequence to the previous point cloud subsequence;

[0025] In S4, for the global optimization after reconnecting, the vehicle pose after reconnecting for each frame in the point cloud subsequence after the bump obtained from the reconnect process is used, and the corresponding point cloud data is transformed into the coordinate system of the point cloud subsequence before the bump, thereby completing the frame-to-submap matching optimization process between the transformed point cloud data for each frame and the point cloud submap formed in the point cloud subsequence before the bump, obtaining a more accurate global vehicle pose after optimization, and updating the point cloud map;

[0026] The segmentation and removal of the complete point cloud sequence includes the following steps:

[0027] S21: The complete point cloud sequence in the driving scene is segmented into several point cloud subsequences based on the start and end times of the detected vehicle bumpy state;

[0028] S22: For the start time of each point cloud subsequence, an initialization operation is performed to eliminate the influence of the removal of point cloud data in the bumpy state on subsequent positioning;

[0029] S23: Perform optimization processing on the point cloud data in the non-bumpy state within each point cloud subsequence to obtain the vehicle pose and point cloud map in the point cloud subsequence coordinate system.

[0030] S24: Connect the vehicle pose in the coordinate system of the point cloud subsequence obtained by optimization with the pose of the point cloud subsequence before the bump through the point cloud subsequence reconnection step, so as to obtain the corresponding vehicle pose in the coordinate system of the previous point cloud subsequence under the current point cloud subsequence, and thus complete the global optimization and point cloud mapping.

[0031] The reconnection of point cloud subsequences under non-bumpy conditions includes the following steps:

[0032] S31: Assume that the vehicle travels at a constant speed during the bumpy period, at the speed of the vehicle before the bumpy moment, in order to obtain the pose transformation relationship between the vehicle pose of the starting frame of the point cloud subsequence after the bumpy and the vehicle pose of the ending frame of the point cloud subsequence before the bumpy in the coordinate system of the point cloud subsequence before the bumpy.

[0033] S32: By applying the displacement obtained based on the uniform velocity assumption to the vehicle pose of the last frame of the point cloud subsequence before the bump, the corresponding vehicle pose of the starting frame of the point cloud subsequence after the bump is obtained in the coordinate system of the point cloud subsequence before the bump, and the point cloud data of the corresponding frame in the point cloud subsequence after the bump is transformed to the coordinate system of the point cloud subsequence before the bump.

[0034] S33: Global pose optimization is achieved by matching the transformed point cloud with the point cloud sub-map, the global pose of the vehicle is obtained, and the point cloud map is updated.

[0035] The beneficial effects of this invention are as follows:

[0036] First, a bumpy state detection algorithm is used to identify and remove point cloud data generated under bumpy conditions, eliminating the impact of bumps on LiDAR SLAM localization and mapping from the data source. Second, the point cloud sequence in the entire scene is segmented to generate several point cloud subsequences to ensure the normal processing of subsequent localization results after point cloud removal under bumpy conditions. Third, by assuming uniform motion during bumpy conditions, the point cloud subsequences are reconnected to ensure the consistency and integrity of localization and mapping. Finally, through comparison of experimental results in indoor and outdoor scenes, it can be seen that the localization accuracy and point cloud mapping quality of this method are superior to the LeGO-LOAM algorithm under bumpy road conditions. Attached Figure Description

[0037] The present invention will now be described in further detail with reference to the accompanying drawings and specific implementation methods.

[0038] Figure 1 This is a schematic diagram of the architecture of the present invention;

[0039] Figure 2 This is a flowchart of the algorithm of the present invention;

[0040] Figure 3 This is a flowchart of the bump state detection process of the present invention;

[0041] Figure 4 This is a flowchart of point cloud data processing under turbulent conditions according to the present invention;

[0042] Figure 5 This is a flowchart of point cloud data processing under non-bumpy conditions according to the present invention;

[0043] Figure 6 This is a flowchart of the point cloud subsequence reconnection process of the present invention;

[0044] Figure 7 This is a schematic diagram comparing the localization results of the present invention and the LeGO-LOAM algorithm for different durations of turbulence;

[0045] Figure 8 This is a comparative diagram of the positioning results of the present invention and the LeGO-LOAM algorithm in outdoor bumpy road conditions with sparse corner features;

[0046] Figure 9 This is a comparative diagram of the positioning results of the present invention and the LeGO-LOAM algorithm in outdoor road conditions with rich corner features;

[0047] Figure 10 This is a schematic diagram comparing the point cloud mapping results of the present invention and the LeGO-LOAM algorithm under indoor bumpy road conditions. Detailed Implementation

[0048] The present invention will now be described in further detail with reference to the accompanying drawings.

[0049] Depend on Figure 10 As can be seen, (a) and (c) in the figure are point cloud map views of the zx plane and zy plane obtained by running the LeGO-LOAM algorithm under bumpy conditions, and (b) and (d) are point cloud map views corresponding to LeGO-LOAM obtained by running the algorithm of the present invention. It can be seen that the point cloud map obtained by running the algorithm of the present invention has lower ghosting phenomenon, and the mapping result in the vertical direction is more consistent with the actual straight corridor scene, thus obtaining better mapping quality than the LeGO-LOAM algorithm.

[0050] Figure 2 This is a flowchart of the algorithm of the present invention, describing the overall process of point cloud sequence segmentation and reconnection strategy. The algorithm architecture is a recursive call process, where the recursive function body is called the point cloud subsequence processing and reconnection process.

[0051] The internal process of recursive calls includes six main steps: initialization, turbulence state detection module, turbulence state point cloud data processing, non-turbulence state point cloud data processing, point cloud subsequence reconnection, and global optimization.

[0052] The point cloud queue in the process is used to cache the address of each frame of point cloud data. The point cloud data contains the three-dimensional spatial coordinates and depth values ​​of all laser points in each frame. The initialization operation of each recursive call includes defining the LiDAR coordinate system corresponding to the first frame of point cloud data in the current point cloud subsequence as the point cloud subsequence coordinate system, initializing the vehicle pose of the first frame in the current point cloud subsequence, and clearing the historical positioning trajectory and historical point cloud map in the current point cloud subsequence.

[0053] The recursion termination condition is jointly determined by whether the vehicle has reached its destination and whether all data in the point cloud queue has been processed. Figures 1 to 10 As shown, in order to solve the technical problem of "how to reduce the impact of bumpy road conditions on the positioning accuracy and mapping quality of LeGO-LOAM", the steps and functions of an improved LeGO-LOAM method that takes into account bumpy road conditions are explained in detail below.

[0054] This invention presents an improved LeGO-LOAM method that considers bumpy road conditions. It uses the advanced LiDAR SLAM algorithm LeGO-LOAM as its basic framework, incorporating a bumpy road condition detection module and reconstructing and recursively encapsulating the algorithm flow to achieve a point cloud sequence segmentation-reconnection processing mode based on a recursive architecture. Global optimization is then performed after reconnecting the point cloud subsequences to obtain more accurate localization and point cloud maps.

[0055] The specific implementation steps are as follows:

[0056] S1: Vehicle bump condition detection;

[0057] The process of the bump condition detection module is as follows: Figure 3 As shown, turbulence detection is achieved by evaluating the point-to-plane distance between the ground feature point cloud of the current frame and the downsampled ground point cloud of the previous frame. The flowchart illustrates that first, a search tree is created using the 3D coordinates of all points in the downsampled ground point cloud of the previous frame as nodes in a KD-tree. Then, feature point number 1 is extracted from the current frame's ground feature point cloud, and the KD-tree searches for three non-collinear point cloud points that are its nearest neighbors. These three non-collinear point cloud points form a plane. By solving the plane equation and obtaining the plane normal vector, the vertical distance d1 from the query point to the plane in 3D space can be calculated. Finally, it is determined whether d1 exceeds a set threshold d. thIf yes, increment the count c; otherwise, continue processing the next point cloud point in the current frame's ground feature point cloud according to the sequence number. If the value of c exceeds the set counting threshold m during the loop processing, the current frame's point cloud is determined to be point cloud data generated under turbulent conditions, and the overall framework process continues. Figure 2 As shown, the point cloud data processing flow under turbulent conditions is as follows: if the value of c is less than the set threshold m, the next point cloud point in the ground feature point cloud of the current frame is processed in a loop.

[0058] If the loop continues processing all point cloud points in the current frame's ground feature point cloud (i.e., j>n), and the value of c does not exceed the set threshold m during this period, then the current frame's point cloud is considered to be point cloud data in a non-bumpy state. Following the overall framework, it is then determined whether point cloud data in a bumpy state existed previously. At this point, the bumpy state detection module process ends.

[0059] S2: Segment the complete point cloud sequence in the scene according to the vehicle bump state time period obtained by the bump state detection algorithm to obtain several point cloud subsequences under non-bump states:

[0060] When the vehicle encounters a bump, the bump detection module determines that the current frame's point cloud data is point cloud data under bump conditions, and then enters the point cloud data processing flow under bump conditions, such as... Figure 4 As shown, at this point, it is determined that the previous frame of data is point cloud data under a non-bumpy state. The timestamp of the previous frame and the vehicle speed at the corresponding timestamp are recorded, and point cloud data under a bumpy state is marked in the driving scene. The process then returns to the recursion termination condition. If the recursion termination condition is not met, the next frame of point cloud data is processed. If the next frame of point cloud data is still point cloud data under a bumpy state, the process returns directly to the recursion termination condition. This process continues until a frame of point cloud data is detected to be point cloud data under a non-bumpy state or the recursion termination condition is met during the loop processing. At this point, the processing flow for point cloud data under a bumpy state ends.

[0061] If the point cloud data processing flow ends under bumpy conditions and the recursion termination condition is not met, the next frame of point cloud data will continue to be processed. The bumpy state detection module determines that the current frame data is point cloud data under non-bumpy conditions, and that there was point cloud data under bumpy conditions before and the previous frame data was point cloud data under bumpy conditions. By assuming that the vehicle moves at a constant speed at the speed before the bumps started, the vehicle displacement during the bumps is calculated, and then the recursive call is performed to establish the next point cloud subsequence.

[0062] The next point cloud subsequence begins with point cloud data in a non-bumpy state, where there was point cloud data in a bumpy state previously, and the previous frame of data was point cloud data in a non-bumpy state. At this point, the processing flow for the point cloud data in the non-bumpy state of the current subsequence is performed, as follows: Figure 5As shown, the process first performs point cloud feature matching between the current frame and the previous frame to obtain the vehicle's inter-frame pose transformation. Then, it checks if the time interval between two consecutive optimizations within the current point cloud subsequence exceeds 0.3 seconds. If it exceeds 0.3 seconds, the optimization within the point cloud subsequence is completed, and the resulting data is inserted into the end of the reconnection queue, then the point cloud subsequence reconnection process begins. If the time interval is less than 0.3 seconds, the process directly returns to the recursive termination condition. The reconnection queue stores a timestamp, the address of the point cloud data at the corresponding timestamp, and the vehicle pose in the point cloud subsequence coordinate system at the corresponding timestamp. The process iterates through the next frame of point cloud data until the bump detection module detects a bump again, ending the current subsequence's non-bump state point cloud data processing flow and entering the current subsequence's bump point cloud processing flow.

[0063] In the algorithm design, in order to improve the overall stability of the algorithm, we set that if the point cloud data in the non-bumpy state after the point cloud data processing flow under bumpy conditions is less than 3 frames, then point cloud sequence segmentation will not be performed. This is to alleviate the reconnection anomaly caused by the previous subsequence not having enough time to complete processing and the next subsequence being reconnected due to the excessively short point cloud subsequence.

[0064] S3: After segmentation, the point cloud subsequences in the non-bumpy state are reconnected according to the temporal order:

[0065] The previous point cloud subsequence, after processing, reconnection, and global optimization, yields the pose and point cloud map in the global coordinate system. Indicates the position t in the previous point cloud subsequence i The global pose of the vehicle at all times. use Indicates the current point cloud subsequence t i+k Vehicle pose in the cloud subsequence coordinate system at a given time point.

[0066] The first reconnection between the current point cloud subsequence and the previous point cloud subsequence is achieved via t i Time to t i+k Assuming uniform velocity at time t, calculate the displacement of the cart s = v*(t). i+k -t i )=[s x ,s y ,s z ], where t i+k -t i Let v be the time interval, and t be the value of v. i The linear velocity of the vehicle at any given moment.

[0067] By applying s Get t i+k global pose of the vehicle at all times and t i+k The point cloud data corresponding to each moment is transformed to the global pose, completing global optimization. In subsequent reconnection processes, t i+k+1 Global pose at time step It is by... arrive The pose transformation matrix is ​​applied to The flowchart of the point cloud subsequence reconnection process is as follows: Figure 6 As shown.

[0068] S4: After reconnecting adjacent point cloud subsequences under non-turbulent conditions, global optimization is performed and the global pose and point cloud map are output:

[0069] The global optimization process is the same as the LeGO-LOAM global optimization process. The difference is that the point cloud data and poses to be optimized are all derived from the results of optimization within the point cloud subsequence. By reconnecting the vehicle pose and point cloud data in the current point cloud subsequence coordinate system to obtain the corresponding vehicle pose and point cloud data in the previous point cloud subsequence coordinate system, the matching optimization between the point cloud data and the point cloud sub-map of the previous point cloud subsequence is completed, resulting in a more accurate vehicle pose and an updated point cloud map.

[0070] The following comparison and verification of the positioning accuracy and mapping quality of the technical solution of the present invention under bumpy road conditions is conducted in indoor and outdoor scenarios.

[0071] The evaluation strategy involves artificially creating bumps in a straight corridor scenario and conducting 10 experiments in the same scenario. The average difference between the positioning results of the starting and ending points of the driving route in each direction is used to represent the positioning error of the algorithm in the straight corridor scenario. This error has six directional components: x (directly left), y (directly up), z (directly forward), and pitch, yaw, and roll around the x, y, and z axes.

[0072] Since it is impossible to guarantee that the driving route of the vehicle will be exactly the same in each experiment, the experiment only compares and evaluates y, pitch, and roll.

[0073] Define the vehicle's positioning error in the y-direction as e y for:

[0074]

[0075] in Let Y be the component of the vehicle's pose in the y-direction at the moment the vehicle arrives at its destination. y represents the vehicle's pose in the y-direction at the moment of vehicle start-up. n represents the number of times the algorithm is run repeatedly on the same dataset.

[0076] The evaluation focuses on the degree of positioning error correction in the y-direction, i.e., the vertical direction, and defines the vertical error correction ratio:

[0077]

[0078] in e obtained by running the LeGO-LOAM algorithm y , To propose the e obtained from the algorithm execution y .

[0079] To test the applicability of this invention in solving the positioning error problem caused by bumpy conditions, we will test the stability of the algorithm proposed in this invention in terms of three aspects: number of bumps, frequency of bumps, and duration of bumps.

[0080] The experimental results are shown in the table below:

[0081] Table 1. Localization error of LeGO-LOAM algorithm at different vehicle speeds:

[0082]

[0083] Table 1 concludes that when the vehicle speed is <0.4 m / s, bumps have almost no impact on the localization results of the LeGO-LOAM algorithm; when the vehicle speed is ≥0.4 m / s, the results are significantly improved. y The changes show that turbulence has a significant impact on the localization results of the LeGO-LOAM algorithm.

[0084] Table 2. Positioning error of the algorithm under different number of bumps:

[0085]

[0086] Table 2 shows that bumps are the main cause of positioning errors in the y-direction, and the algorithm proposed in this paper can obtain stable positioning results under different numbers of bumps. Compared with LeGO-LOAM, the error correction ratio in the vertical direction reaches the highest of 98.6%.

[0087] Table 3. Positioning error of the algorithm under different bump frequencies:

[0088]

[0089] Table 3 shows that when the turbulence frequency is less than or equal to 1.0 Hz, the proposed algorithm achieves a vertical positioning error correction rate of over 89.2% compared to the LeGO-LOAM algorithm. When the turbulence frequency exceeds 10.0 Hz and the turbulence duration is less than 3.1 seconds, the proposed algorithm reduces the vertical positioning error by an average of 58% compared to the LeGO-LOAM algorithm.

[0090] When the frequency reaches 1.3Hz or 1.9Hz, due to the short time between two adjacent bumps, the processing of the point cloud subsequence before the bump does not have enough time to complete the optimization within the subsequence. As a result, the point cloud subsequence after the bump cannot transform the vehicle pose coordinates to the coordinate system of the point cloud subsequence before the bump by assuming the vehicle moves at a constant speed during the bump. This leads to the failure of the point cloud subsequence reconnection, and the algorithm proposed in this paper fails at this time.

[0091] Depend on Figure 7 The localization results show that if the duration of the bumpy frequency (>10.0Hz) exceeds 3.1 seconds, the overlap area between the point cloud sub-map before the bump and the point cloud data after the bump will decrease, the similarity features in the point cloud matching process will decrease, and the cumulative distance of the bump process will be long. At the same time, due to the long cumulative distance of the bump process, the error of using the assumption of uniform speed of the vehicle in the bumpy state to perform coordinate transformation of the vehicle pose before and after the bump is too large, which leads to point cloud matching errors during point global optimization. At this time, the algorithm proposed in this paper fails.

[0092] Figure 8 The two images show a comparison of positioning results under outdoor corner feature sparse bumpy road conditions; Figure 9 The two images show a comparison of localization results under bumpy road conditions with abundant outdoor corner features. In outdoor scenarios with abundant corner features, the impact of bumps on the localization and mapping results of the LeGO-LOAM algorithm is negligible. However, in scenarios where surface features are the primary feature and corner features are relatively few, bumps significantly affect the localization results of the LeGO-LOAM algorithm. In scenarios with abundant corner features, the proposed algorithm achieves similar localization accuracy to the LeGO-LOAM algorithm. In scenarios lacking corner features, the proposed algorithm obtains vertical localization results that more closely resemble reality.

[0093] It should be noted that the above content merely illustrates the technical concept of the present invention and should not be construed as limiting the scope of protection of the present invention. For those skilled in the art, various improvements and modifications can be made without departing from the principle of the present invention, and all such improvements and modifications fall within the scope of protection of the claims of the present invention.

Claims

1. An improved LeGO-LOAM method considering bumpy road conditions, characterized in that: The method includes the following steps: S1: Vehicle bump condition detection; S2: The complete point cloud sequence in the scene is segmented according to the vehicle bump state period obtained by the bump state detection algorithm to obtain several point cloud subsequences under non-bump state conditions. S3: After segmentation, the point cloud subsequences in the non-bumpy state are reconnected according to the time sequence. S4: After reconnecting the point cloud subsequences under adjacent time-series non-turbulent conditions, complete the global optimization and output the global pose and point cloud map; In step S2, the point cloud data under the detected turbulent state is removed and the relevant data of the frame before the turbulent state occurs is stored. After the turbulent state ends, the point cloud sequence is segmented and the displacement of the uniform motion assumption during the turbulence is obtained. The point cloud data under the detected non-turbulent state is optimized within the point cloud subsequence. In step S3, a reconnection operation is performed on the point cloud subsequences before and after the bump. For the first reconnection, the transformation relationship between the vehicle pose at the end of the bump and the vehicle pose at the beginning of the bump is obtained by assuming uniform motion during the bump. For subsequent reconnection processes, the pose transformation relationship between the vehicle pose obtained in the previous reconnection is obtained by transforming the vehicle pose in adjacent frames within the point cloud subsequence after the bump. This completes the reconnection process from the current point cloud subsequence to the vehicle pose and corresponding point cloud data of the previous point cloud subsequence.

2. The improved LeGO-LOAM method considering bumpy road conditions according to claim 1, characterized in that: The vehicle bump state detection is based on the vertical distance between the nearest neighbor planes of the ground point clouds in the complete point cloud sequence of the scene. This reflects the degree of overlap in the vertical direction between the ground point clouds of two adjacent frames, and thus determines whether there is a significant change in the vehicle pose in the vertical direction between the two adjacent frames. In other words, it determines whether the current frame point cloud data is point cloud data generated under bump conditions.

3. The improved LeGO-LOAM method considering bumpy road conditions according to claim 1, characterized in that: The vehicle bump condition detection includes the following steps: S11: Define t i The point cloud data generated at each moment is Ground point set obtained after point cloud data segmentation Non-ground point set In the point cloud data feature extraction stage, the curvature values ​​of all point cloud points in their neighborhoods are sorted from largest to smallest to obtain the corresponding main point set. Secondary Corner Set Subplane point set and principal plane point set All points in the principal plane point set come from the ground point set, which is called the ground feature point set. S12: By adjusting t i Each point in the ground feature point set at time t i-1 In the KD-tree constructed from the ground point set at each time step, search for three non-collinear nearest neighbors to form a point-to-plane correspondence, calculate the perpendicular distance between the correspondences, and count the distances exceeding a set threshold d. th The number of correspondences can reflect t i time relative to t i-1 The degree of change in the vehicle's position and orientation in the vertical direction at any given time.

4. The improved LeGO-LOAM method considering bumpy road conditions according to claim 3, characterized in that: The formula for calculating the distance from a point to a surface is: in For t i The three-dimensional coordinates of the j-th point in the ground feature point set at time step [time]. They are respectively In t i-1 The three-dimensional coordinates of the three nearest neighboring points in the ground point set at any given time.

5. An improved LeGO-LOAM method considering bumpy road conditions according to claim 4, characterized in that: The discriminant formula for detecting the bumpy state is: where n is the number of points in the ground feature point set at time t, f(·) is a counting function, c is the count value, and d i is the distance threshold from a point to a plane; when the distance d th > d j , the value of f(·) is 1, and when d th ≤ d j , f(·) is 0; when the count c ≥ m, it is considered that the vehicle is in a bumpy state at time t th ; after processing all the ground feature points of the current frame in a loop, if the count c < m, it is considered that the vehicle is in a non-bumpy state at time t i . i ​ 6. The improved LeGO-LOAM method considering bumpy road conditions according to claim 1, characterized in that: In step S4, the global optimization after reconnection involves transforming the vehicle pose of each frame in the post-bump point cloud subsequence obtained during the reconnection process to the coordinate system of the pre-bump point cloud subsequence. This transforms the transformed point cloud data and the point cloud sub-map formed in the pre-bump point cloud subsequence to complete the frame-to-sub-map matching optimization process, resulting in a more accurate global vehicle pose after optimization, and updating the point cloud map.

7. An improved LeGO-LOAM method considering bumpy road conditions according to claim 1, characterized in that: The segmentation and removal of the complete point cloud sequence includes the following steps: S21: By detecting the start and end times of the vehicle bumpy state, the complete point cloud sequence in the driving scene is divided into several point cloud subsequences; S22: For the start time of each point cloud subsequence, perform an initialization operation to eliminate the impact of point cloud data removal under turbulent conditions on subsequent positioning; S23: Perform optimization processing on the point cloud data in the non-bumpy state within each point cloud subsequence to obtain the vehicle pose and point cloud map in the point cloud subsequence coordinate system. S24: Connect the vehicle pose in the coordinate system of the point cloud subsequence obtained by optimization with the pose of the point cloud subsequence before the bump through the point cloud subsequence reconnection step, so as to obtain the corresponding vehicle pose in the coordinate system of the previous point cloud subsequence under the current point cloud subsequence, and thus complete the global optimization and point cloud mapping.

8. The improved LeGO-LOAM method considering bumpy road conditions according to claim 1, characterized in that: The reconnection of point cloud subsequences under non-bumpy conditions includes the following steps: S31: Assume that the vehicle travels at a constant speed during the bumpy period, at the speed of the vehicle before the bumpy moment, in order to obtain the pose transformation relationship between the vehicle pose of the starting frame of the point cloud subsequence after the bumpy and the vehicle pose of the ending frame of the point cloud subsequence before the bumpy in the coordinate system of the point cloud subsequence before the bumpy. S32: By applying the displacement obtained based on the uniform velocity assumption to the vehicle pose of the last frame of the point cloud subsequence before the bump, the corresponding vehicle pose of the starting frame of the point cloud subsequence after the bump is obtained in the coordinate system of the point cloud subsequence before the bump, and the point cloud data of the corresponding frame in the point cloud subsequence after the bump is transformed to the coordinate system of the point cloud subsequence before the bump. S33: Global pose optimization is achieved by matching the transformed point cloud with the point cloud sub-map, the global pose of the vehicle is obtained, and the point cloud map is updated.

Images