Laser radar motion compensation method and device, equipment and storage medium

Abstract

The invention discloses a laser radar motion compensation method, device and equipment and a storage medium, and the method comprises the steps: carrying out the analysis based on a rosbag data package collected by a laser radar, and generating a point cloud frame list based on an analysis result; based on a first laser point timestamp of each frame of point cloud in the point cloud frame list and self-parking space attitude information collected by an IMU sensor, performing motion compensation on all laser points of each frame of point cloud by using a binary search method and an interpolation method to obtain a point cloud after motion compensation; performing target detection and post-processing optimization on the point cloud after motion compensation based on a truth value algorithm of a truth value system, and generating a dynamic truth value target attribute; and performing motion compensation on the dynamic truth value target attribute by using the historical track of the dynamic target to obtain the dynamic truth value target attribute after motion compensation. Compared with the prior art, the method can carry out the motion compensation of the dynamic true value target on the basis of not affecting the original point cloud form, and improves the precision of the laser radar data.

Description

Technical Field

[0001] The present invention relates to the field of autonomous driving technology, and in particular to a laser radar motion compensation method, device, equipment and storage medium. Background Art

[0002] The closed loop of data in autonomous driving has always been the key to improving various algorithms, especially perception algorithms. In the process of improving perception performance, a lot of true data generated by high-value scene data is needed to provide a basis for the continuous iteration, upgrading and evolution of vehicle-side perception algorithms. The quality and reliability of true data will directly affect the training effect of the vehicle-side algorithm. Improving the quality and reliability of true data has become an extremely important part of the closed loop of autonomous driving data. Rotating LiDAR has always been the preferred LiDAR for vehicles with true value systems because of its more wiring harnesses, denser point clouds, and wider detection distance. Improving the quality of the point cloud output by the rotating LiDAR, reducing the distortion of the laser point cloud caused by the movement of the vehicle and other moving targets, and making the laser point cloud projection onto the image more fit are key links to improving the quality and reliability of true data.

[0003] However, at the current stage, many motion compensation methods of rotating lidar can solve the motion compensation of static targets or dedistort dynamic targets with relatively small relative speeds. However, they cannot solve the shape distortion caused by the rapid movement of dynamic targets with a large speed difference from the vehicle.

[0004] Therefore, there is an urgent need for a LiDAR motion compensation method that can solve the technical problem that for dynamic targets with a large speed difference relative to the vehicle's speed, errors in their own size and position occur due to their own movement, which in turn leads to reduced accuracy of LiDAR data. Summary of the invention

[0005] The main purpose of the present invention is to provide a laser radar motion compensation method, device, equipment and storage medium, aiming to solve the technical problem in the prior art that for dynamic targets with a large speed difference relative to the speed of the vehicle, errors in their own size and position are generated due to their own movement, thereby reducing the accuracy of the laser radar data.

[0006] To achieve the above object, the present invention provides a laser radar motion compensation method, the method comprising the following steps:

[0007] Parse the rosbag data packet collected by the lidar and generate a point cloud frame list based on the parsing results;

[0008] Based on the timestamp of the first laser point of each frame of the point cloud in the point cloud frame list and the vehicle posture information collected by the IMU sensor, motion compensation is performed on all laser points of each frame of the point cloud using a binary search method and an interpolation method to obtain a point cloud after motion compensation;

[0009] Performing target detection and post-processing optimization on the motion-compensated point cloud based on a truth value algorithm of a truth value system to generate dynamic truth value target attributes;

[0010] The dynamic true value target attribute is motion compensated by utilizing the historical trajectory of the dynamic target to obtain the dynamic true value target attribute after motion compensation.

[0011] Optionally, the step of performing motion compensation on all laser points of each frame of point cloud using a binary search method and an interpolation method based on the timestamp of the first laser point of each frame of point cloud in the point cloud frame list and the vehicle posture information collected by the IMU sensor to obtain a point cloud after motion compensation includes:

[0012] Based on the first laser point timestamp of each frame of point cloud in the point cloud frame list and the self-vehicle posture information collected by the IMU sensor, the approximate self-vehicle posture information corresponding to the first laser point timestamp of each frame of point cloud is determined by using a binary search method and an interpolation method;

[0013] Traversing all laser points of each frame of the point cloud to determine the approximate position information of the vehicle corresponding to the timestamps of all laser points of each frame of the point cloud;

[0014] Determine the rotation and translation matrix using the approximate pose information of the vehicle corresponding to the timestamps of all laser points in each frame of the point cloud;

[0015] Motion compensation is performed on all laser points of each frame of the point cloud based on the rotation and translation matrix to obtain a point cloud after motion compensation.

[0016] Optionally, the step of performing motion compensation on the dynamic true value target attribute by using the historical trajectory of the dynamic target to obtain the dynamic true value target attribute after motion compensation includes:

[0017] Based on the motion-compensated point cloud, using the historical trajectory of the dynamic target, determining the trajectory information of the dynamic target in the continuous frame segments corresponding to the motion-compensated point cloud;

[0018] Determining the size optimization amount of the dynamic target using the true value algorithm according to the trajectory information;

[0019] Determine the position translation of the dynamic target according to the dynamic true value target attribute, the laser radar delay compensation angle and the laser radar scanning time;

[0020] The dynamic true value target attribute is motion compensated based on the size optimization amount and the position translation amount to obtain the dynamic true value target attribute after motion compensation.

[0021] Optionally, the step of performing motion compensation on all laser points of each frame of the point cloud based on the rotation and translation matrix to obtain a point cloud after motion compensation comprises:

[0022] All laser points of each frame of the point cloud are converted to the world coordinate system corresponding to the timestamp of each laser point through the rotation and translation matrix;

[0023] The approximate posture information of the vehicle corresponding to the timestamp of the first laser point of each frame of the point cloud is used as reference data;

[0024] Based on the reference data, all laser points of each frame of the point cloud are converted from the world coordinate system to the vehicle coordinate system corresponding to the timestamp of the first laser point of each frame of the point cloud to obtain a motion compensated point cloud.

[0025] Optionally, the step of determining the approximate posture information of the self-vehicle corresponding to the first laser point timestamp of each frame of point cloud in the point cloud frame list by using a binary search method and an interpolation method based on the first laser point timestamp of each frame of point cloud and the self-vehicle posture information collected by the IMU sensor includes:

[0026] Construct an ascending timestamp list based on the vehicle posture information collected by the IMU sensor;

[0027] Based on the first laser point timestamp of each frame of point cloud in the point cloud frame list and the ascending timestamp list, determine the first timestamp and the second timestamp in the ascending timestamp list that are closest to the first laser point timestamp of each frame of point cloud by using a binary search method;

[0028] Respectively obtain the vehicle position information corresponding to the first timestamp and the second timestamp;

[0029] Based on the self-vehicle posture information corresponding to the first timestamp and the second timestamp respectively, the approximate self-vehicle posture information corresponding to the first laser point timestamp of each frame of the point cloud is determined by using spherical interpolation method and linear interpolation method.

[0030] Optionally, the self-vehicle approximate posture information includes the self-vehicle approximate Euler angle value and the self-vehicle approximate position information, and the step of determining the self-vehicle approximate posture information corresponding to the first laser point timestamp of each frame point cloud by using spherical interpolation and linear interpolation based on the self-vehicle posture information corresponding to the first timestamp and the second timestamp respectively, comprises:

[0031] Based on the ego vehicle approximate Euler angle values ​​corresponding to the first timestamp and the second timestamp respectively, determine the ego vehicle approximate Euler angle value corresponding to the first laser point timestamp of each frame point cloud by using spherical interpolation method;

[0032] Based on the approximate position information of the vehicle corresponding to the first timestamp and the second timestamp respectively, the approximate position information of the vehicle corresponding to the first laser point timestamp of each frame of the point cloud is determined by linear interpolation.

[0033] Optionally, the step of parsing the rosbag data packet collected by the laser radar and generating a point cloud frame list based on the parsing result includes:

[0034] Parse the rosbag data packet collected by the laser radar based on the user manual corresponding to the laser radar and the starting parsing angle of the laser radar, and obtain the point cloud data packet according to the parsing result;

[0035] The point cloud data contained in each frame of point cloud is determined according to the data structure of the point cloud data packet, and a point cloud frame list is generated based on the point cloud data contained in each frame of point cloud, wherein each element in the point cloud frame list represents the full amount of data constituting one frame of point cloud.

[0036] In addition, to achieve the above-mentioned purpose, the present invention also proposes a laser radar motion compensation device, the device comprising:

[0037] Point cloud data parsing module, used to parse rosbag data packets collected by lidar and generate point cloud frame lists based on the parsing results;

[0038] A point cloud motion compensation module is used to perform motion compensation on all laser points of each frame of point cloud by using a binary search method and an interpolation method based on the timestamp of the first laser point of each frame of point cloud in the point cloud frame list and the self-vehicle posture information collected by the IMU sensor to obtain a point cloud after motion compensation;

[0039] A point cloud data processing module, used for performing target detection and post-processing optimization on the motion-compensated point cloud based on a truth value algorithm of a truth value system, and generating dynamic truth value target attributes;

[0040] The target motion compensation module is used to perform motion compensation on the dynamic true value target attribute by using the historical trajectory of the dynamic target to obtain the dynamic true value target attribute after motion compensation.

[0041] In addition, to achieve the above-mentioned purpose, the present invention also proposes a laser radar motion compensation device, which includes: a memory, a processor, and a laser radar motion compensation program stored in the memory and executable on the processor, and the laser radar motion compensation program is configured to implement the steps of the laser radar motion compensation method described above.

[0042] In addition, to achieve the above-mentioned purpose, the present invention also proposes a storage medium, on which a laser radar motion compensation program is stored. When the laser radar motion compensation program is executed by a processor, the steps of the laser radar motion compensation method described above are implemented.

[0043] The present invention discloses a method for parsing a rosbag data packet collected by a laser radar, and generating a point cloud frame list based on the parsing result; based on the first laser point timestamp of each frame of point cloud in the point cloud frame list and the self-vehicle posture information collected by an IMU sensor, all laser points of each frame of point cloud are motion compensated by using a binary search method and an interpolation method to obtain a point cloud after motion compensation; target detection and post-processing optimization are performed on the point cloud after motion compensation based on a truth value algorithm of a truth value system to generate dynamic truth value target attributes; and the dynamic truth value target attributes are motion compensated by using the historical trajectory of the dynamic target to obtain the dynamic truth value target attributes after motion compensation. Since the present invention uses binary search and interpolation methods to perform motion compensation on all laser points of each frame of point cloud in the point cloud frame list, and performs target detection and post-processing optimization on the point cloud after motion compensation based on the true value algorithm, and combines the historical trajectory of the dynamic target to perform motion compensation on the dynamic true value target attributes, compared with the prior art, the present invention can perform motion compensation on the dynamic true value target without affecting the original point cloud morphology, thereby improving the accuracy of the lidar data and solving the technical problem in the prior art that for dynamic targets with a large speed difference relative to the vehicle's speed, errors in their own size and position are generated due to their own movement, thereby reducing the accuracy of the lidar data. BRIEF DESCRIPTION OF THE DRAWINGS

[0044] Figure 1 It is a schematic diagram of the flow chart of the first embodiment of the laser radar motion compensation method of the present invention;

[0045] Figure 2 A detailed flow chart of point cloud data analysis in the laser radar motion compensation method of the present invention;

[0046] Figure 3 It is a schematic diagram of the overall process of the laser radar motion compensation method of the present invention;

[0047] Figure 4 It is a schematic diagram of the flow chart of the second embodiment of the laser radar motion compensation method of the present invention;

[0048] Figure 5 A detailed flow chart of point cloud motion compensation in the laser radar motion compensation method of the present invention;

[0049] Figure 6 A schematic diagram of a flow chart of a third embodiment of a laser radar motion compensation method according to the present invention;

[0050] Figure 7 A detailed flow chart of dynamic true value target motion compensation in the laser radar motion compensation method of the present invention;

[0051] Figure 8 A reference graph for calculating the velocity information of the dynamic target in the current k-th frame in the laser radar motion compensation method of the present invention;

[0052] Fig. 9 A reference diagram for calculating the back-off displacement distance required for dynamic targets in the laser radar motion compensation method of the present invention;

[0053] Fig.10 A reference image for optimizing the size of a dynamic target in the laser radar motion compensation method of the present invention;

[0054] Fig.11 It is a structural block diagram of the first embodiment of the laser radar motion compensation device of the present invention;

[0055] Fig.12 It is a structural diagram of a laser radar motion compensation device in a hardware operating environment involved in an embodiment of the present invention.

[0056] The realization of the purpose, functional features and advantages of the present invention will be further explained in conjunction with embodiments and with reference to the accompanying drawings. DETAILED DESCRIPTION

[0057] It should be understood that the specific embodiments described herein are only used to explain the present invention, and are not used to limit the present invention.

[0058] The embodiment of the present invention provides a laser radar motion compensation method, referring to Figure 1 , Figure 1 Schematic diagram of the flow chart of the first embodiment of the laser radar motion compensation method of the present invention.

[0059] In this embodiment, the laser radar motion compensation method includes steps S10 to S40:

[0060] Step S10: Parse the rosbag data packet collected by the lidar, and generate a point cloud frame list based on the parsing results.

[0061] It should be noted that the execution subject of this embodiment can be a computer server device with data processing, network communication and program running functions used in the autonomous driving scene, such as a server, a tablet computer, a smart phone, a vehicle-mounted mobile device, etc., or an electronic device capable of realizing the above functions, a laser radar motion compensation device, etc. The following takes a system including a laser radar motion compensation device (hereinafter referred to as the system) as an example to illustrate this embodiment and the following embodiments.

[0062] It should be noted that the laser radar in this embodiment and the following implementations can be a rotating laser radar, which is a scanning sensor that uses non-contact laser ranging technology. It senses and measures the surrounding environment by emitting laser and measuring its reflection.

[0063] It should be understood that the rosbag data packet is a special file format in the ROS system. The rosbag data packet can be a rosbag data packet of road collection, which is used to store and replay various sensor data in the road surface collection process. Through the rosbag data packet, the user can process, analyze and debug the collected data in an offline state. In this embodiment, the data collected by the laser radar is compressed and encoded and then put into the rosbag data packet to obtain a rosbag data packet based on the laser radar collection.

[0064] It should be explained that the point cloud frame list may refer to a collection of a series of point cloud data frames continuously collected by a sensor device such as a lidar within a certain period of time. Each frame of point cloud data contains the three-dimensional point information of the sensor's surrounding environment at a specific moment.

[0065] In a specific implementation, step S10 may include steps S101 to S102:

[0066] Step S101: parse the rosbag data packet collected by the laser radar based on the user manual corresponding to the laser radar and the starting parsing angle of the laser radar, and obtain a point cloud data packet according to the parsing result.

[0067] Step S102: determining the point cloud data contained in each frame of point cloud according to the data structure of the point cloud data packet, and generating a point cloud frame list based on the point cloud data contained in each frame of point cloud, wherein each element in the point cloud frame list represents the full amount of data constituting one frame of point cloud.

[0068] In the specific implementation, based on the user manual corresponding to the laser radar and by dynamically configuring the starting analysis angle of the laser radar, one or several frames can be flexibly analyzed. The properties of each laser point required can also be flexibly configured. In addition, a web service container can be constructed offline and deployed to a cluster for parallel analysis of large-scale data.

[0069] For example, refer to Figure 2 , Figure 2 The following is a detailed flow chart of the point cloud data analysis in the laser radar motion compensation method of the present invention. The rosbag data packet is mainly a data format for recording and storing data in the ROS system. There will be multiple Topics in it, and each Topic will store different sensor modules, vehicle body signals and other data. Based on the Topic of the rosbag data packet, the point cloud data of the corresponding Topic can be read. Using the user manual provided by the corresponding laser radar manufacturer, the data structure of each point cloud data packet data can be defined. The point cloud data packet data can contain many attributes, such as the data header and data tail of the data packet, the timestamp when the point cloud data packet data is generated, and the block storing the laser points contained in the point cloud data packet data.

[0070] It should be noted that each point cloud data packet data may include two blocks (the specific number of blocks in each point cloud data packet data may vary depending on different laser radar manufacturers). Block represents a data set or data unit in the point cloud data packet data. Each block will contain the azimuth of the block, as well as the timestamp offset of each stored laser point (generally one block will store 128 laser points, and the specific number of laser points will vary depending on the model, beam and other parameters of the laser radar), the position information of the object in three-dimensional space, the intensity information of the laser beam irradiated to the target surface and reflected back, and other attributes. Based on the pre-configured laser radar starting resolution angle θ (the angle value may vary due to the laser radar and camera calibration scheme), a double-ended queue deque can be set to calculate the azimuth of the first block of each point cloud data packet data, recursively find the azimuth of the block closest to the laser radar starting resolution angle θ, and use the point cloud data packet data corresponding to the closest azimuth as the index k of the first point cloud data packet data constituting the first frame of point cloud.

[0071] In the specific implementation, a double-ended queue deque can be constructed, and the size of the double-ended queue deque is constantly maintained at 2, that is, it contains 2 rosbag corresponding point cloud topic message bodies, throws the message body to the left, and adds the message body to the right (that is, one end continuously throws the message body, and the other end continuously adds the message body). Each message body can contain 1800 (the number of point cloud data packets contained in the message body varies depending on the definition of the rosbag message body) point cloud data packets data. Based on the index k of the first point cloud data packet data that constitutes the first frame of point cloud found, the double-ended queue is traversed in a loop, and 900 (this data varies due to different laser radar models and manufacturers) point cloud data packets data constitute the full amount of data for a frame of point cloud. Add these 900 point cloud data packets data to the point cloud frame list list. The output list size is the number of point cloud frames contained in the rosbag data packet, such as a 20s rosbag package. The lidar topic represents the category of the lidar. For example, a 10HZ lidar will have 200 frames of point cloud data. Each element in the point cloud frame list represents the complete data that constitutes a frame of point cloud.

[0072] Step S20: Based on the timestamp of the first laser point of each frame of point cloud in the point cloud frame list and the vehicle posture information collected by the IMU sensor, motion compensation is performed on all laser points of each frame of point cloud using a binary search method and an interpolation method to obtain a point cloud after motion compensation.

[0073] It should be understood that the IMU sensor, or inertial measurement unit, is an advanced sensor device that integrates sensors such as accelerometers and gyroscopes (sometimes including magnetometers) to measure and monitor key information such as acceleration, angular velocity, and direction of an object. The IMU sensor can help the vehicle maintain stability and navigation by measuring the vehicle's posture, direction, and acceleration in real time. In autonomous driving technology, the IMU sensor is one of the indispensable core components.

[0074] In the specific implementation, the first laser point timestamp of each frame of point cloud in the point cloud frame list can be combined with the self-vehicle posture information output by the IMU sensor, and binary search can be used to find the nearest IMU timestamps to the first laser point timestamp of each frame of point cloud. Spherical interpolation is performed on the rotation parameters in the self-vehicle posture information, and linear interpolation is performed on the translation parameters in the self-vehicle posture information to obtain the self-vehicle approximate posture information of the first laser point timestamp of each frame of point cloud. The self-vehicle approximate posture information is used as a reference to traverse all laser points in each frame of point cloud, and at the same time, combined with binary search, motion compensation of laser points is performed to obtain the point cloud after motion compensation.

[0075] Step S30: performing target detection and post-processing optimization on the motion-compensated point cloud based on the truth value algorithm of the truth value system to generate dynamic truth value target attributes.

[0076] It should be noted that step S20 can only perform motion compensation on static targets or dynamic targets with a small speed difference relative to the vehicle. However, for targets with a large speed difference relative to the vehicle, errors in their own size and position will occur due to their own movement. Therefore, in order to further improve the accuracy of the lidar data, motion compensation can be performed on dynamic targets as much as possible without affecting the original shape of the point cloud.

[0077] It should be understood that the truth system is a data acquisition system composed of a variety of vehicle-mounted sensors (such as millimeter-wave radar, lidar, high-precision combined inertial navigation, etc.) and efficient data recording equipment. Its core function is to generate data (i.e., the truth value) with higher reliability than the sensor under test by processing the collected data. These data are often used to evaluate the performance of the sensor under test and can be used for further data mining and scene construction. The laser point cloud output by the rotating lidar will be used to build a truth system in the cloud, which can include truth algorithms such as 2D target detection, 3D target detection, and post-processing.

[0078] The truth value algorithm has a wide range of application value in the truth value system of rotating lidar. By generating more reliable and accurate truth value data, the truth value algorithm can be used to evaluate the performance of sensors such as lidar and provide reliable data support for fields such as autonomous driving. In addition, the truth value data can also be used for data mining and scenario construction, providing rich data sets and test scenarios for the development and testing of autonomous driving systems.

[0079] It needs to be explained that target detection is the process of identifying target objects in point cloud data, and post-processing optimization is the process of further processing and improving the target detection results. In point cloud processing, post-processing optimization usually includes point cloud segmentation, classification, denoising, smoothing and other steps, which can help improve the accuracy and completeness of target detection while reducing the impact of noise and redundant information.

[0080] It should be noted that the above-mentioned dynamic true value target attributes may include static attributes (such as shape, size, etc.) and dynamic attributes (such as speed, trajectory, etc.) of the dynamic true value target (ie, dynamic target).

[0081] Step S40: performing motion compensation on the dynamic true value target attribute by using the historical trajectory of the dynamic target to obtain the dynamic true value target attribute after motion compensation.

[0082] In a specific implementation, the position of the dynamic target can be compensated based on the dynamic true target attributes, the laser radar delay compensation angle and the laser radar scanning time. At the same time, the size of the dynamic target can be optimized in combination with the historical trajectory of the dynamic target to achieve motion compensation for the dynamic true target attributes and obtain the dynamic true target attributes after motion compensation.

[0083] It should be noted that there is a delay in the laser radar scanning process, which will cause a deviation between the measured data and the actual position. Therefore, the position of the dynamic target can be compensated based on the dynamic true value target attributes, the laser radar delay compensation angle and the laser radar scanning time. Size optimization can be to adjust the size parameters (such as length, width, height, etc.) of the dynamic target according to its historical trajectory to more accurately reflect the actual shape and size of the dynamic target.

[0084] It should be noted that the overall process of the present invention can refer to Figure 3 , Figure 3 The figure is a schematic diagram of the overall process of the laser radar motion compensation method of the present invention. In the figure, the laser radar motion compensation method mainly includes individual parts: point cloud data analysis, point cloud motion compensation and dynamic true value target motion compensation. Point cloud data analysis: Based on the user manual corresponding to the laser radar and the starting analysis angle of the laser radar, the rosbag data packet collected by the laser radar is analyzed (the rosbag data packet can be read by binary), and the point cloud data packet list is obtained according to the analysis result; the point cloud data contained in each frame of point cloud is determined according to the data structure of the point cloud data packet, and a point cloud frame list is generated based on the point cloud data contained in each frame of point cloud. Point cloud motion compensation: According to the timestamp of the first laser point of the single-frame point cloud in the point cloud frame list and the self-vehicle posture information collected by the IMU sensor, the self-vehicle approximate posture information of the first laser point of the single-frame point cloud is determined by the binary search method and the interpolation method, and then the self-vehicle approximate posture information is used as a reference, and the single-frame point cloud is dedistorted by traversing all the point clouds of the single frame, that is, the laser point line motion compensation is realized. Dynamic truth target motion compensation: The truth value algorithm based on the truth value system performs target detection and post-processing optimization on the dedistorted point cloud to generate dynamic truth target attributes; the dynamic truth target size optimization amount is determined by using the historical trajectory of the dynamic target, and the dynamic truth target attributes are motion compensated based on the dynamic truth target size optimization amount to obtain the dynamic truth target attributes after motion compensation (i.e., the truth value attributes after dynamic truth motion compensation).

[0085] This embodiment discloses parsing a rosbag data packet collected by a laser radar, and generating a point cloud frame list based on the parsing result; based on the first laser point timestamp of each frame of point cloud in the point cloud frame list and the vehicle posture information collected by the IMU sensor, all laser points of each frame of point cloud are motion compensated by using a binary search method and an interpolation method to obtain a point cloud after motion compensation; target detection and post-processing optimization are performed on the point cloud after motion compensation based on a truth value algorithm of a truth value system to generate dynamic truth value target attributes; the dynamic truth value target attributes are motion compensated by using the historical trajectory of the dynamic target to obtain the dynamic truth value target attributes after motion compensation. Since the present embodiment uses binary search and interpolation methods to perform motion compensation on all laser points of each frame of point cloud in the point cloud frame list, and performs target detection and post-processing optimization on the motion-compensated point cloud based on the true value algorithm, and performs motion compensation on the dynamic true value target attributes in combination with the historical trajectory of the dynamic target, compared with the prior art, the present embodiment can perform motion compensation on the dynamic true value target without affecting the original point cloud morphology, thereby improving the accuracy of the lidar data and solving the technical problem in the prior art that for dynamic targets with a large speed difference relative to the vehicle's speed, errors in their own size and position are generated due to their own motion, thereby reducing the accuracy of the lidar data.

[0086] refer to Figure 4 , Figure 4 Schematic diagram of the flow chart of the second embodiment of the laser radar motion compensation method of the present invention.

[0087] Based on the above first embodiment, in this embodiment, step S20 includes steps S201 to S204:

[0088] Step S201: Based on the first laser point timestamp of each frame of point cloud in the point cloud frame list and the vehicle posture information collected by the IMU sensor, the approximate vehicle posture information corresponding to the first laser point timestamp of each frame of point cloud is determined by using a binary search method and an interpolation method.

[0089] It should be understood that binary search is a search algorithm for finding a specific element in an ordered array. It compares the target value with the middle element of the array and decides whether to continue searching in the left or right half of the array based on the comparison result, thereby gradually narrowing the search range until the target value is found or it is determined that the target value does not exist.

[0090] In a specific implementation, an ascending timestamp list can be constructed based on the self-vehicle posture information collected by the IMU sensor; based on the first laser point timestamp of each frame of point cloud in the point cloud frame list and the ascending timestamp list, a binary search method is used to determine the first timestamp and the second timestamp in the ascending timestamp list that are closest to the first laser point timestamp of each frame of point cloud; the self-vehicle posture information corresponding to the first timestamp and the second timestamp are respectively obtained; based on the self-vehicle posture information corresponding to the first timestamp and the second timestamp, the approximate self-vehicle posture information corresponding to the first laser point timestamp of each frame of point cloud is determined using spherical interpolation and linear interpolation.

[0091] It should be noted that the above-mentioned approximate posture information of the vehicle includes the approximate Euler angle value of the vehicle and the approximate position information of the vehicle.

[0092] Correspondingly, the above-mentioned method of determining the approximate posture information of the self-vehicle corresponding to the first laser point timestamp of each frame point cloud by using spherical interpolation and linear interpolation based on the self-vehicle posture information corresponding to the first timestamp and the second timestamp respectively, includes: determining the approximate Euler angle value of the self-vehicle corresponding to the first laser point timestamp of each frame point cloud by using spherical interpolation based on the approximate Euler angle value of the self-vehicle corresponding to the first timestamp and the second timestamp respectively; and determining the approximate position information of the self-vehicle corresponding to the first laser point timestamp of each frame point cloud by using linear interpolation based on the approximate position information of the self-vehicle corresponding to the first timestamp and the second timestamp respectively.

[0093] Step S202: Traverse all laser points of each frame of point cloud to determine approximate pose information of the vehicle corresponding to the timestamps of all laser points of each frame of point cloud.

[0094] Step S203: Determine the rotation and translation matrix using the vehicle approximate pose information corresponding to all laser point timestamps of each frame of the point cloud.

[0095] Step S204: performing motion compensation on all laser points of each frame of the point cloud based on the rotation and translation matrix to obtain a point cloud after motion compensation.

[0096] In a specific implementation, all laser points of each frame of point cloud can be converted to the world coordinate system corresponding to the timestamp of each laser point through the rotation and translation matrix; the approximate posture information of the vehicle corresponding to the first laser point timestamp of each frame of point cloud is used as reference data; based on the reference data, all laser points of each frame of point cloud are converted from the world coordinate system to the vehicle coordinate system corresponding to the first laser point timestamp of each frame of point cloud to obtain a motion compensated point cloud.

[0097] It should be noted that based on the point cloud frame list, point cloud motion compensation can be performed while parsing the coordinates of each laser point. For example, parsing the mth frame of the point cloud frame list, that is, list[m]. This frame of point cloud contains 900 data packets, each of which has a header timestamp information. The header timestamp of the first data packet is T m As the file name of the point cloud of this frame, the timestamp of the first laser point of the first block of the first data packet data is used as the reference timestamp. The timestamp of the first laser point of the first block of the first data packet data is:

[0098]

[0099] In the formula, block0 represents the first block of the first data packet of the frame point cloud, sweep0 represents the first laser point of the block, and Ts_offset represents the offset of the timestamp of each laser point in each block. Based on the reference timestamp T ref , and then all laser points in this frame are T ref Based on the vehicle posture information at the moment, all laser points in the frame are transferred to T ref In the vehicle coordinate system corresponding to the moment, motion compensation is performed on each laser point.

[0100] For example, refer to Figure 5 , Figure 5 This is a detailed flow chart of point cloud motion compensation in the laser radar motion compensation method of the present invention. First, obtain the vehicle posture information of the IMU sensor. The IMU sensor is generally high-frequency (100HZ) and can output the vehicle's Euler angle and position information at multiple times (the vehicle posture information includes the vehicle's Euler angle and position information). Use the Euler angle dictionary and position dictionary formed by the vehicle's Euler angle and position information to construct an ascending timestamp list:

[0101] Dict(t, euler) = {t: [roll t , pitch t , yaw t ], t∈T};

[0102] Dict(t,pos)={t:[X t , Y t , Z t ], t∈T};

[0103] sorted_timestamp_list=sorted(t,t∈T).

[0104] In the formula, sorted_timestamp_list represents the ascending timestamp list, sorted is the function used to sort in ascending order, t represents the timestamp, T represents the range of the timestamp, euler represents the Euler angle, pos represents the position, Dict(t, euler) represents the Euler angle dictionary, Dict(t, pos) represents the position dictionary, roll t , pitch t , yaw t Represents the three angle values ​​in Euler angle, X t , Y t , Z t Indicates the coordinate value of the corresponding vehicle position.

[0105] Based on the ascending timestamp list and the first laser point timestamp information T of the frame point cloud ref , use the binary search method to determine the timestamps of the previous and next IMU sensors closest to the first laser point timestamp of the frame point cloud in the ascending timestamp list, closest_T ref _left and closest_T ref _right, and then obtain the Euler angle value and position information corresponding to the previous and next timestamps respectively.

[0106]

[0107]

[0108]

[0109]

[0110] Where, closest_T ref _left and closest_T ref _right respectively represent the first and second timestamps closest to the first laser point timestamp of a single frame point cloud in the ascending timestamp list, and Represents the Euler angle values ​​corresponding to the first timestamp and the second timestamp respectively. and Respectively represent the location information corresponding to the first timestamp and the second timestamp.

[0111] Based on the IMU sensor and binary search method, the vehicle posture information corresponding to the nearest IMU sensor timestamp before and after the first laser point of the frame point cloud is obtained (the vehicle posture information includes the Euler angle value corresponding to the first timestamp and the second timestamp and the position information corresponding to the first timestamp and the second timestamp). Using the posture information corresponding to the adjacent IMU sensor timestamps, the spherical interpolation and linear interpolation methods can be used to approximately solve the posture information at the time corresponding to the first laser point of the frame point cloud. First, when calculating the Euler angle information corresponding to the first laser point of the frame point cloud, it is necessary to and Convert from Euler angle to quaternion, then use spherical interpolation to interpolate the quaternion value corresponding to the first laser point timestamp, and then convert back to Euler angle To calculate the position information corresponding to the first laser point, and Linear interpolation is performed to obtain The specific formula is as follows.

[0112]

[0113]

[0114] ratio=(T ref -closest_T ref _left) / (closest_T ref _right-closest_T ref _left);

[0115]

[0116]

[0117]

[0118] In the formula, ratio represents the ratio value, euler2quaternion is the function used to convert Euler angles to quaternions, quaternion2euler is the function used to convert quaternions to Euler angles, Quaternion.slerp represents spherical interpolation, is the quaternion value corresponding to the timestamp of the first laser point.

[0119] Through the above steps, the approximate pose information of the ego vehicle corresponding to the timestamp of the first laser point can be calculated. The approximate pose information of the ego vehicle includes the Euler angle and position parameters of the ego vehicle. and

[0120] After obtaining the approximate pose information of the ego vehicle corresponding to the timestamp of the first laser point, all laser points of the point cloud of the frame can be traversed to calculate the Euler angles and position parameter values ​​of the ego vehicle corresponding to the timestamp of each laser point. The rotation and translation matrix is ​​calculated using the Euler angles and position parameter values ​​of the ego vehicle. Each laser point is first transferred to the world coordinate system at the corresponding time of each laser point through the rotation and translation matrix. Then, combined with the Euler angles and position parameter values ​​of the ego vehicle corresponding to the first laser point, each laser point is transferred from the world coordinate system to the ego vehicle coordinate system corresponding to the first laser point. This completes the dedistortion operation of all laser points in the frame based on the ego vehicle coordinate system corresponding to the first laser point.

[0121] First, we need to calculate the IMU timestamp closest to each laser point timestamp. For example, to dedistort the laser point with index i, we need to calculate the time T of the laser point with index i. point_i The corresponding first and second timestamps (closest_T point_i _left, closest_T point_i _right). Then obtain the nearest timestamps before and after the timestamp of the laser point indexed as i (closest_T point_i _left, closest_T point_i _right) corresponding to the Euler angle value ( and ) and location information ( and ), the specific formula is as follows.

[0122]

[0123]

[0124]

[0125]

[0126] Then, based on the IMU sensor and the binary search method, the vehicle's position information corresponding to the nearest IMU moment before and after the laser point with index i is obtained. Using the position information corresponding to the timestamps of the adjacent IMU sensors, the position information of the laser point with index i can be approximately calculated using the spherical interpolation and linear interpolation methods. First, when calculating the Euler angle information corresponding to the laser point with index i, it is necessary to and Convert from Euler angle to quaternion, then use spherical interpolation to interpolate the quaternion value corresponding to the laser point index i, and then convert back to Euler angle To calculate the position information corresponding to the laser point index i, we need to and Linear interpolation is performed to obtain The specific formula is as follows.

[0127]

[0128]

[0129]

[0130]

[0131]

[0132]

[0133] Using the above steps, the Euler angles and position parameters corresponding to the timestamp of the laser point with index i can be calculated. Combined with the calculated Euler angles and position parameters corresponding to the timestamp of the first laser point in the frame point cloud, the laser point with index i can be dedistorted.

[0134] In the specific implementation, the X, Y, and Z coordinates of each laser point in the vehicle coordinate system can be calculated by binary operation and combining the distance information of each laser point contained in each block and the external parameter information of the laser radar without dedistortion. For example, for the laser point with index i, the corresponding T point_i The coordinate in the vehicle coordinate system at this moment is EgoCoordinate point_i =(X, Y, Z). Then the rotation and translation matrix RT is calculated using the Euler angles and position parameter values ​​corresponding to the laser point indexed as i. point_i , using the matrix RT point_i Left multiply the coordinates of the laser point with index i by EgoCoordinate point_i , we can get the coordinates of the laser point with index i in the world coordinate system WorldCoordinate point_i Based on the time T of the first laser point in the frame point cloud ref The corresponding Euler angles and position parameter values ​​can be used to calculate the rotation and translation matrix RT of the reference point (i.e. the timestamp of the first laser point in the point cloud of this frame) ref Using this matrix RT ref Multiply the inverse matrix of WorldCoordinate by point_i The coordinates of the laser point indexed as i in the world coordinate system can be converted to the first laser point timestamp T ref The corresponding coordinate RefCoordinate in the vehicle coordinate system point_iBy traversing all the laser points of the laser radar and repeating the above dedistortion steps, the dedistortion operation of the laser point can be completed while parsing the laser point coordinates. The implementation formula is shown below.

[0135]

[0136] This embodiment discloses that based on the first laser point timestamp of each frame of point cloud in the point cloud frame list and the self-vehicle posture information collected by the IMU sensor, the self-vehicle approximate posture information corresponding to the first laser point timestamp of each frame of point cloud is determined by using a binary search method and an interpolation method; all laser points of each frame of point cloud are traversed to determine the self-vehicle approximate posture information corresponding to all laser point timestamps of each frame of point cloud; the rotation and translation matrix is ​​determined using the self-vehicle approximate posture information corresponding to all laser point timestamps of each frame of point cloud; and all laser points of each frame of point cloud are motion compensated based on the rotation and translation matrix to obtain a point cloud after motion compensation. Compared with the prior art, this embodiment reduces the error of point cloud data by performing motion compensation on all laser points of each frame of point cloud, thereby improving the positioning accuracy of the autonomous driving system and reducing the risk of positioning drift.

[0137] refer to Figure 6 , Figure 6 Schematic diagram of the flow chart of the third embodiment of the laser radar motion compensation method of the present invention.

[0138] Based on the above embodiments, in this embodiment, step S40 includes steps S401 to S404:

[0139] Step S401: Based on the motion-compensated point cloud, the trajectory information of the dynamic target in the continuous frame segments corresponding to the motion-compensated point cloud is determined using the historical trajectory of the dynamic target.

[0140] Step S402: Determine the size optimization amount of the dynamic target using the true value algorithm according to the trajectory information.

[0141] Step S403: Determine the position translation of the dynamic target according to the dynamic true value target attribute, the laser radar delay compensation angle and the laser radar scanning time.

[0142] Step S404: performing motion compensation on the dynamic true value target attribute based on the size optimization amount and the position translation amount to obtain the dynamic true value target attribute after motion compensation.

[0143] It should be understood that through point cloud motion compensation, the dedistortion operation of each point can be quickly completed while parsing each laser point. However, through point cloud motion compensation, motion compensation can only be performed on static targets or dynamic targets with a small speed difference relative to the vehicle. However, for targets with a large speed difference relative to the vehicle, due to their own movement, errors in their own size and position will occur. Therefore, this embodiment performs motion compensation on dynamic targets as much as possible without affecting the original form of the point cloud.

[0144] For example, refer to Figure 7 , Figure 7 The following is a detailed flow chart of the dynamic true value target motion compensation in the laser radar motion compensation method of the present invention. The figure illustrates that through the dynamic true value target motion compensation, the dynamic target is motion compensated as much as possible without changing the original point cloud form. The laser radars targeted by this embodiment are mostly rotating laser radars used to build a true value system. The laser point cloud output by the rotating laser radar will be used to build a cloud-based true value system. The true value system generally includes true value algorithms such as 2D target detection, 3D target detection, and post-processing. By using the dedistorted point cloud of continuous frames after motion compensation, the dynamic true value target attributes are output through the target detection true value algorithm and post-processing. At the same time, trajectory optimization is used based on the dedistorted point cloud of continuous frames and the historical trajectory to output the trajectory information of the dynamic target in the continuous frame segment. According to the trajectory information, the size optimization amount of the dynamic target is determined by the true value algorithm, and the position translation amount of the dynamic target is determined according to the dynamic true value target attributes, the laser radar delay compensation angle ε and the laser radar scanning time. Finally, the dynamic true value target attributes are motion compensated based on the size optimization amount and the position translation amount to obtain the dynamic true value target attributes after motion compensation, that is, the dynamic true value motion compensated true value attributes.

[0145] For example, refer to Figure 8 , Figure 8 It is a reference diagram for calculating the velocity information of the dynamic target Car in the current k-th frame in the laser radar motion compensation method of the present invention. The orange frame in the coordinate system represents the vehicle, the blue frame represents the position of the dynamic target Car without displacement retreat, V x and V yRespectively represent the speed information in the x-axis and y-axis directions, Δd represents the movement distance, Δt represents the corresponding time difference, V represents the speed, and N represents the current frame number. In the kth frame of the continuous frame segment, it is necessary to perform motion compensation on the true value of the dynamic target Car in the kth frame. First, based on the true value algorithm, the 3D frame information of the dynamic target Car in the current kth frame is output, including the posture information and the corresponding speed information. To calculate the speed information of the dynamic target Car in the current kth frame, it is necessary to strictly convert the true value frame coordinates of the dynamic target Car to the world coordinate system, and calculate its movement distance delta_d and the corresponding time difference delta_t based on the multiple frames of information before and after the dynamic target Car (for example, 5 frames). The speed information of the dynamic target Car in the current kth frame is calculated as follows:

[0146]

[0147] refer to Fig. 9 , Fig. 9 This is a reference diagram for calculating the back-off displacement distance required for dynamic targets in the laser radar motion compensation method of the present invention. Based on the pre-configured laser radar starting resolution angle θ (this angle value will vary due to the laser radar and camera calibration scheme), calculate the time it takes for the laser radar to scan from the starting resolution angle θ to the tangent line of the left front of the dynamic target Car. Combined with the vehicle's speed information in the current frame Calculate the retraction displacement distance D that the dynamic target Car needs to make, in meters. The retraction displacement distance needs to be combined with the yaw angle information in the Euler angle of the dynamic target Car, and the position is retracted in the corresponding direction. As shown in the figure, the orange box represents the vehicle, and the blue box represents the position of the dynamic target Car that has not been retracted. After calculating the retraction displacement distance D, it is combined with the yaw angle orientation information in the Euler angle of the corresponding dynamic target Car to retract to the light orange position. The calculation formula of the retraction displacement distance D is as follows.

[0148]

[0149] Where D is the retreat displacement distance, α is the laser radar delay compensation angle, and F lidar Represents the force measured or generated by the lidar.

[0150] It should be noted that the size of the dynamic target Car in the current k-th frame needs to be compensated, because there is a large speed difference between the dynamic target Car and the vehicle in the current k-th frame. The time it takes for the laser radar to scan from the front to the rear of the vehicle combined with the large speed difference will cause the size of the dynamic target Car to be distorted. In order to solve this problem, trajectory optimization is used to first output the true value information of the dynamic target Car in the entire continuous frame segment, and then find the true value size of the frame with the smallest absolute speed of the dynamic target Car in the world coordinate system in the entire continuous frame segment to optimize the dynamic target Car in the k-th frame. For example, refer to Fig.10 , Fig.10 This is a reference diagram for optimizing the size of dynamic targets in the laser radar motion compensation method of the present invention. Assuming that for a 20s continuous frame segment, the dynamic target Car appears in the m-nth frame (m≤k≤n), and it is calculated that in the pth frame (m≤p≤n), the dynamic target Car has the smallest absolute speed in the world coordinate system. Then, the size value of the dynamic target Car corresponding to the pth frame is used to replace the size value of the dynamic target Car in the kth frame to complete its size optimization. As shown in the figure below, the absolute speed of the dynamic target Car on the right front side of the pth frame is the smallest, so the size of the gray box on the right front side of the corresponding figure is directly used to optimize the dynamic Car target corresponding to the kth frame.

[0151] This embodiment discloses that based on the point cloud after motion compensation, the historical trajectory of the dynamic target is used to determine the trajectory information of the dynamic target in the continuous frame segments corresponding to the point cloud after motion compensation; according to the trajectory information, the size optimization amount of the dynamic target is determined by the true value algorithm; according to the dynamic true value target attribute, the laser radar delay compensation angle and the laser radar scanning time, the position translation amount of the dynamic target is determined; based on the size optimization amount and the position translation amount, the dynamic true value target attribute is motion compensated to obtain the dynamic true value target attribute after motion compensation. This embodiment combines the time taken by the laser radar beam to scan from the laser radar starting resolution angle to the front or rear of the dynamic target or the lateral tangent of the target, calculates the position translation amount of the dynamic target to compensate the position of the dynamic true value target, and then optimizes the size of the target through the historical trajectory of the dynamic target and its size. Compared with the prior art, this embodiment realizes motion compensation for the dynamic target as much as possible without affecting the original form of the point cloud, and further improves the accuracy of the laser radar data.

[0152] In addition, an embodiment of the present invention further proposes a storage medium, on which a laser radar motion compensation program is stored. When the laser radar motion compensation program is executed by a processor, the steps of the laser radar motion compensation method described above are implemented.

[0153] Reference Fig.11 , Fig.11 This is a structural block diagram of the first embodiment of the laser radar motion compensation device of the present invention.

[0154] like Fig.11 As shown, the laser radar motion compensation device proposed in the embodiment of the present invention includes: a point cloud data parsing module 501, a point cloud motion compensation module 502, a point cloud data processing module 503 and a target motion compensation module 504.

[0155] The point cloud data parsing module 501 is used to parse the rosbag data packet collected by the lidar and generate a point cloud frame list based on the parsing results.

[0156] The point cloud motion compensation module 502 is used to perform motion compensation on all laser points of each frame of point cloud in the point cloud frame list by using a binary search method and an interpolation method based on the timestamp of the first laser point of each frame of point cloud and the vehicle posture information collected by the IMU sensor to obtain a point cloud after motion compensation.

[0157] The point cloud data processing module 503 is used to perform target detection and post-processing optimization on the motion compensated point cloud based on the truth value algorithm of the truth value system to generate dynamic truth value target attributes.

[0158] The target motion compensation module 504 is used to perform motion compensation on the dynamic true value target attribute by using the historical trajectory of the dynamic target to obtain the dynamic true value target attribute after motion compensation.

[0159] The point cloud data parsing module 501 is also used to parse the rosbag data packet collected by the laser radar based on the user manual corresponding to the laser radar and the starting parsing angle of the laser radar, and obtain the point cloud data packet according to the parsing result; determine the point cloud data contained in each frame of point cloud according to the data structure of the point cloud data packet, and generate a point cloud frame list based on the point cloud data contained in each frame of point cloud, wherein each element in the point cloud frame list represents the full amount of data constituting a frame of point cloud.

[0160] The embodiment of the present device discloses parsing rosbag data packets collected by a laser radar, and generating a point cloud frame list based on the parsing result; based on the timestamp of the first laser point of each frame of point cloud in the point cloud frame list and the self-vehicle posture information collected by the IMU sensor, all laser points of each frame of point cloud are motion compensated by using a binary search method and an interpolation method to obtain a point cloud after motion compensation; target detection and post-processing optimization are performed on the point cloud after motion compensation based on a truth value algorithm of a truth value system to generate dynamic truth value target attributes; and the dynamic truth value target attributes are motion compensated by using the historical trajectory of the dynamic target to obtain the dynamic truth value target attributes after motion compensation. Since the embodiment of the device uses binary search and interpolation methods to perform motion compensation on all laser points of each frame of point cloud in the point cloud frame list, and performs target detection and post-processing optimization on the point cloud after motion compensation based on the true value algorithm, and combines the historical trajectory of the dynamic target to perform motion compensation on the dynamic true value target attributes, compared with the prior art, the embodiment of the device can perform motion compensation on the dynamic true value target without affecting the original point cloud morphology, thereby improving the accuracy of the lidar data and solving the technical problem in the prior art that for dynamic targets with a large speed difference relative to the vehicle's speed, errors in their own size and position are generated due to their own movement, thereby reducing the accuracy of the lidar data.

[0161] Based on the above-mentioned first embodiment of the laser radar motion compensation device of the present invention, a second embodiment of the laser radar motion compensation device of the present invention is proposed.

[0162] In this embodiment, the point cloud motion compensation module 502 is also used to determine the approximate self-vehicle pose information corresponding to the first laser point timestamp of each frame of point cloud in the point cloud frame list by using a binary search method and an interpolation method based on the first laser point timestamp of each frame of point cloud and the self-vehicle pose information collected by the IMU sensor; traverse all laser points in each frame of point cloud to determine the approximate self-vehicle pose information corresponding to all laser point timestamps of each frame of point cloud; determine the rotation and translation matrix using the approximate self-vehicle pose information corresponding to all laser point timestamps of each frame of point cloud; perform motion compensation on all laser points in each frame of point cloud based on the rotation and translation matrix to obtain a point cloud after motion compensation.

[0163] The point cloud motion compensation module 502 is also used to convert all laser points of each frame of point cloud to the world coordinate system corresponding to the timestamp of each laser point through the rotation and translation matrix; use the approximate posture information of the vehicle corresponding to the first laser point timestamp of each frame of point cloud as reference data; and based on the reference data, convert all laser points of each frame of point cloud from the world coordinate system to the vehicle coordinate system corresponding to the first laser point timestamp of each frame of point cloud to obtain a point cloud after motion compensation.

[0164] The point cloud motion compensation module 502 is also used to construct an ascending timestamp list based on the self-vehicle posture information collected by the IMU sensor; based on the first laser point timestamp of each frame of point cloud in the point cloud frame list and the ascending timestamp list, use a binary search method to determine the first timestamp and the second timestamp in the ascending timestamp list that are closest to the first laser point timestamp of each frame of point cloud; respectively obtain the self-vehicle posture information corresponding to the first timestamp and the second timestamp; based on the self-vehicle posture information corresponding to the first timestamp and the second timestamp, use spherical interpolation and linear interpolation to determine the self-vehicle approximate posture information corresponding to the first laser point timestamp of each frame of point cloud.

[0165] The point cloud motion compensation module 502 is further used to determine the approximate Euler angle value of the vehicle corresponding to the first laser point timestamp of each frame of the point cloud by using the spherical interpolation method based on the approximate Euler angle value of the vehicle corresponding to the first timestamp and the second timestamp respectively; and to determine the approximate position information of the vehicle corresponding to the first laser point timestamp of each frame of the point cloud by using the linear interpolation method based on the approximate position information of the vehicle corresponding to the first timestamp and the second timestamp respectively.

[0166] Other embodiments or specific implementation methods of the laser radar motion compensation device of the present invention can refer to the above-mentioned method embodiments and will not be repeated here.

[0167] The present application provides a laser radar motion compensation device, which includes: at least one processor; and a memory that is communicatively connected to the at least one processor; wherein the memory stores instructions that can be executed by the at least one processor, and the instructions are executed by the at least one processor so that the at least one processor can execute the laser radar motion compensation method in the above-mentioned embodiment one.

[0168] Reference below Fig.12 , which shows a schematic diagram of the structure of a laser radar motion compensation device suitable for implementing the embodiment of the present application. The laser radar motion compensation device in the embodiment of the present application may include but is not limited to mobile terminals such as mobile phones, laptop computers, digital broadcast receivers, PDAs (Personal Digital Assistants), PADs (Portable Application Descriptions), PMPs (Portable Media Players), vehicle-mounted terminals (such as vehicle-mounted navigation terminals), etc., and fixed terminals such as digital TVs, desktop computers, etc. Fig.12 The laser radar motion compensation device shown is merely an example and should not bring any limitation to the functions and scope of use of the embodiments of the present application.

[0169] like Fig.12 As shown, the laser radar motion compensation device may include a processing device 1001 (such as a central processing unit, a graphics processor, etc.), which can perform various appropriate actions and processes according to a program stored in a read-only memory (ROM: Read Only Memory) 1002 or a program loaded from a storage device 1003 to a random access memory (RAM: Random Access Memory) 1004. In RAM1004, various programs and data required for the operation of the laser radar motion compensation device are also stored. The processing device 1001, ROM1002 and RAM1004 are connected to each other through a bus 1005. An input / output (I / O) interface 1006 is also connected to the bus. Generally, the following systems can be connected to the I / O interface 1006: an input device 1007 including, for example, a touch screen, a touchpad, a keyboard, a mouse, an image sensor, a microphone, an accelerometer, a gyroscope, etc.; an output device 1008 including, for example, a liquid crystal display (LCD: Liquid Crystal Display), a speaker, a vibrator, etc.; a storage device 1003 including, for example, a magnetic tape, a hard disk, etc.; and a communication device 1009. The communication device 1009 can allow the laser radar motion compensation device to communicate with other devices wirelessly or by wire to exchange data. Although the laser radar motion compensation device with various systems is shown in the figure, it should be understood that it is not required to implement or have all the systems shown. More or fewer systems can be implemented or have alternatively.

[0170] In particular, according to the embodiments disclosed in the present application, the process described above with reference to the flowchart can be implemented as a computer software program. For example, the embodiments disclosed in the present application include a computer program product, which includes a computer program carried on a computer-readable medium, and the computer program includes a program code for executing the method shown in the flowchart. In such an embodiment, the computer program can be downloaded and installed from a network through a communication device, or installed from a storage device 1003, or installed from a ROM 1002. When the computer program is executed by the processing device 1001, the above-mentioned functions defined in the method of the embodiment disclosed in the present application are executed.

[0171] The laser radar motion compensation device provided by the present application adopts the laser radar motion compensation method in the above embodiment, which can solve the technical problem in the prior art that for dynamic targets with a large speed difference relative to the speed of the vehicle, errors in their own size and position are generated due to their own movement, thereby reducing the accuracy of the laser radar data. Compared with the prior art, the beneficial effects of the laser radar motion compensation device provided by the present application are the same as the beneficial effects of the laser radar motion compensation method provided by the above embodiment, and the other technical features of the laser radar motion compensation device are the same as the features disclosed in the method of the previous embodiment, which will not be repeated here.

[0172] It should be understood that the various parts disclosed in this application can be implemented by hardware, software, firmware or a combination thereof. In the description of the above embodiments, specific features, structures, materials or characteristics can be combined in any one or more embodiments or examples in a suitable manner.

[0173] The above is only a specific implementation of the present application, but the protection scope of the present application is not limited thereto. Any person skilled in the art who is familiar with the present technical field can easily think of changes or substitutions within the technical scope disclosed in the present application, which should be included in the protection scope of the present application. Therefore, the protection scope of the present application should be based on the protection scope of the claims.

[0174] It should be noted that, in this article, the terms "include", "comprises" or any other variations thereof are intended to cover non-exclusive inclusion, so that a process, method, article or system including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or system. In the absence of further restrictions, an element defined by the sentence "comprises a ..." does not exclude the existence of other identical elements in the process, method, article or system including the element.

[0175] The serial numbers of the above embodiments of the present invention are only for description and do not represent the advantages or disadvantages of the embodiments.

[0176] Through the description of the above implementation methods, those skilled in the art can clearly understand that the above-mentioned embodiment methods can be implemented by means of software plus a necessary general hardware platform, and of course can also be implemented by hardware, but in many cases the former is a better implementation method. Based on such an understanding, the technical solution of the present invention, or the part that contributes to the prior art, can be embodied in the form of a software product, which is stored in a storage medium (such as a read-only memory / random access memory, a magnetic disk, or an optical disk), and includes a number of instructions for a terminal device (which can be a mobile phone, a computer, a server, an air conditioner, or a network device, etc.) to execute the methods described in each embodiment of the present invention.

[0177] The above are only preferred embodiments of the present invention, and are not intended to limit the patent scope of the present invention. Any equivalent structure or equivalent process transformation made using the contents of the present invention specification and drawings, or directly or indirectly applied in other related technical fields, are also included in the patent protection scope of the present invention.

Claims

1. A laser radar motion compensation method, characterized in that: The method comprises: Parse the rosbag data packet collected by the lidar and generate a point cloud frame list based on the parsing results; Based on the timestamp of the first laser point of each frame of the point cloud in the point cloud frame list and the vehicle posture information collected by the IMU sensor, motion compensation is performed on all laser points of each frame of the point cloud using a binary search method and an interpolation method to obtain a point cloud after motion compensation; Performing target detection and post-processing optimization on the motion-compensated point cloud based on a truth value algorithm of a truth value system to generate dynamic truth value target attributes; The dynamic true value target attribute is motion compensated by utilizing the historical trajectory of the dynamic target to obtain the dynamic true value target attribute after motion compensation.

2. The laser radar motion compensation method according to claim 1, characterized in that: The step of performing motion compensation on all laser points of each frame of point cloud using a binary search method and an interpolation method based on the timestamp of the first laser point of each frame of point cloud in the point cloud frame list and the self-vehicle posture information collected by the IMU sensor to obtain a point cloud after motion compensation includes: Based on the first laser point timestamp of each frame of point cloud in the point cloud frame list and the self-vehicle posture information collected by the IMU sensor, the approximate self-vehicle posture information corresponding to the first laser point timestamp of each frame of point cloud is determined by using a binary search method and an interpolation method; Traversing all laser points of each frame of the point cloud to determine the approximate position information of the vehicle corresponding to the timestamps of all laser points of each frame of the point cloud; Determine the rotation and translation matrix using the approximate pose information of the vehicle corresponding to the timestamps of all laser points in each frame of the point cloud; Motion compensation is performed on all laser points of each frame of the point cloud based on the rotation and translation matrix to obtain a point cloud after motion compensation.

3. The laser radar motion compensation method according to claim 1, characterized in that: The step of performing motion compensation on the dynamic true value target attribute by using the historical trajectory of the dynamic target to obtain the dynamic true value target attribute after motion compensation comprises: Based on the motion-compensated point cloud, using the historical trajectory of the dynamic target, determining the trajectory information of the dynamic target in the continuous frame segments corresponding to the motion-compensated point cloud; Determining the size optimization amount of the dynamic target using the true value algorithm according to the trajectory information; Determine the position translation of the dynamic target according to the dynamic true value target attribute, the laser radar delay compensation angle and the laser radar scanning time; The dynamic true value target attribute is motion compensated based on the size optimization amount and the position translation amount to obtain the dynamic true value target attribute after motion compensation.

4. The laser radar motion compensation method according to claim 2, characterized in that: The step of performing motion compensation on all laser points of each frame of point cloud based on the rotation and translation matrix to obtain a point cloud after motion compensation comprises: All laser points of each frame of the point cloud are converted to the world coordinate system corresponding to the timestamp of each laser point through the rotation and translation matrix; The approximate posture information of the vehicle corresponding to the timestamp of the first laser point of each frame of the point cloud is used as reference data; Based on the reference data, all laser points of each frame of the point cloud are converted from the world coordinate system to the vehicle coordinate system corresponding to the timestamp of the first laser point of each frame of the point cloud to obtain a motion compensated point cloud.

5. The laser radar motion compensation method according to claim 2, characterized in that: The step of determining the approximate posture information of the self-vehicle corresponding to the first laser point timestamp of each frame of point cloud in the point cloud frame list by using a binary search method and an interpolation method based on the first laser point timestamp of each frame of point cloud and the self-vehicle posture information collected by the IMU sensor includes: Construct an ascending timestamp list based on the vehicle posture information collected by the IMU sensor; Based on the first laser point timestamp of each frame of point cloud in the point cloud frame list and the ascending timestamp list, determine the first timestamp and the second timestamp in the ascending timestamp list that are closest to the first laser point timestamp of each frame of point cloud by using a binary search method; Respectively obtain the vehicle position information corresponding to the first timestamp and the second timestamp; Based on the self-vehicle posture information corresponding to the first timestamp and the second timestamp respectively, the approximate self-vehicle posture information corresponding to the first laser point timestamp of each frame of the point cloud is determined by using spherical interpolation method and linear interpolation method.

6. The laser radar motion compensation method according to claim 5, characterized in that: The self-vehicle approximate posture information includes the self-vehicle approximate Euler angle value and the self-vehicle approximate position information. The step of determining the self-vehicle approximate posture information corresponding to the first laser point timestamp of each frame point cloud by using spherical interpolation and linear interpolation based on the self-vehicle posture information corresponding to the first timestamp and the second timestamp respectively includes: Based on the ego vehicle approximate Euler angle values ​​corresponding to the first timestamp and the second timestamp respectively, determine the ego vehicle approximate Euler angle value corresponding to the first laser point timestamp of each frame point cloud by using spherical interpolation method; Based on the approximate position information of the vehicle corresponding to the first timestamp and the second timestamp respectively, the approximate position information of the vehicle corresponding to the first laser point timestamp of each frame of the point cloud is determined by linear interpolation.

7. The laser radar motion compensation method according to claim 1, characterized in that: The step of parsing the rosbag data packet collected by the laser radar and generating a point cloud frame list based on the parsing result includes: Parse the rosbag data packet collected by the laser radar based on the user manual corresponding to the laser radar and the starting parsing angle of the laser radar, and obtain the point cloud data packet according to the parsing result; The point cloud data contained in each frame of point cloud is determined according to the data structure of the point cloud data packet, and a point cloud frame list is generated based on the point cloud data contained in each frame of point cloud, wherein each element in the point cloud frame list represents the full amount of data constituting one frame of point cloud.

8. A laser radar motion compensation device, characterized in that: The device comprises: Point cloud data parsing module, used to parse rosbag data packets collected by lidar and generate point cloud frame lists based on the parsing results; A point cloud motion compensation module is used to perform motion compensation on all laser points of each frame of point cloud by using a binary search method and an interpolation method based on the timestamp of the first laser point of each frame of point cloud in the point cloud frame list and the self-vehicle posture information collected by the IMU sensor to obtain a point cloud after motion compensation; A point cloud data processing module, used for performing target detection and post-processing optimization on the motion-compensated point cloud based on a truth value algorithm of a truth value system, and generating dynamic truth value target attributes; The target motion compensation module is used to perform motion compensation on the dynamic true value target attribute by using the historical trajectory of the dynamic target to obtain the dynamic true value target attribute after motion compensation.

9. A laser radar motion compensation device, characterized in that: The device includes: a memory, a processor, and a laser radar motion compensation program stored in the memory and executable on the processor, wherein the laser radar motion compensation program is configured to implement the steps of the laser radar motion compensation method as described in any one of claims 1 to 7.

10. A storage medium, characterized in that: The storage medium stores a laser radar motion compensation program, and when the laser radar motion compensation program is executed by the processor, the steps of the laser radar motion compensation method according to any one of claims 1 to 7 are implemented.

Images