Three-dimensional dense point cloud mapping method and system based on depth camera fusion lidar

Abstract

The present invention provides a three-dimensional dense point cloud mapping method and system by integrating a depth camera with a laser radar. The method generates a two-dimensional grid map while constructing a three-dimensional dense point cloud map, thereby improving the accuracy of obtaining environment and obstacle information. The method comprises the following steps: first, accurate synchronization of the laser radar and the depth camera is achieved through sensor timestamp alignment and spatial calibration; then, the depth image acquired by the depth camera is processed, the three-dimensional dense point cloud is calculated, and the data quality is optimized through a Gaussian filtering algorithm; then, the optical flow method is used to estimate the optical flow vectors of the pixel points in the image sequence and mark dynamic objects; a two-dimensional grid map is generated in combination with the laser radar data, and obstacles are identified and segmented; further, loop detection is performed through feature point matching, feature points are accurately matched, and graph optimization is performed after the closed loop is detected to enhance map consistency; finally, a more complete and robust dense point cloud map is constructed through incremental updating and global optimization.

Description

Technical Field

[0001] The present invention relates to the technical field of positioning and mapping of indoor mobile robots, and in particular to a three-dimensional dense point cloud mapping method and system integrating a depth camera with a laser radar. Background Art

[0002] With the rapid development of intelligent mobile robots, they are widely used in logistics distribution, medical care, environmental monitoring and high-risk operations. These intelligent devices not only significantly reduce labor costs, but also improve the quality and efficiency of production and services. Therefore, accurately understanding and perceiving complex working environments is a key challenge for the widespread application of mobile robots.

[0003] SLAM technology is the core of robots' accurate understanding and perception of complex working environments. This technology also takes into account the important tasks of positioning robots and confirming the robot's surrounding environment. Today's single-sensor SLAM technology can be mainly divided into laser SLAM and visual SLAM. Single sensor technology faces many challenges in the field of robot perception, especially in the application requirements of different scenarios. Although lidar performs well in ranging and positioning, it is often difficult to perform effective closed-loop detection and spatial recognition in scenes with poor features such as corridors and open spaces, relying on laser-based SLAM algorithms such as Hector SLAM or GMapping. These algorithms usually rely on structured features in the environment for matching and positioning, but in environments with sparse features, there is a lack of sufficient information to support the algorithm's decision-making, resulting in a decrease in positioning accuracy. On the other hand, cameras, as visual sensors, can capture rich image information, which helps to understand the environment and perform object recognition. Visual SLAM algorithms such as ORB-SLAM and PTAM rely on feature points in the image to reconstruct and locate the environment in three dimensions. However, the performance of these algorithms is heavily dependent on lighting conditions. In environments that are too dark or too bright, the performance of the camera will be limited, affecting the accuracy of feature extraction and the stability of the algorithm.

[0004] SLAM algorithms that fuse camera and lidar data, such as RTAB-MAP, can alleviate the limitations of each sensor to a certain extent and achieve three-dimensional environment mapping. However, most of these methods are dominated by camera data and only use lidar data as an auxiliary. This dependency structure makes the algorithm still strongly dependent on visual features in the environment. Therefore, in scenes with poor environmental conditions or poor features, these fusion methods may not be able to achieve accurate positioning and mapping, thus affecting the overall mapping and recognition effects. Summary of the invention

[0005] The present application provides a three-dimensional dense point cloud mapping method and system that integrates a depth camera with a laser radar, so as to at least solve the problems of insufficient positioning and mapping accuracy of traditional methods in complex environments.

[0006] According to a first aspect, the present invention provides a three-dimensional dense point cloud mapping method using a depth camera fused with a laser radar, the method comprising:

[0007] Time synchronization and spatial calibration: Ensure the time consistency of the lidar and depth camera data, synchronize the sensor clock through the network time protocol, obtain the original timestamp and mark the capture time of each frame of data, and adjust the spatial parameters of the sensor through the calibration tool to ensure the consistency of the relative position and posture of the sensor;

[0008] Point cloud processing and optimization: Process the depth image collected by the depth camera, extract the three-dimensional dense point cloud and its corresponding coordinate information, and use the Gaussian filtering algorithm to remove noise and redundant points to improve the accuracy of the point cloud data;

[0009] Dynamic object recognition: Use the optical flow method to identify dynamic objects in image sequences, calculate the motion vector of each pixel, and mark the dynamic area by setting the speed threshold;

[0010] Obstacle fusion and positioning: The obstacle information identified by the lidar is integrated with the data of the depth camera to generate a two-dimensional grid map to achieve accurate positioning and segmentation of obstacles and dynamic objects;

[0011] Data fusion optimization: Use the extended Kalman filter method to optimize multi-sensor data fusion, reduce noise and errors, and improve the quality and accuracy of the final map;

[0012] Feature matching and loop detection: loop detection is performed through feature point matching to correct drift and improve map consistency during map optimization;

[0013] Map update and maintenance: Incrementally update and maintain the constructed three-dimensional dense point cloud map to ensure that the map continues to reflect changes in the real environment.

[0014] Furthermore, the time synchronization and calibration specifically include:

[0015] Use the NTP time protocol to synchronize timestamps, as shown below:

[0016]

[0017] Where, t sync is the time after synchronization, t L is the lidar timestamp, t D is the depth camera timestamp.

[0018] Then use the system internal clock error to compensate t sync =t L +δ L -δ D , eliminate the influence of clock error on time,δ L is the lidar clock error, δ D is the depth camera clock error. Then use the linear regression model to predict the time difference, t sync =t L +β0+β1t D , β0 and β1 are regression coefficients obtained by fitting historical time data to ensure that the clock error is eliminated to the greatest extent;

[0019] Use the least squares method to solve for the rotation and translation matrices: Get the rotation matrix R and the translation matrix t, and finally use the SDV method to decompose and calculate the best rotation and translation from the two sets of points, so that one set can be as close to the other as possible: R, t = SVD (p L ,p D ) for spatial calibration.

[0020] Furthermore, the point cloud processing and optimization specifically includes:

[0021] The pixels in the depth image are converted into a three-dimensional point cloud in combination with the depth information, and their coordinates p(X, Y, Z) in the three-dimensional space are calculated:

[0022] The Z axis coordinate is: Z = D (u, v)

[0023] The X-axis coordinates are:

[0024] The Y-axis coordinates are:

[0025] Where u, v are the coordinates of the pixel in the depth image, D(u, v) is the depth value obtained from the depth image, (f x ,f y ) is the focal length of the image, (c x ,c y ) is the image center;

[0026] Then use Gaussian filtering to filter the point cloud data projected into the three-dimensional space in the depth image, and use Gaussian function to determine the weights of different point clouds: In the formula, w ij is the weight based on the Gaussian function, point (p i ,p j ) is p i and p j The Euclidean distance between them, σ is the pre-set standard deviation;

[0027] Then confirm the position of each point cloud after filtering by weight: N(i) is the point p i The neighborhood of p j is a point in the neighborhood.

[0028] Furthermore, the dynamic object recognition specifically includes:

[0029] Estimate the three-dimensional position of the pixel points in the image captured by the depth camera: obtain the depth value of each pixel point from the depth camera and convert it into a position in the three-dimensional coordinate system. If the position of the pixel point in the image is (x, y), the depth is Z(x, y), and the intrinsic parameter matrix of the camera is K, then the three-dimensional coordinate P(x, y) can be expressed as:

[0030]

[0031] In the formula, K -1 is the inverse matrix of the camera intrinsic parameter matrix;

[0032] Perform 3D optical flow estimation on the pixels extracted to specific locations: Use the 3D position data of continuous frames to calculate the 3D optical flow vector of each point:

[0033] V(x,y)=P t+1 (x,y)-P t (x,y)

[0034] Where P t (x, y) is the three-dimensional position of the pixel at time t, P t+1 (x, y) is the three-dimensional position at time t+1, V(x, y) is the three-dimensional optical flow vector;

[0035] Perform three-dimensional velocity estimation on the optical flow vector of the pixel:

[0036]

[0037] Where V x , V y , V z They are the components of the optical flow vector in three-dimensional space;

[0038] Set the speed threshold, and perform threshold judgment and allocation on the estimated motion pixels: Set a threshold θ of the judgment condition to determine whether the optical flow vector exceeds the range of this threshold:

[0039] Condition(x,y,z)=||v(x,y,z)||>θ

[0040] Then, based on the result of the above threshold judgment, a function is defined to allocate pixel conditions;

[0041]

[0042] When the condition V(x,y,z)>0 is true, the pixel is marked as dynamic 1; otherwise, it is marked as static 0;

[0043] Use the Region Growing algorithm to divide the pixels after the threshold judgment into regions: perform similarity calibration on the optical flow vectors after the previous step, compare the differences between the optical flow vectors, and consider spatial proximity:

[0044] Similarity((x,y,z),(x′,y′,z′))=||v(x,y,z)-v(x′,y′,z′)|| <tolerance

[0045] Where (x′, y′, z′) is the neighboring point of (x, y, z) in three-dimensional space, and tolerance is the preset tolerance;

[0046] Starting from the initial optical flow vector of each area, the adjacent points that meet the similarity criterion are added to the current dynamic area to complete the division of the dynamic area and the static area.

[0047] GrowRegion(region,(x,y,z))=U all(x′,y′,z′)around(x,y,z)satisfyingsimilarity {(x′,y′,z′)}

[0048] In the formula, region is the dynamic region that has been identified, and Similarity is the set judgment criterion, which defines under what circumstances an adjacent point (x′, y′, z′) and the vector center point (x, y, z) can be judged to be similar.

[0049] Furthermore, the obstacle fusion and positioning specifically include:

[0050] Use LiDAR to scan the environment to generate point cloud data and obtain a series of 3D points of the point cloud data:

[0051] Downsampling the point cloud data using a voxel grid reduces the point cloud data to a more manageable size.

[0052]

[0053] Where V i is the set of points i in the voxel grid, p i is the representative point of voxel grid i;

[0054] Use statistical filters to remove outliers, calculate the mean distance between each point and its k nearest neighbors, and remove points whose mean distance is greater than a certain threshold.

[0055]

[0056] In the formula, It's point p i The k neighboring point sets of ;

[0057] The processed point cloud is projected onto a two-dimensional plane to construct a grid map:

[0058] Project the point cloud data onto a two-dimensional plane to achieve (x,y,z)->(x,y) conversion and remove the influence of the z-axis;

[0059] The two-dimensional plane is divided into fixed-size grids, and each grid records the number and height information of the points it contains.

[0060] G(i,j)={(x,y)∣x∈[i·s,(i+1)·s),y∈[j·s,(j+1)·s)}

[0061] Where s is the grid size, G(i,j) is the constructed grid coordinate;

[0062] At the same time, a two-dimensional height map is constructed, and the height of each grid is set to the maximum height of the points it contains.

[0063] H(i,j)=max{z∣(x,y,z)∈G(i,j)}

[0064] Where H(i,j) is the coordinate of the constructed two-dimensional height map;

[0065] Use the energy function to completely construct the obstacle point cloud data and refine the difference between the background and obstacles.

[0066] E data (p) = -log P(O|p)

[0067] In the formula, E data is the energy of the obstacle data item, P(O|p) represents the probability that point p belongs to an obstacle;

[0068] Construct the smoothing energy of obstacles for subsequent fusion processing.

[0069] E smooth (p,q)=λ·δ(O p ≠O q )

[0070] In the formula, E smooth is the smoothing term energy, λ is the smoothing weight, δ is the indicator function, if O p ≠O q , then δ is 1, otherwise δ is 0;

[0071] Construct the total energy function,

[0072]

[0073] Where N is the adjacent point pair in the point cloud data;

[0074] Integrate point cloud processing and optimized data to build more detailed obstacle information;

[0075] Processing raster obstacle probabilities extracted from lidar,

[0076]

[0077] Process the obstacle pixel probability extracted by the depth camera,

[0078]

[0079] By integrating the obstacle probabilities of the two, we can obtain the joint distribution probability of obstacles.

[0080] (O|p) = P LiDAR (O|p)·P Depth (O|p)

[0081] Through joint probability, more accurate and complete obstacle information can be obtained;

[0082] The energy function E after sensor data fusion is minimized by the graph cut algorithm to segment obstacles and the environment:

[0083] E(O)=E data (O)+λE smooth (O)

[0084] In the formula, the data item E data (O) measures the matching degree between the segmentation result and the observed data, and the smoothing term E smooth (O) measures the smoothness of the segmentation result, and λ is the weight coefficient of the smoothness term;

[0085] The data items are,

[0086]

[0087] Where P(O|p) is the joint probability of obstacles calculated in the previous step;

[0088] The smoothing term is,

[0089]

[0090] Where N is the set of pixel and point cloud pairs;

[0091] By minimizing the above energy function, the optimal segmentation result is found using the maximum flow - minimum cut algorithm. Define the weight for each edge (p, q).

[0092]

[0093] where I(p) and I(q) respectively represent the fused intensity values, and σ is a parameter controlling the weight;

[0094] Through the result of the maximum flow, the corresponding minimum cut is obtained, and the constructed point cloud map is segmented into obstacle and background information.

[0095] Furthermore, the feature matching and loop detection specifically include:

[0096] Perform image corner judgment and extract corner information:

[0097]

[0098] where I is the given image, p is the pixel point, and define its neighborhood pixel point set as {p1, p2, …, p 16}, and set the threshold as t;

[0099] If I(p i ) > I(p) + t or I(p i ) < I(p) - t, then p is considered a corner point;

[0100] Use the ORB feature point extraction algorithm to extract feature points:

[0101] Use the FAST algorithm to detect key points and obtain the initial set of key points {k1, k2, …, k n},

[0102] k i = FAST(I)

[0103] Use the Harris corner measure to score the key points detected by FAST,

[0104] H(p) = det(M) - κ·(trace(M)) 2

[0105] where M is the autocorrelation matrix and κ is a constant;

[0106] Then calculate its direction according to the local image patch of the key point,

[0107]

[0108] Finally, BRIEF descriptor generation is used. Each descriptor generates a binary string by comparing random comparison point pairs around the key point.

[0109] d i =BRIEF(I,k i ,θ i )

[0110] In the formula, θ i is the key point k i direction;

[0111] Filter and match the feature points between adjacent frames, and use Low's ratio test to improve the RANSAC algorithm to accurately match the feature points:

[0112] Calculate the Hamming distance between descriptors and use the nearest neighbor algorithm for matching.

[0113]

[0114] Use the Low's ratio test method to filter out false matches.

[0115]

[0116] Where, d j1 and d j2 is with d i The two descriptors closest to each other, τ is the set threshold;

[0117] Use the RANSAC algorithm to estimate the geometric transformation between feature points and further filter out false matches:

[0118] Randomly select a set of feature point pairs for estimating the transformation model, and estimate the transformation model based on the selected feature point pairs.

[0119] H=estimate(k i ,k j )

[0120] Where H is the homography matrix of the estimated model;

[0121] Then calculate the projection error of all feature point pairs and find the internal point set.

[0122] error(k i ,k j )=||k j -Hk i ||

[0123] inliers={(k i ,k j)|error(k i ,k j )<∈}

[0124] In the formula, ∈ is the set threshold, error(k i ,k j ) is the projection error, inliers is the set of interior points under the projection error;

[0125] Finally, the transformation model is re-estimated using the interior point set, and the above process is iterated until the optimal model is found;

[0126] When the number of matching feature points between two frames exceeds the set threshold, that is, when the condition if|inliers|>threshold is met, it is considered as a closed loop detection, and the closed loop constraint is incorporated into the graph optimization to correct drift and enhance the consistency of the map;

[0127] Incorporate loop closure constraints into graph optimization, optimize the global pose graph by minimizing the observation error between nodes, and add the constraints of detected loop closures to the optimization objective function.

[0128]

[0129] Where ε is the set of common edges, is the set of closed loop edges, T loop is the relative pose obtained by closed-loop detection, Ω loop is the information matrix of the closed-loop constraint.

[0130] Furthermore, the map update and maintenance specifically includes:

[0131] Use cross-update to incrementally update the map.

[0132] Map new =Map old ∪Data new

[0133] In the formula, Map old It is the existing map data of the previous stage. new New map data updated in real time

[0134] The local map is optimized through iterative optimization.

[0135]

[0136] Where, X local is the location point of the local map, C local is the depth camera sensor parameter;

[0137] Then the global map is iteratively optimized.

[0138]

[0139] Where, X i is a point on the map, p ij is the i-th point observed in fixed image j.

[0140] According to a second aspect, the present invention provides a three-dimensional dense point cloud mapping system integrating a depth camera with a laser radar, the system comprising:

[0141] The time synchronization and spatial calibration module is used to ensure the time consistency of the lidar and depth camera data. It synchronizes the sensor clock through the network time protocol, obtains the original timestamp and marks the capture time of each frame of data. At the same time, the spatial parameters of the sensor are adjusted through the calibration tool to ensure the consistency of the relative position and posture of the sensor.

[0142] Point cloud processing and optimization module, used to process the depth image collected by the depth camera, extract the three-dimensional dense point cloud and its corresponding coordinate information, and remove noise and redundant points through Gaussian filtering algorithm to improve the accuracy of point cloud data;

[0143] A dynamic object recognition module is used to recognize dynamic objects in image sequences using an optical flow method, calculate the motion vector of each pixel, and mark dynamic areas by setting a speed threshold;

[0144] The obstacle fusion and positioning module is used to fuse the obstacle information identified by the lidar with the data of the depth camera to generate a two-dimensional grid map to achieve accurate positioning and segmentation of obstacles and dynamic objects;

[0145] Data fusion optimization module, which is used to optimize multi-sensor data fusion using the extended Kalman filter method, reduce noise and errors, and improve the quality and accuracy of the final map;

[0146] Feature matching and loop detection module, which is used to perform loop detection through feature point matching, correct drift and improve map consistency during map optimization;

[0147] The map update and maintenance module is used to incrementally update and maintain the constructed three-dimensional dense point cloud map to ensure that the map continues to reflect changes in the real environment.

[0148] According to a third aspect, the present invention provides an electronic device, the device comprising: a processor and a memory;

[0149] The memory is used to store one or more program instructions;

[0150] The processor is used to run one or more program instructions to execute the steps of the three-dimensional dense point cloud mapping method of depth camera fusion lidar as described in the first aspect.

[0151] According to a fourth aspect, the present invention provides a computer-readable storage medium having a computer program stored thereon, and when the computer program is executed by a processor, the steps of the three-dimensional dense point cloud mapping method of the depth camera fused with lidar as described in the first aspect are implemented.

[0152] The method of the present invention uses a method that integrates the data of laser radar and depth camera to adapt to complex indoor and outdoor environments. Through the improved optical flow method dynamic detection technology, the system can identify and exclude moving objects in real time, significantly improve the accuracy of the map and reduce misleading information; in addition, the method uses an extended Kalman filter (EKF) to optimize the time synchronization and error correction process, solves the data inconsistency problem caused by the accumulated error of the sensor, and thus ensures the high accuracy of data fusion; in terms of feature extraction, the system uses the ORB (Oriented FAST and Rotated BRIEF) algorithm to intelligently extract image feature points; at the same time, in order to improve the accuracy of feature matching, the system improves the RANSAC algorithm and introduces the Low's ratio test. This method determines the effectiveness of the match by comparing the ratio of the optimal match to the suboptimal match, effectively reduces false matches, and improves the robustness of the matching process. BRIEF DESCRIPTION OF THE DRAWINGS

[0153] Figure 1 An overall structural block diagram of an algorithm provided for one embodiment of the present invention;

[0154] Figure 2 A flowchart of visual dynamic object segmentation provided by one embodiment of the present invention;

[0155] Figure 3 A flow chart of performing obstacle and environmental background segmentation by sensor fusion provided in one embodiment of the present invention;

[0156] Figure 4 A flow chart of feature point matching and loop detection using an improved RANSAC algorithm provided in one embodiment of the present invention;

[0157] Figure 5 A diagram of the software and hardware structure provided for one embodiment of the present invention.

[0158] Figure 6 The figure is a schematic diagram of the physical structure of an electronic device according to an embodiment of the present invention. DETAILED DESCRIPTION

[0159] The present invention is further described in detail below by specific embodiments in conjunction with the accompanying drawings. Wherein similar elements in different embodiments adopt associated similar element numbers. In the following embodiments, many detailed descriptions are for making the present application better understood. However, those skilled in the art can easily recognize that some features can be omitted in different situations, or can be replaced by other elements, materials, methods. In some cases, some operations related to the present application are not shown or described in the specification, this is to avoid the core part of the present application being overwhelmed by too much description, and for those skilled in the art, it is not necessary to describe these related operations in detail, and they can fully understand the related operations according to the description in the specification and the general technical knowledge in the art.

[0160] In addition, the features, operations or characteristics described in the specification can be combined in any appropriate manner to form various implementations. At the same time, the steps or actions in the method description can also be interchanged or adjusted in a manner that is obvious to those skilled in the art. Therefore, the various sequences in the specification and the drawings are only for the purpose of clearly describing a certain embodiment and are not meant to be a required sequence, unless otherwise specified that a certain sequence must be followed.

[0161] First, the basic idea of ​​the present invention is introduced:

[0162] like Figure 1 As shown, the present invention provides a three-dimensional dense point cloud mapping method by integrating a depth camera with a laser radar, which is mainly implemented by the following steps: Figure 2 As shown in FIG, first, the depth image collected by the depth camera is processed using a three-dimensional optical flow algorithm to estimate the optical flow velocity and posture to generate high-precision three-dimensional dense point cloud data, and by analyzing the point cloud optical flow, the dynamic and static point clouds can be accurately distinguished to facilitate subsequent data processing; then, as shown in FIG. Figure 3 As shown in the figure, the obstacle point cloud information of the surrounding environment is collected by LiDAR to generate a two-dimensional grid map, and the obstacle and background data are classified by energy function. At the same time, the dynamic data of the depth camera is integrated to achieve accurate segmentation of obstacles and background in the two-dimensional plane, and further improve the map construction; finally, as shown in the figure Figure 4 As shown in the figure, an improved RANSAC algorithm is used in combination with the Low's ratio test method to match the image feature points of different frames to complete the closed-loop detection. The constructed map is corrected and optimized as a whole through graph optimization technology to improve the accuracy and consistency of the map.

[0163] The following is a specific embodiment, combined with Figure 1-Figure 5 , the detailed process of the present invention is specifically described:

[0164] A three-dimensional dense point cloud mapping method using a depth camera and laser radar, comprising the following steps:

[0165] Step S1: perform time stamp calibration on the depth camera and lidar, and then minimize the spatial error between the two to perform spatial calibration.

[0166] Use the NTP time protocol to synchronize timestamps, as shown below:

[0167]

[0168] Where, t sync is the time after synchronization, t L is the lidar timestamp, t D is the depth camera timestamp.

[0169] Then use the system internal clock error to compensate t sync =t L +δ L -δ D , eliminate the influence of clock error on time,δ L is the lidar clock error, δ D is the depth camera clock error. Then use the linear regression model to predict the time difference, t sync =t L +β0+β1t D , β0 and β1 are regression coefficients obtained by fitting historical time data to ensure that the clock error is eliminated to the greatest extent.

[0170] Use the least squares method to solve for the rotation and translation matrices: Get the rotation matrix R and the translation matrix t, and finally use the SDV method to decompose and calculate the best rotation and translation from the two sets of points, so that one set can be as close to the other as possible: R, t = SVD (p L ,p D ) for spatial calibration.

[0171] Step S2: Use the depth image extracted by the depth camera, combined with the camera's internal parameters and distortion parameters, to obtain detailed data and position of each pixel. The value of each pixel represents the distance from the point to the camera, usually in millimeters; convert the pixel points into point cloud information in the 3D point cloud map, including the focal length (f x ,f y ) and the image center (c x ,c y ); These parameters are critical for converting points in the image coordinate system into the three-dimensional space coordinate system; for each pixel point (u, v) and its depth value D(u, v) in the depth map, its coordinates in the three-dimensional space are calculated by the following formula:

[0172] The Z axis coordinate is: Z = D (u, v)

[0173] The X-axis coordinates are:

[0174] The Y-axis coordinates are:

[0175] In the above formula, u, v are the coordinates of the pixel in the image, and D(u, v) is the depth value obtained from the depth image; these 3D coordinates are the points in the point cloud. By adding color information to the point cloud, the color values ​​(R, G, B) can be obtained from the RGB image corresponding to the depth data position, and these color values ​​can be associated with the corresponding 3D points.

[0176] After conversion to a three-dimensional point cloud, different areas are divided and denoised to achieve an average effect; the denoising filter algorithm is implemented. In this embodiment, Gaussian filtering is used to filter the point cloud data projected into the three-dimensional space in the depth image. The new position of each point is the weighted average of the positions of its neighboring points, and the weight is determined by the Gaussian function; for each point p in the point cloud i , its filtered position It can be calculated as follows:

[0177]

[0178] Where N(i) is the point p i The neighborhood of p j is a point in the neighborhood, w ij are weights based on Gaussian functions.

[0179] w ij Given by:

[0180]

[0181] In the formula, point (p i ,p j ) is p i and p j The Euclidean distance between them, σ is the pre-set standard deviation.

[0182] The point cloud after Gaussian filtering through the above steps has a smoother surface, and the point cloud noise can be effectively suppressed.

[0183] Step S3: Use the optical flow method to estimate the motion of the pixel points, and perform three-dimensional optical flow estimation on the pixel points extracted to the specific position to obtain a three-dimensional optical flow vector V(x, y) = P t+1 (x,y)-P t (x,y).

[0184] Perform velocity estimation on the optical flow vectors to obtain Set a threshold θ for the decision condition to determine whether the optical flow vectors exceed the range of this threshold. If they exceed, they are determined to be dynamic pixel points. Use the Region Growing algorithm to divide the range of the dynamic points and calibrate the divided dynamic range: Similarity((x,y,z),(x′,y′,z′))=||v(x,y,z)-v(x′,y′,z′)||<tolerance to complete the recognition and division of the dynamic region.

[0185] Step S4: Use a lidar to scan the environment to generate point cloud data, and obtain the point cloud data. Downsample the point cloud data: Then use a statistical filter to remove outliers.

[0186] Project the point cloud onto a two-dimensional plane: G(i,j)={(x,y)∣x∈[i·s,(i+1)·s),y∈[j·s,(j+1)·s)} to construct a two-dimensional grid map.

[0187] Calculate the two-dimensional grid obstacles through an energy function to obtain detailed obstacle information: E data (p)=-log P(O∣p), and perform smoothing processing.

[0188] Finally, fuse the dynamic information recognized by the depth camera to segment the dynamic objects and obstacles in the environment and complete the construction of the two-dimensional map.

[0189] Step S5: Establish the state transition equation of the lidar and record the change in state information from time k to time k+1:

[0190]

[0191] In the formula, v k is the velocity during movement, ω k is the angular velocity.

[0192] Predict the movements of the depth camera and the lidar through the EKF, F t is the state transition matrix, B t is the control input matrix, u t is the control vector.

[0193] Then update the motion state: K t is the Kalman gain, R t is the observation model noise, H t is the observation model matrix, z t is the updated observation vector.

[0194] Finally, the error covariance matrix is ​​updated: t+1 =(IK t H t ) t ; To complete the optimization and filtering of sensor data.

[0195] Step S6: To complete the loop detection of the overall map, we first need to determine the image corners and extract the corner information, and then use the FAST method to extract the feature points: k i = FAST(I), use Harris corner measure to score the key points detected by FAST: H(p) = det(M)-κ·(trace(M)) 2 , and finally use BRIEF to generate the descriptor: i =BRIEF(I,k i ,θ i ).

[0196] Continue with the above steps, filter and match the feature points between adjacent frames, use Low's ratiotest to improve the RANSAC algorithm, calculate the Hamming distance between descriptors, and use the nearest neighbor algorithm for matching: Use the Low's ratio test method to filter out false matches.

[0197] Finally, the closed-loop constraints are incorporated into the overall graph optimization, and the global pose graph is optimized by minimizing the observation error between nodes.

[0198] Step S7: Ensure that the map continues to reflect changes in the real environment and maintain the accuracy and practicality of the map data, and incrementally update the constructed map:

[0199] Map new =Map old ∪Data new

[0200] In the formula, Map old It is the existing map data of the previous stage. new It is new map data that is updated in real time. The new data is merged into the existing map and the corresponding area is updated.

[0201] Then optimize the local map obtained during the update process:

[0202]

[0203] Where, X local is the location point of the local map, C localThis method optimizes local areas of the map for depth camera sensor parameters to improve the accuracy and consistency of local features.

[0204] By iteratively maintaining the local map, a complete global map can be obtained and optimized:

[0205]

[0206] Where, X i is a point on the map, p ij is the i-th point observed in a fixed image j, and the map data is globally optimized by minimizing the total reprojection error in all observations to improve the overall accuracy and consistency of the map. After completing the above steps, check the validity of the map data to ensure that all data meet the preset quality and accuracy standards to complete the construction of the overall 3D dense point cloud map.

[0207] Finally, if Figure 5 The embodiment of the present invention shown in the figure uses a 4-wheeled Mecanum wheel car as the chassis, and the lower computer uses a Raspberry Pi 4b. The ROS melodic version of the robot operating system is installed in the lower computer, and the data information of the depth camera and the lidar is received at the same time. The upper computer also uses the Ubuntu 18.04 system, and uses rviz as the visualization module to observe the positioning and mapping effects in real time.

[0208] In summary, the present invention provides a three-dimensional dense point cloud mapping method and system for a depth camera fused with a laser radar. The algorithm simultaneously constructs a two-dimensional grid map and a three-dimensional dense point cloud map, which can better obtain environmental and obstacle information, including the following steps: achieving precise synchronization through sensor timestamp alignment and spatial calibration; processing the depth image collected by the depth camera, calculating the three-dimensional dense point cloud and optimizing the data through a Gaussian filtering algorithm; identifying dynamic objects in the marked image sequence, estimating the optical flow vector of the pixel point using an optical flow method, and marking the dynamic objects by setting a speed threshold; combining the laser radar with obstacle recognition, constructing the point cloud data into a two-dimensional grid map, using an energy function to obtain obstacle point cloud data items, fusing the depth camera data to obtain segmented background and obstacle information, and then using an extended Kalman filter (EKF) to reduce noise and errors in multi-sensor data fusion; performing loop detection through feature point matching, extracting feature point data from the depth camera, introducing a Low's ratio test method to improve the RANSAC algorithm to accurately pair feature points, incorporating loop constraints into graph optimization when a loop is detected, and correcting drift enhancement. Figure 1 consistency; finally, the 3D dense point cloud map is incrementally updated and globally optimized to build a more complete and robust dense point cloud map.

[0209] The embodiment of the present invention also provides an electronic device for a method for constructing a three-dimensional dense point cloud map by integrating a depth camera with a laser radar. Figure 6 A schematic diagram of the physical structure of an electronic device provided in an embodiment of the present invention, the electronic device may include: a processor 301, a communications interface 302, a memory 303 and a bus 304, wherein the processor 301, the communications interface 302, and the memory 303 communicate with each other through the bus 304. The processor 301 can call a computer program stored in the memory 303 and can be run on the processor 301 to execute the positioning and mapping method based on the fusion of a single-line laser radar and a depth camera provided in the above embodiment. For example, it includes: time synchronization and spatial calibration: ensuring the consistency of the data time of the laser radar and the depth camera, synchronizing the sensor clock through the network time protocol, obtaining the original timestamp and marking the capture time of each frame of data, and adjusting the spatial parameters of the sensor through the calibration tool to ensure the consistency of the relative position and posture of the sensor; point cloud processing and optimization: processing the depth image collected by the depth camera, extracting the three-dimensional dense point cloud and its corresponding coordinate information, and removing noise and redundant points through the Gaussian filtering algorithm to improve the accuracy of the point cloud data; dynamic object recognition: using the optical flow method to identify dynamic objects in the image sequence, calculating the motion vector of each pixel, and through Set speed thresholds to mark dynamic areas; obstacle fusion and positioning: fuse obstacle information identified by the lidar with the data from the depth camera to generate a two-dimensional grid map to achieve accurate positioning and segmentation of obstacles and dynamic objects; data fusion optimization: use the extended Kalman filter method to optimize multi-sensor data fusion, reduce noise and errors, and improve the quality and accuracy of the final map; feature matching and loop detection: perform loop detection through feature point matching, correct drift and improve map consistency during map optimization; map update and maintenance: incrementally update and maintain the constructed three-dimensional dense point cloud map to ensure that the map continues to reflect changes in the real environment.

[0210] In addition, the logic instructions in the above-mentioned memory 303 can be implemented in the form of a software functional unit and can be stored in a computer-readable storage medium when it is sold or used as an independent product. Based on such an understanding, the technical solution of the embodiment of the present invention is essentially or the part that contributes to the prior art or the part of the technical solution can be embodied in the form of a software product, and the computer software product is stored in a storage medium, including several instructions to enable a computer device (which can be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the method described in each embodiment of the present invention. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), disk or optical disk and other media that can store program codes.

[0211] The above specific examples are used to illustrate the present invention, which are only used to help understand the present invention and are not used to limit the present invention. For those skilled in the art to which the present invention belongs, some simple deductions, modifications or substitutions can be made according to the idea of ​​the present invention.

Claims

1. A three-dimensional dense point cloud mapping method combining a depth camera with a laser radar, characterized in that: include: Time synchronization and spatial calibration: Ensure the time consistency of the lidar and depth camera data, synchronize the sensor clock through the network time protocol, obtain the original timestamp and mark the capture time of each frame of data, and adjust the spatial parameters of the sensor through the calibration tool to ensure the consistency of the relative position and posture of the sensor; Point cloud processing and optimization: Process the depth image collected by the depth camera, extract the three-dimensional dense point cloud and its corresponding coordinate information, and use the Gaussian filtering algorithm to remove noise and redundant points to improve the accuracy of the point cloud data; Dynamic object recognition: Use the optical flow method to identify dynamic objects in image sequences, calculate the motion vector of each pixel, and mark the dynamic area by setting the speed threshold; Obstacle fusion and positioning: The obstacle information identified by the lidar is integrated with the data of the depth camera to generate a two-dimensional grid map to achieve accurate positioning and segmentation of obstacles and dynamic objects; Data fusion optimization: Use the extended Kalman filter method to optimize multi-sensor data fusion, reduce noise and errors, and improve the quality and accuracy of the final map; Feature matching and loop detection: loop detection is performed through feature point matching to correct drift and improve map consistency during map optimization; Map update and maintenance: Incrementally update and maintain the constructed three-dimensional dense point cloud map to ensure that the map continues to reflect changes in the real environment.

2. The three-dimensional dense point cloud mapping method of depth camera fusion laser radar according to claim 1 is characterized in that: The time synchronization and calibration specifically include: Use the NTP time protocol to synchronize timestamps, as shown below: Where, t sync is the time after synchronization, t L is the lidar timestamp, t D is the depth camera timestamp; Then use the system internal clock error to compensate t sync =t L +δ L -δ D , eliminate the influence of clock error on time,δ L is the laser radar clock error, δ D is the depth camera clock error; the linear regression model is used to predict the time difference, t sync =t L +β0+β1t D , β0 and β1 are regression coefficients obtained by fitting historical time data to ensure that the clock error is eliminated to the greatest extent; Use the least squares method to solve for the rotation and translation matrices: Get the rotation matrix R and the translation matrix t, and finally use the SDV method to decompose and calculate the best rotation and translation from the two sets of points, so that one set can be as close to the other as possible: R, t = SVD (p L ,p D ) for spatial calibration.

3. The three-dimensional dense point cloud mapping method of depth camera fusion laser radar according to claim 1 is characterized in that: The point cloud processing and optimization specifically includes: The pixels in the depth image are converted into a three-dimensional point cloud in combination with the depth information, and their coordinates p(X, Y, Z) in the three-dimensional space are calculated: The Z axis coordinate is: Z = D (u, v) The Y-axis coordinates are: Where u, v are the coordinates of the pixel in the depth image, D(u, v) is the depth value obtained from the depth image, (f x ,f y ) is the focal length of the image, (c x ,c y ) is the image center; Then use Gaussian filtering to filter the point cloud data projected into the three-dimensional space in the depth image, and use Gaussian function to determine the weights of different point clouds: In the formula, w ij is the weight based on the Gaussian function, point (p i ,p j ) is p i and p j The Euclidean distance between them, σ is the pre-set standard deviation; Then confirm the position of each point cloud after filtering by weight: It's point p i The neighborhood of p j is a point in the neighborhood.

4. The three-dimensional dense point cloud mapping method of depth camera fusion laser radar according to claim 1 is characterized in that: The dynamic object recognition specifically includes: Estimate the three-dimensional position of the pixel points in the image captured by the depth camera: obtain the depth value of each pixel point from the depth camera and convert it into a position in the three-dimensional coordinate system. If the position of the pixel point in the image is (x, y), the depth is Z(x, y), and the intrinsic parameter matrix of the camera is K, then the three-dimensional coordinate P(x, y) can be expressed as: In the formula, K -1 is the inverse matrix of the camera intrinsic parameter matrix; Perform 3D optical flow estimation on the pixels extracted to specific locations: Use the 3D position data of continuous frames to calculate the 3D optical flow vector of each point: V(x,y)=P t+1 (x,y)-P t (x,y) Where P t (x, y) is the three-dimensional position of the pixel at time t, P t+1 (x, y) is the three-dimensional position at time t+1, V(x, y) is the three-dimensional optical flow vector; Perform three-dimensional velocity estimation on the optical flow vector of the pixel: Where V x , V y , V z They are the components of the optical flow vector in three-dimensional space; Set the speed threshold, and perform threshold judgment and allocation on the estimated motion pixels: Set a threshold θ of the judgment condition to determine whether the optical flow vector exceeds the range of this threshold: Condition(x,y,z)=||v(x,y,z)||>θ Then, based on the result of the above threshold judgment, a function is defined to allocate pixel conditions; When the condition V(x,y,z)>0 is true, the pixel is marked as dynamic 1; otherwise, it is marked as static 0; Use the Region Growing algorithm to divide the pixels after the threshold judgment into regions: perform similarity calibration on the optical flow vectors after the previous step, compare the differences between the optical flow vectors, and consider spatial proximity: Similarity((x,y,z),(x′,y′,z′))=||v(x,y,z)-v(x′,y′,z′)|| <tolerance Where (x′, y′, z′) is the neighboring point of (x, y, z) in three-dimensional space, and tolerance is the preset tolerance; Starting from the initial optical flow vector of each area, the adjacent points that meet the similarity criterion are added to the current dynamic area to complete the division of the dynamic area and the static area; GrowRegion(region,(x,y,z))=U all(x′,y′,z′)around(x,y,z)satisfying Similarity {(x′,y′,z′)} In the formula, region is the dynamic region that has been identified, and Similarity is the set judgment criterion, which defines under what circumstances an adjacent point (x′, y′, z′) and the vector center point (x, y, z) can be judged to be similar.

5. The three-dimensional dense point cloud mapping method of depth camera fusion laser radar according to claim 1 is characterized in that: The obstacle fusion and positioning specifically include: Use LiDAR to scan the environment to generate point cloud data and obtain a series of 3D points of the point cloud data: Downsampling the point cloud data using a voxel grid reduces the point cloud data to a more manageable size. Where V i is the set of points i in the voxel grid, p i is the representative point of voxel grid i; Use statistical filters to remove outliers, calculate the mean distance between each point and its k nearest neighbors, and remove points whose mean distance is greater than a certain threshold. In the formula, It's point p i The k neighboring point sets of ; The processed point cloud is projected onto a two-dimensional plane to construct a grid map: Project the point cloud data onto a two-dimensional plane to achieve (x,y,z)->(x,y) conversion and remove the influence of the z-axis; The two-dimensional plane is divided into fixed-size grids, and each grid records the number and height information of the points it contains. G(i,j)={(x,y)∣x∈[i·s,(i+1)·s),y∈[j·s,(j+1)·s)} Where s is the grid size, G(i,j) is the constructed grid coordinate; At the same time, a two-dimensional height map is constructed, and the height of each grid is set to the maximum height of the points it contains. H(i,j)=max{z∣(x,y,z)∈G(i,j)} Where H(i,j) is the coordinate of the constructed two-dimensional height map; Use the energy function to completely construct the obstacle point cloud data and refine the difference between the background and obstacles. E data (p)=-logP(O∣p) In the formula, E data is the energy of the obstacle data item, P(O|p) represents the probability that point p belongs to an obstacle; Construct the smoothing energy of obstacles for subsequent fusion processing. E smooth (p,q)=λ·δ(O p ≠O q ) In the formula, E smooth is the smoothing term energy, λ is the smoothing weight, δ is the indicator function, if O p ≠O q , then δ is 1, otherwise δ is 0; Construct the total energy function, Where N is the adjacent point pair in the point cloud data; Integrate point cloud processing and optimized data to build more detailed obstacle information; Processing raster obstacle probabilities extracted from lidar, Process the obstacle pixel probability extracted by the depth camera, By integrating the obstacle probabilities of the two, we can obtain the joint distribution probability of obstacles. (O∣p)=P LiDAR (O∣p)·P Depth (O∣p) Through joint probability, more accurate and complete obstacle information can be obtained; The energy function E after sensor data fusion is minimized by the graph cut algorithm to segment obstacles and the environment: E(O)=E data (O)+λE smooth (THE) In the formula, the data item E data (O) measures the matching degree between the segmentation result and the observed data, and the smoothing term E smooth (O) measures the smoothness of the segmentation result, and λ is the weight coefficient of the smoothness term; The data items are, Where P(O|p) is the joint probability of obstacles calculated in the previous step; The smoothing term is, Where N is the set of pixel and point cloud pairs; By minimizing the above energy function, the maximum flow-minimum cut algorithm is used to find the optimal segmentation result, and the weight is defined for each edge (p, q). In the formula, I(p) and I(q) represent the intensity values ​​after fusion, and σ is the parameter controlling the weight; Through the result of the maximum flow, the corresponding minimum cut is obtained, and the constructed point cloud image is divided into obstacles and background information.

6. The three-dimensional dense point cloud mapping method of depth camera fusion laser radar according to claim 1 is characterized in that: The feature matching and loop detection specifically include: Perform image corner point judgment and extract corner point information: Where I is a given image, p is a pixel, and its neighborhood pixel set is defined as {p1, p2, …, p 16 }, set the threshold to t; If I(p i ) > I(p) + t or I(p i ) < I(p) - t, then p is considered a corner point; Use ORB feature point extraction algorithm to extract feature points: Use the FAST algorithm to detect key points and get the initial key point set {k1, k2, ..., k n }, k i =FAST(I) Harris corner measure is used to score the key points detected by FAST. H(p)=det(M)-κ·(trace(M)) 2 Where M is the autocorrelation matrix and κ is a constant; Then the direction of the key point is calculated based on the local image block. Finally, BRIEF descriptor generation is used. Each descriptor generates a binary string by comparing random comparison point pairs around the key point. d i =BRIEF(I,k i ,θ i ) In the formula, θ i is the key point k i direction; Filter and match the feature points between adjacent frames, and use Low's ratio test to improve the RANSAC algorithm to accurately match the feature points: Calculate the Hamming distance between descriptors and use the nearest neighbor algorithm for matching. Use the Low's ratio test method to filter out false matches. Where, d j1 and d j2 is with d i The two descriptors closest to each other, τ is the set threshold; Use the RANSAC algorithm to estimate the geometric transformation between feature points and further filter out false matches: Randomly select a set of feature point pairs for estimating the transformation model, and estimate the transformation model based on the selected feature point pairs. H=estimate(k i ,k j ) Where H is the homography matrix of the estimated model; Then calculate the projection error of all feature point pairs and find the internal point set. error(k i ,k j )=||k j -Hk i || inliers={(k i ,k j )∣error(k i ,k j )<∈} In the formula, ∈ is the set threshold, error(k i ,k j ) is the projection error, inliers is the set of interior points under the projection error; Finally, the transformation model is re-estimated using the interior point set, and the above process is iterated until the optimal model is found; When the number of matching feature points between two frames exceeds the set threshold, that is, when the condition if|inliers|>threshold is met, it is considered as a closed loop detection, and the closed loop constraint is incorporated into the graph optimization to correct drift and enhance the consistency of the map; Incorporate loop closure constraints into graph optimization, optimize the global pose graph by minimizing the observation error between nodes, and add the constraints of detected loop closures to the optimization objective function. Where ε is the set of common edges, is the set of closed loop edges, T loop is the relative pose obtained by closed-loop detection, Ω loop is the information matrix of the closed-loop constraint.

7. The three-dimensional dense point cloud mapping method of depth camera fusion laser radar according to claim 1 is characterized in that: The map update and maintenance specifically include: Use cross-update to incrementally update the map. Map new =Map old ∪Data new In the formula, Map old It is the existing map data of the previous stage. new New map data updated in real time The local map is optimized through iterative optimization. In the formula, X local is the location point of the local map, C local is the depth camera sensor parameter; Then the global map is iteratively optimized. In the formula, X i is a point on the map, p ij is the i-th point observed in fixed image j.

8. A three-dimensional dense point cloud mapping system integrating a depth camera and a laser radar, characterized in that: include: The time synchronization and spatial calibration module is used to ensure the time consistency of the lidar and depth camera data. It synchronizes the sensor clock through the network time protocol, obtains the original timestamp and marks the capture time of each frame of data. At the same time, the spatial parameters of the sensor are adjusted through the calibration tool to ensure the consistency of the relative position and posture of the sensor. Point cloud processing and optimization module, used to process the depth image collected by the depth camera, extract the three-dimensional dense point cloud and its corresponding coordinate information, and remove noise and redundant points through Gaussian filtering algorithm to improve the accuracy of point cloud data; A dynamic object recognition module is used to recognize dynamic objects in image sequences using an optical flow method, calculate the motion vector of each pixel, and mark dynamic areas by setting a speed threshold; The obstacle fusion and positioning module is used to fuse the obstacle information identified by the lidar with the data of the depth camera to generate a two-dimensional grid map to achieve accurate positioning and segmentation of obstacles and dynamic objects; Data fusion optimization module, which is used to optimize multi-sensor data fusion using the extended Kalman filter method, reduce noise and errors, and improve the quality and accuracy of the final map; Feature matching and loop detection module, which is used to perform loop detection through feature point matching, correct drift and improve map consistency during map optimization; The map update and maintenance module is used to incrementally update and maintain the constructed three-dimensional dense point cloud map to ensure that the map continues to reflect changes in the real environment.

9. An electronic device, characterized in that: The device comprises: a processor and a memory; The memory is used to store one or more program instructions; The processor is used to run one or more program instructions to execute the steps of the three-dimensional dense point cloud mapping method of depth camera fusion lidar as described in any one of claims 1 to 7.

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program, which, when executed by a processor, implements the steps of the three-dimensional dense point cloud mapping method of depth camera fusion lidar as described in any one of claims 1 to 7.

Images