A perception system and method for constructing a mobile robot color point cloud map

Abstract

The present invention belongs to the technical field of point cloud map construction, and specifically relates to a perception system and method for constructing a color point cloud map of a mobile machine. The perception system includes a solid-state lidar module, a binocular camera module, an inertial odometer module, and a computer. The computer includes a timestamp synchronization module, a dynamic obstacle detection module, a data fusion module, and a display. The present invention collects point clouds and optical images of the environment through the solid-state lidar module and the binocular camera module, and detects inertial data of the mechanical movement through the inertial measurement unit module; and performs data processing, correction, interpolation, RGB assignment, and image overlay on the computer to construct a color three-dimensional point cloud map. Compared with the point cloud without color and the two-dimensional image lacking direct distance information, the color three-dimensional point cloud map obtained by the present invention represents more environmental data dimensions, and at the same time provides a more intuitive visual feedback, providing a data basis for the three-dimensional reconstruction technology.

Description

Technical Field

[0001] The present invention belongs to the technical field of point cloud map construction, and specifically relates to a perception system and method for constructing a color point cloud map of a mobile machine. Background Art

[0002] In the mechanical field, environmental perception and map construction based on a monocular camera have low cost and basically meet the requirements, but cannot provide reliable spatial depth information. Depth cameras can provide spatial information, but are also greatly affected by light, and the reliability of the given spatial depth information is not high, only suitable for small-scale applications such as indoors. Solid-state lidar has low cost, is less affected by light conditions, has a long measurement distance, and can provide high-precision spatial information.

[0003] Therefore, the fusion perception of solid-state lidar and a binocular camera can improve the performance and reliability of the perception system. In the field of mobile machines, machines are often used for tasks such as detection and search and rescue, saving manpower and improving operation safety. Among them, the feature extraction and classification recognition technology of three-dimensional point clouds is a key and cutting-edge technology in the research field of intelligent mobile machine environmental perception, and is playing an increasingly important role in fields such as autonomous driving, AR, digital cities, historical site restoration, and intelligent mines. In the past, in the field of intelligent mechanical point cloud data processing, mainly by extracting the spatial geometric features of the point cloud as environmental features, after adding color information, based on the geometric features, the color and texture features can be comprehensively used to understand the point cloud data; however, currently, when a machine moves outdoors to construct a map, a big problem faced is that dynamic obstacles greatly affect the mapping accuracy, which is inevitable in outdoor environments. In addition, although the density of the point cloud obtained by solid-state lidar is improved compared with mechanical lidar, the density is still relatively low compared with that of a camera. Therefore, the fusion effect of the existing solid-state lidar and the images taken by a binocular camera is not good; thus, how to provide a method to remove dynamic obstacles during the map construction process, thereby improving the mapping accuracy, and at the same time be able to process the point cloud obtained by solid-state lidar to improve its density, so as to obtain a high-precision color point cloud image after fusing with the images of a binocular camera, is the research direction of this industry. Summary of the Invention

[0004] Aiming at the problems existing in the above-mentioned prior art, the present invention provides a perception system and method for constructing a color point cloud map of a mobile machine, which can remove dynamic obstacles, thereby improving the mapping accuracy, and at the same time be able to process the point cloud obtained by solid-state lidar to improve its density, and finally obtain a high-precision color point cloud image after fusing with the images of a binocular camera.

[0005] In order to achieve the above object, the technical solution adopted by the present invention is:

[0006] A perception system for constructing a color point cloud map of a mobile machine, including installing the perception system on the mobile machine. The perception system includes a solid-state lidar module for collecting point cloud images of the surrounding environment, a binocular camera module for collecting optical images of the surrounding environment, an inertial odometer module for detecting inertial data when the machine moves, and a computer for receiving data from the solid-state lidar module, the binocular camera module, and the inertial odometer module;

[0007] The computer includes a timestamp synchronization module for synchronizing the clock of the solid-state lidar with the main clock of the computer, a dynamic obstacle detection module for detecting optical images with dynamic obstacles collected by the binocular camera module, a data fusion module for fusing the point cloud image and the optical image to form a color point cloud map, and a display for displaying the color point cloud map.

[0008] Furthermore, the method for constructing the color point cloud map of the perception system specifically includes the following steps

[0009] (1) Calibrate the internal parameters and distortion coefficients of the binocular camera module in advance, as well as the external parameters from the solid-state lidar to the binocular camera module, the external parameters from the inertial odometer module to the binocular camera module, and the external parameters from the solid-state lidar module to the inertial odometer module, and store them in the data fusion module;

[0010] (2) Synchronize the clock of the solid-state lidar with the main clock of the computer through the timestamp synchronization module;

[0011] (3) The computer synchronously receives the point cloud image of the surrounding environment collected by the solid-state lidar module, the optical image of the surrounding environment collected by the binocular camera module, and the inertial data detected by the inertial measurement unit module when the machine moves;

[0012] (4) The computer corrects the obtained binocular camera optical image through the internal parameters and distortion coefficients calibrated in step (1). Then, the dynamic obstacle detection module performs target detection on each corrected frame of the image using the deep neural network YOLOv3 to obtain the frame of the image with dynamic obstacles;

[0013] (5) The computer obtains the point cloud image data and the inertial data, uses the timestamp synchronization module to synchronize and align the times of the two sets of data, then interpolates the inertial data to obtain the pose of the current frame of the solid-state lidar in the world coordinate system, and performs coordinate transformation on all the point clouds of the current frame based on the obtained motion state of the current frame of the solid-state lidar, so as to correct the distortion of the point cloud image data;

[0014] (6) Perform point cloud interpolation on the corrected point cloud image data to obtain a densified point cloud image;

[0015] (7) Combine the densified point cloud image with the extrinsic parameters from the solid-state lidar to the binocular camera module and the intrinsic parameters of the binocular camera module calibrated in step (1), and project the point cloud image onto the imaging plane in the uv coordinate system of the binocular camera; combine the image frame with dynamic obstacles obtained in step (4), remove the point cloud image data at the corresponding pixel coordinates, then assign the RGB information of each pixel position of the optical image on the imaging plane to the corresponding point cloud image, and thus obtain each frame of color-dense point cloud image of the surrounding environment; finally, stack each frame of the obtained color-dense point cloud image to obtain a color three-dimensional point cloud map of the surrounding environment.

[0016] Further, the intrinsic parameters of the binocular camera module in step (1) are K and the distortion coefficients are k 1 , k 2 , k 3 , p 1 , p 2 ; the extrinsic parameters from the solid-state lidar to the binocular camera module are The extrinsic parameters from the inertial odometer module to the binocular camera module are The extrinsic parameters from the solid-state lidar module to the inertial odometer module are

[0017] Further, the calibration process in step (4) is as follows:

[0018] Assume that the pixel coordinates of the image obtained by the binocular camera module before calibration in the uv coordinate system are (u, v). Combine the intrinsic parameters and distortion coefficients calibrated in step (1) and correct each pixel coordinate according to the following formula:

[0019]

[0020] where (u′, v′) are the pixel coordinates of the image after calibration; r is the distance of the pixel point from the imaging center.

[0021] Further, the specific process of step (5) is as follows:

[0022] Assume that the point cloud image data of the k-th frame of the lidar is expressed as:

[0023] The inertial data pose corresponding to the k-th frame header obtained by interpolating the inertial data poses of the two frames before and after the k-th frame header is expressed as:

[0024]

[0025] where is the rotation matrix of the i-th point of the k-th frame of the point cloud image data to the corresponding local world coordinate system, is the translation matrix from the i-th point of the k-th frame of the point cloud image data to the corresponding local world coordinate system;

[0026] Using the same method, the inertial data poses at the middle and end of the k-th frame can be obtained as Perform quadratic curve fitting to obtain the point cloud pose curve fitting equation with respect to time:

[0027] Based on this equation, the formula for correcting the point cloud distortion can be obtained as:

[0028] Thereby correcting the distortion of the point cloud image data.

[0029] Furthermore, the specific process of step (6) is as follows:

[0030] According to the characteristics of non-repetitive scanning of the solid-state lidar, select four adjacent points at intervals and perform interpolation processing:

[0031] Select two adjacent points on the first and third of the three scan lines of the solid-state lidar and denote them as Q 11 (x 1 ,y 1 ,z 1 ),Q 12 (x 1 ,y 2 ,z 2 ),Q 21 (x 2 ,y 1 ,z 3 ),Q 22 (x 2 ,y 2 ,z 4 ), denote R 1 ,R 2 as the two points interpolated in the x direction, and P as the finally interpolated point, then there are:

[0032]

[0033] Summarized into the form of a polynomial as:

[0034] z = a 0 + a 1 x + a 2 y + a 3 xy,

[0035] a 0 = f(Q 11 ),

[0036] a 1 = f(Q 21 ) - f(Q 11),

[0037] a 2 = f(Q 12 ) - f(Q 11 ),

[0038] a 3 = f(Q 22 ) - f(Q 21 ) - f(Q 12 ) + f(Q 11 ),

[0039] Four adjacent point coordinates are selected and substituted into the above polynomial to calculate the coordinates of the interpolation point P(x, y, z). A threshold of 0.3 is set for the z value. If the calculated z value is greater than this threshold, it is considered that the depth value error between the interpolation point and the selected four points is too large, and no subsequent interpolation processing is performed. Four adjacent points are reselected and step (6) is repeated. If the calculated z value is less than or equal to this threshold, interpolation processing is performed according to the calculated coordinates of the interpolation point P(x, y, z); thus, a densified point cloud image is obtained, ensuring the accuracy of the interpolated point cloud data.

[0040] Further, the specific process of step (7) is as follows:

[0041] Let the data of each frame of the densified point cloud image be P l (x 0 , y 0 , z 0 ) T , and the data of each frame of the image after each frame of the point cloud image is projected onto the imaging plane in the uv coordinate system of the binocular camera is P c (x 1 , y 1 ) T ;

[0042] The internal parameters of the binocular camera module

[0043] The external parameters from the solid-state lidar to the binocular camera module

[0044] Then there is:

[0045] That is

[0046] Thus, each frame of the point cloud image is projected onto the imaging plane in the uv coordinate system of the binocular camera; combined with the frame of image with dynamic obstacles obtained in step (4), the point cloud image data at the corresponding pixel coordinates is removed. Then, the RGB information of each pixel position of the optical image on the imaging plane is assigned to the corresponding frame of the point cloud image. The point cloud image data after assignment is based on the formula Backproject it into the three-dimensional space to obtain each frame of color-dense point cloud image P'. c (x, y, z, r, g, b) T ; Finally, stack each frame of the obtained color-dense point cloud image to obtain a color three-dimensional point cloud map of the surrounding environment.

[0047] Compared with the prior art, the present invention first collects the point cloud image of the surrounding environment through the solid-state lidar module, the optical image of the surrounding environment through the binocular camera module, and the inertial data during mechanical movement through the inertial measurement unit module; and corrects the optical image through the calibrated internal and distortion coefficients, then performs object detection on each frame of the corrected image using the deep neural network YOLOv3 to obtain the frame of image with dynamic obstacles; then combines the obtained point cloud image data with the inertial data for coordinate transformation to correct the distortion of the point cloud image data; then designs an interlaced point cloud interpolation algorithm according to the non-repetitive scanning characteristics of the solid-state lidar, reduces the computational amount, ensures real-time performance, makes the point cloud data dense, and projects the dense point cloud image data onto the imaging plane in the uv coordinate system of the binocular camera. At this time, remove the frame of image with dynamic obstacles; finally, assign the RGB information of each pixel position of the optical image on the imaging plane to the corresponding point cloud image to obtain each frame of color-dense point cloud image of the surrounding environment; and stack each frame of color-dense point cloud image to obtain a color three-dimensional point cloud map of the surrounding environment. The color three-dimensional point cloud map obtained by the present invention has more dimensions of environmental data represented by the three-dimensional color point cloud compared with the colorless point cloud and the two-dimensional image lacking direct distance information, and is also closer to the world in which humans live. Therefore, when the mobile machine performs tasks, based on the map construction method of the present invention, a more intuitive visual feedback can be given, providing a data basis for three-dimensional reconstruction technologies in scenarios such as digital cities and smart mines. Description of the Drawings

[0048] Figure 1 is a schematic structural diagram of the perception system in the present invention;

[0049] Figure 2 is the overall flowchart of the construction of the color point cloud map in the present invention. Detailed Embodiments

[0050] The present invention will be further described below.

[0051] As Figure 1As shown in the figure, a perception system is installed on a mobile machine. The perception system includes a solid-state lidar module, a binocular camera module, an inertial odometer module, and a computer. The computer includes a timestamp synchronization module, a dynamic obstacle detection module, a data fusion module, and a display. The solid-state lidar module is used to collect the point cloud image of the surrounding environment and transmit it to the computer; the binocular camera module is used to collect the optical image of the surrounding environment and transmit it to the computer; the inertial measurement unit module is used to detect the inertial data during the movement of the machine and transmit it to the computer; the timestamp synchronization module is used to synchronize the clock of the solid-state lidar with the main clock of the computer; the dynamic obstacle detection module is used to detect the image with dynamic obstacles in the optical image collected by the binocular camera module; the data fusion module is used to fuse the point cloud image and the optical image to form a color point cloud map and display it through the display.

[0052] As Figure 2 shown, the steps for constructing the specific color point cloud map are as follows:

[0053] (1) Calibrate the internal parameters of the binocular camera module in advance as K and the distortion coefficients as k 1 , k 2 , k 3 , p 1 , p 2 ; the external parameters from the solid-state lidar to the binocular camera module are the external parameters from the inertial odometer module to the binocular camera module are the external parameters from the solid-state lidar module to the inertial odometer module are and store them in the data fusion module;

[0054] (2) Synchronize the clock of the solid-state lidar with the main clock of the computer through the timestamp synchronization module. The synchronization process uses the Delay request-response mechanism (two steps) of IEEE1588v2.0PTP. The Livox device acts as the slave end and performs ptp time synchronization with the computer (master) clock device.

[0055] (3) The computer synchronously receives the point cloud image of the surrounding environment collected by the solid-state lidar module, the optical image of the surrounding environment collected by the binocular camera module, and the inertial data detected by the inertial measurement unit module during the movement of the machine;

[0056] (4) The computer corrects the obtained binocular camera optical image with the internal parameters and distortion coefficients calibrated in step (1). Then, the dynamic obstacle detection module uses the deep neural network YOLOv3 to perform target detection on each corrected frame of the image to obtain the frame of the image with dynamic obstacles; the specific correction process:

[0057] Before correction, let the pixel coordinates of the image obtained by the binocular camera module in the uv coordinate system be (u, v). Combine the internal parameters and distortion coefficients calibrated in step (1) and correct each pixel coordinate according to the following formula:

[0058]

[0059] where (u′, v′) are the pixel coordinates of the image after correction; r is the distance of the pixel point from the imaging center.

[0060] (5) The computer will obtain the point cloud image data and inertial data, use the timestamp synchronization module to synchronize and align the time of the two sets of data, and then interpolate the inertial data to obtain the pose of the current frame of solid-state lidar in the world coordinate system. Based on the obtained motion state of the current frame of solid-state lidar, perform coordinate transformation on all the point clouds of the current frame, so as to correct the distortion of the point cloud image data. The specific process is as follows:

[0061] Let the point cloud image data of the k-th frame of the lidar be expressed as:

[0062] The pose of the inertial data corresponding to the header of the k-th frame obtained by interpolating the poses of the inertial data of the two frames before and after the header of the k-th frame is expressed as:

[0063]

[0064] where is the rotation matrix from the i-th point of the k-th frame of the point cloud image data to the corresponding local world coordinate system, is the translation matrix from the i-th point of the k-th frame of the point cloud image data to the corresponding local world coordinate system;

[0065] Using the same method, the poses of the inertial data in the middle and at the end of the k-th frame can be obtained as Perform quadratic curve fitting to obtain the point cloud pose curve fitting equation with respect to time:

[0066] According to this equation, the formula for correcting the point cloud distortion can be obtained as:

[0067] Thus, the distortion of the point cloud image data is corrected.

[0068] (6) Perform point cloud interpolation on the corrected point cloud image data to obtain a densified point cloud image. The specific process is as follows:

[0069] According to the characteristics of the non-repetitive scanning of the solid-state lidar, select four adjacent points at intervals and perform interpolation processing in three-dimensional space using the bilinear interpolation method:

[0070] Select two adjacent points on the first and third of the three scan lines of the solid-state lidar and denote them as Q 11 (x 1 ,y 1 ,z 1 ), Q 12 (x 1 ,y 2 ,z 2 ), Q 21 (x 2 ,y 1 ,z 3 ), Q 22 (x 2 ,y 2 ,z 4 ). Denote R 1 , R 2 as two points interpolated in the x direction, and P as the finally interpolated point. Then there is:

[0071]

[0072] Summarized into the form of a polynomial as:

[0073] z = a 0 + a 1 x + a 2 y + a 3 xy,

[0074] a 0 = f(Q 11 ),

[0075] a 1 = f(Q 21 ) - f(Q 11 ),

[0076] a 2 = f(Q 12 ) - f(Q 11 ),

[0077] a 3 = f(Q 22 ) - f(Q 21 ) - f(Q 12 ) + f(Q 11 ),

[0078] Four adjacent point coordinates will be selected and substituted into the above polynomial to calculate the coordinates of the interpolation point P(x, y, z). A threshold of 0.3 will be set for the z value. If the calculated z value is greater than this threshold, it is considered that the depth value error between the interpolation point and the selected four points is too large, and no subsequent interpolation processing will be performed. Instead, four adjacent points will be reselected and step (6) will be repeated. If the calculated z value is less than or equal to this threshold, interpolation processing will be performed according to the calculated coordinates of the interpolation point P(x, y, z), thereby obtaining a densified point cloud image and ensuring the accuracy of the interpolated point cloud data.

[0079] (7) Combine the densified point cloud image with the external parameters of the solid-state lidar to the binocular camera module calibrated in step (1) and the internal parameters of the binocular camera module, so that the point cloud image is projected onto the imaging plane in the uv coordinate system of the binocular camera. Combine the image with dynamic obstacles obtained in step (4), and remove the point cloud image data at the corresponding pixel coordinates. Then, assign the RGB information at each pixel position of the optical image on the imaging plane to the corresponding point cloud image, and then obtain each frame of color-dense point cloud image of the surrounding environment. Finally, stack each frame of the obtained color-dense point cloud image to obtain a color three-dimensional point cloud map of the surrounding environment. The specific process is as follows:

[0080] Let the data of each frame of the densified point cloud image be P l (x 0 ,y 0 ,z 0 ) T , and the data of each frame of the image after each frame of the point cloud image is projected onto the imaging plane in the uv coordinate system of the binocular camera is P c (x 1 ,y 1 ) T ;

[0081] Internal parameters of the binocular camera module

[0082] External parameters of the solid-state lidar to the binocular camera module

[0083] Then there is:

[0084] That is

[0085] Thus, each frame of the point cloud image is projected onto the imaging plane in the uv coordinate system of the binocular camera. Combine the image with dynamic obstacles obtained in step (4), and remove the point cloud image data at the corresponding pixel coordinates. Then, assign the RGB information at each pixel position of the optical image on the imaging plane to the corresponding each frame of the point cloud image. The data of the point cloud image after assignment is based on the formula Back-project it into the three-dimensional space to obtain each frame of color dense point cloud image P'. c (x, y, z, r, g, b) T ; Finally, stack each frame of the obtained color dense point cloud image to obtain a color three-dimensional point cloud map of the surrounding environment.

[0086] The above are only the preferred embodiments of the present invention. It should be noted that for those of ordinary skill in the art, without departing from the principle of the present invention, several improvements and refinements can be made, and these improvements and refinements should also be regarded as the protection scope of the present invention.

Claims

1. A method for constructing a color point cloud map for mobile machinery, characterized in that, a sensing system is installed on the mobile machinery, and the sensing system includes a solid-state lidar module for collecting point cloud images of the surrounding environment, a binocular camera module for collecting optical images of the surrounding environment, an inertial odometer module for detecting inertial data when the machinery moves, a computer for receiving data from the solid-state lidar module, the binocular camera module, and the inertial odometer module; the computer includes a timestamp synchronization module for synchronizing the clock of the solid-state lidar with the main clock of the computer, a dynamic obstacle detection module for detecting optical images with dynamic obstacles collected by the binocular camera module, a data fusion module for fusing the point cloud image and the optical image to form a color point cloud map and a display for displaying the color point cloud map; The specific steps for constructing the color point cloud map are as follows: (1) Calibrate the internal parameters and distortion coefficients of the binocular camera module in advance, as well as the external parameters from the solid-state lidar to the binocular camera module, the external parameters from the inertial odometer module to the binocular camera module, and the external parameters from the solid-state lidar module to the inertial odometer module, and store them in the data fusion module; (2) Synchronize the clock of the solid-state lidar with the main clock of the computer through the timestamp synchronization module; (3) The computer synchronously receives the point cloud image of the surrounding environment collected by the solid-state lidar module, the optical image of the surrounding environment collected by the binocular camera module, and the inertial data detected by the inertial measurement unit module when the machinery moves; (4) The computer corrects the obtained binocular camera optical image with the internal parameters and distortion coefficients calibrated in step (1), and then the dynamic obstacle detection module performs target detection on each corrected frame of the image using the deep neural network YOLOv3 to obtain the frame of the image with dynamic obstacles; (5) The computer obtains the point cloud image data and inertial data, uses the timestamp synchronization module to synchronize and align the time of the two sets of data, then interpolates the inertial data to obtain the pose of the current frame of the solid-state lidar in the world coordinate system, and performs coordinate transformation on all the point clouds of the current frame based on the obtained motion state of the current frame of the solid-state lidar, so as to correct the distortion of the point cloud image data; (6) Perform point cloud interpolation on the corrected point cloud image data to obtain a densified point cloud image; (7) Combine the densified point cloud image with the external parameters from the solid-state lidar to the binocular camera module and the internal parameters of the binocular camera module calibrated in step (1) to project the point cloud image onto the imaging plane in the uv coordinate system of the binocular camera; Combine the frame of the image with dynamic obstacles obtained in step (4), remove the point cloud image data at the corresponding pixel coordinates, then assign the RGB information of each pixel position of the optical image on the imaging plane to the corresponding point cloud image, and further obtain each frame of the color dense point cloud image of the surrounding environment; Finally, stack the obtained each frame of the color dense point cloud image to obtain the color three-dimensional point cloud map of the surrounding environment.

2. The method for constructing a color point cloud map according to claim 1, characterized in that: In step (1), the internal parameters of the binocular camera module are K and the distortion coefficient is k 1 , k 2 , k 3 , p 1 , p 2 ; The external parameter from the solid-state lidar to the binocular camera module is The external parameter from the inertial odometer module to the binocular camera module is The external parameter from the solid-state lidar module to the inertial odometer module is 3. The method for constructing a color point cloud map according to claim 2, It is characterized in that: In the calibration process of step (4): Assume that the pixel coordinates of the image obtained by the binocular camera module before calibration are (u, v) in the uv coordinate system. Combine the internal parameters and distortion coefficients calibrated in step (1) and correct each pixel coordinate according to the following formula: where (u', v') are the pixel coordinates of the image after calibration; r is the distance of the pixel point from the imaging center.

4. The method for constructing a color point cloud map according to claim 1, It is characterized in that: The specific process of step (5) is: Let the point cloud image data of the k-th frame of the lidar be represented as: The inertial data pose corresponding to the k-th frame header obtained by interpolating the inertial data poses of the two frames before and after the k-th frame header is expressed as: Among them, is the rotation matrix of the i-th point in the k-th frame of the point cloud image data to the corresponding local world coordinate system, is the translation matrix of the i-th point in the k-th frame of the point cloud image data to the corresponding local world coordinate system; The inertial data poses at the middle and end of the k-th frame can be obtained using the same method as Perform quadratic curve fitting to obtain the point cloud pose curve fitting equation with respect to time: Based on this equation, the formula for correcting the distortion of the point cloud is as follows: Thereby correcting the distortion of the point cloud image data.

5. The method for constructing a color point cloud map according to claim 1, It is characterized in that: The specific process of step (6) is: According to the characteristics of non-repetitive scanning of the solid-state lidar, select four adjacent points in an interlaced manner and perform interpolation processing: Select two adjacent points on the first and third of the three scanning lines of the solid-state lidar and denote them as Q 11 (x 1 ,y 1 ,z 1 ), Q 12 (x 1 ,y 2 ,z 2 ), Q 21 (x 2 ,y 1 ,z 3 ), Q 22 (x 2 ,y 2 ,z 4 ). Denote R 1 , R 2 as two points interpolated in the x direction, and P as the finally interpolated point. Then we have: The summary in the form of a polynomial is: z = a 0 + a 1 x + a 2 y + a 3 xy, a 0 = f(Q 11 ), a 1 = f(Q 21 ) - f(Q 11 ), a 2 = f(Q 12 ) - f(Q 11 ), a 3 = f(Q 22 ) - f(Q 21 ) - f(Q 12 ) + f(Q 11 ), Substitute the coordinates of the selected four adjacent points into the above polynomial to calculate the coordinates of the interpolation point P(x, y, z), and set the threshold value of the z value to 0.

3. If the calculated z value is greater than this threshold, it is considered that the depth value error between the interpolation point and the selected four points is too large, and no subsequent interpolation processing is performed, and four adjacent points are reselected to repeat step (6); if the calculated z value is less than or equal to this threshold, interpolation processing is performed according to the calculated coordinates of the interpolation point P(x, y, z); thereby obtaining a densified point cloud image.

6. The method for constructing a color point cloud map according to claim 1, It is characterized in that: The specific process of step (7) is: Let each frame of point cloud image data after densification be P l (x 0 , y 0 , z 0 ) T , and each frame of image data after each frame of point cloud image is projected onto the imaging plane in the uv coordinate system of the binocular camera is P c (x 1 , y 1 ) T ; Internal parameters of the binocular camera module Extrinsic Parameters from Solid-State LiDAR to Binocular Camera Module Then there is: That is Thus, each frame of point cloud image is projected onto the imaging plane in the uv coordinate system of the binocular camera; combining with the image of the frame with dynamic obstacles obtained in step (4), the point cloud image data at the corresponding pixel coordinates is removed, and then the RGB information of each pixel position of the optical image on the imaging plane is assigned to each corresponding frame of point cloud image. The point cloud image data after assignment is back-projected into the three-dimensional space according to the formula to obtain each frame of colored dense point cloud image P’ c (x, y, z, r, g, b) T ; finally, each frame of colored dense point cloud image obtained is superimposed to obtain the colored three-dimensional point cloud map of the surrounding environment.

Images