CN111708048B - Method, device and system for motion compensation of point cloud - Google Patents

Method, device and system for motion compensation of point cloud Download PDF

Info

Publication number
CN111708048B
CN111708048B CN202010839029.4A CN202010839029A CN111708048B CN 111708048 B CN111708048 B CN 111708048B CN 202010839029 A CN202010839029 A CN 202010839029A CN 111708048 B CN111708048 B CN 111708048B
Authority
CN
China
Prior art keywords
point cloud
cloud data
angular velocity
motion parameter
motion
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202010839029.4A
Other languages
Chinese (zh)
Other versions
CN111708048A (en
Inventor
刘哲铭
肖梓栋
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Suteng Innovation Technology Co Ltd
Original Assignee
Suteng Innovation Technology Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Suteng Innovation Technology Co Ltd filed Critical Suteng Innovation Technology Co Ltd
Priority to CN202011629746.0A priority Critical patent/CN112731450B/en
Priority to CN202010839029.4A priority patent/CN111708048B/en
Publication of CN111708048A publication Critical patent/CN111708048A/en
Application granted granted Critical
Publication of CN111708048B publication Critical patent/CN111708048B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/93—Lidar systems specially adapted for specific applications for anti-collision purposes
    • G01S17/931—Lidar systems specially adapted for specific applications for anti-collision purposes of land vehicles
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02A—TECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE
    • Y02A90/00—Technologies having an indirect contribution to adaptation to climate change
    • Y02A90/10—Information and communication technologies [ICT] supporting adaptation to climate change, e.g. for weather forecasting or climate simulation

Abstract

The application discloses a motion compensation device, a device and a system of point cloud. The method and the device can realize that the pose of each row point cloud in the point cloud data interval is corrected for multiple times based on the measurement frequency of the angular velocity data in the point cloud data frame, and solve the problems of poor correction precision caused by compensation based on the generation period of the point cloud data frame and large calculation amount caused by compensation with the point cloud data as granularity in the related technology.

Description

Method, device and system for motion compensation of point cloud
Technical Field
The application relates to the field of automatic driving, in particular to a method, a device and a system for motion compensation of point cloud.
Background
The laser radar is widely applied to the field of automatic driving, and can quickly establish a three-dimensional model around a vehicle through laser scanning, so that basic data are provided for high-precision map making, obstacle identification and accurate vehicle positioning, and the driving environment of the vehicle is sensed. When a bearing platform configured by the laser radar is in a motion state, a three-dimensional model of the surrounding environment established only by collecting laser point clouds is deformed, and the surrounding environment of the laser radar at a certain moment cannot be truly reflected.
Disclosure of Invention
The technical problem to be solved by the embodiments of the present application is to provide a method, an apparatus and a system for motion compensation of a laser point cloud, so as to implement compensation of a pose of the point cloud based on a motion parameter of a laser radar, and improve accuracy of the motion compensation.
In a first aspect, the present application provides a method for motion compensation of a point cloud, including: acquiring a plurality of motion parameter data measured by a measuring unit; wherein the motion parameter data comprises a timestamp and a motion parameter;
acquiring a point cloud data frame generated by a laser radar; wherein the point cloud data frame comprises a plurality of point cloud data, each point cloud data comprising a timestamp and a spatial location coordinate;
determining a first motion parameter and a second motion parameter according to the time stamp of the point cloud data; the first motion parameter is a motion parameter which is before the time stamp of the point cloud data and is closest to the time stamp of the point cloud data, and the second motion parameter is a motion parameter which is after the time stamp of the point cloud data and is closest to the time stamp of the point cloud data;
calculating a motion parameter of the point cloud data according to the first motion parameter and the second motion parameter;
calculating pose compensation quantity of the point cloud data according to the motion parameters of the point cloud data;
and carrying out pose compensation on the point cloud data according to the pose compensation amount.
Optionally, the motion parameter is an angular velocity or a linear velocity,
the acquiring of the plurality of motion parameter data measured by the measuring unit includes:
acquiring a plurality of motion parameter data measured by a measuring unit in a first period;
the acquiring of the point cloud data frame generated by the laser radar comprises:
acquiring a point cloud data frame generated by the laser radar in a second period; the second period is greater than the first period, and the second period is an integer multiple of the first period.
Optionally, the motion parameter is an angular velocity or a linear velocity;
before the point cloud data frame generated by the laser radar is obtained, the method further comprises the following steps:
calculating a current motion parameter according to the motion parameter data, and determining that the current motion parameter is greater than a motion parameter threshold; or
And calculating the current motion parameter according to the motion parameter data, and when the current motion parameter is less than or equal to a motion parameter threshold value, not performing pose compensation.
Optionally, the determining the first motion parameter and the second motion parameter according to the timestamp of the point cloud data includes:
dividing the point cloud data frame into a plurality of point cloud data intervals according to the first period;
determining a target point cloud data interval where the point cloud data are located;
and determining a first motion parameter according to the starting time stamp of the target point cloud data interval, and determining a second motion parameter according to the ending time stamp of the target point cloud data interval.
Optionally, the motion parameters include an angular velocity and a linear velocity;
the acquiring of the plurality of motion parameter data measured by the measuring unit includes:
acquiring a plurality of angular velocity data measured by a measuring unit at a first period;
acquiring a plurality of linear velocity data measured by the measuring unit in a third period;
the method for acquiring the point cloud data frame generated by the laser radar comprises the following steps:
acquiring a point cloud data frame generated by the laser radar in a second period; wherein the second period is greater than the first period and the third period, and the second period is an integer multiple of the first period and the third period.
Optionally, before the acquiring the point cloud data frame generated by the laser radar, the method further includes:
calculating the current angular velocity according to the angular velocity data, and determining that the current angular velocity is greater than an angular velocity threshold;
and calculating the current linear velocity according to the plurality of linear velocity data, and determining that the current linear velocity is greater than a linear velocity threshold.
Optionally, when the current angular velocity is less than or equal to an angular velocity threshold and the current linear velocity is greater than a linear velocity threshold, the pose compensation amount is a linear velocity pose compensation amount; or
When the current angular velocity is greater than an angular velocity threshold and the current linear velocity is less than or equal to the linear velocity threshold, the pose compensation amount is an angular velocity pose compensation amount; or
And when the current angular velocity is less than or equal to the angular velocity threshold and the current linear velocity is less than or equal to the linear velocity threshold, not performing pose compensation.
Optionally, the point cloud data frame is composed of a plurality of point cloud data intervals, each point cloud data interval is composed of a plurality of row point clouds, and the point cloud data in the same row point cloud have the same timestamp;
the pose compensation of the point cloud data according to the pose compensation amount comprises the following steps:
calculating a reference pose compensation amount between the first end point array point cloud and the second end point array point cloud according to the motion parameters of the point cloud data interval;
determining the row point cloud to be corrected;
calculating a proportionality coefficient of the row point cloud to be corrected according to a row distance between the row point cloud to be corrected and a reference row point cloud and according to the number of the row point clouds in the point cloud data interval; the reference array point cloud is the first end point array point cloud or the second end point array point cloud;
and carrying out pose correction on the column point cloud to be corrected according to the proportionality coefficient of the column point cloud to be corrected and the reference pose compensation amount.
Optionally, each point cloud data interval in the point cloud data frame is subjected to pose correction in a parallel manner, and each row point cloud in the point cloud data interval is corrected in a parallel manner.
Optionally, the motion parameters are angular velocity and linear velocity;
the calculating of the reference pose compensation amount between the first end point array point cloud and the second end point array point cloud according to the angular velocity of the point cloud data interval and the linear velocity of the point cloud data interval comprises:
calculating the reference pose compensation amount using the following formula:
Figure 383682DEST_PATH_IMAGE001
；
Figure 942839DEST_PATH_IMAGE002
；
Figure 796525DEST_PATH_IMAGE003
；
Figure 251778DEST_PATH_IMAGE004
；
Figure 807393DEST_PATH_IMAGE005
；
Figure 904662DEST_PATH_IMAGE006
；
wherein the content of the first and second substances,
Figure 409592DEST_PATH_IMAGE007
representing the anti-symmetric operation on the matrix,
Figure 661844DEST_PATH_IMAGE008
representing the corresponding angular speed of the point cloud data interval,
Figure 580122DEST_PATH_IMAGE009
indicating the linear velocity corresponding to the point cloud data interval, t1 indicating the start time stamp of the point cloud data interval, t2 indicating the end time stamp of the point cloud data interval,
Figure 356448DEST_PATH_IMAGE010
represents a time duration of the point cloud data interval,
Figure 778202DEST_PATH_IMAGE011
representing a flow space algebra corresponding to the relative motion;
Figure 231049DEST_PATH_IMAGE012
representing the displacement between the start timestamp and the end timestamp of the point cloud data interval, inverse () representing the inversion operation of the matrix,
Figure 371043DEST_PATH_IMAGE013
is composed of
Figure 951060DEST_PATH_IMAGE011
The anti-symmetric matrix of (a) is,
Figure 492900DEST_PATH_IMAGE014
the pose of the first end-column matrix is represented,
Figure 618113DEST_PATH_IMAGE015
the pose of the second endpoint column matrix is represented,
Figure 245404DEST_PATH_IMAGE016
and representing the reference pose compensation amount, wherein the reference column point cloud is the second end point column point cloud.
Optionally, the calculating the motion parameter of the point cloud data according to the first motion parameter and the second motion parameter includes:
and averaging the first angular velocity and the second angular velocity to obtain the angular velocity of the point cloud data.
Optionally, before the acquiring the point cloud data frame generated by the laser radar, the method further includes:
and determining that the point cloud data frame is in an uncompressed state.
In a second aspect, the present application provides a device for motion compensation of a point cloud, comprising:
an acquisition unit configured to acquire a plurality of pieces of motion parameter data measured by the measurement unit; wherein the motion parameter data comprises a timestamp and a motion parameter;
the acquisition unit is also used for acquiring a point cloud data frame generated by the laser radar; wherein the point cloud data frame comprises a plurality of point cloud data, each point cloud data comprising a timestamp and a spatial location coordinate;
a determining unit, configured to determine a first angular velocity and a second angular velocity according to a timestamp of the point cloud data; wherein the first angular velocity is an angular velocity before and closest to a timestamp of the point cloud data, and the second angular velocity is an angular velocity after and closest to the timestamp of the point cloud data;
a calculation unit configured to calculate an angular velocity of the point cloud data from the first angular velocity and the second angular velocity; calculating the pose compensation amount of the point cloud data according to the angular speed of the point cloud data;
and the compensation unit is used for carrying out pose compensation on the point cloud data according to the pose compensation amount.
In a third aspect, the present application proposes a computer program product comprising instructions which, when run on a computer, cause the computer to perform the method according to any one of claims 1 to 12.
In a fourth aspect, the present application proposes a device for motion compensation of a point cloud, comprising a processor and a memory, the memory being configured to store a computer program or instructions, the processor being configured to execute the computer program or instructions in the memory to implement the method according to any one of claims 1 to 12.
In a fifth aspect, the present application provides a motion compensation system, comprising: the motion compensation apparatus, lidar, measurement unit, and load-bearing platform of claim 13 or 15; the bearing platform is used for bearing the laser radar, the measuring unit and the motion compensation device, and the bearing platform is an unmanned aerial vehicle, a vehicle or a mechanical arm.
Optionally, the measurement unit is disposed inside the lidar, or the measurement unit is independently disposed outside the lidar.
In the embodiment of the application, a first motion parameter and a second motion parameter are determined according to a timestamp of point cloud data, the first motion parameter is a motion parameter which is before the timestamp of the point cloud data and is closest to the timestamp of the point cloud data, the second motion parameter is a motion parameter which is after the timestamp of the point cloud data and is closest to the timestamp of the point cloud data, the motion parameter of the point cloud data is calculated according to the first motion parameter and the second motion parameter, the pose compensation quantity of the point cloud data is calculated according to the motion parameter of the point cloud data, the pose of the point cloud data in a point cloud data frame is corrected for multiple times, the problem that the point cloud data are distorted due to sudden change of the motion parameter in the point cloud data frame in the related technology is solved, the measurement precision of the point cloud data is improved, in addition, for each point cloud data, the motion parameters of the two motion parameters which are closest to the previous time and the next time are determined based on the timestamp of the point cloud data, the motion parameters are prevented from being measured at the time stamp of each point cloud data, so that the times of measuring the motion parameters can be reduced, and the calculation amount of pose compensation can be reduced.
Drawings
In order to more clearly illustrate the technical solutions in the embodiments or the background art of the present application, the drawings required to be used in the embodiments or the background art of the present application will be described below. Fig. 1A is a schematic diagram of an architecture of a motion compensation system according to an embodiment of the present application;
fig. 1B is a schematic diagram of another architecture of a motion compensation system according to an embodiment of the present application;
fig. 1C is a schematic diagram of another architecture of a motion compensation system according to an embodiment of the present application;
fig. 2A is a schematic flowchart of a method for motion compensation of a point cloud according to an embodiment of the present disclosure;
fig. 2B is another schematic flow chart of a method for motion compensation of a point cloud according to an embodiment of the present disclosure;
fig. 2C is another schematic flow chart of a method for motion compensation of a point cloud according to an embodiment of the present disclosure;
fig. 3 is another schematic flow chart of a method for motion compensation of a point cloud according to an embodiment of the present disclosure;
fig. 4 is a schematic diagram of the motion compensation provided by the present embodiment;
fig. 5 is another schematic diagram of motion compensation provided by the present embodiment;
fig. 6 is another schematic diagram of motion compensation provided by the present embodiment;
fig. 7 is another schematic diagram of motion compensation provided by the present embodiment;
fig. 8 is a schematic structural diagram of a device for motion compensation of a point cloud according to an embodiment of the present disclosure;
fig. 9 is another schematic structural diagram of a device for motion compensation of a point cloud according to an embodiment of the present disclosure.
Detailed Description
In order to make the objects, features and advantages of the embodiments of the present application more obvious and understandable, the technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the drawings in the embodiments of the present application, and it is obvious that the described embodiments are only a part of the embodiments of the present application, and not all the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present application.
When the following description refers to the accompanying drawings, like numbers in different drawings represent the same or similar elements unless otherwise indicated. The embodiments described in the following exemplary embodiments do not represent all embodiments consistent with the present application. Rather, they are merely examples of apparatus and methods consistent with certain aspects of the application, as detailed in the appended claims.
In the description of the present application, it is to be understood that the terms "first," "second," and the like are used for descriptive purposes only and are not to be construed as indicating or implying relative importance. The specific meaning of the above terms in the present application can be understood in a specific case by those of ordinary skill in the art.
Referring to fig. 1A to 1C, a schematic structural diagram of a motion compensation system for point cloud provided in an embodiment of the present application is shown, where the motion compensation system includes: laser radar 10, measuring unit 12, motion compensation device 13 and bearing platform 11, and measuring unit 12 may be an inertial measuring unit. The bearing platform 11 is used for bearing and driving the laser radar to move, so that the laser radar 10 generates corresponding linear velocity and angular velocity. The lidar 10, the measurement unit 12 and the motion compensation device 13 are arranged on the load-bearing platform 11. Communication is performed between the motion compensation device 13 and the laser radar 10, and communication is performed between the motion compensation device 13 and the measurement unit 12. Bearing platform 11 is used for bearing laser radar 10, and bearing platform 11 can be vehicle, unmanned aerial vehicle or other devices, and this application does not make the restriction.
The lidar 10 may be a mechanical lidar, a solid-state lidar, or the like, and the application does not limit the type of the lidar.
The laser radar 10 is used for emitting detection laser signals, the detection laser signals are reflected after encountering obstacles to generate echo laser signals, the laser radar 10 generates point cloud data frames according to the echo laser signals, the point cloud data frames comprise a plurality of point cloud data, and each point cloud data comprises a spatial position coordinate, a timestamp and echo intensity. The measurement unit 12 is used for periodically measuring the motion state data of the lidar, such as: the measurement unit 12 measures angular velocity data, linear velocity data, acceleration data, and the like of the laser radar 10, and the motion state data measured by the measurement unit 12 further includes a time stamp. The motion compensation device 13 is configured to perform motion compensation on the point cloud data frame generated by the laser radar 10 according to the motion state parameter measured by the measurement unit 12, so as to reduce a pose error of the point cloud data due to motion.
Wherein the measurement unit 12 may be disposed inside the lidar 10 (shown in fig. 1C), integrated with the lidar 10, to enhance the compactness of the apparatus; or the measuring unit 12 is independently disposed outside the laser radar 10 to improve the heat dissipation efficiency of the laser radar.
Referring to fig. 2A, fig. 2A is a schematic flow chart of a method for motion compensation of a point cloud according to an embodiment of the present disclosure, where the method includes, but is not limited to, the following steps:
s201, acquiring a plurality of motion parameter data measured by the measuring unit.
The motion parameters represent the motion attributes of the laser radar, and include one or more of angular velocity, linear acceleration and angular acceleration. The angular velocity is used for the speed of the object moving around the circle center, the particles move circularly, the rotating angle in the time delta t is delta theta, and the ratio of delta theta to delta t is the angular velocity. The linear velocity is the instantaneous velocity of the particle as it moves in a curve. Linear acceleration is a physical quantity describing the time rate of change of the linear velocity and the direction of the rigid body, and the unit is the square of meters per second; the related concepts are average linear acceleration and instantaneous linear acceleration: the average linear acceleration refers to the ratio of the linear velocity change of the rotating rigid body from a certain instant to the corresponding time interval, and is called as the average linear acceleration; the instantaneous linear acceleration refers to a linear acceleration when the time interval during which the object is accelerated approaches 0. The angular acceleration represents the amount of change in angular velocity per unit time called angular acceleration;assuming that the angular velocity of the rigid body at the time t1 is ω 1 and the angular velocity of the rigid body at the time Δ t is ω 2, the average angular acceleration of the rigid body rotating around the fixed axis during the time Δ t is ω 2
Figure 160270DEST_PATH_IMAGE017
. Any of the above-mentioned motion parameters may be measured using a measurement unit, for example: the measuring unit is a device for measuring the three-axis attitude angle or acceleration of an object, and generally comprises three single-axis accelerometers and three single-axis gyroscopes, wherein the accelerometers are used for measuring the linear acceleration or linear velocity of the object, and the gyroscopes are used for measuring the angular velocity or angular acceleration of the object. In this embodiment, the measurement unit utilizes a gyroscope to measure lidar motion parameters and record timestamps of the motion parameters at a first cycle, and angular velocity may be expressed in radians per second or angles per second. The motion compensation device obtains a plurality of motion parameter data measured by the measuring unit, and the measuring unit can measure the motion parameter data in a periodic mode or in a non-periodic mode. It should be understood that the motion parameter measured by the measuring unit may be a motion parameter in any one or more of three axes, for example, when the carrying platform is a vehicle, the lidar is disposed on the vehicle, the vehicle mainly runs along a road surface, the change of angular velocity caused by the turning of the vehicle is also based on a horizontal plane or other planes (for example, a plane perpendicular to the horizontal plane), and the measuring unit measures the motion parameter in the horizontal plane.
In one or more embodiments, S201 further comprises:
and determining the current motion parameter of the laser radar according to the collected multiple pieces of motion parameter data, and determining that the current motion parameter is greater than a motion parameter threshold value.
The method for averaging the motion parameters can be arithmetic average, geometric average or weighted average, the method is not limited in the application, the motion parameter threshold can be determined according to actual requirements, when the current motion parameters of the laser radar are small, the collected point cloud data frame can only be slightly distorted, the measurement result is not greatly influenced, and the position and pose correction is not needed to be carried out on the point cloud data frame.
S202, point cloud data frames generated by the laser radar are obtained.
Taking a mechanical radar as an example, the mechanical laser radar completes scanning of the surrounding environment in a mechanical rotation mode, and the time of one rotation is the duration of a point cloud data frame. Generally, point cloud data frames are generated periodically, and the frame period of the laser radar is the duration of the point cloud data frames; the point cloud data frame comprises a plurality of point cloud data, each point cloud data comprises a spatial position coordinate and a timestamp, the timestamp represents the time for generating the point cloud data, the point cloud data frame comprises a plurality of row point clouds, the point cloud data in the same row point cloud have the same timestamp based on the scanning characteristics of the laser radar, and when the row point clouds are used as the minimum granularity to perform pose compensation and correction, each point cloud data is prevented from being corrected respectively, so that the calculation amount of correction is reduced. Under the condition that the motion parameters are periodically measured, the period of the point cloud data frame is greater than the measurement period of the motion parameter data, and the duration of the point cloud data frame is integral multiple of the period of the motion parameters, so that the laser radar motion parameter data can be measured by the measuring unit in one point cloud data frame for multiple times. For example: the period of the motion parameter is 0.01s, namely the measuring unit measures the motion parameter 100 times in 1s, the length of the period of the point cloud data frame is 0.1s, and then the measuring unit measures the motion parameter data 10 times in the point cloud data frame. In addition, the time boundary of the point cloud data frame and the measurement time of the motion parameter data are aligned, i.e. the start time or the end time of the point cloud data frame coincides with the measurement time of a certain motion parameter data.
S203, determining a first motion parameter and a second motion parameter according to the time stamp of the point cloud data.
The first motion parameter is a motion parameter before the time stamp of the point cloud data and closest to the time stamp of the point cloud data, and the second motion parameter is a motion parameter after the time stamp of the point cloud data and closest to the time stamp of the point cloud data. The method comprises the steps that point cloud data are any one of point cloud data frames, the point cloud data are provided with time stamps, a plurality of collected motion parameter data are respectively provided with time stamps, a motion parameter which is located before the time stamps of the point cloud data and has the minimum time interval is inquired in the plurality of collected motion parameter data according to the time stamps of the point cloud data, and the motion parameter is a first motion parameter; and inquiring the motion parameter which is positioned behind the time stamp of the point cloud data and has the minimum time interval in the collected multiple pieces of motion parameter data according to the time stamp of the point cloud data, wherein the motion parameter is a second motion parameter.
And S204, calculating the motion parameters of the point cloud data according to the first motion parameters and the second motion parameters.
The method for calculating the motion parameter of the point cloud data may be to perform weighted average on the first motion parameter and the second motion parameter to obtain the motion parameter of the point cloud data, for example: and taking the arithmetic mean, the geometric mean or the weighted mean of the first motion parameter and the second motion parameter as the motion parameter of the point cloud data.
And S205, calculating the pose compensation amount of the point cloud data interval according to the motion parameters of the point cloud data.
The pose compensation amount is a motion parameter pose compensation amount, the pose compensation amount comprises a rotation compensation amount, and the point cloud data interval comprises a plurality of row point clouds. Alternatively, the motion compensation device may pre-store or pre-configure a reference column point cloud, for example: the motion compensation device can estimate the pose of the point cloud to be corrected based on the motion parameters of the point cloud data interval and the position of the point cloud to be corrected in the point cloud data interval, and then calculates the pose compensation amount between the point cloud to be corrected and the reference point cloud, wherein each point cloud to be corrected has different pose compensation amounts.
And S206, carrying out pose compensation on the point cloud data according to the pose compensation quantity of the point cloud data interval.
Rotation compensation and/or displacement compensation are performed on the point cloud data interval according to the pose compensation amount calculated in S205, the point cloud data frame includes a plurality of point cloud data, the pose compensation process of each point cloud data is the same, and reference may be made to the description of S201 to S206, which is not described herein again.
In one or more embodiments, acquiring a plurality of motion parameter data measured by a measurement unit includes:
acquiring a plurality of motion parameter data measured by a measuring unit in a first period;
the acquiring of the point cloud data frame generated by the laser radar comprises:
and acquiring a point cloud data frame generated by the laser radar in a second period, wherein the second period is greater than the first period and is an integral multiple of the first period.
For example: the laser radar acquires a point cloud data frame at the frequency of 10Hz and acquires a plurality of motion parameter data at the frequency of 100Hz, the second period of the point cloud data frame is 10 times of the first period of the motion parameter data, and then 10 motion parameter data exist in 1 point cloud data frame.
In one or more embodiments, before the acquiring the frame of point cloud data generated by the lidar, the method further includes:
calculating the current motion parameter according to the motion parameter data, judging whether the current motion parameter is greater than a motion parameter threshold value, and if so, executing the step of acquiring a point cloud data frame generated by the laser radar; or if not, not executing pose compensation.
The method for averaging the motion parameters can be arithmetic average, geometric average or weighted average, the method is not limited in the application, the motion parameter threshold can be determined according to actual requirements, when the current motion parameters of the laser radar are small, the collected point cloud data frame can only be slightly distorted, the measurement result is not greatly influenced, and the position and pose correction is not needed to be carried out on the point cloud data frame.
In one or more embodiments, determining the first motion parameter and the second motion parameter from the timestamp of the point cloud data comprises:
dividing the point cloud data frame into a plurality of point cloud data intervals according to a first period;
determining a target point cloud data interval where the point cloud data are located;
and determining a first motion parameter according to the starting time stamp of the target point cloud data interval, and determining a second motion parameter according to the ending time stamp of the target point cloud data interval.
For example: the period of the point cloud data frame is 10 times of the motion parameter data, and the point cloud data frame is divided into 10 point cloud data intervals according to the period of the motion parameter data: the method comprises the steps that point cloud data intervals 1-10 are formed, a target point cloud data interval is determined in the 10 point cloud data intervals according to time stamps of point cloud data, the target point cloud data interval is assumed to be a point cloud data interval 1, the starting time stamp of the point cloud data interval 1 corresponds to a first motion parameter (motion parameter 1), and the ending time stamp of the point cloud data interval 1 corresponds to a second motion parameter (motion parameter 2).
In this embodiment, the point cloud data frame is divided into a plurality of point cloud data intervals by using the acquisition period of the motion parameter, and the motion compensation is performed on the point cloud data in each point cloud data interval, so that the problem of inaccurate compensation caused by sudden change of the motion parameter in the point cloud data frame due to the motion compensation on the whole point cloud data frame in the related art is solved, and the precision of the motion compensation can be improved; in addition, the actual motion parameters of the point cloud data frames are calculated through the motion parameters aligned by the starting time stamps and the motion parameters aligned by the finishing time stamps of the point cloud data frames, so that the calculation cost can be reduced under the condition of ensuring the accuracy, the time cost for calculating the motion parameters of the point cloud data is reduced, and the motion compensation efficiency is improved.
In one or more embodiments, the point cloud data frame is composed of a plurality of point cloud data intervals, each point cloud data interval is composed of a plurality of row point clouds, and the point cloud data in the same row point cloud have the same time stamp;
the pose compensation of the point cloud data according to the pose compensation amount comprises the following steps:
calculating a reference pose compensation amount between the first end point array point cloud and the second end point array point cloud according to the motion parameters of the point cloud data interval;
determining the row point cloud to be corrected;
calculating a proportionality coefficient of the row point cloud to be corrected according to a row distance between the row point cloud to be corrected and a reference row point cloud and according to the number of the row point clouds in the point cloud data interval; the reference array point cloud is the first end point array point cloud or the second end point array point cloud;
and carrying out pose correction on the column point cloud to be corrected according to the proportionality coefficient of the corrected column point cloud and the reference pose compensation amount.
According to the description of the figure 2A, a first motion parameter and a second motion parameter are determined according to a time stamp of point cloud data, the first motion parameter is a motion parameter which is before the time stamp of the point cloud data and is closest to the time stamp of the point cloud data, the second motion parameter is a motion parameter which is after the time stamp of the point cloud data and is closest to the time stamp of the point cloud data, the motion parameter of the point cloud data is calculated according to the first motion parameter and the second motion parameter, the pose compensation quantity of the point cloud data is calculated according to the motion parameter of the point cloud data, the pose of the point cloud data in a point cloud data frame is corrected for multiple times, the motion parameter which is closest to the point cloud data can be found according to the time stamp of each point cloud, the pose compensation quantity of each point cloud is calculated, and therefore the problems that the correction precision is poor due to the compensation based on the generation period of the point cloud data frame and the calculation quantity is large due to the compensation based on the point .
Referring to fig. 2B, fig. 2B is a schematic flow chart of a method for motion compensation of point clouds according to an embodiment of the present disclosure, where the embodiment takes a motion parameter as an angular velocity as an example to explain the method for motion compensation of the present disclosure, and the method includes, but is not limited to, the following steps:
s301, a plurality of angular velocity data measured by the measuring unit are obtained.
The motion parameter represents a motion attribute of the laser radar, and includes an angular velocity and/or a linear velocity, and accordingly, the motion parameter data is angular velocity data, which is exemplified by the present embodiment. The measuring unit is a device for measuring the three-axis attitude angle or acceleration of an object, and generally comprises three single-axis accelerometers and three single-axis gyroscopes, wherein the accelerometers are used for measuring the acceleration of the object, and the gyroscopes are used for measuring the angular velocity of the object. In the present embodiment, the measurement unit uses a gyroscope to measure the lidar angular velocity and record a time stamp of the angular velocity, which may be expressed in radians per second or angles per second, for a first period. The motion compensation device obtains a plurality of angular velocity data measured by the measuring unit, and the measuring unit can measure the angular velocity data in a periodic mode or in a non-periodic mode. It should be understood that the angular velocity measured by the measuring unit may be the angular velocity in any one or more of three axes, for example, when the carrying platform is a vehicle, the lidar is disposed on the vehicle, the vehicle mainly runs along a road surface, the angular velocity change caused by the vehicle turning is also based on a horizontal plane or other plane (for example, a plane perpendicular to the horizontal plane), and the measuring unit measures the angular velocity in the horizontal plane or other plane (for example, a plane perpendicular to the horizontal plane).
In one or more embodiments, S301 further comprises:
and determining the current angular speed of the laser radar according to the collected multiple pieces of angular speed data, and determining that the current angular speed is greater than an angular speed threshold value.
The method for averaging the collected angular velocities can be arithmetic average, geometric average or weighted average, the method is not limited in the application, the angular velocity threshold can be determined according to actual requirements, when the current angular velocity of the laser radar is small, the collected point cloud data frame can only be slightly distorted, the measurement result is not greatly influenced, and the position and pose correction is not needed to be carried out on the point cloud data frame.
And S302, acquiring a point cloud data frame generated by the laser radar.
Taking a mechanical radar as an example, the mechanical laser radar completes scanning of the surrounding environment in a mechanical rotation mode, and the time of one rotation is the duration of a point cloud data frame. Generally, the point cloud data frame is generated periodically, and the frame period of the laser radar is the duration of the point cloud data frame. The point cloud data frame comprises a plurality of point cloud data, each point cloud data comprises a spatial position coordinate and a timestamp, the timestamp represents the time for generating the point cloud data, the point cloud data frame comprises a plurality of row point clouds, the point cloud data in the same row point cloud have the same timestamp based on the scanning characteristics of the laser radar, and when the row point clouds are used as the minimum granularity to perform pose compensation and correction, each point cloud data is prevented from being corrected respectively, so that the calculation amount of correction is reduced. Under the condition that the angular velocity is measured periodically, the period of the point cloud data frame is greater than the measurement period of the angular velocity data, and the duration of the point cloud data frame is integral multiple of the period of the angular velocity, so that the measurement unit can easily understand that the measurement unit can measure the angular velocity data of the laser radar in one point cloud data frame for multiple times. For example: the period of the angular velocity is 0.01s, that is, the measuring unit measures the angular velocity 100 times in 1s, and the length of the period of the point cloud data frame is 0.1s, then the measuring unit measures the angular velocity data 10 times in the point cloud data frame 10. In addition, the time boundaries of the point cloud data frames and the measurement times of the angular velocity data are aligned, i.e. the start time or the end time of a point cloud data frame coincides with the measurement time of a certain angular velocity data.
S303, determining a first angular velocity and a second angular velocity according to the time stamp of the point cloud data.
The first angular velocity is an angular velocity before the timestamp of the point cloud data and closest to the timestamp of the point cloud data, and the second angular velocity is an angular velocity after the timestamp of the point cloud data and closest to the timestamp of the point cloud data. The method comprises the steps that point cloud data are any one of point cloud data frames, the point cloud data are provided with time stamps, a plurality of collected angular velocity data are provided with time stamps respectively, the angular velocity which is located before the time stamps of the point cloud data and has the minimum time interval is inquired in the plurality of collected angular velocity data according to the time stamps of the point cloud data, and the angular velocity is a first angular velocity; and inquiring the angular velocity which is positioned behind the time stamp of the point cloud data and has the minimum time interval in the collected multiple pieces of angular velocity data according to the time stamp of the point cloud data, wherein the angular velocity is the second angular velocity.
S304, calculating the angular speed of the point cloud data according to the first angular speed and the second angular speed.
The method for calculating the angular velocity of the point cloud data may be to perform weighted average on the first angular velocity and the second angular velocity to obtain the angular velocity of the point cloud data, for example: and taking an arithmetic average, a geometric average or a weighted average of the first angular velocity and the second angular velocity as the angular velocity of the point cloud data.
And S305, calculating the pose compensation amount of the point cloud data interval according to the angular speed of the point cloud data.
The pose compensation amount is angular velocity pose compensation amount, the pose compensation amount comprises rotation compensation amount, and the point cloud data interval comprises a plurality of row point clouds. Alternatively, the motion compensation device may pre-store or pre-configure a reference column point cloud, for example: the motion compensation device can be used for estimating the pose of the point cloud to be corrected based on the angular velocity of the point cloud data interval and the position of the point cloud to be corrected in the point cloud data interval, then calculating the pose compensation amount between the point cloud to be corrected and the reference point cloud, and each point cloud to be corrected has different pose compensation amounts.
And S306, carrying out pose compensation on the point cloud data according to the pose compensation quantity of the point cloud data interval.
Rotation compensation and/or displacement compensation are performed on the point cloud data interval according to the pose compensation amount calculated in S305, the point cloud data frame includes a plurality of point cloud data, the pose compensation process of each point cloud data is the same, and reference may be made to the description of S301 to S306, which is not described herein again.
In one or more embodiments, acquiring a plurality of angular velocity data measured by a measurement unit includes:
acquiring a plurality of angular velocity data measured by a measuring unit at a first period;
the acquiring of the point cloud data frame generated by the laser radar comprises:
and acquiring a point cloud data frame generated by the laser radar in a second period, wherein the second period is greater than the first period and is an integral multiple of the first period.
For example: the laser radar acquires a point cloud data frame at the frequency of 10Hz and acquires a plurality of angular velocity data at the frequency of 100Hz, the second period of the point cloud data frame is 10 times of the first period of the angular velocity data, and then 10 angular velocity data exist in 1 point cloud data frame.
In one or more embodiments, before the acquiring the frame of point cloud data generated by the lidar, the method further includes:
calculating the current angular velocity according to the angular velocity data, judging whether the current angular velocity is greater than an angular velocity threshold value, and if so, executing the step of acquiring a point cloud data frame generated by the laser radar; or if not, not executing pose compensation.
The method for averaging the collected angular velocities can be arithmetic average, geometric average or weighted average, the method is not limited in the application, the angular velocity threshold can be determined according to actual requirements, when the current angular velocity of the laser radar is small, the collected point cloud data frame can only be slightly distorted, the measurement result is not greatly influenced, and the position and pose correction is not needed to be carried out on the point cloud data frame.
In one or more embodiments, determining the first angular velocity and the second angular velocity from the time stamp of the point cloud data comprises:
dividing the point cloud data frame into a plurality of point cloud data intervals according to a first period;
determining a target point cloud data interval where the point cloud data are located;
and determining a first angular velocity according to the start time stamp of the target point cloud data interval, and determining a second angular velocity according to the end time stamp of the target point cloud data interval.
For example: referring to fig. 5, the period of the point cloud data frame is 10 times that of the angular velocity data, and the point cloud data frame is divided into 10 point cloud data intervals according to the period of the angular velocity data: the method comprises the steps that point cloud data intervals 1-10 are formed, a target point cloud data interval is determined in the 10 point cloud data intervals according to time stamps of point cloud data, the target point cloud data interval is assumed to be a point cloud data interval 1, a starting time stamp of the point cloud data interval 1 corresponds to a first angular velocity (angular velocity 1), and an ending time stamp of the point cloud data interval 1 corresponds to a second angular velocity (angular velocity 2).
In this embodiment, the point cloud data frame is divided into a plurality of point cloud data intervals by using the acquisition cycle of the angular velocity, and the point cloud data in each point cloud data interval is subjected to motion compensation, so that the problem of inaccurate compensation caused by sudden change of the angular velocity in the point cloud data frame due to the motion compensation of the whole point cloud data frame in the related art is solved, and the precision of the motion compensation can be improved; in addition, the actual angular velocity of each point cloud data frame is calculated through the angular velocity aligned by the start time stamp and the angular velocity aligned by the end time stamp of each point cloud data frame, so that the calculation overhead can be reduced under the condition of ensuring the accuracy, the time overhead for calculating the angular velocity of the point cloud data can be reduced, and the efficiency of motion compensation can be improved.
In one or more embodiments, the point cloud data frame is composed of a plurality of point cloud data intervals, each point cloud data interval is composed of a plurality of row point clouds, and the point cloud data in the same row point cloud have the same time stamp;
the pose compensation of the point cloud data according to the pose compensation amount comprises the following steps:
calculating a reference pose compensation amount between the first end point array point cloud and the second end point array point cloud according to the angular speed of the point cloud data interval;
determining the row point cloud to be corrected;
calculating a proportionality coefficient of the row point cloud to be corrected according to a row distance between the row point cloud to be corrected and a reference row point cloud and according to the number of the row point clouds in the point cloud data interval; the reference array point cloud is the first end point array point cloud or the second end point array point cloud;
and carrying out pose correction on the column point cloud to be corrected according to the proportionality coefficient of the corrected column point cloud and the reference pose compensation amount.
According to the description of fig. 2B, a first angular velocity and a second angular velocity are determined according to a timestamp of the point cloud data, the first angular velocity is an angular velocity before the timestamp of the point cloud data and closest to the timestamp of the point cloud data, the second angular velocity is an angular velocity after the timestamp of the point cloud data and closest to the timestamp of the point cloud data, the angular velocity of the point cloud data is calculated according to the first angular velocity and the second angular velocity, a pose compensation amount of the point cloud data is calculated according to the angular velocity of the point cloud data, the pose of the point cloud data in the point cloud data frame is corrected for multiple times, and the problems that correction accuracy is poor due to compensation based on a generation period of the point cloud data frame and operation amount is large due to compensation with the point cloud data as granularity in the related technology are solved.
Referring to fig. 2C, another schematic flow chart of a method for motion compensation of a point cloud according to an embodiment of the present application is shown, where in the embodiment of the present application, the method for motion compensation includes:
s401, acquiring a plurality of linear velocity data measured by the measuring unit.
Wherein, linear velocity data package linear velocity and timestamp, linear velocity represent the displacement in the unit interval, and the timestamp represents the time of measuring the linear velocity, and measuring unit can measure laser radar's linear velocity through the accelerometer, for example: when the lidar is carried on a vehicle, the linear velocity may be a linear velocity in a direction of travel of the vehicle. The measuring unit can acquire a plurality of linear velocity data according to a preset period, the period can be determined according to actual requirements, and the method is not limited in the application.
In one or more embodiments, before acquiring the frame value of the point cloud data generated by the laser radar, the method further includes:
and determining the current linear velocity of the laser radar according to the collected linear velocity data, and determining that the current linear velocity is greater than a linear velocity threshold.
The method for averaging the collected linear velocities can be arithmetic average, geometric average or weighted average, the application is not limited, a linear velocity threshold can be determined according to actual requirements, when the current linear velocity of the laser radar is small, the collected point cloud data frame is only slightly distorted, the influence on a measurement result is small, and the position and pose correction is not required to be performed on the point cloud data frame.
S402, point cloud data frames generated by the laser radar are obtained.
The scanning of the mechanical laser radar to the surrounding environment is completed in a mechanical rotation mode, and the time of one rotation is the duration of the point cloud data frame. Generally, the point cloud data frame is generated periodically, and the frame period of the laser radar is the duration of the point cloud data frame. The point cloud data frame comprises a plurality of point cloud data, each point cloud data comprises a spatial position coordinate and a timestamp, the timestamps represent the time for generating the point cloud data, the point cloud data frame is composed of a plurality of row point clouds, the point cloud data in the same row point cloud have the same timestamp based on the scanning characteristics of the laser radar, the point cloud can be arranged to be the minimum granularity for pose compensation and correction, and the effect of reducing the corrected operation amount is achieved. Under the condition that the linear velocity is measured periodically, the period of the point cloud data frame is greater than the measurement period of the linear velocity data, and the duration of the point cloud data frame is integral multiple of the period of the linear velocity, so that the measurement unit can measure the linear velocity data of the laser radar in one point cloud data frame for multiple times.
And S403, determining a first linear velocity and a second linear velocity according to the time stamp of the point cloud data.
The first linear velocity is a linear velocity before the timestamp of the point cloud data and closest to the timestamp of the point cloud data, and the second linear velocity is a linear velocity after the timestamp of the point cloud data and closest to the timestamp of the point cloud data. The method comprises the steps that point cloud data are any one of point cloud data frames, the point cloud data are provided with time stamps, a plurality of collected linear velocity data are respectively provided with time stamps, a linear velocity which is located before the time stamps of the point cloud data and has the minimum time interval is inquired in the plurality of collected linear velocity data according to the time stamps of the point cloud data, and the linear velocity is a first linear velocity; and inquiring the linear velocity which is behind the time stamp of the point cloud data and has the minimum time interval in the collected multiple linear velocities according to the time stamp of the point cloud data, wherein the linear velocity is the second linear velocity.
S404, calculating the linear velocity of the point cloud data according to the first linear velocity and the second linear velocity.
The method for calculating the linear velocity of the point cloud data may be to perform weighted average on the first linear velocity and the second linear velocity to obtain the linear velocity of the point cloud data, for example: and taking the arithmetic average, the geometric average or the weighted average of the first linear speed and the second linear speed as the linear speed of the point cloud data.
S405, calculating the pose compensation amount of the point cloud data according to the linear speed of the point cloud data.
The pose compensation amount is linear velocity pose compensation amount, the pose compensation amount comprises rotation compensation amount, and the point cloud data interval comprises a plurality of row point clouds. Alternatively, the motion compensation device may pre-store or pre-configure a reference column point cloud, for example: the motion compensation device can be used for estimating the pose of the point cloud to be corrected based on the linear speed of the point cloud data interval and the position of the point cloud to be corrected in the point cloud data interval, then calculating the pose compensation amount between the point cloud to be corrected and the reference point cloud, and each point cloud to be corrected has different pose compensation amounts.
And S406, performing pose compensation on the point cloud data according to the pose compensation amount.
Rotation compensation and/or displacement compensation are performed on the point cloud data interval according to the pose compensation amount obtained by calculation in the step S405, the point cloud data frame includes a plurality of point cloud data, the pose compensation process of each point cloud data is the same, and the descriptions of S401 to S406 can be referred to, which is not described herein again.
In one or more embodiments, acquiring a plurality of linear velocity data measured by a measurement unit includes:
acquiring a plurality of linear velocity data measured by a measuring unit at a first period;
the acquiring of the point cloud data frame generated by the laser radar comprises:
and acquiring a point cloud data frame generated by the laser radar in a second period, wherein the second period is greater than the first period and is an integral multiple of the first period.
For example: the laser radar acquires a point cloud data frame at the frequency of 10Hz and a plurality of linear velocity data at the frequency of 100Hz, the second period of the point cloud data frame is 10 times of the first period of the linear velocity data, and then 10 linear velocity data exist in 1 point cloud data frame.
In one or more embodiments, before the acquiring the frame of point cloud data generated by the lidar, the method further includes:
calculating the current linear velocity according to the linear velocity data, judging whether the current linear velocity is greater than a linear velocity threshold value, and if so, executing the step of acquiring a point cloud data frame generated by the laser radar; or if not, not executing pose compensation.
The method for averaging the collected linear velocities can be arithmetic average, geometric average or weighted average, the application is not limited, a linear velocity threshold can be determined according to actual requirements, when the current linear velocity of the laser radar is small, the collected point cloud data frame is only slightly distorted, the influence on a measurement result is small, and the position and pose correction is not required to be performed on the point cloud data frame.
In one or more embodiments, determining the first and second linear velocities from the timestamps of the point cloud data includes:
dividing the point cloud data frame into a plurality of point cloud data intervals according to a first period;
determining a target point cloud data interval where the point cloud data are located;
determining a first linear velocity according to a start timestamp of the target point cloud data interval, and determining a second linear velocity according to an end timestamp of the target point cloud data interval.
For example: referring to fig. 5, the period of the point cloud data frame is 10 times of the linear velocity data, and the point cloud data frame is divided into 10 point cloud data intervals according to the period of the linear velocity data: the method comprises the following steps that point cloud data intervals 1-10 are defined, a target point cloud data interval is determined in the 10 point cloud data intervals according to time stamps of point cloud data, the target point cloud data interval is assumed to be the point cloud data interval 1, a starting time stamp of the point cloud data interval 1 corresponds to a first linear speed (linear speed 1), and an ending time stamp of the point cloud data interval 1 corresponds to a second linear speed (linear speed 2).
In the embodiment, the point cloud data frame is divided into a plurality of point cloud data intervals by using the acquisition cycle of the linear velocity, and the point cloud data in each point cloud data interval is subjected to motion compensation, so that the problem of inaccurate compensation caused by sudden change of the linear velocity in the point cloud data frame due to the fact that the motion compensation is carried out on the whole point cloud data frame in the related art is solved, and the precision of the motion compensation can be improved; in addition, the actual linear velocity of each point cloud data frame is calculated through the linear velocity aligned with the start timestamp and the linear velocity aligned with the end timestamp of each point cloud data frame, so that the calculation cost can be reduced under the condition of ensuring the accuracy, the time cost for calculating the linear velocity of the point cloud data can be reduced, and the motion compensation efficiency can be improved.
In one or more embodiments, the point cloud data frame is composed of a plurality of point cloud data intervals, each point cloud data interval is composed of a plurality of row point clouds, and the point cloud data in the same row point cloud have the same time stamp;
the pose compensation of the point cloud data according to the pose compensation amount comprises the following steps:
calculating a reference pose compensation amount between the first end point array point cloud and the second end point array point cloud according to the linear speed of the point cloud data interval;
determining the row point cloud to be corrected;
calculating a proportionality coefficient of the row point cloud to be corrected according to a row distance between the row point cloud to be corrected and a reference row point cloud and according to the number of the row point clouds in the point cloud data interval; the reference array point cloud is the first end point array point cloud or the second end point array point cloud;
and carrying out pose correction on the column point cloud to be corrected according to the proportionality coefficient of the corrected column point cloud and the reference pose compensation amount.
According to the description of fig. 2C, a first linear velocity and a second linear velocity are determined according to the timestamp of the point cloud data, the first linear velocity is a linear velocity before the timestamp of the point cloud data and closest to the timestamp of the point cloud data, the second linear velocity is a linear velocity after the timestamp of the point cloud data and closest to the timestamp of the point cloud data, the linear velocity of the point cloud data is calculated according to the first linear velocity and the second linear velocity, the pose compensation amount of the point cloud data is calculated according to the linear velocity of the point cloud data, the pose of the point cloud data in the point cloud data frame is corrected for multiple times, and the problems that in the related art, the correction precision is poor due to compensation based on the generation period of the point cloud data frame and the operation amount is large due to compensation with the point cloud data as granularity are solved.
Referring to fig. 3, another schematic flow chart of a method for motion compensation of a point cloud according to an embodiment of the present application is shown, where the method for motion compensation includes:
s501, a plurality of angular velocity data measured by the measuring unit at a first cycle.
Wherein the angular velocity comprises the angular velocity and a time stamp indicating the time of the measured angular velocity. The measuring unit is a device for measuring the three-axis attitude angle or acceleration of an object, and generally comprises three single-axis accelerometers and three single-axis gyroscopes, wherein the accelerometers are used for measuring the acceleration of the object, and the gyroscopes are used for measuring the angular velocity of the object. In this embodiment, the measurement unit measures angular velocity data of the laser radar at a first period through the gyroscope, that is, the measurement unit measures angular velocity data of the laser radar at intervals of the first period, and the motion compensation device obtains the angular velocity data periodically measured by the measurement unit. It should be understood that the angular velocity measured by the measurement unit may be an angular velocity in any one or more of three axes, for example: when the carrying platform is a vehicle, the laser radar is arranged on the vehicle, the vehicle mainly runs along a road surface, the angular speed change caused by the turning of the vehicle is also based on a horizontal plane or other planes (for example, a plane perpendicular to the horizontal plane), and the measuring unit measures the angular speed on the horizontal plane or other planes (for example, a plane perpendicular to the horizontal plane).
In one or more embodiments, S501 further comprises:
and determining the current angular speed of the laser radar according to the collected multiple pieces of angular speed data, and determining that the current angular speed is greater than an angular speed threshold value.
The method for averaging the collected multiple angular velocities can be arithmetic average, geometric average or weighted average, the method is not limited in the application, the angular velocity threshold can be determined according to actual requirements, when the current angular velocity of the laser radar is small, the collected point cloud data frame can only be slightly distorted, and the influence on the measurement result is small.
And S502, acquiring a point cloud data frame generated by the laser radar in a second period.
The laser radar scans the surrounding environment through line scanning and column scanning to generate a point cloud data frame, wherein the frame period of the laser radar is the second period, and the second period is the duration of the point cloud data frame. The point cloud data frame comprises a plurality of point cloud data, each point cloud data comprises a spatial position coordinate and a time stamp, the spatial position coordinate is a coordinate in a spatial coordinate system, and the time stamp is the time for measuring the spatial position coordinate. The time length of the point cloud data frame is greater than the first period, and the time length of the point cloud data frame is integral multiple of the first period, so that the measuring unit can easily understand that the measuring unit can measure the angular velocity data in the point cloud data frame for multiple times. The time boundaries of the point cloud data frame and the measurement times of the angular velocity data are aligned, i.e. the start time or end time of the point cloud data frame and the measurement times of the angular velocity data are aligned. The point cloud data frame comprises a plurality of point cloud data, the point cloud data frames are distributed in a row-column mode, namely the point cloud data frame comprises a plurality of row point clouds or a plurality of column point clouds, and the point cloud data in the same column point cloud have the same time stamp.
For example, as shown in fig. 4, the laser radar generates 4 point cloud data frames based on a frame period, where the 4 point cloud data frames are a point cloud data frame 1, a point cloud data frame 2, a point cloud data frame 3, and a point cloud data frame 4, and the time lengths of the point cloud data frames are equal, and for any one point cloud data frame, the point cloud data frame includes a plurality of line point clouds or a plurality of column point clouds, and each point cloud data in the same line point cloud has the same timestamp. Taking the point cloud data frame 1 as an example, the line point cloud of the point cloud data frame 1 includes 38 point cloud data, the 38 point cloud data are scanned or produced at the same time, the 38 point cloud data have the same timestamp, and the motion compensation device performs pose correction by using the line point cloud as the minimum granularity when performing pose correction on the point cloud data frame, so that the calculation amount of pose correction can be reduced.
And S503, acquiring a plurality of linear velocity data measured by the measuring unit in a third period.
The linear velocity data comprise a linear velocity and a time stamp, the linear velocity represents displacement in unit time, the time stamp represents time for measuring the linear velocity, and the measuring unit measures the linear velocity of the laser radar through the accelerometer. In the application, the duration of the point cloud data frame is greater than the third period, and the duration of the point cloud data frame is an integral multiple of the third period, so that the measuring unit can measure the linear velocity in the point cloud data frame for multiple times. For example: the second period is 0.02s, i.e. the measuring unit measures the linear velocity 50 times within 1s, and the length of the second period is 0.1s, then the measuring unit measures the linear velocity 5 times within the point cloud data frame.
It should be understood that the linear velocity measured by the measurement unit in this application may be linear velocity in any one or more of three axes, for example: when the bearing platform is a vehicle, the laser radar is arranged on the vehicle, the vehicle mainly runs along a road surface, the linear velocity change of the vehicle moving forward instead of the vehicle is also based on the horizontal plane, and the measuring unit can measure the linear velocity on the horizontal plane.
In one or more embodiments, S503 further comprises:
and determining the current linear velocity of the laser radar according to the collected linear velocity data, and determining that the current linear velocity is greater than a linear velocity threshold.
The method for averaging the collected linear velocities can be arithmetic average, geometric average or weighted average, the application is not limited, a linear velocity threshold can be determined according to actual requirements, when the current linear velocity of the laser radar is small, the collected point cloud data frame is only slightly distorted, the influence on a measurement result is small, when the current linear velocity of the laser radar is larger than the linear velocity threshold, the point cloud data frame is subjected to pose correction, and therefore the corrected operation amount can be reduced.
In one or more embodiments, when the current angular velocity is less than or equal to the angular velocity threshold and the current linear velocity is greater than the linear velocity threshold, the pose compensation amount is a linear velocity pose compensation amount, and the linear velocity pose compensation amount is calculated according to the linear velocity of the point cloud data, that is, the present application performs pose compensation on the point cloud data only by using the linear velocity pose compensation amount; or
When the current angular velocity is greater than the angular velocity threshold and the current linear velocity is less than or equal to the linear velocity threshold, the pose compensation amount is an angular velocity pose compensation amount, and the angular velocity pose compensation amount is obtained by calculation according to the angular velocity of the point cloud data, namely the pose compensation is performed on the point cloud data only by using the angular velocity pose compensation amount in the embodiment of the application; or
And when the current angular velocity is less than or equal to the angular velocity threshold and the current linear velocity is less than or equal to the linear velocity threshold, not performing pose compensation.
S504, determining the point cloud data frame to be in an uncompressed state.
The motion compensation device determines whether the point cloud data frame is in an uncompressed state, and specifically, the determining method may include: the method comprises the steps of obtaining the height of each point cloud data in a point cloud data frame, judging whether the height is 1 or not, if the height is 1, indicating that the point cloud data frame is compressed, namely the point cloud data frame is in a compressed state, and the point cloud data in the point cloud data frame is disordered point cloud, so that the pose correction process of the application cannot be executed; if the height is not 1, the point cloud data frame is not compressed, namely the point cloud data frame is in an uncompressed state, the point cloud data of the point cloud data frame is ordered point cloud, and the pose correction process of the application can be executed.
And S505, dividing the point cloud data frame into a plurality of point cloud data intervals according to a first period.
Because the first period is less than the time length of the point cloud data frame, and the time length of the point cloud data frame is an integral multiple of the first period, the point cloud data frame can be divided into a plurality of point cloud data intervals according to the time stamp of the angular velocity data and the integral multiple, each point cloud data interval comprises a plurality of row point clouds, and the point clouds in each row point cloud have the same time stamp.
For example, as shown in fig. 5, the time length of a point cloud data frame is 0.1s, the measurement period (i.e., the first period) of an angular velocity is 0.01s, the boundary of the point cloud data frame and the measurement time of the angular velocity are aligned, the point cloud data frame can be divided into 10 point cloud data intervals according to the measurement period 0.01s of the angular velocity, the point cloud data intervals are respectively a point cloud data interval 1-10, the time length of each point cloud data interval is 0.01s, each point cloud data interval includes a plurality of row point clouds, and the point cloud data in each row point cloud has the same timestamp.
In this embodiment, the point cloud data frame is divided into a plurality of point cloud data intervals by using the acquisition cycle of the angular velocity, and the point cloud data in each point cloud data interval is subjected to motion compensation, so that the problem of inaccurate compensation caused by sudden change of the angular velocity in the point cloud data frame due to the motion compensation of the whole point cloud data frame in the related art is solved, and the precision of the motion compensation can be improved; in addition, the actual angular velocity of each point cloud data frame is calculated through the angular velocity aligned by the start time stamp and the angular velocity aligned by the end time stamp of each point cloud data frame, so that the calculation overhead can be reduced under the condition of ensuring the accuracy, the time overhead for calculating the angular velocity of the point cloud data can be reduced, and the efficiency of motion compensation can be improved.
S506, determining a first angular velocity corresponding to the starting time stamp of the point cloud data interval and a second angular velocity corresponding to the ending time stamp of the point cloud data interval.
The motion compensation device determines a first angular velocity corresponding to the start time stamp of the point cloud data interval and determines a second angular velocity corresponding to the end time stamp of the point cloud data interval, the first angular velocity is the angular velocity with the minimum time interval between the first angular velocity and the start time stamp in the angular velocities measured by the measuring unit, and the second angular velocity is the angular velocity with the minimum time interval between the second angular velocity and the end time stamp in the angular velocities measured by the measuring unit.
For example, referring to fig. 5, a start time stamp of the point cloud data interval 1 is obtained, and the measured angular velocity data is obtained, and a time stamp with the minimum time interval from the start time stamp is traversed in the measured angular velocity data, and the time stamp corresponds to the angular velocity 1. And acquiring an ending time stamp of the point cloud data interval 1, traversing the time stamp with the minimum time interval between the ending time stamp and the measured angular velocity data, wherein the time stamp corresponds to the angular velocity 2.
And S507, averaging the first angular velocity and the second angular velocity to obtain the angular velocity of the point cloud data interval.
Wherein the first angular velocity and the second angular velocity can be addedThe weighted average obtains the angular velocity of the point cloud data interval, for example: averaging the first angular velocity and the second angular velocity to obtain the angular velocity of the point cloud data interval, and assuming that the first angular velocity is
Figure 681250DEST_PATH_IMAGE018
The second angular velocity is
Figure 351266DEST_PATH_IMAGE019
Then the angular velocity of the point cloud data interval is
Figure 75639DEST_PATH_IMAGE020
。
S508, determining a first linear velocity corresponding to the starting time stamp of the point cloud data interval and a second linear velocity corresponding to the ending time stamp of the point cloud data interval.
The motion compensation device traverses the linear velocity with the minimum time interval with the starting timestamp in the measured linear velocity data, and the linear velocity is the first linear velocity; and traversing the linear velocity data with the minimum time interval between the end time stamp and the minimum time interval in the measured linear velocity data, wherein the linear velocity is the second linear velocity.
For example, referring to fig. 5, the motion compensation apparatus determines a start time stamp of the point cloud data interval 1, a linear velocity whose time interval between the traversal and the start time stamp in the measured linear velocity data is the smallest is the linear velocity 1, and a linear velocity whose time interval between the traversal and the end time stamp in the measured linear velocity data is the smallest is the linear velocity 2.
And S509, averaging the first linear speed and the second linear speed to obtain the linear speed of the point cloud data interval.
The motion compensation device may perform weighted average on the first linear velocity and the second linear velocity to obtain a linear velocity of the point cloud data interval, specifically, average the first angular velocity and the second angular velocity to obtain a linear velocity of the point cloud data interval, and assume that the first angular velocity is
Figure 279350DEST_PATH_IMAGE021
The second angular velocity is
Figure 795782DEST_PATH_IMAGE022
Then the angular velocity of the point cloud data interval is
Figure 512065DEST_PATH_IMAGE023
Because the measurement period of the linear velocity data is longer than that of the angular velocity data, and the point cloud data frames are divided according to the measurement period of the angular velocity, a plurality of point cloud data intervals in the point cloud data frames have the same angular velocity.
For example: in fig. 5, the start timestamps of the point cloud data interval 1, the point cloud data interval 2, and the point cloud data interval 3 all correspond to the linear velocity 1, and the end timestamps all correspond to the linear velocity 2, that is, the point cloud data interval 1, the point cloud data interval 2, and the point cloud data interval 3 have the same angular velocity.
And S510, calculating a reference pose compensation amount between the first end point array point cloud and the second end point array point cloud according to the angular velocity of the point cloud data interval and the linear velocity of the point cloud data interval.
The point cloud data interval comprises a plurality of point cloud arrays, the point cloud arrays at the head end and the tail end of the point cloud arrays are two end point array point clouds, the first end point array point cloud is the first end point array point cloud, the second end point array point cloud is the last end point array point cloud, or the first end point array point cloud is the last end point array point cloud, and the second end point array point cloud is the first end point array point cloud. The reference row point cloud is the first row point cloud or the last row point cloud in the point cloud data interval. And the row point clouds in the point cloud data interval except the reference row point cloud are row point clouds to be corrected. The motion compensation device calculates a reference pose compensation amount between the pose of the end point array point cloud and the reference array point cloud according to the angular velocity of the point cloud data interval and the linear velocity of the point cloud data interval so as to correct the pose of the end point array point cloud to the pose of the reference array point cloud, wherein the reference pose compensation amount comprises a displacement compensation amount and a rotation compensation amount, and the reference pose compensation amount comprises a reference angular velocity pose compensation amount and a reference line velocity pose compensation amount.
For example, referring to fig. 6, the point cloud data interval 1 includes a plurality of column point clouds, such as: the plurality of row point clouds include a row point cloud i, a row point cloud j, a row point cloud k and the like, the reference row point cloud is the last row point cloud in the point cloud data interval 1, and the end point row point cloud is the first row point cloud in the point cloud data interval.
In some embodiments of the present application, the motion compensation apparatus may calculate the reference pose compensation amount using the following formula:
Figure 317210DEST_PATH_IMAGE024
；
Figure 901775DEST_PATH_IMAGE002
；
Figure 397347DEST_PATH_IMAGE003
；
Figure 409166DEST_PATH_IMAGE004
；
Figure 108131DEST_PATH_IMAGE005
；
Figure 496387DEST_PATH_IMAGE006
：
wherein the content of the first and second substances,
Figure 347931DEST_PATH_IMAGE007
representing the anti-symmetric operation on the matrix,
Figure 530651DEST_PATH_IMAGE008
representing the corresponding angular speed of the point cloud data interval,
Figure 982492DEST_PATH_IMAGE009
represents the linear velocity corresponding to the point cloud data interval, and t1 represents the point cloud data intervalT2 denotes an end timestamp of the point cloud data interval,
Figure 908859DEST_PATH_IMAGE010
represents a time duration of the point cloud data interval,
Figure 379024DEST_PATH_IMAGE011
representing a flow space algebra corresponding to the relative motion;
Figure 998224DEST_PATH_IMAGE012
representing the displacement between the start timestamp and the end timestamp of the point cloud data interval, inverse () representing the inversion operation of the matrix,
Figure 671782DEST_PATH_IMAGE013
is composed of
Figure 401841DEST_PATH_IMAGE011
The anti-symmetric matrix of (a) is,
Figure 974116DEST_PATH_IMAGE014
the pose of the first end-column matrix is represented,
Figure 764217DEST_PATH_IMAGE015
the pose of the second endpoint column matrix is represented,
Figure 925072DEST_PATH_IMAGE016
and representing the reference pose compensation amount, wherein the reference column point cloud is the second end point column point cloud.
And S511, determining the point cloud of the row to be corrected.
For each row point cloud to be corrected in the point cloud data interval, the motion compensation device selects one row point cloud to be corrected from the point cloud data interval, and the row point cloud to be corrected is other row point clouds except the reference row point cloud in the point cloud data interval. For example: referring to fig. 7, the corrected row point clouds are the ith, jth and kth row point clouds in the point cloud data interval 1.
S512, calculating a proportionality coefficient according to the distance between the row point cloud to be corrected and the reference row point cloud and the number of the plurality of row point clouds in the point cloud data interval.
Wherein, the distance between the column point cloud to be corrected and the reference column point cloud represents the column interval, for example: the distance is obtained by calculating the difference between the serial number of the row point cloud to be corrected and the serial number of the reference row point cloud, and the distance is divided by the number of the plurality of row point clouds in the point cloud data interval to obtain a proportionality coefficient
Figure 193242DEST_PATH_IMAGE025
And i is the serial number of the row point cloud to be corrected in the point cloud data interval.
For example, the point cloud data interval 1 includes 100 column point clouds, the distance between the column point cloud to be corrected and the reference column point cloud is 50, and then the scaling factor is 50/100= 0.5.
And S513, performing pose correction on the column point cloud to be corrected according to the proportionality coefficient and the reference pose compensation amount.
Obtaining actual pose compensation quantity of the to-be-corrected column point cloud according to the proportionality coefficient and the reference pose compensation quantity, and performing pose correction on the to-be-corrected column point cloud according to the actual pose compensation quantity, for example: according to the calculation formula of S310, for the ith row point cloud to be corrected, the parameters of the row point cloud to be corrected
Figure 451048DEST_PATH_IMAGE026
And therefore, the actual pose compensation amount of the ith column point cloud to be corrected can be determined. Optionally, the motion compensation device may perform pose correction on the row point clouds in the point cloud data intervals in a parallel manner, and perform pose correction on the point cloud data frames in a parallel manner.
Referring to fig. 7, the point cloud data frame is divided into n point cloud data intervals: the correction method comprises the steps of respectively carrying out point cloud data intervals delta sm _ 1-delta sm _ n, wherein each point cloud data interval comprises a plurality of row point clouds, the row point clouds in each point cloud data interval execute a correction process in a parallel mode, and the n point cloud data intervals execute the correction process in a parallel mode.
As described with reference to fig. 3 to 7, the measurement period of the angular velocity is shorter than the duration of the point cloud data frame, the point cloud data frame is divided into a plurality of point cloud time intervals according to the measurement period of the angular velocity, determining a corresponding first angular velocity according to a start time stamp of the point cloud data interval and a corresponding second angular velocity according to an end time stamp of the point cloud data interval, the method comprises the steps of calculating the angular speed of a point cloud data interval according to the first angular speed and the second angular speed, determining pose compensation amount according to the angular speed of the point cloud data interval to perform pose compensation, correcting the poses of each row of point clouds in the point cloud data interval for multiple times based on the measurement frequency of angular speed data in a point cloud data frame, and solving the problems of poor correction precision caused by compensation based on the generation period of the point cloud data frame and large calculation amount caused by compensation with point cloud data as granularity in the related technology.
The above describes in detail a method for motion compensation of a point cloud according to an embodiment of the present application, and a motion compensation apparatus (hereinafter referred to as apparatus 3) according to an embodiment of the present application is provided below.
The device 3 shown in fig. 8 can implement the method for motion compensation of point cloud of the embodiment shown in fig. 2A to fig. 3, and the device 3 includes an obtaining unit 301, a determining unit 302, a calculating unit 303 and a compensating unit 304.
An acquisition unit 301 configured to acquire a plurality of pieces of motion parameter data measured by the measurement unit; wherein the motion parameter data comprises a timestamp and a motion parameter;
the acquiring unit 301 is further configured to acquire a point cloud data frame generated by a laser radar; wherein the point cloud data frame comprises a plurality of point cloud data, each point cloud data comprising a timestamp and a spatial location coordinate;
a determining unit 302, configured to determine a first angular velocity and a second angular velocity according to a timestamp of the point cloud data; wherein the first angular velocity is an angular velocity before and closest to a timestamp of the point cloud data, and the second angular velocity is an angular velocity after and closest to the timestamp of the point cloud data;
a calculating unit 303, configured to calculate an angular velocity of the point cloud data according to the first angular velocity and the second angular velocity; calculating the pose compensation amount of the point cloud data according to the angular speed of the point cloud data;
and the compensation unit 304 is used for performing pose compensation on the point cloud data according to the pose compensation amount.
The method of claim 1, wherein, when the motion parameter is angular velocity or linear velocity,
the acquiring of the plurality of motion parameter data measured by the measuring unit includes:
acquiring a plurality of motion parameter data measured by a measuring unit in a first period;
the acquiring of the point cloud data frame generated by the laser radar comprises:
acquiring a point cloud data frame generated by the laser radar in a second period; the second period is greater than the first period, and the second period is an integer multiple of the first period.
In one or more possible embodiments, the motion parameter is angular velocity or linear velocity;
the apparatus 3 further comprises:
the judging unit is used for calculating the current motion parameter according to the motion parameter data and determining that the current motion parameter is larger than a motion parameter threshold; or
And calculating the current motion parameter according to the motion parameter data, and when the current motion parameter is less than or equal to a motion parameter threshold value, not performing pose compensation.
In one or more possible embodiments, the determining the first motion parameter and the second motion parameter according to the time stamp of the point cloud data includes:
dividing the point cloud data frame into a plurality of point cloud data intervals according to the first period;
determining a target point cloud data interval where the point cloud data are located;
and determining a first motion parameter according to the starting time stamp of the target point cloud data interval, and determining a second motion parameter according to the ending time stamp of the target point cloud data interval.
In one or more possible embodiments, the motion parameters include angular velocity and linear velocity;
the acquiring of the plurality of motion parameter data measured by the measuring unit includes:
acquiring a plurality of angular velocity data measured by a measuring unit at a first period;
acquiring a plurality of linear velocity data measured by the measuring unit in a third period;
the method for acquiring the point cloud data frame generated by the laser radar comprises the following steps:
acquiring a point cloud data frame generated by the laser radar in a second period; wherein the second period is greater than the first period and the third period, and the second period is an integer multiple of the first period and the third period.
In one or more possible embodiments, the device 3 further comprises:
the judging unit is used for calculating the current angular velocity according to the plurality of angular velocity data and determining that the current angular velocity is greater than an angular velocity threshold;
and calculating the current linear velocity according to the plurality of linear velocity data, and determining that the current linear velocity is greater than a linear velocity threshold.
In one or more possible embodiments, when the current angular velocity is less than or equal to an angular velocity threshold and the current linear velocity is greater than a linear velocity threshold, the pose compensation amount is a linear velocity pose compensation amount; or
When the current angular velocity is greater than an angular velocity threshold and the current linear velocity is less than or equal to the linear velocity threshold, the pose compensation amount is an angular velocity pose compensation amount; or
And when the current angular velocity is less than or equal to the angular velocity threshold and the current linear velocity is less than or equal to the linear velocity threshold, not performing pose compensation.
In one or more possible embodiments, the point cloud data frame is composed of a plurality of point cloud data intervals, each point cloud data interval is composed of a plurality of row point clouds, and point cloud data in the same row point cloud have the same time stamp;
the pose compensation of the point cloud data according to the pose compensation amount comprises the following steps:
calculating a reference pose compensation amount between the first end point array point cloud and the second end point array point cloud according to the motion parameters of the point cloud data interval;
determining the row point cloud to be corrected;
calculating a proportionality coefficient of the row point cloud to be corrected according to a row distance between the row point cloud to be corrected and a reference row point cloud and according to the number of the row point clouds in the point cloud data interval; the reference array point cloud is the first end point array point cloud or the second end point array point cloud;
and carrying out pose correction on the column point cloud to be corrected according to the proportionality coefficient of the column point cloud to be corrected and the reference pose compensation amount.
In one or more possible embodiments, the pose correction is performed in a parallel manner for each point cloud data interval in the point cloud data frame, and the correction is performed in a parallel manner for each row of point clouds in the point cloud data interval.
In one or more possible embodiments, the motion parameters are angular velocity and linear velocity;
the calculating of the reference pose compensation amount between the first end point array point cloud and the second end point array point cloud according to the angular velocity of the point cloud data interval and the linear velocity of the point cloud data interval comprises:
calculating the reference pose compensation amount using the following formula:
Figure 536684DEST_PATH_IMAGE001
；
Figure 43889DEST_PATH_IMAGE002
；
Figure 991117DEST_PATH_IMAGE003
；
Figure 165746DEST_PATH_IMAGE004
；
Figure 658169DEST_PATH_IMAGE005
；
Figure 918249DEST_PATH_IMAGE006
；
wherein the content of the first and second substances,
Figure 403589DEST_PATH_IMAGE007
representing the anti-symmetric operation on the matrix,
Figure 432724DEST_PATH_IMAGE008
representing the corresponding angular speed of the point cloud data interval,
Figure 594584DEST_PATH_IMAGE009
indicating the linear velocity corresponding to the point cloud data interval, t1 indicating the start time stamp of the point cloud data interval, t2 indicating the end time stamp of the point cloud data interval,
Figure 341960DEST_PATH_IMAGE010
represents a time duration of the point cloud data interval,
Figure 896570DEST_PATH_IMAGE011
representing a flow space algebra corresponding to the relative motion;
Figure 780212DEST_PATH_IMAGE012
representing the displacement between the start timestamp and the end timestamp of the point cloud data interval, inverse () representing the inversion operation of the matrix,
Figure 880017DEST_PATH_IMAGE013
is composed of
Figure 849110DEST_PATH_IMAGE011
The anti-symmetric matrix of (a) is,
Figure 676252DEST_PATH_IMAGE014
the pose of the first end-column matrix is represented,
Figure 414401DEST_PATH_IMAGE015
the pose of the second endpoint column matrix is represented,
Figure 918064DEST_PATH_IMAGE016
and representing the reference pose compensation amount, wherein the reference column point cloud is the second end point column point cloud.
In one or more possible embodiments, the calculating the motion parameter of the point cloud data according to the first motion parameter and the second motion parameter includes:
and averaging the first angular velocity and the second angular velocity to obtain the angular velocity of the point cloud data.
In one or more possible embodiments, the determining unit is further configured to: and determining that the point cloud data frame is in an uncompressed state.
The embodiment of the present application and the method embodiments of fig. 1 to 7 are based on the same concept, and the technical effects brought by the embodiment are also the same, and the specific process can refer to the description of the method embodiments of fig. 1 to 7, which is not described herein again.
The device 3 may be a field-programmable gate array (FPGA), an application-specific integrated chip, a system on chip (SoC), a Central Processing Unit (CPU), a Network Processor (NP), a digital signal processing circuit, a Micro Controller Unit (MCU), or a Programmable Logic Device (PLD) or other integrated chips.
The above describes in detail a method for motion compensation of a point cloud according to an embodiment of the present application, and a device for motion compensation of a point cloud (hereinafter referred to as device 4) according to an embodiment of the present application is provided below.
Fig. 9 is a schematic structural diagram of an apparatus provided in the present embodiment, which is hereinafter referred to as an apparatus 4 for short, where the apparatus 4 may be integrated in the laser radar or the carrying platform of the above embodiment, as shown in fig. 4, the apparatus includes: memory 402, processor 401, transmitter 404, and receiver 403.
The memory 402 may be a separate physical unit, and may be connected to the processor 401, the transmitter 404, and the receiver 403 via a bus. The memory 402, processor 401, transmitter 404, and receiver 401 may also be integrated, implemented in hardware, etc.
The transmitter 404 is used for transmitting signals and the receiver 403 is used for receiving signals.
The memory 402 is used for storing a program implementing the above method embodiment, or various modules of the apparatus embodiment, which is called by the processor 401 to perform the operations of the above method embodiment.
Alternatively, when part or all of the method for motion compensation of point cloud of the above embodiment is implemented by software, the apparatus may also include only a processor. The memory for storing the program is located outside the device and the processor is connected to the memory by means of circuits/wires for reading and executing the program stored in the memory.
The processor may be a Central Processing Unit (CPU), a Network Processor (NP), or a combination of a CPU and an NP.
The processor may further include a hardware chip. The hardware chip may be an application-specific integrated circuit (ASIC), a Programmable Logic Device (PLD), or a combination thereof. The PLD may be a Complex Programmable Logic Device (CPLD), a field-programmable gate array (FPGA), a General Array Logic (GAL), or any combination thereof.
The memory may include volatile memory (volatile memory), such as random-access memory (RAM); the memory may also include a non-volatile memory (non-volatile memory), such as a flash memory (flash memory), a Hard Disk Drive (HDD) or a solid-state drive (SSD); the memory may also comprise a combination of memories of the kind described above.
In the above embodiments, the transmitting unit or the transmitter performs the steps of transmitting in the above respective method embodiments, the receiving unit or the receiver performs the steps of receiving in the above respective method embodiments, and other steps are performed by other units or processors. The transmitting unit and the receiving unit may constitute a transceiving unit, and the receiver and the transmitter may constitute a transceiver.
The embodiment of the application also provides a computer storage medium, which stores a computer program, and the computer program is used for executing the method for motion compensation of point cloud provided by the embodiment.
Embodiments of the present application further provide a computer program product containing instructions, which when run on a computer, cause the computer to execute the method for motion compensation of a point cloud provided in the foregoing embodiments.
As will be appreciated by one skilled in the art, embodiments of the present application may be provided as a method, system, or computer program product. Accordingly, the present application may take the form of an entirely hardware embodiment, an entirely software embodiment or an embodiment combining software and hardware aspects. Furthermore, the present application may take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, and the like) having computer-usable program code embodied therein.
The present application is described with reference to flowchart illustrations and/or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the application. It will be understood that each flow and/or block of the flow diagrams and/or block diagrams, and combinations of flows and/or blocks in the flow diagrams and/or block diagrams, can be implemented by computer program instructions. These computer program instructions may be provided to a processor of a general purpose computer, special purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, create means for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.
These computer program instructions may also be stored in a computer-readable memory that can direct a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer-readable memory produce an article of manufacture including instruction means which implement the function specified in the flowchart flow or flows and/or block diagram block or blocks.
These computer program instructions may also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer implemented process such that the instructions which execute on the computer or other programmable apparatus provide steps for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.

Claims (13)

1. A method for motion compensation of a point cloud, comprising:
acquiring a plurality of motion parameter data measured by a measuring unit; wherein the motion parameter data comprises a timestamp and a motion parameter;
acquiring a point cloud data frame generated by a laser radar; wherein the point cloud data frame comprises a plurality of point cloud data, each point cloud data comprising a timestamp and a spatial location coordinate;
determining a first motion parameter and a second motion parameter according to the time stamp of the point cloud data; the first motion parameter is a motion parameter which is before the time stamp of the point cloud data and is closest to the time stamp of the point cloud data, and the second motion parameter is a motion parameter which is after the time stamp of the point cloud data and is closest to the time stamp of the point cloud data;
calculating a motion parameter of the point cloud data according to the first motion parameter and the second motion parameter;
calculating pose compensation quantity of the point cloud data according to the motion parameters of the point cloud data;
and carrying out pose compensation on the point cloud data according to the pose compensation amount.
2. The method of claim 1, wherein the motion parameter in the motion parameter data comprises an angular velocity or a linear velocity,
the acquiring of the plurality of motion parameter data measured by the measuring unit includes:
acquiring a plurality of motion parameter data measured by a measuring unit in a first period;
the acquiring of the point cloud data frame generated by the laser radar comprises:
acquiring a point cloud data frame generated by the laser radar in a second period; the second period is greater than the first period, and the second period is an integer multiple of the first period.
3. The method according to claim 2, wherein the motion parameter in the motion parameter data is an angular velocity or a linear velocity;
before the point cloud data frame generated by the laser radar is obtained, the method further comprises the following steps:
calculating a current motion parameter according to the motion parameter data, and determining that the current motion parameter is greater than a motion parameter threshold; or
And calculating the current motion parameter according to the motion parameter data, and when the current motion parameter is less than or equal to a motion parameter threshold value, not performing pose compensation.
4. The method of claim 2, wherein determining the first motion parameter and the second motion parameter from the time stamp of the point cloud data comprises:
dividing the point cloud data frame into a plurality of point cloud data intervals according to the first period;
determining a target point cloud data interval where the point cloud data are located;
and determining a first motion parameter according to the starting time stamp of the target point cloud data interval, and determining a second motion parameter according to the ending time stamp of the target point cloud data interval.
5. The method of claim 1, wherein the motion parameters in the motion parameter data include angular velocity and linear velocity;
the acquiring of the plurality of motion parameter data measured by the measuring unit includes:
acquiring a plurality of angular velocity data measured by a measuring unit at a first period;
acquiring a plurality of linear velocity data measured by the measuring unit in a third period;
the method for acquiring the point cloud data frame generated by the laser radar comprises the following steps:
acquiring a point cloud data frame generated by the laser radar in a second period; wherein the second period is greater than the first period and the third period, and the second period is an integer multiple of the first period and the third period.
6. The method of claim 5, wherein prior to obtaining the frame of point cloud data generated by the lidar, further comprising:
calculating a current angular velocity according to the plurality of angular velocity data, and determining that the current angular velocity is greater than an angular velocity threshold;
and calculating the current linear speed according to the plurality of linear speed data, and determining that the current linear speed is greater than a linear speed threshold.
7. The method of claim 6,
when the current angular velocity is less than or equal to an angular velocity threshold and the current linear velocity is greater than a linear velocity threshold, the pose compensation amount is a linear velocity pose compensation amount; or
When the current angular velocity is greater than an angular velocity threshold and the current linear velocity is less than or equal to a linear velocity threshold, the pose compensation amount is an angular velocity pose compensation amount; or
And when the current angular velocity is less than or equal to the angular velocity threshold and the current linear velocity is less than or equal to the linear velocity threshold, not performing pose compensation.
8. The method of claim 1, wherein the frame of point cloud data is comprised of a plurality of point cloud data intervals, each point cloud data interval being comprised of a plurality of columns of point clouds, the point cloud data in a same column of point clouds having a same timestamp;
the pose compensation of the point cloud data according to the pose compensation amount comprises the following steps:
calculating a reference pose compensation amount between the first end point array point cloud and the second end point array point cloud according to the motion parameters of the point cloud data interval;
determining the row point cloud to be corrected;
calculating a proportionality coefficient of the row point cloud to be corrected according to a row distance between the row point cloud to be corrected and a reference row point cloud and according to the number of the row point clouds in the point cloud data interval; the reference array point cloud is the first end point array point cloud or the second end point array point cloud;
and carrying out pose correction on the column point cloud to be corrected according to the proportionality coefficient of the column point cloud to be corrected and the reference pose compensation amount.
9. An apparatus for motion compensation of a point cloud, comprising:
an acquisition unit configured to acquire a plurality of pieces of motion parameter data measured by the measurement unit; wherein the motion parameter data comprises a timestamp and a motion parameter;
the acquisition unit is also used for acquiring a point cloud data frame generated by the laser radar; wherein the point cloud data frame comprises a plurality of point cloud data, each point cloud data comprising a timestamp and a spatial location coordinate;
a determining unit, configured to determine a first angular velocity and a second angular velocity according to a timestamp of the point cloud data; wherein the first angular velocity is an angular velocity before and closest to a timestamp of the point cloud data, and the second angular velocity is an angular velocity after and closest to the timestamp of the point cloud data;
a calculation unit configured to calculate an angular velocity of the point cloud data from the first angular velocity and the second angular velocity; calculating the pose compensation amount of the point cloud data according to the angular speed of the point cloud data;
and the compensation unit is used for carrying out pose compensation on the point cloud data according to the pose compensation amount.
10. A computer storage medium, characterized in that it stores a computer program which, when run on a computer, causes the computer to perform the method according to any one of claims 1 to 8.
11. An apparatus for motion compensation of a point cloud, comprising a processor and a memory, the memory for storing a computer program or instructions, the processor for executing the computer program or instructions in the memory to implement the method of any one of claims 1 to 8.
12. A motion compensation system, comprising: the motion compensation apparatus, lidar, measurement unit, and load-bearing platform of claim 9 or 11; the bearing platform is used for bearing the laser radar, the measuring unit and the motion compensation device, and the bearing platform is an unmanned aerial vehicle, a vehicle or a mechanical arm.
13. The system of claim 12, wherein the measurement unit is disposed inside the lidar or the measurement unit is independently disposed outside the lidar.
CN202010839029.4A 2020-08-19 2020-08-19 Method, device and system for motion compensation of point cloud Active CN111708048B (en)

Priority Applications (2)

Application Number Priority Date Filing Date Title
CN202011629746.0A CN112731450B (en) 2020-08-19 2020-08-19 Point cloud motion compensation method, device and system
CN202010839029.4A CN111708048B (en) 2020-08-19 2020-08-19 Method, device and system for motion compensation of point cloud

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010839029.4A CN111708048B (en) 2020-08-19 2020-08-19 Method, device and system for motion compensation of point cloud

Related Child Applications (1)

Application Number Title Priority Date Filing Date
CN202011629746.0A Division CN112731450B (en) 2020-08-19 2020-08-19 Point cloud motion compensation method, device and system

Publications (2)

Publication Number Publication Date
CN111708048A CN111708048A (en) 2020-09-25
CN111708048B true CN111708048B (en) 2021-02-05

Family

ID=72547230

Family Applications (2)

Application Number Title Priority Date Filing Date
CN202010839029.4A Active CN111708048B (en) 2020-08-19 2020-08-19 Method, device and system for motion compensation of point cloud
CN202011629746.0A Active CN112731450B (en) 2020-08-19 2020-08-19 Point cloud motion compensation method, device and system

Family Applications After (1)

Application Number Title Priority Date Filing Date
CN202011629746.0A Active CN112731450B (en) 2020-08-19 2020-08-19 Point cloud motion compensation method, device and system

Country Status (1)

Country Link
CN (2) CN111708048B (en)

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112382088B (en) * 2020-11-10 2022-01-14 苏州艾氪英诺机器人科技有限公司 Vehicle data compensation method and system
CN112819861A (en) * 2021-02-26 2021-05-18 广州小马慧行科技有限公司 Method and device for motion compensation of point cloud and computer readable storage medium
CN113391300B (en) * 2021-05-21 2022-02-01 中国矿业大学 Laser radar three-dimensional point cloud real-time motion compensation method based on IMU
CN114111568B (en) * 2021-09-30 2023-05-23 深圳市速腾聚创科技有限公司 Method and device for determining appearance size of dynamic target, medium and electronic equipment
EP4166989A1 (en) * 2021-10-13 2023-04-19 Aptiv Technologies Limited Methods and systems for determining a position and an acceleration of a vehicle

Family Cites Families (18)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2014145611A (en) * 2013-01-28 2014-08-14 Furuno Electric Co Ltd Moving body angular velocity calculating device, moving body velocity calculating device, moving body, moving body angular velocity calculating method, moving body velocity calculating method, moving body angular velocity calculating program, and moving body velocity calculating program
US11567201B2 (en) * 2016-03-11 2023-01-31 Kaarta, Inc. Laser scanner with real-time, online ego-motion estimation
CN107817502B (en) * 2016-09-14 2020-08-07 北京百度网讯科技有限公司 Laser point cloud data processing method and device
CN107817503B (en) * 2016-09-14 2018-12-21 北京百度网讯科技有限公司 Motion compensation process and device applied to laser point cloud data
CN108732584B (en) * 2017-04-17 2020-06-30 百度在线网络技术（北京）有限公司 Method and device for updating map
CN108307174A (en) * 2018-01-26 2018-07-20 上海深视信息科技有限公司 A kind of depth image sensor precision improvement method and system
CN108957466B (en) * 2018-04-18 2022-01-25 广东宝乐机器人股份有限公司 Radar data compensation method, device, equipment and storage medium for mobile robot
CN109031339B (en) * 2018-05-23 2020-11-10 北京控制工程研究所 Three-dimensional point cloud motion compensation method
CN111220992B (en) * 2018-11-26 2022-05-20 长沙智能驾驶研究院有限公司 Radar data fusion method, device and system
CN109887057B (en) * 2019-01-30 2023-03-24 杭州飞步科技有限公司 Method and device for generating high-precision map
CN109975792B (en) * 2019-04-24 2021-02-05 福州大学 Method for correcting point cloud motion distortion of multi-line laser radar based on multi-sensor fusion
CN110376604B (en) * 2019-08-09 2022-11-15 北京智行者科技股份有限公司 Road edge detection method based on single line laser radar
CN110705543A (en) * 2019-08-23 2020-01-17 芜湖酷哇机器人产业技术研究院有限公司 Method and system for recognizing lane lines based on laser point cloud
KR102083911B1 (en) * 2019-09-03 2020-03-04 주식회사 모빌테크 Method for building map including point cloud using LiDAR
CN110703229A (en) * 2019-09-25 2020-01-17 禾多科技（北京）有限公司 Point cloud distortion removal method and external reference calibration method for vehicle-mounted laser radar reaching IMU
CN110889808B (en) * 2019-11-21 2023-02-28 广州文远知行科技有限公司 Positioning method, device, equipment and storage medium
CN110888125B (en) * 2019-12-05 2020-06-19 奥特酷智能科技(南京)有限公司 Automatic driving vehicle positioning method based on millimeter wave radar
CN111123280B (en) * 2019-12-31 2023-02-03 武汉万集光电技术有限公司 Laser radar positioning method, device and system, electronic equipment and storage medium

Also Published As

Publication number Publication date
CN112731450B (en) 2023-06-30
CN112731450A (en) 2021-04-30
CN111708048A (en) 2020-09-25

Similar Documents

Publication Publication Date Title
CN111708048B (en) Method, device and system for motion compensation of point cloud
CN109975792B (en) Method for correcting point cloud motion distortion of multi-line laser radar based on multi-sensor fusion
CN111007530B (en) Laser point cloud data processing method, device and system
KR102284565B1 (en) Correction of motion-based inaccuracies in point clouds
US20160063717A1 (en) Point cloud position data processing device, point cloud position data processing system, point cloud position data processing method, and program therefor
CN109752003B (en) Robot vision inertia point-line characteristic positioning method and device
WO2022127532A1 (en) Method and apparatus for calibrating external parameter of laser radar and imu, and device
KR101711964B1 (en) Free space map construction method, free space map construction system, foreground/background extraction method using the free space map, and foreground/background extraction system using the free space map
US9002513B2 (en) Estimating apparatus, estimating method, and computer product
CN103412565A (en) robot with global location rapid estimating capability and positioning method thereof
CN107527382B (en) Data processing method and device
CN104715469A (en) Data processing method and electronic device
CN112455502B (en) Train positioning method and device based on laser radar
CN103076023A (en) Method and device for calculating step
CN113767264A (en) Parameter calibration method, device, system and storage medium
CN106403999A (en) GNSS-based real-time compensation method for inertial navigation accelerometer drifting
TWI477800B (en) Positioning device capable of detecting three dimensional trace and the three dimensional trace detecting method thereof
CN110471074B (en) Method for improving planar laser point cloud distance measurement precision
CN111983632A (en) Self-navigation positioning method and system based on double-line laser radar
CN116182905A (en) Laser radar and combined inertial navigation space-time external parameter calibration method, device and system
CN113280813B (en) Inertial measurement data compensation method and device based on neural network model
CN115727871A (en) Track quality detection method and device, electronic equipment and storage medium
JP2022149051A (en) Map creation device, map creation system, map creation method, and program
CN110646788A (en) Method and system for automatically correcting angle of speedometer radar
CN117346768B (en) Multi-sensor fusion sensing positioning method suitable for indoor and outdoor

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant