Three-dimensional point cloud reconstruction method using single-line laser radar and inertial measurement unit

Abstract

The invention relates to a three-dimensional point cloud reconstruction method and device using a single-line laser radar and an inertial measurement unit (IMU). According to the method, a stepping motor drives a single-line laser radar to periodically pitch and swing, and the scanning dimension of the single-line laser radar is expanded to a three-dimensional space; the method comprises the following steps: synchronously acquiring point cloud data and IMU (Inertial Measurement Unit) data of a laser radar, performing intra-frame motion distortion compensation on each frame of point cloud data by using the IMU data, and eliminating distortion generated by carrier motion; projecting the compensated three-dimensional point cloud data to a two-dimensional aerial view (BEV) plane to generate a two-dimensional point set, optimizing and solving inter-frame two-dimensional rigid body transformation by adopting an iterative closest point (ICP) algorithm based on the BEV point set of adjacent frames, then lifting the inter-frame two-dimensional rigid body transformation into three-dimensional transformation, and updating the global three-dimensional pose of a carrier; according to the method, low-cost and high-precision three-dimensional point cloud reconstruction is realized, the hardware cost is reduced, the operation efficiency is improved, and the method is suitable for various devices such as a mobile robot, a sweeper and a simple scanner.

Description

Technical Field

[0001] The present invention belongs to the technical field of mobile robot positioning and three-dimensional mapping, and specifically relates to the technical field of fusion of single-line laser radar and inertial measurement unit (IMU). Background Art

[0002] With the rapid development of mobile robotics, autonomous driving, and 3D scanning technologies, accurate 3D perception of the environment has become increasingly important. Single-line LiDAR (LiDAR) is widely used in mobile robotics and low-speed autonomous driving platforms due to its low cost and low power consumption. However, single-line LiDAR only captures two-dimensional surface data and lacks depth information, which significantly limits its effectiveness in 3D perception tasks.

[0003] To overcome this limitation, a common practice is to mount a single-line LiDAR on a mechanical swinging structure, allowing it to scan in the up and down directions while rotating horizontally, thereby gradually generating sparse three-dimensional point cloud data. However, this method faces many challenges in practical applications. In particular, during the movement of the carrier, due to the long scanning time and the relative motion between the LiDAR and the swinging mechanism, the generated three-dimensional point cloud data exhibits significant motion distortion, which seriously affects the reconstruction accuracy. To address the motion distortion problem, some studies have attempted to introduce inertial measurement unit (IMU) data to correct the point cloud. Despite this, existing compensation methods still have deficiencies in accuracy or real-time performance, making it difficult to meet the requirements of high-precision three-dimensional reconstruction in complex environments.

[0004] Therefore, a new technical solution is urgently needed to solve the above problems. Summary of the Invention

[0005] In order to solve the above technical problems, the present invention adopts the following technical solutions: A method for reconstructing a three-dimensional point cloud using a single-line laser radar and an inertial measurement unit comprises the following steps: S1, uses a stepper motor to drive the single-line laser radar to perform periodic pitch and swing, expanding its scanning dimension from a two-dimensional plane to a three-dimensional conical space; S2 collects point cloud data from a single-line lidar and preprocesses the point cloud data to obtain the three-dimensional coordinates of each point in the point cloud data relative to the lidar coordinate system, generates ordered point cloud data for each frame, and synchronously collects IMU data and caches it in an ordered queue. S3, using IMU data to perform intra-frame motion distortion compensation on each frame of point cloud data to obtain three-dimensional point cloud data with motion distortion eliminated; S4, projects the 3D point cloud data with motion distortion eliminated onto the 2D bird's-eye view (BEV) plane to generate a 2D point set; S5, based on the 2D point sets of adjacent frames, optimally aligns the BEV point set of the current frame to the BEV point set of the previous frame, and uses the iterative closest point (ICP) algorithm to optimize and obtain the accurate 2D inter-frame pose transformation, which is then promoted to a 3D transformation and the global 3D pose of the carrier is updated; S6, through the global optimization algorithm, fuses all the 3D point cloud data that have been dedistorted and transformed to reconstruct a globally consistent 3D map.

[0006] Furthermore, step S3 includes: S31, aligning the timestamps of the point cloud data and the IMU data to ensure that each point cloud data has a corresponding IMU state and calculating the acquisition timestamp of each point cloud data; S32, finding two IMU measurement values ​​adjacent to the acquisition timestamp of each point cloud data in the IMU data stream, interpolating the angular velocity and linear acceleration of the IMU measurement values ​​to obtain an IMU measurement value sequence that is precisely time-aligned with each point cloud data; S33, calculating the carrier motion increment from the start time of the frame to the time of each laser point in the point cloud data based on the IMU data pre-integration; S34, through the pre-calibrated external parameter transformation matrix, the carrier motion increment at each laser point moment is converted into a motion transformation in the lidar coordinate system, and each laser point in the point cloud data is reprojected to the coordinate system at the start time of the frame to obtain three-dimensional point cloud data with motion distortion eliminated.

[0007] Furthermore, the pre-processing of the point cloud data in step S2 includes coordinate conversion, data screening, timestamp labeling and formatting to generate each frame of point cloud data ordered by timestamp.

[0008] Furthermore, in step S32, the interpolation calculation formula for the angular velocity and linear acceleration measurement values ​​of the IMU is:

[0009] in, is the acquisition timestamp of the current laser point, and The timestamps of the two IMU measurements adjacent to the current laser point acquisition timestamp, is the interpolation scale factor, and are the measured angular velocity and linear acceleration, and are two adjacent time points in the IMU data record.

[0010] Furthermore, the carrier motion increment integral calculation formula in step S33 is:

[0011] in, is the attitude increment, is the velocity increment, is the position increment, is the exponential mapping from Lie algebra to Lie group, For IMU at time The measured angular velocity, is the gyroscope bias, is the time interval, For IMU at time The measured acceleration, is the accelerometer bias, is the transpose of the attitude rotation matrix at the start of the frame, is the gravitational acceleration in the world coordinate system.

[0012] Furthermore, the formula for reprojecting each laser point in the point cloud data to the coordinate system at the start time of the frame in step S34 is:

[0013] in, and are the rotational and translational components of the LiDAR motion, is the coordinate of the laser point in the current coordinate system.

[0014] Furthermore, the BEV projection and optimization cost function formula in step S5 is:

[0015] in, are the coordinate components of the point on the (x, y) plane in the three-dimensional coordinate system, T represents the transpose operation of the matrix or vector, is a two-dimensional rotation matrix, is a two-dimensional translation vector, is a point in the BEV point set of the current frame, It is the corresponding point closest to the transformed point in the target point set.

[0016] Furthermore, the global three-dimensional pose change formula in step S5 is:

[0017] in, is a three-dimensional inter-frame transform, is the global pose of the previous frame.

[0018] Compared with the prior art, the present invention has the following beneficial effects: The present invention uses a stepper motor to drive a single-line laser radar to perform periodic pitch and roll swings, combines IMU data pre-integration to compensate for motion distortion, and innovatively uses bird's-eye view projection to reduce the dimensionality of the three-dimensional registration to a two-dimensional plane, thereby achieving low-cost, high-precision three-dimensional reconstruction, reducing hardware costs, and improving point cloud accuracy and mapping efficiency. It has broad application prospects and market potential. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] Figure 1 This is a system framework diagram of the present invention; Figure 2 This is the point cloud BEV projection effect diagram of the method of the present invention. DETAILED DESCRIPTION

[0020] The present invention will be further described in detail below in conjunction with specific embodiments to make the purpose, technical solutions and advantages of the present invention clearer. It should be noted that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention.

[0021] like Figure 1 and 2 As shown, the present invention provides a three-dimensional point cloud reconstruction method using a single-line laser radar and an inertial measurement unit, comprising the following steps: Swing mechanism parameter settings: Single-line lidar While completing a 360° rotation scan in the horizontal direction, the stepper motor drives it to swing periodically around the pitch axis (swing angle range: -10° to +30°, with the horizontal plane set to 0°). The swing period is coordinated with the lidar rotation period to form a three-dimensional conical scanning space.

[0022] The period of a laser radar scanning frame is the inverse of its frequency:

[0023] Time of a single swing It must be equal to the time it takes the LiDAR to scan one frame, so:

[0024] The total angle of a single swing is the difference between the maximum angle and the minimum angle:

[0025] Average angular velocity The definition of is the angle change divided by the time change, according to the above derivation:

[0026] Based on the above, the angular velocity of the single-line laser radar driven by the stepper motor to periodically swing around the pitch axis is precisely set to .

[0027] Data collection and preprocessing stage: A stepper motor drives the single-line LiDAR to periodically pitch and oscillate, expanding its scanning dimension from a two-dimensional plane to a three-dimensional conical space. Point cloud data from the single-line LiDAR and IMU data are simultaneously collected and preprocessed to obtain the three-dimensional coordinates of each point in the point cloud relative to the LiDAR coordinate system, generating ordered point cloud data for each frame.

[0028] The collected point cloud data is preprocessed, including coordinate conversion, data screening, timestamp labeling and formatting, to generate each frame of point cloud data ordered by timestamp. Specifically, the two-dimensional point cloud data collected by the single-line lidar is converted into coordinates in a three-dimensional coordinate system; obviously erroneous or invalid data points are removed, such as those that are beyond the measurement range, have a low signal-to-noise ratio, or are greatly affected by environmental interference; an accurate timestamp is added to each laser point to record the specific moment when the point was collected; the point cloud data is organized into a frame structure arranged in chronological order to ensure that each frame of data contains all points within the complete scanning cycle and has a unified format.

[0029] Distortion compensation stage: Align the timestamps of the point cloud data with the IMU data to ensure that each point cloud data has a corresponding IMU state. Calculate the acquisition timestamp of each point cloud data, find the two IMU measurements adjacent to the acquisition timestamp of each point cloud data in the IMU data stream, interpolate the angular velocity and linear acceleration of the IMU measurements, and obtain an IMU measurement sequence that is precisely time-aligned with each point cloud data. The specific steps and formulas are as follows: Assuming that the scanning angular velocity of the laser radar is constant within one frame, the The acquisition timestamps of each laser point can be calculated according to its proportion in the scanning sequence:

[0030] in, is the start time of a frame scan, The number of laser points contained in this frame.

[0031] For each calculated , find the two adjacent IMU measurement values ​​in the IMU data stream (time is and ). Estimated by linear interpolation The IMU status at the moment.

[0032] Calculate interpolation scale factor :

[0033] Interpolate the angular velocity and linear acceleration measurements from the IMU:

[0034] After interpolation, we get the value corresponding to each laser point Precisely time-aligned IMU measurement sequences .

[0035] Based on the pre-integration of IMU data, the carrier motion increment from the start of the frame to the moment of each laser point in the point cloud data is calculated. The specific steps and formula are as follows: The continuous relative motion of the IMU itself within a frame scan cycle is calculated by IMU integration. This integration process is completely in the IMU body coordinate system. For each small time interval The IMU readings are discretely integrated to iteratively calculate the increments of attitude, velocity, and position. Before integration, the effects of bias and gravity must be deducted from the acceleration readings. The gyroscope bias is recorded as , the accelerometer bias is recorded as .

[0036] Calculating attitude increments , here is the exponential mapping from Lie algebra to Lie group, used to convert rotation vectors into rotation matrices:

[0037] Calculating velocity increment To correctly deduct gravity, the world coordinate system Gravity vector under Convert to the IMU coordinate system at the start of the frame This requires the help of the posture at the start of the frame (Rotation matrix from world system to IMU system) to complete:

[0038] Calculating position increments :

[0039] After calculating the increment of IMU, we can get the value of IMU from the starting time. At each sampling moment The relative motion transformation of .

[0040] Finally, the point cloud motion distortion is corrected using the incremental integration of the IMU. , but the correction of point cloud requires the motion of LiDAR The relationship between the two can be obtained by pre-calibrating the external parameter transformation matrix (From the IMU coordinate system To LiDAR coordinate system The transformation of ) is derived:

[0041] Expanding the above formula, we can obtain the rotation and translation components of LiDAR motion:

[0042] The significance of this formula is to calculate and compensate for the additional displacement of the point cloud caused by the displacement and rotation of the lidar carrier.

[0043] Using the precise motion of the LiDAR itself obtained in the previous step , each in Coordinate system at time Points measured in , reprojected to the coordinate system of the starting time middle:

[0044] For all points in a frame After executing the transformation of this formula, a set of laser point clouds with consistent internal geometric relationships and completely eliminated motion distortion can be obtained.

[0045] Data projection and inter-frame matching stage: This component addresses the problem of vehicle pose estimation and long-term drift between LiDAR scans. Long-term IMU integration inevitably generates cumulative errors, leading to accumulated map errors. By projecting the dedistorted laser point cloud frame into a bird's-eye view (BEV) and matching it with geometric features in the environment, we can obtain highly accurate relative motion between frames.

[0046] Project the motion-distorted 3D point cloud data onto the 2D bird's-eye view (BEV) plane to generate a 2D point set. The specific steps and formulas are as follows: First, input the undistorted 3D point cloud after the previous dedistortion: . In which, each point All have been unified to the start time of the kth frame The lidar coordinate system The point cloud is geometrically rigid and free of motion distortion. Input the undistorted 3D point cloud of the previous frame (frame k-1): and the global pose of the previous frame .in, Is a 4x4 homogeneous transformation matrix that describes the starting coordinate system of the previous frame In the world coordinate system Position and posture in.

[0047] Secondly, in order to reduce the computational complexity of matching and improve robustness, we reduce the dimensionality of the 3D point cloud and project it onto the 2D bird’s-eye view plane. Every three-dimensional point in , we ignore its Axis coordinates, get its two-dimensional coordinates on the BEV plane: . Thus, we get a two-dimensional point set, which is recorded as: . For the previous frame point cloud Perform the same operation and get: .

[0048] Based on the 2D point sets of adjacent frames, the BEV point set of the current frame is optimally aligned to the BEV point set of the previous frame. The iterative closest point (ICP) algorithm is used for optimization to obtain an accurate 2D inter-frame pose transformation. The 2D transformation is then promoted to a 3D transformation, and the global 3D pose of the carrier is updated. The specific steps and formulas are as follows: The core task of optimization is to find an optimal two-dimensional rigid body transformation , which can transform the BEV point set of the current frame Best aligned to the BEV point set of the previous frame Above. The relative pose transformation to be determined By a two-dimensional rotation matrix and a 2D translation vector Composition. Construct the cost function:

[0049] in, In the target point set The transformed point The closest corresponding point is optimized using the classic Iterative Closest Point (ICP) algorithm, whose goal is to minimize the sum of squared distances between the transformed source point cloud and the corresponding points in the target point cloud.

[0050] Due to the corresponding point relationship is unknown, and this problem requires an iterative solution. Use the inter-frame pose estimate obtained by IMU pre-integration as the initial transformation, specifically, using arrive The IMU integration result is projected onto a two-dimensional plane as the initial value of the ICP. This can greatly accelerate convergence and avoid falling into local optimality.

[0051] For the source point set Every point in , using the transformation obtained in the previous iteration, in the target point set Find the nearest neighbor point , thereby establishing a set of temporary corresponding point pairs.

[0052] After fixing the relationship between corresponding points, this minimization problem has an analytical solution based on singular value decomposition (SVD).

[0053] a. Calculate the centroid of two sets of corresponding points and .

[0054] b. Calculate the decentralized covariance matrix .

[0055] c. Yes Perform SVD decomposition: .

[0056] d. Calculate the optimal transformation for the current iteration: .

[0057] in, is the decentralized covariance matrix for SVD decomposition, They are the three matrices obtained after H is decomposed by SVD.

[0058] If the amount of change between two iterations or the amount of change in the cost function is less than the preset threshold, the loop terminates. This is the precise two-dimensional inter-frame pose transformation we seek The calculated two-dimensional transformation Lifting to a 3D transform It is usually assumed that the inter-frame motion occurs mainly in the XY plane, that is, the translation is , the rotation is around the Z axis.

[0059] in, .

[0060] Use this accurate 3D inter-frame transformation to update the global 3D pose of the carrier. The specific formula is as follows:

[0061] in, .

[0062] Global map construction phase: A global optimization algorithm is used to fuse all the distorted and pose-transformed 3D point cloud data to reconstruct a globally consistent 3D map.

[0063] Through the above steps, the present invention realizes low-cost, high-precision three-dimensional point cloud reconstruction, reduces hardware costs, improves point cloud accuracy and mapping efficiency, and is suitable for a variety of devices such as mobile robots, sweepers, and simple scanners, with broad application prospects and market potential.

[0064] The above description is only a preferred embodiment of the present invention. It should be pointed out that for ordinary technicians in this technical field, several improvements can be made without departing from the principles of the present invention. These improvements should also be regarded as the scope of protection of the present invention.

Claims

1. A three-dimensional point cloud reconstruction method using a single-line laser radar and an inertial measurement unit, characterized in that: The following steps are involved: S1, uses a stepper motor to drive the single-line laser radar to perform periodic pitch and swing, expanding its scanning dimension from a two-dimensional plane to a three-dimensional conical space; S2, synchronously collects point cloud data and IMU data from a single-line lidar, and preprocesses the point cloud data to obtain the three-dimensional coordinates of each point in the point cloud data relative to the lidar coordinate system, generating ordered point cloud data for each frame; S3, using IMU data to perform intra-frame motion distortion compensation on each frame of point cloud data to obtain three-dimensional point cloud data with motion distortion eliminated; S4, projects the 3D point cloud data with motion distortion eliminated onto the 2D bird's-eye view (BEV) plane to generate a 2D point set; S5, based on the 2D point sets of adjacent frames, uses the Iterative Closest Point (ICP) algorithm to optimize and obtain the accurate 2D inter-frame pose transformation, promotes the 2D transformation to a 3D transformation, and updates the global 3D pose of the carrier; S6, through the global optimization algorithm, fuses all the 3D point cloud data that have been dedistorted and transformed to reconstruct a globally consistent 3D map.

2. The method for reconstructing a three-dimensional point cloud using a single-line laser radar and an inertial measurement unit according to claim 1, characterized in that: The step S3 comprises: S31, aligning the timestamps of the point cloud data and the IMU data to ensure that each point cloud data has a corresponding IMU state and calculating the acquisition timestamp of each point cloud data; S32, finding two IMU measurement values ​​adjacent to the acquisition timestamp of each point cloud data in the IMU data stream, interpolating the angular velocity and linear acceleration of the IMU measurement values ​​to obtain an IMU measurement value sequence that is precisely time-aligned with each point cloud data; S33, calculating the carrier motion increment from the start time of the frame to the time of each laser point in the point cloud data based on the IMU data pre-integration; S34, through the pre-calibrated external parameter transformation matrix, the carrier motion increment at each laser point moment is converted into a motion transformation in the lidar coordinate system, and each laser point in the point cloud data is reprojected to the coordinate system at the start time of the frame to obtain three-dimensional point cloud data with motion distortion eliminated.

3. The method for reconstructing a three-dimensional point cloud using a single-line laser radar and an inertial measurement unit according to claim 1, wherein: The pre-processing of the point cloud data in step S2 includes coordinate conversion, data screening, timestamp labeling and formatting to generate each frame of point cloud data ordered by timestamp.

4. The method for reconstructing a three-dimensional point cloud using a single-line laser radar and an inertial measurement unit according to claim 2, wherein: The interpolation calculation formula for the angular velocity and linear acceleration measurement values ​​of the IMU in step S32 is: in, is the acquisition timestamp of the current laser point, and The timestamps of the two IMU measurements adjacent to the current laser point acquisition timestamp, is the interpolation scale factor, and are the measured angular velocity and linear acceleration, and are two adjacent time points in the IMU data record.

5. The method for reconstructing three-dimensional point clouds using a single-line laser radar and an inertial measurement unit according to claim 2, wherein: The carrier motion increment integral calculation formula in step S33 is: in, is the attitude increment, is the speed increment, is the position increment, is the exponential mapping from Lie algebra to Lie group, For IMU at time The measured angular velocity, is the gyroscope bias, is the time interval, For IMU at time The measured acceleration, is the accelerometer bias, is the transpose of the attitude rotation matrix at the start of the frame, is the gravitational acceleration in the world coordinate system.

6. The method for reconstructing three-dimensional point clouds using a single-line laser radar and an inertial measurement unit according to claim 2, wherein: The formula for reprojecting each laser point in the point cloud data to the coordinate system at the start time of the frame in step S34 is: in, and are the rotational and translational components of the LiDAR motion, is the coordinate of the laser point in the current coordinate system.

7. The method for reconstructing a three-dimensional point cloud using a single-line laser radar and an inertial measurement unit according to claim 1, characterized in that: The BEV projection and optimization cost function formula in step S5 is: in, are the coordinate components of the point on the (x, y) plane in the three-dimensional coordinate system, T represents the transpose operation of the matrix or vector, is a two-dimensional rotation matrix, is a two-dimensional translation vector, is a point in the BEV point set of the current frame, It is the corresponding point closest to the transformed point in the target point set.

8. The method for reconstructing three-dimensional point clouds using a single-line laser radar and an inertial measurement unit according to claim 1, wherein: The global three-dimensional pose change formula in step S5 is: in, is a three-dimensional inter-frame transform, is the global pose of the previous frame.

Images