System and method for heterogeneous sensing information fusion and point cloud registration in man-machine co-fusion environment

Abstract

The invention discloses a system and a method for heterogeneous sensing information fusion and point cloud registration in a man-machine co-fusion environment, and aims to solve the problems that point cloud densities between a depth camera and a laser radar are different, range scales are obviously different, point cloud arrangement rules are completely different, and sensing information fusion faces challenges. Through sliding ball point cloud coarse positioning, coarse registration based on an improved FPFH algorithm of a coincidence ratio, and fine registration based on an improved ICP algorithm generated based on corresponding points, the problems of point cloud range scale difference and density difference are solved, sensing information accurate fusion and point cloud registration between a depth camera and a laser radar in a man-machine co-fusion environment are realized, and the accuracy of point cloud registration is improved. Meanwhile, the method can cope with the indoor environment, especially the situation of non-planar features of the robot, and meets the requirements of man-machine co-fusion safety detection for multi-source sensing information fusion.

Description

Technical Field

[0001] The present invention belongs to the technical field of multi-sensor information fusion, and specifically relates to a system and method for heterogeneous sensor information fusion and point cloud registration in a human-machine co-integration environment. Background Art

[0002] With the continuous development of modern manufacturing, robotics technology is experiencing a huge leap. The human-machine collaborative working mode has gradually become the development direction of intelligent manufacturing, and the safety guarantee in the process of human-machine collaboration has become an urgent problem to be solved. Multi-sensor fusion is one of the key technologies for monitoring the on-site environment and ensuring the safety of humans and machines. Taking the typical depth camera and lidar as an example, the depth camera can collect point clouds and images of the on-site area by taking pictures, providing rich visual information. The lidar can detect targets through laser beams, and the collected point cloud data has high accuracy and good anti-interference. The multi-sensor fusion of depth cameras and lidars can achieve information complementarity, obtain richer three-dimensional environmental information on the industrial site, provide data support for human-machine safety monitoring, and is an important solution to ensure the safety of human-machine collaboration.

[0003] However, due to the physical properties of depth cameras and lidars, the point cloud collected by depth cameras is dense and has a small range. Lidars have a wide range but lack color and texture information. The collected point cloud is sparse and the density is limited by the laser beam. The density of the point clouds between the two is different, the range scale is significantly different, and the point cloud arrangement rules are completely different, which has become the main challenge faced by the fusion of sensor information between depth cameras and lidars. In addition, the environment in the context of human-machine integration is mainly indoor, involving various non-planar features such as robots, which increases the difficulty of sensor information fusion.

[0004] Currently, in the field of multi-sensor fusion research, most researchers study the data fusion of color cameras and lidar, that is, the association between images and point clouds, and there is less research on the data fusion of depth cameras and multi-line lidar. In the field of point cloud registration, most researchers currently study point cloud registration algorithms based on public data sets, such as The Stanford 3DScanning Repository, 3D Match, Sydney Urban Objects Dataset, etc. However, the above data sets do not involve the registration between sparse point clouds and dense point clouds, large-scale point clouds and small-scale point clouds. Therefore, it is still of great research significance to study the information fusion and point cloud registration of depth cameras and lidar sensors in a human-machine collaborative environment. Summary of the invention

[0005] In response to the problems existing in the above-mentioned prior art, the present invention proposes a system and method for heterogeneous sensor information fusion and point cloud registration in a human-machine collaborative environment, so as to solve the problems of point cloud range scale difference and density difference between depth cameras and lidars, and meet the requirements of multi-source sensor information fusion for human-machine collaborative safety detection.

[0006] In order to achieve the above technical objectives, the present invention proposes the following technical solutions:

[0007] A method for heterogeneous sensor information fusion and point cloud registration in a human-machine collaborative environment, specifically comprising:

[0008] S1, point cloud acquisition, use laser radar to scan the environment point cloud to obtain the original laser radar point cloud; use the depth camera to collect the point cloud within the field of view to obtain the original depth camera point cloud;

[0009] S2, taking the original lidar point cloud and the depth camera point cloud as input, performing coarse positioning based on the point cloud coarse positioning algorithm of the sliding sphere, and outputting the coarsely positioned lidar point cloud;

[0010] S3, taking the roughly positioned lidar point cloud and the original depth camera point cloud as input, and performing rough registration of the point clouds based on the improved FPFH algorithm of the overlap ratio;

[0011] S4, taking the roughly registered point cloud pair as input, and performing precise point cloud registration based on the improved ICP algorithm generated by corresponding points;

[0012] S5. After precise registration, the conversion matrix is ​​output, which is the coordinate system calibration matrix between the lidar and the depth camera, thus realizing the sensor information fusion and point cloud registration.

[0013] Furthermore, step S2 specifically includes:

[0014] S21, performing through filtering on the original depth camera point cloud to remove background point cloud information;

[0015] S22, performing minimum enclosing sphere fitting on the original depth camera point cloud after the straight-through filtering, generating the minimum enclosing sphere of the depth camera point cloud, and obtaining the sphere radius;

[0016] S23, performing AABB bounding box fitting on the original laser radar point cloud;

[0017] S24, according to the obtained sphere radius, perform 3D sphere sliding segmentation in the AABB bounding box of the original laser radar point cloud, perform descriptor and similarity calculation on the point cloud inside the sphere, and after the sliding is completed, sort the similarity; the point cloud inside the sphere is the laser radar point cloud;

[0018] S25, selecting the point cloud inside the sphere with the highest similarity as a new laser radar point cloud, and performing AABB bounding box fitting on it again;

[0019] S26. Repeat steps S24-S25 until the number of cycles reaches a threshold or the point cloud inside the sphere with the highest similarity remains unchanged; the point cloud inside the sphere with the highest similarity, that is, the roughly positioned lidar point cloud, is output.

[0020] More specifically, step S24 includes the following sub-steps:

[0021] S241, 3D sphere sliding segmentation first adopts the form of simple cubic stacking, that is, the three-dimensional xyz coordinates of the 3D sphere slide 2R each time, R is the radius of the sphere;

[0022] S242, performing FPFH descriptor calculation on the point cloud inside the sphere after each sliding to describe the local features of each point;

[0023] S243, calculate the hierarchical proportional descriptor for the point cloud inside the sphere after each sliding, that is, divide the sphere into m concentric rings, where m is a positive integer; calculate the number of point clouds in each ring respectively, and use the ratio between the number of point clouds in the current layer and the total number of point clouds as the hierarchical proportional descriptor to measure the spatial distribution of the point cloud inside the sphere; for a single sphere, the total number of internal point clouds is N; divide the sphere into layers, with i as the index, and calculate the number of point clouds in the i-th layer, n i ; Then the formula of the hierarchical ratio descriptor is expressed as:

[0024]

[0025] Among them, HPD is a hierarchical proportional descriptor, which is a one-dimensional vector;

[0026] After the calculation of the hierarchical ratio descriptor is completed, the gap between the hierarchical ratios needs to be defined. Assuming two groups of hierarchical ratio descriptors HPD1 and HPD2, the hierarchical ratio similarity HPDS between them is defined by the formula:

[0027]

[0028] Among them, l is the highest dimension of HPD1 and HPD2, that is, the number of stratification layers of the sphere; HPDS is the sum of the absolute differences between each dimension of the two stratification ratio indicators.

[0029] S244, performing Harris corner point space similarity calculation on the point cloud inside the sphere after each sliding.

[0030] More specifically, the Harris corner point space similarity calculation in step S244 is specifically as follows:

[0031] S2441, respectively calculate the FPFH descriptor and 3D Harris corner point for the depth camera point cloud and the lidar point cloud;

[0032] S2442, find the FPFH descriptor corresponding to the Harris corner point through KD tree neighbor search, and obtain the Harris corner point set P of the depth camera point cloud with FPFH description s , Harris corner set P of lidar point cloud with FPFH description t ;

[0033] S2443, from P s Randomly select 2 points from and Use KD tree nearest neighbor query in P t Find the closest corner points described by FPFH in each of them, denoted as and The formula for defining the spatial similarity HCSS is expressed as:

[0034]

[0035] Where n is the number of random sampling and j is the index.

[0036] Furthermore, step S3 specifically includes:

[0037] S31, calculating the FPFH descriptor of the input laser radar point cloud and the depth camera point cloud;

[0038] S32, randomly selecting 3 sampling points from the depth camera point cloud, and ensuring that the distance between any two sampling points is greater than a preset threshold;

[0039] S33, for each sampling point of the depth camera point cloud, use KD tree nearest neighbor search in the lidar point cloud to find the point closest to the FPFH descriptor of the sampling point as the corresponding point of the sampling point;

[0040] S34, using SVD decomposition to calculate the coarse registration transformation matrix between the point clouds;

[0041] S35, applying the coarse registration transformation matrix to the depth camera point cloud, and uniformly filtering the transformed depth camera point cloud to reduce its data volume;

[0042] S36, performing a coincidence analysis on the laser radar point cloud around the filtered depth camera point cloud to form a coincidence index R1; similarly, performing a coincidence analysis on the depth camera point cloud around the laser radar point cloud to form a coincidence index R2; defining an overall coincidence index R=R1+R2; and evaluating the performance of the current registration transformation;

[0043] S37. Repeat steps S32-S36 until the number of cycles exceeds the threshold.

[0044] More specifically, the specific calculation formula of the overlap index in step S36 is:

[0045]

[0046]

[0047] Among them, N s is the number of points in the depth camera point cloud, N T is the number of points in the laser radar point cloud; n s is the number of points in the depth camera point cloud that cannot be found in the neighborhood of the corresponding point in the lidar point cloud; n T It is the number of points in the lidar point cloud that cannot find any point in the depth camera point cloud within the neighborhood of the matching point; R1 reflects the degree of non-overlap between the depth camera point cloud and the lidar point cloud, and R2 represents the degree of non-overlap between the lidar point cloud and the depth camera point cloud. R=R1+R2 is used as an error indicator to evaluate the degree of fit between the depth camera point cloud and the lidar point cloud during each registration process.

[0048] Furthermore, step S4 specifically includes:

[0049] S41. Based on the architecture of the ICP algorithm, a corresponding point generation strategy is added to the corresponding point matching link;

[0050] S42, for the point cloud pair after rough registration, for any point in the depth camera point cloud, find the point in the lidar point cloud within its neighborhood, calculate the normal vector difference and point-to-surface distance between any two points, and define a composite measurement index ε;

[0051] S43. Sort the composite measurement index ε obtained in step S42, select the best corresponding point, obtain the precise registration transformation matrix through SVD decomposition, and apply the precise registration transformation matrix to the depth camera point cloud. The obtained new point cloud is used as the depth camera point cloud for the next cycle.

[0052] S44. Repeat steps S41-S43 until the number of cycles exceeds the threshold or the composite measurement indicator ε exceeds the threshold.

[0053] More specifically, the corresponding point generation strategy in step S41 is:

[0054] The corresponding point generation strategy in step S41 is specifically:

[0055] For the point cloud pair after rough registration; P sis any point in the depth camera point cloud, n s is its corresponding normal vector, They are respectively the laser radar point clouds in P s For any two points in the neighborhood, their corresponding normal vectors are by is the normal vector, passing through the point Construct a plane passing through point P s Draw a perpendicular line to the plane and find the foot of the perpendicular Similarly, we can get the foot and That is, s The corresponding newly generated corresponding points are used to replace and

[0056] For vertical foot set up P s ={s x ,s y ,s z}, Satisfies the following formula:

[0057]

[0058] n x (xp x )+n y (yp y )+n z (zp z )=0;

[0059] Similarly, for the vertical foot The above formula is also satisfied.

[0060] More specifically, the composite measurement indicator ε in step S42 is specifically defined as:

[0061] remember With n s The normal vector difference between them is θ a , With n s The normal vector difference between them is θ b , P s arrive The point-to-surface distances between the planes are

[0062] Then the composite measure ε is defined as Indicates point P s The kth corresponding point of .

[0063] In addition, the present invention also establishes a system for heterogeneous sensor information fusion and point cloud registration in a human-machine co-integration environment, on which the above-mentioned sensor information fusion and point cloud registration method can be implemented, specifically including the following hardware configuration:

[0064] A depth camera and a laser radar; used to collect point cloud images, wherein the two different point cloud images have overlapping areas;

[0065] Six-axis robot a and six-axis robot b; the six-axis robot a and six-axis robot b are located within the field of view of the depth camera and the laser radar and serve as feature processing objects in the context of the human-computer interaction scene;

[0066] Radar and depth camera connector; the radar and depth camera connector is fixedly connected to the depth camera and the laser radar;

[0067] As well as supporting structural parts for robot grippers, tooling connections, and electrical equipment for driving robot movement.

[0068] Based on the above technical solution, the present invention has the following beneficial effects:

[0069] The present invention proposes a method for heterogeneous sensor information fusion and point cloud registration in a human-machine co-integration environment, which solves the problem of range scale differences of heterogeneous sensor information by designing a mobile sphere spatial coarse positioning;

[0070] The improved FPFH coarse registration algorithm based on the coincidence ratio is used to coarsely register point clouds of different densities, and the improved ICP fine registration algorithm based on corresponding point generation is combined to refine the registration results and solve the problem of sparse differences in point cloud density.

[0071] The method proposed in this paper solves the problem of sensor information fusion between large-scale sparse lidar point cloud and small-scale dense depth camera point cloud in a human-machine collaborative environment, realizes point cloud registration between depth camera and lidar, and can also cope with indoor environments, especially non-planar features of robots, meeting the requirements of multi-source sensor information fusion for human-machine collaborative safety detection. BRIEF DESCRIPTION OF THE DRAWINGS

[0072] Figure 1 A schematic diagram of the hardware configuration of the system for heterogeneous sensor information fusion and point cloud registration in a human-machine co-integration environment established by the present invention;

[0073] In the figure: 1- radar and depth camera connector, 2- depth camera, 3- laser radar, 4- six-axis robot a, 5- six-axis robot b;

[0074] Figure 2This is an overall algorithm framework diagram of a method for heterogeneous sensor information fusion and point cloud registration in a human-machine collaborative environment proposed by the present invention;

[0075] Figure 3 It is a framework diagram of the coarse positioning algorithm in the method proposed by the present invention;

[0076] Figure 4 Schematic diagram of the corresponding point generation strategy in the method proposed in the present invention. DETAILED DESCRIPTION

[0077] In order to make the purpose, technical solution and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention.

[0078] Although the steps in the present invention are arranged with numbers, they are not used to limit the order of the steps. Unless the order of the steps is clearly stated or the execution of a certain step requires other steps as a basis, the relative order of the steps can be adjusted. It is understood that the term "and / or" used in this article involves and covers any and all possible combinations of one or more of the associated listed items.

[0079] like Figure 1 As shown, a system for heterogeneous sensor information fusion and point cloud registration in a human-machine co-integration environment established by the present invention specifically includes the following hardware configurations:

[0080] Depth camera 2 and laser radar 3; used to collect point cloud images, wherein the two different point cloud images have overlapping areas;

[0081] Six-axis robot a 4 and six-axis robot b 5; the six-axis robot a 4 and six-axis robot b 5 are located within the field of view of the depth camera and the laser radar, and serve as feature processing objects in the context of the human-computer interaction scene;

[0082] Radar and depth camera connector 1; the radar and depth camera connector is fixedly connected to the depth camera and the laser radar;

[0083] As well as supporting structural parts for robot grippers, tooling connections, and electrical equipment for driving robot movement.

[0084] On the above system, the present invention can be implemented, such as Figure 2 As shown, a method for heterogeneous sensor information fusion and point cloud registration in a human-machine collaborative environment specifically includes the following steps:

[0085] S1, point cloud acquisition, use laser radar to scan the environment point cloud to obtain the original laser radar point cloud; use the depth camera to collect the point cloud within the field of view to obtain the original depth camera point cloud;

[0086] S2, taking the original lidar point cloud and the depth camera point cloud as input, performing coarse positioning based on the point cloud coarse positioning algorithm of the sliding sphere, and outputting the coarsely positioned lidar point cloud;

[0087] In this embodiment, the coarse positioning algorithm framework is as follows: Figure 3 As shown in the figure, the problem of difficulty in registration due to the range scale difference between the lidar point cloud and the depth camera point cloud is solved.

[0088] As a preferred implementation, step S2 specifically includes:

[0089] S21, performing through filtering on the depth camera point cloud to remove background point cloud information;

[0090] S22, performing minimum enclosing sphere fitting on the depth camera point cloud after the through filtering, generating the minimum enclosing sphere of the depth camera point cloud, and obtaining the sphere radius;

[0091] S23, performing AABB bounding box fitting on the laser radar point cloud;

[0092] S24, according to the obtained sphere radius, perform 3D sphere sliding segmentation in the AABB bounding box of the laser radar point cloud, perform descriptor and similarity calculation on the point cloud inside the sphere each time, and sort the similarities after the sliding is completed;

[0093] In this embodiment, the 3D sphere carries the information of the original depth camera point cloud, and the point cloud inside the sphere only contains the laser radar point cloud. That is, the coarse positioning is to filter the laser radar point cloud based on the depth camera point cloud. The specific process includes:

[0094] S241, 3D sphere sliding segmentation first adopts the form of simple cubic stacking, that is, the three-dimensional xyz coordinates of the 3D sphere slide 2R each time, R is the radius of the sphere;

[0095] S242, performing FPFH descriptor calculation on the point cloud inside the sphere after each sliding to describe the local features of each point;

[0096] S243, calculate the hierarchical proportional descriptor for the point cloud inside the sphere after each sliding, that is, divide the sphere into m concentric rings, where m is a positive integer; calculate the number of point clouds in each ring respectively, and use the ratio between the number of point clouds in the current layer and the total number of point clouds as the hierarchical proportional descriptor to measure the spatial distribution of the point cloud inside the sphere; for a single sphere, the total number of internal point clouds is N; divide the sphere into layers, with i as the index, and calculate the number of point clouds in the i-th layer, ni ; Then the formula of the hierarchical ratio descriptor is expressed as:

[0097]

[0098] Among them, HPD is the hierarchical proportion descriptor, which is a one-dimensional vector, and each dimension stores the ratio between the number of point clouds in the current layer and the total number of point clouds;

[0099] After the calculation of the hierarchical ratio descriptor is completed, the gap between the hierarchical ratios needs to be defined. Assuming two groups of hierarchical ratio descriptors HPD1 and HPD2, the hierarchical ratio similarity HPDS between the two is defined by the formula:

[0100]

[0101] Among them, n is the highest dimension of HPD1 and HPD2, and HPDS is the sum of the absolute differences between each dimension of the two stratified proportion indicators.

[0102] S244, performing Harris corner point space similarity calculation on the point cloud inside the sphere after each sliding, specifically:

[0103] S2441, respectively calculate the FPFH descriptor and 3D Harris corner point for the depth camera point cloud and the lidar point cloud;

[0104] S2442, find the FPFH descriptor corresponding to the Harris corner point through KD tree neighbor search, and obtain the Harris corner point set P of the depth camera point cloud with FPFH description s , Harris corner set P of lidar point cloud with FPFH description t ;

[0105] S2443, from P s Randomly select 2 points from and Use KD tree nearest neighbor query in P t Find the closest corner points described by FPFH in each of them, denoted as and The formula for defining the spatial similarity HCSS is expressed as:

[0106]

[0107] Among them, n is the number of random sampling, with as the index;

[0108] S25. Expand the candidate area: The radius of the sphere with the highest similarity is R. best , expand its radius to 1.5*R best, the center of the sphere remains unchanged, the point cloud inside the sphere is selected as the new lidar point cloud, and the AABB bounding box is fitted again;

[0109] S26, repeating steps S24-S25 until the number of cycles reaches the threshold or the point cloud inside the sphere with the highest similarity remains unchanged; the point cloud inside the sphere with the highest similarity is used as the output of the rough positioning. 3D sliding segmentation refinement means that in step S241, the sliding distance step of the sphere is shortened from 2R by 0.5 times, that is, the sliding distance step is set to R;

[0110] S3. The roughly positioned LiDAR point cloud and the original depth camera point cloud are used as input, and the improved FPFH algorithm based on the overlap ratio is used to perform rough registration of the point clouds; the problem of difficulty in rough registration due to the density difference between the LiDAR point cloud and the depth camera point cloud is solved.

[0111] As a preferred implementation, step S3 specifically includes:

[0112] S31, calculating the FPFH descriptor of the input laser radar point cloud and the depth camera point cloud;

[0113] S32, randomly selecting 3 sampling points from the depth camera point cloud, and ensuring that the distance between any two sampling points is greater than a preset threshold;

[0114] S33, for each sampling point of the depth camera point cloud, use KD tree nearest neighbor search in the lidar point cloud to find the point closest to the FPFH descriptor of the sampling point as the corresponding point of the sampling point;

[0115] S34, using SVD decomposition to calculate the coarse registration transformation matrix between the point clouds;

[0116] S35, applying the coarse registration transformation matrix to the depth camera point cloud, and uniformly filtering the transformed depth camera point cloud to reduce its data volume;

[0117] S36, performing a coincidence analysis on the laser radar point cloud around the filtered depth camera point cloud to form a coincidence index R1; similarly, performing a coincidence analysis on the depth camera point cloud around the laser radar point cloud to form a coincidence index R2; defining an overall coincidence index R=R1+R2; and evaluating the performance of the current registration transformation;

[0118] The specific calculation formula of the coincidence index is:

[0119]

[0120]

[0121] Among them, N sis the number of points in the depth camera point cloud, N T is the number of points in the laser radar point cloud; n s is the number of points in the depth camera point cloud that cannot be found in the neighborhood of the corresponding point in the lidar point cloud; n T It is the number of points in the LiDAR point cloud that cannot be found in the neighborhood of the matching point in the depth camera point cloud; R1 reflects the degree of non-overlap between the source point cloud and the LiDAR point cloud, and R2 represents the degree of non-overlap between the LiDAR point cloud and the depth camera point cloud. R=R1+R2 is used as an error indicator to evaluate the degree of fit between the source point cloud and the target point cloud during each registration process; in the algorithm implementation, the KD tree is used to search the neighborhood of the point;

[0122] S37. Repeat steps S32-S36 until the number of cycles exceeds the threshold.

[0123] S4, using the roughly registered point cloud pair as input, and performing precise registration of the point cloud based on the improved ICP algorithm generated by corresponding points; solving the problem of difficulty in precise registration due to the density difference between the lidar point cloud and the depth camera point cloud;

[0124] As a preferred implementation, step S4 specifically includes:

[0125] S41. Based on the architecture of the ICP algorithm, a corresponding point generation strategy is added to the corresponding point matching link;

[0126] More specifically, Figure 4 As shown, the corresponding point generation strategy is:

[0127] For the point cloud pair after rough registration; P s is any point in the depth camera point cloud, n s is its corresponding normal vector, They are respectively the laser radar point clouds in P s For any two points in the neighborhood, their corresponding normal vectors are by is the normal vector, passing through the point Construct a plane passing through point P s Draw a perpendicular line to the plane and find the foot of the perpendicular Similarly, we can get the foot and That is, s The corresponding newly generated corresponding points are used to replace and In this embodiment, a=1, b=2;

[0128] For vertical foot set up P s ={sx ,s y ,s z}, Satisfies the following formula:

[0129]

[0130] n x (xp x )+n y (yp y )+n z (z-pz ) =0;

[0131] Similarly, for the vertical foot It also satisfies the above formula;

[0132] S42. For the point cloud pair after rough registration, for any point in the depth camera point cloud, find the point in the lidar point cloud within its neighborhood, calculate the normal vector difference and point-to-surface distance between any two points, and define the composite measurement index ε; the specific definition process is:

[0133] remember With n s The normal vector difference between them is θ a , With n s The normal vector difference between them is θ b , P s arrive The point-to-surface distances between the planes are

[0134] Then the composite measure ε is defined as Indicates point P s The kth corresponding point of .

[0135] S43. Sort the composite measurement index ε obtained in step S42, select the best corresponding point, obtain the precise registration transformation matrix through SVD decomposition, and apply the precise registration transformation matrix to the depth camera point cloud. The obtained new point cloud is used as the depth camera point cloud for the next cycle.

[0136] S44. Repeat steps S41-S43 until the number of cycles exceeds the threshold or the composite measurement indicator ε exceeds the threshold.

[0137] In this embodiment, each point has a corresponding perpendicular foot and a composite measurement index, which is related to itself. Selecting any two points is just for the ranking between the two groups of indicators, and the best one between the two is selected. Therefore, any two points can also be expanded into multiple points.

[0138] S5. After precise registration, the conversion matrix is ​​output, which is the coordinate system calibration matrix between the lidar and the depth camera, thus realizing the sensor information fusion and point cloud registration.

[0139] At this point, the system and method proposed in the present invention solve the problems of scale difference and density difference in point cloud range, and realize the accurate fusion of sensor information and point cloud registration between depth camera and lidar in a human-machine collaborative environment. At the same time, it can also cope with indoor environments, especially the non-planar features of robots, and meet the requirements of multi-source sensor information fusion for human-machine collaborative safety detection.

[0140] It should be noted that it is obvious to those skilled in the art that the present invention is not limited to the details of the exemplary embodiments described above, and that the present invention can be implemented in other specific forms without departing from the spirit or essential features of the present invention. Therefore, the embodiments should be regarded as exemplary and non-restrictive from any point of view, and the scope of the present invention is defined by the appended claims rather than the above description, and it is intended that all changes falling within the meaning and scope of the equivalent elements of the claims are included in the present invention. Any reference numerals in the claims should not be regarded as limiting the claims involved.

[0141] In addition, it should be understood that although the present specification is described according to implementation modes, not every implementation mode contains only one independent technical solution. This description of the specification is only for the sake of clarity. Those skilled in the art should regard the specification as a whole. The technical solutions in each embodiment may also be appropriately combined to form other implementation modes that can be understood by those skilled in the art.

Claims

1. A method for heterogeneous sensor information fusion and point cloud registration in a human-machine collaborative environment, characterized in that: The specific steps include: S1, point cloud acquisition, use laser radar to scan the environment point cloud to obtain the original laser radar point cloud; use the depth camera to collect the point cloud within the field of view to obtain the original depth camera point cloud; S2, taking the original lidar point cloud and the depth camera point cloud as input, performing coarse positioning based on the point cloud coarse positioning algorithm of the sliding sphere, and outputting the coarsely positioned lidar point cloud; S3, taking the roughly positioned lidar point cloud and the original depth camera point cloud as input, and performing rough registration of the point clouds based on the improved FPFH algorithm of the overlap ratio; S4, taking the roughly registered point cloud pair as input, and performing precise point cloud registration based on the improved ICP algorithm generated by corresponding points; S5. After precise registration, the conversion matrix is ​​output, which is the coordinate system calibration matrix between the lidar and the depth camera, thus realizing the sensor information fusion and point cloud registration.

2. According to the method of heterogeneous sensor information fusion and point cloud registration in a human-machine collaborative environment according to claim 1, it is characterized in that: Step S2 specifically includes: S21, performing through filtering on the original depth camera point cloud to remove background point cloud information; S22, performing minimum enclosing sphere fitting on the original depth camera point cloud after the straight-through filtering, generating the minimum enclosing sphere of the depth camera point cloud, and obtaining the sphere radius; S23, performing AABB bounding box fitting on the original laser radar point cloud; S24, according to the obtained sphere radius, perform 3D sphere sliding segmentation in the AABB bounding box of the original laser radar point cloud, perform descriptor and similarity calculation on the point cloud inside the sphere, and after the sliding is completed, sort the similarity; the point cloud inside the sphere is the laser radar point cloud; S25, selecting the point cloud inside the sphere with the highest similarity as a new laser radar point cloud, and performing AABB bounding box fitting on it again; S26. Repeat steps S24-S25 until the number of cycles reaches a threshold or the point cloud inside the sphere with the highest similarity remains unchanged; the point cloud inside the sphere with the highest similarity, that is, the roughly positioned lidar point cloud, is output.

3. According to the method of heterogeneous sensor information fusion and point cloud registration in a human-machine collaborative environment as described in claim 2, it is characterized in that: Step S24 specifically includes: S241, 3D sphere sliding segmentation first adopts the form of simple cubic stacking, that is, the three-dimensional xyz coordinates of the 3D sphere slide 2R each time, R is the radius of the sphere; S242, performing FPFH descriptor calculation on the point cloud inside the sphere after each sliding to describe the local features of each point; S243, calculate the hierarchical proportional descriptor for the point cloud inside the sphere after each sliding, that is, divide the sphere into m concentric rings, where m is a positive integer; calculate the number of point clouds in each ring respectively, and use the ratio between the number of point clouds in the current layer and the total number of point clouds as the hierarchical proportional descriptor to measure the spatial distribution of the point cloud inside the sphere; for a single sphere, the total number of internal point clouds is N; divide the sphere into l layers, take i as the index, and calculate the number of point clouds in the i-th layer n i ; Then the formula of the hierarchical ratio descriptor is expressed as: Among them, HPD is a hierarchical proportional descriptor, which is a one-dimensional vector; After the calculation of the hierarchical ratio descriptor is completed, the gap between the hierarchical ratios needs to be defined. Assuming two groups of hierarchical ratio descriptors HPD1 and HPD2, the hierarchical ratio similarity HPDS between them is defined by the formula: Among them, l is the highest dimension of HPD1 and HPD2, that is, the number of stratification layers of the sphere; HPDS is the sum of the absolute differences between each dimension of the two stratification ratio indicators; S244, performing Harris corner point space similarity calculation on the point cloud inside the sphere after each sliding.

4. According to the method of heterogeneous sensor information fusion and point cloud registration in a human-machine collaborative environment as described in claim 3, it is characterized in that: Step S244 is specifically as follows: S2441, respectively calculate the FPFH descriptor and 3D Harris corner point for the depth camera point cloud and the lidar point cloud; S2442, find the FPFH descriptor corresponding to the Harris corner point through KD tree neighbor search, and obtain the Harris corner point set P of the depth camera point cloud with FPFH description s , Harris corner set P of lidar point cloud with FPFH description t ; S2443, from P s Randomly select 2 points from and Use KD tree nearest neighbor query in P t Find the closest corner points described by FPFH in each of them, denoted as and The formula for defining the spatial similarity HCSS is expressed as: Where n is the number of random sampling and j is the index.

5. The method for heterogeneous sensor information fusion and point cloud registration in a human-machine collaborative environment according to claim 1 is characterized in that: Step S3 specifically includes: S31, calculating the FPFH descriptor of the input coarsely positioned laser radar point cloud and the original depth camera point cloud; S32, randomly selecting 3 sampling points from the original depth camera point cloud, and ensuring that the distance between any two sampling points is greater than a preset threshold; S33, for each sampling point of the depth camera point cloud, use KD tree nearest neighbor search in the lidar point cloud to find the point closest to the FPFH descriptor of the sampling point as the corresponding point of the sampling point; S34, using SVD decomposition to calculate the coarse registration transformation matrix between the point clouds; S35, applying the coarse registration transformation matrix to the depth camera point cloud, and uniformly filtering the transformed depth camera point cloud to reduce its data volume; S36, performing a coincidence analysis on the laser radar point cloud around the filtered depth camera point cloud to form a coincidence index R1; similarly, performing a coincidence analysis on the depth camera point cloud around the laser radar point cloud to form a coincidence index R2; defining an overall coincidence index R=R1+R2; and evaluating the performance of the current registration transformation; S37. Repeat steps S32-S36 until the number of cycles exceeds the threshold.

6. The method for heterogeneous sensor information fusion and point cloud registration in a human-machine collaborative environment according to claim 5 is characterized in that: The specific calculation formula of the overlap index in step S36 is: Among them, N s is the number of points in the depth camera point cloud, N T is the number of points in the laser radar point cloud; n s is the number of points in the depth camera point cloud that cannot be found in the neighborhood of the corresponding point in the lidar point cloud; n T It is the number of points in the lidar point cloud that cannot find any point in the depth camera point cloud within the neighborhood of the matching point; R1 reflects the degree of non-overlap between the depth camera point cloud and the lidar point cloud, and R2 represents the degree of non-overlap between the lidar point cloud and the depth camera point cloud. R=R1+R2 is used as an error indicator to evaluate the degree of fit between the depth camera point cloud and the lidar point cloud during each registration process.

7. The method for heterogeneous sensor information fusion and point cloud registration in a human-machine collaborative environment according to claim 1 is characterized in that: Step S4 specifically includes: S41. Based on the architecture of the ICP algorithm, a corresponding point generation strategy is added to the corresponding point matching link; S42, for the point cloud pair after rough registration, for any point A in the depth camera point cloud, find the point set in the lidar point cloud within its neighborhood, calculate the normal vector difference and point-to-surface distance between any point B in the point set and point A, and define a composite measurement index ε; S43, sorting the composite measurement index ε obtained in step S42, selecting the best corresponding point with the smallest ε, obtaining a precise registration transformation matrix through SVD decomposition, and applying the precise registration transformation matrix to the depth camera point cloud, and the obtained new point cloud is used as the depth camera point cloud of the next cycle; S44. Repeat steps S41-S43 until the number of cycles exceeds the threshold or the composite measurement indicator ε exceeds the threshold.

8. The method for heterogeneous sensor information fusion and point cloud registration in a human-machine collaborative environment according to claim 7 is characterized in that: The corresponding point generation strategy in step S41 is specifically: For the point cloud pair after rough registration; P s is any point in the depth camera point cloud, n s is its corresponding normal vector, They are respectively the laser radar point clouds in P s For any two points in the neighborhood, their corresponding normal vectors are by is the normal vector, passing through the point Construct a plane passing through point P s Draw a perpendicular line to the plane and find the foot of the perpendicular Similarly, we can get the foot and That is, s The corresponding newly generated corresponding points are used to replace and For vertical foot set up P s ={s x ,s y ,s z }, Satisfies the following formula: n x (xp x )+n y (yp y )+n z (zp z )=0: Similarly, for the vertical foot The above formula is also satisfied.

9. The method for heterogeneous sensor information fusion and point cloud registration in a human-machine collaborative environment according to claim 8, characterized in that: The composite measurement index ε in step S42 is specifically defined as: remember With n s The normal vector difference between them is θ a , With n s The normal vector difference between them is θ b , P s arrive The point-to-surface distances between the planes are Then the composite measure ε is defined as Indicates point P s The kth corresponding point of .

10. A system for heterogeneous sensor information fusion and point cloud registration in a human-machine collaborative environment, characterized in that: Any one of the above claims 1-9 can be implemented on the system, specifically including hardware configuration: A depth camera and a laser radar; used to collect point cloud images, wherein the two different point cloud images have overlapping areas; Six-axis robot a and six-axis robot b; the six-axis robot a and six-axis robot b are located within the field of view of the depth camera and the laser radar and serve as feature processing objects in the context of the human-computer interaction scene; Radar and depth camera connector; the radar and depth camera connector is fixedly connected to the depth camera and the laser radar; As well as supporting structural parts for robot grippers, tooling connections, and electrical equipment for driving robot movement.

Images