Multi-sensor cooperative positioning and mapping method based on point cloud feature enhancement

Abstract

The invention discloses a multi-sensor cooperative positioning and mapping method based on point cloud feature enhancement. The method comprises the following steps: firstly, acquiring laser radar point cloud data generated by a laser radar, image data shot by a camera, and a calibration file between the laser radar and the camera; then generating a virtual laser radar point cloud with pixel level density by using a depth completion network; then, features of the laser radar point cloud and the virtual laser radar point cloud are extracted respectively, and the features are classified and recombined; then, according to the extracted edge feature points and plane feature points, carrying out accurate six-degree-of-freedom pose estimation on the vehicle; and finally, constructing a feature map by using the calculated six-degree-of-freedom poses. According to the invention, through combined processing of virtual point cloud and real laser point cloud features, the precision and robustness of multi-sensor cooperative positioning and mapping are significantly improved.

Description

Technical Field

[0001] The present invention belongs to the technical field of autonomous driving, and in particular relates to a multi-sensor collaborative positioning and mapping method based on point cloud feature enhancement. Background Art

[0002] As a representative industry of the new round of technological revolution, autonomous vehicles have gradually become a strategic competitive high ground for the world's industrial powers. Vehicle positioning, as the core function of the autonomous driving system, can provide real-time and accurate location services for vehicle trajectory prediction, obstacle avoidance, etc. At present, the most common vehicle positioning technology is the Global Navigation Satellite System (GNSS). Although advanced GNSS technology can control the positioning error within the centimeter level, under the influence of adverse conditions such as blockage, interference and delay, GNSS is difficult to meet the needs of high real-time and high-precision vehicle positioning. For this reason, positioning and mapping technology based on vehicle-mounted sensor data has emerged. It is used to estimate the position and direction changes of vehicles over time when the map is unknown and GNSS is unavailable. It has become another important means of vehicle positioning.

[0003] At present, sensors such as cameras and lidar have been widely used in positioning and mapping technologies. Lidar can provide accurate three-dimensional environmental structure information, while cameras can capture rich color and texture details. Traditional lidar-vision hybrid positioning and mapping technology usually uses two independent processing pipelines to process lidar point clouds and camera images respectively. Each pipeline performs feature extraction, matching and positioning independently, and then combines the results of the two pipelines through a fusion strategy. This method is relatively simple to implement and has low computational complexity, which can improve the robustness and accuracy of the system to a certain extent. However, due to the insufficient sharing of point cloud and image information during processing, the accuracy and robustness of this method may be limited when facing complex environments. In addition, due to cost factors, most autonomous vehicles are often equipped with low-cost and sparsely sampled 32-line or 64-line lidars. Compared with the high-density pixels of the camera, when the point cloud of the 64-line lidar is projected onto the image, only about 10% of the pixels have corresponding point cloud data. This sparse point cloud poses a huge challenge to the precise positioning of the vehicle. In complex environments, especially when there are dynamic obstacles and rapidly changing scenes, sparse point cloud data is more likely to cause error accumulation and positioning failure. In addition, due to the large differences in data density and resolution between lidar and cameras, how to effectively match and supplement information during data fusion to avoid information loss and noise interference has also become a technical problem that needs to be solved urgently. Summary of the invention

[0004] In order to overcome the shortcomings of the prior art, the present invention provides a multi-sensor collaborative positioning and mapping method based on point cloud feature enhancement, firstly obtaining the laser radar point cloud data generated by the laser radar, the image data captured by the camera, and the calibration file between the laser radar and the camera; then using the deep completion network to generate a virtual laser radar point cloud with pixel-level density; next, extracting the features of the laser radar point cloud and the virtual laser radar point cloud respectively and classifying and reorganizing the features; then accurately estimating the six-degree-of-freedom pose of the vehicle based on the extracted edge feature points and plane feature points; finally, constructing a feature map using the calculated six-degree-of-freedom pose. The present invention significantly improves the accuracy and robustness of multi-sensor collaborative positioning and mapping by jointly processing the features of the virtual point cloud and the real laser point cloud.

[0005] The technical solution adopted by the present invention to solve the technical problem is as follows:

[0006] Step 1: Obtain the laser radar point cloud data P generated by the laser radar L , image data captured by the camera I, calibration file between the lidar and the camera;

[0007] Step 2: Use the deep completion network M to generate a virtual lidar point cloud P with pixel-level density v ;

[0008] Step 3: Extract the LiDAR point clouds P separately L and virtual lidar point cloud P v The features are classified and reorganized;

[0009] Step 4: Accurately estimate the vehicle’s six-degree-of-freedom pose based on the extracted edge feature points E and plane feature points H;

[0010] Step 5: Use the calculated 6-DOF pose to construct a feature map.

[0011] Furthermore, the step 2 is specifically as follows:

[0012] Step 2-1: Obtain the projection matrix T using the calibration file between the lidar and the camera 3D→2D , the laser radar point cloud P L =[x L ,y L , z L , 1] T Projected onto the image plane to obtain a sparse depth map S = [u L , v L , 1] T , the formula is as follows:

[0013] S=T 3D→2D (P L )

[0014] Among them, x L ,y L , z L Represent the point cloud P L The three-dimensional coordinates in the laser radar coordinate system, u L , v L Represent the point cloud P L Two-dimensional coordinates in the image coordinate system;

[0015] Step 2-2: Use the depth completion network M to supplement the depth of the remaining pixels, use the sparse depth map S and the image as the input of the depth completion network M to generate a dense depth map D. The process is expressed as follows:

[0016] D=M(SI)

[0017] Step 2-3: Using the inverse projection matrix T 2D→3D Project the dense depth map D to the laser radar coordinate system to obtain the virtual laser radar point cloud P v , the specific formula is as follows:

[0018] P v =T 2D→3D (P L , D).

[0019] Furthermore, the step 3 is specifically as follows:

[0020] Step 3-1: Based on the curvature calculation method of the point cloud, calculate the laser radar point cloud P L and virtual lidar point cloud P v For each point p i =[x i ,y i , z i ] T The curvature c i , the calculation formula is as follows:

[0021]

[0022] in, Represents point p i The set of the nearest neighbors of , |·| represents the Euclidean norm of the vector, and the denominator is used to normalize the curvature value; x i ,y i , z i Respectively represent point p i Three-dimensional coordinates in the laser radar coordinate system;

[0023] Step 3-2: Select the threshold c th Classify the feature type of the point. If the point p i The curvature ci Greater than c th , then the point is classified as an edge feature point E. If point p i The curvature c i Less than c th , then the point is divided into plane feature point H;

[0024] Step 3-3: Use the slope-based ground plane fitting method to determine whether the plane feature point H belongs to the ground; L and P v Divide into multiple pieces with side length d 2 The grid cells and the slope threshold Sl are selected th ; For each grid, find the lowest height P min =[x min ,y min , z min ] T point, and calculate the slope value Sl of all other points in the grid cell relative to the lowest point i , using the following formula:

[0025]

[0026] Step 3-4: Select the slope threshold Sl th Identify ground feature points from plane feature points H and non-ground feature points

[0027] If point p i The slope c i Greater than c th , then the point is classified as an edge feature point E; if the midpoint p of the plane feature point H i The slope value Sl i Less than Sl th , then the point is classified as a ground feature point Otherwise click p i Classification into non-ground feature points

[0028] Furthermore, the step 4 is specifically as follows:

[0029] Step 4-1: Using the transformation matrix T LiDAR→World The laser radar point cloud P L and virtual lidar point cloud P v The fused point cloud P C Convert from radar coordinate system to world coordinate system. The specific formula is as follows:

[0030] P G =T LiDAR→World (P C)

[0031] Among them, P G Represents the fused point cloud P in the world coordinate system C ;

[0032] Step 4-2: Use the two-step LM optimization method to locate the current vehicle;

[0033] Point Cloud G The features extracted from the laser radar point cloud P include two parts, one of which is L The ground feature points extracted from Non-ground feature points and edge feature points E L And from the virtual lidar point cloud P v Extracted ground feature points Non-ground feature points and edge feature points E v ; Then these features are divided into two groups, namely Group 1: and Group 2: Point Cloud G The two sets of feature points from frame 1 to frame k-1 are expressed as follows:

[0034]

[0035]

[0036] in, Represents the point cloud P G Group 1 feature points from frame 1 to frame k-1, Represents the point cloud P G Represents the point cloud P G Group 2 feature points from frame 1 to frame k-1, Represents the point cloud P G Ground feature points from frame 1 to frame k-1, Represents the point cloud P G Non-ground feature points from frame 1 to frame k-1, Represents the point cloud P G Edge feature points from frame 1 to frame k-1;

[0037] Step 4-3: Initialize the vehicle's six-degree-of-freedom pose x = [x, y, z, θ roll ,θ pitch ,θ yaw ] T , where (x, y, z) represents the position of the vehicle in the world coordinate system, (θ roll ,θ pitch ,θ yaw) represent the three rotation angles of the vehicle around its own coordinates, X-axis, Y-axis and Z-axis, according to the Rodriguez formula:

[0038]

[0039] Among them, δ ij is the identity matrix; θ is the rotation amplitude, which can be obtained by (|[θ roll ,θ itch ,θ yaw ]|)Calculate; is the unit vector ω=[θ roll ,θ pitch ,θ yaw ] T A skew-symmetric matrix;

[0040] Step 4-4: Exploitation and Optimize the pose of x(z,θ) roll ,θ pitch ), the calculation formula is as follows:

[0041]

[0042] Among them, dist(·,·) is an operator for calculating the Euclidean distance between feature points; Represents the point cloud P G The feature points of group 1 of the current k-th frame;

[0043] Step 4-5: Exploitation and Optimize the remaining parameters (x, y, θ) of the pose x yaw ), the calculation formula is as follows:

[0044]

[0045] in, Represents the point cloud P G The group 2 feature points of the current k-th frame;

[0046] Furthermore, the step 5 is specifically as follows:

[0047] Using the calculated six-degree-of-freedom pose x = [x, y, z, θ roll ,θ pitch ,θ yaw ] T , construct feature map G k , the specific formula is as follows:

[0048]

[0049] Among them, G k-1represents the global feature map of the k-1th frame, x k represents the pose transformation matrix of the current frame k, They represent the ground feature points, non-ground feature points and edge feature points of the current frame k respectively.

[0050] A computer program enables a computer to execute the multi-sensor collaborative positioning and mapping method.

[0051] An electronic device comprises: a processor and a memory; the memory is used to store a computer program, and the processor is used to execute the computer program stored in the memory, so that the electronic device executes the above-mentioned multi-sensor collaborative positioning and mapping method.

[0052] A computer-readable storage medium stores a computer program, which, when executed by a processor, implements the multi-sensor collaborative positioning and mapping method.

[0053] A chip includes: a processor, which is used to call and run a computer program from a memory, so that a device equipped with the chip executes the above-mentioned multi-sensor collaborative positioning and mapping method.

[0054] A computer program product, the computer program product comprising a computer storage medium, the computer storage medium storing a computer program, the computer program comprising instructions executable by at least one processor, and when the instructions are executed by the at least one processor, the above-mentioned multi-sensor collaborative positioning and mapping method is implemented.

[0055] The beneficial effects of the present invention are as follows:

[0056] 1. The method of the present invention improves the accuracy of multi-sensor fusion: The present invention introduces a feature joint processing strategy for virtual lidar point cloud and real lidar point cloud, which makes up for the problem of sparse and incomplete data of a single sensor in complex scenes, significantly improves the accuracy of point cloud matching and feature extraction, and provides higher accuracy guarantee for multi-sensor collaborative positioning.

[0057] 2. The method of the present invention enhances the robustness in multiple scenarios: in the process of curvature calculation and edge feature recognition, the high-density information of the virtual point cloud is added, so that the system can still maintain stable feature recognition and data processing capabilities in sparse point cloud scenarios (such as obstacle occlusion or long-distance detection), which significantly improves the robustness of the positioning system in multiple environments.

[0058] 3. The method of the present invention improves the accuracy of positioning and mapping: It adopts an accurate pose estimation method based on a two-step Levenberg-Marquardt (LM) optimization strategy, effectively solves the nonlinear optimization problem in complex driving environments, significantly improves the estimation accuracy of six-degree-of-freedom pose, and realizes more refined feature map construction, providing a more reliable environment representation for subsequent navigation and path planning. BRIEF DESCRIPTION OF THE DRAWINGS

[0059] Figure 1 The figure is a flow chart of the method of the present invention.

[0060] Figure 2 Implement the flow chart for step 2.

[0061] Figure 3 Implement the flow chart for step 3.

[0062] Figure 4 Implement the flow chart for step 4.

[0063] Figure 5 Graph showing the number of edge features according to an embodiment of the present invention.

[0064] Figure 6 Graph showing the number of plane features according to an embodiment of the present invention.

[0065] Figure 7 ATE (%) in the vertical direction of the embodiment of the present invention.

[0066] Figure 8 is the ATE (%) in the horizontal direction of the embodiment of the present invention.

[0067] Fig. 9 This is the result of constructing the feature map of sequence 00 according to an embodiment of the present invention. DETAILED DESCRIPTION

[0068] The present invention is further described below in conjunction with the accompanying drawings and embodiments.

[0069] The purpose of the present invention is to provide a multi-sensor collaborative localization and mapping method based on point cloud feature enhancement to solve the problems of insufficient feature extraction of sparse point clouds and insufficient fusion of lidar point clouds and images.

[0070] according to Figures 1 to 4 As shown, the present invention provides a multi-sensor collaborative positioning and mapping method based on point cloud feature enhancement, comprising the following steps:

[0071] Step 1: Obtain the laser radar point cloud data P generated by the laser radar L , image data captured by the camera I, calibration file between the lidar and the camera;

[0072] Step 2: Use the deep completion network M to generate a virtual lidar point cloud P with pixel-level density v ;

[0073] Step 2-1: Obtain the projection matrix T using the calibration file between the lidar and the camera 3D→2D , the laser radar point cloud P L =[x L ,y L , z L , 1] T Projected onto the image plane to obtain a sparse depth map S = [u L , v L , 1] T , the formula is as follows:

[0074] S=T 3D→2D (P L )

[0075] Step 2-2: Use the depth completion network M to supplement the depth of the remaining pixels, use the sparse depth map S and the image as the input of the depth completion network M to generate a dense depth map D. The process is expressed as follows:

[0076] D=M(S,I)

[0077] Step 2-3: Using the inverse projection matrix T 2D→3D Project the dense depth map D to the laser radar coordinate system to obtain the virtual laser radar point cloud P v , the specific formula is as follows:

[0078] P v =T 2D→3D (P L , D)

[0079] Step 3: Extract the LiDAR point clouds P separately L and virtual lidar point cloud P v The features are classified and reorganized;

[0080] Step 3-1: Based on the curvature calculation method of the point cloud, calculate the laser radar point cloud P L and virtual lidar point cloud P v For each point p i =[x i ,y i , z i ] T The curvature c i , the calculation formula is as follows:

[0081]

[0082] Step 3-2: Select threshold Cth Classify the feature type of the point. If the point p i The curvature c i Greater than c th , then the point is classified as an edge feature point E. If point p i The curvature c i Less than c th , then the point is classified as a plane feature point H; edge feature points E are usually located on the edge or boundary of an object. Plane feature points H are usually located on the ground, wall or other planes;

[0083] Step 3-3: Use the slope-based ground plane fitting method to determine whether the plane feature point H belongs to the ground; L and P v Divide into multiple pieces with side length d 2 The grid cells and the slope threshold Sl are selected th ; For each grid, find the lowest height p min =[x min ,y min , z min ] T point, and calculate the slope value Sl of all other points in the grid cell relative to the lowest point i , using the following formula:

[0084]

[0085] Step 3-4: Select the slope threshold Sl th Identify ground feature points from plane feature points H and non-ground feature points

[0086] If point p i The slope c i Greater than c th , then the point is classified as an edge feature point E; if the midpoint p of the plane feature point H i The slope value Sl i Less than Sl th , then the point is classified as a ground feature point Otherwise click p i Classification into non-ground feature points

[0087] Step 4: Accurately estimate the vehicle’s six-degree-of-freedom pose based on the extracted edge feature points E and plane feature points H;

[0088] Step 4-1: Using the transformation matrix T LiDAR→World The laser radar point cloud P L and virtual lidar point cloud P vThe fused point cloud P C Convert from radar coordinate system to world coordinate system. The specific formula is as follows:

[0089] P G =T LiDAR→World (P C )

[0090] Among them, P G Represents the fused point cloud P in the world coordinate system C ;

[0091] Step 4-2: Use the two-step LM optimization method to locate the current vehicle;

[0092] Point Cloud G The features extracted from the laser radar point cloud P include two parts, one of which is L The features extracted from and E L And from the virtual lidar point cloud P v Extracted features and E v ; Then these features are divided into two groups, namely Group 1: and Group 2: Point Cloud G The two sets of feature points from frame 1 to frame k-1 are expressed as follows:

[0093]

[0094] Step 4-3: Initialize the vehicle's six-degree-of-freedom pose x = [x, y, z, θ roll ,θ pitch ,θ yaw ] T , where (θ roll ,θ pitch ,θ yaw ) is the rotation angle of the vehicle, according to the Rodriguez formula:

[0095]

[0096] Among them, δ ij is the identity matrix; θ is the rotation amplitude, which can be obtained by (|[θ roll ,θ itch ,θ yaw ]|)Calculate; is the unit vector ω=[θ roll ,θ pitch ,θ yaw ] T A skew-symmetric matrix;

[0097] Step 4-4: Exploitation and Optimize the pose of x(z,θ) roll ,θ pitch ), the calculation formula is as follows:

[0098]

[0099] Step 4-5: Exploitation and The remaining parameters (x, y, θyaw) of the optimized x pose are calculated as follows:

[0100]

[0101] Step 5: Use the calculated six-degree-of-freedom pose x = [x, y, z, θ roll ,θ pitch ,θ yaw ] T , construct feature map G k , the specific formula is as follows:

[0102]

[0103] Example:

[0104] refer to Figure 5 and Figure 6 As shown, Figure 5 and Figure 6 The average number of feature points extracted from LiDAR and virtual LiDAR in different environments is given. In all scenarios, the median and mean of the features extracted from the virtual LiDAR point cloud are very close, and the variance is smaller than that of the features extracted from the LiDAR point cloud. This is because the pseudo LiDAR relies on the depth completion network to generate depth information from the image. Since the input of the depth completion network is an image with a constant resolution, the present invention can obtain a virtual LiDAR point cloud with relatively stable density in different environments.

[0105] In urban scenes, rural scenes, and forest scenes. The distribution patterns of feature points of the lidar point cloud and the virtual lidar point cloud are similar. Among them, the number and variance of feature points extracted from the lidar point cloud are large, and the feature means of the two point clouds are close to the median. It can be inferred in these three scenes that both point clouds show consistency in feature extraction. In the highway scene, the number of edge feature points is less than that in other scenes, and the variance is larger than that in other scenes. This is because the highway scene is relatively simple and lacks complex structures and details. The average value of edge features in the lidar point cloud is smaller than the median, indicating that the edge features in some frames are very sparse. The mean value of plane features is greater than the median because there are many flat areas (such as road surfaces) in the highway scene. In dynamic scenes, the virtual lidar point cloud has more edge features, which means that it can capture the contours or changes of objects more effectively.

[0106] As shown in Table 1, Table 1 gives a specific numerical comparison of the average trajectory error (%) in various environments using different point clouds. In all urban and rural scenes, the method of the present invention achieves the lowest ATE. In particular, compared with using only lidar, the method of the present invention reduces the ATE by 63.41% and 73.83% in sequence 0 (urban scene) and sequence 3 (rural scene), respectively. Compared with the performance in other scenes, the virtual lidar point cloud only achieves the lowest ATE (09, 10) in the forest scene, and the method of the present invention also achieves the lowest ATE in the forest scene.

[0107] Table 1 Average trajectory error (%) using different point clouds in various environments

[0108]

[0109] refer to Figure 7 and Figure 8 As shown, Figure 7 and Figure 8 A comparison of ATE (%) in the horizontal and vertical directions between the LiDAR and the proposed method in different scenarios is given. Except for the highway scenario, using the proposed method as the input to the odometer can significantly reduce the ATE in the vertical direction. Except for the dynamic scenario, using the proposed method as the input to the odometer can reduce the ATE in the horizontal direction.

[0110] refer to Fig. 9 As shown, Fig. 9The results of map reconstruction of sequence 00 using the method of the present invention are given. The circled part in the figure is located near the 2800th frame of sequence 00, from which it can be seen that the map reconstruction using the method of the present invention can obtain a map with rich details (such as vehicle appearance, lighting and shadows, etc.). In addition, although the virtual lidar point cloud contains many noise points caused by the deep completion network, these noise points can be effectively filtered by constructing a feature map using edge feature points and plane feature points.

[0111] Without loss of generality, in this embodiment, the multi-sensor collaborative positioning and mapping method based on point cloud feature enhancement is experimented on the KITTI odometer dataset. The dataset acquisition platform consists of two grayscale cameras, two color cameras, a Velodyne HDL-64E laser radar, four optical lenses and a GPS navigation system. This embodiment uses images captured by color cameras, point clouds generated by laser radars, and calibration files between cameras and laser radars. The dataset consists of 11 sequences of different lengths and different environments, which are divided into five scenes: urban scenes are mainly composed of structural objects (such as static vehicles and buildings, etc.), forest scenes are mainly composed of unstructured objects (such as grass and trees, etc.); rural scenes have the characteristics of both urban scenes and forest scenes; highway scenes contain more moving vehicles and fewer structural objects; dynamic scenes contain many dynamic objects (such as moving vehicles and pedestrians, etc.). As shown in Table 2.

[0112] Table 2 KITTI odometry dataset

[0113]

[0114] For the deep completion network, the embodiment adopts PENet. Among them, PENet is trained on the KITTI training set. The standardized resolution of the image is 1226×370. The lidar point cloud and the virtual lidar point cloud are standardized at a vertical field of view (FoV) of [-25°, 25°] and a vertical resolution of 64. For feature extraction, this embodiment divides the 360-degree range point cloud into 6 sub-regions. In order to balance the feature distribution in various environments, the maximum number of edge points and plane points in each sub-region is set to 2 and 8, respectively. Considering the performance, this embodiment selects the feature set within 60 meters of the current posture of the sensor for mapping. The embodiment uses the average displacement error (ATE%) to evaluate the positioning accuracy. All embodiments are performed on a laptop equipped with an AMD Ryzen 7 5800H 3.20GHz CPU, NVIDIA GeForce RTX 3060 laptop GPU, and 16GB RAM.

Claims

1. A multi-sensor collaborative positioning and mapping method based on point cloud feature enhancement, characterized in that: The steps include: Step 1: Obtain the laser radar point cloud data P generated by the laser radar L , image data captured by the camera I, calibration file between the lidar and the camera; Step 2: Use the deep completion network M to generate a virtual lidar point cloud P with pixel-level density v ; Step 3: Extract the LiDAR point clouds P separately L and virtual lidar point cloud P v The features are classified and reorganized; Step 4: Accurately estimate the vehicle’s six-degree-of-freedom pose based on the extracted edge feature points E and plane feature points H; Step 5: Use the calculated 6-DOF pose to construct a feature map.

2. The multi-sensor collaborative positioning and mapping method based on point cloud feature enhancement according to claim 1, characterized in that: The step 2 is specifically as follows: Step 2-1: Obtain the projection matrix T using the calibration file between the lidar and the camera 3D→2D , the laser radar point cloud P L =[x L ,y L , z L , 1] T Projected onto the image plane to obtain a sparse depth map B = [u L , v L , 1] T , the formula is as follows: S=T 3D→2D (P L ) Among them, x L ,y L , z L Represent the point cloud P L The three-dimensional coordinates in the laser radar coordinate system, u L , v L Represent the point cloud P L Two-dimensional coordinates in the image coordinate system; Step 2-2: Use the depth completion network M to supplement the depth of the remaining pixels, use the sparse depth map S and the image as the input of the depth completion network M to generate a dense depth map D. The process is expressed as follows: D=M(SI) Step 2-3: Using the inverse projection matrix T 2D→3D Project the dense depth map D to the laser radar coordinate system to obtain the virtual laser radar point cloud P v , the specific formula is as follows: P v =T 2D→3D (P L ,D)。 3. The multi-sensor collaborative positioning and mapping method based on point cloud feature enhancement according to claim 2 is characterized in that: The step 3 is specifically as follows: Step 3-1: Based on the curvature calculation method of the point cloud, calculate the laser radar point cloud P L and virtual lidar point cloud P v For each point p i =[x i ,y i , z i ] T The curvature c i , the calculation formula is as follows: in, Represents point p i The set of the nearest neighbors of , |·| represents the Euclidean norm of the vector, and the denominator is used to normalize the curvature value; x i ,y i , z i Respectively represent point p i Three-dimensional coordinates in the laser radar coordinate system; Step 3-2: Select the threshold c th Classify the feature type of the point. If the point p i The curvature c i Greater than c th , then the point is classified as an edge feature point E. If point p i The curvature c i Less than c th , then the point is divided into plane feature point H; Step 3-3: Use the slope-based ground plane fitting method to determine whether the plane feature point H belongs to the ground; L and P v Divide into multiple pieces with side length d 2 The grid cells and the slope threshold Sl are selected th ; For each grid, find the lowest height p min =[x min ,y min , z min ] T point, and calculate the slope value Sl of all other points in the grid cell relative to the lowest point i , using the following formula: Step 3-4: Select the slope threshold Sl th Identify ground feature points from plane feature points H and non-ground feature points If point p i The slope c i Greater than c th , then the point is classified as an edge feature point E; if the midpoint p of the plane feature point H i The slope value Sl i Less than Sl th , then the point is classified as a ground feature point Otherwise click p i Classification into non-ground feature points 4. The multi-sensor collaborative positioning and mapping method based on point cloud feature enhancement according to claim 3 is characterized in that: The step 4 is specifically as follows: Step 4-1: Using the transformation matrix T LiDAR→World The laser radar point cloud P L and virtual lidar point cloud P v The fused point cloud P C The specific formula for converting from the radar coordinate system to the world coordinate system is as follows: P G =T LiDAR→World (P C ) Among them, P G Represents the fused point cloud P in the world coordinate system C ; Step 4-2: Use the two-step LM optimization method to locate the current vehicle; Point Cloud G The features extracted from the laser radar point cloud P include two parts, one of which is L The ground feature points extracted from Non-ground feature points and edge feature points E L And from the virtual lidar point cloud P v Extracted ground feature points Non-ground feature points and edge feature points E v ; Then these features are divided into two groups, namely Group 1: and Group 2: Point Cloud G The two sets of feature points from frame 1 to frame k-1 are expressed as follows: in, Represents the point cloud P G Group 1 feature points from frame 1 to frame k-1, Represents the point cloud P G Represents the point cloud P G Group 2 feature points from frame 1 to frame k-1, Represents the point cloud P G Ground feature points from frame 1 to frame k-1, Represents the point cloud P G Non-ground feature points from frame 1 to frame k-1, Represents the point cloud P G Edge feature points from frame 1 to frame k-1; Step 4-3: Initialize the vehicle's six-degree-of-freedom pose x = [x, y, z, θ roll ,θ pitch ,θ yaw ] T , where (x, y, z) represents the position of the vehicle in the world coordinate system, and (θ roll, θ pitch, θ yaw) represent the three rotation angles of the vehicle around its own coordinate X axis, Y axis and Z axis respectively. According to the Rodriguez formula: Among them, δ ij is the identity matrix; θ is the rotation amplitude, which can be obtained by (|[θ roll ,θ itch ,θ yaw ]|)Calculate; is the unit vector ω=[θ roll ,θpitc h ,θya w ] T A skew-symmetric matrix; Step 4-4: Exploitation and Optimize the x pose (z, θ roll, θ pitch), the calculation formula is as follows: Among them, dist(·,·) is an operator for calculating the Euclidean distance between feature points; Represents the point cloud P G The feature points of group 1 of the current k-th frame; Step 4-5: Exploitation and The remaining parameters (x, y, θyaw) of the optimized x pose are calculated as follows: in, Represents the point cloud P G The group 2 feature points of the current k-th frame.

5. The multi-sensor collaborative positioning and mapping method based on point cloud feature enhancement according to claim 4 is characterized in that: The step 5 is specifically as follows: Using the calculated six-degree-of-freedom pose Construct feature map G k , the specific formula is as follows: Among them, G k-1 represents the global feature map of the k-1th frame, represents the pose transformation matrix of the current frame k, E k They represent the ground feature points, non-ground feature points and edge feature points of the current frame k respectively.

6. A computer program, characterized in that The computer program enables a computer to execute the method according to any one of claims 1 to 5.

7. An electronic device, characterized in that: include: Processor and memory; The memory is used to store a computer program, and the processor is used to execute the computer program stored in the memory, so that the electronic device executes the method as claimed in any one of claims 1 to 5.

8. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the method according to any one of claims 1 to 5 is implemented.

9. A chip, characterized in that: include: A processor, configured to call and run a computer program from a memory, so that a device equipped with the chip executes a method as claimed in any one of claims 1 to 5.

10. A computer program product, characterized in that The computer program product comprises a computer storage medium storing a computer program, wherein the computer program comprises instructions executable by at least one processor, and when the instructions are executed by the at least one processor, the method according to any one of claims 1 to 5 is implemented.

Images