Point cloud motion compensation method, device and electronic device

Abstract

The present invention provides a point cloud motion compensation method, device, and electronic device. These methods obtain each frame of point cloud and the corresponding multiple frames of raw measurement data. For each bundle of point clouds within each frame, the multiple frames of raw measurement data are interpolated to obtain interpolated measurement data corresponding to each laser point in the bundle of point clouds. Within the time period corresponding to each two adjacent frames of raw measurement data, the acceleration and angular velocity of the target vehicle in the interpolated measurement data corresponding to each two adjacent laser points increase uniformly over time. Motion compensation is then performed on the bundle of point clouds based on the interpolated measurement data. In this method, the acceleration and angular velocity of the target vehicle in the interpolated measurement data corresponding to each two adjacent laser points increase uniformly over time. This method of performing motion compensation and dedistortion on the point cloud using uniform acceleration minimizes distortion during the point cloud dedistortion process. This less distorted point cloud can improve the accuracy of subsequent positioning and mapping.

Description

Technical Field

[0001] The present invention relates to the field of autonomous driving technology, and in particular to a device and electronic equipment. Background Art

[0002] In autonomous driving systems, due to the advantages of lidar in measurement accuracy and detection distance, it plays a relatively important role in real-time positioning and mapping. Vehicles with dynamic changing characteristics will generate three-dimensional linear velocity, linear acceleration, angular velocity and angular acceleration when driving on the road, and the point cloud scanned by the lidar during movement will produce motion distortion. When mechanical radar calculates the coordinates of laser points, it generally uses the lidar coordinate system at the moment of receiving the laser beam as the standard. However, when the vehicle is driving, the reference coordinate axis of each point is different. In the existing technology, a uniform speed model is usually used as an assumption to calculate the transformation relationship between the lidar coordinate system at the moment of acquisition of all laser points in a frame of point cloud and the lidar coordinate system at the initial moment for systematic compensation. This method of using a uniform speed model for motion compensation will cause serious distortion in the point cloud de-distortion process, which in turn leads to inaccurate positioning and mapping. Summary of the Invention

[0003] The object of the present invention is to provide a point cloud motion compensation method, device and electronic device to improve the accuracy of point cloud dedistortion, thereby improving the accuracy of positioning and mapping.

[0004] The present invention provides a motion compensation method for a point cloud, comprising: obtaining each frame of a point cloud obtained by scanning a multi-beam laser radar, and multiple frames of original measurement data corresponding to each frame of the point cloud generation time period; wherein the multiple frames of original measurement data are collected by an inertial measurement unit; the multi-beam laser radar and the inertial measurement unit are arranged on a target vehicle; for each beam of point clouds in each frame of the point cloud, the following operations are performed: interpolating the multiple frames of original measurement data corresponding to the frame of the point cloud to obtain interpolated measurement data corresponding to each laser point in the beam of point clouds; wherein, in the time period corresponding to each two adjacent frames of original measurement data, in the interpolated measurement data corresponding to each two adjacent laser points, the acceleration and angular velocity of the target vehicle increase uniformly over time; based on the interpolated measurement data, motion compensation is performed on the beam of point clouds to obtain a motion-compensated point cloud corresponding to the beam of point clouds.

[0005] Furthermore, each bundle of point clouds in each frame of point cloud is obtained by traversing each frame of point cloud according to the line bundle to obtain each bundle of point clouds in the frame of point cloud; wherein the number of laser points contained in each bundle of point clouds is greater than the number of frames of the multiple frames of original measurement data corresponding to the frame of point cloud.

[0006] Furthermore, the step of interpolating the multiple frames of raw measurement data corresponding to the frame point cloud to obtain interpolated measurement data corresponding to each laser point in the bundle point cloud includes: performing the following operations on each two adjacent frames of raw measurement data in the multiple frames of raw measurement data corresponding to the frame point cloud: obtaining a first acquisition time and a second acquisition time corresponding to the two current adjacent frames of raw test data, wherein the first acquisition time is earlier than the second acquisition time; calculating the difference between the first acquisition time and the second acquisition time to obtain a first time difference; for each laser point in a time period corresponding to the two current adjacent frames of raw measurement data, calculating the difference between the acquisition time of the current laser point and the first acquisition time to obtain a second time difference; calculating the ratio of the second time difference to the first time difference to obtain a proportional coefficient corresponding to the current laser point; obtaining a first acceleration, a first angular velocity, a first velocity value, and a first position data corresponding to the target vehicle at the first acquisition time, and a second acceleration and a second angular velocity corresponding to the target vehicle at the second acquisition time; and determining the interpolated measurement data corresponding to each laser point in the bundle point cloud based on the proportional coefficient, the first acceleration, the first angular velocity, the first velocity value, the first position data, the second acceleration, and the second angular velocity.

[0007] Furthermore, the step of determining the interpolated measurement data corresponding to each laser point in the beam point cloud based on the proportional coefficient, the first acceleration, the first angular velocity, the first velocity value, the first position data, the second acceleration and the second angular velocity includes: determining the target acceleration corresponding to the current laser point based on the proportional coefficient, the first acceleration, the second acceleration, and the preset first zero bias; determining the target angular velocity corresponding to the current laser point based on the proportional coefficient, the first angular velocity, the second angular velocity, and the preset second zero bias; determining the rotation parameter corresponding to the current laser point based on the preset quaternion, the target angular velocity and the second time difference; determining the position parameter corresponding to the current laser point based on the first position data, the first velocity value, the second time difference and the first acceleration; and determining the interpolated measurement data corresponding to the current laser point based on the rotation parameter and the position parameter.

[0008] Furthermore, based on the interpolated measurement data, motion compensation is performed on the bundle point cloud to obtain a motion-compensated point cloud corresponding to the bundle point cloud, including the following steps: calculating the difference between the interpolated measurement data corresponding to the current laser point and the first original measurement data corresponding to the first acquisition time to obtain a first difference; determining a first rotation and translation matrix between the current laser point and the first original measurement data based on the first difference; repeatedly performing the step of calculating the difference between the acquisition time of the current laser point and the first acquisition time for each laser point in the time period corresponding to the two adjacent frames of original measurement data to obtain a second time difference, thereby obtaining a second rotation and translation matrix between the next laser point and the first original measurement data; determining a rotation and translation matrix between the current laser point and the next laser point based on the first rotation and translation matrix and the second rotation and translation matrix; and obtaining the motion-compensated point cloud corresponding to the bundle point cloud based on each rotation and translation matrix.

[0009] Furthermore, based on each rotation and translation matrix, the step of obtaining a motion-compensated point cloud corresponding to the beam point cloud includes: determining a target timestamp based on the output frequency of the multi-beam lidar and the output frequency of the inertial measurement unit; performing time integral processing on the interpolated measurement data corresponding to each laser point based on each rotation and translation matrix and the target timestamp to obtain the rotation and translation amount of each laser point converted to the target timestamp; performing rotation and translation processing on each laser point according to the corresponding rotation and translation amount to obtain a motion-compensated laser point corresponding to each laser point; and combining each motion-compensated laser point in the beam point cloud to obtain a motion-compensated point cloud corresponding to the beam point cloud.

[0010] Furthermore, based on the output frequency of the multi-beam lidar and the output frequency of the inertial measurement unit, the step of determining the target timestamp includes: selecting the output frequency with the smallest value among the output frequency of the multi-beam lidar and the output frequency of the inertial measurement unit as the target frequency; calculating the target period value based on the target frequency, and determining the target timestamp from the time period of generating the beam point cloud according to the target period value.

[0011] Furthermore, based on each rotation and translation matrix and the target timestamp, the interpolated measurement data corresponding to each laser point is time-integrated to obtain the rotation and translation amount of each laser point converted to the target timestamp, including the following steps: for each laser point, if the acquisition time corresponding to the laser point is less than the target timestamp, obtaining a first target laser point; wherein the acquisition time corresponding to the first target laser point is less than or equal to the target timestamp, and the first target laser point is the laser point closest to the target timestamp; obtaining a first rotation and translation matrix for converting the first target laser point to the target timestamp; calculating the product of each rotation and translation matrix from the laser point to the first target laser point to obtain a first result; calculating the product of the first result and the first rotation and translation matrix to obtain the rotation and translation amount corresponding to the laser point converted to the target timestamp.

[0012] Furthermore, the method also includes: if the acquisition time corresponding to the laser point is greater than the target timestamp, obtaining a second target laser point; wherein the acquisition time corresponding to the second target laser point is greater than or equal to the target timestamp, and the second target laser point is the laser point closest to the target timestamp; obtaining a second rotation and translation matrix for converting the second target laser point to the target timestamp; calculating the product of each rotation and translation matrix from the laser point to the second target laser point to obtain a second result; calculating the product of the second result and the second rotation and translation matrix to obtain the rotation and translation amount corresponding to the laser point converted to the target timestamp.

[0013] Furthermore, for each laser point in the time period corresponding to the two adjacent frames of original measurement data, the step of calculating the difference between the acquisition time of the current laser point and the first acquisition time to obtain the second time difference includes: starting from the starting laser point of the beam point cloud, sequentially selecting multiple thinned laser points at preset time intervals; taking each thinned laser point in the time period corresponding to the two adjacent frames of original measurement data as the current laser point in turn, calculating the difference between the acquisition time of the current laser point and the first acquisition time, and obtaining the second time difference.

[0014] Furthermore, the method also includes: combining the motion-compensated point clouds corresponding to each bundle of point clouds in the first frame point cloud to obtain the first frame compensated point cloud corresponding to the first frame point cloud after motion compensation, and determining the first frame compensated point cloud as the compensated point cloud to be spliced; obtaining the adjacent next frame point cloud, combining the motion-compensated point clouds corresponding to each bundle of point clouds in the next frame point cloud to obtain the next frame compensated point cloud corresponding to the next frame point cloud after motion compensation; performing matching calculation on the next frame compensated point cloud and the compensated point cloud to be spliced ​​to obtain the compensated rotation and translation amount of the next frame compensated point cloud relative to the compensated point cloud to be spliced; splicing the next frame compensated point cloud and the compensated point cloud to be spliced ​​according to the compensated rotation and translation amount to obtain the spliced ​​compensated point cloud; and determining the positioning result and mapping result of the target vehicle based on the spliced ​​compensated point cloud.

[0015] Furthermore, based on the spliced ​​compensated point cloud, the step of determining the positioning result and mapping result of the target vehicle includes: using the spliced ​​compensated point cloud as a new point cloud to be spliced, and repeatedly executing the step of obtaining the adjacent next frame point cloud until the compensated rotation and translation corresponding to each next frame compensated point cloud and the final spliced ​​compensated point cloud are obtained; based on the compensated rotation and translation corresponding to each next frame compensated point cloud, determining the positioning result of the target vehicle; and based on the final spliced ​​compensated point cloud, determining the mapping result.

[0016] The present invention provides a point cloud motion compensation device, which includes: an acquisition module for acquiring each frame of point cloud obtained by scanning with a multi-beam laser radar, and multiple frames of original measurement data corresponding to each frame of point cloud generation time period; wherein the multiple frames of original measurement data are collected by an inertial measurement unit; the multi-beam laser radar and the inertial measurement unit are arranged on a target vehicle; for each beam of point cloud in each frame of point cloud, the device also includes the following modules: an interpolation processing module for interpolating the multiple frames of original measurement data corresponding to the frame of point cloud to obtain interpolated measurement data corresponding to each laser point in the beam of point cloud; wherein, in the time period corresponding to each two adjacent frames of original measurement data, in the interpolated measurement data corresponding to each two adjacent laser points, the acceleration and angular velocity of the target vehicle increase uniformly over time; a motion compensation module for performing motion compensation on the beam of point cloud based on the interpolated measurement data to obtain a motion-compensated point cloud corresponding to the beam of point cloud.

[0017] The present invention provides an electronic device comprising a processor and a memory, wherein the memory stores machine-executable instructions that can be executed by the processor, and the processor executes the machine-executable instructions to implement any of the above-mentioned point cloud motion compensation methods.

[0018] The present invention provides a machine-readable storage medium, which stores machine-executable instructions. When the machine-executable instructions are called and executed by a processor, the machine-executable instructions prompt the processor to implement any of the above-mentioned point cloud motion compensation methods.

[0019] The present invention provides a point cloud motion compensation method, device, and electronic device. The method obtains each frame of point cloud obtained by multi-beam laser radar scanning, as well as multiple frames of raw measurement data generated during the generation time period of each frame of point cloud. For each bundle of point clouds within each frame of point cloud, the following operations are performed: interpolation processing is performed on the multiple frames of raw measurement data corresponding to the frame of point cloud to obtain interpolated measurement data corresponding to each laser point in the bundle of point cloud. In the time period corresponding to each adjacent frame of raw measurement data, the acceleration and angular velocity of the target vehicle in the interpolated measurement data corresponding to each adjacent pair of laser points increase uniformly over time. Based on the interpolated measurement data, motion compensation is performed on the bundle of point clouds to obtain a motion-compensated point cloud corresponding to the bundle of point clouds. In this method, the acceleration and angular velocity of the target vehicle in the interpolated measurement data corresponding to each adjacent pair of laser points increase uniformly over time. This method of performing motion compensation and dedistortion on the point cloud using a uniform acceleration method can minimize the degree of distortion during the point cloud dedistortion process. This point cloud with less distortion can improve the accuracy of subsequent positioning and mapping. BRIEF DESCRIPTION OF THE DRAWINGS

[0020] In order to more clearly illustrate the specific embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the specific embodiments or the description of the prior art. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0021] Figure 1 A point cloud image corresponding to a vehicle moving forward on an open road provided in an embodiment of the present invention;

[0022] Figure 2 A point cloud image corresponding to a vehicle moving forward in a closed scene provided by an embodiment of the present invention;

[0023] Figure 3 A flow chart of a point cloud motion compensation method provided by an embodiment of the present invention;

[0024] Figure 4 A schematic diagram of the relationship between a laser point and IMU raw measurement data provided by an embodiment of the present invention;

[0025] Figure 5 A schematic diagram of the relationship between another laser point and IMU raw measurement data provided by an embodiment of the present invention;

[0026] Figure 6 A schematic diagram of a time axis provided by an embodiment of the present invention;

[0027] Figure 7 A schematic diagram of an integral processing effect provided by an embodiment of the present invention;

[0028] Figure 8 A schematic diagram of an overall point cloud compensation solution provided by an embodiment of the present invention;

[0029] Figure 9 A schematic diagram of a de-distortion process provided by an embodiment of the present invention;

[0030] Figure 10 A schematic structural diagram of a point cloud motion compensation device provided by an embodiment of the present invention;

[0031] Figure 11 A schematic structural diagram of an electronic device provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0032] The following will clearly and completely describe the technical solutions of the present invention in conjunction with the embodiments. Obviously, the embodiments described are only some embodiments of the present invention, not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of the present invention.

[0033] Currently, in autonomous driving systems, lidar measurement accuracy and detection range play a more important role in real-time positioning and mapping than other sensors. According to the principles of mechanical radar, real-time means motion, and motion means distortion.

[0034] The working principle of a multi-beam mechanical radar, that is, the principle of generating a frame of a multi-beam point cloud, is as follows: multiple laser transmitters and receivers are arranged vertically on the mechanical radar. The beams of the multiple transmitters in the vertical direction will emit and collect laser information in a certain order at a fixed horizontal angle, and obtain one point at a time. The so-called orderly emission also means that the emission of multiple beams in the vertical direction is not performed simultaneously, and there will be some time difference between the beams. For a single beam, if the output frequency of the radar is 10Hz, the device will perform a mechanical scan within 100ms, continuously emitting and receiving laser beams during the rotation process, and storing the information of each laser point through the scanning angle and time. After the scan is completed, the point set obtained by all beam scans within 100ms will be packaged into a point cloud and output externally. This is the origin of a frame of point cloud.

[0035] The aforementioned issues concerning real-time positioning and mapping arise naturally. A vehicle with dynamic characteristics will generate three-dimensional linear velocity, linear acceleration, angular velocity, and angular acceleration when driving on the road. Imagine that a car moving forward at a constant speed of 60km / h (16.67m / s) will move forward 1.667m within the time it takes for a laser frame to be formed (100ms). However, the data provided by the laser point cloud is based on the assumption of a static vehicle coordinate system. Using point cloud data with an internal error of more than 1m for subsequent matching, positioning, and mapping algorithms will definitely result in suboptimal errors. See [1]. Figure 1 The point cloud corresponding to a forward-moving vehicle on an open road is shown in the figure. The vehicle moves forward on an open road, and a laser beam hits the ground. In the vehicle coordinate system (or the laser radar coordinate system at the time of point cloud frame reception, where the laser radar coordinate system usually takes the laser emission center as the coordinate system origin), the point cloud obtained will be as follows Figure 1 As shown in the left figure, the ground point cloud is closed, but in reality, in the world coordinate system, as the vehicle moves forward, the reflection point of the laser beam on the ground continues to extend forward, rather than being a closed shape. Figure 1 As shown in the right figure.

[0036] See also Figure 2 The point cloud corresponding to a forward-moving vehicle in a closed scene is shown in the figure. When a vehicle moves forward in a closed scene (surrounded by closed circular walls) and a laser beam hits an obstacle, the point cloud obtained in the vehicle coordinate system (or the laser radar coordinate system at the time of point cloud frame reception) will be as follows: Figure 2 As shown in the left figure, the obstacle point cloud is a non-closed curve. In reality, in the world coordinate system, as the vehicle moves forward and approaches the obstacle, the reflection point of the laser beam on the wall and the distance between the vehicle and the vehicle change. However, at a certain moment, the point cloud should be a closed shape, as shown in the figure below. Figure 2 As shown in the right figure.

[0037] When mechanical radar calculates the coordinates of laser points, it generally uses the laser radar coordinate system at the moment the laser beam is received as the standard. However, when the vehicle is driving, the reference coordinate axes of each column and each point are different. In the same frame point cloud, the process of unifying the coordinate system is particularly basic and important.

[0038] Existing technology systematically compensates for the relative motion of each laser beam reception moment and all radar points relative to the initial moment. In other words, the transformation is performed by calculating the transformation relationship between the coordinate system of all laser points in a frame of point cloud at the time of acquisition and the lidar coordinate system at the initial moment. A uniform velocity model is generally used as an assumption, and the processing process is as follows:

[0039] 1. Obtain the motion information of the carrier (vehicle) from the raw data, such as angular velocity, linear velocity, etc.

[0040] 2. Calculate relative angles and relative displacements: Obtain the time difference between the laser points and the starting time, and calculate the relative rotation angle and displacement of each laser point relative to the first laser point.

[0041] 3. Conversion completed.

[0042] The existing technology mainly has the following shortcomings:

[0043] 1. Using a uniform velocity model will cause distortion in point cloud dedistortion, resulting in inaccurate positioning and mapping.

[0044] 2. Converting the point cloud to the coordinate system of the first laser point will cause time asynchrony when the timestamp is fused with other sensors, affecting the subsequent fusion positioning.

[0045] 3. When performing motion compensation on a single point in a point cloud, the calculation will consume a lot of computing time and cannot guarantee both accuracy and real-time performance of the application.

[0046] Inaccurate positioning can cause the vehicle to have a distorted perception of itself in autonomous driving mode, leading to traffic accidents; and inaccurate mapping can cause the difference between the constructed high-precision map and the real environment to increase as the error increases over time, rendering the final map unusable.

[0047] Specifically, the above technical shortcomings will cause the following three problems in the real-time positioning and mapping of vehicles based on laser point clouds: (1) The original data of the vehicle as the carrier can generally obtain the acceleration and angular velocity information of the carrier through the inertial measurement unit (IMU). If the carrier is simply regarded as a uniform velocity model in the 10Hz point cloud information, it will cause serious distortion in the dedistortion process; (2) When fusing with IMU or other sensors, it is necessary to consider the different frame rates and timestamps of different sensors. Therefore, time synchronization processing must be performed while dedistorting. For example, the IMU engineering application is 100Hz and outputs in 0.01s, while the mechanical radar output frequency is 10Hz and outputs point cloud information in 1μs units. (3) The laser radar can obtain multiple IMU raw data in one scan. Considering the uniform acceleration motion as a model for motion compensation, the integration operation of each laser point will consume a lot of computing time, which cannot guarantee the real-time performance of the application.

[0048] Based on this, an embodiment of the present invention provides a method, device, and electronic device for motion compensation of a point cloud. This technology can be applied to scenarios where motion compensation of a point cloud is required.

[0049] To facilitate understanding of this embodiment, the point cloud motion compensation method disclosed in the embodiment of the present invention is first introduced in detail; Figure 3 As shown, the method includes the following steps:

[0050] Step S302: Acquire each frame of point cloud obtained by the multi-beam laser radar scan, and multiple frames of raw measurement data corresponding to the generation time period of each frame of point cloud; wherein the multiple frames of raw measurement data are collected by an inertial measurement unit; the multi-beam laser radar and the inertial measurement unit are set on the target vehicle.

[0051] A multi-beam lidar and an inertial measurement unit are usually installed on the target vehicle, wherein the multi-beam lidar is usually a mechanical radar with multiple laser transmitters and receivers arranged longitudinally, and the emitted beams are multi-line; each frame of point cloud scanned by the multi-beam lidar and the corresponding multiple frames of original measurement data within the time period of each frame of point cloud generation obtained by the inertial measurement unit can be obtained, wherein each frame of point cloud includes multiple beams.

[0052] For each bundle of point clouds in each frame, the following operations are performed:

[0053] Step S304: Interpolate the multiple frames of raw measurement data corresponding to the frame point cloud to obtain interpolated measurement data corresponding to each laser point in the beam point cloud; wherein, within the time period corresponding to each two adjacent frames of raw measurement data, in the interpolated measurement data corresponding to each two adjacent laser points, the acceleration and angular velocity of the target vehicle increase uniformly over time.

[0054] Since each frame of point cloud usually includes multiple beams of point clouds, and each beam of point cloud includes multiple laser points, and the number of frames of multiple frames of raw measurement data corresponding to each frame of point cloud is usually much less than the number of laser points, it is necessary to interpolate the multiple frames of raw measurement data to obtain interpolated measurement data corresponding to each laser point in each beam of point cloud. For example, a frame of point cloud includes 40 beams, each beam corresponds to 1800 laser points, and 1800 laser points correspond to 10 raw measurement data. By interpolating the 10 raw measurement data, 1800 raw measurement data are obtained, so that each interpolated raw measurement data corresponds to one laser point, that is, the raw measurement data and the laser points have a one-to-one correspondence. Among them, in the interpolated measurement data corresponding to each two adjacent laser points, the acceleration and angular velocity of the target vehicle increase uniformly over time, that is, the interpolation is performed using a uniform acceleration method.

[0055] Step S306 : performing motion compensation on the bundle point cloud based on the interpolated measurement data to obtain a motion compensated point cloud corresponding to the bundle point cloud.

[0056] According to the interpolated measurement data corresponding to the laser points in each beam of point cloud, motion compensation is performed on each beam of point cloud to obtain the motion-compensated point cloud corresponding to each beam of point cloud, thereby achieving dedistortion of the point cloud. Subsequently, the positioning result and mapping result of the target vehicle can be determined based on the motion-compensated point cloud.

[0057] The above-mentioned point cloud motion compensation method obtains each frame of point cloud obtained by multi-beam lidar scanning and multiple frames of raw measurement data corresponding to each frame of point cloud generation time period. For each bundle of point clouds in each frame of point cloud, the following operations are performed: the multiple frames of raw measurement data corresponding to the frame of point cloud are interpolated to obtain interpolated measurement data corresponding to each laser point in the bundle of point cloud; wherein, within the time period corresponding to each two adjacent frames of raw measurement data, the acceleration and angular velocity of the target vehicle in the interpolated measurement data corresponding to each two adjacent laser points increase uniformly over time; based on the interpolated measurement data, motion compensation is performed on the bundle of point clouds to obtain a motion-compensated point cloud corresponding to the bundle of point clouds. In this method, the acceleration and angular velocity of the target vehicle in the interpolated measurement data corresponding to each two adjacent laser points increase uniformly over time. This method of motion-compensating and dedistorting the point cloud using uniform acceleration can minimize the degree of distortion during the point cloud dedistortion process. Based on this less distorted point cloud, the accuracy of subsequent positioning and mapping can be improved.

[0058] An embodiment of the present invention provides another method for motion compensation of a point cloud, which is implemented based on the method of the above embodiment; the method includes the following steps:

[0059] Step 1: Obtain each frame of point cloud obtained by the multi-beam laser radar scan, and multiple frames of raw measurement data corresponding to the time period of each frame of point cloud generation; wherein the multiple frames of raw measurement data are collected by an inertial measurement unit; the multi-beam laser radar and the inertial measurement unit are set on the target vehicle.

[0060] For each bundle of point clouds in each frame, the following operations are performed:

[0061] Step 2: Interpolate the multiple frames of original measurement data corresponding to the frame point cloud to obtain interpolated measurement data corresponding to each laser point in the beam point cloud; wherein, within the time period corresponding to each two adjacent frames of original measurement data, in the interpolated measurement data corresponding to each two adjacent laser points, the acceleration and angular velocity of the target vehicle increase uniformly over time.

[0062] Each bundle of point clouds in each frame of point cloud is obtained by traversing each frame of point cloud as a line bundle to obtain each bundle of point clouds in that frame of point cloud; wherein the number of laser points contained in each bundle of point clouds is greater than the number of frames of raw measurement data corresponding to that frame of point cloud. For example, if the number of frames of raw measurement data corresponding to that frame of point cloud is 10, and each bundle of point clouds is obtained by traversing each frame of point cloud as a line bundle, each bundle of point clouds contains 1800 laser points, which is greater than the number of frames of raw measurement data.

[0063] The second step can be obtained by following steps 20 to 25:

[0064] Step 20: For each two adjacent frames of raw measurement data in the multiple frames of raw measurement data corresponding to the frame of point cloud, the following operations are performed:

[0065] A first acquisition time and a second acquisition time corresponding to two adjacent frames of original test data are obtained, wherein the first acquisition time is earlier than the second acquisition time.

[0066] Step 21: Calculate the difference between the first acquisition time and the second acquisition time to obtain a first time difference.

[0067] Step 22 : For each laser point in the time period corresponding to two adjacent frames of original measurement data, calculate the difference between the acquisition time of the current laser point and the first acquisition time to obtain a second time difference.

[0068] In actual implementation, due to the problem of longitudinal asynchrony between the time of each beam, it is necessary to traverse the system by beam, unitize the multi-beam laser point cloud, and process all beams separately. The specific application will be refined into the following three steps for processing:

[0069] a. IMU linear interpolation (the accelerometer measures acceleration acc, represented by a, and the gyroscope measures angular velocity gyr, represented by w).

[0070] 1) IMU measurement values ​​are:

[0071]

[0072] in, Represent the observed values ​​of acceleration and angular velocity respectively, and the superscript i indicates that they are in the IMU coordinate system; The quaternion q represents the conversion from the world coordinate system W to the IMU coordinate system. This value is usually a preset fixed value and can be obtained from the IMU; a W Indicates the true value of acceleration in the world coordinate system; g W Indicates the true value of gravitational acceleration in the world coordinate system; b a 、b g Represents the IMU accelerometer bias and gyroscope bias respectively; δ a , δ w are the noise of the accelerometer and the noise of the gyroscope respectively; w i Indicates the true value of the angular velocity in the IMU coordinate system.

[0073] The true value in the above formula is an unknown number, and the other values ​​are known numbers that can be obtained. For example, the observation value can be read from the sensor.

[0074] 2) If Figure 4The diagram shows the relationship between a laser point and the original IMU measurement data; the IMU value is linearly interpolated to obtain the vehicle's motion between adjacent laser points. Figure 4 In the figure, the black line represents the time axis; the first black circle and the third black circle are the IMU original measurement data of the mth frame and the m+1th frame; the black triangle represents the arrangement of the laser points received by the single-beam mechanical transmission on the time axis; the corresponding values ​​of the two adjacent frames of IMU original measurement data can be calculated based on the Lie group Lie algebra. That is, the rotation and translation matrix between two adjacent frames of IMU original measurement data. Similarly, the rotation and translation matrix between two adjacent laser points after linear interpolation can also be calculated. .

[0075] The model is established based on the Lie group and Lie algebra foundation. The model is a uniform acceleration model:

[0076]

[0077] Where ξ is the Lie algebra established The model contains a 3D vector posture θ and a 3D vector position p, where θ can be understood as a rotation parameter and p can be understood as a translation parameter; T is the rotation-translation relationship matrix, R is a 3*3 rotation matrix; θ′ is the modulus of θ, and n is the unit vector of θ R is obtained by the Rodrigues rotation formula; I 3×3 Represents the unit matrix; in this formula, T is the parameter to be obtained, and other parameters can be obtained based on the IMU original measurement data and the calculation process in the formula.

[0078] For the convenience of explanation, taking the current two adjacent frames of original test data as the mth frame original measurement data and the m+1th frame original measurement data as an example, the first acquisition time corresponding to the mth frame original measurement data can be obtained as t m Indicates that the second acquisition time corresponding to the original measurement data of the m+1th frame can be obtained by t m+1 Indicates that the difference between the two acquisition times is calculated, i.e., t m+1 -t m , and obtain the above-mentioned first time difference.

[0079] Since the number of laser points contained in each point cloud is greater than the number of frames of the original measurement data, there are multiple laser points in the time period corresponding to two adjacent frames of original measurement data. For each laser point in the time period corresponding to the mth frame of original measurement data and the m+1th frame of original measurement data, for example, the current laser point is represented by j, and the acquisition time of the current laser point is represented by t j As an example, calculate the acquisition time t of the current laser point j With the first acquisition time tm The difference between Δt and t j -t m , which is the second time difference mentioned above.

[0080] Step 23: Calculate the ratio of the second time difference to the first time difference to obtain a proportional coefficient corresponding to the current laser point.

[0081] Calculate the above △t=t j -t m With t m+1 -t m The ratio of the current laser point j is obtained. According to this method, we can get m With t m+1 The distance t of the current laser point j in the time interval m The ratio of moments.

[0082] Step 24 : Acquire the first acceleration, first angular velocity, first speed value, and first position data of the target vehicle corresponding to the first acquisition time, and the second acceleration and second angular velocity corresponding to the second acquisition time.

[0083] Step 25 : determining interpolated measurement data corresponding to each laser point in the beam point cloud based on the proportional coefficient, the first acceleration, the first angular velocity, the first velocity value, the first position data, the second acceleration, and the second angular velocity.

[0084] This step 25 can be obtained by following the steps 25A to 25E:

[0085] Step 25A: Determine the target acceleration corresponding to the current laser point based on the proportional coefficient, the first acceleration, the second acceleration, and the preset first zero bias.

[0086] Step 25B: Determine the target angular velocity corresponding to the current laser point based on the proportional coefficient, the first angular velocity, the second angular velocity, and the preset second zero bias.

[0087] Establish Lie algebra at t m The model at the moment is The m That is, the first original measurement data corresponding to the first acquisition time, where θ m Indicates that at t m The three-dimensional vector posture of the vehicle at time t (also called rotation parameter); m Indicates that at t m The three-dimensional vector position of the vehicle at time θ (also called translation parameter); m and p m Can be obtained directly; in t m With t m+1The acceleration change and angular velocity change in the time interval are considered to be uniform, then:

[0088]

[0089] Where a corresponds to the target acceleration mentioned above; r represents the distance t from the current laser point j m The ratio of the moments (corresponding to the above proportional coefficient); a m+1 Indicates that the target vehicle is at the second acquisition time t m+1 The corresponding vehicle acceleration (corresponding to the above-mentioned second acceleration); a m Indicates that the target vehicle is at the first acquisition time t m The corresponding vehicle acceleration when (corresponding to the first acceleration mentioned above); b a represents the IMU accelerometer zero bias (corresponding to the above-mentioned preset first zero bias); w corresponds to the above-mentioned target angular velocity; w m+1 Indicates that the target vehicle is at the second acquisition time t m+1 The corresponding vehicle angular velocity when (corresponding to the second angular velocity mentioned above); w m Indicates that the target vehicle is at the first acquisition time t m The corresponding vehicle angular velocity when (corresponding to the first angular velocity mentioned above); b g Indicates the gyroscope bias (corresponding to the above-mentioned preset second bias).

[0090] Among them, a m+1 and a m are all true values ​​and can be calculated using the above formula (1).

[0091] Step 25C: Determine the rotation parameter corresponding to the current laser point based on the preset quaternion, the target angular velocity, and the second time difference.

[0092] Step 25D: determining a position parameter corresponding to the current laser point based on the first position data, the first velocity value, the second time difference, and the first acceleration.

[0093] Step 25E: Determine the interpolated measurement data corresponding to the current laser point based on the rotation parameter and the position parameter.

[0094] t m+1 The Lie algebra model of the moment is Among them, θ j represents the rotation parameter corresponding to the current laser point j; p j represents the position parameter corresponding to the current laser point j; q m is the attitude representation of quaternion; is a quaternion operator; w represents the target angular velocity; the above △t represents the second time difference; p m Indicates that the target vehicle is at the first acquisition time t mThe corresponding vehicle position at time v (corresponding to the first position data above); m Indicates that the target vehicle is at the first acquisition time t m The corresponding vehicle speed (corresponding to the above-mentioned first speed value) can be obtained from the IMU original measurement data; a represents the target acceleration.

[0095] The j It is calculated using the above target acceleration a and target angular velocity w, and corresponds to the IMU data corresponding to the j-th laser point, that is, the interpolated measurement data corresponding to the current laser point j.

[0096] Step three: Based on the interpolated measurement data, motion compensation is performed on the bundle point cloud to obtain a motion compensated point cloud corresponding to the bundle point cloud.

[0097] The step three can be specifically obtained by following steps 30 to 34:

[0098] Step 30, calculating the difference between the interpolated measurement data corresponding to the current laser point and the first original measurement data corresponding to the first acquisition time to obtain a first difference;

[0099] Will j With ξ m You can get it by making a difference

[0100]

[0101] in, Indicates the rotation parameter corresponding to the current laser point j and t m The difference matrix between the rotation parameters of the vehicle at time; Indicates the translation parameter corresponding to the current laser point j and t m The difference matrix between the translation parameters of the vehicle at time .

[0102] Step 31, determining a first rotation and translation matrix between the current laser point and the first original measurement data based on the first difference;

[0103] Step 32, repeatedly performing the step of calculating the difference between the acquisition time of the current laser point and the first acquisition time for each laser point in the time period corresponding to the two adjacent frames of raw measurement data to obtain a second time difference, and obtaining a second rotation and translation matrix between the next laser point and the first raw measurement data;

[0104] Step 33, determining a rotation and translation matrix between the current laser point and the next laser point based on the first rotation and translation matrix and the second rotation and translation matrix;

[0105] According to Lie algebra, we can get That is the first rotation and translation matrix mentioned above. Similarly, we can get That is the second rotation and translation matrix mentioned above. The rotation and translation matrix between the current laser point and the next laser point can be expressed as Get, such as Figure 5 Another schematic diagram of the relationship between laser points and IMU raw measurement data is shown.

[0106] Step 34 : Based on each rotation and translation matrix, obtain the motion compensated point cloud corresponding to the bundle of point clouds.

[0107] This step 34 can be achieved by following the steps 34A to 34D:

[0108] Step 34A: Determine the target timestamp based on the output frequency of the multi-beam laser radar and the output frequency of the inertial measurement unit.

[0109] This step 34A can be obtained by following the steps 34A(1) to 34A(2):

[0110] Step 34A(1), selecting the output frequency of the multi-beam laser radar and the output frequency of the inertial measurement unit with the smallest value as the target frequency;

[0111] Step 34A(2) calculates a target period value based on the target frequency, and determines a target timestamp from the time period of generating the beam point cloud according to the target period value.

[0112] For example, the IMU output frequency is 100 Hz, and the output is 0.01 points; the multi-beam lidar output frequency is 10 Hz, and the output is not 0.01 points. Since the output frequency of the multi-beam lidar is lower than the IMU output frequency, the multi-beam lidar output frequency of 10 Hz can be selected as the target frequency, and the target period value corresponding to this target frequency is 0.1s.

[0113] See also Figure 6 A timeline diagram is shown in Figure 1; the black line is the timeline, the black dots above the timeline are the IMU outputs (e.g., output timestamps: 1.17s, 1.18s, 1.19s, ..., 1.26s); the black dots below the timeline are the start and end times of a frame of radar scan (e.g., original output timestamps: 1.172s & 1.261s), and the only hourly timestamp t in units of 0.1s within 100ms from the start to the end of the frame of radar scan is found. target ,for example, Figure 6 The target timestamp in the example is 1.2s. This embodiment uses 0.1s as the integral point to remove distortion and synchronize with the IMU, change the output time of the radar, and provide the synchronized laser odometry for later positioning fusion.

[0114] Step 34B: Based on each rotation and translation matrix and the target timestamp, the interpolated measurement data corresponding to each laser point is time-integrated to obtain the rotation and translation amount of each laser point converted to the target timestamp;

[0115] This step 34B can be obtained by following the steps 34B(1) to 34B(8):

[0116] Step 34B(1): for each laser point, if the acquisition time corresponding to the laser point is less than the target timestamp, obtain a first target laser point; wherein the acquisition time corresponding to the first target laser point is less than or equal to the target timestamp, and the first target laser point is the laser point closest to the target timestamp;

[0117] Step 34B(2), obtaining a first rotation and translation matrix for converting the first target laser point to the target timestamp;

[0118] Step 34B(3), calculating the product of each rotation and translation matrix between the laser point and the first target laser point to obtain a first result;

[0119] Step 34B(4): Calculate the product of the first result and the first rotation and translation matrix to obtain the rotation and translation amount corresponding to the laser point converted to the target timestamp.

[0120] Step 34B(5): if the acquisition time corresponding to the laser point is greater than the target timestamp, obtain a second target laser point; wherein the acquisition time corresponding to the second target laser point is greater than or equal to the target timestamp, and the second target laser point is the laser point closest to the target timestamp;

[0121] Step 34B(6), obtaining a second rotation and translation matrix for converting the second target laser point to the target timestamp;

[0122] Step 34B(7), calculating the product of each rotation and translation matrix between the laser point and the second target laser point to obtain a second result;

[0123] Step 34B(8): Calculate the product of the second result and the second rotation and translation matrix to obtain the rotation and translation amount corresponding to the laser point converted to the target timestamp.

[0124] The following can be processed by integration to get the time integral, such as Figure 7 The following is a schematic diagram of the effect of the hourly processing. Specifically, the hourly processing can be performed using the following formula:

[0125]

[0126] Among them, in formula (1) of formula (5), when the acquisition time t corresponding to the laser point j is j Less than the target timestamp ttarget When j' is t j’ is less than or equal to t target The maximum value of (corresponding to the first target laser point above); Represents the rotation and translation matrix of laser point i to laser point i+1; Represents the first rotation and translation matrix of the first target laser point converted to the target timestamp; In formula (5), (2) when the acquisition time t corresponding to the laser point j is j Less than the target timestamp t target When j' is t j’ is greater than or equal to t target The minimum value of (corresponding to the second target laser point); Represents the rotation and translation matrix of laser point i to laser point i-1; represents the second rotation and translation matrix of the second target laser point converted to the target timestamp; π represents the continuous product.

[0127] From the overall technical level, the more precise the integral of the above integral is, the closer the point cloud after motion compensation will be to the real situation. In the process of program running, it is not difficult to find that the time difference between two adjacent laser points on the same beam is at the microsecond level. In engineering applications, the difference between microsecond-level integration and millisecond-level integration is not significant. Instead, it affects the computational time and increases the amount of computation. Therefore, it is necessary to upgrade the microsecond-level integration to the millisecond-level integration. The specific operation is as follows:

[0128] The above step 22 can be obtained by following the steps 22A to 22B:

[0129] Step 22A: Starting from the starting laser point of the beam point cloud, a plurality of thinned laser points are sequentially selected at preset time intervals.

[0130] Step 22B: taking each thinned-out laser point in the time period corresponding to two adjacent frames of original measurement data as the current laser point, and calculating the difference between the acquisition time of the current laser point and the first acquisition time to obtain a second time difference.

[0131] After following the above steps, the formula for converting the integral into an integer can be as follows:

[0132]

[0133]

[0134] The above formula means taking a point for calculation every 50ms For example, there are 100 laser points within 50ms, and these 100 laser points use the same The method corresponding to this formula is a further optimization of the method corresponding to formula (5), wherein i represents any laser point i among the laser points, i' is a point 50ms away from i; t j represents the time corresponding to the jth laser point; t i represents the time corresponding to laser point i; t i’ represents the time corresponding to the laser point i' which is 50ms away from the laser point i; t target Indicates the time corresponding to the target timestamp; Indicates that when the time point corresponding to the target timestamp is less than 50ms away from the laser point i, the position i' in the queue where the target is located is taken target .

[0135] By using the thinning method, the system's computing time can be kept at a level that can be used in real-time engineering applications while ensuring accuracy.

[0136] Step 34C, performing rotation and translation processing on each laser point according to the corresponding rotation and translation amount, thereby obtaining a motion-compensated laser point corresponding to each laser point;

[0137] Step 34D: combine each motion-compensated laser point in the bundle of point clouds to obtain a motion-compensated point cloud corresponding to the bundle of point clouds.

[0138] After calculating the rotation and translation of each laser point to the target timestamp, each laser point can be processed according to its corresponding rotation and translation to obtain the true value of the laser point after removing the distortion and rounding. Specifically, Figure 1 For example, each laser point can be Multiplying by the position of the laser point in the left picture, we can get Figure 1 The dedistorted pose of each laser point in the right figure is shown in Figure 1. By combining each dedistorted laser point in the point cloud, we can get the motion compensated point cloud.

[0139] Step 4: combining the motion-compensated point clouds corresponding to each bundle of point clouds in the first frame point cloud to obtain the first frame compensated point cloud corresponding to the first frame point cloud, and determining the first frame compensated point cloud as the compensated point cloud to be spliced;

[0140] Step 5: Obtain the adjacent next frame point cloud, combine the motion compensated point clouds corresponding to each bundle of point clouds in the next frame point cloud, and obtain the next frame compensated point cloud corresponding to the next frame point cloud;

[0141] Step 6: Perform matching calculation on the next frame compensation point cloud and the compensation point cloud to be spliced, and obtain the compensation rotation and translation of the next frame compensation point cloud relative to the compensation point cloud to be spliced;

[0142] Step 7: According to the compensation rotation and translation amount, the next frame compensation point cloud is spliced ​​with the compensation point cloud to be spliced ​​to obtain the spliced ​​compensation point cloud;

[0143] Step 8: Based on the spliced ​​compensated point cloud, determine the positioning result and mapping result of the target vehicle.

[0144] This step eight can be obtained by following the steps 80 to 82:

[0145] Step 80: Using the spliced ​​compensated point cloud as a new point cloud to be spliced, repeatedly performing the step of obtaining the adjacent next frame point cloud until the compensated rotation and translation corresponding to each next frame of compensated point cloud and the final spliced ​​compensated point cloud are obtained;

[0146] Step 81, determining the positioning result of the target vehicle based on the compensation rotation and translation corresponding to each next frame compensation point cloud;

[0147] Step 82: Determine the mapping result based on the final spliced ​​compensated point cloud.

[0148] like Figure 8 The figure shows a schematic diagram of an overall point cloud compensation solution. Based on the different frame rates of the mechanical lidar and IMU installed on the vehicle, the real-time positioning and mapping solution is determined. The specific steps are as follows:

[0149] 1. Determine the output frequency of the lidar and IMU information. Specifically, when the lidar and IMU are installed on the vehicle, these output frequencies are inherent information that can be directly obtained. The IMU frequency of different vehicles may be different. Taking 100Hz IMU raw data and 10Hz point cloud data as the input source as an example, the generation of one frame of point cloud corresponds to the update of at least 10 frames of IMU raw data. The IMU data memory is used to temporarily package all IMU data containing acceleration and angular velocity information that can be used for motion compensation in the point cloud generation stage (for n frames of IMU data, where 1≤m≤n, 10≤n, where m represents the mth frame of IMU data and n represents a total of n frames of IMU data) for temporary storage. A single-frame laser point cloud (one beam scan generates N laser points in one circle. For a multi-beam lidar, the single-frame laser point cloud is multi-beam) and the IMU raw data set are sent for dedistortion processing. Among them, for the current frame point cloud, the IMU data storage device stores the IMU data of the current frame point cloud generation stage and all IMU data of the multi-frame point cloud generation stage before the current frame point cloud.

[0150] 2. Determine whether the system is initialized. A system can be understood as a positioning and mapping system, such as a laser odometry. If it is not initialized, the initialization system must be entered to obtain the initial velocity of the carrier (usually the vehicle itself) in the laser radar coordinate system at the current moment to initialize the velocity. The current moment usually refers to the moment when the system begins operation. The initial velocity can be obtained from the IMU. Generally, vehicles start from a standstill, and the initial velocity is usually 0. The first frame's motion-compensated point cloud image is then saved as a local map for inter-frame matching, which is required for subsequent odometry. The laser odometry is then initialized, specifically the rotation and translation parameters. The rotation parameters are initialized to the unit matrix (assuming no rotation), and the translation parameters are initialized to [0, 0, 0]. Initialization is determined when the system begins operation. After initialization, all subsequent frames will no longer undergo this process.

[0151] 3. Input the second frame of laser point cloud and the corresponding IMU raw measurement data to start updating the system: First, the current dedistorted point cloud frame is matched with the previously obtained local map to calculate the laser odometry relative to the first frame. The matching calculation process can be implemented using existing technologies, such as comparing the similarity of the current frame point cloud image with the previous frame point cloud image. Different moments correspond to different moments of the entire laser odometry, which can also be understood as different frame times. For example, if a total of 10 frames are output, the laser odometry will output the rotation and translation from frame 0 to frame 1, and from frame 1 to frame 2. Then, based on the laser odometry results, the motion-compensated and time-synchronized point cloud image of the current frame is rotated and translated to the spliced ​​map in the laser radar coordinate system of the first frame. The spliced ​​point cloud image is used as the matching reference for the next frame to update the local map. The spliced ​​point cloud image is the point cloud image converted to the laser radar coordinate system of the first frame. The map and odometry are then output. The map is a point cloud map, and the odometry is a laser odometry. The laser odometry is used as the front-end positioning result, and the point cloud map is used as the mapping result. The laser odometry includes the corresponding rotation and translation values ​​when matching each frame of the point cloud. The entire positioning and mapping process includes front-end and back-end optimization. The front-end positioning result is a rough vehicle positioning result. After back-end optimization processing, the final mapping is obtained from the back-end. The front-end positioning result is the vehicle positioning result desired in this solution.

[0152] See also Figure 9The schematic diagram of a de-distortion process shown in FIG. 1 is that after determining the target hour time ttarget, the line bundle is traversed to obtain a single line bundle. The single line bundle, single-frame laser point cloud and IMU raw data set are input into the IMU linear interpolator in the system to obtain an IMU interpolation data set, where each laser point has its corresponding IMU data, including the time, velocity, acceleration, angular velocity and other data information corresponding to each laser point; based on this information, the IMU interpolation data set is calculated. Points obtained This completes the dedistortion of all points and finally outputs the time-synchronized motion-compensated point cloud.

[0153] The aforementioned point cloud motion compensation method uses high-frequency IMU raw measurement data to perform motion compensation on the lower-output-frequency radar laser point cloud to remove distortion, thereby improving overall positioning and mapping results, especially when the vehicle is accelerating forward, driving at high speed, or turning. Based on the laser information and sensor characteristics of each frame of the intelligent driving system environment, a knowledge base is constructed to output a corresponding uniform acceleration motion model. This model is then used to calculate the true state of each frame of the point cloud in the world coordinate system. This state is then used as input for the odometry calculation and point cloud map stitching process, resulting in expected real-time positioning and mapping results. Compared to the classic uniform velocity model point cloud distortion removal method, the positioning and mapping results are improved.

[0154] Furthermore, laser odometry is generated by inter-frame matching of the time-synchronized point cloud. This fusion improves positioning performance. The system's computational time remains within a range suitable for real-time engineering applications while maintaining accuracy. This approach fully leverages the diverse knowledge related to laser point clouds and inertial measurement units, increasing the utilization of raw data in the system while balancing the accuracy and computational speed requirements of engineering applications. This approach improves real-time vehicle positioning and mapping using laser point clouds.

[0155] The embodiment of the present invention provides a point cloud motion compensation device, such as Figure 10As shown, the device includes: an acquisition module 100, which is used to acquire each frame of point cloud obtained by scanning with a multi-beam laser radar, and multiple frames of original measurement data corresponding to each frame of point cloud generation time period; wherein the multiple frames of original measurement data are collected by an inertial measurement unit; the multi-beam laser radar and the inertial measurement unit are arranged on a target vehicle; for each beam of point cloud in each frame of point cloud, the device also includes the following modules: an interpolation processing module 101, which is used to perform interpolation processing on the multiple frames of original measurement data corresponding to the frame of point cloud, and obtain interpolated measurement data corresponding to each laser point in the beam of point cloud; wherein, in the time period corresponding to each two adjacent frames of original measurement data, in the interpolated measurement data corresponding to each two adjacent laser points, the acceleration and angular velocity of the target vehicle increase uniformly over time; a motion compensation module 102, which is used to perform motion compensation on the beam of point cloud based on the interpolated measurement data, and obtain a motion-compensated point cloud corresponding to the beam of point cloud.

[0156] The aforementioned point cloud motion compensation device obtains each frame of point cloud obtained by multi-beam laser radar scanning and multiple frames of raw measurement data corresponding to each frame of point cloud generation time period. For each bundle of point clouds in each frame of point cloud, the following operations are performed: interpolation processing is performed on the multiple frames of raw measurement data corresponding to the frame of point cloud to obtain interpolated measurement data corresponding to each laser point in the bundle of point cloud; wherein, within the time period corresponding to each two adjacent frames of raw measurement data, the acceleration and angular velocity of the target vehicle in the interpolated measurement data corresponding to each two adjacent laser points increase uniformly over time; and based on the interpolated measurement data, motion compensation is performed on the bundle of point clouds to obtain a motion-compensated point cloud corresponding to the bundle of point clouds. In this device, the acceleration and angular velocity of the target vehicle in the interpolated measurement data corresponding to each two adjacent laser points increase uniformly over time. This method of performing motion compensation and dedistortion on the point cloud using a uniform acceleration method can minimize the degree of distortion during the point cloud dedistortion process. The point cloud with less distortion can improve the accuracy of subsequent positioning and mapping.

[0157] Furthermore, it also includes a single-beam point cloud acquisition module, and each beam of point cloud in each frame of point cloud is obtained through the single-beam point cloud acquisition module: for each frame of point cloud, the frame of point cloud is traversed according to the line beam to obtain each beam of point cloud in the frame of point cloud; wherein the number of laser points contained in each beam of point cloud is more than the number of frames of the multi-frame original measurement data corresponding to the frame of point cloud.

[0158] Furthermore, the interpolation processing module 101 is further configured to: perform the following operations on each of two adjacent frames of raw measurement data in the multiple frames of raw measurement data corresponding to the frame point cloud: obtain a first acquisition time and a second acquisition time corresponding to the two current adjacent frames of raw test data, respectively; wherein the first acquisition time is earlier than the second acquisition time; calculate the difference between the first acquisition time and the second acquisition time to obtain a first time difference; for each laser point in the time period corresponding to the two current adjacent frames of raw measurement data, calculate the difference between the acquisition time of the current laser point and the first acquisition time to obtain a second time difference; calculate the ratio of the second time difference to the first time difference to obtain a proportional coefficient corresponding to the current laser point; obtain a first acceleration, a first angular velocity, a first velocity value, and a first position data corresponding to the target vehicle at the first acquisition time, as well as a second acceleration and a second angular velocity corresponding to the target vehicle at the second acquisition time; and determine the interpolated measurement data corresponding to each laser point in the beam point cloud based on the proportional coefficient, the first acceleration, the first angular velocity, the first velocity value, the first position data, the second acceleration, and the second angular velocity.

[0159] Furthermore, the interpolation processing module 101 is also used to: determine the target acceleration corresponding to the current laser point based on the proportional coefficient, the first acceleration, the second acceleration, and the preset first zero bias; determine the target angular velocity corresponding to the current laser point based on the proportional coefficient, the first angular velocity, the second angular velocity, and the preset second zero bias; determine the rotation parameter corresponding to the current laser point based on the preset quaternion, the target angular velocity and the second time difference; determine the position parameter corresponding to the current laser point based on the first position data, the first velocity value, the second time difference and the first acceleration; and determine the interpolation measurement data corresponding to the current laser point based on the rotation parameter and the position parameter.

[0160] Furthermore, the interpolation processing module 101 is further configured to: calculate the difference between the interpolated measurement data corresponding to the current laser point and the first original measurement data corresponding to the first acquisition time to obtain a first difference; determine a first rotation and translation matrix between the current laser point and the first original measurement data based on the first difference; repeatedly perform the step of calculating the difference between the acquisition time of the current laser point and the first acquisition time for each laser point in the time period corresponding to the two adjacent frames of original measurement data to obtain a second time difference, thereby obtaining a second rotation and translation matrix between the next laser point and the first original measurement data; determine a rotation and translation matrix between the current laser point and the next laser point based on the first rotation and translation matrix and the second rotation and translation matrix; and obtain a motion-compensated point cloud corresponding to the beam point cloud based on each rotation and translation matrix.

[0161] Furthermore, the interpolation processing module 101 is also used to: determine the target timestamp based on the output frequency of the multi-beam laser radar and the output frequency of the inertial measurement unit; perform time integral processing on the interpolation measurement data corresponding to each laser point based on each rotation and translation matrix and the target timestamp to obtain the rotation and translation amount of each laser point converted to the target timestamp; perform rotation and translation processing on each laser point according to the corresponding rotation and translation amount to obtain the motion compensated laser point corresponding to each laser point; combine each motion compensated laser point in the beam point cloud to obtain the motion compensated point cloud corresponding to the beam point cloud.

[0162] Furthermore, the interpolation processing module 101 is also used to: select the output frequency with the smallest value among the output frequency of the multi-beam laser radar and the output frequency of the inertial measurement unit as the target frequency; calculate the target period value based on the target frequency, and determine the target timestamp from the time period of generating the beam point cloud according to the target period value.

[0163] Furthermore, the interpolation processing module 101 is further configured to: for each laser point, if the acquisition time corresponding to the laser point is less than the target timestamp, obtain a first target laser point; wherein the acquisition time corresponding to the first target laser point is less than or equal to the target timestamp, and the first target laser point is the laser point closest to the target timestamp; obtain a first rotation and translation matrix for converting the first target laser point to the target timestamp; calculate the product of each rotation and translation matrix between the laser point and the first target laser point to obtain a first result; and calculate the product of the first result and the first rotation and translation matrix to obtain a rotation and translation amount corresponding to the laser point converted to the target timestamp.

[0164] Furthermore, the interpolation processing module 101 is further configured to: if the acquisition time corresponding to the laser point is greater than the target timestamp, obtain a second target laser point; wherein the acquisition time corresponding to the second target laser point is greater than or equal to the target timestamp, and the second target laser point is the laser point closest to the target timestamp; obtain a second rotation and translation matrix for converting the second target laser point to the target timestamp; calculate the product of each rotation and translation matrix between the laser point and the second target laser point to obtain a second result; and calculate the product of the second result and the second rotation and translation matrix to obtain a rotation and translation amount corresponding to the laser point converted to the target timestamp.

[0165] Furthermore, the interpolation processing module 101 is also used to: start from the starting laser point of the beam point cloud, select multiple thinned laser points in sequence according to a preset time interval; take each thinned laser point in the time period corresponding to the current two adjacent frames of original measurement data as the current laser point, calculate the difference between the acquisition time of the current laser point and the first acquisition time, and obtain a second time difference.

[0166] Furthermore, the device is also used to: combine the motion-compensated point clouds corresponding to each bundle of point clouds in the first frame point cloud to obtain the first frame compensated point cloud corresponding to the first frame point cloud after motion compensation, and determine the first frame compensated point cloud as the compensated point cloud to be spliced; obtain the adjacent next frame point cloud, combine the motion-compensated point clouds corresponding to each bundle of point clouds in the next frame point cloud to obtain the next frame compensated point cloud corresponding to the next frame point cloud after motion compensation; perform matching calculation on the next frame compensated point cloud and the compensated point cloud to be spliced ​​to obtain the compensated rotation and translation amount of the next frame compensated point cloud relative to the compensated point cloud to be spliced; splice the next frame compensated point cloud and the compensated point cloud to be spliced ​​according to the compensated rotation and translation amount to obtain the spliced ​​compensated point cloud; and determine the positioning result and mapping result of the target vehicle based on the spliced ​​compensated point cloud.

[0167] Furthermore, the device is also used to: use the spliced ​​compensated point cloud as a new point cloud to be spliced, and repeatedly execute the step of obtaining the adjacent next frame point cloud until the compensated rotation and translation corresponding to each next frame compensated point cloud and the final spliced ​​compensated point cloud are obtained; determine the positioning result of the target vehicle based on the compensated rotation and translation corresponding to each next frame compensated point cloud; and determine the mapping result based on the final spliced ​​compensated point cloud.

[0168] The point cloud motion compensation device provided in the embodiment of the present invention has the same implementation principle and technical effects as those in the aforementioned point cloud motion compensation method embodiment. For the sake of brief description, for matters not mentioned in the embodiment of the point cloud motion compensation device, reference may be made to the corresponding content in the aforementioned point cloud motion compensation method embodiment.

[0169] The embodiment of the present invention further provides an electronic device, see Figure 11 As shown, the electronic device includes a processor 130 and a memory 131 , wherein the memory 131 stores machine executable instructions that can be executed by the processor 130 , and the processor 130 executes the machine executable instructions to implement the above-mentioned point cloud motion compensation method.

[0170] Furthermore, Figure 11 The electronic device shown further includes a bus 132 and a communication interface 133 , and the processor 130 , the communication interface 133 and the memory 131 are connected via the bus 132 .

[0171] The memory 131 may include a high-speed random access memory (RAM), and may also include a non-volatile memory, such as at least one disk storage. The communication connection between the system network element and at least one other network element is achieved through at least one communication interface 133 (which may be wired or wireless), and the Internet, wide area network, local area network, metropolitan area network, etc. may be used. The bus 132 may be an ISA bus, a PCI bus, or an EISA bus. The bus can be divided into an address bus, a data bus, a control bus, etc. For ease of representation, Figure 11 Only one bidirectional arrow is used in the diagram, but this does not mean that there is only one bus or one type of bus.

[0172] The processor 130 may be an integrated circuit chip with signal processing capabilities. During implementation, each step of the above method can be completed by hardware integrated logic circuits in the processor 130 or by software instructions. The above processor 130 may be a general-purpose processor, including a central processing unit (CPU), a network processor (NP), etc.; it may also be a digital signal processor (DSP), an application-specific integrated circuit (ASIC), a field-programmable gate array (FPGA), or other programmable logic devices, discrete gate or transistor logic devices, or discrete hardware components. It can implement or execute the various methods, steps, and logic block diagrams disclosed in the embodiments of the present invention. The general-purpose processor may be a microprocessor or any conventional processor. The steps of the method disclosed in conjunction with the embodiments of the present invention can be directly implemented and executed by a hardware decoding processor, or by a combination of hardware and software modules in the decoding processor. The software module can be located in a storage medium well-known in the art, such as random access memory, flash memory, read-only memory, programmable read-only memory, electrically erasable programmable memory, registers, etc. The storage medium is located in memory 131, and processor 130 reads information in memory 131 and, in conjunction with its hardware, completes the steps of the method of the aforementioned embodiment.

[0173] An embodiment of the present invention also provides a machine-readable storage medium, which stores machine-executable instructions. When the machine-executable instructions are called and executed by a processor, the machine-executable instructions prompt the processor to implement the above-mentioned point cloud motion compensation method. The specific implementation can be found in the method embodiment, which will not be repeated here.

[0174] The computer program product of the point cloud motion compensation method, device, and electronic device provided in the embodiments of the present invention includes a computer-readable storage medium storing program code. The instructions included in the program code can be used to execute the method described in the previous method embodiment. The specific implementation can be found in the method embodiment and will not be repeated here.

[0175] If the functions are implemented in the form of software functional units and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, or the part that contributes to the prior art, or the part of the technical solution, can be embodied in the form of a software product. The computer software product is stored in a storage medium and includes several instructions for enabling a computer device (which can be a personal computer, a server, or a network device, etc.) to execute all or part of the steps of the method described in each embodiment of the present invention. The aforementioned storage medium includes various media that can store program codes, such as a USB flash drive, a mobile hard disk, a read-only memory (ROM), a random access memory (RAM), a magnetic disk, or an optical disk.

[0176] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the above embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the above embodiments, or replace some or all of the technical features therein with equivalents. However, these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A point cloud motion compensation method, characterized in that: The method comprises: Acquire each frame of point cloud obtained by scanning a multi-beam laser radar, and multiple frames of raw measurement data corresponding to each frame of point cloud generated within a time period; wherein the multiple frames of raw measurement data are collected by an inertial measurement unit; and the multi-beam laser radar and the inertial measurement unit are disposed on a target vehicle; For each bundle of point clouds in each frame of point cloud, the following operations are performed: Interpolating the multiple frames of raw measurement data corresponding to the frame point cloud to obtain interpolated measurement data corresponding to each laser point in the beam point cloud; wherein, within a time period corresponding to each two adjacent frames of raw measurement data, in the interpolated measurement data corresponding to each two adjacent laser points, the acceleration and angular velocity of the target vehicle increase uniformly over time; performing motion compensation on the bundle point cloud based on the interpolated measurement data to obtain a motion-compensated point cloud corresponding to the bundle point cloud; The step of performing interpolation processing on the multiple frames of original measurement data corresponding to the frame point cloud to obtain interpolated measurement data corresponding to each laser point in the beam point cloud comprises: For each two adjacent frames of raw measurement data in the multiple frames of raw measurement data corresponding to the frame point cloud, the following operations are performed: Obtaining a first acquisition time and a second acquisition time corresponding to two adjacent frames of original test data, respectively; wherein the first acquisition time is earlier than the second acquisition time; Calculating a difference between the first acquisition time and the second acquisition time to obtain a first time difference; For each laser point in a time period corresponding to two adjacent frames of raw measurement data, calculating the difference between the acquisition time of the current laser point and the first acquisition time to obtain a second time difference; Calculating a ratio of the second time difference to the first time difference to obtain a proportional coefficient corresponding to the current laser point; Acquire a first acceleration, a first angular velocity, a first velocity value, and a first position data corresponding to the target vehicle at the first acquisition time, and a second acceleration and a second angular velocity corresponding to the second acquisition time; Based on the proportional coefficient, the first acceleration, the first angular velocity, the first velocity value, the first position data, the second acceleration and the second angular velocity, interpolated measurement data corresponding to each laser point in the beam point cloud is determined.

2. The method according to claim 1, characterized in that Each bundle of point clouds in each frame of point cloud is obtained by the following method: For each frame of point cloud, the frame of point cloud is traversed according to the line bundle to obtain each bundle of point clouds in the frame of point cloud; wherein the number of laser points contained in each bundle of point clouds is greater than the number of frames of the multiple frames of original measurement data corresponding to the frame of point cloud.

3. The method according to claim 1, characterized in that The step of determining interpolated measurement data corresponding to each laser point in the beam point cloud based on the proportional coefficient, the first acceleration, the first angular velocity, the first velocity value, the first position data, the second acceleration, and the second angular velocity includes: Determining a target acceleration corresponding to the current laser point based on the proportional coefficient, the first acceleration, the second acceleration, and a preset first zero bias; Determining a target angular velocity corresponding to the current laser point based on the proportional coefficient, the first angular velocity, the second angular velocity, and a preset second zero bias; Determining a rotation parameter corresponding to the current laser point based on a preset quaternion, the target angular velocity, and the second time difference; Determining a position parameter corresponding to the current laser point based on the first position data, the first velocity value, the second time difference, and the first acceleration; Based on the rotation parameter and the position parameter, interpolation measurement data corresponding to the current laser point is determined.

4. The method according to claim 1, wherein The step of performing motion compensation on the bundle point cloud based on the interpolated measurement data to obtain a motion-compensated point cloud corresponding to the bundle point cloud comprises: Calculating a difference between interpolated measurement data corresponding to the current laser point and first original measurement data corresponding to the first acquisition time to obtain a first difference; determining a first rotation and translation matrix between the current laser point and the first raw measurement data based on the first difference; Repeating the step of calculating, for each laser point in a time period corresponding to two adjacent frames of raw measurement data, the difference between the acquisition time of the current laser point and the first acquisition time to obtain a second time difference, and obtaining a second rotation and translation matrix between the next laser point and the first raw measurement data; Determining a rotation and translation matrix between the current laser point and the next laser point based on the first rotation and translation matrix and the second rotation and translation matrix; Based on each of the rotation and translation matrices, a motion-compensated point cloud corresponding to the bundle of point clouds is obtained.

5. The method according to claim 4, characterized in that The step of obtaining a motion-compensated point cloud corresponding to the bundle of point clouds based on each of the rotation and translation matrices includes: Determining a target timestamp based on an output frequency of the multi-beam laser radar and an output frequency of the inertial measurement unit; Based on each of the rotation and translation matrices and the target timestamp, performing time integral processing on the interpolation measurement data corresponding to each laser point to obtain the rotation and translation amount of each laser point converted to the target timestamp; After performing rotation and translation processing on each laser point according to the corresponding rotation and translation amount, a motion-compensated laser point corresponding to each laser point is obtained; Each motion-compensated laser point in the bundle of point clouds is combined to obtain a motion-compensated point cloud corresponding to the bundle of point clouds.

6. The method according to claim 5, characterized in that The step of determining a target timestamp based on an output frequency of the multi-beam laser radar and an output frequency of the inertial measurement unit includes: Selecting the output frequency of the multi-beam laser radar and the output frequency of the inertial measurement unit with the smallest value as the target frequency; A target period value is calculated based on the target frequency, and the target timestamp is determined from the time period of generating the beam point cloud according to the target period value.

7. The method according to claim 5, characterized in that The step of performing time integral processing on the interpolation measurement data corresponding to each laser point based on each rotation and translation matrix and the target timestamp to obtain the rotation and translation amount of each laser point converted to the target timestamp includes: For each laser point, if the acquisition time corresponding to the laser point is less than the target timestamp, obtain a first target laser point; wherein the acquisition time corresponding to the first target laser point is less than or equal to the target timestamp, and the first target laser point is the laser point closest to the target timestamp; Obtain a first rotation and translation matrix for converting the first target laser point to the target timestamp; Calculating the product of each rotation and translation matrix between the laser point and the first target laser point to obtain a first result; The product of the first result and the first rotation and translation matrix is ​​calculated to obtain the rotation and translation amount corresponding to the laser point converted to the target timestamp.

8. The method according to claim 7, characterized in that The method further comprises: If the acquisition time corresponding to the laser point is greater than the target timestamp, a second target laser point is acquired; wherein the acquisition time corresponding to the second target laser point is greater than or equal to the target timestamp, and the second target laser point is the laser point closest to the target timestamp; Obtain a second rotation and translation matrix for converting the second target laser point to the target timestamp; Calculate the product of each rotation and translation matrix between the laser point and the second target laser point to obtain a second result; The product of the second result and the second rotation and translation matrix is ​​calculated to obtain the rotation and translation amount corresponding to the laser point converted to the target timestamp.

9. The method according to claim 1, characterized in that The step of calculating, for each laser point in a time period corresponding to two adjacent frames of raw measurement data, a difference between an acquisition time of the current laser point and the first acquisition time to obtain a second time difference comprises: Starting from the starting laser point of the beam point cloud, multiple thinning laser points are sequentially selected at preset time intervals; Each thinned-out laser point in a time period corresponding to two adjacent frames of original measurement data is sequentially used as a current laser point, and the difference between the acquisition time of the current laser point and the first acquisition time is calculated to obtain a second time difference.

10. The method according to claim 1, characterized in that The method further comprises: Combining the motion-compensated point clouds corresponding to each bundle of point clouds in the first frame point cloud to obtain a first-frame compensated point cloud corresponding to the first frame point cloud after motion compensation, and determining the first-frame compensated point cloud as the compensated point cloud to be spliced; Acquire the adjacent next frame point cloud, combine the motion-compensated point clouds corresponding to each bundle of point clouds in the next frame point cloud, and obtain the next frame compensated point cloud corresponding to the next frame point cloud; Performing a matching calculation on the next frame compensation point cloud and the compensation point cloud to be spliced, and obtaining a compensation rotation and translation amount of the next frame compensation point cloud relative to the compensation point cloud to be spliced; splicing the next frame of compensation point cloud with the compensation point cloud to be spliced ​​according to the compensation rotation and translation amount to obtain a spliced ​​compensation point cloud; Based on the spliced ​​compensated point cloud, a positioning result and a mapping result of the target vehicle are determined.

11. The method according to claim 10, characterized in that The step of determining the positioning result and mapping result of the target vehicle based on the spliced ​​compensated point cloud includes: The spliced ​​compensated point cloud is used as a new point cloud to be spliced, and the step of obtaining the adjacent next frame point cloud is repeatedly performed until the compensated rotation and translation corresponding to each next frame compensated point cloud and the final spliced ​​compensated point cloud are obtained; Determining a positioning result of the target vehicle based on the compensated rotation and translation amount corresponding to each next frame of the compensated point cloud; A mapping result is determined based on the final spliced ​​compensated point cloud.

12. A point cloud motion compensation device, characterized in that: The device comprises: an acquisition module, configured to acquire each frame of a point cloud obtained by scanning a multi-beam laser radar, and multiple frames of raw measurement data correspondingly generated during a time period in which each frame of the point cloud is generated; wherein the multiple frames of raw measurement data are acquired by an inertial measurement unit; and the multi-beam laser radar and the inertial measurement unit are disposed on a target vehicle; For each bundle of point clouds in each frame of point cloud, the following modules are also included: an interpolation processing module, configured to perform interpolation processing on the multiple frames of raw measurement data corresponding to the frame point cloud to obtain interpolated measurement data corresponding to each laser point in the beam point cloud; wherein, within a time period corresponding to each two adjacent frames of raw measurement data, in the interpolated measurement data corresponding to each two adjacent laser points, the acceleration and angular velocity of the target vehicle increase uniformly over time; a motion compensation module, configured to perform motion compensation on the bundle point cloud based on the interpolated measurement data to obtain a motion-compensated point cloud corresponding to the bundle point cloud; The interpolation processing module is further used for: For each two adjacent frames of raw measurement data in the multiple frames of raw measurement data corresponding to the frame point cloud, the following operations are performed: Obtaining a first acquisition time and a second acquisition time corresponding to two adjacent frames of original test data, respectively; wherein the first acquisition time is earlier than the second acquisition time; Calculating a difference between the first acquisition time and the second acquisition time to obtain a first time difference; For each laser point in a time period corresponding to two adjacent frames of raw measurement data, calculating the difference between the acquisition time of the current laser point and the first acquisition time to obtain a second time difference; Calculating a ratio of the second time difference to the first time difference to obtain a proportional coefficient corresponding to the current laser point; Acquire a first acceleration, a first angular velocity, a first velocity value, and a first position data corresponding to the target vehicle at the first acquisition time, and a second acceleration and a second angular velocity corresponding to the second acquisition time; Based on the proportional coefficient, the first acceleration, the first angular velocity, the first velocity value, the first position data, the second acceleration and the second angular velocity, interpolated measurement data corresponding to each laser point in the beam point cloud is determined.

13. An electronic device, characterized in that: The method comprises a processor and a memory, wherein the memory stores machine-executable instructions that can be executed by the processor, and the processor executes the machine-executable instructions to implement the point cloud motion compensation method according to any one of claims 1 to 11.

14. A machine-readable storage medium, characterized in that The machine-readable storage medium stores machine-executable instructions. When the machine-executable instructions are called and executed by the processor, the machine-executable instructions prompt the processor to implement the point cloud motion compensation method according to any one of claims 1 to 11.

Images