Environmental perception method based on visual sensor and laser radar information fusion

Abstract

The invention relates to an environment sensing method based on information fusion of a visual sensor and a laser radar, and the method comprises the following steps: S10, carrying out the data alignment of the visual sensor and the single-line laser radar, S11, obtaining a rotation matrix of a calibration plate plane in a camera coordinate system to which the visual sensor belongs, establishing a plane constraint equation of the laser points on the calibration plate according to the laser points falling on the calibration plate; according to the method, grid division is carried out on point cloud data and statistical information of point clouds in grids is used for completing grid preliminary classification, and ground seed point information is searched according to grid types; by means of the method, the problem of robot ground wrong segmentation caused by shaking and ground pits in the running process of the robot can be avoided.

Description

Technical Field

[0001] The present invention relates to the technical field of information fusion, and in particular to an environment perception method based on the fusion of visual sensor and laser radar information. Background Art

[0002] Mobile robots often work in unknown dynamic environments and encounter various obstacles during movement. Faced with complex working environments, effective obstacle avoidance technology is the basis of mobile robot navigation, and being able to effectively identify and quickly avoid obstacles is the key to completing the target task.

[0003] Existing environmental perception methods mostly use monocular cameras and single-line laser radars to detect environmental obstacles. Since monocular cameras are sensitive to dark environments, it is difficult for robots to work normally in such environments. In addition, most mainstream color image processing methods use deep learning methods, which involve a large number of matrix operations. However, mobile robot industrial computers often do not have GPU video memory, so matrix processing is time-consuming and occupies a huge amount of system memory, making it difficult to output detection results in real time. Summary of the invention

[0004] In view of the shortcomings of the prior art, the present invention provides an environment perception method based on the fusion of visual sensor and laser radar information. The specific technical solution is as follows:

[0005] The environmental perception method based on the fusion of visual sensor and lidar information includes the following steps:

[0006] S10: Alignment of vision sensor and single-line lidar data

[0007] S11: Obtain the rotation matrix of the calibration plate plane in the camera coordinate system of the visual sensor, and establish a constraint equation of the laser point on the calibration plate plane according to the laser point falling on the calibration plate;

[0008] S12: The plane constraint equation is transformed using a nonlinear least square method to obtain a transformation matrix, and the data space of the laser radar and the visual sensor are aligned according to the transformation matrix to obtain fused point cloud data;

[0009] S20: Ground point cloud removal based on grid segmentation and merging

[0010] The fused point cloud data is divided into sectors, and the ground models fitted in each sector are spliced ​​according to the conditional threshold to obtain the classified obstacle point cloud and ground point cloud information.

[0011] S30: Obstacle information 2D plane compression

[0012] During the mobile robot navigation process, a two-dimensional grid map is used for path planning and local control. The obstacle point cloud and ground point cloud information detected in step S20 are projected onto the robot motion plane to complete obstacle detection.

[0013] As an improvement of the above technical solution, step S11 further includes the following steps:

[0014] S111: first use the laser radar to extract straight lines from the laser data to obtain laser points falling on the calibration plate;

[0015] In step S112: determine whether the extracted laser point meets the length range of the calibration plate, if not, return to step S111;

[0016] S113: collecting an image of the calibration plate through a visual sensor to obtain image coordinates of feature points in the image;

[0017] S114: Determine whether the laser data and image data meet the requirements, if not, return to step S11;

[0018] S115: Construct plane constraint equations.

[0019] As an improvement of the above technical solution, in step S115, the rotation matrix from the laser radar coordinate system to the camera coordinate system of the visual sensor is recorded as The translation vector is denoted as Assume that the laser point falling on the calibration plate is P laser , then the constructed plane constraint equation is as follows:

[0020]

[0021] Among them, n c and d c is the plane equation parameter of the calibration plate in the camera coordinate system of the visual sensor, n c is the transpose of the three-dimensional normal vector, d c is the distance from the origin of the camera coordinate system to which the visual sensor belongs to the plane.

[0022] As an improvement of the above technical solution, in step S12, the plane constraint equation is transformed to define the least squares problem:

[0023]

[0024] Assume that the left matrix is ​​expanded to n c *H*P laser = b, where b = -d c , further transform and decompose the left equation to get

[0025] A*H=b (3)

[0026] Among them, H is a 3×3 matrix, A is an N×9 matrix, b is an N×1 matrix, and N represents the number of groups of collected calibration data. The least squares problem is solved for the converted equation to obtain the following formula:

[0027] J(x)=min||AH-b|| 2 (4)

[0028] Among them, J(x) is a convex function, and its first-order derivative is set to 0, and H = (A T A) -1 A T b

[0029] Assume that the SVD of matrix A is defined as:

[0030] A=U∑V T (5)

[0031] Among them, U is a 3×3 unitary matrix, ∑ is a 3×3 matrix (except for the elements on the main diagonal, the rest are zero, and each element on the main diagonal is called a singular value) and V is a 3×3 unitary matrix.

[0032] Substituting formula (5) into formula (4) yields

[0033] After solving H, substitute it into formula (2) to solve the transformation matrix and the final calibration result, and complete the data space alignment of the lidar and depth camera to obtain the fused point cloud data.

[0034] As an improvement of the above technical solution, the step S20 further includes the following steps:

[0035] S21: Divide the entire point cloud data into four sectors, traverse each sector, and calculate the internal point cloud data normal vector information.

[0036] S22: The sector is refined into k grids of constant size, and the average height of the point cloud data inside each grid is calculated The standard deviation σ, and the normal vector n c ;

[0037] S23: Determine the grid points that meet the threshold conditions. If all grid points meet the threshold conditions, add seed points and perform ground model fitting based on the seed points.

[0038] S24: Determine whether each sector is completed, if not, return to step S21;

[0039] S25: Traversing the ground models fitted in each sector, performing a sector classification point cloud stitching operation according to a conditional threshold, and obtaining the classified obstacle point cloud and ground point cloud information.

[0040] As an improvement of the above technical solution, in step S22, the average height The standard deviation σ, and the normal vector n c Calculation formula:

[0041]

[0042]

[0043]

[0044] Among them, N is the total number of points in the current grid, z i is the z-axis coordinate of the i-th point in the current grid, v i Represents point p to center point p i The vector of Represents the average value of the vector, the normal vector n c It represents the eigenvector corresponding to the minimum eigenvalue of the covariance Cov.

[0045] As an improvement of the above technical solution, in step S30, assuming that the general equation of the ground model is Ax+By+Cz+D=0, and the coordinates of the obstacle point in the three-dimensional space are (x0, y0, z0), the coordinates of the obstacle point after projection are:

[0046]

[0047] The projected obstacle point cloud is calculated through the above formula to complete the obstacle detection.

[0048] Beneficial effects of the present invention:

[0049] 1. Through the automatic extraction and calibration of calibration plate images and laser data within a certain range, end-to-end visual sensor and lidar external parameter calibration is realized, which simplifies the calibration process and effectively avoids a large number of matrix operations.

[0050] 2. The preliminary grid classification is completed by dividing the point cloud data into grids and the statistical information of the point cloud inside the grid. The ground seed point information is found according to the grid type, and the ground information is removed by fusing multiple ground models inside the grid. This method can avoid the problem of robot ground mis-segmentation caused by the robot's jitter during driving and potholes on the ground.

[0051] 3. By compressing the fused data to the robot motion plane, it is convenient to update the grid map during navigation. BRIEF DESCRIPTION OF THE DRAWINGS

[0052] Figure 1 This is a flow chart of the visual sensor and laser radar calibration of the present invention;

[0053] Figure 2 This is a schematic diagram of the calibration of the visual sensor and the laser radar of the present invention;

[0054] Figure 3 It is a flow chart of the ground point cloud removal method based on grid segmentation and merging in the present invention;

[0055] Figure 4 This is the point cloud data division diagram in the present invention. DETAILED DESCRIPTION

[0056] 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 intended to limit the present invention.

[0057] The environmental perception method based on the fusion of visual sensor and lidar information includes the following steps:

[0058] S10: Alignment of vision sensor and single-line lidar data;

[0059] S11: Obtain the rotation matrix of the calibration plate plane in the camera coordinate system of the visual sensor, and establish a constraint equation of the laser point on the calibration plate plane according to the laser point falling on the calibration plate;

[0060] S111: first use the laser radar to extract straight lines from the laser data to obtain laser points falling on the calibration plate;

[0061] S112: Determine whether the extracted laser point meets the length range of the calibration plate. If the length range of the calibration plate is greater than 1.5 m, return to step S111. If the length range of the calibration plate is less than or equal to 1.5 m, proceed to the next step.

[0062] S113: collecting an image of the calibration plate through a visual sensor to obtain image coordinates of feature points in the image;

[0063] S114: Determine whether the laser data and the image data meet 20 groups. If not, return to step S11. If so, proceed to the next step.

[0064] S115: Construct the plane constraint equation, that is, the rotation matrix from the laser radar coordinate system to the camera coordinate system of the visual sensor is recorded as The translation vector is denoted as Assume that the laser point falling on the calibration plate is P laser , then the constructed plane constraint equation is as follows:

[0065]

[0066] Among them, n c and d c is the plane equation parameter of the calibration plate in the camera coordinate system of the visual sensor, nw is the transpose of the three-dimensional normal vector, d c is the distance from the origin of the camera coordinate system to which the visual sensor belongs to the plane.

[0067] S12: The plane constraint equation is transformed using a nonlinear least square method to obtain a transformation matrix, and the data space of the laser radar and the visual sensor are aligned according to the transformation matrix to obtain fused point cloud data;

[0068] Transform the plane constraint equation and define the least squares problem:

[0069]

[0070] Assume that the left matrix is ​​expanded to n c *H*P laser = b, where b = -d c , further transform and decompose the left equation to get

[0071] A*H=b (3)

[0072] Among them, H is a 3×3 matrix, A is an N×9 matrix, b is an N×1 matrix, and N represents the number of groups of collected calibration data. The least squares problem is solved for the converted equation to obtain the following formula:

[0073] J(x)=min||AH-b|| 2 (4)

[0074] Among them, J(x) is a convex function, and its first-order derivative is set to 0, and H = (A T A) -1 A T b

[0075] Assume that the SVD of matrix A is defined as:

[0076] A=U∑V T (5)

[0077] Among them, U is a 3×3 unitary matrix, ∑ is a 3×3 matrix (except for the elements on the main diagonal, the rest are zero, and each element on the main diagonal is called a singular value) and V is a 3×3 unitary matrix.

[0078] Substituting formula (5) into formula (4) yields

[0079] After solving H, substitute it into formula (2) to solve the transformation matrix and the final calibration result, and complete the data space alignment of the lidar and depth camera to obtain the fused point cloud data;

[0080] By automatically extracting and calibrating the calibration plate image and laser data within a certain range, end-to-end visual sensor and lidar external parameter calibration is achieved, simplifying the sensor calibration process.

[0081] S20: Ground point cloud removal based on grid segmentation and merging

[0082] The fused point cloud data is divided into sectors, and the ground models fitted in each sector are spliced ​​according to the conditional threshold to obtain the classified obstacle point cloud and ground point cloud information.

[0083] S21: Divide the entire point cloud data into four sectors, traverse each sector, and calculate the internal point cloud data normal vector information.

[0084] S22: The sector is refined into k grids of constant size, and the average height of the point cloud data inside each grid is calculated The standard deviation σ, and the normal vector n c ;

[0085] S23: Determine the grid points that meet the threshold conditions, that is, the average height is greater than 1.2 times the distance between the laser radar and the ground, the standard deviation of the point cloud data is 0.02m, and the angle between the normal vector of the point cloud data and the z-axis normal vector is less than 15 degrees. If all of them are met, add seed points and fit the ground model based on the seed points. This step is implemented using the least squares SVD algorithm in step S12. If any of them is not met, return to the previous step. The average height The standard deviation σ, and the normal vector n c Calculation formula:

[0086]

[0087]

[0088]

[0089] Among them, N is the total number of points in the current grid, z i is the z-axis coordinate of the i-th point in the current grid, v i Represents point p to center point p i The vector of Represents the average value of the vector, the normal vector n c It represents the eigenvector corresponding to the minimum eigenvalue of the covariance Cov;

[0090] S24: Determine whether each sector has been refined. If not, return to step S21. If so, proceed to the next step.

[0091] S25: traverse the ground models fitted in each sector and perform a sector classification point cloud splicing operation according to a conditional threshold to obtain the classified obstacle point cloud and ground point cloud information;

[0092] The preliminary grid classification is completed by dividing the point cloud data into grids and the statistical information of the point cloud inside the grid. The ground seed point information is found according to the grid type, and the ground information is removed by fusing multiple ground models inside the grid. This method can avoid the problem of robot ground mis-segmentation caused by the robot's jitter during driving and potholes on the ground.

[0093] S30: Obstacle information 2D plane compression

[0094] During the mobile robot navigation process, a two-dimensional grid map is used for path planning and local control, and the obstacle point cloud and ground point cloud information detected in step S20 are projected onto the robot motion plane to complete obstacle detection;

[0095] Assuming that the general equation of the ground model is Ax+By+Cz+D=0, and the coordinates of the obstacle point in three-dimensional space are (x0, y0, z0), the coordinates of the obstacle point after projection are

[0096]

[0097] Obstacle point cloud after projection is obtained by the above calculation to complete obstacle detection;

[0098] By compressing the fused data to the robot motion plane, it is convenient to update the grid map during navigation.

[0099] The present application can accurately segment the low plane above the ground, while the traditional plane segmentation and merging algorithm will lose some obstacle point clouds. Since this part of the point cloud is close to the ground height, the traditional algorithm will have misdetection problems when fitting. The grid segmentation and merging algorithm proposed in the present application focuses on the local geometric relationship of the point cloud to improve the segmentation accuracy.

[0100] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions and improvements made within the spirit and principles of the present invention should be included in the protection scope of the present invention.

Claims

1. An environmental perception method based on the fusion of visual sensor and laser radar information, characterized in that: The following steps are involved: S10: Alignment of vision sensor and single-line lidar data S11: Obtain the rotation matrix of the calibration plate plane in the camera coordinate system of the visual sensor, and establish a constraint equation of the laser point on the calibration plate plane according to the laser point falling on the calibration plate; S12: The plane constraint equation is transformed using a nonlinear least square method to obtain a transformation matrix, and the data space of the laser radar and the visual sensor are aligned according to the transformation matrix to obtain fused point cloud data; S20: Ground point cloud removal based on grid segmentation and merging The fused point cloud data is divided into sectors, and the ground models fitted in each sector are spliced ​​according to the conditional threshold to obtain the classified obstacle point cloud and ground point cloud information. S30: Obstacle information 2D plane compression During the mobile robot navigation process, a two-dimensional grid map is used for path planning and local control. The obstacle point cloud and ground point cloud information detected in step S20 are projected onto the robot motion plane to complete obstacle detection.

2. The method for environmental perception based on the fusion of visual sensor and laser radar information according to claim 1 is characterized in that: Step S11 also includes the following steps: S111: first use the laser radar to extract straight lines from the laser data to obtain laser points falling on the calibration plate; S112: Determine whether the extracted laser point is within the length range of the calibration plate, if not, return to step S111; S113: collecting an image of the calibration plate through a visual sensor to obtain image coordinates of feature points in the image; S114: Determine whether the laser data and the image data meet the requirements, if not, return to step S11; S115: Construct plane constraint equations.

3. The method for environmental perception based on the fusion of visual sensor and laser radar information according to claim 2 is characterized in that: In step S115, the rotation matrix from the laser radar coordinate system to the camera coordinate system of the visual sensor is recorded as The translation vector is denoted as Assume that the laser point falling on the calibration plate is P laser , then the constructed plane constraint equation is as follows: Among them, n c and d c is the plane equation parameter of the calibration plate in the camera coordinate system of the visual sensor, n c is the transpose of the three-dimensional normal vector, d c is the distance from the origin of the camera coordinate system to which the visual sensor belongs to the plane.

4. The method for environmental perception based on the fusion of visual sensor and laser radar information according to claim 3 is characterized in that: In step S12, the plane constraint equation is transformed to define the least squares problem: Assume that the left matrix is ​​expanded to n c *H*P laser = b, where b = -d c , further transform and decompose the left equation to get A*H=b (3) Among them, H is a 3×3 matrix, A is an N×9 matrix, b is an N×1 matrix, and N represents the number of groups of collected calibration data. The least squares problem is solved for the converted equation to obtain the following formula: J(x)=min||AH-b|| 2 (4) Among them, J(x) is a convex function, and its first-order derivative is set to 0, and H = (A T A) -1 A T b Assume that the SVD of matrix A is defined as: A=U∑V T (5) Among them, U is a 3×3 unitary matrix, ∑ is a 3×3 matrix (except for the elements on the main diagonal, the rest are zero, and each element on the main diagonal is called a singular value) and V is a 3×3 unitary matrix. Substituting formula (5) into formula (4) yields After solving H, substitute it into formula (2) to solve the transformation matrix and the final calibration result, and complete the data space alignment of the lidar and depth camera to obtain the fused point cloud data.

5. The method for environmental perception based on the fusion of visual sensor and laser radar information according to claim 4 is characterized in that: The step S20 further includes the following steps: S21: Divide the entire point cloud data into four sectors, traverse each sector, and calculate the internal point cloud data normal vector information. S22: The sector is refined into k grids of constant size, and the average height of the point cloud data inside each grid is calculated The standard deviation σ, and the normal vector n c ; S23: Determine the grid points that meet the threshold conditions. If all grid points meet the threshold conditions, add seed points and perform ground model fitting based on the seed points. S24: Determine whether each sector is completed, if not, return to step S21; S25: Traversing the ground models fitted in each sector, performing a sector classification point cloud stitching operation according to a conditional threshold, and obtaining the classified obstacle point cloud and ground point cloud information.

6. The method for environmental perception based on the fusion of visual sensor and laser radar information according to claim 5 is characterized in that: In step S22, the average height The standard deviation σ, and the normal vector n c Calculation formula: Among them, N is the total number of points in the current grid, z i is the z-axis coordinate of the i-th point in the current grid, v i Represents point p to center point p i The vector of Represents the average value of the vector, the normal vector n c It represents the eigenvector corresponding to the minimum eigenvalue of the covariance Cov.

7. The method for environmental perception based on the fusion of visual sensor and laser radar information according to claim 1 is characterized in that: In step S30, assuming that the general equation of the ground model is Ax+By+Cz+D=0, and the coordinates of the obstacle point in the three-dimensional space are (x0, y0, z0), the coordinates of the obstacle point after projection are: The projected obstacle point cloud is calculated through the above formula to complete the obstacle detection.

Images