Intelligent automobile laser radar multi-source point cloud motion compensation method
Through multi-source lidar data acquisition, time and space synchronization conversion, motion compensation calculation and point cloud fusion, the motion distortion problem of point cloud data in multi-source lidar systems is solved, and the environmental perception accuracy and security of smart cars are improved.
Patent Information
- Application Number
- CN202510387004.8
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2025-03-31
- Publication Date
- 2025-07-11
AI Technical Summary
In smart cars, due to errors in time synchronization and spatial synchronization in multi-source lidar systems, point cloud data motion distortion is caused, affecting environmental perception accuracy and decision-making control performance.
Through multi-source lidar data acquisition, synchronous conversion of time and space coordinates, motion compensation calculation and point cloud data fusion, a more accurate environmental point cloud model is generated.
It effectively solves the problem of time and space synchronization errors between multi-source lidars, improves the accuracy and reliability of point cloud data, generates a more complete environmental point cloud model, and improves the environmental perception and control capabilities of smart cars.
Smart Images
Figure CN120294780A_ABST
Abstract
Description
Technical Field
[0001] The present invention belongs to the technical field of intelligent vehicles. Specifically, it particularly relates to a motion compensation method for multi-source point clouds of lidars in intelligent vehicles. Background Art
[0002] In an intelligent vehicle system, perception sensors such as cameras and lidars capture environmental information, providing an important basis for the system's planning and decision-making. Therefore, ensuring the normal operation of perception sensors is also very important for realizing the safety of intelligent vehicles. A lidar is one of the important sensors for obtaining surrounding environmental information. A lidar obtains the distance information of target objects by emitting laser beams and receiving reflected light, thereby generating point cloud data. However, since an intelligent vehicle is in a continuously moving state during driving, the lidar will also move during the scanning process, resulting in motion distortion in the obtained point cloud data, which affects the accuracy and reliability of the point cloud data. Especially in a multi-source lidar system, there are certain errors in the time synchronization and space synchronization between different lidars. When the point cloud data obtained by each lidar scanning is fused, the problem of motion distortion is more prominent, seriously affecting the accurate perception of the surrounding environment by intelligent vehicles, and further affecting the decision-making and control performance of intelligent vehicles. Currently, some motion compensation methods do not have an ideal effect on the processing of multi-source point clouds and cannot well solve the motion compensation problem of point cloud data of multi-source lidars in different motion states. Summary of the Invention
[0003] The purpose of the present invention is to address the deficiencies of the existing technology and provide a motion compensation method for multi-source point clouds of lidars in intelligent vehicles with consistent point cloud data, which can effectively solve the motion compensation of point cloud data and is easy to generate a complete environmental point cloud model.
[0004] In order to achieve the above technical objectives, the technical solution adopted by the motion compensation method for multi-source point clouds of lidars in the intelligent vehicle of the present invention is as follows: A motion compensation method for multi-source point clouds of lidars in intelligent vehicles includes the following steps: S1 Multi-source lidar data acquisition: Use multiple lidars installed on an intelligent vehicle to perform real-time scanning of the surrounding environment according to their respective scanning frequencies and time intervals, and obtain multi-source point cloud data at different times; record the timestamp information of each lidar and the real-time motion state information of the intelligent vehicle, including vehicle speed, acceleration, steering angle, etc.; S2 Lidar synchronization processing: According to the timestamp information of the lidars, match the point cloud data obtained by different lidars at similar times, and perform time correction on the point cloud data with time differences through an interpolation algorithm to synchronize the point cloud data of different lidars in time; Spatial coordinate transformation of point cloud data: Establish a unified spatial coordinate system, and transform the point cloud data obtained by each lidar from its respective local coordinate system to the world coordinate system; S4 Motion compensation calculation of point cloud data: According to the motion state information of the intelligent vehicle, perform motion compensation on the point cloud data transformed into the spatial coordinate system; S5 Multi-source point cloud fusion: Adopt a method based on feature matching or distance measurement to match and fuse the point cloud data of different lidars to generate a more accurate and complete environmental point cloud model.
[0005] Preferably, the step S3 includes calculating the position and attitude of each lidar relative to the world coordinate system at different times according to the real-time motion state information of the intelligent vehicle, and using the coordinate transformation matrix to accurately transform the point cloud data from the local coordinate system to the spatial coordinate system.
[0006] Preferably, the step S4 includes, for the translational motion of the intelligent vehicle, calculating the displacement of the point cloud data in the translational direction and making corresponding adjustments to the coordinates of each point; for the rotational motion of the intelligent vehicle, according to the rotation angle and rotation axis, using the rotation matrix to perform rotational compensation on the coordinates of each point to eliminate the distortion of the point cloud data caused by the motion of the intelligent vehicle.
[0007] Preferably, the step S1 is specifically: Assume that three lidars are installed on the intelligent vehicle, namely lidar A, lidar B, and lidar C. During the vehicle driving process, the three lidars scan the surrounding environment at their respective scanning frequencies. For example, lidar A scans once every 0.1 s, lidar B scans once every 0.08 s, and lidar C scans once every 0.12 s to obtain point cloud data. At the same time, the vehicle's sensors record the motion state information such as vehicle speed, acceleration, and steering angle in real time, and add timestamps to the scan data of each lidar; The step S2 is specifically: Taking the timestamp of lidar A as the reference, for the point cloud data of lidar B and lidar C, use the linear interpolation algorithm to correct their timestamps to a moment close to that of lidar A; Let the scanning period of lidar A be T A , then the timestamp at the nth scan is t A,n = nT A (n = 0, 1, 2,...); The scanning period of lidar B is T B , and the timestamp at the mth scan is t B,m = mT B (m = 0, 1, 2,...); The scanning period of lidar C is Tc, and the timestamp at the kth scan is t C,k = KT C(k = 0, 1, 2, …); Assume that the point cloud data of radars B and C are synchronized to the time reference of radar A: Time synchronization of the point cloud data of radar B: At a certain scanning moment t of radar B B,m , find the two scanning moments t of radar A that are closest to it A,n and t A,n+1 , such that t A,n ≤ t B,m ≤ t A , n + 1, that is, nT A ≤ mT B ≤ (n + 1)T A , Determine the value of n through ( represents rounding down); Let the point cloud data of radar B at the moment t B,m be P B,m . Under the time reference of radar A, the corresponding point cloud data at the moments t A,n and t A,n+1 are calculated by linear interpolation; Let the coordinates of a certain point in the point cloud data be p = (x, y, z). At the moment t B,m of radar B, the coordinates of this point are . After linear interpolation, the corresponding coordinates at the moment T A,n under the time reference of radar A and the corresponding coordinates at the moment t A,n+1 Time synchronization of the point cloud data of radar C: Similar to radar B, for a certain scanning moment t C,k of radar C, find the two scanning moments t A,n and t A,n+1 that are closest to it, and determine the value of n through ; Let the point cloud data of radar C at the moment t C,k be P C,k . The coordinates of a certain point among them are p = (x, y, z). At the moment T C,k the coordinates of this point are . After linear interpolation, the corresponding coordinates at the moment T A,n under the time reference of radar A and the corresponding coordinates at the moment t A,n+1 ; In this way, through the linear interpolation method, the point cloud data of Radar B and Radar C are synchronized in time to the time reference of Radar A; The specific steps of step S3 are as follows: establish a three-dimensional space coordinate system, determine the origin and axis directions of the world coordinate system according to the initial position and attitude of the vehicle. For Radar A, Radar B, and Radar C, measure their installation positions and attitude parameters on the vehicle, such as installation angles, offset distances, etc. According to the real-time motion state information of the vehicle, calculate the positions and attitudes of each lidar relative to the world coordinate system at different times, and use the coordinate transformation formula to transform the point cloud data obtained by the three lidars from their respective local coordinate systems to the space coordinate system: Define relevant parameters: Let the three-dimensional coordinate system be 0-XYZ, with the initial position of the vehicle as the origin O of the three-dimensional coordinate system, the initial forward direction of the vehicle as the positive direction of the X-axis, and the vertically upward direction as the positive direction of the Z-axis. Determine the direction of the Y-axis according to the right-hand rule; For the lidar ( ), the installation position vector in the local coordinate system of the vehicle is , and the installation attitude is represented by the rotation matrix (which can be calculated from the installation angle, such as converting to a rotation matrix using Euler angles or quaternions); Let the position vector of the vehicle at time be , and the attitude of the vehicle at time is represented by the rotation matrix (which can be calculated from the attitude parameters of the vehicle, such as steering angle, pitch angle, roll angle, etc.); Calculate the positions and attitudes of each lidar relative to the three-dimensional coordinate system at different times; The lidar at time The position vector relative to the three-dimensional coordinate system is: The lidar at time The attitude rotation matrix relative to the world coordinate system is: Coordinate transformation formula: Let the point cloud data point coordinates of the lidar in the local coordinate system be , and convert it to homogeneous coordinate form ; After coordinate transformation, the homogeneous coordinates of this point in the world coordinate system is: Expand the calculation: where is the rotation part, is the translation part, and then convert to the non - homogeneous coordinate form to obtain the coordinates of this point in the world coordinate system , that is: Through the above formula, according to the motion state information of the vehicle, the position and attitude of each lidar relative to the world coordinate system at different times can be calculated, and the point cloud data obtained by the three lidars can be transformed from their respective local coordinate systems to the world coordinate system; The specific step S4 is as follows: Assume that the vehicle speed at a certain moment is 20m / s, the acceleration is 2m / s², and the steering angle is 5°. According to this motion state information, calculate the displacement of the point cloud data in the translation direction. For each point in the point cloud data, calculate the coordinate change caused by the vehicle translation and rotation according to its position in the world coordinate system and the vehicle motion parameters, and adjust the coordinates of each point through the corresponding translation matrix and rotation matrix to achieve motion compensation; Calculate the displacement of the vehicle within Δt time: According to the uniformly accelerated linear motion formula, the displacement of the vehicle within time The displacement components in the direction are respectively , , , assuming that the vehicle motion direction is along the positive direction of the axis, then: The translation matrix T is used to describe the translation transformation of points, and its form is: Express the point P in the homogeneous coordinate form again , and the coordinate after the translation transformation The rotation matrix, assuming that this vehicle rotates around the z - axis (the steering angle θ is the rotation angle around the z - axis), the rotation matrix R is: The coordinate after the rotation transformation is: Obtain the coordinates after motion compensation: Convert Pr to non - homogeneous coordinate form to obtain the point coordinates after motion compensation p’=(x’,y’,z’); ; y’=(x + x)+(y + y) ; The above is the model constructed based on the formula expression for calculating the coordinate change of point cloud data according to the vehicle motion state information and performing motion compensation; The specific step S5 is as follows: Adopt a method based on feature matching to fuse the point cloud data of three lidars after motion compensation: First, extract feature points (such as edge points, corner points, etc.) in the point cloud data, and then match and fuse the point cloud data of different lidars by calculating the distance and similarity between feature points to generate the final environmental point cloud model; Feature point extraction: Suppose for lidar At time The point cloud data is , and the feature point set is obtained through feature extraction algorithms (such as curvature calculation, normal estimation, etc.), where represents the th feature point of lidar at time , is the number of feature points; Feature point distance calculation: For feature points and (i,k = A,B,C and i≠k) of different lidars, calculate the Euclidean distance between them: where and are the coordinates of the feature points in the world coordinate system (after the previous coordinate transformation); Feature point similarity calculation: In addition to distance, similarity can also be calculated through other features , such as normal vector similarity, local geometric structure similarity, etc. Assuming that normal vector is used to calculate similarity, let and be the feature points and If they are the normal vectors, the normal vector similarity can be expressed as: Feature point matching: By setting a distance threshold and a similarity threshold , the matching feature point pairs can be found. If , it is considered that the feature points and are matched; Point cloud fusion: For the matched feature point pairs, they can be fused into the final environmental point cloud model through weighted average or other fusion strategies . Assuming weighted average is used, for the matched feature point pairs and , the fused point is: where is the weight factor, , and the weight can be determined according to factors such as the reliability and distance of the feature points. For example, the weight can be set according to the inverse of the distance: Final environmental point cloud model generation: The final environmental point cloud model is composed of all the fused feature points and the un-matched but retained feature points: Through the above formula, feature extraction, feature point matching, and fusion can be performed on the point cloud data of the three lidars after motion compensation, and finally a complete environmental point cloud model can be generated.
[0008] Preferably, the feature point extraction, similarity calculation, and fusion strategy in step S5 can be optimized and adjusted according to specific situations. For example, more complex feature descriptors (such as SHOT, FPFH, etc.) and matching algorithms (such as ICP, NDT, etc.) can be used to improve the degree and robustness of fusion.
[0009] Compared with the prior art, the beneficial effects of the present invention are: 1. Through the model calculation method for time and space coordinate synchronous conversion of multi-source lidar data, the present invention can effectively solve the time and space synchronization error problems between multi-source lidars and ensure the consistency of point cloud data; 2. The present invention uses the motion state information of an intelligent vehicle to perform accurate motion compensation calculations, effectively solving the problem in the prior art that the multi-source lidar point cloud data has motion distortion, which affects the environmental perception accuracy of the intelligent vehicle, and improving the accuracy and reliability of the point cloud data; 3. The multi-source point cloud data after motion compensation in the present invention is fused, which can generate a more accurate and complete environmental point cloud model, provide more reliable data support for the environmental perception and control of the intelligent vehicle, and improve the driving safety and intelligent level of the intelligent vehicle. BRIEF DESCRIPTION OF THE DRAWINGS
[0010] Figure 1 is the system block diagram of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS
[0011] The present invention will be further described below in conjunction with the drawings and specific embodiments: In the prior art, the odometer information of the vehicle is used to obtain the motion state of the lidar. By combining the displacement and rotation information measured by the odometer with the point cloud data, the position of each point is corrected to compensate for the motion of the lidar. Through a uniform motion model, the radar is assumed to be in a uniform motion state within a period, the change difference between two frames is calculated, and the set point values are supplemented within the spatial model to achieve the purpose of constructing a spatial structure model. However, there are cumulative errors in the odometer of this method. As time and driving distance increase, the errors will continuously increase, resulting in a decrease in the point cloud compensation accuracy. Moreover, in non-uniform motion situations such as vehicle acceleration, deceleration, or turning, the interpolation error of the uniform motion model interpolation method will increase, affecting the compensation effect.
[0012] As Figure 1 shown, a method for motion compensation of multi-source point clouds of an intelligent vehicle lidar includes the following steps: S1 Multi-source lidar data acquisition: Use multiple lidars installed on the intelligent vehicle to perform real-time scanning of the surrounding environment according to their respective scanning frequencies and time intervals to obtain multi-source point cloud data at different times; record the timestamp information of each lidar and the real-time motion state information of the intelligent vehicle, including vehicle speed, acceleration, steering angle, etc.; S2 Lidar synchronization processing: According to the timestamp information of the lidar, match the point cloud data obtained by different lidars at similar times, and perform time correction on the point cloud data with time differences through an interpolation algorithm to synchronize the point cloud data of different lidars in time; S3 Spatial coordinate transformation of point cloud data: Establish a unified spatial coordinate system, transform the point cloud data obtained by each lidar from its respective local coordinate system to the world coordinate system. According to the real-time motion state information of the intelligent vehicle, calculate the position and attitude of each lidar relative to the world coordinate system at different times, and use the coordinate transformation matrix to accurately transform the point cloud data from the local coordinate system to the spatial coordinate system; S4 Motion compensation calculation of point cloud data: According to the motion state information of the intelligent vehicle, perform motion compensation on the point cloud data transformed into the spatial coordinate system. For the translational motion of the intelligent vehicle, by calculating the displacement of the point cloud data in the translational direction, adjust the coordinates of each point accordingly. For the rotational motion of the intelligent vehicle, according to the rotation angle and rotation axis, use the rotation matrix to perform rotational compensation on the coordinates of each point to eliminate the distortion of the point cloud data caused by the motion of the intelligent vehicle; S5 Multi-source point cloud fusion: Adopt a method based on feature matching or distance measurement to match and fuse the point cloud data of different lidars to generate a more accurate and complete environmental point cloud model.
[0013] The specific steps of S1 are as follows: Assume that three lidars are installed on the intelligent vehicle, namely lidar A, lidar B, and lidar C. During the vehicle driving process, the three lidars scan the surrounding environment at their respective scanning frequencies. For example, lidar A scans once every 0.1 s, lidar B scans once every 0.08 s, and lidar C scans once every 0.12 s to obtain point cloud data. At the same time, the vehicle's sensors record real-time motion state information such as vehicle speed, acceleration, and steering angle, and add timestamps to the scan data of each lidar; The specific steps of S2 are as follows: Taking the timestamp of lidar A as the reference, for the point cloud data of lidar B and lidar C, through the linear interpolation algorithm, correct their timestamps to a moment close to that of lidar A; Let the scanning period of lidar A be T A , then the timestamp at the nth scan is t A,n = nT A (n = 0, 1, 2,...); The scanning period of lidar B is T B , and the timestamp at the mth scan is t B,m = mT B (m = 0, 1, 2,...); The scanning period of lidar C is Tc, and the timestamp at the kth scan is t C,k = KT C (k = 0, 1, 2,...); Assume that the point cloud data of lidar B and C are synchronized to the time reference of lidar A: Time synchronization of lidar B point cloud data: At a certain scanning moment t of Radar B B,m , find the two scanning moments t of Radar A that are closest to it A,n and t A,n+1 , such that t A,n ≤t B,m ≤t A ,n + 1, that is, nT A ≤ mT B ≤(n + 1)T A , By determining the value of n ( indicating rounding down); Let the point cloud data of Radar B at time t B,m be P B,m . Under the time reference of Radar A, the corresponding point cloud data at times t A,n and t A,n+1 is calculated by linear interpolation; Let the coordinates of a certain point in the point cloud data be p = (x, y, z). At time t B,m of Radar B, the coordinates of this point are . After linear interpolation, the corresponding coordinates at time T A,n under the time reference of Radar A and the corresponding coordinates at time t A,n+1 are Time synchronization of the point cloud data of Radar C: Similar to Radar B, for a certain scanning moment t of Radar C C,k , find the two scanning moments t of Radar A that are closest to it A,n and t A,n+1 , and by determine the value of n; Let the point cloud data of Radar C at time t C,k be P C,k . The coordinates of a certain point in it are p = (x, y, z). At time T C,k the coordinates of this point are . After linear interpolation, the corresponding coordinates at time T A,n under the time reference of Radar A and the corresponding coordinates at time t A,n+1 are ; In this way, through the linear interpolation method, the point cloud data of Radar B and Radar C are synchronized in time to the time reference of Radar A; The specific steps of step S3 are as follows: Establish a three-dimensional space coordinate system, determine the origin and axis directions of the world coordinate system according to the initial position and attitude of the vehicle. For lidar A, lidar B, and lidar C, measure their installation positions and attitude parameters on the vehicle, such as installation angles, offset distances, etc. According to the real-time motion state information of the vehicle, calculate the positions and attitudes of each lidar relative to the world coordinate system at different times, and use the coordinate transformation formula to convert the point cloud data obtained by the three lidars from their respective local coordinate systems to the space coordinate system: Define relevant parameters: Let the three-dimensional coordinate system be 0-XYZ, with the initial position of the vehicle as the origin O of the three-dimensional coordinate system, the initial forward direction of the vehicle as the positive direction of the X-axis, and the vertically upward direction as the positive direction of the Z-axis. Determine the direction of the Y-axis according to the right-hand rule; For the lidar ( ), the installation position vector in the vehicle local coordinate system is , and the installation attitude is represented by the rotation matrix (which can be obtained by calculating the installation angle, such as converting to the rotation matrix using Euler angles or quaternions); Let the position vector of the vehicle at time be , and the attitude of the vehicle at time is represented by the rotation matrix (which can be calculated according to the attitude parameters such as the steering angle, pitch angle, and roll angle of the vehicle); Calculate the positions and attitudes of each lidar relative to the three-dimensional coordinate system at different times; The lidar At time The position vector relative to the three-dimensional coordinate system is: The lidar At time The attitude rotation matrix relative to the world coordinate system is: Coordinate transformation formula: Let the point cloud data point coordinates of the lidar in the local coordinate system be , and convert it to the homogeneous coordinate form ; After coordinate transformation, the homogeneous coordinates of this point in the world coordinate system are: Expand the calculation: Among them is the rotating part is the translation part, and then is converted to the non-homogeneous coordinate form to obtain the coordinates of the point in the world coordinate system , that is Through the above formula, according to the motion state information of the vehicle, the position and attitude of each lidar relative to the world coordinate system at different times can be calculated, and the point cloud data obtained by the three lidars can be converted from their respective local coordinate systems to the world coordinate system; The specific steps of step S4 are as follows: Assume that the vehicle speed at a certain moment is 20m / s, the acceleration is 2m / s², and the steering angle is 5°. According to this motion state information, calculate the displacement of the point cloud data in the translation direction. For each point in the point cloud data, calculate the coordinate change caused by the vehicle translation and rotation according to its position in the world coordinate system and the motion parameters of the vehicle, and adjust the coordinates of each point through the corresponding translation matrix and rotation matrix to achieve motion compensation; Calculate the displacement of the vehicle within Δt time: According to the uniform acceleration linear motion formula, the displacement of the vehicle within time is: In direction, the displacement components are respectively , , , assuming that the vehicle motion direction is along the positive direction of the axis, then: The translation matrix T is used to describe the translation transformation of points, and its form is: Express the point P in the form of homogeneous coordinates again , and the coordinates after translation transformation are: Rotation matrix, assuming that the vehicle rotates around the z-axis (the steering angle θ is the rotation angle around the z-axis), the rotation matrix R is: The coordinates after rotation transformation are: Obtain the coordinates after motion compensation: Convert Pr to inhomogeneous coordinate form to obtain the point coordinates after motion compensation p’=(x’, y’, z’); ; y’ = (x + x) + (y + y) ; The above is the model constructed based on the formula expression for calculating the coordinate change of point cloud data according to the vehicle motion state information and performing motion compensation; The specific step S5 is as follows: Adopt a method based on feature matching to fuse the point cloud data of three lidars after motion compensation: First, extract the feature points (such as edge points, corner points, etc.) in the point cloud data, and then calculate the distance and similarity between the feature points to match and fuse the point cloud data of different lidars to generate the final environmental point cloud model; Feature point extraction: Let for the lidar At time The point cloud data is , and the feature point set is proposed through a feature extraction algorithm (such as curvature calculation, normal estimation, etc.), where represents the th feature point of the lidar at time , is the number of feature points; Feature point distance calculation For the feature points and of different lidars (i, k = A, B, C and i ≠ k), calculate the Euclidean distance between them: where and are the coordinates of the feature points in the world coordinate system (after the previous coordinate transformation); Feature point similarity calculation: In addition to the distance, the similarity can also be calculated through other features , such as normal vector similarity, local geometric structure similarity, etc. Assuming that the normal vector is used to calculate the similarity, let and be the normal vectors of the feature points and , then the normal vector similarity can be expressed as: Feature point matching: By setting a distance threshold and a similarity threshold , find the matching feature point pairs. If , then the feature points and are considered to be matched; Point cloud fusion: For the matching feature point pairs, they can be fused into the final environmental point cloud model through weighted average or other fusion strategies . Assuming weighted average is used, for the matching feature point pairs and , the fused point is: where is the weight factor, , and the weight can be determined according to factors such as the reliability and distance of the feature points. For example, the weight can be set according to the inverse of the distance: Generation of the final environmental point cloud model: The final environmental point cloud model is composed of all the fused feature points and the unmatched but retained feature points: Through the above formula, feature extraction, feature point matching and fusion can be performed on the point cloud data of three lidars after motion compensation, and finally a complete environmental point cloud model is generated. Among them, feature point extraction, similarity calculation and fusion strategies can be optimized and adjusted according to specific situations, such as using more complex feature descriptors (such as SHOT, FPFH, etc.) and matching algorithms (such as ICP, NDT, etc.) to improve the degree and robustness of fusion.
[0014] Through the above specific embodiments, the present invention can effectively solve the problem of motion distortion of multi-source lidar point cloud data and improve the environmental perception accuracy of intelligent vehicles.
[0015] In summary, this is only a preferred embodiment of the present invention, and is not used to limit the scope of implementation of the present invention. All equivalent changes and modifications made according to the shape, structure, features and spirit of the claims of the present invention should be included within the scope of the claims of the present invention.
Claims
1. A motion compensation method for multi-source point clouds of an intelligent vehicle lidar, characterized in that: It includes the following steps: S1 Multi-source lidar data acquisition: Using multiple lidars installed on an intelligent vehicle, the surrounding environment is scanned in real time according to their respective scanning frequencies and time intervals to obtain multi-source point cloud data at different times; Record the timestamp information of each lidar and the real-time motion state information of the intelligent vehicle, including vehicle speed, acceleration, steering angle, etc.; S2 Lidar synchronization processing: According to the timestamp information of the lidars, the point cloud data obtained by different lidars at similar times are matched, and the point cloud data with time differences are corrected in time through an interpolation algorithm to synchronize the point cloud data of different lidars in time; S3 Spatial coordinate transformation of point cloud data: Establish a unified spatial coordinate system and transform the point cloud data obtained by each lidar from its respective local coordinate system to the world coordinate system; S4 Motion compensation calculation of point cloud data: According to the motion state information of the intelligent vehicle, motion compensation is performed on the point cloud data transformed into the spatial coordinate system; S5 Multi-source point cloud fusion: Adopt a method based on feature matching or distance measurement to match and fuse the point cloud data of different lidars to generate a more accurate and complete environmental point cloud model.
2. The motion compensation method for multi-source point clouds of an intelligent vehicle lidar according to claim 1, characterized in that: The step S3 includes calculating the position and attitude of each lidar relative to the world coordinate system at different times according to the real-time motion state information of the intelligent vehicle, and using a coordinate transformation matrix to accurately transform the point cloud data from the local coordinate system to the spatial coordinate system.
3. The motion compensation method for multi-source point clouds of an intelligent vehicle lidar according to claim 1, characterized in that: The step S4 includes, for the translational motion of the intelligent vehicle, by calculating the displacement of the point cloud data in the translational direction, the coordinates of each point are adjusted accordingly. For the rotational motion of the intelligent vehicle, according to the rotation angle and rotation axis, the coordinates of each point are rotationally compensated using a rotation matrix to eliminate the distortion of the point cloud data caused by the motion of the intelligent vehicle.
4. The motion compensation method for multi-source point clouds of an intelligent vehicle lidar according to claim 1, wherein: The step S1 is specifically as follows: Assume that three lidars are installed on the intelligent vehicle, namely lidar A, lidar B, and lidar C. During the vehicle's driving, the three lidars scan the surrounding environment at their respective scanning frequencies. For example, lidar A scans once every 0.1 s, lidar B scans once every 0.08 s, and lidar C scans once every 0.12 s to obtain point cloud data. At the same time, the vehicle's sensors record the motion state information such as vehicle speed, acceleration, and steering angle in real time, and add timestamps to the scanning data of each lidar; The step S2 is specifically as follows: Taking the timestamp of lidar A as the reference, for the point cloud data of lidar B and lidar C, through a linear interpolation algorithm, their timestamps are corrected to a time close to that of lidar A; Let the scanning period of radar A be T A , then the timestamp at the nth scan is t A,n = nT A (n = 0, 1, 2, …); The scanning period of Radar B is T B , and the timestamp at the m-th scan is t B,m = mT B (m = 0, 1, 2, …); The scanning period of radar C is Tc, and the timestamp at the k-th scan is t C,k = KT C (k = 0, 1, 2, …); Assume that the point cloud data of lidars B and C are synchronized to the time reference of lidar A: Time synchronization of lidar B point cloud data: A certain scanning moment t of radar B B,m , find the two scanning moments t of radar A that are closest to it A,n and t A,n+1 , such that t A,n ≤t B,m ≤t A , n + 1, that is, nT A ≤ mT B ≤(n + 1)T A , By determining the value of n ( representing rounding down); Let the point cloud data of radar B at time t B,m be P B,m . Under the time reference of radar A, the corresponding point cloud data at times t A,n and t A,n+1 is calculated by linear interpolation; Let the coordinates of a certain point in the point cloud data be p = (x, y, z). At time t of radar B B,m , the coordinates of this point are . After linear interpolation, the corresponding coordinates at time T A,n under the time reference of radar A and the corresponding coordinates at time t A,n+1 are Time synchronization of lidar C point cloud data: Similar to radar B, for a certain scanning moment t of radar C C,k , find the two scanning moments t A,n and t A,n+1 of radar A that are closest to it, and determine the value of n by ; Let the point cloud data of radar C at time t C,k be P C,k , and the coordinates of a certain point among them be p = (x, y, z). At time T C,k , the coordinates of this point are . After linear interpolation, the corresponding coordinates at time T A,n under the time reference of radar A and the corresponding coordinates at time t A,n+1 are ; In this way, through the linear interpolation method, the point cloud data of lidars B and C are synchronized in time to the time reference of lidar A; The specific steps of step S3 are as follows: Establish a three-dimensional space coordinate system, determine the origin and axis directions of the world coordinate system according to the initial position and attitude of the vehicle. For radar A, radar B, and radar C, measure their installation positions and attitude parameters on the vehicle, such as installation angles, offset distances, etc. According to the real-time motion state information of the vehicle, calculate the positions and attitudes of each lidar relative to the world coordinate system at different times. Using the coordinate transformation formula, transform the point cloud data obtained by the three lidars from their respective local coordinate systems to the space coordinate system: Define relevant parameters: Let the three-dimensional coordinate system be 0-XYZ. Take the initial position of the vehicle as the origin O of the three-dimensional coordinate system, the initial forward direction of the vehicle as the positive direction of the X axis, the vertically upward direction as the positive direction of the Z axis, and determine the direction of the Y axis according to the right-hand rule; For lidar ( ), the installation position vector in the vehicle local coordinate system is , and the installation attitude is represented by the rotation matrix (which can be calculated from the installation angles, such as converting to a rotation matrix using Euler angles or quaternions); Suppose the position vector of the vehicle at time is , and the attitude of the vehicle at time is represented by the rotation matrix (which can be calculated based on attitude parameters such as the steering angle, pitch angle, roll angle, etc.); Calculate the positions and attitudes of each lidar relative to the three-dimensional coordinate system at different times; LiDAR At the moment Position vector relative to the three-dimensional coordinate system is as follows: LiDAR At the moment The attitude rotation matrix relative to the world coordinate system is as follows: Coordinate transformation formula: Set lidar The coordinates of the point cloud data points in the local coordinate system are , and convert it into homogeneous coordinate form ; After coordinate transformation, the homogeneous coordinates of this point in the world coordinate system are as follows: Expand the calculation: Among them is the rotating part, is the translating part, and then convert into the non-homogeneous coordinate form to obtain the coordinates of this point in the world coordinate system , that is: Through the above formula, according to the motion state information of the vehicle, the positions and attitudes of each lidar relative to the world coordinate system at different times can be calculated, and the point cloud data obtained by the three lidars can be transformed from their respective local coordinate systems to the world coordinate system; The specific steps of step S4 are as follows: Assume that the vehicle speed at a certain moment is 20m / s, the acceleration is 2m / s², and the steering angle is 5°. According to this motion state information, calculate the displacement of the point cloud data in the translation direction. For each point in the point cloud data, calculate the coordinate changes caused by the vehicle translation and rotation according to its position in the world coordinate system and the motion parameters of the vehicle. Through the corresponding translation matrix and rotation matrix, adjust the coordinates of each point to achieve motion compensation; Calculate the displacement of the vehicle within Δt time: According to the formula for uniformly accelerated rectilinear motion, the displacement of the vehicle within time is: In the displacement components in the directions are respectively , , , assuming that the vehicle moving direction is along the positive direction of the axis, then: The translation matrix T is used to describe the translation transformation of points, and its form is: Express point P in the form of rectangular coordinates again , and the coordinates after the translation transformation are as follows: Rotation matrix. Assume that the vehicle rotates around the z axis (the steering angle θ is the rotation angle around the z axis), and the rotation matrix R is: Coordinates after rotation transformation are as follows: Obtain the coordinates after motion compensation: Convert Pr to the non-homogeneous coordinate form to obtain the point coordinates after motion compensation p’=(x’,y’,z’); ; y’= (x+x)+(y+y) ; The above is the model constructed by the formula expression for calculating the coordinate changes of the point cloud data according to the vehicle motion state information and performing motion compensation; The specific steps of step S5 are as follows: Adopt a method based on feature matching to fuse the point cloud data of the three lidars after motion compensation: First, extract the feature points (such as edge points, corner points, etc.) in the point cloud data, and then calculate the distances and similarities between the feature points to match and fuse the point cloud data of different lidars to generate the final environmental point cloud model; Feature point extraction: Suppose for lidar At time the point cloud data is , and a set of feature points is extracted through a feature extraction algorithm (such as curvature calculation, normal estimation, etc.), where represents the th feature point of the lidar at time , and is the number of feature points; Feature point distance calculation For the feature points of different lidars and (i, k = A, B, C and i ≠ k), calculate the Euclidean distance between them : Among them and They are the coordinates of the feature points in the world coordinate system (after the previous coordinate transformation); Feature point similarity calculation: In addition to distance, similarity can also be calculated based on other features , such as normal vector similarity, local geometric structure similarity, etc. Assuming that normal vectors are used to calculate similarity, let and be the feature points and be their normal vectors, then the normal vector similarity can be expressed as: Feature point matching: By setting a distance threshold and a similarity threshold , matching feature point pairs are found. If , it is considered that the feature points and are matched; Point cloud fusion: For the matched feature point pairs, they can be fused into the final environmental point cloud model through weighted averaging or other fusion strategies In it, assuming weighted averaging is used, for the matched feature point pairs and , the fused point is as follows: Among them is the weight factor, , and the weight can be determined according to factors such as the reliability and distance of the feature points. For example, the weight can be set according to the inverse of the distance: Generation of the final environmental point cloud model: Final environmental point cloud model is composed of all the fused feature points and the un-matched but retained feature points Composition: Through the above formula, feature extraction, feature point matching, and fusion can be performed on the point cloud data of the three lidars after motion compensation, and finally a complete environmental point cloud model can be generated.
5. The motion compensation method for multi-source point clouds of an intelligent vehicle lidar according to claim 4, characterized in that: In step S5, the feature point extraction, similarity calculation, and fusion strategy can be optimized and adjusted according to specific situations. For example, more complex feature descriptors (such as SHOT, FPFH, etc.) and matching algorithms (such as ICP, NDT, etc.) can be used to improve the degree and robustness of fusion.