Laser point cloud map construction method and device, equipment and storage medium

Abstract

The application provides a laser point cloud map construction method, device and equipment and a storage medium. The method comprises the following steps: acquiring first laser point cloud feature data and pose data of a target device, wherein the first laser point cloud feature data is laser point cloud feature data in a laser radar coordinate system; performing de-distortion on the first laser point cloud feature data according to the pose data to obtain first laser point cloud correction data corresponding to the first laser point cloud feature data; performing device pose estimation according to the first laser point cloud correction data and the pose data to obtain a pose matrix of the target device, wherein the pose matrix is used to indicate a conversion relationship between a world coordinate system and the laser radar coordinate system; performing coordinate conversion on the first laser point cloud correction data according to the pose matrix to obtain second laser point cloud feature data, and adding the second laser point cloud feature data to a dense laser point cloud map of the target device. The technical solution can improve the accuracy of the laser point cloud map obtained by construction.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of laser radar mapping, and particularly to a laser point cloud map construction method and device, equipment and a storage medium. BACKGROUND

[0002] A laser radar measures the distance and orientation of surrounding environment objects by emitting laser beams, so as to determine the relative position between a device (such as a sweeping robot, a vehicle, etc.) provided with the laser radar and an obstacle. When the laser beams emitted by the laser radar are sufficient, the laser points emitted by the laser radar gather to form a point cloud, which is used to outline the three-dimensional environment information in which the device provided with the laser radar is located.

[0003] The laser point cloud is used to construct a map. In one mapping scheme, the mapping relationship between the laser radar coordinate system and the world coordinate system is calibrated in advance, and then after the laser point cloud data in the laser radar coordinate system is obtained, the coordinates of the laser point cloud are converted from the radar coordinate system to the world coordinate system according to the pre-calibrated mapping relationship, and then added to the map, so as to complete the map construction. This mapping method is only applicable to the case where the device is stationary. When the position and attitude of the device change, the mapping relationship between the laser radar coordinate system and the world coordinate system changes, and the map constructed by converting the coordinates of the laser point cloud from the radar coordinate system to the world coordinate system according to the pre-calibrated mapping relationship is not accurate enough. SUMMARY

[0004] The present application provides a laser point cloud map construction method, device, equipment and storage medium to solve the technical problem of inaccurate map construction.

[0005] In a first aspect, a laser point cloud map construction method is provided, comprising:

[0006] obtaining first laser point cloud feature data and pose data of a target device, wherein the target device is provided with a laser radar and an inertial sensor, and the first laser point cloud feature data is laser point cloud feature data in a laser radar coordinate system;

[0007] de-distorting the first laser point cloud feature data according to the pose data to obtain first laser point cloud correction data corresponding to the first laser point cloud feature data;

[0008] performing device pose estimation according to the first laser point cloud correction data and the pose data to obtain a pose matrix of the target device, the pose matrix being used to indicate the conversion relationship between the world coordinate system and the laser radar coordinate system;

[0009] According to the pose matrix, the first laser point cloud correction data is converted to the world coordinate system to obtain second laser point cloud feature data, and the second laser point cloud feature data is added to the dense laser point cloud map of the target device.

[0010] In the technical solution, the first laser point cloud feature data and the pose data of the target device with the laser radar and the inertial sensor are acquired, the first laser point cloud feature data is de-warped according to the pose data to obtain the first laser point cloud correction data; the device pose is estimated according to the first laser point cloud correction data and the pose data to obtain the pose matrix of the target device; finally, the first laser point cloud correction data is converted to the world coordinate system according to the pose matrix, and the second laser point cloud feature data obtained by the conversion is added to the dense laser point cloud map of the target device, thereby completing the creation of the dense laser point cloud map. Since the first laser point cloud correction data is obtained by de-warping the first laser point cloud feature data, the first laser point cloud correction data can accurately reflect the environment around the target device when the first laser point of the laser radar of the target device is emitted. The pose matrix is obtained by pose estimation based on the first laser point cloud correction data and the pose data, and the pose matrix can accurately reflect the conversion relationship between the world coordinate system and the laser radar coordinate system when the first laser point of the laser radar of the target device is emitted. The world coordinates of the laser point cloud feature data converted based on the pose matrix are more accurate, and thus the accuracy of the dense laser point cloud map constructed can be improved.

[0011] In combination with the first aspect, in a possible implementation manner, the first laser point cloud feature data of the target device is acquired by acquiring a plurality of laser point cloud data of the target device, and calculating the curvature of each laser point cloud data in the plurality of laser point cloud data, the plurality of laser point cloud data being laser point cloud data generated by scanning a circle by the laser radar; in the plurality of laser point cloud data, the laser point cloud data with a curvature greater than a first curvature threshold or a curvature less than a second curvature threshold is determined as the first laser point cloud feature data, the first curvature threshold being greater than the second curvature threshold. By calculating the curvature of the laser point cloud data in the same scanning period to determine the laser point cloud feature and determining the laser point cloud data with a curvature greater than the first curvature threshold and a curvature less than the second curvature threshold as the laser point cloud feature data, the laser point cloud feature data can be comprehensive and effective, and thus the dense laser point cloud map constructed is sufficiently dense.

[0012] With reference to the first aspect, in a possible implementation manner, the de-distortion of the first laser point cloud feature data according to the pose data to obtain first laser point cloud correction data corresponding to the first laser point cloud feature data comprises: determining the pose data at a first scanning time and the pose data at a second scanning time, the first scanning time being a scanning time of first laser point cloud data corresponding to the first laser point cloud feature data, the first laser point cloud data and the first laser point cloud feature data belonging to a same scanning period, and the second scanning time being a scanning time of the first laser point cloud feature data; and determining a correction position of the first laser point cloud feature data at the first scanning time according to a pose increment between the pose data at the second scanning time and the pose data at the first scanning time, to obtain the first laser point cloud correction data. By correcting the laser point cloud feature data according to the pose increment between two scanning times in the same scanning period, the de-distortion of the laser point cloud feature data can be realized, it is ensured that the laser point cloud feature data belongs to the same scanning period and reflects the environment at the starting scanning time in the scanning period, and thus the accurate restoration of the three-dimensional environment can be realized.

[0013] With reference to the first aspect, in a possible implementation manner, the determining of the correction position of the first laser point cloud feature data at the first scanning time according to the pose increment between the pose data at the second scanning time and the pose data at the first scanning time to obtain the first laser point cloud correction data comprises: determining a rotation matrix according to the pose increment, the rotation matrix being used to indicate a pose rotation relationship between the second scanning time and the first scanning time; and determining the correction position of the first laser point cloud feature data at the first scanning time according to position data corresponding to the first laser point cloud feature data and the rotation matrix to obtain the first laser point cloud correction data, wherein the position data corresponding to the first laser point cloud feature data is obtained based on the pose data at the second scanning time. By restoring the position of the laser point cloud feature data based on the rotation matrix, the implementation manner is simple.

[0014] With reference to the first aspect, in a possible implementation manner, the device pose estimation according to the first laser point cloud correction data and the pose data to obtain a pose matrix of the target device comprises: downsampling the first laser point cloud correction data to obtain first laser point cloud sparse data corresponding to the first laser point cloud correction data; and performing filter updating according to the first laser point cloud sparse data and the pose data to obtain the pose matrix of the target device. By downsampling the corrected laser point cloud feature data and performing filter updating in combination with the pose data to obtain the pose matrix of the target device, the real-time updating of the pose matrix can be realized, and thus it is ensured that the constructed map is accurate.

[0015] With reference to the first aspect, in a possible implementation manner, after the filtering update is performed according to the first laser point cloud sparse data and the pose data to obtain the pose matrix of the target device, the method further includes: converting the first laser point cloud sparse data to the world coordinate system according to the pose matrix to obtain second laser point cloud sparse data, and adding the second laser point cloud sparse data to the sparse laser point cloud map of the target device. After the laser point cloud sparse data is converted based on the pose matrix, the converted laser point cloud sparse data is added to the laser point cloud sparse map, which can improve the accuracy of the laser point cloud sparse map constructed.

[0016] With reference to the first aspect, in a possible implementation manner, the adding the second laser point cloud feature data to the dense laser point cloud map of the target device includes: obtaining a first device position and a second device position, the first device position being a current position of the target device, and the second device position being a position of the target device when the laser point cloud feature data is last added to the dense laser point cloud map; and in a case where a distance between the first device position and the second device position is greater than a preset distance threshold, adding the second laser point cloud feature data to the dense laser point cloud map of the target device. In a case where the distance between the current position of the device and the position when the laser point cloud feature data is last added is greater than the preset distance threshold, the laser point cloud feature data is added to the dense laser point cloud map, which can improve the generation efficiency of the dense laser point cloud map.

[0017] The second aspect provides a laser point cloud map construction device, including:

[0018] The acquisition module is configured to acquire first laser point cloud feature data and pose data of a target device, wherein the target device has a laser radar and an inertial sensor, and the first laser point cloud feature data is laser point cloud feature data in a laser radar coordinate system.

[0019] The de-distortion module is configured to perform de-distortion on the first laser point cloud feature data according to the pose data to obtain first laser point cloud correction data corresponding to the first laser point cloud feature data.

[0020] The pose estimation module is configured to perform device pose estimation according to the first laser point cloud correction data and the pose data to obtain a pose matrix of the target device, the pose matrix being used to indicate a conversion relationship between a world coordinate system and the laser radar coordinate system.

[0021] a map construction module, configured to convert the first laser point cloud correction data to the world coordinate system according to the pose matrix to obtain second laser point cloud feature data, and add the second laser point cloud feature data to the dense laser point cloud map of the target device.

[0022] In a third aspect, a computer device is provided, including a memory and one or more processors, the memory being connected to the one or more processors, and the one or more processors being configured to execute one or more computer programs stored in the memory, and the one or more processors, when executing the one or more computer programs, cause the computer device to implement the laser point cloud map construction method of the first aspect.

[0023] In a fourth aspect, a computer readable storage medium is provided, and the computer readable storage medium stores a computer program, the computer program including program instructions, and the program instructions, when executed by a processor, cause the processor to execute the laser point cloud map construction method of the first aspect.

[0024] The present application can achieve the following technical effects: since the first laser point cloud correction data is obtained by de-distorting the first laser point cloud feature data, the first laser point cloud correction data can accurately reflect the environment around the target device when the laser radar of the target device emits the first laser point, the pose matrix is obtained based on the pose estimation of the first laser point cloud correction data and the pose data, and the pose matrix can accurately reflect the conversion relationship between the world coordinate system and the laser radar coordinate system when the laser radar of the target device emits the first laser point, so that the world coordinates of the laser point cloud feature data converted based on the pose matrix are more accurate, and thus the accuracy of the dense laser point cloud map constructed can be improved. BRIEF DESCRIPTION OF DRAWINGS

[0025] Figure 1 FIG. 1 is a flowchart of a laser point cloud map construction method provided by an embodiment of the present application;

[0026] Figure 2 FIG. 2 is a distortion diagram of laser point cloud data provided by an embodiment of the present application;

[0027] Figure 3 FIG. 4 is a flowchart of another laser point cloud map construction method provided by an embodiment of the present application;

[0028] Figure 4 FIG. 5 is a structural diagram of a laser point cloud map construction device provided by an embodiment of the present application;

[0029] Figure 5 FIG. 6 is a structural diagram of a computer device provided by an embodiment of the present application. DETAILED DESCRIPTION

[0030] The technical solutions in the embodiments of the present application will be described below with reference to the drawings in the embodiments of the present application.

[0031] The technical solutions of the present application are applicable to a scenario of map construction based on a laser radar. In the scenario of map construction based on a laser radar, a target device provided with a laser radar can emit laser pulses outward through a transmitting system of the laser radar, receive laser pulses reflected from external targets (such as walls, obstacles, etc.) through a receiving system of the laser radar, and then process the laser pulses through a signal processing system of the laser radar to form laser point cloud data about the external targets. The laser point cloud data has spatial coordinate information for representing the spatial coordinates of a point of the external targets. After data processing (such as filtering and feature extraction) of the laser point cloud data by the target device, the laser point cloud data can be put into a map to form a map about the external environment.

[0032] The technical solutions of the present application can be specifically applied to a target device provided with a laser radar, which can be, for example, a delivery robot, a transportation robot, a sweeping robot, etc., and is not limited to the examples herein. Alternatively, the technical solutions of the present application can also be applied to other devices having a connection relationship or a matching relationship with the target device. The connection relationship between the target device and the other device can be a wired connection relationship or a wireless connection relationship. For example, the target device is a sweeping robot, and the other device can be a base station matched with the sweeping robot, or the other device can also be a background server (such as a cloud server) corresponding to the sweeping robot, and the background server and the sweeping robot transmit data based on wireless communication.

[0033] The technical principle of the present application is generally as follows: after obtaining laser point cloud feature data and pose data, first, the laser point cloud feature data is de-distorted according to the pose data to restore laser point cloud feature data for representing the environment around the target device when the first laser pulse in a laser scanning period is emitted by the target device, i.e., laser point cloud correction data; second, device pose estimation is performed based on the laser point cloud correction data and the pose data to obtain a pose matrix reflecting the conversion relationship between the world coordinate system and the laser radar coordinate system when the first laser pulse in the laser scanning period is emitted by the target device; third, the laser point cloud feature data is converted in coordinates according to the pose matrix to obtain laser point cloud feature data in the world coordinates, so that the laser point cloud feature data in the world coordinates reflects the position of the surrounding object when the first laser pulse in the laser scanning period is emitted by the target device, and the laser point cloud feature data in the world coordinates is added to the laser point cloud map, so that the laser point cloud map can accurately reflect the external environment of the target device.

[0034] The technical solutions of the present application will be described below.

[0035] Reference is made toFigure 1 , Figure 1 A flowchart of a laser point cloud map construction method provided by an embodiment of the present application is shown in FIG. 1. The method can be applied to the target device mentioned above or other devices having a connection relationship or a matching relationship with the target device. As shown in FIG. 1, the method comprises the following steps: Figure 1

[0036] S101, acquiring first laser point cloud feature data and pose data of the target device.

[0037] The target device is a device having a laser radar and an inertial sensor. The laser radar in the target device can generate laser point cloud data for reflecting a three-dimensional environment around the target device; and the inertial sensor in the target device can generate IMU data for reflecting a pose change of the laser radar, the IMU data including acceleration and angular velocity.

[0038] The first laser point cloud feature data is laser point cloud feature data in a laser radar coordinate system. The laser radar coordinate system refers to a coordinate system with a geometric center of the laser radar as a coordinate origin. The laser point cloud feature data refers to representative laser point cloud data generated by the laser radar, which can be laser point cloud data capable of reflecting environmental features; wherein the laser point cloud feature data is obtained by feature extraction from the laser point cloud data generated by the laser radar.

[0039] In some possible implementation scenarios, laser point cloud data for expressing lines and planes in the laser point cloud data can be used as laser point cloud features. The first laser point cloud feature can be obtained by the following steps A1-A2.

[0040] A1, acquiring a plurality of laser point cloud data of the target device, and calculating a curvature of each laser point cloud data in the plurality of laser point cloud data.

[0041] The plurality of laser point cloud data of the target device is laser point cloud data generated by the laser radar of the target device in one scanning cycle, and the time of one scanning cycle of the laser radar of the target device is referred to as a scanning period. The plurality of laser point cloud data of the target device belongs to the same scanning period. In some possible cases, the plurality of laser point cloud data can be the qth frame of laser point cloud data generated by the laser radar of the target device after starting scanning, and q is greater than 1.

[0042] Specifically, the curvature of each laser point cloud data can be calculated by the following formula 1.

[0043]

[0044] ​Wherein, (x, y, z) is the coordinate of the target laser point cloud data in the laser radar coordinate system, and the target laser point cloud data is any one of the plurality of laser point cloud data of the target device; (x i , y i , z i ) refers to the coordinate of the laser point cloud data closest to the index of the target laser point cloud data in the laser radar coordinate system, taking the target laser point cloud data as the index. For example, the laser radar of the target device scans a circle to generate a total of 200 laser point cloud data, and the target laser point cloud data is the 7th laser point cloud data generated by scanning. The laser point cloud data closest to the index of the target laser point cloud data is the 2nd laser point cloud data, the 3rd laser point cloud data,..., and the 15th laser point cloud data generated by scanning.

[0045] A2, among the plurality of laser point cloud data of the target device, the laser point cloud data with a curvature greater than a first curvature threshold or a curvature less than a second curvature threshold is determined as the first laser point cloud feature data.

[0046] Wherein, the first curvature threshold is greater than the second curvature threshold. The laser point cloud data with a curvature greater than the first curvature threshold is a line feature, which is used to express a line in the external environment, and the laser point cloud data with a curvature less than the second curvature threshold is a surface feature, which is used to express a plane in the external environment.

[0047] By determining the laser point cloud data with a curvature greater than the first curvature threshold or a curvature less than the second curvature threshold as the laser point cloud feature data, the laser point cloud feature data can be comprehensive and effective, the external environment can be fully expressed, and the dense laser point cloud map constructed is sufficiently dense.

[0048] Optionally, the laser point cloud data used to express other features can also be used as laser point cloud features, for example, the laser point cloud data expressing corner, arc and other features can also be used as laser point cloud feature data from the plurality of laser point cloud data of the target device. The present application does not make any limitation.

[0049] The pose data refers to data used to represent the pose of the laser radar, wherein the pose data is obtained by pose calculation of the IMU data.

[0050] Specifically, the pose data of the target device can be calculated by the following formula 2-formula 6:

[0051] P j =P i +V i *Δt+0.5*a i *Δt 2 Formula 2

[0052] V j =Vi +a i *Δt Equation 3

[0053] roll j = roll i + ωx i *Δt Equation 4

[0054] pitch j = pitch i + ωy i *Δt Equation 5

[0055] yaw j = yaw i + ωz i *Δt Equation 6

[0056] where i and j represent two adjacent time instants respectively, P represents position, V represents velocity, a represents acceleration, roll, pitch, and yaw represent Euler angles respectively, ωx, ωy, and ωz represent angular velocities on three axes, and Δt represents the time difference between the two adjacent time instants.

[0057] After the IMU data is acquired, the pose data can be obtained by decomposing the IMU data according to the above Equation 2-Equation 6, for representing the position and Euler angles of the lidar.

[0058] In S102, the first laser point cloud feature data of the target device is de-distorted according to the pose data of the target device, to obtain first laser point cloud correction data corresponding to the first laser point cloud feature data.

[0059] Here, de-distorting the first laser point cloud feature data of the target device according to the pose data of the target device means performing position recovery correction on the first laser point cloud feature data, to offset the position change of the laser point cloud data caused by the movement of the target device, so as to obtain the laser point cloud data that should be generated by the lidar scanning before the target device moves. The distortion phenomenon of the laser point cloud data can be referred to Figure 2 When the target device remains stationary, the multiple laser point cloud data generated by the lidar of the target device scanning a circle can be as shown in Figure 3As shown in A, a plurality of laser point cloud data is formed as a ring-shaped scanning line with the geometric center O of the lidar as the center, and the positions of the laser point cloud data at the start scanning time and the end scanning time coincide at position P. In this case, the plurality of laser point cloud data generated is with the geometric center O of the lidar as the coordinate origin in the lidar coordinate system. In the case of target device motion, the geometric center of the lidar moves from O1 to O2 in the time of one scanning cycle of the lidar, and the laser point cloud data generated by the lidar of the target device in the same scanning period is not with the same geometric center as the center. The laser point cloud data that should end at position P1 ends at position P2. In this case, the plurality of laser point cloud data generated is with different positions as the coordinate origin in the lidar coordinate system.

[0060] Specifically, the first laser point cloud feature data of the target device can be de-distorted through the following steps B1-B2.

[0061] B1, determine the pose data at the first scanning time and the pose data at the second scanning time.

[0062] The scanning time refers to the time when the lidar of the target device emits a laser pulse outward and receives the laser pulse reflected from the outside. The first scanning time is the scanning time of the first laser point cloud data corresponding to the first laser point cloud feature data, which can be understood as the time when the lidar of the target device obtains the first laser point cloud data in one scanning period. The first laser point cloud data corresponding to the first laser point cloud feature data belongs to the same scanning period as the first laser point cloud feature data. The second scanning time is the scanning time of the first laser point cloud feature data, which can be understood as the time when the lidar scans and obtains the first laser point cloud feature data when the target device is stationary at the spatial position corresponding to the first scanning time.

[0063] The first scanning time can be obtained according to the time when the lidar of the target device generates the first laser point cloud data. The second scanning time can be calculated based on the first scanning time and the scanning period of the lidar of the target device. The second scanning time can be calculated through the following formula 7:

[0064] tk=t0+T*(k+1) / n formula 7

[0065] Wherein, tk is the second scanning time, t0 is the first scanning time, T is the scanning period of the lidar of the target device, k represents the acquisition order of the first laser point cloud feature data in the laser scanning period, and n represents the number of laser point cloud data obtained by the lidar in one rotation scanning, which is related to the scanning frequency of the lidar.

[0066] B2, determine a corrected position of the first laser point cloud feature data at the first scanning time according to the pose increment between the pose data of the second scanning time and the pose data of the first scanning time, to obtain first laser point cloud corrected data.

[0067] Wherein, the pose increment between the pose data of the second scanning time and the pose data of the first scanning time includes a position increment and an angle increment between the second scanning time and the first scanning time, the position increment is used to reflect the position difference of the laser radar of the target device between the second scanning time and the first scanning time, and the angle increment is used to reflect the angle difference of the laser radar of the target device between the second scanning time and the first scanning time.

[0068] Specifically, the position increment and the angle increment can be determined by the following formula 8-formula 11:

[0069] Δp=p tk -p t0 Formula 8

[0070] Δroll=roll tk -roll t0 Formula 9

[0071] Δpitch=pitch tk -pitch t0 Formula 10

[0072] Δyaw=yaw tk -yaw t0 Formula 11

[0073] Wherein, Δp is the position increment between the second scanning time and the first scanning time, Δroll, Δpitch and Δyaw are the angle increments between the second scanning time and the first scanning time; p tk , roll tk , pitch tk , yaw tk is the pose data of the second scanning time, p t0 , roll t0 , pitch t0 , yaw t0 is the pose data of the first scanning time.

[0074] After the pose increment between the pose data of the second scanning moment and the first scanning moment is obtained, the pose change of the laser radar of the target between the second scanning moment and the first scanning moment can be determined, so that the distorted laser point cloud data obtained at the second scanning moment can be restored to the laser radar coordinate system at the first scanning moment, to obtain the laser point cloud data generated by the laser radar scanning of the target device at the spatial position corresponding to the first scanning moment, to reflect the three-dimensional environment at the spatial position corresponding to the first scanning moment.

[0075] Specifically, the distorted laser point cloud data can be restored to the laser radar coordinate system at the first scanning moment by the following steps B21-B22.

[0076] B21, determine the rotation matrix according to the pose increment between the pose data of the second scanning moment and the pose data of the first scanning moment.

[0077] Here, the rotation matrix is used to indicate the pose rotation relationship between the second scanning moment and the first scanning moment, that is, to indicate the conversion relationship between the laser radar coordinate system at the first scanning moment and the laser radar coordinate system at the second scanning moment.

[0078] Specifically, the rotation matrix can be calculated by the following formulas 12-15:

[0079] R tk = R z (Δyaw)*R y (Δpitch)*R x (Δroll) Formula 12

[0080]

[0081]

[0082]

[0083] B22, determine the corrected position of the first laser point cloud feature data at the first scanning moment according to the position data corresponding to the first laser point cloud feature data and the rotation matrix, to obtain the first laser point cloud correction data.

[0084] Wherein, the position data corresponding to the first laser point cloud feature data is obtained based on the pose data of the second scanning moment, and the position data corresponding to the first laser point cloud feature data is the aforementioned p tk .

[0085] Specifically, the second laser point cloud correction data can be calculated according to the following formula 16:

[0086] p’tk=R tk *ptk + Δp Formula 16

[0087] In the above steps B1-B2, by correcting the laser point cloud feature data according to the pose increment of the two scanning moments in the same scanning period, the distortion of the laser point cloud feature data can be corrected, it is ensured that the laser point cloud feature data belongs to the same scanning period and reflects the environment at the starting scanning moment in the scanning period, so as to realize accurate restoration of the three-dimensional environment.

[0088] S103, according to the first laser point cloud correction data and the pose data of the target device, device pose estimation is performed to obtain the pose matrix of the target device.

[0089] The pose matrix of the target device is used to indicate the conversion relationship between the world coordinate system corresponding to the target device and the laser radar coordinate system.

[0090] Specifically, the pose estimation of the target device can be performed through the following steps C1-C2 to obtain the pose matrix of the target device.

[0091] C1, down-sampling the first laser point cloud correction data to obtain first laser point cloud sparse data corresponding to the first laser point cloud correction data.

[0092] In a specific implementation, the first laser point cloud sparse data can be obtained by down-sampling the first laser point cloud correction data through voxel filtering of a point cloud library (PCL).

[0093] C2, filtering update according to the first laser point cloud sparse data and the pose data to obtain the pose matrix of the target device.

[0094] Specifically, based on the iterative Kalman filter (IESKF) algorithm, the pose data at the first scanning moment can be taken as the pose prediction value, and the matching result between the first laser point cloud sparse data and the sparse laser point cloud map can be taken as the observation, and the state update can be performed to obtain the pose matrix of the target device.

[0095] By down-sampling the corrected laser point cloud feature data and combining the pose data for filtering update to obtain the pose matrix of the target device, real-time update of the pose matrix can be realized, so as to ensure that the constructed map is accurate.

[0096] Optionally, in some possible cases, after obtaining the pose matrix of the target device, the first laser point cloud sparse data can be converted to the world coordinate system according to the pose matrix of the target device to obtain second laser point cloud sparse data, and the second laser point cloud sparse data can be added to the sparse laser point cloud map of the target device.

[0097] The conversion formula for converting the coordinates in the laser radar coordinate system to the coordinates in the world coordinate system based on the pose matrix of the target device can be shown as formula 17:

[0098] P w = T wn P i Formula 17

[0099] wherein Pw is the position coordinate in the world coordinate system, Pi is the coordinate in the laser radar coordinate system, and Twn is the pose matrix of the target device.

[0100] The first laser point cloud sparse data can be converted to the world coordinate system according to the above formula 17 to obtain second laser point cloud sparse data. By adding the converted laser point cloud sparse data to the laser point cloud sparse map after the coordinate conversion of the laser point cloud sparse data based on the pose matrix, the accuracy of the laser point cloud sparse map constructed can be improved.

[0101] In S104, the first laser point cloud correction data is converted to the world coordinate system based on the pose matrix of the target device to obtain second laser point cloud feature data, and the second laser point cloud feature data is added to the dense laser point cloud map of the target device.

[0102] Specifically, the first laser point cloud correction data can be converted to the world coordinate system according to the above formula 17 to obtain second laser point cloud feature data.

[0103] Alternatively, after the second laser point cloud feature data is added to the dense laser point cloud map of the target device, the dense laser point cloud map can also be cropped and filtered to control the dense laser point cloud map within a preset spatial size range. For example, the cropping and filtering of the PCL library can be used to retain a cubic space with a side length of 5m centered on the target device. By cropping and filtering the dense laser point cloud map, the dense laser point cloud map can be controlled within a preset spatial size range, which can reduce the storage space required to store the dense laser point cloud map and reduce memory consumption.

[0104] In the above Figure 1In the corresponding technical solution, the first laser point cloud feature data and the pose data of the target device having a laser radar and an inertial sensor are acquired, the first laser point cloud feature data is de-distorted according to the pose data to obtain first laser point cloud correction data, device pose estimation is performed according to the first laser point cloud correction data and the pose data to obtain a pose matrix of the target device, and finally the first laser point cloud correction data is converted to a world coordinate system according to the pose matrix, and second laser point cloud feature data obtained by the conversion is added to a dense laser point cloud map of the target device, thereby completing the creation of the dense laser point cloud map. Since the first laser point cloud correction data is obtained by de-distorting the first laser point cloud feature data, the first laser point cloud correction data can accurately reflect the environment around the target device when the target device laser radar emits the first laser point, the pose matrix is obtained by performing pose estimation based on the first laser point cloud correction data and the pose data, and the pose matrix can accurately reflect the conversion relationship between the world coordinate system and the laser radar coordinate system when the target device laser radar emits the first laser point, so that the world coordinates of the laser point cloud feature data converted based on the pose matrix are more accurate, and thus the accuracy of the dense laser point cloud map constructed can be improved.

[0105] Referring to Figure 3 , Figure 3 Another flowchart of a laser point cloud map construction method provided by the embodiments of the present application is shown in FIG. 5. The method can be applied to the target device mentioned above or other devices having a connection relationship or a matching relationship with the target device. As shown in FIG. 5, the method comprises the following steps: Figure 3

[0106] S201, acquiring first laser point cloud feature data and pose data of a target device.

[0107] S202, de-distorting the first laser point cloud feature data of the target device according to the pose data of the target device to obtain first laser point cloud correction data corresponding to the first laser point cloud feature data.

[0108] S203, performing device pose estimation according to the first laser point cloud correction data and the pose data of the target device to obtain a pose matrix of the target device.

[0109] S204, converting the first laser point cloud correction data to a world coordinate system according to the pose matrix of the target device to obtain second laser point cloud feature data.

[0110] The specific implementation of steps S201-S204 can refer to the description of steps S101-S104 above, and will not be described here.

[0111] S205, acquiring a first device position and a second device position. ​

[0112] The first device position is a current position of the target device, which can be understood as a position of the target device when the target device acquires all laser point cloud data in a current scanning period by the laser radar of the target device; and the second device position is a position of the target device when the last laser point cloud feature data is added to the dense laser point cloud map.

[0113] S206, in a case where a distance between the first device position and the second device position is greater than a preset distance threshold, adding the second laser point cloud feature data to the dense laser point cloud map of the target device.

[0114] Specifically, the distance between the first device position and the second device position can be calculated based on the following formula 18:

[0115]

[0116] wherein D is the distance between the first device position and the second device position; (P nx , P ny ) is the first device position, and (P lx , P ly ) is the second device position.

[0117] In the above Figure 3 In the corresponding technical solution, after obtaining the corrected laser point cloud feature data in the world coordinate system, the current position of the device and the position of the last time of adding the laser point cloud feature data are acquired, and in a case where the distance between the current position of the device and the position of the last time of adding the laser point cloud feature data is greater than a preset distance threshold, the laser point cloud feature data is added to the dense laser point cloud map, which can improve the generation efficiency of the dense laser point cloud map.

[0118] The method of the application is described above, and the device of the application is described below.

[0119] Referring to Figure 4 , Figure 4 is a structural schematic diagram of a laser point cloud map construction device provided by an embodiment of the application, which can be the target device or other devices having a connection relationship or a matching relationship with the target device. The laser point cloud map construction device 30 comprises:

[0120] The acquisition module 301 is configured to acquire first laser point cloud feature data and pose data of a target device, wherein the target device has a laser radar and an inertial sensor, and the first laser point cloud feature data is laser point cloud feature data in a laser radar coordinate system.

[0121] The distortion removal module 302 is configured to remove distortion from the first laser point cloud feature data according to the pose data, to obtain first laser point cloud correction data corresponding to the first laser point cloud feature data.

[0122] The pose estimation module 303 is configured to perform device pose estimation according to the first laser point cloud correction data and the pose data, to obtain a pose matrix of the target device, the pose matrix being used to indicate a conversion relationship between a world coordinate system and the laser radar coordinate system.

[0123] The map construction module 304 is configured to convert the first laser point cloud correction data to the world coordinate system according to the pose matrix, to obtain second laser point cloud feature data, and add the second laser point cloud feature data to a dense laser point cloud map of the target device.

[0124] In a possible design, the obtaining module 301 is specifically configured to: obtain a plurality of laser point cloud data of the target device, and calculate a curvature of each laser point cloud data in the plurality of laser point cloud data, the plurality of laser point cloud data being laser point cloud data generated by scanning a circle by the laser radar; and determine, in the plurality of laser point cloud data, laser point cloud data with a curvature greater than a first curvature threshold or a curvature less than a second curvature threshold as the first laser point cloud feature data, the first curvature threshold being greater than the second curvature threshold.

[0125] In a possible design, the distortion removal module 302 is specifically configured to: determine pose data at a first scanning moment and pose data at a second scanning moment, the first scanning moment being a scanning moment of a first laser point cloud data corresponding to the first laser point cloud feature data, the first laser point cloud data and the first laser point cloud feature data belonging to a same scanning period, and the second scanning moment being a scanning moment of the first laser point cloud feature data; and determine a correction position of the first laser point cloud feature data at the first scanning moment according to a pose increment between the pose data at the second scanning moment and the pose data at the first scanning moment, to obtain the first laser point cloud correction data.

[0126] In a possible design, the distortion removal module 302 is specifically configured to: determine a rotation matrix according to the pose increment, the rotation matrix being used to indicate a pose rotation relationship between the second scanning moment and the first scanning moment; and determine the correction position of the first laser point cloud feature data at the first scanning moment according to position data corresponding to the first laser point cloud feature data and the rotation matrix, to obtain the first laser point cloud correction data, wherein the position data corresponding to the first laser point cloud feature data is obtained based on the pose data at the second scanning moment.

[0127] In a possible design, the pose estimation module 303 is specifically configured to: down-sample the first laser point cloud correction data to obtain first laser point cloud sparse data corresponding to the first laser point cloud correction data; and perform filter updating according to the first laser point cloud sparse data and the pose data to obtain a pose matrix of the target device.

[0128] In a possible design, the map construction module 304 is further configured to: convert the first laser point cloud sparse data to a world coordinate system according to the pose matrix to obtain second laser point cloud sparse data, and add the second laser point cloud sparse data to the sparse laser point cloud map of the target device.

[0129] In a possible design, the map construction module 304 is further configured to: obtain a first device position and a second device position, the first device position being a current position of the target device, and the second device position being a position of the target device when the last laser point cloud feature data is added to the dense laser point cloud map; and in a case where a distance between the first device position and the second device position is greater than a preset distance threshold, add the second laser point cloud feature data to the dense laser point cloud map of the target device.

[0130] It should be noted that, Figure 4 The content not mentioned in the corresponding embodiments can be referred to the description of the foregoing method embodiments, which will not be described here.

[0131] The apparatus described above, by obtaining the first laser point cloud feature data and the pose data of the target device having a lidar and an inertial sensor, and performing de-distortion on the first laser point cloud feature data according to the pose data to obtain first laser point cloud correction data, then performing device pose estimation according to the first laser point cloud correction data and the pose data to obtain a pose matrix of the target device, and finally converting the first laser point cloud correction data to a world coordinate system according to the pose matrix and adding the second laser point cloud feature data obtained by the conversion to the dense laser point cloud map of the target device, the creation of the dense laser point cloud map is completed. Since the first laser point cloud correction data is obtained by de-distorting the first laser point cloud feature data, the first laser point cloud correction data can accurately reflect the environment around the target device when the target device's lidar emits the first laser point. The pose matrix is obtained by performing pose estimation based on the first laser point cloud correction data and the pose data, and the pose matrix can accurately reflect the conversion relationship between the world coordinate system and the lidar coordinate system when the target device's lidar emits the first laser point, so that the world coordinates of the laser point cloud feature data converted based on the pose matrix are more accurate, and thus the accuracy of the dense laser point cloud map constructed can be improved.

[0132] See Figure 5 , Figure 5 is a structural schematic diagram of a computer device provided by an embodiment of the present application. The computer device can be the target device mentioned above or other devices having a connection relationship or a matching relationship with the target device. The computer device 40 includes a processor 401 and a memory 402. The memory 402 is connected to the processor 401, for example, through a bus.

[0133] The processor 401 is configured to support the computer device 40 to perform the corresponding functions in the methods in the method embodiments described above. The processor 401 can be a central processing unit (CPU), a network processor (NP), a hardware chip or any combination thereof. The hardware chip described above can be an application specific integrated circuit (ASIC), a programmable logic device (PLD) or a combination thereof. The PLD described above can be a complex programmable logic device (CPLD), a field-programmable gate array (FPGA), a generic array logic (GAL) or any combination thereof.

[0134] The memory 402 is used to store program codes and the like. The memory 402 can include a volatile memory (VM), such as a random access memory (RAM); the memory 402 can also include a non-volatile memory (NVM), such as a read-only memory (ROM), a flash memory, a hard disk drive (HDD) or a solid-state drive (SSD); the memory 402 can also include a combination of the above-mentioned types of memories.

[0135] The processor 401 can invoke the program codes to perform the following operations:

[0136] Obtaining first laser point cloud feature data and pose data of a target device, wherein the target device has a laser radar and an inertial sensor, and the first laser point cloud feature data is laser point cloud feature data in a laser radar coordinate system;

[0137] undistort the first laser point cloud feature data according to the pose data to obtain first laser point cloud correction data corresponding to the first laser point cloud feature data;

[0138] perform device pose estimation according to the first laser point cloud correction data and the pose data to obtain a pose matrix of the target device, the pose matrix being used to indicate a conversion relationship between a world coordinate system and the laser radar coordinate system;

[0139] convert the first laser point cloud correction data to the world coordinate system according to the pose matrix to obtain second laser point cloud feature data, and add the second laser point cloud feature data to a dense laser point cloud map of the target device.

[0140] The embodiments of the present application further provide a computer readable storage medium, which stores a computer program, the computer program comprising program instructions, the program instructions causing a computer to execute the method according to the foregoing embodiments when the computer program is executed by the computer.

[0141] Those skilled in the art can understand that all or part of the processes in the above-mentioned embodiments can be completed by a computer program instructing related hardware, and the program can be stored in a computer readable storage medium. When the program is executed, the program can include the processes of the above-mentioned embodiments. The storage medium can be a magnetic disc, an optical disc, a read-only memory (ROM) or a random access memory (RAM), etc.

[0142] The above only describes the preferred embodiments of the present application, and of course cannot limit the scope of the rights of the present application, so equivalent changes made according to the claims of the present application are still within the scope of the present application.

Claims

1. A method for constructing a laser point cloud map, characterized in that, The method comprises: obtaining first laser point cloud feature data and pose data of a target device, wherein the target device has a laser radar and an inertial sensor, and the first laser point cloud feature data is laser point cloud feature data in a laser radar coordinate system; de-warping the first laser point cloud feature data according to the pose data to obtain first laser point cloud correction data corresponding to the first laser point cloud feature data; performing device pose estimation according to the first laser point cloud correction data and the pose data to obtain a pose matrix of the target device, the pose matrix being used to indicate a conversion relationship between a world coordinate system and the laser radar coordinate system; converting the first laser point cloud correction data to the world coordinate system according to the pose matrix to obtain second laser point cloud feature data, and adding the second laser point cloud feature data to a dense laser point cloud map of the target device; the de-warping of the first laser point cloud feature data according to the pose data to obtain the first laser point cloud correction data comprises: determining pose data at a first scanning time and pose data at a second scanning time, the first scanning time being a scanning time of first laser point cloud data corresponding to the first laser point cloud feature data, the first laser point cloud data and the first laser point cloud feature data belonging to a same scanning period, and the second scanning time being the scanning time of the first laser point cloud feature data; determining a correction position of the first laser point cloud feature data at the first scanning time according to a pose increment between the pose data at the second scanning time and the pose data at the first scanning time to obtain the first laser point cloud correction data.

2. The method of claim 1, wherein, the obtaining of the first laser point cloud feature data of the target device comprises: obtaining a plurality of laser point cloud data of the target device and calculating a curvature of each laser point cloud data in the plurality of laser point cloud data, the plurality of laser point cloud data being laser point cloud data generated by the laser radar in one scanning cycle; in the plurality of laser point cloud data, determining laser point cloud data with a curvature greater than a first curvature threshold or a curvature less than a second curvature threshold as the first laser point cloud feature data, the first curvature threshold being greater than the second curvature threshold.

3. The method of claim 1, wherein, the determining of the correction position of the first laser point cloud feature data at the first scanning time according to the pose increment between the pose data at the second scanning time and the pose data at the first scanning time to obtain the first laser point cloud correction data comprises: determining a rotation matrix according to the pose increment, the rotation matrix being used to indicate a pose rotation relationship between the second scanning time and the first scanning time; determining the correction position of the first laser point cloud feature data at the first scanning time according to position data corresponding to the first laser point cloud feature data and the rotation matrix to obtain the first laser point cloud correction data, wherein the position data corresponding to the first laser point cloud feature data is obtained based on the pose data at the second scanning time.

4. The method of claim 1, wherein, The device pose estimation is performed according to the first laser point cloud correction data and the pose data, so as to obtain a pose matrix of the target device, and the method comprises the following steps: The first laser point cloud correction data is down-sampled to obtain first laser point cloud sparse data corresponding to the first laser point cloud correction data; Filtering update is performed according to the first laser point cloud sparse data and the pose data, so as to obtain the pose matrix of the target device.

5. The method of claim 4, wherein, After the filtering update is performed according to the first laser point cloud sparse data and the pose data, so as to obtain the pose matrix of the target device, the method further comprises the following steps: The first laser point cloud sparse data is converted to the world coordinate system according to the pose matrix, so as to obtain second laser point cloud sparse data, and the second laser point cloud sparse data is added to a sparse laser point cloud map of the target device.

6. The method according to any one of claims 1 to 5, characterized in that, The second laser point cloud feature data is added to the dense laser point cloud map of the target device, and the method comprises the following steps: A first device position and a second device position are obtained, the first device position is a current position of the target device, and the second device position is a position of the target device when laser point cloud feature data is last added to the dense laser point cloud map; In a case where a distance between the first device position and the second device position is greater than a preset distance threshold, the second laser point cloud feature data is added to the dense laser point cloud map of the target device. 7.A laser point cloud map construction device, characterized by, The method comprises the following steps: An obtaining module is configured to obtain first laser point cloud feature data and pose data of a target device, wherein the target device is provided with a laser radar and an inertial sensor, and the first laser point cloud feature data is laser point cloud feature data in a laser radar coordinate system; A distortion removal module is configured to remove distortion from the first laser point cloud feature data according to the pose data, so as to obtain first laser point cloud correction data corresponding to the first laser point cloud feature data; A pose estimation module is configured to perform device pose estimation according to the first laser point cloud correction data and the pose data, so as to obtain a pose matrix of the target device, wherein the pose matrix is used to indicate a conversion relationship between a world coordinate system and the laser radar coordinate system; A map construction module is configured to convert the first laser point cloud correction data to the world coordinate system according to the pose matrix, so as to obtain second laser point cloud feature data, and add the second laser point cloud feature data to a dense laser point cloud map of the target device. The de-distortion module is specifically configured to: determine the pose data at a first scanning time and the pose data at a second scanning time, the first scanning time being a scanning time of first laser point cloud data corresponding to the first laser point cloud feature data, the first laser point cloud data and the first laser point cloud feature data belonging to a same scanning period, the second scanning time being a scanning time of the first laser point cloud feature data, and determine a corrected position of the first laser point cloud feature data at the first scanning time according to a pose increment between the pose data at the second scanning time and the pose data at the first scanning time, to obtain the first laser point cloud correction data.

8. A computer device, comprising: The computer device includes a memory connected to a processor, the processor being configured to execute one or more computer programs stored in the memory, and the processor, when executing the one or more computer programs, causes the computer device to implement the method according to any one of claims 1-6.

9. A computer-readable storage medium, characterized in that, The computer readable storage medium stores a computer program, the computer program including program instructions, the program instructions, when executed by a processor, causing the processor to execute the method according to any one of claims 1-6.

Images