A Piecewise SLAM Method Based on Stable Segments in Vigorous Motion Scenes

Abstract

This invention belongs to the field of SLAM technology, specifically relating to a segmented SLAM method based on stable segments in violent motion scenes. The method includes: acquiring point cloud data and IMU data for the current frame; extracting the point cloud transformation angle based on the current frame point cloud data; acquiring the IMU transformation angle of adjacent frames based on the IMU data; comparing the point cloud transformation angle with the IMU rotation angle; if the point cloud transformation angle is less than the IMU rotation angle and the registration score is less than a preset registration score threshold, then registration is successful and SLAM continues; otherwise, a segmented saving mechanism is triggered. This mechanism avoids the overall SLAM drifting problem caused by forcibly registering incorrect data, ensuring relatively high quality point cloud data within each segment.

Description

Technical Field

[0001] This invention belongs to the field of SLAM (Simultaneous Localization and Mapping) technology, specifically relating to a segmented SLAM method based on stable segments in violent motion scenarios. Background Technology

[0002] Point cloud SLAM technology, as one of the key technologies in the fields of robotics and autonomous driving, has made significant progress in recent years. Due to its ability to provide high-precision environmental perception, point cloud SLAM technology has been widely used in indoor and outdoor mapping, autonomous driving of automobiles, drone navigation, and service robots.

[0003] Currently, laser point cloud technology has demonstrated high stability in most application scenarios. In slow-moving or relatively stable fast-moving scenarios, LiDAR can continuously and accurately capture environmental point cloud data, providing reliable input for SLAM algorithms. Thanks to the high precision and anti-interference capabilities of LiDAR, point cloud SLAM systems can operate stably in these scenarios, achieving relatively accurate localization and mapping. For example, in the navigation of AGVs (Automated Guided Vehicles) in logistics warehouses, laser point cloud SLAM technology can ensure that vehicles travel stably on fixed paths, effectively avoiding collisions and positioning loss issues.

[0004] However, point cloud SLAM technology still faces many challenges in some more complex scenarios. For example...

[0005] The complex movement and highly random nature of equipment significantly degrade the quality of point cloud data captured by LiDAR, making it difficult for SLAM algorithms to accurately extract environmental features and perform effective data association and matching. This leads to the accumulation of positioning errors or even complete data drift. This instability severely restricts the widespread application of point cloud SLAM technology in complex industrial scenarios. Summary of the Invention

[0006] To overcome the problems in the prior art, this invention proposes a segmented SLAM method based on stable segments in violent motion scenarios.

[0007] The technical solution of the present invention to solve the above-mentioned technical problems is as follows:

[0008] This invention provides a segmented SLAM method for stable segments in violent motion scenarios, comprising the following steps:

[0009] Acquire point cloud data and IMU data for the current frame; extract the point cloud transformation angle based on the current frame point cloud data; obtain the IMU transformation angle for adjacent frames based on the IMU data;

[0010] The point cloud transformation angle and the IMU rotation angle are compared. If the point cloud transformation angle is less than the IMU rotation angle and the registration score is less than the preset registration score threshold, the registration is successful and SLAM continues. Otherwise, the segment saving mechanism is triggered, the current overall point cloud data is obtained and added to the data list, and a new segment SLAM is performed.

[0011] Further, acquire the current frame point cloud data and IMU data, including:

[0012] Set the registration score threshold F, initialize the segment data list, the last frame point cloud data, and the overall point cloud data; receive a frame of point cloud data and set it as the current frame point cloud data; synchronize the point cloud data and IMU data using timestamps.

[0013] Furthermore, based on the current frame point cloud data, the point cloud transformation angle is extracted, including:

[0014] Based on the current frame point cloud data and the last frame point cloud data, set the original point cloud and the target point cloud;

[0015] Set the initial transformation matrix for registration of the original point cloud and the target point cloud;

[0016] Find the nearest neighbor point cloud for each point cloud in the original point cloud in the target point cloud, and form a set of point pairs;

[0017] A dynamic distance threshold is used to filter the set of points;

[0018] By using the filtered set of point pairs, the transformation matrix of the original point cloud to be registered with the target point cloud is calculated, and the transformation angle of the point cloud is extracted.

[0019] Furthermore, a dynamic distance threshold is used to filter the set of point pairs, including:

[0020] Dynamic distance threshold defined based on point cloud density noise level d The distance between each pair of points is calculated, and pairs of points with a distance greater than a threshold are discarded. The dynamic distance threshold is... d :

[0021] ;

[0022] In the above formula, Indicates a custom coefficient; This represents the distance from the origin to the midpoint of the target point cloud in the point pair; This indicates the angular resolution of the radar.

[0023] Furthermore, using the filtered set of point pairs, the transformation matrix from the original point cloud to the target point cloud is calculated, and the point cloud transformation angle is extracted, including:

[0024] Using the filtered set of point pairs, the Iterative Closest Point (ICP) algorithm is used to calculate the transformation matrix from the original point cloud to the point cloud registered with the target point cloud.

[0025] The point cloud transformation matrix is ​​decomposed into a point cloud rotation matrix and a point cloud translation matrix. The point cloud rotation matrix is ​​represented using quaternions to obtain the point cloud transformation angle.

[0026] ;

[0027] In the above formula, This indicates a change in the angle of the point cloud.

[0028] Furthermore, it also includes: calculating the average distance between the registered point pairs to obtain the registration score.

[0029] Furthermore, the IMU transform angle of adjacent frames is obtained based on the IMU data, including:

[0030] Collect the previous angular velocity data and the current angular velocity data. The angular velocity data consists of angular velocity values ​​on three axes.

[0031] Based on two sets of angular velocity data, calculate the angular change on each axis; based on the angular change on each axis, calculate the quaternion for each axis; multiply the quaternions corresponding to the three axes to obtain the total angular change quaternion;

[0032] Extract the IMU rotation angle from the total angular change quaternion. :

[0033] ;

[0034] in, It is the total angular change around the real part of the quaternion.

[0035] Compared with the prior art, the present invention has the following technical effects:

[0036] This invention proposes a stable segmented SLAM method for scenarios with intense motion. By meticulously comparing the point cloud transformation angle with the IMU rotation angle and combining this with the registration score as a key indicator, it achieves precise segmentation of a small spatial area. In complex motion scenarios, when point cloud data quality degrades due to intense motion and becomes difficult to register accurately, this method can promptly trigger a segmentation and preservation mechanism. This mechanism avoids the overall SLAM drift problem caused by forcibly registering erroneous data, ensuring relatively high point cloud data quality within each segment. This lays a solid foundation for subsequent accurate localization and mapping, greatly improving the stability and reliability of the SLAM system in complex motion environments. Attached Figure Description

[0037] To more clearly illustrate the technical solutions and advantages in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

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

[0039] Figure 2 For traditional adjacent frame registration fractional transform;

[0040] Figure 3 Final result. Detailed Implementation

[0041] To further illustrate the technical means and effects adopted by the present invention to achieve its intended purpose, the specific implementation methods, structures, features, and effects of the technical solutions proposed according to the present invention are described in detail below with reference to the accompanying drawings and preferred embodiments. Specific features, structures, or characteristics in one or more embodiments may be combined in any suitable form. Unless otherwise defined, all technical and scientific terms used in this invention have the same meaning as commonly understood by one of ordinary skill in the art to which this invention pertains.

[0042] like Figure 2 This refers to the registration score transformation between adjacent frames during the SLAM process. In some parts, the transformation significantly exceeds reasonable values, confirming a drift issue in the SLAM within these sections. The SLAM within the segments between these unreasonable values ​​is relatively stable. Of course, relying solely on the registration score will still result in some barely discernible drift instances. Figure 2 Even in areas with normal registration scores, some drift issues still exist. This is because the registration score is essentially calculated based on the overlap points of the two registered point clouds, so some incorrect registrations may still have good registration scores.

[0043] This embodiment combines the motion transformation parameters of adjacent frames obtained by the inertial measurement unit (IMU) with the point cloud registration results for comparative analysis; by comparing the pose transformation differences obtained by the two methods, false registration caused by mismatch of point clouds can be effectively identified.

[0044] In one embodiment of the present invention, reference is made to... Figure 1 and Figure 3 This paper presents a segmented SLAM method based on stable segments in violent motion scenarios, including the following steps:

[0045] Acquire point cloud data and IMU data for the current frame; extract the point cloud transformation angle based on the current frame point cloud data; obtain the IMU transformation angle for adjacent frames based on the IMU data;

[0046] The point cloud transformation angle and the IMU rotation angle are compared. If the point cloud transformation angle is less than the IMU rotation angle and the registration score is less than the preset registration score threshold, the registration is successful and SLAM continues. Otherwise, the segment saving mechanism is triggered, the current overall point cloud data is obtained and added to the data list, and a new segment SLAM is performed.

[0047] The following is a detailed explanation of each of the above steps:

[0048] Step 100: Obtain the current frame point cloud data and IMU data.

[0049] Initialize the segment data list CdataList, set the registration score threshold F; initialize the last frame point cloud data as framelast, and initialize the overall point cloud data Cdata.

[0050] The device receives a frame of point cloud data and sets it as the current frame point cloud data frame Current and IMU data. The device contains a high-precision timer and synchronizes the point cloud data and IMU data through timestamps.

[0051] Perform data packet integrity checks and identification; if the data packet is incomplete, trigger a resampling mechanism.

[0052] Setting a registration score threshold F includes: gradually increasing the threshold from 0 for the test data according to a specific step size, giving the final result for each, and finding the average of the thresholds corresponding to the best registration result for each test data.

[0053] Step 200: Extract the point cloud transformation angle based on the point cloud data.

[0054] As an example, step 200 includes:

[0055] Step 210: Based on the current frame point cloud data and the last frame point cloud data, set the original point cloud and the target point cloud.

[0056] The origin point cloud P is the current frame point cloud data, representing the latest acquired point cloud information; the target point cloud Q is set from the last frame point cloud data and serves as the reference base for registration.

[0057] Step 220: Set the initial transformation matrix and iteration flag for registration of the original point cloud and the target point cloud.

[0058] That is, setting the initial transformation matrix T to match the current frame point cloud data frame Current with the last frame point cloud data frame Last.k = T0; where T0 is the identity matrix I, which means that in the initial state, no rotation or translation transformation has been performed on the point cloud, and it is a neutral starting state.

[0059] Iteration flag k = 0, used to record the number of iterations in the registration process, so as to track the progress of registration and perform iteration control.

[0060] Step 230: Find the nearest neighbor point cloud of each point cloud in the original point cloud in the target point cloud and form a set of point pairs.

[0061] In the target point cloud Q (i.e., frame Last), find the nearest neighbor point cloud for each point cloud in the original point cloud P (i.e., frame Current). This step aims to find corresponding point pairs in the two frame point clouds, providing the basic data for subsequent transformation matrix calculations. In this way, the correspondence between the two frame point clouds can be established, thereby determining how to transform the current frame point cloud to a position aligned with the previous frame point cloud.

[0062] Step 240: Filter the point pair set using a dynamic distance threshold.

[0063] Considering the issue of point cloud density variation in large-scale radar point clouds, a dynamic distance threshold is defined based on the point cloud density noise level. d The distance between each pair of points is calculated, and pairs of points with a distance greater than a threshold are discarded. The dynamic distance threshold is... d :

[0064] ;

[0065] In the above formula, Indicates a custom coefficient; This represents the distance from the origin to the midpoint of the target point cloud in the point pair; This indicates the angular resolution of the radar.

[0066] The advantage of using a dynamic distance threshold is that it adaptively adjusts the filtering criteria based on the local density of the point cloud. In areas with high point cloud density, the dynamic threshold increases accordingly, allowing more reasonable point pairs to be retained; while in areas with low point cloud density, the dynamic threshold decreases, avoiding the incorrect rejection of reasonable point pairs that are far apart due to sparse density. This allows the filtering parameters to match the local point cloud density, effectively avoiding filtering failures caused by changes in point cloud density, and improving the robustness and accuracy of point cloud registration.

[0067] Step 250: Calculate the transformation matrix from the original point cloud to the target point cloud using the filtered set of point pairs, and extract the point cloud transformation angle.

[0068] As an example, this step includes:

[0069] Step 251: Using the filtered set of point pairs, calculate the transformation matrix T from the original point cloud to the point cloud registered with the target point cloud using the Iterative Closest Point (ICP) algorithm. D .

[0070] As an example, this step includes:

[0071] Step 2511: Preset the set of filtered point pairs as follows ,in, ; ;

[0072] Step 2512: For each point cloud Apply the current transformation matrix T k ， Obtain the transformed point cloud ;

[0073] Step 2513: For each transformed point, find its nearest point in the target point cloud. .

[0074] Step 2514: Calculate the transformation update point cloud transformation matrix and point cloud translation vector to update the minimum point cloud transformation matrix.

[0075] Based on the filtered set of point pairs Calculate the transformation matrix ΔT that minimizes the sum of squared distances between pairs of points. k ;

[0076] Let the error vector between point pairs be... Then the objective function is:

[0077] ;

[0078] Let the centroids of the point pair be:

[0079] ;

[0080] Constructing a matrix ;

[0081] right H Perform SVD decomposition. Then the point cloud rotation matrix ;

[0082] Point cloud translation vector .

[0083] This yields the transformation matrix ΔT that minimizes the sum of squared distances between updated point pairs. k Its rotating part is The translation part is .

[0084] Step 2515: Update the point cloud transformation matrix based on the point cloud transformation matrix that minimizes the sum of squared distances between point pairs.

[0085] ;

[0086] Calculate the average distance change Δ between point pairs before and after this iteration. d If Δd < convergence threshold or k If the maximum number of iterations Kmax is reached, the iteration stops, and the final transformation matrix is ​​output. T D =T k+1 ;otherwise, k=k +1, return to step 2512 and continue iterating.

[0087] Step 252: Calculate the average distance or mean square error between the registered point pairs to obtain the registration score.

[0088] Registration score f This score is used to measure the quality of registration between two point cloud frames. It is typically obtained by calculating metrics such as the average distance and mean square error between the registered point pairs. The lower the registration score, the better the registration effect between the two point cloud frames and the higher the degree of matching between the point pairs.

[0089] Average distance: , among which, T D It is the transformation matrix obtained through calculation.

[0090] Step 253: Transform the point cloud matrix T D The analysis was performed to extract the point cloud transformation angle.

[0091] Point cloud transformation matrix T D It can be decomposed into a point cloud rotation matrix R D and point cloud translation matrix P D :

[0092] ;

[0093] Rotation matrix R of point cloud D Using quaternions, we obtain the point cloud transformation angle:

[0094] Define the rotation matrix of the point cloud:

[0095] ;

[0096] Based on the point cloud rotation matrix R D Calculate quaternions ,in:

[0097] ;

[0098] The rotation angle of the point cloud can be extracted from the quaternion, and then the transformation angle of the point cloud can be obtained:

[0099] ;

[0100] In the above formula, This indicates a change in the angle of the point cloud.

[0101] Step 300: Calculate the IMU transform angle of adjacent frames based on the IMU data.

[0102] In IMU data processing, it is necessary to calculate the expected transformation angle between adjacent frames based on the angular velocity data collected in two consecutive acquisitions.

[0103] As an example, this step 300 includes:

[0104] Step 310: Collect the previous angular velocity data and the current angular velocity data.

[0105] The previously collected angular velocity data is known to be: ,in, , , These represent the angular velocity values ​​on the three axes (roll, pitch, and yaw axes).

[0106] The angular velocity data collected this time are And the time interval between the two data acquisitions is Δt.

[0107] Step 320: Calculate the angle change on each axis.

[0108] The change in angle can be approximated as the product of angular velocity and time interval. For each axis, the change in angle is calculated as follows:

[0109] ;

[0110] In the above formula, express x Axis angle change; express y Axis angle change; express z Changes in axis angle.

[0111] Step 330: Calculate the quaternion for each axis based on the angle change on each axis.

[0112] Rotation is represented using quaternions. The angular change on each axis is represented as a quaternion, and these quaternions are multiplied together to obtain the total angular change. The rotation represented by quaternions is:

[0113] ;

[0114] In the above formula, Indicates circling x Quaternions that rotate around an axis; A quaternion representing rotation about the y-axis; This represents a quaternion that rotates about the z-axis.

[0115] Step 340: Multiply the quaternions corresponding to the three axes to obtain the total angle change quaternion.

[0116] ;

[0117] In the above formula, This represents the total angular change around a quaternion.

[0118] Step 350: Extract the IMU rotation angle from the total angle change quaternion.

[0119] like ;

[0120] IMU rotation angle It can be calculated using the following formula:

[0121] ;

[0122] in, It is the total angular change around the real part of the quaternion.

[0123] Through the above steps, the expected transition angle eur_imu between adjacent frames can be calculated based on the IMU data, providing a basis for subsequent operations such as condition judgment in the SLAM process.

[0124] Step 400: Compare the point cloud transformation angle and the IMU rotation angle. If the point cloud transformation angle is less than the IMU rotation angle and the registration score is less than the preset registration score threshold, the registration is considered successful, the global map is updated, and SLAM continues. Otherwise, the registration is abnormal, the current overall point cloud data Cdata is obtained and added to the data list CdataList, and the next SLAM segment is performed. After the SLAM segment is completed, the point cloud data in the data list CdataList is traversed and registration is performed to obtain the final complete point cloud data. Figure 3 This is the SLAM stitching of point cloud data collected by radar equipment placed on a mining vehicle inside the ship's hold, using this method.

[0125] The above embodiments are only used to illustrate the technical solutions of the present invention, and are not intended to limit it. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention, and should all be included within the protection scope of the present invention.

Claims

1. A stable segment-based SLAM method in a vigorous motion scene, characterized in that, The method comprises the following steps: Obtain current frame point cloud data and IMU data; Based on the current frame point cloud data, extract the point cloud transformation angle, including: according to the current frame point cloud data and the last frame point cloud data, set the original point cloud and the target point cloud; Set the initial transformation matrix of the registration of the original point cloud and the target point cloud; Find the nearest point cloud of each point cloud in the original point cloud in the target point cloud to form a point pair set; The point pair set is screened by using a dynamic distance threshold; Through the screened point pair set, calculate the transformation matrix of the original point cloud to the registration of the target point cloud, and extract the point cloud transformation angle; According to the IMU data, obtain the adjacent frame IMU rotation angle, including: collect the last angular velocity data and this time's angular velocity data, and the angular velocity data is the angular velocity value on three axes; Based on the twice angular velocity data, calculate the angle change on each axis; based on the angle change on each axis, calculate the quaternion of each axis; multiply the corresponding quaternions of the three axes to obtain the total angle change quaternion; extract the IMU rotation angle from the total angle change quaternion : ； wherein is the total angular change about the quaternion real part; Compare the point cloud transformation angle and the IMU rotation angle, if the point cloud transformation angle is less than the IMU rotation angle, and the registration score is less than the preset registration score threshold, the registration is successful, and the SLAM is continued; Otherwise, trigger the segmented saving mechanism, obtain the current overall point cloud data and add it to the data list, and perform new segmented SLAM.

2. The SLAM method based on stable segment segmentation in a violent motion scene according to claim 1, wherein, Before obtaining the current frame point cloud data and the IMU data, comprising: Set the registration score threshold F, initialize the segment data list, the last frame point cloud data, and the overall point cloud data; Receive a frame of point cloud data and set it as the current frame point cloud data; Synchronize the point cloud data and the IMU data through the time stamp.

3. The SLAM method based on stable segment segmentation in a violent motion scene according to claim 1, wherein, Screen the point pair set by using a dynamic distance threshold, including: Defining a dynamic distance threshold based on point cloud density noise level d , the distance of each point pair is calculated, and the point pairs greater than the threshold are removed, wherein the dynamic distance threshold d : ； In the above formula, represents a custom coefficient; represents the distance of the point in the target point cloud from the origin; represents the angular resolution of the radar.

4. The SLAM method based on stable segment segmentation in a violent motion scene according to claim 1, wherein, Through the screened point pair set, calculate the transformation matrix of the original point cloud to the registration of the target point cloud, and extract the point cloud transformation angle, including: Through the screened point pair set, use the iterative closest point algorithm ICP to calculate the point cloud transformation matrix of the original point cloud to the registration of the target point cloud; The point cloud transformation matrix is decomposed into a point cloud rotation matrix and a point cloud translation matrix, the point cloud rotation matrix is represented by using a quaternion, and the point cloud transformation angle is obtained: ； In the above formula, represents the point cloud transformation angle.

5. The method according to claim 4, wherein, Also including: Calculate the average distance between the registered point pairs to obtain the registration score.

Images