A laser radar motion compensation method, device, system, equipment and medium

Abstract

The present application relates to the field of artificial intelligence technology, and in particular to a laser radar motion compensation method, device, system, equipment and medium, which is used to perform position compensation on the laser radar point cloud data collected in a violent motion scene. The method includes: obtaining the point cloud data of the laser radar current frame, the positioning data corresponding to the current frame of the positioning device, and the angle change data of the gimbal in the target time period; based on the point cloud data and the angle change data, determining the gimbal angle of each spatial point in the point cloud data at the sampling moment, and converting each spatial point to a first coordinate system according to the predetermined position calibration parameters between the laser radar and the gimbal; based on the positioning data, determining the target rotation offset matrix; based on the target rotation offset matrix, performing position compensation on each spatial point in the first coordinate system, obtaining the position coordinates of each spatial point in the predefined second coordinate system, and obtaining the compensated point cloud data.

Description

Technical Field

[0001] The present application relates to the field of artificial intelligence technology, and in particular to a motion compensation method, device, system, equipment and medium for a laser radar. Background Art

[0002] With the rapid development of artificial intelligence and semiconductor technology, smart construction has gradually come into people's eyes. Compared with the construction management of traditional construction sites, smart construction integrates more advanced technologies such as sensors, artificial intelligence and virtual reality. Smart construction sites use information technology to accurately control the progress of engineering projects and simulate construction scenarios. They also conduct in-depth data mining and analysis by managing construction process data, provide project construction trend forecasts and project expert plans, and establish an information ecosystem of smart construction sites with interconnected collaboration, smart production, and scientific management.

[0003] The main source of information for smart construction sites is the 3D reconstructed point cloud model, but the 3D reconstructed point cloud model of a large scene (radius ≥ 300 meters) cannot be obtained by rotating a fixed laser radar (ranging accuracy ≤ 100m). The optimal method at this stage is to use a rotating laser radar mounted on a mobile vehicle to collect point cloud data at different points and angles, and splice the point cloud data according to the posture information to achieve 3D reconstruction of a large scene range. However, since the laser radar may be in a rotating state during the collection of each frame of point cloud data, and is affected by factors such as the movement of the mobile vehicle and produces violent movements, the collection accuracy of the point cloud data is reduced, which in turn affects the model accuracy when using point cloud data for 3D reconstruction.

[0004] Therefore, it is necessary to design a motion compensation method for lidar for intense motion scenes to overcome the above defects. Summary of the invention

[0005] The embodiments of the present application provide a laser radar motion compensation method, device, system, equipment and medium for performing position compensation on laser radar point cloud data collected in intense motion scenes.

[0006] In a first aspect, an embodiment of the present application provides a motion compensation method for a laser radar, which is applied to a laser radar connected to a rotating component of a gimbal, wherein the gimbal is mounted on a mobile vehicle, and a non-rotating component of the gimbal is connected to a positioning device, and the method includes:

[0007] Acquire the point cloud data of the current frame of the laser radar, the positioning data corresponding to the current frame of the positioning device, and the angle change data of the pan / tilt within a target time period;

[0008] Based on the point cloud data and the angle change data, determine the gimbal angle of each spatial point in the point cloud data at a sampling moment, and according to a predetermined position calibration parameter between the laser radar and the gimbal, use the gimbal angle corresponding to each spatial point sampling moment to transform each spatial point into a first coordinate system, where the first coordinate system is a coordinate system established with the current position of the gimbal as the origin;

[0009] Based on the positioning data, a target rotation offset matrix is ​​determined for characterizing a change in the posture of the current position of the gimbal relative to the initial position of the gimbal;

[0010] Based on the target rotation offset matrix, position compensation is performed on each spatial point in the first coordinate system to obtain the position coordinates of each spatial point in the predefined second coordinate system, thereby obtaining compensated point cloud data.

[0011] In a second aspect, an embodiment of the present application provides a motion compensation device of a laser radar, which is applied to a laser radar connected to a rotating part of a gimbal, and the gimbal is mounted on a mobile vehicle, and a non-rotating part of the gimbal is connected to a positioning device, including:

[0012] An acquisition unit, used to acquire point cloud data of the current frame of the laser radar, positioning data corresponding to the current frame of the positioning device, and angle change data of the pan / tilt within a target time period;

[0013] a first position compensation unit, configured to determine the pan-tilt angle of each spatial point in the point cloud data at a sampling moment based on the point cloud data and the angle change data, and to convert each spatial point into a first coordinate system using the pan-tilt angle corresponding to each spatial point sampling moment according to a predetermined position calibration parameter between the laser radar and the pan-tilt, wherein the first coordinate system is a coordinate system established with the current position of the pan-tilt as an origin;

[0014] A processing unit, configured to determine, based on the positioning data, a target rotation offset matrix for characterizing a change in the posture of the current position of the gimbal relative to the initial position of the gimbal;

[0015] The second position compensation unit is used to perform position compensation on each spatial point in the first coordinate system based on the target rotation offset matrix, obtain the position coordinates of each spatial point in the predefined second coordinate system, and obtain compensated point cloud data.

[0016] Optionally, the first position compensation unit is specifically used to:

[0017] Using the timestamp information of the point cloud data as the acquisition time of the last spatial point in the point cloud data, and determining the radar sampling time corresponding to each spatial point in the point cloud data based on the sampling time of the last spatial point according to the attribute parameters of the laser radar;

[0018] According to the granularity of the pan / tilt angle change, the angle change data is split to determine the pan / tilt angle at each pan / tilt sampling moment;

[0019] For each spatial point, perform the following operations:

[0020] If the radar sampling moment corresponding to the spatial point has a corresponding pan / tilt sampling moment, the pan / tilt angle at the pan / tilt sampling moment is determined as the pan / tilt angle corresponding to the spatial point sampling moment;

[0021] If there is no corresponding gimbal sampling moment for the radar sampling moment corresponding to the spatial point, a preset algorithm is used to calculate the gimbal angle corresponding to the spatial point sampling moment based on the gimbal angles at two adjacent gimbal sampling moments of the radar sampling moment.

[0022] Optionally, when the first position compensation unit calculates the pan-tilt angle corresponding to the spatial point sampling moment using a preset algorithm, the following formula is used to calculate the pan-tilt angle corresponding to the spatial point sampling moment:

[0023]

[0024] in, is the pan / tilt angle corresponding to the spatial point sampling moment, is the jth PTZ sampling moment, is the j-1th PTZ sampling time, T is the time difference between two adjacent PTZ sampling times, is the i-th radar sampling moment, and lie in and between, for The corresponding gimbal angle at the moment, for The gimbal angle corresponding to the moment.

[0025] Optionally, the first position compensation unit is specifically used to:

[0026] Transform each spatial point into the first coordinate system using the following formula:

[0027]

[0028] in, is the position of the spatial point in the first coordinate system, is the pan / tilt angle corresponding to the spatial point sampling moment, pr R l and pr T l is a predetermined position calibration parameter between the laser radar and the gimbal, is the sampling position of the spatial point.

[0029] Optionally, the positioning device includes: an inertial measurement unit (IMU) and a global navigation satellite system (GNSS).

[0030] Optionally, the processing unit is specifically used to:

[0031] Accumulating and integrating the IMU data of the current frame of the IMU to determine an estimated value of the posture change of the current position of the gimbal relative to the initial position of the gimbal, determining an observed value of the posture change of the current position of the gimbal relative to the initial position of the gimbal based on the GNSS data of the current frame of the GNSS, and fusing the predicted value and the observed value using an extended Kalman filter to obtain an estimated rotation offset matrix;

[0032] Performing KD tree indexing on the global point cloud data in the second coordinate system, calculating the covariance matrix of the neighborhood point cloud of each spatial point in the global point cloud data, and calculating the plane unit normal vector of the neighborhood point cloud of each spatial point based on the covariance matrix, wherein the global point cloud data is point cloud data that has been position compensated;

[0033] Based on the plane unit normal vector, an objective function for calculating the distance error between the spatial point in the first coordinate system and the spatial point in the global point cloud data and the plane is constructed, and the estimated rotation offset matrix is ​​used as the initial position compensation parameter of the objective function. The minimum value of the objective function is determined through multiple iterative calculations using an optimization algorithm, and the position compensation parameter corresponding to the minimum value of the objective function is determined as the target rotation offset matrix.

[0034] Optionally, the objective function is:

[0035]

[0036] in, is a spatial point in the first coordinate system, is the i-th spatial point in the global point cloud data, w R ptz and w T ptzis a position compensation parameter, the initial value of which is the value of the estimated rotation offset matrix, is the plane unit normal vector of the ith spatial point in the global point cloud data.

[0037] Optionally, the second position compensation unit is specifically used to:

[0038] Use the following formula to perform position compensation on each spatial point in the first coordinate system:

[0039]

[0040] in, is the position coordinate of the spatial point in the second coordinate system, is the position coordinate of the spatial point in the first coordinate system, w R ptz and w T ptz The value of is determined according to the target rotation offset matrix.

[0041] Optionally, the processing unit is further used for:

[0042] Based on part or all of the global point cloud data in the second coordinate system, three-dimensional point cloud reconstruction is performed to generate a three-dimensional map.

[0043] In a third aspect, an embodiment of the present application provides a three-dimensional point cloud reconstruction system, the system comprising: a laser radar, a gimbal, a mobile vehicle, and a positioning device, wherein the laser radar is connected to the rotating part of the gimbal, the positioning device is connected to the non-rotating part of the gimbal, and the gimbal is mounted on a mobile vehicle, the laser radar collects point cloud data, and uses any one of the laser radar motion compensation methods in the first aspect mentioned above to perform position compensation on the collected point cloud data, determine the position coordinates of the point cloud data, and reconstruct the three-dimensional point cloud based on the position coordinates of part or all of the point cloud data.

[0044] In a fourth aspect, an embodiment of the present application provides an electronic device comprising a processor and a memory, wherein the memory stores a computer program, and when the computer program is executed by the processor, the processor executes the steps of any one of the laser radar motion compensation methods in the first aspect above.

[0045] In a fifth aspect, an embodiment of the present application provides a machine-readable storage medium, on which instructions are stored, and when the instructions are executed by a processor, the processor is configured to execute any one of the laser radar motion compensation methods in the first aspect mentioned above.

[0046] The beneficial effects of this application are as follows:

[0047] The laser radar motion compensation method, device, system, equipment and medium provided in the embodiments of the present application obtain the point cloud data of the current frame of the laser radar, the positioning data corresponding to the current frame of the positioning device, and the angle change data of the gimbal within the target time period. When performing position compensation on the point cloud data collected by the laser radar, the motion compensation process is decomposed into rotation compensation in which the gimbal drives the laser radar to rotate and movement compensation in which the mobile vehicle drives the gimbal to move.

[0048] Firstly, based on the point cloud data and the angle change data, the gimbal angle of each spatial point in the point cloud data at the sampling moment is determined, and according to the predetermined position calibration parameters between the laser radar and the gimbal, the gimbal angle corresponding to each spatial point at the sampling moment is used to convert each spatial point into the first coordinate system, and rotation compensation is performed on the point cloud data collected by the laser radar; secondly, the target rotation offset matrix is ​​determined based on the positioning data, and based on the target rotation offset matrix, position compensation is performed on each spatial point in the first coordinate system to obtain the position coordinates of each spatial point in the second coordinate system, and motion compensation is performed on the point cloud data collected by the laser radar. Compared with the prior art, motion compensation of the point cloud data collected by the laser radar is realized in the scenario where the laser radar rotates with the gimbal and the gimbal moves with the mobile vehicle, that is, in the scenario where the laser radar moves violently.

[0049] Other features and advantages of the present application will be described in the following description, and partly become apparent from the description, or be understood by practicing the present application. The purpose and other advantages of the present application can be realized and obtained by the structures specifically pointed out in the written description, claims, and drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0050] The drawings described herein are used to provide a further understanding of the present application and constitute a part of the present application. The illustrative embodiments of the present application and their descriptions are used to explain the present application and do not constitute an improper limitation on the present application. In the drawings:

[0051] Figure 1 A schematic diagram of an application scenario in an embodiment of the present application;

[0052] Figure 2 This is a flowchart of an implementation method of a laser radar motion compensation method in an embodiment of the present application;

[0053] Figure 3 It is an implementation flow chart of a specific implementation process of a motion compensation method of another laser radar in an embodiment of the present application;

[0054] Figure 4 This is a schematic diagram of the composition structure of a motion compensation device of a laser radar in an embodiment of the present application. DETAILED DESCRIPTION

[0055] In order to make the purpose, technical solution and advantages of the embodiments of the present application clearer, the technical solution of the present application will be clearly and completely described below in conjunction with the drawings in the embodiments of the present application. Obviously, the described embodiments are part of the embodiments of the technical solution of the present application, rather than all of the embodiments. Based on the embodiments recorded in the application documents, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the technical solution of the present application.

[0056] The following is an introduction to some concepts involved in the embodiments of the present application.

[0057] LiDAR: refers to a radar system that emits laser beams to detect the position, speed and other characteristic quantities of a target. Its working principle is to transmit a measurement signal to the target, then compare the signal reflected from the target with the transmitted signal, and after appropriate processing, obtain target-related information such as target distance, direction, altitude, speed, attitude and other parameters.

[0058] Motion compensation: It is an essential requirement for using LiDAR on a mobile vehicle to obtain environmental data. Because the LiDAR is fixed on a vehicle or robot, and the vehicle or robot may be in motion, by introducing motion compensation, the point cloud data collected by the LiDAR is corrected into a static map and environmental data in the reference coordinate system.

[0059] The following is a brief introduction to the design concept of the embodiment of the present application:

[0060] With the rapid development of artificial intelligence and semiconductor technology, smart construction has gradually come into people's eyes. Compared with the construction management of traditional construction sites, smart construction integrates more advanced technologies such as sensors, artificial intelligence and virtual reality. Smart construction sites use information technology to accurately control the progress of engineering projects and simulate construction scenarios. They also conduct in-depth data mining and analysis by managing construction process data, provide project construction trend forecasts and project expert plans, and establish an information ecosystem of smart construction sites with interconnected collaboration, smart production, and scientific management.

[0061] The main source of information for smart construction sites is the 3D reconstructed point cloud model, but the 3D reconstructed point cloud model of a large scene (for example, a radius ≥ 300 meters) cannot be obtained by rotating a fixed laser radar (usually with a ranging accuracy of ≤ 100m). The optimal method at this stage is to use a rotating laser radar mounted on a mobile vehicle to collect point cloud data at different points and angles, and to splice the point cloud data according to the posture information to achieve 3D reconstruction of a large scene range. However, since the laser radar may be in a rotating state during the collection of each frame of point cloud data, and is affected by factors such as the movement of the mobile vehicle and produces violent movements, the collection accuracy of the point cloud data is reduced, which in turn affects the model accuracy when using point cloud data for 3D reconstruction.

[0062] In view of this, the embodiments of the present application provide a laser radar motion compensation method, device, system, equipment and medium. After obtaining the point cloud data of the current frame of the laser radar, the positioning data corresponding to the current frame of the positioning device, and the angle change data of the gimbal within the target time period, when performing position compensation on the point cloud data collected by the laser radar, the motion compensation process is decomposed into rotation compensation in which the gimbal drives the laser radar to rotate and movement compensation in which the mobile vehicle drives the gimbal to move.

[0063] Firstly, based on the point cloud data and the angle change data, the gimbal angle of each spatial point in the point cloud data at the sampling moment is determined, and according to the predetermined position calibration parameters between the laser radar and the gimbal, the gimbal angle corresponding to each spatial point at the sampling moment is used to convert each spatial point into the first coordinate system, and rotation compensation is performed on the point cloud data collected by the laser radar; secondly, the target rotation offset matrix is ​​determined based on the positioning data, and based on the target rotation offset matrix, position compensation is performed on each spatial point in the first coordinate system to obtain the position coordinates of each spatial point in the second coordinate system, and motion compensation is performed on the point cloud data collected by the laser radar. Compared with the prior art, motion compensation of the point cloud data collected by the laser radar is realized in the scenario where the laser radar rotates with the gimbal and the gimbal moves with the mobile vehicle, that is, in the scenario where the laser radar moves violently.

[0064] The preferred embodiments of the present application are described below in conjunction with the drawings in the specification. It should be understood that the preferred embodiments described herein are only used to illustrate and explain the present application, and are not used to limit the present application. In addition, the embodiments and features in the embodiments of the present application may be combined with each other if there is no conflict.

[0065] like Figure 1 As shown, it is a schematic diagram of an application scenario of an embodiment of the present application. The application scenario diagram includes a laser radar 10, a gimbal 11 and a mobile vehicle (not shown in the figure). Among them, the rotating part 110 of the gimbal 11 is connected to the laser radar 10, and the gimbal 11 is loaded on the mobile vehicle, and the non-rotating part 120 of the gimbal 11 is connected to a positioning device.

[0066] In an embodiment of the present application, a mobile vehicle is equipped with a gimbal 11 to perform spatial rotation and translation in the acquisition scene. The gimbal 11 drives the laser radar 10 to rotate with high-precision angular resolution (for example, 1°). By controlling the gimbal 11 to rotate horizontally 360°, the laser radar 10 is controlled to obtain a three-dimensional point cloud of the acquisition scene.

[0067] It should be noted that the motion compensation method of the laser radar in the embodiment of the present application can be executed by the processor of the laser radar 10 itself, or by a controller or processor that is communicatively connected to the laser radar 10, the gimbal 11, and the positioning device. The embodiment of the present application does not limit this.

[0068] Taking the execution of a controller or processor connected to the laser radar 10 for example, the point cloud data of the current frame of the laser radar, the positioning data corresponding to the current frame of the positioning device, and the angle change data of the gimbal within the target time period are obtained; based on the point cloud data and the angle change data, the gimbal angle at the sampling moment of each spatial point in the point cloud data is determined, and according to the predetermined position calibration parameters between the laser radar and the gimbal, each spatial point is converted to the first coordinate system using the gimbal angle corresponding to the sampling moment of the gimbal, and then a target rotation offset matrix for characterizing the posture change of the gimbal relative to the initial position of the gimbal is determined based on the positioning data, and based on the target rotation offset matrix, position compensation is performed on each spatial point in the first coordinate system to obtain the position coordinates of each spatial point in the second coordinate system, and the position coordinates of the point cloud data of the current frame in the second coordinate system are obtained, that is, the compensated point cloud data is obtained to achieve motion compensation for the point cloud data of the current frame.

[0069] The following describes the motion compensation method of the laser radar provided by the exemplary embodiment of the present application in combination with the application scenario described above and with reference to the accompanying drawings. It should be noted that the above application scenario is only shown to facilitate the understanding of the spirit and principles of the present application, and the implementation of the present application is not limited in this regard.

[0070] It should be noted that before the implementation of the embodiments of the present application, the laser radar, gimbal and positioning device need to be synchronized in time and space, and the synchronization of time and space of the laser radar, gimbal and positioning device can be performed offline.

[0071] Taking IMU and GNSS as an example of the positioning device, the synchronization process specifically includes the following four parts: (1) Calibrate the laser radar and the rotation center of the gimbal through the rotating laser radar external parameter calibration method; (2) Use the laser radar and IMU calibration technology to calibrate the external parameters, and the embodiment of the present application does not limit this method; (3) Use the GNSS and laser radar calibration technology to calibrate the external parameters, and the embodiment of the present application does not limit this method; (4) Synchronize the time of the gimbal, GNSS, IMU and laser radar, for example, by triggering the clock systems of the gimbal, GNSS, IMU and laser radar at the same time.

[0072] Among them, when calibrating the rotation center of the laser radar and the gimbal, first, control the gimbal to drive the multi-line laser radar to rotate within 360° at a resolution of 1°, collect and save point cloud data, and divide the point cloud into two parts, the upper and lower semicircles, with 180 degrees as the dividing point (divided into the first point cloud set and the second point cloud set, where the upper semicircle point cloud is the first point cloud set and the lower semicircle point cloud is the second point cloud set). The rotation property is used (the rotation center of the gimbal coincides with the rotation center of the laser radar, and the front and rear semicircle models are consistent and have no offset) to convert the external parameter calibration of the laser radar 3D reconstruction system into a point cloud registration algorithm; secondly, establish a KD tree index on the upper semicircle point cloud, process the upper semicircle point cloud, and cluster the neighborhood range of each feature point to realize dynamic Neighborhood selection, and calculation of the covariance of each feature point in the neighborhood range, the covariance is used to estimate the plane normal vector in the neighborhood of the point cloud clustering plane, and the eigenvalues ​​of the covariance matrix are used to evaluate the flatness of the plane normal vector in the neighborhood of the point cloud; finally, the corresponding points of the front and back semicircles are found through the KD tree index, and based on the front and back corresponding points, the difference between the distances of the two points and the unit normal vector of the plane in the corresponding neighborhood is used as the error, and a quasi-registration nonlinear least squares loss function is constructed by weighting the plane quality degree in the neighborhood of the corresponding neighborhood. The rotation and translation matrix, that is, the extrinsic parameter matrix of the rotational lidar reconstruction system, is obtained by optimization. This method can solve the extrinsic parameter calibration of the lidar 3D reconstruction system without calibration objects and realize the reconstruction of large scenes.

[0073] After synchronizing the laser radar, gimbal and positioning device in time and space, the laser radar can be used to collect point cloud data in the scene, and the collected point cloud data can be motion compensated. Then, three-dimensional reconstruction is performed based on the motion compensated point cloud data. The motion compensation method of the laser radar provided in the embodiment of the present application is described in detail below in conjunction with the accompanying drawings.

[0074] See also Figure 2 As shown, it is a flow chart of an implementation of a motion compensation method of a laser radar in an embodiment of the present application. The specific implementation process of the method is as follows S201-S204:

[0075] S201: Acquire the point cloud data of the current frame of the laser radar, the positioning data corresponding to the current frame of the positioning device, and the angle change data of the gimbal within the target time period.

[0076] In specific implementation, the point cloud data of the current frame of the laser radar and the positioning data corresponding to the current frame of the positioning device can be directly obtained from the laser radar sensor and the positioning device respectively, and the angle change data of the pan-tilt can be obtained from the pan-tilt angle encoder, which is not limited in the embodiment of the present application. Among them, the target time period specifically refers to the time period between the sampling time of the current frame and the sampling time of the previous frame.

[0077] It should be noted that the positioning device in the embodiment of the present application is used to obtain the positioning data (including posture data) of the lidar. The positioning device can be an IMU and GNSS, or a real-time differential (Real-time kinematic) positioning device. Of course, it can also be other positioning devices, which are not specifically limited here.

[0078] S202: Based on the point cloud data and the angle change data, determine the gimbal angle of each spatial point in the point cloud data at the sampling moment, and according to the predetermined position calibration parameters between the laser radar and the gimbal, use the gimbal angle corresponding to each spatial point sampling moment to convert each spatial point into a first coordinate system, where the first coordinate system is a coordinate system established with the current position of the gimbal as the origin.

[0079] In specific implementation, when determining the pan / tilt angle corresponding to the sampling moment of each spatial point in the point cloud data based on the point cloud data and the angle change data, firstly, the timestamp information of the point cloud data is used as the acquisition moment of the last spatial point in the point cloud data, and the radar sampling moment corresponding to each spatial point in the point cloud data is determined based on the attribute parameters of the laser radar and the sampling moment of the last spatial point. Among them, the attribute parameters of the laser radar include the time error compensation coefficient for each spatial point in each row or each spatial point in each column. Therefore, according to the time error compensation coefficient for each spatial point in each row or each spatial point in each column, and the sampling moment of the last spatial point, the radar sampling moment of each spatial point in the point cloud data can be determined.

[0080] Then, according to the granularity of the gimbal angle change, the angle change data is split to determine the gimbal angle at each gimbal sampling moment.

[0081] In practical applications, the frame frequency of the laser radar point cloud data is decomposed into the point frequency of the spatial point (≥2000HZ) and the feedback frequency of the pan-tilt horizontal angle (≤500HZ). Therefore, the point frequency of the point cloud data is greater than the feedback frequency of the pan-tilt horizontal angle. In other words, the number of radar sampling moments is greater than the number of pan-tilt sampling moments, and some radar sampling moments have no corresponding pan-tilt sampling moments. Therefore, when determining the pan-tilt angle of each spatial point sampling moment in the point cloud data, there are the following two situations:

[0082] Case 1: If the radar sampling moment corresponding to the spatial point has a corresponding gimbal sampling moment, the gimbal angle at the gimbal sampling moment is determined as the gimbal angle corresponding to the spatial point sampling moment.

[0083] Case 2: If there is no corresponding gimbal sampling moment for the radar sampling moment corresponding to the spatial point, a preset algorithm is used to calculate the gimbal angle corresponding to the spatial point sampling moment based on the gimbal angles at two gimbal sampling moments adjacent to the radar sampling moment.

[0084] The preset algorithm may be a bilinear difference algorithm, and specifically, the following formula (1) may be used to calculate the gimbal angle corresponding to the spatial point sampling moment:

[0085]

[0086] in, is the pan / tilt angle corresponding to the spatial point sampling moment, is the jth PTZ sampling moment, is the j-1th PTZ sampling time, T is the time difference between two adjacent PTZ sampling times, is the i-th radar sampling moment, and lie in and between, for The corresponding gimbal angle at the moment, for The gimbal angle corresponding to the moment.

[0087] In specific implementation, after determining the gimbal angle corresponding to each spatial point sampling moment, each spatial point can be converted to the first coordinate system using the gimbal angle corresponding to each spatial point sampling moment according to the pre-determined position calibration parameters between the laser radar and the gimbal, and rotation compensation can be performed on the point cloud data collected by the laser radar. The first coordinate system is a coordinate system established with the current position of the gimbal as the origin.

[0088] Specifically, according to the predetermined position calibration parameters between the laser radar and the gimbal, each spatial point is converted to the first coordinate system using the gimbal angle corresponding to the sampling moment of each spatial point. The following formula (2) can be used to convert each spatial point to the first coordinate system:

[0089]

[0090] in, is the coordinate position of the spatial point in the first coordinate system, is the pan / tilt angle corresponding to the spatial point sampling moment, pr R l and pr T l is a predetermined position calibration parameter between the laser radar and the gimbal. The position calibration parameter can be obtained when the laser radar and the gimbal rotation center are calibrated, that is, it is determined according to the external parameter matrix calculated when the laser radar and the gimbal rotation center are calibrated. is the sampling position of the spatial point, that is, the position of the spatial point in the coordinate system with the lidar as the origin.

[0091] S203: Determine a target rotation offset matrix for characterizing a change in the posture of the current position of the gimbal relative to the initial position of the gimbal based on the positioning data.

[0092] In the following embodiments of the present application, the positioning device includes IMU and GNSS as an example, and the positioning data includes IMU data and GNSS data. Based on the IMU data and GNSS data, a target rotation offset matrix for characterizing the posture change of the current position of the gimbal relative to the initial position of the gimbal is determined. First, the IMU data is accumulated and integrated using a pre-integration method to determine an estimated value of the posture change of the current position of the gimbal relative to the initial position of the gimbal (that is, the position when the mobile vehicle has not moved and the gimbal has not changed its posture). Based on the GNSS data, the observed value of the posture change of the current position of the gimbal relative to the initial position of the gimbal is determined. Then, the Kalman gain of the extended Kalman filter is calculated, and the Kalman gain is used as a weight to fuse the observed value and the estimated value, and the estimated value is updated. Finally, the covariance matrix of the current position is updated to obtain the optimal estimated rotation offset matrix.

[0093] The above method of determining the estimated rotation offset matrix reduces the problem of IMU data integration offset by introducing GNSS data, avoids the subsequent initial value offset being large, and avoids the iterative solution falling into the local optimum and being unable to complete the second stage motion compensation (i.e., the position compensation in S204).

[0094] After obtaining the estimated rotation offset matrix, the global point cloud data in the second coordinate system is indexed by a KD tree, the covariance matrix of the neighborhood point cloud of each spatial point in the global point cloud data is calculated, and based on the covariance matrix, the plane unit normal vector of the neighborhood point cloud of each spatial point is calculated. The global point cloud data is point cloud data that has been position compensated. The second coordinate system is a predefined coordinate system that can take any point as its origin, for example, the initial position of the gimbal as its origin. The initial position of the gimbal refers to the position when the mobile vehicle has not moved and the gimbal has not changed its posture. The second coordinate system can also be a geodetic coordinate system, a world coordinate system, or a map coordinate system, etc.

[0095] Global point cloud data refers to point cloud data in the second coordinate system, that is, point cloud data that has been position-compensated. For example, for the second frame of point cloud data, the global point cloud data is the point cloud data of the first frame that has been position-compensated, and for the third frame of point cloud data, the global point cloud data is the point cloud data of the first frame and the point cloud data of the second frame that have been position-compensated.

[0096] It should be noted that the domain point cloud of each spatial point refers to the space composed of a preset number (can be set based on empirical values, and the embodiment of the present application does not limit this, for example, 2 meters) of domain points within the threshold range of each spatial point (can be set based on empirical values, and the embodiment of the present application does not limit this, for example: 50).

[0097] When calculating the covariance matrix of the neighborhood point cloud of each spatial point in the global point cloud data, the following formula (3) can be used:

[0098]

[0099] Among them, N(x,r) represents the point cloud set within the threshold range (radius r) of the spatial point x, i Represents the coordinates of the spatial points in the point cloud set N(x,r), Represents the centroid point of the point cloud set N(x,r).

[0100] Based on the covariance matrix, the following formula (4) can be used to calculate the plane unit normal vector of the neighborhood point cloud of each spatial point:

[0101]

[0102] Among them, n T is the unit normal vector of the plane, λ2 and λ1 are the eigenvalues ​​of the covariance matrix in formula (3), and λ2≥λ1, trace(∑(x,r)) is the trace of the covariance matrix in formula (3).

[0103] After obtaining the plane unit normal vector of each spatial point domain point cloud, an objective function for calculating the distance error between the spatial point in the first coordinate system and the spatial point in the global point cloud data to the plane is constructed based on the plane unit normal vector to estimate the rotation offset matrix as the initial position compensation parameter of the objective function. The optimization algorithm is used to determine the minimum value of the objective function through multiple iterative calculations, and the position compensation parameter corresponding to the minimum value of the objective function is determined as the target rotation offset matrix.

[0104] Specifically, the objective function is as follows:

[0105]

[0106] in, is the spatial point in the first coordinate system, that is, the spatial point to be compensated in the point cloud data of the current frame. is the i-th spatial point in the global point cloud data, w R ptz and w T ptz is the position compensation parameter, and the initial value of the position compensation parameter is determined by estimating the rotation offset matrix. is the plane unit normal vector of the i-th spatial point in the global point cloud data.

[0107] In the specific implementation, for the objective function in the above formula (5), an optimization algorithm (for example, Gauss-Newton algorithm, LM algorithm or SVD algorithm, etc., which is not limited in the embodiment of the present application) is used to iterate and solve the minimum value of the objective function continuously, and the position compensation parameter corresponding to the minimum value of the objective function is determined, and the position compensation parameter is determined as the value of the target rotation offset matrix.

[0108] S204: Based on the target rotation offset matrix, position compensation is performed on each spatial point in the first coordinate system to obtain the position coordinates of each spatial point in the predefined second coordinate system, and to obtain compensated point cloud data.

[0109] Specifically, based on the target rotation offset matrix, when position compensation is performed on each spatial point in the first coordinate system, the following formula (6) can be used:

[0110]

[0111] in, is the position coordinate of the spatial point in the second coordinate system, is the position coordinate of the spatial point in the first coordinate system, calculated by the above formula (2), w R ptz and w T ptzThe value of is determined according to the target rotation offset matrix.

[0112] When the embodiment of the present application performs motion compensation on the point cloud data collected by the laser radar, the motion compensation process is decoupled and decomposed into rotation compensation of the laser radar driven by the gimbal and movement compensation of the gimbal driven by the mobile vehicle. First, the horizontal angle compensation is performed on each spatial point in the point cloud data through the bilinear interpolation algorithm, and the rotation compensation of each spatial point is performed in combination with the calibrated rotation offset matrix between the laser radar and the gimbal, so that the spatial points in each frame of the point cloud data correspond to the angle of the gimbal one by one during the continuous rotation of the gimbal, thereby solving the laser radar acquisition distortion caused by the rotation of the gimbal. Secondly, the extended Kalman filter is used to fuse the IMU data and GNSS data as the estimated rotation offset matrix, and an objective function is established to characterize the distance error between the spatial point in the point cloud data and the spatial point in the global point cloud data and the plane. Finally, the optimal rotation offset matrix, that is, the target rotation offset matrix, is obtained by iterative solution with the estimated rotation offset matrix as the initial value, and is brought into the motion compensation model to complete the motion compensation of the mobile rotating laser radar.

[0113] Therefore, when performing motion compensation on the point cloud data collected by the laser radar, the embodiment of the present application decouples the complex posture change matrix solution, simplifies the solution process, and improves the practicality of the algorithm. At the same time, an extended Kalman filter is introduced, and the IMU data and GNSS data are integrated as the initial posture (that is, the estimated rotation offset matrix), providing a better initial posture, so that the optimal value of the solution exists near the initial value, avoiding the solution of the objective function falling into the local optimal point, and improving the practicality of the algorithm.

[0114] In addition, the extended Kalman filter is used to fuse IMU data and GNSS data. When the GNSS signal is blocked and there are data anomalies, the system calculates the Kalman gain to assign low weights to the abnormal data, and the calculation results are biased towards state estimation to ensure the stable operation of the system. Similarly, when the IMU device is in a long-term operation state, the observation value will constrain the state estimation value to avoid the accumulation of drift due to the long-term operation of the IMU sensor, which makes it impossible to iteratively solve the global optimal rotation offset matrix.

[0115] In practical applications, after position compensation is performed on the point cloud data collected by the lidar, three-dimensional point cloud reconstruction can be performed based on part or all of the global point cloud data in the second coordinate system to generate a three-dimensional map.

[0116] In the embodiment of the present application, the low-beam laser radar is driven to rotate horizontally by the rotation of the gimbal to simulate the large-beam laser radar with a 360°×360° field of view. The mobile vehicle drives the laser radar to expand the perception range, and the motion compensation method of the mobile rotating laser radar is combined to achieve three-dimensional reconstruction in large scenes (radius ≥300 meters). The embodiment of the present application greatly reduces the hardware threshold of the ultra-high beam laser radar (≥128 lines) required for large scene modeling (radius ≥300 meters). By using a low beam (16 lines), the hardware requirements for ultra-high beams can be met, effectively reducing costs. At the same time, the embodiment of the present application is not limited to mechanical, MESH and repetitive scanning laser radar types, and has strong applicability.

[0117] In addition, the embodiments of the present application do not rely on feature extraction, so the motion compensation effect is better in the absence of obvious features (application scenarios such as long corridors) and has a wider applicability.

[0118] Combine the following Figure 3 The specific implementation process of the motion compensation method of the laser radar provided in the embodiment of the present application is described in detail, and the positioning device is still taken as an example of IMU and GNSS. Before the implementation of the embodiment of the present application, the laser radar, the gimbal, the IMU and the GNSS can be synchronized in time and space in advance, and then the point cloud data collected by the laser radar can be motion compensated. Specifically, Figure 3 As shown, including:

[0119] S301, obtaining the IMU data of the current frame, the GNSS data, the point cloud data of the current frame of the laser radar, and the angle change data of the gimbal in the target time period, wherein the target time period is the time period between the sampling time of the current frame and the sampling time of the previous frame.

[0120] S302, using the timestamp information of the point cloud data as the acquisition time of the last spatial point in the point cloud data, and determining the radar sampling time corresponding to each spatial point in the point cloud data.

[0121] Specifically, the radar sampling time of each spatial point in the point cloud data can be determined based on the sampling time of the last spatial point according to the time error compensation coefficient for each spatial point in each row or each spatial point in each column included in the attribute parameters of the laser radar.

[0122] S303, splitting the angle change data according to the granularity of the pan / tilt angle change, and determining the pan / tilt angle at each pan / tilt sampling moment.

[0123] S304, calculating the pan / tilt angle of each spatial point in the point cloud data at the sampling moment.

[0124] In specific implementation, since the sampling frequency of point cloud data is greater than the frequency of horizontal rotation of the gimbal, the number of radar sampling moments is greater than the number of gimbal sampling moments. When determining the gimbal angle at the sampling moment of each spatial point in the point cloud data, the following two situations may exist:

[0125] Case 1: If the radar sampling moment corresponding to the spatial point has a corresponding gimbal sampling moment, the gimbal angle at the gimbal sampling moment is determined as the gimbal angle corresponding to the spatial point sampling moment.

[0126] Case 2: If there is no corresponding gimbal sampling moment for the radar sampling moment corresponding to the spatial point, the bilinear interpolation algorithm is used to calculate the gimbal angle corresponding to the spatial point sampling moment based on the gimbal angles at two adjacent gimbal sampling moments of the radar sampling moment.

[0127] S305, according to the predetermined position calibration parameters between the laser radar and the gimbal, using the gimbal angle corresponding to the sampling moment of each spatial point, each spatial point is converted into a first coordinate system, wherein the first coordinate system is a coordinate system established with the current position of the gimbal as the origin.

[0128] S306, fusing the IMU data and GNSS data of the current frame to obtain an estimated rotation offset matrix.

[0129] Specifically, the IMU data is first accumulated and integrated using the pre-integration method to determine the estimated value of the posture change of the gimbal's current position relative to the gimbal's initial position (that is, the position when the mobile vehicle has not moved and the gimbal has not changed its posture). Based on the GNSS data, the observed value of the posture change of the gimbal's current position relative to the gimbal's initial position is determined. Then, the Kalman gain of the extended Kalman filter is calculated, and the Kalman gain is used as the weight to fuse the observed value and the estimated value, and the estimated value is updated. Finally, the covariance matrix of the current position is updated to obtain the optimal estimated rotation offset matrix.

[0130] S307, perform KD tree indexing on the global point cloud data in the second coordinate system, calculate the covariance matrix of the neighborhood point cloud of each spatial point in the global point cloud data, and calculate the plane unit normal vector of the neighborhood point cloud of each spatial point based on the covariance matrix; wherein the second coordinate system is a coordinate system established with the initial position of the gimbal as the origin, the initial position of the gimbal refers to the position when the mobile vehicle has not moved and the gimbal has not changed its posture, and the global point cloud data is the point cloud data that has been position compensated.

[0131] S308, based on the unit normal vector of the plane, construct an objective function for calculating the distance error between the spatial point in the first coordinate system and the spatial point in the global point cloud data and the plane, estimate the rotation offset matrix as the initial position compensation parameter of the objective function, and use the optimization algorithm for iterative calculation.

[0132] S309, judging whether the iteration termination condition is met, if the iteration termination condition is met (for example, the objective function converges and reaches the minimum value), executing S310, otherwise, continuing to execute S308 for iterative calculation.

[0133] S310, determining the minimum value of the objective function, and determining the position compensation parameter corresponding to the minimum value of the objective function as the target rotation offset matrix.

[0134] S311, based on the target rotation offset matrix, position compensation is performed on each spatial point in the first coordinate system to obtain the position coordinates of each spatial point in the second coordinate system, and to obtain compensated point cloud data.

[0135] Based on the same inventive concept, the present application also provides a motion compensation device for a laser radar. Figure 4 As shown, it is a schematic diagram of the structure of a motion compensation device 400 of a laser radar, which may include:

[0136] The acquisition unit 401 is used to acquire the point cloud data of the current frame of the laser radar, the positioning data corresponding to the current frame of the positioning device, and the angle change data of the pan / tilt during the target time period;

[0137] The first position compensation unit 402 is used to determine the pan-tilt angle of each spatial point in the point cloud data at the sampling moment based on the point cloud data and the angle change data, and according to the predetermined position calibration parameters between the laser radar and the pan-tilt, use the pan-tilt angle corresponding to each spatial point sampling moment to convert each spatial point into a first coordinate system, where the first coordinate system is a coordinate system established with the current position of the pan-tilt as the origin;

[0138] The processing unit 403 is used to determine, based on the positioning data, a target rotation offset matrix for characterizing a change in the posture of the current position of the gimbal relative to the initial position of the gimbal;

[0139] The second position compensation unit 404 is used to perform position compensation on each spatial point in the first coordinate system based on the target rotation offset matrix, obtain the position coordinates of each spatial point in the predefined second coordinate system, and obtain compensated point cloud data.

[0140] Optionally, the first position compensation unit 402 is specifically configured to:

[0141] The timestamp information of the point cloud data is used as the acquisition time of the last spatial point in the point cloud data, and the radar sampling time corresponding to each spatial point in the point cloud data is determined based on the attribute parameters of the laser radar and the sampling time of the last spatial point;

[0142] According to the granularity of the gimbal angle change, the angle change data is split to determine the gimbal angle at each gimbal sampling moment;

[0143] For each spatial point, perform the following operations:

[0144] If the radar sampling moment corresponding to the spatial point has a corresponding pan / tilt sampling moment, the pan / tilt angle at the pan / tilt sampling moment is determined as the pan / tilt angle corresponding to the spatial point sampling moment;

[0145] If there is no corresponding gimbal sampling moment for the radar sampling moment corresponding to the spatial point, a preset algorithm is used to calculate the gimbal angle corresponding to the spatial point sampling moment based on the gimbal angles at two adjacent gimbal sampling moments of the radar sampling moment.

[0146] Optionally, when the first position compensation unit 402 calculates the pan-tilt angle corresponding to the spatial point sampling moment using a preset algorithm, the following formula is used to calculate the pan-tilt angle corresponding to the spatial point sampling moment:

[0147]

[0148] in, is the pan / tilt angle corresponding to the spatial point sampling moment, is the jth PTZ sampling moment, is the j-1th PTZ sampling time, T is the time difference between two adjacent PTZ sampling times, is the i-th radar sampling moment, and lie in and between, for The corresponding gimbal angle at the moment, for The gimbal angle corresponding to the moment.

[0149] Optionally, the first position compensation unit 402 is specifically configured to:

[0150] Transform each spatial point into the first coordinate system using the following formula:

[0151]

[0152] in, is the position of the spatial point in the first coordinate system, is the pan / tilt angle corresponding to the spatial point sampling moment, pr R l and pr T l The position calibration parameters between the pre-determined LiDAR and the gimbal are: is the sampling position of the spatial point.

[0153] Optionally, the positioning device includes: IMU and GNSS.

[0154] Optionally, the processing unit 403 is specifically configured to:

[0155] The IMU data of the current frame of the IMU is accumulated and integrated to determine the estimated value of the posture change of the current position of the gimbal relative to the initial position of the gimbal. Based on the GNSS data of the current frame of the GNSS, the observed value of the posture change of the current position of the gimbal relative to the initial position of the gimbal is determined. The predicted value and the observed value are fused using the extended Kalman filter to obtain the estimated rotation offset matrix.

[0156] Perform KD tree indexing on the global point cloud data in the second coordinate system, calculate the covariance matrix of the neighborhood point cloud of each spatial point in the global point cloud data, and calculate the plane unit normal vector of the neighborhood point cloud of each spatial point based on the covariance matrix. The global point cloud data is the point cloud data that has been position compensated.

[0157] Based on the unit normal vector of the plane, an objective function for calculating the distance error between the spatial point in the first coordinate system and the spatial point in the global point cloud data and the plane is constructed to estimate the rotation offset matrix as the initial position compensation parameter of the objective function. The optimization algorithm is used to determine the minimum value of the objective function through multiple iterative calculations, and the position compensation parameter corresponding to the minimum value of the objective function is determined as the target rotation offset matrix.

[0158] Optionally, the objective function is:

[0159]

[0160] in, is a spatial point in the first coordinate system, is the i-th spatial point in the global point cloud data, w R ptz and w T ptz is the position compensation parameter, and the initial value of the position compensation parameter is the value of the estimated rotation offset matrix. is the plane unit normal vector of the i-th spatial point in the global point cloud data.

[0161] Optionally, the second position compensation unit 404 is specifically configured to:

[0162] Use the following formula to compensate the position of each spatial point in the first coordinate system:

[0163]

[0164] in, is the position coordinate of the spatial point in the second coordinate system, is the position coordinate of the spatial point in the first coordinate system, w R ptz and w T ptz The value of is determined according to the target rotation offset matrix.

[0165] Optionally, the processing unit 403 is further configured to:

[0166] Based on part or all of the global point cloud data in the second coordinate system, three-dimensional point cloud reconstruction is performed to generate a three-dimensional map.

[0167] Based on the same inventive concept, an embodiment of the present application provides a three-dimensional point cloud reconstruction system, the system including: a laser radar, a gimbal, a mobile vehicle, and a positioning device, wherein the laser radar is connected to the rotating part of the gimbal, the positioning device is connected to the non-rotating part of the gimbal, and the gimbal is mounted on the mobile vehicle. The laser radar collects point cloud data, and any one of the laser radar motion compensation methods provided in the embodiments of the present application is used to perform position compensation on the collected point cloud data, determine the position coordinates of the point cloud data, and reconstruct the three-dimensional point cloud based on the position coordinates of part or all of the point cloud data.

[0168] An embodiment of the present application also provides an electronic device, which includes a processor and a memory, wherein the memory stores a computer program, and when the computer program is executed by the processor, the processor executes the steps of any one of the laser radar motion compensation methods in the embodiments of the present application.

[0169] An embodiment of the present application provides a machine-readable storage medium, on which instructions are stored. When the instructions are executed by a processor, the processor is configured to execute any one of the laser radar motion compensation methods in the embodiments of the present application.

[0170] In some possible implementations, various aspects of the laser radar motion compensation method provided in the present application may also be implemented in the form of a program product, which includes a program code. When the program product is run on a computer device, the program code is used to enable the computer device to execute the steps of the laser radar motion compensation method according to various exemplary embodiments of the present application described above in this specification. For example, the computer device may execute the following steps: Figure 2 Steps shown.

[0171] The program product may use any combination of one or more readable media. The readable medium may be a readable signal medium or a readable storage medium. The readable storage medium may be, for example, but not limited to, an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor system, device or device, or any combination of the above. More specific examples of readable storage media (a non-exhaustive list) include: an electrical connection with one or more wires, a portable disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the above.

[0172] The program product of the embodiment of the present application may adopt a portable compact disk read-only memory (CD-ROM) and include program code, and can be run on a computing device. However, the program product of the present application is not limited thereto. In the embodiment of the present application, the readable storage medium may be any tangible medium containing or storing a program, which may be used by or in combination with a command execution system, device or device.

[0173] The readable signal medium may include a data signal propagated in baseband or as part of a carrier wave, wherein the readable program code is carried. Such propagated data signals may take a variety of forms, including but not limited to electromagnetic signals, optical signals, or any suitable combination of the above. The readable signal medium may also be any readable medium other than a readable storage medium, which may send, propagate, or transmit a program for use by or in conjunction with a command execution system, apparatus, or device.

[0174] The program code embodied on the readable medium may be transmitted using any appropriate medium, including but not limited to wireless, wired, optical cable, RF, etc., or any suitable combination of the foregoing.

[0175] Program code for performing the operations of the present application may be written in any combination of one or more programming languages, including object-oriented programming languages ​​such as Java, C++, etc., and conventional procedural programming languages ​​such as "C" or similar programming languages. The program code may be executed entirely on the user computing device, partially on the user equipment, as a separate software package, partially on the user computing device and partially on a remote computing device, or entirely on a remote computing device or server. In the case of a remote computing device, the remote computing device may be connected to the user computing device via any type of network including a local area network (LAN) or a wide area network (WAN), or may be connected to an external computing device (e.g., via the Internet using an Internet service provider).

[0176] In the embodiments of the present application, the term "module" or "unit" refers to a computer program or a part of a computer program with a predetermined function, and works together with other related parts to achieve a predetermined goal, and can be implemented in whole or in part by using software, hardware (such as processing circuits or memories) or a combination thereof. Similarly, a processor (or multiple processors or memories) can be used to implement one or more modules or units. In addition, each module or unit can be part of an overall module or unit that includes the function of the module or unit.

[0177] It should be noted that, although several units or subunits of the device are mentioned in the above detailed description, this division is merely exemplary and not mandatory. In fact, according to the embodiments of the present application, the features and functions of two or more units described above can be embodied in one unit. Conversely, the features and functions of one unit described above can be further divided into multiple units to be embodied.

[0178] In addition, although the operations of the method of the present application are described in a specific order in the drawings, this does not require or imply that the operations must be performed in this specific order, or that all the operations shown must be performed to achieve the desired results. Additionally or alternatively, some steps may be omitted, multiple steps may be combined into one step, and / or one step may be decomposed into multiple steps.

[0179] Those skilled in the art will appreciate that the embodiments of the present application may be provided as methods, systems, or computer program products. Therefore, the present application may adopt the form of a complete hardware embodiment, a complete software embodiment, or an embodiment in combination with software and hardware. Moreover, the present application may adopt the form of a computer program product implemented in one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) that include computer-usable program code.

[0180] The present application is described with reference to the flowcharts and / or block diagrams of the methods, devices (systems), and computer program products according to the embodiments of the present application. It should be understood that each process and / or box in the flowchart and / or block diagram, as well as the combination of the processes and / or boxes in the flowchart and / or block diagram, can be implemented by computer program commands. These computer program commands can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to generate a machine, so that the commands executed by the processor of the computer or other programmable data processing device generate commands for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0181] These computer program commands may also be stored in a computer readable memory capable of directing a computer or other programmable data processing device to operate in a specific manner, so that the commands stored in the computer readable memory produce an article of manufacture comprising a command device, the command device implementing the process Figure 1 A process or multiple processes and / or boxes Figure 1 A function specified in one or more boxes.

[0182] These computer program commands can also be loaded onto a computer or other programmable data processing device so that a series of operation steps are executed on the computer or other programmable device to produce a computer-implemented process, so that the commands executed on the computer or other programmable device provide the instructions for implementing the process. Figure 1 A process or multiple processes and / or boxes Figure 1 The steps for the functions specified in one or more boxes.

[0183] Although the preferred embodiments of the present application have been described, those skilled in the art may make additional changes and modifications to these embodiments once they have learned the basic creative concept. Therefore, the appended claims are intended to be interpreted as including the preferred embodiments and all changes and modifications falling within the scope of the present application.

[0184] Obviously, those skilled in the art can make various changes and modifications to the present application without departing from the spirit and scope of the present application. Thus, if these modifications and variations of the present application fall within the scope of the claims of the present application and their equivalents, the present application is also intended to include these modifications and variations.

Claims

1. A motion compensation method for a laser radar, applied to a laser radar connected to a rotating part of a gimbal, wherein the gimbal is mounted on a mobile vehicle, and a non-rotating part of the gimbal is connected to a positioning device, characterized in that: The method comprises: Acquire the point cloud data of the current frame of the laser radar, the positioning data corresponding to the current frame of the positioning device, and the angle change data of the pan / tilt within a target time period; Based on the point cloud data and the angle change data, determine the gimbal angle of each spatial point in the point cloud data at a sampling moment, and according to a predetermined position calibration parameter between the laser radar and the gimbal, use the gimbal angle corresponding to each spatial point sampling moment to transform each spatial point into a first coordinate system, where the first coordinate system is a coordinate system established with the current position of the gimbal as the origin; Based on the positioning data, a target rotation offset matrix is ​​determined for characterizing a change in the posture of the current position of the gimbal relative to the initial position of the gimbal; Based on the target rotation offset matrix, position compensation is performed on each spatial point in the first coordinate system to obtain the position coordinates of each spatial point in the predefined second coordinate system, thereby obtaining compensated point cloud data.

2. The method according to claim 1, characterized in that The determining, based on the point cloud data and the angle change data, a pan / tilt angle corresponding to a sampling moment of each spatial point in the point cloud data comprises: Using the timestamp information of the point cloud data as the acquisition time of the last spatial point in the point cloud data, and determining the radar sampling time corresponding to each spatial point in the point cloud data based on the sampling time of the last spatial point according to the attribute parameters of the laser radar; According to the granularity of the pan / tilt angle change, the angle change data is split to determine the pan / tilt angle at each pan / tilt sampling moment; For each spatial point, perform the following operations: If the radar sampling moment corresponding to the spatial point has a corresponding pan / tilt sampling moment, the pan / tilt angle at the pan / tilt sampling moment is determined as the pan / tilt angle corresponding to the spatial point sampling moment; If there is no corresponding gimbal sampling moment for the radar sampling moment corresponding to the spatial point, a preset algorithm is used to calculate the gimbal angle corresponding to the spatial point sampling moment based on the gimbal angles at two adjacent gimbal sampling moments of the radar sampling moment.

3. The method according to claim 2, characterized in that The preset algorithm uses the following formula to calculate the gimbal angle corresponding to the spatial point sampling moment: in, is the pan / tilt angle corresponding to the spatial point sampling moment, is the jth PTZ sampling moment, is the j-1th PTZ sampling time, T is the time difference between two adjacent PTZ sampling times, is the i-th radar sampling moment, and lie in and between, for The gimbal angle corresponding to the moment, for The gimbal angle corresponding to the moment.

4. The method according to claim 3, characterized in that The method of converting each spatial point into a first coordinate system according to a predetermined position calibration parameter between the laser radar and the gimbal and using a gimbal angle corresponding to a sampling moment of each spatial point includes: Transform each spatial point into the first coordinate system using the following formula: in, is the position of the spatial point in the first coordinate system, is the pan / tilt angle corresponding to the spatial point sampling moment, pr R l and pr T l is a predetermined position calibration parameter between the laser radar and the gimbal, is the sampling position of the spatial point.

5. The method according to any one of claims 1 to 4, characterized in that The positioning device includes: an inertial measurement unit IMU and a global navigation satellite system GNSS.

6. The method according to claim 5, characterized in that The step of determining, based on the positioning data, a target rotation offset matrix for characterizing a change in the posture of the current position of the gimbal relative to the initial position of the gimbal comprises: Accumulating and integrating the IMU data of the current frame of the IMU to determine an estimated value of the posture change of the current position of the gimbal relative to the initial position of the gimbal, determining an observed value of the posture change of the current position of the gimbal relative to the initial position of the gimbal based on the GNSS data of the current frame of the GNSS, and fusing the estimated value and the observed value using an extended Kalman filter to obtain an estimated rotation offset matrix; Performing KD tree indexing on the global point cloud data in the second coordinate system, calculating the covariance matrix of the neighborhood point cloud of each spatial point in the global point cloud data, and calculating the plane unit normal vector of the neighborhood point cloud of each spatial point based on the covariance matrix, wherein the global point cloud data is point cloud data that has been position compensated; Based on the plane unit normal vector, an objective function for calculating the distance error between the spatial point in the first coordinate system and the spatial point in the global point cloud data and the plane is constructed, and the estimated rotation offset matrix is ​​used as the initial position compensation parameter of the objective function. An optimization algorithm is used to determine the minimum value of the objective function through multiple iterative calculations, and the position compensation parameter corresponding to the minimum value of the objective function is determined as the target rotation offset matrix.

7. The method according to claim 6, characterized in that The objective function is: in, is a spatial point in the first coordinate system, is the i-th spatial point in the global point cloud data, w R ptz and w T ptz is a position compensation parameter, the initial value of which is the value of the estimated rotation offset matrix, is the plane unit normal vector of the ith spatial point in the global point cloud data.

8. The method according to any one of claims 1 to 4, characterized in that The step of performing position compensation on each spatial point in the first coordinate system based on the target rotation offset matrix to obtain the position coordinates of each spatial point in a predefined second coordinate system includes: Use the following formula to perform position compensation on each spatial point in the first coordinate system: in, is the position coordinate of the spatial point in the second coordinate system, is the position coordinate of the spatial point in the first coordinate system, w R ptz and w T ptz The value of is determined according to the target rotation offset matrix.

9. The method according to any one of claims 1 to 4, characterized in that: The method further comprises: Based on part or all of the global point cloud data in the second coordinate system, three-dimensional point cloud reconstruction is performed to generate a three-dimensional map.

10. A motion compensation device for a laser radar, applied to a laser radar connected to a rotating part of a gimbal, wherein the gimbal is mounted on a mobile vehicle, and a non-rotating part of the gimbal is connected to a positioning device, characterized in that: include: An acquisition unit, used to acquire point cloud data of the current frame of the laser radar, positioning data corresponding to the current frame of the positioning device, and angle change data of the pan / tilt within a target time period; a first position compensation unit, configured to determine the pan-tilt angle of each spatial point in the point cloud data at a sampling moment based on the point cloud data and the angle change data, and to convert each spatial point into a first coordinate system using the pan-tilt angle corresponding to each spatial point sampling moment according to a predetermined position calibration parameter between the laser radar and the pan-tilt, wherein the first coordinate system is a coordinate system established with the current position of the pan-tilt as an origin; A processing unit, configured to determine, based on the positioning data, a target rotation offset matrix for characterizing a change in the posture of the current position of the gimbal relative to the initial position of the gimbal; The second position compensation unit is used to perform position compensation on each spatial point in the first coordinate system based on the target rotation offset matrix, obtain the position coordinates of each spatial point in the predefined second coordinate system, and obtain compensated point cloud data.

11. A three-dimensional point cloud reconstruction system, characterized in that: The system includes: a laser radar, a gimbal, a mobile vehicle, and a positioning device, wherein the laser radar is connected to the rotating part of the gimbal, the positioning device is connected to the non-rotating part of the gimbal, and the gimbal is mounted on a mobile vehicle. The laser radar collects point cloud data, and adopts the method described in any one of claims 1 to 9 to perform position compensation on the collected point cloud data, determine the position coordinates of the point cloud data, and reconstruct the three-dimensional point cloud based on the position coordinates of part or all of the point cloud data.

12. An electronic device, characterized in that: It comprises a processor and a memory, wherein the memory stores a computer program, and when the computer program is executed by the processor, the processor executes the steps of any one of the methods of claims 1 to 9.

13. A machine-readable storage medium having instructions stored thereon, characterized in that: When the instruction is executed by a processor, the processor is configured to perform the method according to any one of claims 1-9.

Images