A laser radar point cloud motion compensation method, control system, medium and product

Abstract

The application discloses a laser radar point cloud motion compensation method, a control system, a medium and a product, and relates to the field of inertial navigation. According to the application, the control system calculates a predicted attitude quaternion and a predicted gravity vector of a vehicle at a second time, compensates the attitude of the vehicle by using the predicted gravity vector, the inverse of the predicted attitude quaternion and a z-axis vector of a world coordinate system, obtains a second attitude quaternion of the vehicle at the second time, and determines the attitude change of the vehicle from a first time to the second time. The control system calculates the linear velocity of the vehicle, determines the displacement change of the vehicle from the first time to the second time, and performs accurate motion compensation on the laser radar point cloud based on the displacement change and the attitude change. Compared with a traditional linear interpolation method, the method considers the gravity constraint, can more accurately describe the motion trajectory of the vehicle in a complex scene such as high-speed driving or sharp turning, and effectively reduces the accumulation of attitude estimation errors and improves the point cloud registration accuracy.

Description

Technical Field

[0001] This application relates to the field of inertial navigation, and more particularly to a method, control system, medium, and product for motion compensation of lidar point clouds. Background Technology

[0002] With the rapid development of autonomous driving technology, LiDAR, as an important sensor for environmental perception, is widely used in autonomous driving systems. LiDAR acquires three-dimensional information about the surrounding environment by emitting laser beams and receiving reflected signals, forming point cloud data. In practical applications, the point cloud data collected by LiDAR will be distorted due to the movement of the vehicle, which poses challenges to subsequent point cloud processing and environmental perception.

[0003] Existing lidar point cloud motion compensation technologies mainly employ a time-based compensation method. This involves first acquiring the start and end times of the lidar scan, then calculating the acquisition time for each point based on the lidar's scanning frequency, and finally using vehicle motion information to interpolate and compensate the point cloud data. During the compensation process, linear interpolation is used to estimate the vehicle's pose information at different times, and this pose information is then applied to the point cloud correction.

[0004] However, in complex scenarios such as high-speed driving or sharp turns, the vehicle's motion state will change drastically, and the pose estimation error generated by linear interpolation will gradually accumulate over time, affecting the accuracy of point cloud registration. Summary of the Invention

[0005] This application provides a method, control system, medium, and product for motion compensation of lidar point clouds, which can improve the accuracy of lidar point cloud registration.

[0006] In a first aspect, this application provides a laser radar point cloud motion compensation method, applied to a control system. The method includes: acquiring the first IMU angular velocity, first IMU acceleration, first gravity vector, and first attitude quaternion of a vehicle at a first moment, and the second IMU angular velocity and second IMU acceleration of the vehicle at a second moment, wherein the first moment is before the second moment and the time difference between the first moment and the second moment is less than a preset time difference threshold; multiplying the time difference by the first IMU angular velocity to obtain the vehicle's rotation vector; constructing a rotation quaternion based on the rotation vector; determining the predicted attitude quaternion of the vehicle at the second moment using the rotation quaternion and the first attitude quaternion; calculating an exponential smoothing coefficient based on the time difference; and multiplying the exponential smoothing coefficient and the second IMU angular velocity by the first IMU angular velocity. Substituting the MU acceleration and the first gravity vector into the gravity update formula, the predicted gravity vector of the vehicle at the second moment is obtained. Based on the predicted gravity vector, the inverse of the predicted attitude quaternion, and the z-axis vector of the world coordinate system, the alignment rotation quaternion is determined. Through the alignment rotation quaternion and the predicted attitude quaternion, the second attitude quaternion of the vehicle at the second moment is determined. The linear velocity of the vehicle between the first and second moments is calculated. Multiplying the vehicle linear velocity by the time difference, the displacement change between the second and first moments is obtained. Based on the second attitude quaternion and the first attitude quaternion, the attitude change between the second and first moments is calculated. Based on the displacement change and attitude change, each point in the lidar point cloud is compensated to obtain the corrected lidar point cloud.

[0007] By employing the above technical solution, the control system calculates the predicted attitude quaternion and predicted gravity vector of the vehicle at the second moment. It then compensates for the vehicle's attitude using the predicted gravity vector, the inverse of the predicted attitude quaternion, and the z-axis vector of the world coordinate system, obtaining the second attitude quaternion of the vehicle at the second moment, thereby determining the vehicle's attitude change from the first moment to the second moment. The control system calculates the vehicle's linear velocity to determine the vehicle's displacement change from the first moment to the second moment. Based on the displacement and attitude changes, the control system performs precise motion compensation on the LiDAR point cloud. Compared with traditional linear interpolation methods, this method considers gravity constraints and can more accurately describe the vehicle's trajectory in complex scenarios such as high-speed driving or sharp turns, thereby effectively reducing the accumulation of attitude estimation errors and improving point cloud registration accuracy.

[0008] In conjunction with some embodiments of the first aspect, in some embodiments, an exponential smoothing coefficient is calculated based on the time difference. The exponential smoothing coefficient, the second IMU acceleration, and the first gravity vector are substituted into the gravity update formula to obtain the predicted gravity vector of the vehicle at the second moment. Specifically, this includes: substituting the time difference into the exponential smoothing coefficient calculation method to obtain the exponential smoothing coefficient; the exponential smoothing coefficient calculation method is: α = 1 - exp(-Δt / τ); where α represents the exponential smoothing coefficient, Δt represents the time difference, and τ represents the gravity time constant; the gravity update formula is: g1 = (1-α) × g0 + α × a1; where g1 represents the predicted gravity vector, α represents the exponential smoothing coefficient, g0 represents the first gravity vector, and a1 represents the second IMU acceleration.

[0009] By adopting the above technical solution, the control system introduces an exponential smoothing coefficient and a gravity update formula to establish an adaptive gravity vector update mechanism. The calculation of the exponential smoothing coefficient takes into account the influence of time difference and can automatically adjust the smoothing degree according to the sampling interval. The gravity update formula maintains the stability of the gravity vector and can quickly respond to changes in vehicle attitude by weighted fusion of historical gravity vectors and current IMU acceleration. This adaptive gravity vector update mechanism can improve the tracking ability of vehicle motion state changes while ensuring system stability, thereby obtaining more accurate attitude estimation results.

[0010] In conjunction with some embodiments of the first aspect, in some embodiments, calculating the vehicle linear velocity between the first time moment and the second time moment specifically includes: obtaining the first transformation matrix of the odometer at the first time moment and the second transformation matrix of the odometer at the second time moment; inverting the second transformation matrix of the odometer and multiplying it by the first transformation matrix of the odometer to obtain the relative transformation matrix between the first time moment and the second time moment; extracting the displacement vector in the relative transformation matrix; multiplying the ratio of the displacement vector to the time difference by a preset scaling factor to obtain the initial linear velocity; and transforming the initial linear velocity to the world coordinate system to obtain the vehicle linear velocity.

[0011] By adopting the above technical solution, the control system calculates the vehicle's linear velocity based on the odometer transformation matrix at adjacent time points, fully utilizing the high-precision position measurement characteristics of the odometer. Introducing a preset proportional coefficient can eliminate odometer measurement errors. This odometer-based method for calculating vehicle linear velocity, compared to simply relying on IMU integration or position difference, provides a more stable and accurate velocity estimate, contributing to improved accuracy of point cloud motion compensation.

[0012] In conjunction with some embodiments of the first aspect, in some embodiments, calculating the vehicle linear velocity between the first time moment and the second time moment specifically includes: obtaining the first position vector of the vehicle at the first time moment and the second position vector at the second time moment; calculating the position vectors of the first position vector and the second position vector; and dividing the position vectors by the time difference to obtain the vehicle linear velocity.

[0013] By adopting the above technical solution, the control system acquires the position vectors of adjacent time points, calculates the difference, and then divides it by the time difference to obtain the vehicle's linear velocity. This achieves direct measurement of the vehicle's motion state, reducing errors introduced by intermediate steps, and is particularly suitable for scenarios with high-precision positioning equipment. This simplified method for calculating vehicle linear velocity not only reduces computational complexity but also maintains high estimation accuracy, providing a reliable velocity reference for point cloud motion compensation.

[0014] In conjunction with some embodiments of the first aspect, in some embodiments, each point in the lidar point cloud is compensated based on displacement and attitude changes to obtain a corrected lidar point cloud. Specifically, this includes: determining the pose compensation amount based on displacement and attitude changes; multiplying the original coordinates of each point in the lidar point cloud by the pose compensation amount to obtain the corrected coordinates of each point in the lidar point cloud, so as to determine the corrected lidar point cloud.

[0015] By adopting the above technical solution, the control system calculates the pose compensation amount based on displacement and attitude changes, and then applies the pose compensation amount to the coordinate correction of each point in the lidar point cloud. This compensation method takes into account the vehicle's translational and rotational motion in space, and can accurately describe the motion distortion during the lidar point cloud acquisition process.

[0016] In some embodiments of the first aspect, before obtaining the first IMU angular velocity, first IMU acceleration, first gravity vector, and first attitude quaternion of the vehicle at a first moment, and the second IMU angular velocity and second IMU acceleration of the vehicle at a second moment, the method further includes: obtaining the initial IMU acceleration of the vehicle in a stationary state, determining the initial gravity vector by low-pass filtering; aligning the initial gravity vector with the z-axis vector of the world coordinate system to obtain the initial attitude quaternion of the vehicle in a stationary state.

[0017] By adopting the above technical solution, the control system acquires the initial IMU acceleration while the vehicle is stationary and determines the initial gravity vector through low-pass filtering, effectively eliminating the influence of sensor noise. The control system aligns the initial gravity vector with the z-axis of the world coordinate system to obtain the initial attitude quaternion, establishing an accurate correspondence between the vehicle coordinate system and the world coordinate system. This gravity-alignment-based initialization method requires no additional calibration equipment, is simple to operate, and has high reliability. By providing an accurate initial state for the vehicle, it can significantly improve the accuracy of subsequent motion compensation and reduce error accumulation.

[0018] In conjunction with some embodiments of the first aspect, in some embodiments, after the step of compensating each point in the lidar point cloud based on displacement and attitude changes to obtain a corrected lidar point cloud, the method further includes: converting the corrected lidar point cloud into a three-dimensional mesh model, performing surface reconstruction on the three-dimensional mesh model to obtain a scene three-dimensional geometric model; and storing the scene three-dimensional geometric model in a scene database.

[0019] By adopting the above technical solution, the control system converts the corrected LiDAR point cloud into a 3D mesh model and performs surface reconstruction, realizing the transformation from discrete point cloud to continuous geometric model. This processing method not only provides a more intuitive scene representation, but also fills in holes and occluded areas in the point cloud data.

[0020] In a second aspect, embodiments of this application provide a control system comprising: one or more processors and a memory; the memory is coupled to the one or more processors and is used to store computer program code, the computer program code including computer instructions, wherein the one or more processors invoke the computer instructions to cause the control system to perform the method described in the first aspect and any possible implementation thereof.

[0021] Thirdly, embodiments of this application provide a computer program product containing instructions that, when the computer program product is run on a control system, cause the control system to perform the method described in the first aspect and any possible implementation thereof.

[0022] Fourthly, embodiments of this application provide a computer-readable storage medium including instructions that, when executed on a control system, cause the control system to perform the method described in the first aspect and any possible implementation thereof.

[0023] Understandably, the control system provided in the second aspect, the computer program product provided in the third aspect, and the computer storage medium provided in the fourth aspect are all used to execute the methods provided in the embodiments of this application. Therefore, the beneficial effects they can achieve can be referred to the beneficial effects in the corresponding methods, and will not be repeated here.

[0024] One or more technical solutions provided in the embodiments of this application have at least the following technical effects or advantages:

[0025] 1. By adopting the above technical solution, the control system calculates the predicted attitude quaternion and predicted gravity vector of the vehicle at the second moment. It then compensates for the vehicle's attitude using the predicted gravity vector, the inverse of the predicted attitude quaternion, and the z-axis vector of the world coordinate system, obtaining the second attitude quaternion of the vehicle at the second moment, thereby determining the vehicle's attitude change from the first moment to the second moment. The control system calculates the vehicle's linear velocity to determine the vehicle's displacement change from the first moment to the second moment. Based on the displacement and attitude changes, the control system performs precise motion compensation on the LiDAR point cloud. Compared with traditional linear interpolation methods, this method considers gravity constraints and can more accurately describe the vehicle's trajectory in complex scenarios such as high-speed driving or sharp turns, thereby effectively reducing the accumulation of attitude estimation errors and improving point cloud registration accuracy.

[0026] 2. By adopting the above technical solution, the control system introduces an exponential smoothing coefficient and a gravity update formula to establish an adaptive gravity vector update mechanism: the calculation of the exponential smoothing coefficient takes into account the influence of time difference and can automatically adjust the smoothing degree according to the sampling interval; the gravity update formula maintains the stability of the gravity vector and can quickly respond to changes in vehicle attitude by weighted fusion of historical gravity vectors and current IMU acceleration. This adaptive gravity vector update mechanism can improve the tracking ability of vehicle motion state changes while ensuring system stability, thereby obtaining more accurate attitude estimation results.

[0027] 3. By adopting the above technical solution, the control system acquires the initial IMU acceleration while the vehicle is stationary and determines the initial gravity vector through low-pass filtering, effectively eliminating the influence of sensor noise. The control system aligns the initial gravity vector with the z-axis of the world coordinate system to obtain the initial attitude quaternion, establishing an accurate correspondence between the vehicle coordinate system and the world coordinate system. This gravity alignment-based initialization method does not require additional calibration equipment, is simple to operate, and has high reliability. By providing an accurate initial state for the vehicle, it can significantly improve the accuracy of subsequent motion compensation and reduce error accumulation. Attached Figure Description

[0028] Figure 1 This is a flowchart illustrating a laser radar point cloud motion compensation method in an embodiment of this application.

[0029] Figure 2 This is another flowchart illustrating the laser radar point cloud motion compensation method in this application embodiment;

[0030] Figure 3This is a schematic diagram of the physical device structure of a control system in an embodiment of this application. Detailed Implementation

[0031] The terminology used in the following embodiments of this application is for the purpose of describing particular embodiments only and is not intended to be limiting of this application. As used in the specification of this application, the singular expressions “a,” “an,” “the,” “the,” and “this” are intended to include the plural expressions as well, unless the context clearly indicates otherwise. It should also be understood that the term “and / or” as used in this application refers to any or all possible combinations including one or more of the listed items.

[0032] Hereinafter, the terms "first" and "second" are used for descriptive purposes only and should not be construed as implying or suggesting relative importance or implicitly indicating the number of indicated technical features. Thus, a feature defined as "first" or "second" may explicitly or implicitly include one or more of that feature, and in the description of the embodiments of this application, unless otherwise stated, "multiple" means two or more.

[0033] The following describes the process of the method provided in this implementation. Please refer to [link / reference]. Figure 1 This is a flowchart illustrating the laser radar point cloud motion compensation method in an embodiment of this application.

[0034] S101. Obtain the first IMU angular velocity, first IMU acceleration, first gravity vector and first attitude quaternion of the vehicle at the first moment, and the second IMU angular velocity and second IMU acceleration of the vehicle at the second moment, wherein the first moment is before the second moment and the time difference between the first moment and the second moment is less than a preset time difference threshold.

[0035] Among them, IMU angular velocity refers to the rotational speed of the vehicle around the three coordinate axes as measured by the inertial measurement unit (IMU), usually in radians per second; IMU acceleration refers to the acceleration of the vehicle in the three coordinate axis directions as measured by the inertial measurement unit (IMU), usually in meters per square second; gravity vector refers to the gravitational acceleration vector measured in the vehicle coordinate system, used to characterize the degree of deviation of the vehicle attitude from the direction of gravity; attitude quaternion refers to the vehicle attitude represented by quaternions, used to describe the rotation relationship of the vehicle coordinate system relative to the world coordinate system; time difference represents the time interval between two adjacent sampling moments, usually in seconds; preset time difference threshold refers to the pre-set maximum allowable time interval, used to ensure that the time span between adjacent sampling points is within a reasonable range.

[0036] Specifically, the control system collects vehicle motion state data through IMU sensors at a fixed frequency (such as 100Hz or 200Hz): the control system acquires the first IMU angular velocity and first IMU acceleration of the vehicle at the first moment, then acquires the second IMU angular velocity and second IMU acceleration of the vehicle at the second moment, and so on. At the same time, the control system calculates the time difference between the two moments after acquiring data from two moments.

[0037] The control system calculates the first gravity vector and the first attitude quaternion of the vehicle at the first moment based on the previous IMU angular velocity and acceleration of the vehicle at the previous moment, as well as the first IMU angular velocity and acceleration of the vehicle at the first moment.

[0038] S102. Multiply the time difference by the first IMU angular velocity to obtain the vehicle's rotation vector. Construct a rotation quaternion based on the rotation vector. Determine the vehicle's predicted attitude quaternion at the second moment by using the rotation quaternion and the first attitude quaternion.

[0039] Among them, the rotation vector represents a three-dimensional vector describing the vehicle's rotational motion, with its direction representing the rotation axis and its magnitude representing the rotation angle; the rotation quaternion refers to the rotation transformation expressed in quaternion form, used to realize rotation operations in three-dimensional space; the predicted attitude quaternion represents the predicted value of the vehicle's attitude at the second moment, used to represent the predicted rotation relationship between the vehicle coordinate system and the world coordinate system.

[0040] Specifically, first, the control system performs component multiplication of the time difference with the first IMU angular velocity vector to obtain a rotation vector describing the overall rotation effect of the vehicle. Then, the control system uses the Rodriguez formula to convert the rotation vector into a rotation quaternion, which describes the vehicle attitude change from the first time step to the second time step. Finally, the control system performs a quaternion multiplication operation between the rotation quaternion and the first attitude quaternion to obtain the predicted attitude quaternion for the second time step. The predicted attitude quaternion considers the vehicle's angular motion during this time step, providing an initial estimate for subsequent attitude corrections.

[0041] S103. Calculate the exponential smoothing coefficient based on the time difference, and substitute the exponential smoothing coefficient, the second IMU acceleration, and the first gravity vector into the gravity update formula to obtain the predicted gravity vector of the vehicle at the second moment.

[0042] Among them, the exponential smoothing coefficient represents a weighting coefficient between 0 and 1, used to balance the influence of the historical gravity vector and the new gravity vector; the gravity update formula is a mathematical expression used to calculate the new gravity vector, used to describe the evolution of the gravity vector over time; the predicted gravity vector represents the predicted value of the vehicle's gravity vector at the second moment, used to represent the predicted gravity direction in the vehicle coordinate system.

[0043] Specifically, the control system calculates the exponential smoothing coefficient based on the time difference. This time difference can be substituted into the exponential smoothing coefficient calculation method. The exponential smoothing coefficient is calculated as: α = 1 - exp(-Δt / τ); where α represents the exponential smoothing coefficient, Δt represents the time difference, and τ represents the gravity time constant (e.g., 0.5 seconds), used to control the rate of gravity vector update.

[0044] The control system substitutes the calculated exponential smoothing coefficient, the vehicle's second IMU acceleration at the second moment, and the vehicle's first gravity vector at the first moment into the gravity update formula. The gravity update formula is: g1 = (1-α) × g0 + α × a1; where g1 represents the predicted gravity vector, α represents the exponential smoothing coefficient, g0 represents the first gravity vector, and a1 represents the second IMU acceleration. The gravity update formula achieves smooth updating of the gravity vector through weighted fusion. The weight of the historical gravity vector g0 is (1-α), and the weight of the new IMU acceleration a1 is α. The resulting new gravity vector g1 is the predicted gravity vector of the vehicle at the second moment. This update method maintains the stability of gravity estimation and can respond promptly to changes in vehicle attitude.

[0045] S104. Based on the predicted gravity vector, the inverse of the predicted attitude quaternion, and the z-axis vector of the world coordinate system, determine the alignment rotation quaternion. Through the alignment rotation quaternion and the predicted attitude quaternion, determine the second attitude quaternion of the vehicle at the second moment.

[0046] Among them, the alignment rotation quaternion refers to the rotation transformation required to rotate the predicted gravity vector to the direction after the inverse of the predicted attitude quaternion is applied to the z-axis vector of the world coordinate system, which is used to correct the error in attitude prediction; the z-axis vector of the world coordinate system refers to the unit vector [0, 0, 1] pointing to the zenith in the world coordinate system, which is used to represent the reference of the direction of gravity; the inverse of the predicted attitude quaternion is the conjugate of the predicted attitude quaternion divided by the square of its magnitude, which is used to achieve the reverse rotation; the second attitude quaternion refers to the corrected attitude quaternion of the vehicle at the second moment, which is used to represent the accurate rotation relationship of the vehicle coordinate system relative to the world coordinate system.

[0047] Specifically, first, the control system applies the inverse of the predicted attitude quaternion to the z-axis vector of the world coordinate system to obtain the direction in which the gravity direction corresponding to the predicted attitude quaternion should point. Then, the control system uses the FromTwoVectors function to calculate the rotation quaternion required to rotate the predicted gravity vector to this correct direction, i.e., the alignment rotation quaternion. This calculation process essentially searches for a supplementary rotation that can correct the predicted attitude error. Finally, the control system performs a quaternion multiplication between the alignment rotation quaternion and the predicted attitude quaternion to obtain the corrected second attitude quaternion of the vehicle at the second time step. This gravity alignment-based correction method can effectively detect and correct errors in attitude estimation.

[0048] S105. Calculate the linear velocity of the vehicle between the first and second time points;

[0049] Among them, the vehicle linear velocity represents the translational velocity vector of the vehicle's center of mass in the world coordinate system, which is used to describe the vehicle's translational motion state.

[0050] Optionally, under normal circumstances, the vehicle linear velocity between the first and second time moments can be calculated in the following way, without limitation: obtain the first transformation matrix of the odometer at the first time moment and the second transformation matrix of the odometer at the second time moment; invert the second transformation matrix of the odometer and multiply it with the first transformation matrix of the odometer to obtain the relative transformation matrix between the first and second time moments; extract the displacement vector in the relative transformation matrix; multiply the ratio of the displacement vector to the time difference by a preset scaling factor to obtain the initial linear velocity; transform the initial linear velocity to the world coordinate system to obtain the vehicle linear velocity.

[0051] The displacement vector is a three-dimensional vector describing the vehicle's position in the world coordinate system; the odometer transformation matrix is ​​a 4×4 homogeneous transformation matrix describing the vehicle's position and attitude, used to represent the vehicle's complete motion state in the odometer coordinate system; the relative transformation matrix is ​​a transformation matrix describing the relative motion between the first and second moments; the initial linear velocity represents the linear velocity calculated in the vehicle body coordinate system.

[0052] Specifically, the control system can calculate the vehicle's linear velocity between the first and second moments based on odometer data. The control system first obtains the first transformation matrix of the odometer at the first moment and the second transformation matrix at the second moment. After inverting the second transformation matrix, it multiplies it by the first transformation matrix to obtain the relative transformation matrix between the first and second moments. The control system extracts the displacement vector from this matrix, divides it by the time difference, and then multiplies it by a preset scaling factor (e.g., 1.05, used to compensate for factors such as wheel slippage) to obtain the initial linear velocity. Finally, the control system transforms this velocity to the world coordinate system based on the current attitude.

[0053] Optionally, under normal circumstances, the vehicle linear velocity between the first and second time moments can also be calculated in the following ways, which are not limited here: obtain the first position vector of the vehicle at the first time moment and the second position vector at the second time moment, calculate the position vectors of the first and second position vectors; divide the position vectors by the time difference to obtain the vehicle linear velocity.

[0054] Specifically, the control system can use the position information provided by the positioning system to calculate the difference between the vehicle's first position vector at the first moment and the second position vector at the second moment, and then divide by the time difference to obtain the vehicle's linear velocity in the world coordinate system.

[0055] It should be noted that the control system can select the appropriate method for calculating the vehicle linear velocity based on the availability and accuracy of the sensors.

[0056] S106. Multiply the vehicle linear velocity by the time difference to obtain the displacement change between the second moment and the first moment. Calculate the attitude change between the second moment and the first moment based on the second attitude quaternion and the first attitude quaternion.

[0057] Among them, displacement change represents the change in the spatial position of the vehicle between two moments, used to describe the cumulative effect of translational motion; attitude change represents the relative rotational transformation of the vehicle between two moments, used to represent the rotational change of the vehicle coordinate system.

[0058] Specifically, first, the control system multiplies the vehicle's linear velocity by the time difference to obtain the displacement change describing the vehicle's translational motion. Then, the control system performs a quaternion multiplication between the second attitude quaternion and the conjugate of the first attitude quaternion to obtain the attitude change describing the vehicle's rotational motion. These two changes comprehensively describe the rigid body motion of the vehicle within the sampling time, providing the necessary motion parameters for subsequent point cloud compensation. The control system saves this data for processing all LiDAR point clouds acquired within that sampling time.

[0059] S107. Based on displacement and attitude changes, each point in the lidar point cloud is compensated to obtain the corrected lidar point cloud.

[0060] Specifically, since the lidar uses a rotating scanning method to acquire lidar point clouds, each point's acquisition time t has a time offset relative to the scanning time t0. The control system calculates the time proportion λ = (t - t0) / (t1 - t0) for each point. Then, based on the assumption of uniform motion, the control system calculates the partial displacement δt = λ × Δt and the partial rotation δq = slerp(q0, q1, λ) at that moment, where slerp represents quaternion spherical linear interpolation. Next, the control system compensates for the original point coordinates p using the formula p' = δq × p + δt to obtain the corrected point coordinates p'. This process is repeated for all points in the lidar point cloud, ultimately resulting in a complete corrected lidar point cloud that eliminates motion distortion. This compensation method, which considers the differences in point acquisition times, can accurately restore the true spatial structure of the point cloud.

[0061] Optionally, in general, compensation is performed on each point in the lidar point cloud based on displacement and attitude changes. The corrected lidar point cloud can be obtained in the following way, which is not limited here: determine the pose compensation amount based on displacement and attitude changes; multiply the original coordinates of each point in the lidar point cloud by the pose compensation amount to obtain the corrected coordinates of each point in the lidar point cloud, so as to determine the corrected lidar point cloud.

[0062] Pose compensation represents the transformation matrix used to correct the lidar point cloud, which is used to achieve motion compensation of the lidar point cloud; the original coordinates refer to the coordinate values ​​of each point in the lidar point cloud at the time of acquisition; the corrected coordinates refer to the coordinate values ​​after motion compensation; the corrected lidar point cloud refers to the complete lidar point cloud after motion compensation.

[0063] Specifically, firstly, the control system constructs a complete pose compensation transformation matrix, i.e., the pose compensation amount, based on displacement and attitude changes. For each point in the LiDAR point cloud, the control system calculates the corresponding partial compensation amount based on the scanning time of that point. This calculation process assumes that the vehicle moves at a constant speed during the scanning process, and obtains the precise compensation parameters for each point through linear interpolation. Then, the control system multiplies the original coordinates of each point with its corresponding pose compensation transformation matrix to obtain the corrected coordinates of each point. After completing the compensation calculation for all points, the control system saves the corrected LiDAR point cloud for subsequent use. This point-by-point compensation method can effectively eliminate point cloud distortion caused by vehicle motion, improving the accuracy and usability of the point cloud data.

[0064] By employing the above technical solution, the control system calculates the predicted attitude quaternion and predicted gravity vector of the vehicle at the second moment. It then compensates for the vehicle's attitude using the predicted gravity vector, the inverse of the predicted attitude quaternion, and the z-axis vector of the world coordinate system, obtaining the second attitude quaternion of the vehicle at the second moment, thereby determining the vehicle's attitude change from the first moment to the second moment. The control system calculates the vehicle's linear velocity to determine the vehicle's displacement change from the first moment to the second moment. Based on the displacement and attitude changes, the control system performs precise motion compensation on the LiDAR point cloud. Compared with traditional linear interpolation methods, this method considers gravity constraints and can more accurately describe the vehicle's trajectory in complex scenarios such as high-speed driving or sharp turns, thereby effectively reducing the accumulation of attitude estimation errors and improving point cloud registration accuracy.

[0065] The following provides a more detailed description of the process of the method provided in this implementation. Please refer to [link / reference]. Figure 2 This is another flowchart illustrating the laser radar point cloud motion compensation method in this application embodiment.

[0066] The following steps may or may not be performed before step S101; this is not limited here:

[0067] S201. Obtain the initial IMU acceleration of the vehicle when it is stationary, and determine the initial gravity vector by low-pass filtering.

[0068] Among them, the stationary state indicates that the vehicle is completely stopped and without vibration, which is used to obtain accurate gravity measurement; the initial IMU acceleration refers to the triaxial acceleration value measured by the IMU accelerometer in the stationary state, which mainly reflects the influence of gravity; low-pass filtering refers to a signal processing method used to remove high-frequency noise in the acceleration signal; the initial gravity vector represents the gravity acceleration vector obtained after low-pass filtering, which is used for subsequent attitude initialization.

[0069] Specifically, the control system requires the vehicle to remain completely stationary. At this point, the acceleration measured by the IMU accelerometer only includes the gravity component and sensor noise. The control system continuously acquires multiple frames of IMU acceleration data and performs low-pass filtering on the acceleration value for each axis, using methods such as moving average or Butterworth filtering. After filtering to eliminate random noise, the three-dimensional vector obtained by the control system is the initial gravity vector in the vehicle coordinate system. This initial gravity vector points towards the Earth's center and has a magnitude of approximately 9.81 m / s².

[0070] S202. Align the initial gravity vector with the z-axis vector of the world coordinate system to obtain the initial attitude quaternion of the vehicle in a stationary state.

[0071] Here, the z-axis vector of the world coordinate system refers to the unit vector [0, 0, 1] pointing towards the zenith, opposite to the direction of gravity; the alignment operation represents the calculation of the rotation transformation required to rotate the initial gravity vector to be aligned with the z-axis vector of the world coordinate system in the opposite direction; the initial attitude quaternion represents the quaternion describing the initial orientation of the vehicle coordinate system relative to the world coordinate system.

[0072] Specifically, first, the control system normalizes the initial gravity vector to obtain a unit initial gravity vector g. Then, the control system uses the FromTwoVectors function to calculate the rotation quaternion required to rotate g to -z (i.e., [0, 0, -1]). This rotation quaternion is the vehicle's initial attitude quaternion, which defines the initial transformation relationship between the vehicle coordinate system and the world coordinate system. This gravity-aligned initialization method can accurately determine the vehicle's pitch and roll angles, but because the gravity vector remains unchanged when rotating in the horizontal plane, it cannot determine the yaw angle. For complete attitude information, the cooperation of other sensors (such as GPS or magnetometers) is required.

[0073] The following steps may or may not be performed after step S107; no limitation is made here:

[0074] S203. Convert the corrected LiDAR point cloud into a 3D mesh model, and perform surface reconstruction on the 3D mesh model to obtain the 3D geometric model of the scene.

[0075] Among them, the three-dimensional mesh model represents a discrete spatial representation composed of regular voxel meshes, which is used to structure and organize the corrected LiDAR point cloud; a voxel is the smallest unit in three-dimensional space, similar to a pixel in a two-dimensional image; surface reconstruction represents the process of recovering a continuous surface from a discrete point cloud, which is used to fill the gaps in the point cloud data and obtain a complete geometric surface; the three-dimensional geometric model refers to a scene digital model with a continuous surface, which is used to express the complete geometric structure of the environment.

[0076] Specifically, first, the control system maps the corrected LiDAR point cloud onto a 3D mesh of preset resolution, with each voxel recording the statistical characteristics of the points falling within it. Then, the control system uses algorithms such as Poisson surface reconstruction or moving least squares to reconstruct a continuous surface model based on the meshed point cloud data. This reconstruction process automatically fills in data gaps caused by occlusion or undersampling, while also smoothing out surface irregularities caused by measurement noise. In this way, the control system obtains a complete and accurate 3D geometric model of the scene.

[0077] S204. Store the 3D geometric model of the scene into the scene database.

[0078] The scene database refers to the database system used to store and manage 3D scene data, enabling efficient data access and querying.

[0079] Specifically, first, the control system converts the scene's 3D geometric model into a format suitable for database storage, such as a mesh file or parametric surface description. Then, the control system adds metadata such as timestamps and location information to the scene's 3D geometric model, establishing a spatial index structure to support rapid retrieval. Finally, the control system writes this data into the scene database, ensuring data integrity and consistency.

[0080] The control system in the embodiments of this invention is described below from the perspective of hardware processing. Please refer to [link / reference needed]. Figure 3 This is a schematic diagram of the physical device structure of the control system in an embodiment of this application.

[0081] It should be noted that, Figure 3 The structure of the control system shown is merely an example and should not impose any limitation on the functionality and scope of use of the embodiments of the present invention.

[0082] like Figure 3 As shown, the control system includes a CPU 301, which can perform various appropriate actions and processes based on a program stored in the read-only memory ROM 302 or a program loaded from the storage section 308 into the random access memory RAM 303, such as executing the methods described in the above embodiments. The RAM 303 also stores various programs and data required for system operation. The CPU 301, ROM 302, and RAM 303 are interconnected via a bus 304. An I / O interface 305 is also connected to the bus 304.

[0083] The following components are connected to I / O interface 305: input section 306 including audio input devices, push-button switches, etc.; output section 307 including a liquid crystal display (LCD) and audio output devices, indicator lights, etc.; storage section 308 including a hard disk, etc.; and communication section 309 including a network interface card such as a LAN (Local Area Network) card, modem, etc. Communication section 309 performs communication processing via a network such as the Internet. Drive 310 is also connected to I / O interface 305 as needed. Removable media 311, such as a disk, optical disk, magneto-optical disk, semiconductor memory, etc., are installed on drive 310 as needed so that computer programs read from them can be installed into storage section 308 as needed.

[0084] In particular, according to embodiments of the present invention, the processes described above with reference to the flowcharts can be implemented as computer software programs. For example, embodiments of the present invention include a computer program product comprising a computer program carried on a computer-readable medium, the computer program containing computer programs for performing the methods shown in the flowcharts. In such embodiments, the computer program can be downloaded and installed from a network via communication section 309, and / or installed from removable medium 311. When the computer program is executed by CPU 301, it performs the various functions defined in the present invention.

[0085] It should be noted that specific examples of computer-readable storage media may include, but are not limited to: electrical connections having one or more wires, portable computer disks, hard disks, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM), flash memory, optical fiber, portable compact disc read-only memory (CD-ROM), optical storage devices, magnetic storage devices, or any suitable combination thereof. In this invention, a computer-readable storage medium can be any tangible medium containing or storing a program that can be used by or in conjunction with an instruction execution system, apparatus, or device.

[0086] The flowcharts and block diagrams in the accompanying drawings illustrate the architecture, functionality, and operation of possible implementations of systems, methods, and computer program products according to various embodiments of the present invention. Each block in a flowchart or block diagram may represent a module, program segment, or portion of code, which contains one or more executable instructions for implementing a specified logical function. It should also be noted that in some alternative implementations, the functions indicated in the blocks may occur in a different order than those shown in the drawings.

[0087] Specifically, the control system in this embodiment includes a processor and a memory. The memory stores a computer program, and when the computer program is executed by the processor, it implements the laser radar point cloud motion compensation method provided in the above embodiment.

[0088] In another aspect, the present invention also provides a computer-readable storage medium, which may be included in the control system described in the above embodiments; or it may exist independently and not incorporated into the control system. The storage medium carries one or more computer programs that, when executed by a processor of the control system, cause the control system to implement the lidar point cloud motion compensation method provided in the above embodiments.

[0089] The above-described embodiments are only used to illustrate the technical solutions of this application, and are not intended to limit it. Although this application 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 scope of the technical solutions of the embodiments of this application.

[0090] As used in the above embodiments, depending on the context, the term "when..." can be interpreted as meaning "if...", "after...", "in response to determining...", or "in response to detecting...". Similarly, depending on the context, the phrase "when determining..." or "if (the stated condition or event) is interpreted as meaning "if determining...", "in response to determining...", "when (the stated condition or event) is detected", or "in response to detecting (the stated condition or event)".

[0091] Those skilled in the art will understand that all or part of the processes in the methods of the above embodiments can be implemented by a computer program instructing related hardware. This program can be stored in a computer-readable storage medium, and when executed, it can include the processes described in the above method embodiments. The aforementioned storage medium includes various media capable of storing program code, such as ROM or random access memory (RAM), magnetic disks, or optical disks.

Claims

1. A method for motion compensation of point clouds using lidar, characterized in that, Applied to a control system, the method includes: The vehicle acquires the first IMU angular velocity, first IMU acceleration, first gravity vector, and first attitude quaternion at the first moment, and the second IMU angular velocity and second IMU acceleration at the second moment, wherein the first moment is before the second moment and the time difference between the first moment and the second moment is less than a preset time difference threshold. The time difference is multiplied by the first IMU angular velocity to obtain the vehicle's rotation vector. A rotation quaternion is constructed based on the rotation vector. The predicted attitude quaternion of the vehicle at the second moment is determined by the rotation quaternion and the first attitude quaternion. The exponential smoothing coefficient is calculated based on the time difference. The exponential smoothing coefficient, the second IMU acceleration, and the first gravity vector are substituted into the gravity update formula to obtain the predicted gravity vector of the vehicle at the second moment. Based on the predicted gravity vector, the inverse of the predicted attitude quaternion, and the z-axis vector of the world coordinate system, the alignment rotation quaternion is determined. Then, the second attitude quaternion of the vehicle at the second moment is determined by the alignment rotation quaternion and the predicted attitude quaternion. Calculate the linear velocity of the vehicle between the first time point and the second time point; Multiply the vehicle linear velocity by the time difference to obtain the displacement change between the second moment and the first moment. Calculate the attitude change between the second moment and the first moment based on the second attitude quaternion and the first attitude quaternion. Based on the displacement change and the attitude change, each point in the lidar point cloud is compensated to obtain the corrected lidar point cloud.

2. The method according to claim 1, characterized in that, The step of calculating the exponential smoothing coefficient based on the time difference, and substituting the exponential smoothing coefficient, the second IMU acceleration, and the first gravity vector into the gravity update formula to obtain the predicted gravity vector of the vehicle at the second time moment, specifically includes: Substituting the time difference into the exponential smoothing coefficient calculation method, the exponential smoothing coefficient is obtained; The exponential smoothing coefficient is calculated as follows: α = 1 - exp(-Δt / τ). Where α represents the exponential smoothing coefficient, Δt represents the time difference, and τ represents the gravity time constant; The gravity update formula is: g1 = (1-α) × g0 + α × a1; Where g1 represents the predicted gravity vector, α represents the exponential smoothing coefficient, g0 represents the first gravity vector, and a1 represents the second IMU acceleration.

3. The method according to claim 1, characterized in that, The calculation of the vehicle linear velocity between the first time moment and the second time moment specifically includes: Obtain the first transformation matrix of the odometer at the first time point and the second transformation matrix of the odometer at the second time point. Invert the second transformation matrix of the odometer and multiply it with the first transformation matrix of the odometer to obtain the relative transformation matrix between the first time point and the second time point. Extract the displacement vector from the relative transformation matrix, and multiply the ratio of the displacement vector to the time difference by a preset scaling factor to obtain the initial linear velocity; The initial linear velocity is transformed to the world coordinate system to obtain the vehicle's linear velocity.

4. The method according to claim 1, characterized in that, The calculation of the vehicle linear velocity between the first time moment and the second time moment specifically includes: Obtain the first position vector of the vehicle at the first time and the second position vector at the second time, and calculate the position vector of the first position vector and the second position vector; Divide the position vector by the time difference to obtain the vehicle's linear velocity.

5. The method according to claim 1, characterized in that, The step of compensating each point in the lidar point cloud based on the displacement change and the attitude change to obtain the corrected lidar point cloud specifically includes: The pose compensation amount is determined based on the displacement change and the attitude change; The original coordinates of each point in the lidar point cloud are multiplied by the pose compensation amount to obtain the corrected coordinates of each point in the lidar point cloud, thereby determining the corrected lidar point cloud.

6. The method according to claim 1, characterized in that, Before the steps of acquiring the vehicle's first IMU angular velocity, first IMU acceleration, first gravity vector, and first attitude quaternion at a first moment, and the vehicle's second IMU angular velocity and second IMU acceleration at a second moment, the method further includes: The initial IMU acceleration of the vehicle when it is stationary is obtained, and the initial gravity vector is determined by low-pass filtering; Align the initial gravity vector with the z-axis vector of the world coordinate system to obtain the initial attitude quaternion of the vehicle in a stationary state.

7. The method according to claim 1, characterized in that, After the step of compensating each point in the lidar point cloud based on the displacement change and the attitude change to obtain the corrected lidar point cloud, the method further includes: The corrected lidar point cloud is converted into a three-dimensional mesh model, and the three-dimensional mesh model is reconstructed to obtain a three-dimensional geometric model of the scene. The three-dimensional geometric model of the scene is stored in the scene database.

8. A control system, characterized in that, The control system includes: one or more processors and a memory; the memory is coupled to the one or more processors, the memory is used to store computer program code, the computer program code including computer instructions, and the one or more processors call the computer instructions to cause the control system to perform the method as described in any one of claims 1-7.

9. A computer-readable storage medium comprising instructions, characterized in that, When the instruction is executed on the control system, it causes the control system to perform the method as described in any one of claims 1-7.

10. A computer program product, characterized in that, When the computer program product is run on the control system, the control system performs the method as described in any one of claims 1-7.

Images