Laser point cloud distortion removing method and device, equipment and storage medium

Abstract

The embodiment of the invention provides a laser point cloud distortion removing method and device, electronic equipment and a storage medium, and the method comprises the steps: in the movement process of a carrier carrying an IMU, a millimeter-wave radar and a laser radar, obtaining a distortion removing result through the acceleration and attitude information at the current moment obtained by the IMU according to a pre-constructed system state error model, processing to obtain prediction state information and prediction error information of the carrier at the next moment; updating the prediction error information according to the system state error model, the prediction state information, the prediction error information and the linear speed output by the millimeter wave radar at the current moment to obtain target error information; updating the prediction state information according to the target error information to obtain target state information; obtaining a fusion angular velocity and a fusion linear velocity according to the target error information and the target state information; and according to the fusion angular velocity and the fusion linear velocity, correcting the point cloud data collected by the laser radar at the current moment to obtain corrected target point cloud data.

Description

Technical Field

[0001] The present invention relates to the technical field of laser point cloud processing, and in particular to a laser point cloud dedistortion method, device, equipment and storage medium. Background Art

[0002] LiDAR technology is a technology that emits laser and receives the emitted laser to obtain three-dimensional information of the target object. It is widely used in fields such as autonomous driving, robot navigation and geographic information systems.

[0003] However, during the data collection process of the LiDAR, the movement of the carrier on which it is located may cause the collected point cloud data to be distorted, which in turn affects subsequent processing and application. For example, the map constructed based on these distorted point cloud data may be inaccurate, which in turn leads to low positioning accuracy or even errors based on the map; for another example, the target detection results based on these distorted point cloud data may be inaccurate. Summary of the invention

[0004] In view of this, in order to at least solve the technical problem in the related art that the laser point cloud is distorted due to the movement of the carrier, an object of the present invention is to provide a laser point cloud dedistortion method, device, equipment and storage medium.

[0005] In order to achieve the above purpose, the technical solution adopted by the embodiment of the present invention is as follows:

[0006] A first aspect of an embodiment of the present invention provides a laser point cloud dedistortion method, comprising:

[0007] During the movement of a carrier equipped with an IMU, a millimeter-wave radar, and a laser radar, the predicted state information and predicted error information of the carrier at the next moment are processed according to a pre-constructed system state error model and the acceleration and attitude information at the current moment obtained by the IMU; wherein the predicted state information includes: predicted linear velocity, predicted attitude information, predicted acceleration deviation, predicted angular velocity deviation, and the predicted error information includes: predicted linear velocity error state quantity, predicted attitude error state quantity, predicted acceleration deviation error state quantity, and predicted angular velocity deviation error state quantity;

[0008] According to the system state error model, the predicted state information, the predicted error information and the linear speed output by the millimeter wave radar at the current moment, the predicted error information is updated to obtain target error information;

[0009] The predicted state information is updated according to the target error information to obtain target state information;

[0010] Obtaining a fusion angular velocity and a fusion linear velocity according to the target error information and the target state information;

[0011] According to the fusion angular velocity and the fusion linear velocity, the point cloud data collected by the laser radar at the current moment is corrected to obtain corrected target point cloud data.

[0012] In an optional implementation, the process of constructing the system state error model includes:

[0013] The attitude information, linear velocity, acceleration deviation and angular velocity deviation are configured as motion state quantities of the error state Kalman filter;

[0014] Configuring the attitude error, the linear velocity error, the acceleration deviation error and the angular velocity deviation error as error state quantities of the error state Kalman filter;

[0015] The error state Kalman filter configured with the motion state quantity and the error state quantity is initialized to obtain the system state error model.

[0016] In an optional implementation manner, the step of initializing the error state Kalman filter configured with the motion state quantity and the error state quantity to obtain the system state error model includes:

[0017] Calculate the acceleration mean, angular velocity mean, acceleration variance and angular velocity variance according to a first set number of frame data collected by the IMU in a stationary state;

[0018] Calculating a linear velocity variance according to a second set number of frames of data collected by the millimeter-wave radar in a stationary state;

[0019] Initializing the posture information and linear velocity in the error state Kalman filter to 0, and initializing the acceleration deviation and angular velocity deviation to the acceleration mean and the angular velocity mean, respectively;

[0020] Initializing all error state quantities in the error state Kalman filter to 0;

[0021] According to the acceleration variance, angular velocity variance and linear velocity variance, an IMU noise matrix and a millimeter wave radar noise matrix of the error state Kalman filter are constructed to obtain the system state error model.

[0022] In an optional implementation, the step of updating the prediction error information according to the system state error model, the prediction state information, the prediction error information, and the linear velocity output by the millimeter wave radar at the current moment to obtain the target error information includes:

[0023] Calculate the predicted covariance at the next moment according to the predicted error information, the IMU noise matrix in the system state error model and the covariance obtained from the last prediction;

[0024] The Kalman gain is calculated based on the predicted covariance, the linear velocity observation matrix and the millimeter wave radar noise matrix in the system state error model; wherein the linear velocity observation matrix is ​​calculated based on the predicted state information and the linear velocity output by the millimeter wave radar at the current moment;

[0025] The linear velocity of the carrier at the current moment is calculated according to the preset linear velocity deviation and the linear velocity output by the millimeter wave radar at the current moment;

[0026] Target error information is obtained according to the Kalman gain, the linear velocity of the carrier at the current moment, the linear velocity observation matrix and the predicted state information.

[0027] In an optional implementation, the step of updating the predicted state information according to the target error information to obtain the target state information includes:

[0028] Taking the difference between the predicted state information and the target error information as the target state information;

[0029] And / or, while or after obtaining the target state information, the method further includes:

[0030] The prediction covariance matrix is ​​updated according to the linear velocity observation matrix, the Kalman gain and the unit matrix to obtain an updated covariance; the updated covariance is used to calculate the prediction covariance at the next moment after the next moment;

[0031] And / or, the step of obtaining the fused angular velocity and the fused linear velocity according to the target error information and the target state information comprises:

[0032] The sum of the angular velocity output by the IMU at the current moment and the angular velocity deviation in the target error information is used as the fused angular velocity;

[0033] The linear velocity in the target state information is used as the fused linear velocity.

[0034] In an optional implementation, the step of correcting the point cloud data currently collected by the laser radar according to the fused angular velocity and the fused linear velocity to obtain corrected target point cloud data includes:

[0035] For each non-starting point in the current frame point cloud collected by the laser radar at the current moment, the corrected coordinates of the non-starting point are calculated according to the initial coordinates of the non-starting point, the time difference between the non-starting point and the starting point, the fused angular velocity and the fused linear velocity.

[0036] In an optional implementation, the step of correcting the point cloud data currently collected by the laser radar according to the fused angular velocity and the fused linear velocity to obtain corrected target point cloud data includes:

[0037] For each non-starting point in the current frame point cloud collected by the laser radar at the current moment, the corrected displacement of the non-starting point is obtained according to the corrected displacement of the previous point adjacent to the non-starting point, the corrected linear velocity of the previous point, the acceleration corresponding to the previous point, and the time difference between the non-starting point and the previous point; wherein, when the previous point is the starting point, the corrected linear velocity of the previous point is the fused linear velocity, and when the previous point is not the starting point, the corrected linear velocity of the previous point is calculated based on the corrected linear velocity of the previous point adjacent to the previous point, the acceleration corresponding to the previous point, the acceleration deviation in the target state information, and the time difference between the previous point and the previous point;

[0038] For each non-starting point in the current frame point cloud, the corrected coordinates of the non-starting point are calculated according to the initial coordinates of the non-starting point, the time difference between the non-starting point and the starting point, the fused angular velocity and the corrected displacement of the non-starting point.

[0039] A second aspect of an embodiment of the present invention provides a laser point cloud dedistortion device, comprising:

[0040] The prediction module is configured to: during the movement of a carrier equipped with an IMU, a millimeter-wave radar, and a laser radar, process the predicted state information and predicted error information of the carrier at the next moment according to a pre-constructed system state error model and the acceleration and attitude information at the current moment obtained by the IMU; wherein the predicted state information includes: predicted linear velocity, predicted attitude information, predicted acceleration deviation, and predicted angular velocity deviation; and the predicted error information includes: predicted linear velocity error state quantity, predicted attitude error state quantity, predicted acceleration deviation error state quantity, and predicted angular velocity deviation error state quantity;

[0041] The error updating module is configured to: update the predicted error information according to the system state error model, the predicted state information, the predicted error information and the linear speed output by the millimeter wave radar at the current moment to obtain target error information;

[0042] A state updating module is configured to: update the predicted state information according to the target error information to obtain target state information;

[0043] A fusion module is configured to: obtain a fusion angular velocity and a fusion linear velocity according to the target error information and the target state information;

[0044] The correction module is configured to correct the point cloud data collected by the laser radar at the current moment according to the fusion angular velocity and the fusion linear velocity to obtain corrected target point cloud data.

[0045] According to a third aspect of an embodiment of the present invention, an electronic device is provided, comprising a processor and a memory, wherein the memory stores machine executable instructions that can be executed by the processor, and the processor can execute the machine executable instructions to implement the laser point cloud dedistortion method provided in the first aspect above.

[0046] According to a fourth aspect of an embodiment of the present invention, a computer-readable storage medium is provided, on which a computer program is stored. When the computer program is executed by a processor, the laser point cloud dedistortion method provided in the first aspect is implemented.

[0047] The laser point cloud dedistortion method, device, equipment and storage medium provided by the embodiment of the present invention predict the predicted state information and predicted error information of the carrier at the next moment based on the acceleration and attitude information obtained by IMU (Inertial Measurement Unit) during the movement of the carrier, that is, predict the motion state quantity (linear velocity, attitude information, acceleration deviation, angular velocity deviation) of the carrier at the next moment and the error state quantity of each of these motion state quantities (including linear velocity error state quantity, attitude error state quantity, acceleration deviation error state quantity, angular velocity deviation error state quantity), and then update the above-mentioned predicted error information in combination with the linear velocity output by the millimeter wave radar, that is, update each of the above-mentioned error state quantities, so as to obtain more accurate target error information; then, the predicted state information is updated using the target error information, so as to obtain more accurate target state information, so that the fused angular velocity and fused linear velocity obtained using the target error information and the target state information can accurately express the actual motion state of the carrier. Therefore, by using the fused angular velocity and the fused linear velocity to correct the point cloud data collected by the lidar at the current moment, the distortion of the point cloud data caused by the movement of the carrier can be effectively removed, and the target point cloud data after distortion is removed can be obtained. The problem of point cloud distortion robustness of the carrier in scenes such as uniform motion, variable speed motion, tunnels, open squares, and highways can be effectively solved, and the safety and accuracy of autonomous driving control based on the target point cloud data can be achieved. It can also improve the data processing accuracy and application effect of robot navigation and geographic information processing based on the target point cloud data.

[0048] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, preferred embodiments are given below and described in detail with reference to the accompanying drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0049] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings required for use in the embodiments are briefly introduced below. It should be understood that the following drawings only show certain embodiments of the present invention and therefore should not be regarded as limiting the scope. For ordinary technicians in this field, other related drawings can be obtained based on these drawings without creative work.

[0050] Figure 1 A structural block diagram of an electronic device provided by an embodiment of the present invention is shown;

[0051] Figure 2 A flowchart of a laser point cloud dedistortion method provided by an embodiment of the present invention is shown;

[0052] Figure 3 A functional module diagram of a laser point cloud dedistortion device provided in an embodiment of the present invention is shown. DETAILED DESCRIPTION

[0053] The following will be combined with the accompanying drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Generally, the components of the embodiments of the present invention described and shown in the drawings here can be arranged and designed in various different configurations.

[0054] Therefore, the following detailed description of the embodiments of the present invention provided in the accompanying drawings is not intended to limit the scope of the invention claimed for protection, but merely represents selected embodiments of the present invention. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without making creative work are within the scope of protection of the present invention.

[0055] It should be noted that relational terms such as "first" and "second" are used only to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "include", "comprise" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device. In the absence of further restrictions, the elements defined by the sentence "comprise a ..." do not exclude the existence of other identical elements in the process, method, article or device including the elements.

[0056] During the collection of a frame of laser radar data, it is easy for the collected point cloud data to be distorted due to the rapid movement of the carrier, which in turn affects subsequent processing and applications, such as mapping and detection functions. Although solutions to remove laser point cloud distortion have been proposed, such as point cloud registration and distortion removal solutions, they have the following problems:

[0057] 1) In the scenario of variable speed movement such as acceleration or deceleration of the carrier, the laser point cloud will produce greater distortion during the movement. The current solution has poor effect on the laser point cloud dedistortion in this scenario and cannot effectively remove the point cloud distortion problem.

[0058] 2) When the carrier is in motion scenes with degraded point clouds such as tunnels, open squares, and highways, the geometric features of the point cloud data in these scenes are almost the same or have no geometric features at all. This makes it impossible to align the point clouds of the previous and next frames to calculate the linear motion distance information, and thus the speed information cannot be calculated. Therefore, the current solution is not ideal for removing point cloud distortion in this scene, and may even lead to errors in the point cloud distortion processing results.

[0059] Therefore, in order to at least solve the technical problem of laser point cloud distortion due to carrier motion in the related art, the present invention provides a laser point cloud dedistortion method, which predicts the predicted state information and predicted error information of the carrier at the next moment based on the acceleration and attitude information obtained by the IMU during the carrier motion, that is, predicts the motion state quantity (linear velocity, attitude information, acceleration deviation, angular velocity deviation) of the carrier at the next moment and the error state quantities of these motion state quantities (including linear velocity error state quantity, attitude error state quantity, acceleration deviation error state quantity, angular velocity deviation error state quantity), and then updates the above-mentioned predicted error information in combination with the linear velocity output by the millimeter-wave radar, that is, updates each of the above-mentioned error state quantities, so as to obtain more accurate target error information; then, the predicted state information is updated using the target error information, so as to obtain more accurate target state information, so that the fused angular velocity and fused linear velocity obtained using the target error information and the target state information can accurately express the actual motion state of the carrier. Therefore, by using the fused angular velocity and the fused linear velocity to correct the point cloud data collected by the lidar at the current moment, the distortion of the point cloud data caused by the movement of the carrier can be effectively removed, and the target point cloud data after distortion is removed can be obtained. The problem of point cloud distortion robustness of the carrier in scenes such as uniform motion, variable speed motion, tunnels, open squares, and highways can be effectively solved, and the safety and accuracy of autonomous driving control based on the target point cloud data can be achieved. It can also improve the data processing accuracy and application effect of robot navigation and geographic information processing based on the target point cloud data.

[0060] The laser point cloud dedistortion method provided by the present invention can be applied to electronic devices. Figure 1 , is a block diagram of the structure of an electronic device. The electronic device 100 includes a memory 110, a processor 120 and a communication module 130. The memory 110, the processor 120 and the communication module 130 are electrically connected to each other directly or indirectly to achieve data transmission or interaction. For example, these components can be electrically connected to each other through one or more communication buses or signal lines.

[0061] The memory is used to store programs or data. The memory may be, but is not limited to, a random access memory (RAM), a read only memory (ROM), a programmable read-only memory (PROM), an erasable programmable read-only memory (EPROM), an electrically erasable programmable read-only memory (EEPROM), etc.

[0062] The processor is used to read / write data or programs stored in the memory and execute corresponding functions.

[0063] The communication module is used to establish a communication connection between the electronic device and other communication terminals through the network, and is used to send and receive data through the network.

[0064] It should be understood that Figure 1 The structure shown is only a schematic diagram of the structure of the electronic device, and the electronic device may also include Figure 1 More or fewer components as shown, or with Figure 1 Different configurations are shown. Figure 1 Each component shown in the figure can be implemented by hardware, software or a combination thereof.

[0065] In some embodiments, the electronic device can be loaded into a carrier equipped with an IMU, a millimeter-wave radar, and a laser radar, and can be used as a control system or a data processing system of the carrier to execute the laser point cloud dedistortion method provided in the embodiment of the present invention, and perform dedistortion processing on each frame of point cloud data output by the laser radar in real time to improve the accuracy of the point cloud data, so as to facilitate better subsequent applications, such as navigation and mapping, but not limited thereto. The carrier can be a robot, a car, or an aircraft, but not limited thereto.

[0066] The following, combined Figure 2 The laser point cloud dedistortion method provided by the embodiment of the present invention is described. Figure 2 : is a flow chart of a laser point cloud dedistortion method provided by an embodiment of the present invention, the laser point cloud dedistortion method comprising:

[0067] In step S100, during the movement of a carrier equipped with an IMU, a millimeter-wave radar, and a laser radar, the predicted state information and predicted error information of the carrier at the next moment are processed according to a pre-constructed system state error model and the acceleration and attitude information at the current moment obtained by the IMU; wherein the predicted state information includes: predicted linear velocity, predicted attitude information, predicted acceleration deviation, predicted angular velocity deviation, and the predicted error information includes: predicted linear velocity error state quantity, predicted attitude error state quantity, predicted acceleration deviation error state quantity, and predicted angular velocity deviation error state quantity;

[0068] In step S200, the prediction error information is updated according to the system state error model, the prediction state information, the prediction error information and the linear speed output by the millimeter wave radar at the current moment to obtain target error information;

[0069] In step S300, the predicted state information is updated according to the target error information to obtain target state information;

[0070] In step S400, a fusion angular velocity and a fusion linear velocity are obtained according to the target error information and the target state information;

[0071] In step S500, the point cloud data collected by the laser radar at the current moment is corrected according to the fusion angular velocity and the fusion linear velocity to obtain corrected target point cloud data.

[0072] Before introducing the laser point cloud dedistortion method provided by the embodiment of the present invention, the source of the system state error model mentioned in step S100 is explained first. The system state error model can be a model that takes linear velocity, posture information, acceleration deviation and angular velocity deviation as system state quantities, takes linear velocity error state quantity, posture error state quantity, acceleration deviation error state quantity, angular velocity deviation error state quantity as error state quantities, and predicts and updates the system state quantity and error state quantity at the next moment based on the system state quantity and error state quantity at the current moment. Based on this concept, corresponding implementation means can be obtained from related technologies and adaptively adjusted according to the construction concept of the system error model.

[0073] In order to provide a system state error model that is more suitable for the laser point cloud dedistortion method provided by the embodiment of the present invention, and to provide the prediction accuracy of the system state quantity and error state quantity at the next moment, thereby enhancing the effect and point cloud accuracy of subsequent point cloud dedistortion, in some embodiments, the laser point cloud dedistortion method provided by the embodiment of the present invention further provides a system state error model construction scheme, that is, the system state error model construction process includes:

[0074] In step S101, the posture information, linear velocity, acceleration deviation and angular velocity deviation are configured as motion state quantities of an error state Kalman filter;

[0075] In step S102, the attitude error, the linear velocity error, the acceleration deviation error and the angular velocity deviation error are configured as error state quantities of the error state Kalman filter;

[0076] In step S103, the error state Kalman filter configured with the motion state quantity and the error state quantity is initialized to obtain the system state error model.

[0077] It can be understood that, for step S101, the error-state Kalman Filter (ESKF) can be used to model the system state quantity by the following formula:

[0078]

[0079] In the above formula, X t represents the state information at time t, v t represents the linear velocity of the carrier at time t, R t represents the posture information of the carrier at time t, represents the acceleration deviation at time t, Represents the angular velocity deviation at time t.

[0080] Similarly, for step S102, the error state quantity of the ESKF can be modeled by the following formula:

[0081] d X =[d v ,d R ,d ba ,d bw ]

[0082] In the above formula, d X Represents the error state quantity, d v Indicates the linear velocity error state, d R Represents the attitude error state, d ba Indicates the acceleration deviation error state quantity, d bw Indicates the angular velocity deviation error state quantity.

[0083] After configuring the ESKF through steps S101 and S102, step S103 may be performed to initialize the ESKF to obtain a system state error model applied to the above-mentioned carrier. The initialization process refers to initializing the above-mentioned X t and d XBy assigning an initial value, the system state error model can iteratively process the acceleration and attitude information obtained in real time by the IMU and the linear velocity information output by the millimeter wave radar in real time during the carrier movement based on the initial system state quantity and error state quantity to obtain the state information and error information corresponding to each current moment. The acceleration can be the acceleration data output by the IMU, and the attitude information can be calculated based on the angular velocity output by the IMU.

[0084] Among them, the initialization of the system state quantity can be initialized according to the performance parameters of the IMU and the millimeter-wave radar, and the technical principles involved in the initialization can be referred to in the relevant technology. However, in order to make the initialization of the system state quantity more consistent with the characteristics of the IMU and millimeter-wave radar actually applied to the above-mentioned carrier, in some embodiments, the laser point cloud dedistortion method provided in the embodiment of the present invention also provides an implementation scheme for initializing the system state quantity and the error state quantity, that is, in the above step S103, the error state Kalman filter configured with the motion state quantity and the error state quantity is initialized to obtain the system state error model, which may include:

[0085] In step S1031, according to a first set number of frame data collected by the IMU in a stationary state, the acceleration mean, angular velocity mean, acceleration variance and angular velocity variance are calculated;

[0086] In step S1032, a linear velocity variance is calculated based on a second set number of frames of data collected by the millimeter wave radar in a stationary state;

[0087] In step S1033, the posture information and the linear velocity in the error state Kalman filter are initialized to 0, and the acceleration deviation and the angular velocity deviation are initialized to the acceleration mean and the angular velocity mean, respectively;

[0088] In step S1034, all error state quantities in the error state Kalman filter are initialized to 0;

[0089] In step S1035, the IMU noise matrix and the millimeter wave radar noise matrix of the error state Kalman filter are constructed according to the acceleration variance, the angular velocity variance and the linear velocity variance to obtain the system state error model.

[0090] It is understandable that in a stationary state, the acceleration and angular velocity of the IMU should be 0, and because the IMU has a random walk zero bias, the measured values ​​of the acceleration and angular velocity may not be 0. Therefore, the IMU can be controlled to collect a first set number of frame data. In the process of executing step S1031, the acceleration mean, angular velocity mean, acceleration variance and angular velocity variance of the IMU in a stationary state can be obtained based on these frame data. Then, step S1033 can be executed, so that the acceleration mean can be used as the initial value of the acceleration deviation of the ESKF, the angular velocity mean can be used as the initial value of the angular velocity deviation of the ESKF, and the attitude information and linear velocity of the ESKF are initialized to 0.

[0091] Regardless of the order of the above step S1031, step S1034 may be executed to initialize all error state quantities in the ESKF to zero.

[0092] In addition, since there will be certain noise errors in the IMU and the millimeter-wave radar during operation, in order to improve the ESKF's estimation of state information and error information at future moments, the embodiment of the present invention further configures a corresponding noise matrix for the ESKF. Therefore, in addition to controlling the IMU to collect data of a first set number of frames in a stationary state, it is also necessary to control the millimeter-wave radar to collect data of a second set number of frames in a stationary state. After obtaining the second set number of frames of data collected by the millimeter-wave radar, step S1032 can be executed to calculate the linear velocity variance and the linear velocity mean of the millimeter-wave radar in a stationary state. The purpose of the linear velocity mean will be explained later, and the linear velocity variance is used first here.

[0093] After obtaining the acceleration variance, angular velocity variance, and linear velocity variance, step S1035 can be executed to construct the IMU noise matrix and millimeter wave radar noise matrix of the ESKF. The IMU noise matrix can be expressed as:

[0094]

[0095] In the above formula, Q represents the IMU noise matrix, q a represents the acceleration noise, whose value is the acceleration variance calculated above, q w represents the angular velocity noise, whose value is the angular velocity variance calculated above, q bw represents the angular velocity zero bias noise, q ba represents the acceleration bias noise, q bw and q ba The value can be set based on experience, and the values ​​at “…” can all be set to 0.

[0096] In addition, the millimeter wave radar noise matrix can be expressed as:

[0097] R z =(q v )

[0098] In the above formula, R z represents the millimeter wave radar noise matrix, q v represents the line velocity noise, and its value is the line velocity variance calculated above.

[0099] In the above, the first set number and the second set number can be set according to experience or actual needs, and the two can be the same or different. As an example, the first set number can be 2000 and the second set number can be 200, but it is not limited to this.

[0100] After completing the initialization and configuration of the correlation noise matrix through the above steps, the ESKF obtained at this time is the system state error model in the above step S100.

[0101] In addition, from the steps S100 to S500 included in the laser point cloud dedistortion method provided in the embodiment of the present invention, it can be known that the linear velocity output by the millimeter wave radar is needed to update the prediction error information, and the linear velocity refers to the linear velocity of the carrier measured by the millimeter wave radar. However, the millimeter wave radar detects the relative velocity information of the carrier relative to the environment. In order to convert the relative velocity information into the linear velocity of the carrier itself, the linear velocity of the carrier can be obtained by the conversion operation in the relevant technology. However, since the millimeter wave radar also has zero bias and velocity Gaussian noise, in order to obtain a more accurate linear velocity of the carrier based on the relative velocity detected by the millimeter wave radar, in some embodiments, the laser point cloud dedistortion method provided in the embodiment of the present invention also provides a solution for converting the relative velocity detected by the millimeter wave radar into the linear velocity of the carrier, that is, the second set number of frame data detected by the millimeter wave radar can be used to make a histogram of all the targets detected by the millimeter wave radar according to the velocity information, and then the velocity with the largest number of histograms is selected to calculate the velocity of the carrier itself, and the calculation process involved can be expressed by the following formula:

[0102] i max = argmax i∈[0，n-1] H[i]

[0103]

[0104] In the above formula, i max Indicates the speed with the largest number in the histogram, n indicates the second set number, atgmax i∈[0，n-1] H[i] represents a function that finds the largest element from the data H[i]. represents the linear velocity of the carrier at time t, v stepGenerally limited by the speed resolution of millimeter wave radar, it can be set according to the performance of millimeter wave, for example, set to 0.25m / s, but not limited to this, b v Indicates the zero bias of the millimeter-wave radar, which is generally a fixed value. The average linear velocity calculated above can be used. It represents the linear velocity Gaussian noise at time t, which is a probability distribution and can be obtained by related technologies, which will not be elaborated here.

[0105] Understandably, The calculation formula is used as the carrier linear velocity output model, that is, as a calculation model for calculating the corresponding carrier linear velocity based on the relative velocity detected by the millimeter-wave radar. Based on this, during the execution of the above step S200, for the frame data collected by the millimeter-wave radar at the current moment, the carrier linear velocity can be first calculated by the above two formulas. At this time, the carrier linear velocity is the linear velocity output by the millimeter-wave radar at the current moment recorded in step S200.

[0106] Therefore, after constructing the system state error model and the linear velocity calculation model output by the millimeter wave radar through any of the above embodiments, these models can be pre-configured in the execution body used to execute the laser point cloud dedistortion method provided by the embodiment of the present invention.

[0107] In this way, during the movement of the carrier, the above step S100 can be performed first to obtain the predicted state information and predicted error information of the carrier at the next moment according to the system state error model, the acceleration and attitude information at the current moment. It can be understood that the acceleration and attitude information at the current moment are input into the system state error model so that the system state error model predicts the state and error of the carrier at the next moment.

[0108] The state prediction process can be expressed as:

[0109]

[0110]

[0111] b w (k+1|k)=b w (k|k)

[0112] b a (k+1|k)=b a (k|k)

[0113] Assuming that the current time is k, in the above formula, v(k+1|k) represents the predicted linear velocity at the next time, v(k|k) represents the linear velocity obtained by iterating the system state error model at time k-1. In simple terms, v(k|k) represents the linear velocity state at time k-1, and R(k|k) represents the posture information obtained by iterating the system state error model at time k-1. Indicates the acceleration output by the IMU at the current moment. t It represents the time interval between two consecutive data collected by IMU, which can be calculated based on the sampling frequency of IMU. R(k+1|k) represents the predicted posture information at the next moment. Indicates the angular velocity output by the IMU at the current moment. w (k+1|k) represents the predicted angular velocity deviation at the next moment, b w (k|k) represents the angular velocity deviation at the current moment. a (k+1|k) represents the predicted acceleration deviation at the next moment, b a (k|k) represents the acceleration deviation at the current moment.

[0114] After obtaining the above prediction status information, it can be recorded in a matrix form for the convenience of subsequent description or storage, that is:

[0115] X(k+1|k)=[v(k+1|k),R(k+1|k),b w (k+1|k),b a (k+1|k)]

[0116] In the above formula, X(k+1|k) is the predicted state information obtained in the above step S100, and the meanings of other letters can be found in the above related records and will not be repeated here.

[0117] In addition, the error prediction process can be expressed as:

[0118]

[0119]

[0120] d ba (k+1|k)=d ba (k|k)

[0121] d bw (k+1|k)=d bw (k|k)

[0122] Similarly, assuming that the current time is k, then in the above formula, d v (k+1|k) represents the predicted linear velocity error state at the next moment, d v(k|k) represents the linear velocity error state corresponding to the current moment, d R (k+1|k) represents the predicted attitude error state at the next moment, d R (k|k) represents the attitude error state corresponding to the current moment, d ba (k+1|k) represents the predicted acceleration deviation error state, d ba (k|k) represents the acceleration deviation error state corresponding to the current moment, d bw (k+1|k) represents the predicted angular velocity deviation error state at the next moment, d bw (k|k) represents the angular velocity deviation error state corresponding to the current moment.

[0123] also, b a (k|k), b w (k|k), d t The meanings of , R(k|k), etc. are the same as those described above and will not be repeated here.

[0124] After obtaining the above prediction status information, it can be recorded in a matrix form for the convenience of subsequent description or storage, that is:

[0125] d X (k+1|k)=[d v (k+1|k),d R (k+1|k),d ba (k+1|k),d bw (k+1|k)]

[0126] In the above formula, d X (k+1|k) is the prediction error information obtained in the above step S100. The meanings of other letters can be found in the above related records and will not be repeated here.

[0127] In addition, in order to facilitate the acquisition of target error information in the subsequent step S200, the four equations involved in the above error prediction process can be combined into d X (k+1|k)=Fd X (k|k), it can be seen that the F matrix is ​​composed of the coefficients in the above four equations. The matrix F will be used to obtain the target error information, as will be described later.

[0128] Therefore, after predicting the predicted state information and predicted error information of the carrier at the next moment through step S100, step S200 can be executed to further use the system state error model, the predicted state information, the predicted error information and the linear velocity output by the millimeter wave radar at the current moment to update the predicted error information and obtain the target error information. In this regard, in order to improve the accuracy of the acquired target error information, in some embodiments, the laser point cloud dedistortion method provided by the embodiment of the present invention also provides a corresponding solution, that is, in the above step S200, according to the system state error model, the predicted state information, the predicted error information and the linear velocity output by the millimeter wave radar at the current moment, the prediction error information is updated to obtain the target error information, including:

[0129] In step S210, the prediction covariance at the next moment is calculated according to the prediction error information, the IMU noise matrix in the system state error model and the covariance obtained by the previous prediction;

[0130] In step S220, a Kalman gain is calculated according to the predicted covariance, the linear velocity observation matrix and the millimeter wave radar noise matrix in the system state error model; wherein the linear velocity observation matrix is ​​calculated based on the predicted state information and the linear velocity output by the millimeter wave radar at the current moment;

[0131] In step S230, the linear velocity of the carrier at the current moment is calculated according to the linear velocity deviation and the linear velocity output by the millimeter wave radar at the current moment;

[0132] In step S240, target error information is obtained according to the Kalman gain, the linear velocity of the carrier at the current moment, the linear velocity observation matrix and the predicted state information.

[0133] For the above step S210, the calculation process can be expressed by the following formula:

[0134] cov(k+1|k)=F*cov(k|k)*F T +Q

[0135] Assuming that the current moment is k, in the above formula, cov(k+1|k) represents the predicted covariance of the next moment, cov(k|k) represents the covariance corresponding to the current moment (that is, the covariance obtained in the previous iteration, which can be understood as the covariance obtained in the previous prediction), Q is the IMU noise matrix in the system state error model, and the meaning of the matrix F has been recorded above, so it will not be repeated here.

[0136] As can be seen from the above, the matrix F is constructed based on the system in the four sets of calculation equations of the prediction error information. Therefore, in step S210, it is described as calculating the predicted covariance at the next moment based on the prediction error information, the IMU noise matrix in the system state error model and the covariance obtained from the previous prediction.

[0137] Next, step S220 may be performed to calculate the Kalman gain according to the prediction covariance, the linear velocity observation matrix, and the millimeter wave radar noise matrix in the system state error model. The calculation process may be expressed by the following formula:

[0138] K=cov(k+1|k)*H T *(H*cov(k+1|k)*H T +R z ) -1

[0139] Assuming that the current time is k, in the above formula, K represents the Kalman gain, H represents the linear velocity observation matrix, which is a constant, cov(k+1|k) represents the prediction covariance matrix calculated in step S210, and R z represents the millimeter-wave radar noise matrix.

[0140] In the above, the H matrix can be expressed by the formula Calculated, where X(k+1|k) represents the above prediction state information, Indicates the linear velocity output by the millimeter-wave radar at the current moment. Based on this, in the obtained H matrix, only the value at the position corresponding to the linear velocity is 1, and the values ​​at other positions are all 0.

[0141] Next, step S230 can be executed to further calculate the linear velocity of the carrier at the current moment according to the linear velocity deviation and the linear velocity output by the millimeter wave radar at the current moment. The linear velocity here can be understood as the updated linear velocity. Similarly, the calculation process in step S230 can be expressed by the following formula:

[0142]

[0143] In the above formula, v represents the linear velocity of the carrier at the current moment, Same as the above, indicating the linear velocity output by the millimeter wave radar at the current moment, b v It has the same meaning as described above, indicating the zero bias of the millimeter-wave radar, that is, the linear velocity deviation in step S230. R(k|k) also has the same meaning as described above, indicating the attitude information obtained by iterating the system state error model at time k-1.

[0144] Next, step S240 may be performed to obtain target error information according to the Kalman gain, the linear velocity of the carrier at the current moment, the linear velocity observation matrix and the predicted state information. The calculation process of the target error information may be expressed by the following formula:

[0145] d X (k+1|k+1)=K(vH k *X(k+1|k))

[0146] In the above formula, d X (k+1|k+1) represents the target error information, including the linear velocity error state quantity, attitude error state quantity, acceleration deviation error state quantity and angular velocity deviation error state quantity. K represents the Kalman gain, H kl The meanings of , X(k+1|k), v, etc. are the same as those described above and will not be repeated here.

[0147] After obtaining the target error information through step S200, step S300 may be executed to update the predicted state information according to the target error information to obtain the target state information. In this regard, in order to ensure the update accuracy while improving the update efficiency, in some embodiments, the laser point cloud dedistortion method provided by the embodiment of the present invention proposes a scheme for updating the predicted state information, that is, in the above step S300, the step of updating the predicted state information according to the target error information to obtain the target state information may include:

[0148] In step S310, the difference between the predicted state information and the target error information is used as the target state information. The calculation process involved can be expressed by the following formula:

[0149] X(k+1|k+1)=X(k+1|k)-d X (k+1|k+1)

[0150] In the above formula, X(k+1|k+1) represents the target state information, while X(k+1|k) and d X The meaning of (k+1|k+1) can be found in the above related records and will not be elaborated here.

[0151] It can be seen from the above formula that the principle of obtaining X(k|k) recorded above is the same as the principle of obtaining X(k+1|k+1).

[0152] It can be seen that after obtaining the X(k+1|k+1) that best matches the next moment, in order to ensure the correctness of the next round of iteration, d X (k+1|k+1) performs a zeroing operation, that is, let d X (k+1|k+1)=0, so as not to affect the correctness of subsequent iterations.

[0153] Furthermore, in order to ensure that the covariance used in the subsequent iteration process is the latest, that is, the covariance obtained by the previous prediction used in the calculation process of the above-mentioned prediction covariance is the latest, thereby further improving the correctness of the subsequent iterations, in some embodiments, while or after obtaining the target state information, the laser point cloud dedistortion method provided by the embodiment of the present invention may also include:

[0154] In step S320, the prediction covariance matrix is ​​updated according to the linear velocity observation matrix, the Kalman gain and the unit matrix to obtain an updated covariance; the updated covariance is used to calculate the prediction covariance at the next moment after the next moment.

[0155] The calculation process involved in step S320 can be expressed by the following formula:

[0156] cov(k+1|k+1)=(I-KH k )*cov(k+1|k)

[0157] In the above formula, cov(k+1|k+1) is the covariance updated in step S320, I represents the unit matrix, and the meanings of the remaining letters can be found in the relevant records above and will not be repeated here.

[0158] After obtaining the target error information and the target state information, step S400 can be executed to calculate the fused angular velocity and the fused linear velocity according to the target error information and the target state information. In this regard, in order to take into account the calculation accuracy and calculation efficiency of the fused angular velocity and the fused linear velocity, in some embodiments, the laser point cloud dedistortion method provided by the embodiment of the present invention provides a corresponding calculation scheme, that is, in the above step S400, the step of obtaining the fused angular velocity and the fused linear velocity according to the target error information and the target state information may include:

[0159] In step S410, the sum of the angular velocity output by the IMU at the current moment and the angular velocity deviation in the target error information is used as the fused angular velocity;

[0160] In step S420, the linear velocity in the target state information is used as the fused linear velocity.

[0161] In the above, the calculation of the fusion angular velocity in step S410 can be expressed by the following formula:

[0162]

[0163] Assuming that the current moment is k, then in the above formula, represents the fusion angular velocity at time k, Indicates the angular velocity output by the IMU at the current moment, b w (k+1|k+1) represents the angular velocity deviation in the target error information.

[0164] Similarly, the calculation of the fusion line speed in step S420 can be expressed by the following formula:

[0165]

[0166] Similarly, assuming that the current time is k, then in the above formula, represents the fused linear velocity at time k, and v(k+1|k+1) represents the linear velocity in the target state information.

[0167] Although a highly accurate fused angular velocity and fused linear velocity can be obtained through any of the above embodiments, since the fused angular velocity and fused linear velocity are to be used to remove the distortion of the current frame point cloud collected by the laser radar at the current moment, the fused angular velocity and fused linear velocity preferably have the same timestamp as the starting moment of the current frame point cloud. However, in some cases, the fused angular velocity and fused linear velocity cannot be completely consistent with the starting moment of the current frame point cloud, which may have a certain impact on the point cloud dedistortion effect. Therefore, in order to solve this technical problem, in some embodiments, the laser point cloud dedistortion method provided by the embodiment of the present invention also provides a corresponding solution, that is, after obtaining the fused angular velocity and fused linear velocity, in order to synchronize the fused angular velocity and fused linear velocity used in step S500 with the timestamp of the current frame point cloud, a bilinear interpolation method can be used to interpolate the fused angular velocity and fused linear velocity corresponding to the starting moment of the current frame point cloud based on the fused angular velocity and fused linear velocity, and use the interpolated fused angular velocity and fused linear velocity as the fused angular velocity and fused linear velocity applied in step S500.

[0168] After obtaining the fused angular velocity and the fused linear velocity through any of the above embodiments, step S500 can be executed to correct the point cloud data collected by the laser radar at the current moment. In order to improve the correction efficiency and the dedistortion effect to obtain point cloud data with higher accuracy, in some embodiments, the laser point cloud dedistortion method provided by the embodiment of the present invention also provides a better correction scheme. There are two correction schemes, one for the scenario of uniform motion of the carrier, and the other for the scenario of uniform motion and variable speed motion of the carrier, as follows:

[0169] The first correction scheme is for the scenario where the carrier moves at a constant speed:

[0170] That is, in the above step S500, the step of correcting the point cloud data currently collected by the laser radar according to the fused angular velocity and the fused linear velocity to obtain the corrected target point cloud data may include:

[0171] In step S511, for each non-starting point in the current frame point cloud collected by the laser radar at the current moment, the corrected coordinates of the non-starting point are calculated according to the initial coordinates of the non-starting point, the time difference between the non-starting point and the starting point, the fused angular velocity and the fused linear velocity.

[0172] It can be understood that the starting point in the current frame point cloud is used as a reference, and the initial coordinates of the starting point are not corrected. Instead, the initial coordinates of each non-starting point are corrected through step S511, so that the corrected coordinates of each non-starting point are converted to the coordinate system of the starting point. In this way, it can be ensured that each point in the current frame point cloud is corrected to the corresponding coordinate position of the same coordinate system, thereby achieving distortion removal.

[0173] In the above step S511, the correction process can be expressed by the following formula:

[0174]

[0175] In the above formula, P i adjust represents the corrected coordinates of the i-th point in the current frame point cloud, T d represents the time difference of the i-th point relative to the starting point, represents the initial coordinates of the i-th point, represents the fusion angular velocity, Indicates the fusion line speed.

[0176] It should be added that in the above formula, the default fusion angular velocity, fusion linear velocity and lidar are all in the same coordinate system. If the fusion angular velocity and fusion linear velocity are not in the same coordinate system as the lidar, the fusion angular velocity and fusion linear velocity need to be multiplied by the corresponding external parameters to convert them to the coordinate system where the lidar is located before performing the above correction calculation.

[0177] The second correction scheme is applicable to scenarios where the carrier moves at a constant speed or at a variable speed:

[0178] That is, in the above step S500, the step of correcting the point cloud data currently collected by the laser radar according to the fused angular velocity and the fused linear velocity to obtain the corrected target point cloud data may include:

[0179] In step S510, for each non-starting point in the current frame point cloud collected by the laser radar at the current moment, the corrected displacement of the non-starting point is obtained according to the corrected displacement of the previous point adjacent to the non-starting point, the corrected linear velocity of the previous point, the acceleration corresponding to the previous point, and the time difference between the non-starting point and the previous point; wherein, when the previous point is the starting point, the corrected linear velocity of the previous point is the fused linear velocity, and when the previous point is not the starting point, the corrected linear velocity of the previous point is calculated based on the corrected linear velocity of the previous point adjacent to the previous point, the acceleration corresponding to the previous point, the acceleration deviation in the target state information, and the time difference between the previous point and the previous point;

[0180] In step S520, for each non-starting point in the current frame point cloud, the corrected coordinates of the non-starting point are calculated according to the initial coordinates of the non-starting point, the time difference between the non-starting point and the starting point, the fused angular velocity and the corrected displacement of the non-starting point.

[0181] The calculation process involved in the above step S510 can be expressed by the following formula:

[0182]

[0183] In the above formula, pos i Indicates the corrected displacement of the i-th point in the current frame point cloud. The displacement is relative to the starting point. pos i-1 represents the displacement of the i-1th point corrected by the same principle as above, v i-1 represents the corrected linear velocity of the i-1th point, δt represents the time difference between the i-th point and the i-1th point, which can be determined according to the sampling frequency of the laser radar. It represents the acceleration of the i-1th point corresponding to the timestamp, which can be obtained by any of the implementations described above, for example, the acceleration output by the IMU at the corresponding timestamp.

[0184] From the above displacement correction formula, it can be seen that the displacement correction of the i-th point needs to depend on the displacement correction of the i-1-th point. It can be seen that the displacement correction process also involves the process of point-by-point iterative correction. Therefore, there must be an iteration starting point, which is the starting point in the current frame point cloud. Since the starting point is the first point, its pos0 displacement can be initialized to 0 and no correction is required.

[0185] In addition, the above displacement correction also depends on the corrected linear velocity of the i-1th point. It can be seen that the linear velocity correction also involves a point-by-point iterative correction process. There also needs to be an iteration starting point, that is, the linear velocity of the starting point in the current frame point cloud. In this embodiment, the linear velocity of the starting point is initialized to the fusion linear velocity, that is: Among them, v0 is the corrected linear velocity of the starting point, is the fusion linear velocity. Based on this, for each non-starting point after the starting point, the correction process of its linear velocity can be expressed by the following formula:

[0186]

[0187] In the above formula, v i Indicates the corrected linear velocity of the i-th point in the current frame point cloud, v i-1 It indicates the corrected linear velocity of the i-1th point in the point cloud of the current frame. The meanings of the other letters are recorded above and will not be repeated here.

[0188] After obtaining the corrected displacement of the non-starting point in the current frame point cloud, step S520 can be executed to calculate the corrected coordinates of the non-starting point according to the initial coordinates of the non-starting point, the time difference between the non-starting point and the starting point, the fusion angular velocity and the corrected displacement of the non-starting point. The calculation process involved can be expressed by the following formula:

[0189]

[0190] In the above formula, P i adjust Indicates the corrected coordinates of the i-th point in the current frame point cloud. Indicates the fusion angular velocity at the current moment, T d represents the time difference between the i-th point and the starting point, Indicates the initial coordinates of the i-th point, pos i Represents the corrected displacement of the i-th point.

[0191] In this way, the corrected coordinates of each non-starting point can be converted to the coordinate system of the starting point.

[0192] It should also be added that in the above formula, the default fusion angular velocity, fusion linear velocity and lidar are all in the same coordinate system. If the fusion angular velocity and fusion linear velocity are not in the same coordinate system as the lidar, the fusion angular velocity and fusion linear velocity need to be multiplied by the corresponding external parameters to convert them to the coordinate system where the lidar is located before performing the relevant correction calculations.

[0193] In practical applications, one of the first correction scheme and the second correction scheme can be selected for application according to the application requirements of the carrier to which the laser point cloud dedistortion method provided by the embodiment of the present invention is applied, or both correction schemes can be applied. When applying the two correction schemes, a judgment condition for identifying the uniform motion and variable speed motion of the carrier can be added. When determining that the carrier is in uniform motion, the first correction scheme is selected to improve the correction efficiency; and when determining that the carrier is in deceleration or acceleration motion, the second correction scheme is selected to better remove the point cloud distortion under variable speed motion.

[0194] It is worth noting that the technical features or technical solutions in any of the above embodiments of the present invention can be combined with each other as long as there is no combination contradiction.

[0195] In order to perform the corresponding steps in the above-mentioned laser point cloud dedistortion method embodiment and various possible methods, a method for implementing a laser point cloud dedistortion device is given below. Optionally, the laser point cloud dedistortion device can adopt the above-mentioned Figure 1 For further information, please refer to Figure 3 , Figure 3 This is a functional module diagram of a laser point cloud dedistortion device provided in an embodiment of the present invention. It should be noted that the basic principle and technical effects of the laser point cloud dedistortion device provided in this embodiment are the same as those of the corresponding method embodiments described above. For the sake of brief description, for parts not mentioned in this embodiment, reference can be made to the corresponding contents in the corresponding method embodiments described above. The laser point cloud dedistortion device 300 includes:

[0196] The prediction module 310 is configured to: during the movement of the carrier equipped with the IMU, the millimeter wave radar and the laser radar, process the predicted state information and predicted error information of the carrier at the next moment according to the pre-built system state error model and the acceleration and attitude information at the current moment obtained by the IMU; wherein the predicted state information includes: predicted linear velocity, predicted attitude information, predicted acceleration deviation, predicted angular velocity deviation, and the predicted error information includes: predicted linear velocity error state quantity, predicted attitude error state quantity, predicted acceleration deviation error state quantity, and predicted angular velocity deviation error state quantity;

[0197] The error updating module 320 is configured to: update the predicted error information according to the system state error model, the predicted state information, the predicted error information and the linear velocity output by the millimeter wave radar at the current moment to obtain target error information;

[0198] The state updating module 330 is configured to: update the predicted state information according to the target error information to obtain target state information;

[0199] The fusion module 340 is configured to: obtain a fusion angular velocity and a fusion linear velocity according to the target error information and the target state information;

[0200] The correction module 350 is configured to correct the point cloud data collected by the laser radar at the current moment according to the fusion angular velocity and the fusion linear velocity to obtain corrected target point cloud data.

[0201] In some embodiments, the laser point cloud dedistortion device 300 may further include:

[0202] Building blocks are configured as:

[0203] The attitude information, linear velocity, acceleration deviation and angular velocity deviation are configured as motion state quantities of the error state Kalman filter;

[0204] Configuring the attitude error, the linear velocity error, the acceleration deviation error and the angular velocity deviation error as error state quantities of the error state Kalman filter;

[0205] The error state Kalman filter configured with the motion state quantity and the error state quantity is initialized to obtain the system state error model.

[0206] In some embodiments, the process of initializing the error state Kalman filter configured with the motion state quantity and the error state quantity to obtain the system state error model is configured as follows:

[0207] Calculate the acceleration mean, angular velocity mean, acceleration variance and angular velocity variance according to a first set number of frame data collected by the IMU in a stationary state;

[0208] Calculating a linear velocity variance according to a second set number of frames of data collected by the millimeter-wave radar in a stationary state;

[0209] Initializing the posture information and linear velocity in the error state Kalman filter to 0, and initializing the acceleration deviation and angular velocity deviation to the acceleration mean and the angular velocity mean, respectively;

[0210] Initializing all error state quantities in the error state Kalman filter to 0;

[0211] According to the acceleration variance, angular velocity variance and linear velocity variance, an IMU noise matrix and a millimeter wave radar noise matrix of the error state Kalman filter are constructed to obtain the system state error model.

[0212] In some embodiments, the error updating module 320 updates the predicted error information according to the system state error model, the predicted state information, the predicted error information, and the linear velocity output by the millimeter wave radar at the current moment to obtain the target error information, and the process is configured as follows:

[0213] Calculate the predicted covariance at the next moment according to the predicted error information, the IMU noise matrix in the system state error model and the covariance obtained from the last prediction;

[0214] The Kalman gain is calculated based on the predicted covariance, the linear velocity observation matrix and the millimeter wave radar noise matrix in the system state error model; wherein the linear velocity observation matrix is ​​calculated based on the predicted state information and the linear velocity output by the millimeter wave radar at the current moment;

[0215] The linear velocity of the carrier at the current moment is calculated according to the linear velocity deviation and the linear velocity output by the millimeter wave radar at the current moment;

[0216] Target error information is obtained according to the Kalman gain, the linear velocity of the carrier at the current moment, the linear velocity observation matrix and the predicted state information.

[0217] In some embodiments, the process in which the state updating module 330 updates the predicted state information according to the target error information to obtain the target state information is configured as follows:

[0218] The difference between the predicted state information and the target error information is used as the target state information.

[0219] In some embodiments, the state update module 330 is also configured to: while or after obtaining the target state information, update the predicted covariance matrix according to the linear velocity observation matrix, the Kalman gain and the unit matrix to obtain an updated covariance; the updated covariance is used to calculate the predicted covariance at the next moment after the next moment.

[0220] In some embodiments, the process of obtaining the fused angular velocity and the fused linear velocity by the fusion module 340 according to the target error information and the target state information is configured as follows:

[0221] The sum of the angular velocity output by the IMU at the current moment and the angular velocity deviation in the target error information is used as the fused angular velocity;

[0222] The linear velocity in the target state information is used as the fused linear velocity.

[0223] In some embodiments, the correction module 350 corrects the point cloud data collected by the laser radar at the current moment according to the fusion angular velocity and the fusion linear velocity to obtain the corrected target point cloud data, and the process is configured as follows:

[0224] For each non-starting point in the current frame point cloud collected by the laser radar at the current moment, the corrected coordinates of the non-starting point are calculated according to the initial coordinates of the non-starting point, the time difference between the non-starting point and the starting point, the fused angular velocity and the fused linear velocity.

[0225] In other embodiments, the correction module 350 corrects the point cloud data currently collected by the laser radar according to the fusion angular velocity and the fusion linear velocity to obtain the corrected target point cloud data, and the process is configured as follows:

[0226] For each non-starting point in the current frame point cloud collected by the laser radar at the current moment, the corrected displacement of the non-starting point is obtained according to the corrected displacement of the previous point adjacent to the non-starting point, the corrected linear velocity of the previous point, the acceleration corresponding to the previous point, and the time difference between the non-starting point and the previous point; wherein, when the previous point is the starting point, the corrected linear velocity of the previous point is the fused linear velocity, and when the previous point is not the starting point, the corrected linear velocity of the previous point is calculated based on the corrected linear velocity of the previous point adjacent to the previous point, the acceleration corresponding to the previous point, the acceleration deviation in the target state information, and the time difference between the previous point and the previous point;

[0227] For each non-starting point in the current frame point cloud, the corrected coordinates of the non-starting point are calculated according to the initial coordinates of the non-starting point, the time difference between the non-starting point and the starting point, the fused angular velocity and the corrected displacement of the non-starting point.

[0228] Optionally, the above modules can be stored in the form of software or firmware. Figure 1 The memory shown in the figure or solidified in the operating system (OS) of the electronic device, and can be Figure 1 Meanwhile, the data and program codes required for executing the above modules can be stored in the memory.

[0229] In several embodiments provided in the present application, it should be understood that the disclosed devices and methods can also be implemented in other ways. The device embodiments described above are merely schematic. For example, the flowcharts and block diagrams in the accompanying drawings show the possible architecture, functions and operations of the devices, methods and computer program products according to multiple embodiments of the present invention. In this regard, each box in the flowchart or block diagram can represent a module, a program segment or a part of a code, and the module, a program segment or a part of a code contains one or more executable instructions for implementing the specified logical function. It should also be noted that in some alternative implementations, the functions marked in the box can also occur in a different order from the order marked in the accompanying drawings. For example, two consecutive boxes can actually be executed substantially in parallel, and they can sometimes be executed in the opposite order, depending on the functions involved. It should also be noted that each box in the block diagram and / or flowchart, and the combination of boxes in the block diagram and / or flowchart can be implemented with a dedicated hardware-based system that performs a specified function or action, or can be implemented with a combination of dedicated hardware and computer instructions.

[0230] In addition, the functional modules in the various embodiments of the present invention may be integrated together to form an independent part, or each module may exist independently, or two or more modules may be integrated to form an independent part.

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

[0232] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A laser point cloud dedistortion method, characterized in that: include: During the movement of a carrier equipped with an IMU, a millimeter-wave radar, and a laser radar, the predicted state information and predicted error information of the carrier at the next moment are processed according to a pre-constructed system state error model and the acceleration and attitude information at the current moment obtained by the IMU; wherein the predicted state information includes: predicted linear velocity, predicted attitude information, predicted acceleration deviation, predicted angular velocity deviation, and the predicted error information includes: predicted linear velocity error state quantity, predicted attitude error state quantity, predicted acceleration deviation error state quantity, and predicted angular velocity deviation error state quantity; According to the system state error model, the predicted state information, the predicted error information and the linear speed output by the millimeter wave radar at the current moment, the predicted error information is updated to obtain target error information; The predicted state information is updated according to the target error information to obtain target state information; Obtaining a fusion angular velocity and a fusion linear velocity according to the target error information and the target state information; According to the fusion angular velocity and the fusion linear velocity, the point cloud data collected by the laser radar at the current moment is corrected to obtain corrected target point cloud data.

2. The method according to claim 1, characterized in that The construction process of the system state error model includes: The attitude information, linear velocity, acceleration deviation and angular velocity deviation are configured as motion state quantities of the error state Kalman filter; Configuring the attitude error, the linear velocity error, the acceleration deviation error and the angular velocity deviation error as error state quantities of the error state Kalman filter; The error state Kalman filter configured with the motion state quantity and the error state quantity is initialized to obtain the system state error model.

3. The method according to claim 2, characterized in that The step of initializing the error state Kalman filter configured with the motion state quantity and the error state quantity to obtain the system state error model includes: Calculate the acceleration mean, angular velocity mean, acceleration variance and angular velocity variance according to a first set number of frame data collected by the IMU in a stationary state; Calculating a linear velocity variance according to a second set number of frames of data collected by the millimeter-wave radar in a stationary state; Initializing the posture information and linear velocity in the error state Kalman filter to 0, and initializing the acceleration deviation and angular velocity deviation to the acceleration mean and the angular velocity mean, respectively; Initializing all error state quantities in the error state Kalman filter to 0; According to the acceleration variance, angular velocity variance and linear velocity variance, an IMU noise matrix and a millimeter-wave radar noise matrix of the error state Kalman filter are constructed to obtain the system state error model.

4. The method according to claim 1, characterized in that: The step of updating the predicted error information according to the system state error model, the predicted state information, the predicted error information and the linear speed output by the millimeter wave radar at the current moment to obtain the target error information includes: Calculate the predicted covariance at the next moment according to the predicted error information, the IMU noise matrix in the system state error model and the covariance obtained from the last prediction; The Kalman gain is calculated based on the predicted covariance, the linear velocity observation matrix and the millimeter wave radar noise matrix in the system state error model; wherein the linear velocity observation matrix is ​​calculated based on the predicted state information and the linear velocity output by the millimeter wave radar at the current moment; The linear velocity of the carrier at the current moment is calculated according to the preset linear velocity deviation and the linear velocity output by the millimeter wave radar at the current moment; Target error information is obtained according to the Kalman gain, the linear velocity of the carrier at the current moment, the linear velocity observation matrix and the predicted state information.

5. The method according to claim 4, characterized in that The step of updating the predicted state information according to the target error information to obtain the target state information comprises: Taking the difference between the predicted state information and the target error information as the target state information; And / or, while or after obtaining the target state information, the method further includes: The prediction covariance matrix is ​​updated according to the linear velocity observation matrix, the Kalman gain and the unit matrix to obtain an updated covariance; the updated covariance is used to calculate the prediction covariance at the next moment after the next moment; And / or, the step of obtaining the fused angular velocity and the fused linear velocity according to the target error information and the target state information comprises: The sum of the angular velocity output by the IMU at the current moment and the angular velocity deviation in the target error information is used as the fused angular velocity; The linear velocity in the target state information is used as the fused linear velocity.

6. The method according to claim 1, characterized in that The step of correcting the point cloud data currently collected by the laser radar according to the fused angular velocity and the fused linear velocity to obtain corrected target point cloud data comprises: For each non-starting point in the current frame point cloud collected by the laser radar at the current moment, the corrected coordinates of the non-starting point are calculated according to the initial coordinates of the non-starting point, the time difference between the non-starting point and the starting point, the fused angular velocity and the fused linear velocity.

7. The method according to claim 1, characterized in that The step of correcting the point cloud data currently collected by the laser radar according to the fused angular velocity and the fused linear velocity to obtain corrected target point cloud data comprises: For each non-starting point in the current frame point cloud collected by the laser radar at the current moment, the corrected displacement of the non-starting point is obtained according to the corrected displacement of the previous point adjacent to the non-starting point, the corrected linear velocity of the previous point, the acceleration corresponding to the previous point, and the time difference between the non-starting point and the previous point; wherein, when the previous point is the starting point, the corrected linear velocity of the previous point is the fused linear velocity, and when the previous point is not the starting point, the corrected linear velocity of the previous point is calculated based on the corrected linear velocity of the previous point adjacent to the previous point, the acceleration corresponding to the previous point, the acceleration deviation in the target state information, and the time difference between the previous point and the previous point; For each non-starting point in the current frame point cloud, the corrected coordinates of the non-starting point are calculated according to the initial coordinates of the non-starting point, the time difference between the non-starting point and the starting point, the fused angular velocity and the corrected displacement of the non-starting point.

8. A laser point cloud dedistortion device, characterized in that: include: The prediction module is configured to: during the movement of a carrier equipped with an IMU, a millimeter-wave radar, and a laser radar, process the predicted state information and predicted error information of the carrier at the next moment according to a pre-constructed system state error model and the acceleration and attitude information at the current moment obtained by the IMU; wherein the predicted state information includes: predicted linear velocity, predicted attitude information, predicted acceleration deviation, and predicted angular velocity deviation; and the predicted error information includes: predicted linear velocity error state quantity, predicted attitude error state quantity, predicted acceleration deviation error state quantity, and predicted angular velocity deviation error state quantity; The error updating module is configured to: update the predicted error information according to the system state error model, the predicted state information, the predicted error information and the linear speed output by the millimeter wave radar at the current moment to obtain target error information; A state updating module is configured to: update the predicted state information according to the target error information to obtain target state information; A fusion module is configured to: obtain a fusion angular velocity and a fusion linear velocity according to the target error information and the target state information; The correction module is configured to correct the point cloud data collected by the laser radar at the current moment according to the fusion angular velocity and the fusion linear velocity to obtain corrected target point cloud data.

9. An electronic device, characterized in that: The invention comprises a processor and a memory, wherein the memory stores machine executable instructions that can be executed by the processor, and the processor can execute the machine executable instructions to implement the method according to any one of claims 1 to 7.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the method according to any one of claims 1 to 7 is implemented.

Images