A vehicle pose estimation method fusing roadside sparse point cloud and road features
By combining a two-stage registration method with ground point cloud normal vectors and dense point cloud templates, the accuracy and robustness issues of vehicle pose estimation are solved, achieving efficient and low-cost pose estimation.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- SUN YAT SEN UNIV
- Filing Date
- 2023-05-24
- Publication Date
- 2026-06-02
AI Technical Summary
Existing vehicle pose estimation methods suffer from poor accuracy and low robustness, especially those based on cameras and model-free LiDAR, which struggle to meet requirements in terms of real-time performance and accuracy.
A two-stage registration method is adopted. First, coarse registration is performed using ground point cloud normal vectors and principal component analysis. Then, fine registration is performed using the iterative nearest point algorithm. Finally, the pose is estimated by combining the dense point cloud template.
It improves the accuracy and robustness of vehicle pose estimation, reduces computing and storage costs, and achieves efficient pose estimation.
Smart Images
Figure CN116823944B_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of vehicle tracking technology, and in particular to a method for vehicle pose estimation that integrates roadside sparse point clouds and road features. Background Technology
[0002] Accurate vehicle pose estimation can assess the vehicle's driving status and provide early warnings of abnormal driving conditions. Current vehicle pose estimation methods can be mainly categorized by sensor type into camera-based and LiDAR-based methods. Camera-based vehicle pose estimation suffers from significant errors and cannot meet the requirements for estimating the vehicle's driving status. LiDAR-based vehicle pose estimation can be further divided into model-based and model-free methods.
[0003] The model-based approach first requires establishing a point cloud template library for the target object from multiple perspectives. Then, it calculates the features of the target point cloud and uses a search algorithm to find template point clouds with similar perspectives to the target point cloud from the library. Finally, it obtains the pose by registering the detected point cloud and converting it to the template point cloud. Firstly, establishing a complete template library for the target object is time-consuming and labor-intensive, requiring large storage and being difficult to update. If the template library lacks template point clouds with similar perspectives, the subsequent registration process may get stuck in a local optimum, ultimately failing. Secondly, searching for template point clouds requires accurate features to measure the similarity between the target and template point clouds. Accurate features require dense point clouds for computation, which increases the hardware requirements of the LiDAR, significantly increasing deployment costs and hindering large-scale deployment. Finally, neighbor search of the template library is time-consuming. The contradiction lies in the fact that a complete target template library has target point clouds from more perspectives, which can reduce the probability of registration failure but increases the neighbor search time; an incomplete target template library has fewer target point clouds, which can reduce the neighbor search time but increases the probability of registration failure. This results in a situation where robust methods lack real-time performance, while real-time methods are not robust.
[0004] Model-free methods first calculate the bounding boxes of the clustered point clouds, then calculate the pose of the bounding boxes to determine the pose of the target point cloud. However, the detected point clouds are incomplete due to occlusion. Calculating bounding boxes for incomplete points can easily result in boxes that are missing parts, directly leading to incorrect displacement estimation and further errors in angular offset. Secondly, the detected point clouds rely on the effectiveness of the segmentation and clustering algorithms. If the segmentation and clustering are flawed, it will directly cause errors in bounding box calculation, thus leading to incorrect pose estimation. Therefore, model-free methods generally have poor accuracy.
[0005] Existing technology discloses a long-range, high-precision multi-vehicle detection and tracking method based on roadside radar, comprising: acquiring first point cloud data from a first radar; performing clustering identification based on the first point cloud data to determine at least one first target; when the at least one target enters the overlapping area between the first and second radars, acquiring second point cloud data from the second radar, performing clustering identification based on the second point cloud data to determine at least one second target; performing coordinate transformation on the at least one second target, and comparing its pose and feature matrix with the at least one first target to determine whether they belong to the same tracking target. This patent uses clustering for pose identification; if there is a deviation in the segmentation and clustering, it will directly lead to errors in bounding box calculation, thus resulting in incorrect pose estimation. Therefore, model-free methods generally have poor accuracy. Summary of the Invention
[0006] The purpose of this invention is to provide a robust and accurate method for vehicle pose estimation that integrates roadside sparse point clouds and road features.
[0007] To achieve the above objectives, this invention provides a vehicle pose estimation method that integrates roadside sparse point clouds and road features, characterized by comprising the following steps:
[0008] S1: Acquire real-time point cloud data collected by roadside lidar;
[0009] S2: Extract the point cloud data located in the vehicle driving area from the point cloud data obtained in step S1;
[0010] S3: Fit the ground. Based on the fitted ground, divide the point cloud of the vehicle driving area into ground point cloud and ground point cloud.
[0011] S4: Cluster the ground point cloud to obtain the target vehicle point cloud cluster;
[0012] S5: Estimate the normal vector of the ground point cloud to obtain the ground point cloud normal vector. ;
[0013] S6: Perform coarse registration of the target vehicle point cloud obtained in step S4 and the ground point cloud normal vector obtained in step S5 using principal component analysis to obtain the coarse registration matrix and the coarsely registered target point cloud.
[0014] S7: Perform fine registration on the coarsely registered target point cloud and point cloud template to obtain the fine registration matrix;
[0015] S8: Multiply the coarse registration matrix obtained in step S6 and the fine registration matrix obtained in step S7 to obtain the final registration matrix;
[0016] S9: Convert the final registration matrix obtained in step S8 into a pose expression.
[0017] Specifically, in step S2, when extracting the point cloud located in the vehicle driving area, the distance information is used to extract the point cloud data obtained in step S1.
[0018] Specifically, the distance information includes the x-axis and y-axis information of the point cloud coordinates, and the point cloud within the vehicle's driving area is extracted based on the x-axis and y-axis coordinates.
[0019] Specifically, in step S3, the RANSAC algorithm is used to fit the ground.
[0020] Specifically, in step 4, the ground point cloud is clustered using an adaptive Euclidean clustering method.
[0021] Specifically, step S6 includes:
[0022] S6.1: Cluster the target vehicle point cloud obtained in step S4 As the target point cloud;
[0023] S6.2: Calculate the coordinates of the center point of the target point cloud. ;
[0024] S6.3: Calculate the eigenvector corresponding to the largest eigenvalue of the target point cloud. ;
[0025] S6.4: Calculate the rotation transformation matrix using the following formula:
[0026]
[0027]
[0028]
[0029] in Vector standardization;
[0030] S6.5: Calculate the translation transformation matrix using the following formula:
[0031] ;
[0032] S6.6: Combine the rotation transformation matrix and the translation transformation matrix to obtain the registration matrix.
[0033] .
[0034] Specifically, in step S6.3, SVD decomposition is used to calculate the eigenvector corresponding to the largest eigenvalue of the target point cloud. .
[0035] Specifically, in step S7, the point cloud template is a dense point cloud template with a full view of the target vehicle.
[0036] Specifically, in step S7, the iterative nearest point algorithm is used to perform fine registration on the coarsely registered target point cloud and point cloud template to obtain a fine registration matrix. The form of the fine registration matrix is the same as that of the coarse registration matrix in step S6.
[0037] Specifically, in step S9, the final registration matrix includes the final rotation transformation matrix and the final translation transformation matrix, which are then converted into a pose representation using the following formula:
[0038]
[0039]
[0040] Where T represents the final translation transformation matrix, and R represents the final rotation transformation matrix. It is spatial location information, representing the displacement of the vehicle's center on the three coordinate axes of the world coordinate system; It is spatial attitude information, representing the unit vector of the vehicle's longitudinal axis orientation in the world coordinate system.
[0041] Compared with the prior art, the beneficial effects of the present invention are as follows:
[0042] This invention performs coarse registration and fine registration, using two-stage registration to improve accuracy. The first stage of coarse registration increases the success rate of the algorithm solution, and the second stage of fine registration improves the accuracy of the algorithm. This invention combines the advantages of robustness of model-free methods with the advantages of high accuracy of model-based methods.
[0043] In addition, when performing coarse registration, this invention utilizes the normal vector of the ground point cloud and uses environmental features instead of the target vehicle point cloud features as constraints. The environmental features do not change with the distance between the target vehicle and the radar, thus improving the robustness of the algorithm.
[0044] Furthermore, this invention addresses the issue of high deployment costs for lidar pose estimation algorithms by using a target vehicle full-view dense point cloud template instead of a target vehicle multi-view dense point cloud library, thereby reducing model storage costs and search time costs. Under the same equipment conditions, it is faster and also reduces computing equipment costs. Attached Figure Description
[0045] Figure 1 This is a flowchart of the vehicle pose estimation method according to an embodiment of the present invention.
[0046] Figure 2 This is an information flow diagram of the vehicle pose estimation method according to an embodiment of the present invention.
[0047] Figure 3This is a flowchart of step S6 in an embodiment of the present invention.
[0048] Figure 4 This is an example diagram of the clustered point cloud for target vehicle detection.
[0049] Figure 5 This is an example point cloud diagram of the target vehicle template. Detailed Implementation
[0050] The specific embodiments of the present invention will be described in further detail below with reference to the accompanying drawings and examples. The following examples are for illustrative purposes only and are not intended to limit the scope of the invention.
[0051] For ease of explanation, let's first define the vehicle pose. Typically, the pose of a target vehicle can be represented as a six-dimensional vector as shown below:
[0052]
[0053]
[0054] in, It is spatial location information, representing the displacement of the vehicle's center on the three coordinate axes of the world coordinate system; It is spatial attitude information, representing the unit vector of the vehicle's longitudinal axis orientation in the world coordinate system.
[0055] Example 1
[0056] like Figures 1 to 5 As shown in the figure, a preferred embodiment of the present invention provides a vehicle pose estimation method that fuses roadside sparse point clouds and road features, comprising the following steps:
[0057] S1: Acquire real-time point cloud data collected by roadside lidar;
[0058] S2: Extract the point cloud data located in the vehicle driving area from the point cloud data obtained in step S1;
[0059] S3: Fit the ground. Based on the fitted ground, divide the point cloud of the vehicle driving area into ground point cloud and ground point cloud.
[0060] S4: Cluster the ground point cloud to obtain the target vehicle point cloud cluster;
[0061] S5: Estimate the normal vector of the ground point cloud to obtain the ground point cloud normal vector. ;
[0062] S6: Perform coarse registration of the target vehicle point cloud obtained in step S4 and the ground point cloud normal vector obtained in step S5 using principal component analysis to obtain the coarse registration matrix and the coarsely registered target point cloud.
[0063] S7: Perform fine registration on the coarsely registered target point cloud and point cloud template to obtain the fine registration matrix;
[0064] S8: Multiply the coarse registration matrix obtained in step S6 and the fine registration matrix obtained in step S7 to obtain the final registration matrix;
[0065] S9: Convert the final registration matrix obtained in step S8 into a pose expression.
[0066] This embodiment performs coarse and fine registration, using a two-stage registration process to improve accuracy. The first stage, coarse registration, increases the algorithm's success rate, while the second stage, fine registration, improves the algorithm's precision. This allows the invention to combine the robustness of model-free methods with the high precision of model-based methods. Furthermore, during coarse registration, this embodiment utilizes the normal vectors of the ground point cloud and uses environmental features instead of the target vehicle's point cloud features as constraints. Since the environmental features do not change with the distance between the target vehicle and the radar, this improves the algorithm's robustness.
[0067] Example 2
[0068] This embodiment provides a vehicle pose estimation method that fuses roadside sparse point clouds and road features, comprising the following steps:
[0069] S1: Acquire real-time point cloud data collected by the roadside lidar; this embodiment uses a 32-line lidar, and the experimental vehicle is 3m × 1.4m × 1.3m in size and shape similar to... Figure 5 The vehicles are similar in shape.
[0070] S2: Extract the point cloud data located in the vehicle driving area from the point cloud data obtained in step S1. In step S2, when extracting the point cloud data located in the vehicle driving area, it is extracted from the point cloud data obtained in step S1 using distance information. The distance information includes the x-axis and y-axis information of the point cloud coordinates, and the point cloud data whose x-axis and y-axis coordinates are within the vehicle driving area are extracted.
[0071] Step S2 directly extracts the point cloud within the target area using x-axis and y-axis information. This embodiment was tested on a road with a width of 7m, and the actual parameters were... , Extract the point cloud that meets the requirements.
[0072] S3: Fitting the ground. Based on the fitted ground, the point cloud of the vehicle's driving area is divided into ground point cloud and above-ground point cloud. In step S3, the Random Sample Consensus (RANSAC) algorithm is used to fit the ground.
[0073] S4: Cluster the ground point cloud to obtain the target vehicle point cloud cluster.
[0074] In step 4, the ground point cloud is clustered using the adaptive Euclidean clustering method. The adaptive Euclidean clustering method is an adaptive clustering method based on Euclidean distance. The resulting target vehicle point cloud clustering is as follows: Figure 4 As shown.
[0075] S5: Estimate the normal vector of the ground point cloud to obtain the ground point cloud normal vector. ;
[0076] S6: Perform coarse registration of the target vehicle point cloud obtained in step S4 and the ground point cloud normal vector obtained in step S5 using principal component analysis (ECPC) to obtain the coarse registration matrix and the coarsely registered target point cloud.
[0077] Step S6 includes:
[0078] S6.1: Cluster the target vehicle point cloud obtained in step S4 As the target point cloud;
[0079] S6.2: Calculate the coordinates of the center point of the target point cloud. ;
[0080] S6.3: Calculate the eigenvector corresponding to the largest eigenvalue of the target point cloud. ;
[0081] S6.4: Calculate the rotation transformation matrix using the following formula:
[0082]
[0083]
[0084]
[0085] in Vector standardization;
[0086] S6.5: Calculate the translation transformation matrix using the following formula:
[0087] ;
[0088] S6.6: Combine the rotation transformation matrix and the translation transformation matrix to obtain the registration matrix.
[0089] .
[0090] S7: Perform fine registration on the coarsely registered target point cloud and point cloud template to obtain the fine registration matrix.
[0091] In step S7, the point cloud template is a dense point cloud template of the target vehicle from a full-view perspective. The point cloud template is prepared in advance and can be created through manual modeling or by stitching together data from multi-angle scans, such as... Figure 5 As shown.
[0092] This embodiment addresses the issue of high deployment costs for lidar pose estimation algorithms by using a target vehicle full-view dense point cloud template instead of a target vehicle multi-view dense point cloud library. This reduces model storage costs and search time costs, achieves faster speeds under the same equipment conditions, and also reduces computing equipment costs.
[0093] In addition, in step S7, the Iterative Closest Point (ICP) algorithm is used to perform fine registration on the coarsely registered target point cloud and point cloud template to obtain a fine registration matrix. The form of the fine registration matrix is the same as that of the coarse registration matrix in step S6.
[0094] S8: Multiply the coarse registration matrix obtained in step S6 and the fine registration matrix obtained in step S7 to obtain the final registration matrix;
[0095] S9: Convert the final registration matrix obtained in step S8 into a pose expression.
[0096] In step S9, the final registration matrix includes the final rotation transformation matrix and the final translation transformation matrix, which are then converted into a pose representation using the following formula:
[0097]
[0098]
[0099] Where T represents the final translation transformation matrix, and R represents the final rotation transformation matrix. It is spatial location information, representing the displacement of the vehicle's center on the three coordinate axes of the world coordinate system; It is spatial attitude information, representing the unit vector of the vehicle's longitudinal axis orientation in the world coordinate system.
[0100] Example 3
[0101] The difference between this embodiment and embodiment 2 is that, based on embodiment 2, this embodiment provides further explanation of step S6.3.
[0102] In step S6.3 of this embodiment, SVD decomposition is used to calculate the eigenvector corresponding to the largest eigenvalue of the target point cloud. It should be noted that other methods can also be used to calculate the eigenvector corresponding to the largest eigenvalue of the target point cloud. .
[0103] The other steps in this embodiment are the same as in embodiment 2, and will not be repeated here.
[0104] In summary, this invention provides a vehicle pose estimation method that integrates roadside sparse point clouds and road features. It employs coarse and fine registration, using a two-stage registration process to improve accuracy. The first stage, coarse registration, increases the algorithm's success rate, while the second stage, fine registration, improves accuracy. This invention combines the robustness of model-free methods with the high accuracy of model-based methods, solving the problems of low robustness and accuracy in sparse point cloud pose estimation. Furthermore, during coarse registration, the normal vectors of the ground point cloud are utilized, and environmental features are used instead of target vehicle point cloud features as constraints. Since environmental features do not change with the distance between the target vehicle and the radar, this improves the algorithm's robustness and addresses the issues of limited vehicle features in sparse point clouds and significant distance-dependent variations in vehicle features. Additionally, this invention uses a full-view dense point cloud template for the target vehicle instead of a multi-view dense point cloud library, reducing model storage and search time costs, and achieving a reduction in computing equipment costs at the same operating speed. This invention is used for vehicle pose estimation with roadside lidar, exhibiting good robustness, high accuracy, and low deployment costs.
[0105] The above description is only a preferred embodiment of the present invention. It should be noted that for those skilled in the art, several improvements and substitutions can be made without departing from the technical principles of the present invention, and these improvements and substitutions should also be considered within the scope of protection of the present invention.
Claims
1. A vehicle pose estimation method that integrates roadside sparse point clouds and road features, characterized in that, Includes the following steps: S1: Acquire real-time point cloud data collected by roadside lidar; S2: Extract the point cloud data located in the vehicle driving area from the point cloud data obtained in step S1; S3: Fit the ground. Based on the fitted ground, divide the point cloud of the vehicle driving area into ground point cloud and ground point cloud. S4: Cluster the ground point cloud to obtain the target vehicle point cloud cluster; S5: Estimate the normal vector of the ground point cloud to obtain the ground point cloud normal vector. ; S6: Perform coarse registration of the target vehicle point cloud obtained in step S4 and the ground point cloud normal vector obtained in step S5 using principal component analysis to obtain the coarse registration matrix and the coarsely registered target point cloud. S7: Perform fine registration on the coarsely registered target point cloud and point cloud template to obtain the fine registration matrix; S8: Multiply the coarse registration matrix obtained in step S6 and the fine registration matrix obtained in step S7 to obtain the final registration matrix; S9: Convert the final registration matrix obtained in step S8 into a pose expression.
2. The vehicle pose estimation method based on the fusion of roadside sparse point cloud and road features according to claim 1, characterized in that, In step S2, when extracting the point cloud located in the vehicle driving area, the distance information is used to extract the point cloud data obtained in step S1.
3. The vehicle pose estimation method based on the fusion of roadside sparse point cloud and road features according to claim 2, characterized in that, Distance information includes the x-axis and y-axis information of the point cloud coordinates, and the point cloud within the vehicle's driving area is extracted based on the x-axis and y-axis coordinates.
4. The vehicle pose estimation method based on the fusion of roadside sparse point cloud and road features according to claim 1, characterized in that, In step S3, the RANSAC algorithm is used to fit the ground.
5. The vehicle pose estimation method based on the fusion of roadside sparse point cloud and road features according to claim 1, characterized in that, In step 4, the ground point cloud is clustered using the adaptive Euclidean clustering method.
6. The vehicle pose estimation method based on the fusion of roadside sparse point cloud and road features according to claim 1, characterized in that, Step S6 includes: S6.1: Cluster the target vehicle point cloud obtained in step S4 As the target point cloud; S6.2: Calculate the coordinates of the center point of the target point cloud. ; S6.3: Calculate the eigenvector corresponding to the largest eigenvalue of the target point cloud. ; S6.4: Calculate the rotation transformation matrix using the following formula: in Vector standardization; S6.5: Calculate the translation transformation matrix using the following formula: ； S6.6: Combine the rotation transformation matrix and the translation transformation matrix to obtain the registration matrix. 。 7. The vehicle pose estimation method based on the fusion of roadside sparse point cloud and road features according to claim 6, characterized in that, In step S6.3, SVD decomposition is used to calculate the eigenvector corresponding to the largest eigenvalue of the target point cloud. .
8. The vehicle pose estimation method based on the fusion of roadside sparse point cloud and road features according to claim 1, characterized in that, In step S7, the point cloud template is a dense point cloud template with a full view of the target vehicle.
9. The vehicle pose estimation method based on the fusion of roadside sparse point cloud and road features according to claim 1, characterized in that, In step S7, the iterative nearest point algorithm is used to perform fine registration on the coarsely registered target point cloud and point cloud template to obtain a fine registration matrix. The form of the fine registration matrix is the same as that of the coarse registration matrix in step S6.
10. The vehicle pose estimation method based on the fusion of roadside sparse point cloud and road features according to claim 6, characterized in that, In step S9, the final registration matrix includes the final rotation transformation matrix and the final translation transformation matrix, which are then converted into a pose representation using the following formula: Where T represents the final translation transformation matrix, and R represents the final rotation transformation matrix. It is spatial location information, representing the displacement of the vehicle's center on the three coordinate axes of the world coordinate system; It is spatial attitude information, representing the unit vector of the vehicle's longitudinal axis orientation in the world coordinate system.