Autonomous driving front-end matching method integrating geometric and semantic information

Abstract

The present invention discloses an autonomous driving front-end matching method that integrates geometric information and semantic information, comprising the following steps: based on the current frame point cloud, using a point cloud semantic segmentation network to perform semantic segmentation to obtain the semantic category of each point and its probability, using a cloth simulation filtering algorithm to perform ground filtering to obtain a ground point cloud and a non-ground point cloud; based on the non-ground point cloud, using a clustering algorithm to cluster the point cloud to obtain a non-ground point cloud cluster, and using a principal component analysis method to extract a directional bounding box for each non-ground point cloud cluster; using a spherical projection to map the current frame point cloud to a range image to obtain an OpenGL rectangular texture of the current frame; obtaining an OpenGL rectangular texture of a reference frame in a reference frame coordinate system; based on a shader operation, projecting the current frame to a reference frame coordinate system according to an estimated pose, calculating the point-to-surface distance residual and weight, Jacobian matrix and pose increment between the observation point and the matched point; using a Gauss-Newton method for iterative optimization to obtain an optimal pose estimate of the current frame.

Description

Technical Field

[0001] The present invention relates to the field of autonomous driving technology, and more specifically, to an autonomous driving parallel front-end matching method that integrates local geometric information and semantic information. Background Art

[0002] Providing accurate and real-time positioning is one of the key technologies for achieving autonomous driving of self-driving vehicles. Simultaneous Localization and Mapping (SLAM) has been a hot research topic in recent years. SLAM is used to solve the positioning and mapping problems of self-driving vehicles or mobile robots when working in unknown environments. In unknown environments, they can rely on their own sensors to determine their own position in the environmental map, and at the same time, they can build an environmental point cloud map based on the sensor information. LiDAR is less affected by environmental factors such as lighting and viewpoint changes, and provides high-precision point clouds, making it more likely to achieve higher positioning and mapping accuracy. The key to laser SLAM is to find the pose transformation between the current frame point cloud and the reference point cloud, but the data volume of LiDAR point clouds is large, the ICP (Internet Content Provider) calculation cost is high, and in dynamic scenes, it is easily affected by dynamic objects, resulting in a decrease in point cloud registration accuracy. Summary of the Invention

[0003] The purpose of the present invention is to overcome the problem of inaccurate laser SLAM front-end pose estimation in dynamic scenes in the prior art, and to provide an autonomous driving front-end matching method that integrates geometric information and semantic information. The method can reduce the influence of dynamic points on the accuracy of laser SLAM front-end pose estimation, improve the positioning accuracy of autonomous driving vehicles in dynamic scenes, and reduce calculation time, thereby providing reliable support for the positioning of autonomous driving vehicles in dynamic scenes.

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

[0005] A front-end matching method for autonomous driving that integrates geometric information and semantic information includes the following steps:

[0006] Based on the current frame point cloud of the lidar, semantic segmentation is performed using the point cloud semantic segmentation network to obtain the semantic category and semantic category probability of each point. The cloth simulation filtering algorithm is used for ground filtering to obtain ground point cloud and non-ground point cloud.

[0007] Based on the non-ground point cloud, the DBSCAN clustering algorithm is used to cluster the point cloud to obtain the non-ground point cloud clusters, and the principal component analysis method is used to extract the direction bounding box of each non-ground point cloud cluster;

[0008] Use spherical projection to map the current frame point cloud to the range image to obtain the OpenGL rectangular texture of the current frame, including coordinate texture, semantic texture, normal vector texture and direction bounding box texture;

[0009] Based on the global map, obtain the OpenGL rectangular texture of the reference frame in the reference frame coordinate system, including coordinate texture, semantic texture, normal vector texture and direction bounding box texture;

[0010] Based on shader operations, the current frame is projected into the reference frame coordinate system according to the estimated pose. Based on the reference frame texture pixels corresponding to the current frame texture pixels, the point-to-surface distance residual and residual weight between the current frame observation point and the reference frame matched point are calculated. The Jacobian matrix of the total residual with respect to the pose increment and the pose increment are calculated.

[0011] Based on shader operations, the Gauss-Newton method is used for iterative optimization to obtain the optimal pose estimation for the current frame.

[0012] Furthermore, semantic segmentation is performed using the point cloud semantic segmentation network to obtain the semantic category and semantic category probability of each point. The specific steps are as follows:

[0013] The RangeNet++ point cloud semantic segmentation network imports the trained weight file; inputs the point cloud, and uses RangeNet++ inference to obtain the probability of each point belonging to each semantic category; for each point, the semantic category with the highest probability is selected as its semantic label, and the corresponding probability is saved;

[0014] Use the cloth simulation filtering algorithm to perform ground filtering to obtain ground point cloud and non-ground point cloud. The specific steps are as follows:

[0015] The cloth simulation model sets the resolution, stiffness, and number of iterations parameters; inputs the point cloud, and through iterative reasoning by the cloth simulation model, the point cloud is segmented into ground points and non-ground points.

[0016] Furthermore, based on the non-ground point cloud, the DBSCAN clustering algorithm is used to cluster the point cloud to obtain non-ground point cloud clusters, and the principal component analysis method is used to extract the directional bounding box of each non-ground point cloud cluster. The specific steps are as follows:

[0017] The DBSCAN clustering algorithm sets the cluster radius and the number of neighborhood samples required for a sample point to become a core object;

[0018] Input non-ground point cloud, iterate the DBSCAN clustering algorithm to obtain multiple non-ground point cloud clusters;

[0019] The principal component analysis method is used to solve the size and pose of the directional bounding box of each non-ground point cloud cluster. The specific process is as follows:

[0020] Calculate the covariance matrix A of each non-ground point cloud cluster and get

[0021]

[0022] Where (x, y, z) is the three-dimensional coordinate of the point in the point cloud cluster;

[0023] Perform eigenvalue decomposition and eigenvector calculation on the covariance matrix A, use the eigenvectors as column elements to form a matrix, and obtain the rotation matrix B of the non-ground point cloud cluster relative to the current frame coordinate system;

[0024] Multiply each point of the non-ground point cloud cluster by the inverse of the rotation matrix B to transform it to its directional bounding box posture, count the extreme values ​​of the coordinates, and obtain the geometric center and size of the bounding box of the non-ground point cloud cluster in the directional bounding box posture;

[0025] Multiply the geometric center by the rotation matrix B to the left to transform it to the current frame coordinate system to obtain the position of the bounding box.

[0026] Furthermore, spherical projection is used to map the current frame point cloud to the range image. The specific steps are as follows:

[0027] Project the current frame point cloud onto the sphere, and use Expressed as

[0028]

[0029] Where, θ represents the horizontal angle; represents the pitch angle; f up represents the maximum elevation angle of the laser radar scan line; w represents the number of points obtained by rotating the laser radar scan line one circle; h represents the number of laser radar scan lines; (u, v) represents the coordinates of the point in the current frame point cloud projected on the two-dimensional depth image;

[0030] Use OpenGL rectangular texture to create coordinate texture, and the RGB channels of RGBA pixels store the information of the corresponding three-dimensional coordinate points (x, y, z) of the pixels;

[0031] Use OpenGL rectangular texture to create semantic texture. The R channel of RGBA pixel stores the semantic label of the corresponding 3D coordinate point of the pixel, and the G channel of RGBA pixel stores the semantic category probability of the corresponding 3D coordinate point of the pixel.

[0032] The pixel E on the coordinate texture has two adjacent pixels D and F. D and F are not in the same row or column. The cross product of vectors ED and EF is used as the normal vector of pixel E. The normal vector texture is created using OpenGL rectangular texture. The RGB channels of the RGBA pixel store the unit normal vector coordinates of the corresponding pixel's three-dimensional coordinate point.

[0033] The directional bounding box texture is created using an OpenGL rectangular texture. The RGBA channel of an RGBA pixel stores the size and pose of the directional bounding box of the corresponding pixel.

[0034] Furthermore, an OpenGL rectangular texture is used to create a directional bounding box texture. The RGBA channel of an RGBA pixel stores the size and position of the directional bounding box of the corresponding pixel. The specific process is as follows:

[0035] Simplify the orientation bounding box and only consider the yaw motion to obtain

[0036]

[0037] In the formula, [x x , x y ,0] represents the unit vector of the x-axis of the direction bounding box coordinate system in the current frame coordinate system; [y x ,y y ,0] represents the unit vector of the y-axis of the bounding box coordinate system in the current frame coordinate system; [t x , t y , t z ] represents the position of the bounding box; T represents the pose matrix of the direction bounding box;

[0038] Set the bounding box size x len 、y len 、z len Integrate into the pose matrix and get

[0039]

[0040] Each non-zero element in T1 is represented by a half-precision floating point number. According to the IEEE754 specification, x x *x len and x y *x len Encoded into a single-precision floating point number as the R channel of the directional bounding box texture; x *y len and y y *y len Encoded into a single-precision floating point number as the G channel of the directional bounding box texture; z len and t x Encode into a single-precision floating point number as the B channel of the directional bounding box texture; y and t z Encoded as a single-precision floating-point number as the A channel of the directional bounding box texture.

[0041] Furthermore, based on the global map, the OpenGL rectangular texture of the reference frame in the reference frame coordinate system is obtained, including coordinate texture, semantic texture, normal vector texture, and direction bounding box texture. The specific steps are as follows:

[0042] Based on the global map, OpenGL rendering is used to obtain the coordinate texture, semantic texture and normal vector texture of the reference frame in the reference frame coordinate system;

[0043] Based on the OpenGL context, the coordinate texture of the reference frame is transferred from the GPU (Graphics Processing Unit) to the CPU to obtain the reference frame point cloud;

[0044] Based on the reference frame point cloud, the cloth simulation filtering algorithm is used to perform ground filtering to obtain the ground point cloud and the non-ground point cloud;

[0045] Based on the non-ground point cloud, the DBSCAN clustering algorithm is used to cluster the point cloud to obtain the non-ground point cloud clusters, and the principal component analysis method is used to extract the direction bounding box of each non-ground point cloud cluster;

[0046] The directional bounding box texture of the reference frame point cloud is created using an OpenGL rectangular texture. The RGBA channel of an RGBA pixel stores the size and pose of the directional bounding box of the corresponding pixel.

[0047] Furthermore, the residual distance between the observation point of the current frame and the matched point of the reference frame and the residual weight are calculated. The specific steps are as follows:

[0048] Map the coordinate texture of the current frame to the reference frame coordinate system through the estimated pose, and directly match the points in the current frame with the points in the reference frame through the image index;

[0049] Calculate the distance from all observation points in the current frame to the plane where the corresponding points in the reference frame are located, and take the residual sum of the point-to-plane distances of all observation points in the current frame and the matched points in the reference frame as the optimization target, and get

[0050]

[0051] Where n m is the normal vector of the point in the reference frame; represents the estimated pose from the current frame to the reference frame at the kth iteration; v d Indicates the observation point in the current frame; v m Indicates the matched point in the reference frame; Represents all observation points in the current frame; Represents all matched points in the reference frame; Represents the normal vectors of all matched points in the reference frame;

[0052] Calculate the huber kernel weight, semantic weight, and geometric weight of the residuals of all observation points in the current frame.

[0053] Furthermore, the huber kernel function weight, semantic weight and geometric weight of the residual of all observation points in the current frame are calculated. The specific process is as follows:

[0054] Calculate the huber kernel function weight and get

[0055]

[0056] Where r i Indicates the distance from the i-th point in the current frame to the plane where the corresponding point in the reference frame is located;

[0057] Calculate the semantic weight and get

[0058]

[0059] Where S D (i) represents the pixel of the semantic texture of the i-th point in the current frame; S M (i) represents the pixel of the semantic texture of the matched point in the reference frame corresponding to the i-th point in the current frame; Represents the semantic label of the i-th point in the current frame; Indicates the semantic label of the matched point of the reference frame corresponding to the i-th point of the current frame; P(l SD(i) |i) represents the semantic category probability of the i-th point in the current frame. If the semantic category of the observation point is the same as that of the matched point, the semantic weight of the observation point is the semantic category probability;

[0060] Calculate the geometric weights and convert the direction bounding box of the current frame observation point to the reference frame coordinate system, ignoring the pitch and yaw motions of the vehicle. The intersection-and-union ratio of the three-dimensional bounding box is converted to the product of the intersection-and-union ratio of the two rectangles with rotation angles on the top view and the intersection-and-union ratio of the elevation on the front view. The specific process is as follows:

[0061] The Sutherland-Hodgman algorithm is used to find the vertices of the intersecting polygons of two rectangular boxes with rotation angles on the top view.

[0062] Use Gauss's area formula to solve the area of ​​intersecting polygons and calculate the area of ​​two rectangular boxes;

[0063] According to the areas of the two rectangular boxes, calculate the intersection-and-union ratio of the directional bounding box on the top view and the intersection-and-union ratio of the elevation on the front view, and multiply the two to get the intersection-and-union ratio of the directional bounding box;

[0064] Multiply the huber kernel function weight, semantic weight, and geometric weight to obtain the weight of the residual distance between the observation point and the corresponding matched point.

[0065] Furthermore, the Jacobian matrix and the pose increment of the total residual with respect to the pose increment are calculated. The specific steps are:

[0066] Current frame observation point v d The residual distance between the point and the plane where the matched point of the corresponding reference frame is r d , calculate the corresponding row J of the corresponding Jacobian matrix i , get the Jacobian matrix J, J i The calculation formula is

[0067] J i =[n m T ,(n m xv m ) T ];

[0068] Calculate the pose increment and get

[0069] δ=(J T WJ) -1 J T Wr

[0070] Where W∈R n×n represents the weight diagonal matrix, w uu Residual r u The weight of r∈R n×1 Represents the residual vector; J∈R n×n represents the Jacobian matrix of the residual r with respect to the pose increment δ; the Lie algebra δ∈se(3) represents the linear pose increment.

[0071] Furthermore, based on shader operations, the Gauss-Newton method is used for iterative optimization to obtain the optimal pose estimation for the current frame. The specific steps are as follows:

[0072] For the kth iteration, verify whether the change of the minimum pose increment δ relative to the k-1th iteration is less than the given threshold. If it is less than the given threshold, end the iteration;

[0073] For the kth iteration, verify whether the change of the residual r relative to the k-1th iteration is less than the given threshold. If it is less than the given threshold, end the iteration;

[0074] For the kth iteration, verify whether the number of iterations is greater than or equal to the maximum number of iterations. If it is greater than or equal to the maximum number of iterations, end the iteration;

[0075] If the iteration is completed, the optimal pose estimate is output. If the iteration is not completed, the estimated pose is used as the initial pose and the iterative optimization is repeated until the optimal pose estimate of the current frame is obtained.

[0076] Compared with the prior art, the present invention has the following advantages and beneficial effects:

[0077] To address the problem of inaccurate pose estimation by the laser SLAM front-end in dynamic scenes, the present invention adopts a method of fusing semantic information and geometric information. By segmenting and clustering the point cloud, directional bounding boxes are extracted. The IOU (Intersection Over Union) of the directional bounding boxes of the matching points is used to introduce geometric weights. The consistency of the semantic labels of the matching points is used to introduce semantic weights. This reduces the influence of dynamic objects on pose estimation and improves the accuracy of front-end pose estimation.

[0078] The present invention adopts OpenGL (Open Graphics Library) rectangular texture to represent the current frame and reference frame, takes advantage of the parallel computing characteristics of shaders, and directly pairs them through image indexes, which reduces the time consumption of point cloud registration and improves the calculation speed of laser SLAM front-end. BRIEF DESCRIPTION OF THE DRAWINGS

[0079] Figure 1 Schematic diagram of the flow of the autonomous driving front-end matching method that integrates geometric information and semantic information of the present invention.

[0080] Figure 2 The figure is a flowchart of an autonomous driving front-end matching method that integrates geometric information and semantic information in a specific embodiment.

[0081] Figure 3 The figure is a flowchart of a specific embodiment for processing the point cloud of the current frame.

[0082] Figure 4 The figure is a flowchart of obtaining a reference frame from a global map according to a specific embodiment. DETAILED DESCRIPTION

[0083] The autonomous driving front-end matching method that integrates geometric information and semantic information of the present invention is further described below with reference to the accompanying drawings and specific embodiments.

[0084] The present invention is a parallel front-end matching method for autonomous driving that integrates local geometric information and semantic information. It considers the geometric information and semantic information of dynamic scenes, adjusts the residual weights of point-to-surface distances that may be dynamic points, improves the pose estimation accuracy of the laser SLAM front-end in dynamic scenes, and reduces calculation time.

[0085] See also Figure 1 and Figure 2 The present invention discloses an autonomous driving front-end matching method that integrates geometric information and semantic information, which specifically includes the following steps:

[0086] Based on the current frame point cloud of the lidar, semantic segmentation is performed using the point cloud semantic segmentation network to obtain the semantic category and semantic category probability of each point. The cloth simulation filtering algorithm is used for ground filtering to obtain ground point cloud and non-ground point cloud.

[0087] Based on the non-ground point cloud, the DBSCAN clustering algorithm is used to cluster the point cloud to obtain the non-ground point cloud clusters, and the principal component analysis method is used to extract the direction bounding box of each non-ground point cloud cluster;

[0088] Use spherical projection to map the current frame point cloud to the range image to obtain the OpenGL rectangular texture of the current frame, including coordinate texture, semantic texture, normal vector texture and direction bounding box texture;

[0089] Based on the global map, obtain the OpenGL rectangular texture of the reference frame in the reference frame coordinate system, including coordinate texture, semantic texture, normal vector texture and direction bounding box texture;

[0090] Based on shader operations, the current frame is projected into the reference frame coordinate system according to the estimated pose. Based on the reference frame texture pixels corresponding to the current frame texture pixels, the point-to-surface distance residual and residual weight between the current frame observation point and the reference frame matched point are calculated. The Jacobian matrix of the total residual with respect to the pose increment and the pose increment are calculated.

[0091] Based on shader operations, the Gauss-Newton method is used for iterative optimization to obtain the optimal pose estimation for the current frame.

[0092] See also Figure 2 and Figure 3 Specifically, semantic segmentation is performed using a point cloud semantic segmentation network to obtain the semantic category and semantic category probability of each point, including the following steps:

[0093] First, the RangeNet++ point cloud semantic segmentation network imports the trained weight file.

[0094] Secondly, the point cloud is input and reasoned with RangeNet++ to obtain the probability of each point belonging to each semantic category.

[0095] Then, each point selects the semantic category with the highest probability as its semantic label and saves the corresponding probability.

[0096] Specifically, the cloth simulation filtering algorithm is used to perform ground filtering to obtain ground point cloud and non-ground point cloud, including the following steps:

[0097] First, the cloth simulation model sets the resolution, stiffness, and iteration parameters.

[0098] Then, the point cloud is input and segmented into ground points and non-ground points through iterative reasoning of the cloth simulation model.

[0099] Specifically, based on the non-ground point cloud, the DBSCAN clustering algorithm is used to cluster the point cloud to obtain non-ground point cloud clusters, and the principal component analysis method is used to extract the directional bounding box of each non-ground point cloud cluster, including the following steps:

[0100] First, the DBSCAN clustering algorithm sets the cluster radius, which is the number of neighborhood samples required for a sample point to become a core object.

[0101] Secondly, the non-ground point cloud is input and the DBSCAN clustering algorithm is iterated to obtain multiple non-ground point cloud clusters.

[0102] Then, principal component analysis is used to solve the size and pose of the directional bounding box for each non-ground point cloud cluster.

[0103] The principal component analysis method is used to solve the size and pose of the directional bounding box of each non-ground point cloud cluster. The specific process is as follows:

[0104] Calculate the covariance matrix A of each non-ground point cloud cluster and get

[0105]

[0106] Where (x, y, z) is the three-dimensional coordinate of the point in the point cloud cluster;

[0107] Perform eigenvalue decomposition and eigenvector calculation on the covariance matrix A, use the eigenvectors as column elements to form a matrix, and obtain the rotation matrix B of the non-ground point cloud cluster relative to the current frame coordinate system;

[0108] Multiply each point of the non-ground point cloud cluster by the inverse of the rotation matrix B to transform it to its directional bounding box posture, count the extreme values ​​of the coordinates, and obtain the geometric center and size of the bounding box of the non-ground point cloud cluster in the directional bounding box posture;

[0109] Multiply the geometric center by the rotation matrix B to the left to transform it to the current frame coordinate system to obtain the position of the bounding box.

[0110] Specifically, spherical projection is used to map the current frame point cloud to the range image, including the following steps:

[0111] First, the current frame point cloud is projected onto the sphere, and all points are Expressed as

[0112]

[0113] Where, θ represents the horizontal angle; represents the pitch angle; f uprepresents the maximum elevation angle of the laser radar scan line; w represents the number of points obtained when the laser radar scan line rotates one circle; h represents the number of laser radar scan lines; (u, v) represents the coordinates of the point in the current frame point cloud projected on the two-dimensional depth image.

[0114] Secondly, the coordinate texture is created using OpenGL rectangular texture, and the RGB channels of the RGBA pixel store the information of the corresponding pixel three-dimensional coordinate point (x, y, z).

[0115] Then, the semantic texture is created using OpenGL rectangular texture. The R channel of the RGBA pixel stores the semantic label of the corresponding pixel's three-dimensional coordinate point, and the G channel of the RGBA pixel stores the semantic category probability of the corresponding pixel's three-dimensional coordinate point.

[0116] Next, the pixel point E on the coordinate texture has two adjacent pixels D and F, which are not in the same row or column. The cross product of vectors ED and EF is used as the normal vector of pixel point E. The normal vector texture is created using OpenGL rectangular texture. The RGB channels of the RGBA pixel store the unit normal vector coordinates of the corresponding pixel's three-dimensional coordinate point.

[0117] Finally, the directional bounding box texture is created using the OpenGL rectangle texture, and the RGBA channels of the RGBA pixels store the size and pose of the directional bounding box of the corresponding pixel.

[0118] Among them, the OpenGL rectangular texture is used to create the directional bounding box texture. The RGBA channel of the RGBA pixel stores the size and position of the directional bounding box of the corresponding pixel. The specific process is:

[0119] Simplify the orientation bounding box and only consider the yaw motion to obtain

[0120]

[0121] In the formula, [x x , x y ,0] represents the unit vector of the x-axis of the direction bounding box coordinate system in the current frame coordinate system; [y x ,y y ,0] represents the unit vector of the y-axis of the bounding box coordinate system in the current frame coordinate system; [t x , t y , t z ] represents the position of the bounding box; T represents the pose matrix of the direction bounding box;

[0122] Set the bounding box size x len 、y len 、z len Integrate into the pose matrix and get

[0123]

[0124] Each non-zero element in T1 is represented by a half-precision floating point number (16 bits). According to the IEEE754 specification, x x *x len and x y *x len Encoded into a single-precision floating point number (32 bits) as the R channel of the directional bounding box texture; x *y len and y y *y len Encoded into a single-precision floating point number as the G channel of the directional bounding box texture; z len and t x Encode into a single-precision floating point number as the B channel of the directional bounding box texture; y and t z Encoded as a single-precision floating-point number as the A channel of the directional bounding box texture.

[0125] See also Figure 2 and Figure 4 Specifically, based on the global map, obtaining the OpenGL rectangular texture of the reference frame in the reference frame coordinate system, including coordinate texture, semantic texture, normal vector texture, and direction bounding box texture, includes the following steps:

[0126] Firstly, based on the global map, OpenGL rendering is used to obtain the coordinate texture, semantic texture and normal vector texture of the reference frame in the reference frame coordinate system.

[0127] Secondly, based on the OpenGL context, the coordinate texture of the reference frame is converted from the GPU to the CPU to obtain the reference frame point cloud.

[0128] Then, based on the reference frame point cloud, the cloth simulation filtering algorithm is used to perform ground filtering to obtain the ground point cloud and non-ground point cloud. The specific process is the same as the processing steps of the current frame point cloud.

[0129] Next, based on the non-ground point cloud, the DBSCAN clustering algorithm is used to cluster the point cloud to obtain non-ground point cloud clusters. The principal component analysis method is used to extract the directional bounding box of each non-ground point cloud cluster. The specific process is the same as the processing steps of the current frame point cloud.

[0130] Finally, an OpenGL rectangular texture is used to create a directional bounding box texture for the reference frame point cloud. The RGBA channels of the RGBA pixels store the size and pose of the directional bounding box of the corresponding pixel. The specific process is the same as the processing steps of the current frame point cloud.

[0131] Specifically, the point-to-surface distance residual and the residual weight between the observation point of the current frame and the matched point of the reference frame are calculated, including the following steps:

[0132] First, the coordinate texture of the current frame is mapped to the reference frame coordinate system through the estimated pose, and the points in the current frame are directly matched with the points in the reference frame through the image index.

[0133] Secondly, calculate the distance from all observation points in the current frame to the plane where the corresponding points in the reference frame are located, and take the residual sum of the point-to-plane distances of all observation points in the current frame and the matched points in the reference frame as the optimization target, and get

[0134]

[0135] Where n m is the normal vector of the point in the reference frame; represents the estimated pose from the current frame to the reference frame at the kth iteration; v d Indicates the observation point in the current frame; v m Indicates the matched point in the reference frame; Represents all observation points in the current frame; Represents all matched points in the reference frame; Represents the normal vectors of all matched points in the reference frame.

[0136] Then, the huber kernel function weight, semantic weight and geometric weight of the residual of all observation points in the current frame are calculated.

[0137] Specifically, the huber kernel function weight, semantic weight, and geometric weight of the residuals of all observation points in the current frame are calculated, including the following steps:

[0138] First, calculate the huber kernel function weight and get

[0139]

[0140] Where r i Indicates the distance from the i-th point in the current frame to the plane where the corresponding point in the reference frame is located.

[0141] Secondly, calculate the semantic weight and get

[0142]

[0143] Where S D (i) represents the pixel of the semantic texture of the i-th point in the current frame; S M (i) represents the pixel of the semantic texture of the matched point in the reference frame corresponding to the i-th point in the current frame; Represents the semantic label of the i-th point in the current frame; Indicates the semantic label of the matched point of the reference frame corresponding to the i-th point of the current frame; P(l SD(i)|i) represents the semantic category probability of the i-th point in the current frame. If the semantic category of the observation point is the same as that of the matched point, the semantic weight of the observation point is the semantic category probability.

[0144] Then, the geometric weight is calculated. When the direction bounding box of the observation point in the current frame is converted to the reference frame coordinate system, the pitch and yaw motions of the vehicle are ignored. The intersection-and-union ratio of the three-dimensional bounding box is converted to the product of the intersection-and-union ratio of the two rectangles with rotation angles on the top view and the intersection-and-union ratio of the elevation on the front view.

[0145] Next, the huber kernel function weight, semantic weight, and geometric weight are multiplied together to obtain the weight of the residual distance between the observation point and the corresponding matched point.

[0146] Among them, the geometric weight is calculated. When converting the direction bounding box of the current frame observation point to the reference frame coordinate system, the pitch and yaw motions of the vehicle are ignored. The intersection and union ratio of the three-dimensional bounding box is converted to the product of the intersection and union ratio of the two rectangles with rotation angles on the top view and the intersection and union ratio of the elevation on the front view. The specific process is as follows:

[0147] The Sutherland-Hodgman algorithm is used to find the vertices of the intersecting polygons of two rectangular boxes with rotation angles on the top view.

[0148] Use Gauss's area formula to solve the area of ​​intersecting polygons and calculate the area of ​​two rectangular boxes;

[0149] According to the areas of the two rectangular boxes, the intersection and union ratio of the directional bounding box on the top view and the intersection and union ratio of the elevation on the front view are calculated, and the two are multiplied to obtain the intersection and union ratio of the directional bounding box.

[0150] Specifically, calculating the Jacobian matrix of the total residual with respect to the pose increment and the pose increment includes the following steps:

[0151] First, the current frame observation point v d The residual distance between the point and the plane where the matched point of the corresponding reference frame is r d , calculate the corresponding row J of the corresponding Jacobian matrix i , get the Jacobian matrix J, J i The calculation formula is

[0152] J i =[n m T ,(n m xv m ) T ].

[0153] Secondly, calculate the pose increment and get

[0154] δ=(J T WJ) -1 JT Wr

[0155] Where W∈R n×n represents the weight diagonal matrix, w uu Residual r u The weight of r∈R n×1 Represents the residual vector; J∈R n×n represents the Jacobian matrix of the residual r with respect to the pose increment δ; the Lie algebra δ∈se(3) represents the linear pose increment.

[0156] Specifically, based on shader operations, the Gauss-Newton method is used for iterative optimization to obtain the optimal pose estimation for the current frame, including the following steps:

[0157] For the kth iteration, verify whether the change of the minimum pose increment δ relative to the k-1th iteration is less than the given threshold. If it is less than the given threshold, end the iteration;

[0158] For the kth iteration, verify whether the change of the residual r relative to the k-1th iteration is less than the given threshold. If it is less than the given threshold, end the iteration;

[0159] For the kth iteration, verify whether the number of iterations is greater than or equal to the maximum number of iterations. If it is greater than or equal to the maximum number of iterations, end the iteration;

[0160] If the iteration is completed, the optimal pose estimate is output. If the iteration is not completed, the estimated pose is used as the initial pose and the iterative optimization is repeated until the optimal pose estimate of the current frame is obtained.

[0161] The above description is a detailed description of the preferred embodiments of the present invention, but the embodiments are not intended to limit the scope of the patent application of the present invention. Any equivalent changes or modifications made under the technical spirit disclosed by the present invention should fall within the patent scope covered by the present invention.

Claims

1. A front-end matching method for autonomous driving that integrates geometric information and semantic information, characterized in that: The following steps are involved: Based on the current frame point cloud of the lidar, semantic segmentation is performed using the point cloud semantic segmentation network to obtain the semantic category and semantic category probability of each point. The cloth simulation filtering algorithm is used for ground filtering to obtain ground point cloud and non-ground point cloud. Based on the non-ground point cloud, the DBSCAN clustering algorithm is used to cluster the point cloud to obtain the non-ground point cloud clusters, and the principal component analysis method is used to extract the direction bounding box of each non-ground point cloud cluster; Use spherical projection to map the current frame point cloud to the range image to obtain the OpenGL rectangular texture of the current frame, including coordinate texture, semantic texture, normal vector texture and direction bounding box texture; Based on the global map, obtain the OpenGL rectangular texture of the reference frame in the reference frame coordinate system, including coordinate texture, semantic texture, normal vector texture and direction bounding box texture; Based on shader operations, the current frame is projected into the reference frame coordinate system according to the estimated pose. Based on the reference frame texture pixels corresponding to the current frame texture pixels, the point-to-surface distance residual and residual weight between the current frame observation point and the reference frame matched point are calculated. The Jacobian matrix of the total residual with respect to the pose increment and the pose increment are calculated. Based on shader operations, the Gauss-Newton method is used for iterative optimization to obtain the optimal pose estimation for the current frame.

2. The autonomous driving front-end matching method integrating geometric information and semantic information according to claim 1, characterized in that: Use the point cloud semantic segmentation network to perform semantic segmentation and obtain the semantic category and semantic category probability of each point. The specific steps are as follows: The RangeNet++ point cloud semantic segmentation network imports the trained weight file; inputs the point cloud, and uses RangeNet++ inference to obtain the probability of each point belonging to each semantic category; for each point, the semantic category with the highest probability is selected as its semantic label, and the corresponding probability is saved; Use the cloth simulation filtering algorithm to perform ground filtering to obtain ground point cloud and non-ground point cloud. The specific steps are as follows: The cloth simulation model sets the resolution, stiffness, and number of iterations parameters; inputs the point cloud, and through iterative reasoning by the cloth simulation model, the point cloud is segmented into ground points and non-ground points.

3. The autonomous driving front-end matching method integrating geometric information and semantic information according to claim 1, characterized in that: Based on the non-ground point cloud, the DBSCAN clustering algorithm is used to cluster the point cloud to obtain the non-ground point cloud clusters. The principal component analysis method is used to extract the direction bounding box of each non-ground point cloud cluster. The specific steps are as follows: The DBSCAN clustering algorithm sets the cluster radius and the number of neighborhood samples required for a sample point to become a core object; Input non-ground point cloud, iterate the DBSCAN clustering algorithm to obtain multiple non-ground point cloud clusters; The principal component analysis method is used to solve the size and pose of the directional bounding box of each non-ground point cloud cluster. The specific process is as follows: Calculate the covariance matrix A of each non-ground point cloud cluster and get Where (x, y, z) is the three-dimensional coordinate of the point in the point cloud cluster; Perform eigenvalue decomposition and eigenvector calculation on the covariance matrix A, use the eigenvectors as column elements to form a matrix, and obtain the rotation matrix B of the non-ground point cloud cluster relative to the current frame coordinate system; Multiply each point of the non-ground point cloud cluster by the inverse of the rotation matrix B to transform it to its directional bounding box posture, count the extreme values ​​of the coordinates, and obtain the geometric center and size of the bounding box of the non-ground point cloud cluster in the directional bounding box posture; Multiply the geometric center by the rotation matrix B to the left to transform it to the current frame coordinate system to obtain the position of the bounding box.

4. The autonomous driving front-end matching method integrating geometric information and semantic information according to claim 3, characterized in that: Use spherical projection to map the current frame point cloud to the range image. The specific steps are: Project the current frame point cloud onto the sphere, and use Expressed as Where, θ represents the horizontal angle; represents the pitch angle; f up represents the maximum elevation angle of the laser radar scan line; w represents the number of points obtained by rotating the laser radar scan line one circle; h represents the number of laser radar scan lines; (u, v) represents the coordinates of the point in the current frame point cloud projected on the two-dimensional depth image; Use OpenGL rectangular texture to create coordinate texture, and the RGB channels of RGBA pixels store the information of the corresponding three-dimensional coordinate points (x, y, z) of the pixels; Use OpenGL rectangular texture to create semantic texture. The R channel of RGBA pixel stores the semantic label of the corresponding 3D coordinate point of the pixel, and the G channel of RGBA pixel stores the semantic category probability of the corresponding 3D coordinate point of the pixel. The pixel E on the coordinate texture has two adjacent pixels D and F. D and F are not in the same row or column. The cross product of vectors ED and EF is used as the normal vector of pixel E. The normal vector texture is created using OpenGL rectangular texture. The RGB channels of the RGBA pixel store the unit normal vector coordinates of the corresponding pixel's three-dimensional coordinate point. The directional bounding box texture is created using an OpenGL rectangular texture. The RGBA channel of an RGBA pixel stores the size and pose of the directional bounding box of the corresponding pixel.

5. The autonomous driving front-end matching method integrating geometric information and semantic information according to claim 4, characterized in that: Use OpenGL rectangular texture to create a directional bounding box texture. The RGBA channel of the RGBA pixel stores the size and position of the directional bounding box of the corresponding pixel. The specific process is: Simplify the orientation bounding box and only consider the yaw motion to obtain In the formula, [x x , x y ,0] represents the unit vector of the x-axis of the direction bounding box coordinate system in the current frame coordinate system; [y x ,y y ,0] represents the unit vector of the y-axis of the bounding box coordinate system in the current frame coordinate system; [t x , t y , t z ] represents the position of the bounding box; T represents the pose matrix of the direction bounding box; Set the bounding box size x len 、y len 、z len Integrate into the pose matrix and get Each non-zero element in T1 is represented by a half-precision floating point number. According to the IEEE754 specification, x x *x len and x y *x len Encoded into a single-precision floating point number as the R channel of the directional bounding box texture; x *y len and y y *y len Encoded into a single-precision floating point number as the G channel of the directional bounding box texture; z len and t x Encode into a single-precision floating point number as the B channel of the directional bounding box texture; y and t z Encoded as a single-precision floating-point number as the A channel of the directional bounding box texture.

6. The autonomous driving front-end matching method integrating geometric information and semantic information according to claim 5, characterized in that: Based on the global map, obtain the OpenGL rectangular texture of the reference frame in the reference frame coordinate system, including coordinate texture, semantic texture, normal vector texture, and direction bounding box texture. The specific steps are: Based on the global map, OpenGL rendering is used to obtain the coordinate texture, semantic texture and normal vector texture of the reference frame in the reference frame coordinate system; Based on the OpenGL context, the coordinate texture of the reference frame is converted from the GPU to the CPU to obtain the reference frame point cloud; Based on the reference frame point cloud, the cloth simulation filtering algorithm is used to perform ground filtering to obtain the ground point cloud and the non-ground point cloud; Based on the non-ground point cloud, the DBSCAN clustering algorithm is used to cluster the point cloud to obtain the non-ground point cloud clusters, and the principal component analysis method is used to extract the direction bounding box of each non-ground point cloud cluster; The directional bounding box texture of the reference frame point cloud is created using an OpenGL rectangular texture. The RGBA channel of an RGBA pixel stores the size and pose of the directional bounding box of the corresponding pixel.

7. The autonomous driving front-end matching method integrating geometric information and semantic information according to claim 6, characterized in that: Calculate the point-to-surface distance residual and residual weight between the observation point of the current frame and the matched point of the reference frame. The specific steps are: Map the coordinate texture of the current frame to the reference frame coordinate system through the estimated pose, and directly match the points in the current frame with the points in the reference frame through the image index; Calculate the distance from all observation points in the current frame to the plane where the corresponding points in the reference frame are located, and take the residual sum of the point-to-plane distances of all observation points in the current frame and the matched points in the reference frame as the optimization target, and get Where n m is the normal vector of the point in the reference frame; represents the estimated pose from the current frame to the reference frame at the kth iteration; v d Indicates the observation point in the current frame; v m Indicates the matched point in the reference frame; Represents all observation points in the current frame; Represents all matched points in the reference frame; Represents the normal vectors of all matched points in the reference frame; Calculate the huber kernel weight, semantic weight, and geometric weight of the residuals of all observation points in the current frame.

8. The autonomous driving front-end matching method integrating geometric information and semantic information according to claim 7, characterized in that: Calculate the huber kernel function weight, semantic weight and geometric weight of the residual of all observation points in the current frame. The specific process is: Calculate the huber kernel function weight and get Where r i Indicates the distance from the i-th point in the current frame to the plane where the corresponding point in the reference frame is located; Calculate the semantic weight and get Where S D (i) represents the pixel of the semantic texture of the i-th point in the current frame; S M (i) represents the pixel of the semantic texture of the matched point in the reference frame corresponding to the i-th point in the current frame; Represents the semantic label of the i-th point in the current frame; Indicates the semantic label of the matched point of the reference frame corresponding to the i-th point of the current frame; P(l SD(i) |i) represents the semantic category probability of the i-th point in the current frame. If the semantic category of the observation point is the same as that of the matched point, the semantic weight of the observation point is the semantic category probability; Calculate the geometric weights and convert the direction bounding box of the current frame observation point to the reference frame coordinate system, ignoring the pitch and yaw motions of the vehicle. The intersection-and-union ratio of the three-dimensional bounding box is converted to the product of the intersection-and-union ratio of the two rectangles with rotation angles on the top view and the intersection-and-union ratio of the elevation on the front view. The specific process is as follows: The Sutherland-Hodgman algorithm is used to find the vertices of the intersecting polygons of two rectangular boxes with rotation angles on the top view. Use Gauss's area formula to solve the area of ​​intersecting polygons and calculate the area of ​​two rectangular boxes; According to the areas of the two rectangular boxes, calculate the intersection-and-union ratio of the directional bounding box on the top view and the intersection-and-union ratio of the elevation on the front view, and multiply the two to get the intersection-and-union ratio of the directional bounding box; Multiply the huber kernel function weight, semantic weight, and geometric weight to obtain the weight of the residual distance between the observation point and the corresponding matched point.

9. The autonomous driving front-end matching method integrating geometric information and semantic information according to claim 8, characterized in that: Calculate the Jacobian matrix and pose increment of the total residual with respect to the pose increment. The specific steps are: Current frame observation point v d The residual distance between the point and the plane where the matched point of the corresponding reference frame is r d , calculate the corresponding row J of the corresponding Jacobian matrix i , get the Jacobian matrix J, J i The calculation formula is J i =[n m T ,(n m x v m ) T ]; Calculate the pose increment and get δ=(J T WJ) -1 J T Wr Where W∈R n×n represents the weight diagonal matrix; r∈R n×1 Represents the residual vector; J∈R n×n represents the Jacobian matrix of the residual r with respect to the pose increment δ; the Lie algebra δ∈se(3) represents the linear pose increment.

10. The autonomous driving front-end matching method integrating geometric information and semantic information according to claim 9, characterized in that: Based on shader operations, the Gauss-Newton method is used for iterative optimization to obtain the optimal pose estimation for the current frame. The specific steps are as follows: For the kth iteration, verify whether the change of the minimum pose increment δ relative to the k-1th iteration is less than the given threshold. If it is less than the given threshold, end the iteration; For the kth iteration, verify whether the change of the residual r relative to the k-1th iteration is less than the given threshold. If it is less than the given threshold, end the iteration; For the kth iteration, verify whether the number of iterations is greater than or equal to the maximum number of iterations. If it is greater than or equal to the maximum number of iterations, end the iteration; If the iteration is completed, the optimal pose estimate is output. If the iteration is not completed, the estimated pose is used as the initial pose and the iterative optimization is repeated until the optimal pose estimate of the current frame is obtained.

Images