Pose estimation method and apparatus, vehicle, and storage medium

Abstract

This application relates to the field of autonomous driving technology, and particularly to a pose estimation method, device, vehicle, and storage medium. The method includes: acquiring laser point cloud data of the vehicle's surrounding environment; filtering ground point cloud data from the laser point cloud data, dividing the ground point cloud data into grids, downsampling the point cloud data in each grid, and calculating a score for the point cloud data in each grid; based on the scores of the point cloud data in each grid, filtering out grid point cloud data with scores greater than a preset value, and calculating the pose of the LiDAR based on the grid point cloud data. Therefore, by downsampling the grids, the algorithm efficiency is improved, and statistical analysis from multiple dimensions can effectively filter out noise points on the ground. Furthermore, by calculating the LiDAR pose using the filtered grid point cloud data, the problem of unstable ground pose estimation and large errors in different scenarios is solved, ensuring high accuracy in the fitted ground.

Description

Technical Field

[0001] This application relates to the field of autonomous driving technology, and in particular to a pose estimation method, device, vehicle, and storage medium. Background Technology

[0002] Automotive LiDAR is one of the most important sensors in the field of autonomous driving, primarily used to provide real-time vehicle location and depth information of the surrounding environment. Due to its advantages such as the ability to obtain 3D point information, high measurement accuracy, and independence from external lighting conditions, LiDAR is increasingly being adopted by OEMs. However, this also places more stringent demands on LiDAR perception algorithms; currently, LiDAR suffers from instability and large errors in estimating pose relative to the ground.

[0003] One related technology involves acquiring an image, determining the ground region within the image, estimating the image's depth value, fitting a 3D ground plane using the depth value, and finally confirming the camera's pose relative to the ground. Another approach involves acquiring point cloud data from a lidar illuminating a calibration surface, segmenting the point cloud data into a first target point cloud representing the calibration surface and a second target point cloud representing a non-calibration surface, and finally calculating the 3D coordinates of the target point in the lidar's coordinate system based on the intersection relationship between the first and second target point cloud data.

[0004] However, the first type of ground segmentation is prone to noise in the edge region (especially in congested scenarios), which can easily affect subsequent ground fitting. Furthermore, the depth estimation algorithm is significantly affected and has low accuracy. The other type requires a specific site, cannot output estimated pose values ​​in real time, and sometimes requires manual feature selection. Summary of the Invention

[0005] This application provides a pose estimation method, device, vehicle, and storage medium to solve the problems of unstable ground pose estimation and large error in different scenarios, and can obtain more accurate pose estimation.

[0006] The first aspect of this application provides a pose estimation method, comprising the following steps: acquiring laser point cloud data of the environment surrounding a vehicle; filtering ground point cloud data from the laser point cloud data, dividing the ground point cloud data into grids, and calculating a score for the point cloud data in each grid after downsampling the point cloud data in each grid; and filtering grid point cloud data with scores greater than a preset value based on the scores of the point cloud data in each grid, and calculating the pose of the lidar based on the grid point cloud data.

[0007] Based on the above technical means, this application can collect point cloud data around the vehicle, divide the ground point data into grids, downsample the point cloud data within the grids, score each downsampled grid, select the grid point cloud data with higher scores, and calculate the pose of the LiDAR. This solves the problem of unstable ground pose estimation and large error in different scenarios. By scoring the grids and filtering out noisy grid point clouds, a more accurate pose estimation can be obtained, while improving the efficiency of the algorithm.

[0008] Optionally, in some embodiments, calculating the pose of the LiDAR based on the grid point cloud data includes: performing planar fitting on the grid point cloud data along the X direction of the vehicle coordinate system to obtain a first plane and a second plane; obtaining an initial pose value of the LiDAR based on the first plane and the second plane, and optimizing and iterating the initial pose value to obtain the pose output value of the LiDAR; filtering the pose output value, and calculating the pose of the LiDAR based on the pose output value when the filtered pose output value meets a preset stability condition.

[0009] Based on the above technical means, this application can obtain a first plane and a second plane by performing planar fitting according to the X direction of the vehicle coordinate system based on grid point cloud data. The initial pose value of the LiDAR can be obtained based on the first plane and the second plane. The initial pose value of the LiDAR can be optimized to obtain the pose output value of the LiDAR and filtered. When the filtered pose output value meets the preset stability conditions, the pose of the LiDAR can be calculated based on the pose output value, which can obtain a more accurate pose estimation and avoid unstable pose estimation.

[0010] Optionally, in some embodiments, the pose output value includes the vehicle coordinate system pitch angle, roll angle, and Z-direction offset. The step of filtering the pose output value and calculating the pose of the LiDAR based on the filtered pose output value when it meets a preset stability condition includes: filtering the pitch angle, roll angle, and Z-direction offset respectively to obtain filtered pitch angle, filtered roll angle, and filtered Z-direction offset; if the filtered pitch angle is in a first preset interval, the filtered roll angle is in a second preset interval, and the filtered Z-direction offset is in a third preset interval, then the filtered pose output value is determined to meet the preset stability condition, and the pose of the LiDAR is obtained based on the pitch angle, roll angle, and Z-direction offset.

[0011] Based on the above technical means, this application can verify the pose output value by performing Kalman filtering on the pose output value, and obtain the pose of the lidar when the pose output value meets the preset stability conditions, so as to ensure the stability of the pose output value.

[0012] Optionally, in some embodiments, calculating the score of the point cloud data in each grid includes: fitting a plane within each grid and calculating the angle score between the plane normal vector of each grid and the Z direction of the vehicle coordinate system, the first covariance score of the point cloud data in the Z direction of each grid, and the second covariance score of the point cloud data in the X direction of the vehicle coordinate system; obtaining the score of the point cloud data in each grid based on the angle score, the first covariance score, and the second covariance score.

[0013] Based on the above technical means, this application can evaluate the score of the grid point cloud from multiple dimensions to filter noise, so as to select suitable grid data for final pose estimation, thereby reducing the amount of computation and improving the efficiency of the algorithm.

[0014] Optionally, in some embodiments, obtaining a score for the point cloud data in each grid cell based on the included angle score, the first covariance score, and the second covariance score includes: obtaining a score for the point cloud data in each grid cell based on a preset scoring calculation formula, wherein the preset scoring calculation formula is:

[0015] s = α1s1 + α2s2 + α3s3

[0016] Where s is the preset scoring formula, s1 is the angle score, s2 is the first covariance score, s3 is the second covariance score, and α1, α2 and α3 are weighting coefficients.

[0017] Based on the above technical means, this application can obtain a flatter grid through the included angle score, obtain a grid with more noise through the first covariance, obtain the probability of obstacles through the second covariance score, and combine multiple dimensions such as included angle score, first covariance score and second covariance score to select suitable grid data to reduce the amount of computation and improve the efficiency of the algorithm, so as to perform more accurate pose estimation.

[0018] A second aspect of this application provides a pose estimation device, comprising: an acquisition module for acquiring laser point cloud data of the environment surrounding a vehicle; a filtering module for filtering ground point cloud data from the laser point cloud data, dividing the ground point cloud data into grids, downsampling the point cloud data in each grid, and calculating a score for the point cloud data in each grid; and a calculation module for filtering grid point cloud data with scores greater than a preset value based on the scores of the point cloud data in each grid, and calculating the pose of the lidar based on the grid point cloud data.

[0019] Optionally, in some embodiments, the calculation module further includes: a fitting unit, configured to perform planar fitting on the grid point cloud data according to the X direction of the vehicle coordinate system to obtain a first plane and a second plane; an optimization unit, configured to obtain the initial pose value of the lidar based on the first plane and the second plane, and optimize and iterate the initial pose value to obtain the pose output value of the lidar; and a filtering unit, configured to filter the pose output value, and calculate the pose of the lidar based on the pose output value when the filtered pose output value meets a preset stability condition.

[0020] Optionally, in some embodiments, the pose output value includes a vehicle coordinate system pitch angle, roll angle, and Z-direction offset. The filtering unit includes: a filtering subunit, used to filter the pitch angle, roll angle, and Z-direction offset respectively to obtain the filtered pitch angle, the filtered roll angle, and the filtered Z-direction offset; and a determination subunit, used to determine that the filtered pose output value satisfies a preset stability condition when the filtered pitch angle is in a first preset interval, the filtered roll angle is in a second preset interval, and the filtered Z-direction offset is in a third preset interval, and obtain the pose of the lidar based on the pitch angle, roll angle, and Z-direction offset.

[0021] Optionally, in some embodiments, the filtering module further includes: a second fitting unit, configured to fit the plane within each grid cell and calculate the angle score between the plane normal vector of each grid cell and the Z-direction of the vehicle coordinate system, the first covariance score of the point cloud data in each grid cell in the Z-direction, and the second covariance score of the point cloud data in each grid cell in the X-direction of the vehicle coordinate system; and a scoring unit, configured to obtain a score for the point cloud data in each grid cell based on the angle score, the first covariance score, and the second covariance score.

[0022] Optionally, in some embodiments, the scoring unit is specifically used to: obtain a score for the point cloud data in each grid cell based on the included angle score, the first covariance score, and the second covariance score, according to a preset scoring calculation formula, wherein the preset scoring calculation formula is:

[0023] s = α1s1 + α2s2 + α3s3

[0024] Where s is the preset scoring formula, s1 is the angle score, s2 is the first covariance score, s3 is the second covariance score, and α1, α2 and α3 are weighting coefficients.

[0025] A third aspect of this application provides a vehicle, including: a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the program to implement the pose estimation method as described in the above embodiments.

[0026] A fourth aspect of this application provides a computer-readable storage medium having a computer program stored thereon, which is executed by a processor to implement the pose estimation method as described in the above embodiments.

[0027] Therefore, by filtering out ground points within the region of interest and dividing the region of interest into grids, different point clouds are assigned to corresponding grids. Then, the points in each grid are randomly downsampled to obtain a sparser point cloud. The distribution of points in each grid is then statistically analyzed from multiple dimensions to obtain a statistical score. Grids with high scores are then selected, and the LiDAR pose is calculated based on the point cloud data of the grids with high scores. Thus, downsampling the grids improves algorithm efficiency, and statistical analysis from multiple dimensions effectively filters out ground noise points. By using the LiDAR pose calculated from the filtered grid point cloud, the problem of unstable ground pose estimation and large errors in different scenarios is solved, ensuring high accuracy of the fitted ground.

[0028] Additional aspects and advantages of this application will be set forth in part in the description which follows, and in part will be obvious from the description, or may be learned by practice of this application. Attached Figure Description

[0029] The above and / or additional aspects and advantages of this application will become apparent and readily understood from the following description of the embodiments taken in conjunction with the accompanying drawings, wherein:

[0030] Figure 1 This is a flowchart of the pose estimation method provided according to the embodiments of this application;

[0031] Figure 2 This is a schematic diagram of grid division according to an embodiment of this application;

[0032] Figure 3 This is a schematic diagram of a lidar coordinate system according to an embodiment of this application;

[0033] Figure 4 This is a schematic diagram of a ground coordinate system and a vehicle coordinate system according to an embodiment of this application;

[0034] Figure 5 This is a flowchart of a pose estimation method provided according to an embodiment of this application;

[0035] Figure 6This is a block diagram of a pose estimation device provided according to an embodiment of this application;

[0036] Figure 7 This is a structural schematic diagram of a vehicle provided according to an embodiment of this application.

[0037] Explanation of reference numerals in the attached figures: 10 - pose estimation device, 100 - acquisition module, 200 - screening module, and 300 - calculation module. Detailed Implementation

[0038] The embodiments of this application are described in detail below. Examples of these embodiments are shown in the accompanying drawings, wherein the same or similar reference numerals denote the same or similar elements or elements having the same or similar functions throughout. The embodiments described below with reference to the accompanying drawings are exemplary and intended to explain this application, and should not be construed as limiting this application.

[0039] The pose estimation method, apparatus, vehicle, and storage medium of this application are described below with reference to the accompanying drawings. Addressing the problem of unstable ground pose estimation and large errors in different scenarios mentioned in the background art, this application provides a pose estimation method. In this method, ground points within a region of interest are selected, and the region of interest is divided into grids, assigning different point clouds to corresponding grids. Then, points in each grid are randomly downsampled to obtain a sparser point cloud. The distribution of points in each grid is then statistically analyzed from multiple dimensions to obtain a statistical score. Grids with high scores are then selected, and the pose of the LiDAR is calculated based on the point cloud data of these high-scoring grids. Thus, downsampling the grids improves algorithm efficiency, and statistical analysis from multiple dimensions effectively filters out ground noise points. The LiDAR pose is calculated using the filtered grid point cloud data, solving the problem of unstable ground pose estimation and large errors in different scenarios, ensuring high accuracy in the fitted ground.

[0040] Specifically, Figure 1 This is a flowchart illustrating a pose estimation method provided in an embodiment of this application.

[0041] like Figure 1 As shown, the pose estimation method includes the following steps:

[0042] In step S101, laser point cloud data of the environment surrounding the vehicle is collected.

[0043] Specifically, in this embodiment of the application, laser point cloud data of the environment surrounding the vehicle can be collected by an onboard LiDAR.

[0044] In step S102, ground point cloud data is selected from the laser point cloud data, and the ground point cloud data is divided into grids. After downsampling the point cloud data in each grid, the score of the point cloud data in each grid is calculated.

[0045] As one possible implementation, this application embodiment can collect laser point cloud data of the vehicle's surrounding environment using an onboard LiDAR. The collected laser point cloud data is then preprocessed and processed by a ground detection module to obtain ground and non-ground points. The Regions of Interest (ROIs) within the ground points are then selected, and the ROIs are divided into grids. Different point clouds are assigned to corresponding grids. This application embodiment can use rectangular grids, dividing the grid into near-large and far-small grids according to the near-sparse and far-dense characteristics of the point cloud. Figure 2 As shown, this ensures that the number of points in each grid is as close as possible. Then, random downsampling is performed on the points in each grid. Since this embodiment is sensitive to the number of LiDAR point clouds, a doubling of the number of LiDAR point clouds can increase the algorithm's time consumption by 3-4 times. Therefore, this embodiment needs to balance the number of LiDAR point clouds and algorithm performance by using a random downsampling method to downsample the points within the grid by a factor of 2, resulting in a sparser point cloud.

[0046] Optionally, in some embodiments, calculating the score of the point cloud data in each grid cell includes: fitting a plane within each grid cell and calculating the angle score between the plane normal vector of each grid cell and the Z direction of the vehicle coordinate system, the first covariance score of the point cloud data in the Z direction of each grid cell, and the second covariance score of the point cloud data in the X direction of the vehicle coordinate system; obtaining the score of the point cloud data in each grid cell based on the angle score, the first covariance score, and the second covariance score.

[0047] Among them, the vehicle coordinate system can be combined with Figure 3 The coordinate system of the LiDAR is defined as shown. The vehicle coordinate system can be based on the centroid of the LiDAR as its origin. The X-axis of the vehicle coordinate system can be the direction of vehicle movement, the Y-axis can be the left side of the vehicle, and the Z-axis can be directly above the vehicle. This establishes the coordinate system as shown. Figure 4 The vehicle coordinate system shown.

[0048] Understandably, calculating the angle between the grid plane normal vector and the Z-direction of the vehicle coordinate system can determine whether a ground point exists in the grid. Since the ground can be viewed as a gradually changing curved surface, replacing the curved surface with grid blocks not only approximates the surface to the greatest extent possible, but also simplifies the complex mathematical model. Using this judgment condition, non-ground areas can be filtered out to the greatest extent possible, and the true ground areas can be selected.

[0049] Calculating the first covariance score of the point cloud data in the Z direction for each grid cell can effectively filter out noisy grids. Since the ground detection module struggles to completely remove all non-ground points when processing areas in contact with objects, the first covariance score effectively filters out grids containing non-ground points. Compared to visual solutions that use segmentation results to filter ground regions, which generate more noise in edge areas, this embodiment effectively filters out noisy grids, achieving a significant noise reduction effect.

[0050] Calculating the second covariance score of each grid point cloud in the X direction can effectively filter out grids with obstacles. If there are obstacles in the grid, the covariance in the X direction is relatively small. Therefore, grids with obstacles can be filtered out based on the second covariance score. At the same time, this value can also filter out grids with relatively sparse point clouds, especially distant grids, thus ensuring that the selected grids are free of obstacles and have relatively dense point clouds.

[0051] In summary, the angle score indicates whether the grid is flat; the flatter the grid, the higher the score. The first covariance score indicates whether there is noise above the ground; if there is a lot of noise in the area above the ground, the score will be lower. The second covariance score indicates the probability of obstacles in the grid; if there are obstacles in the grid, the score will be lower.

[0052] The calculation of the included angle score, the first covariance score, and the second covariance score will be explained in detail below with reference to specific embodiments.

[0053] Specifically, for each point within a raster, the distribution of the raster point cloud needs to be evaluated from multiple dimensions, and a score for each raster needs to be calculated. The following are different perspectives for calculating raster scores:

[0054] 1. Calculate the angle score between the normal vector of each grid plane and the Z direction of the vehicle coordinate system.

[0055] First, the PCA algorithm is used to fit the planes within the grid, and then the angle score between the plane normal vector and the Z-axis of each grid is calculated.

[0056] The equation of the plane is:

[0057] ax + by + cz + d = 0; (1)

[0058] The Z-axis normal vector is:

[0059] z = (0, 0, 1); (2)

[0060] The angle between the plane normal vector and the z-axis:

[0061] θ = acos(c); (3)

[0062] Score for the angle between plane normal vectors:

[0063]

[0064] Among them, μ and σ are prior values, and it is necessary to statistically determine the angle between the plane normal and the z-axis in multiple scenarios.

[0065] 2. The first covariance score of the point cloud data in the Z direction for each grid cell.

[0066]

[0067] Where: v z μ represents the covariance of the raster point cloud in the Z direction. z σ represents the mean of the raster point cloud in the Z direction. z This represents the standard deviation of the raster point cloud in the Z direction.

[0068] 3. The second covariance score of the point cloud data in each grid in the X direction of the vehicle coordinate system.

[0069]

[0070] Among them, v x μ represents the covariance of the raster point cloud in the X direction. x σ represents the mean of the raster point cloud in the X direction. x This represents the standard deviation of the raster point cloud in the X direction.

[0071] Optionally, in some embodiments, a score for the point cloud data in each grid cell is obtained based on the included angle score, the first covariance score, and the second covariance score, including:

[0072] Based on a preset scoring formula, the score for the point cloud data in each grid cell is obtained according to the included angle score, the first covariance score, and the second covariance score. The preset scoring formula is as follows:

[0073] s = α1s1 + α2s2 + α3s3; (7)

[0074] Where s is the preset scoring formula, s1 is the angle score, s2 is the first covariance score, s3 is the second covariance score, and α1, α2 and α3 are weighting coefficients.

[0075] In actual implementation, the embodiments of this application can also set different weight coefficients for the included angle score, the first covariance score and the second covariance score when calculating the score of the point cloud data in each grid, so as to adapt to different application scenarios, thereby improving the quality of the selected grid point cloud data and effectively enhancing the robustness of the algorithm.

[0076] By calculating the angle score, the first covariance score, and the second covariance score, the final score for each grid cell can be obtained. Calculating these three scores helps to filter out ground grids with minimal noise and obstacles, maximizing noise reduction and ensuring the algorithm's robustness.

[0077] It should be noted that α1+2+3=1, where α1, α2, and α3 can be preset by the user, obtained through a limited number of experiments, or obtained through a limited number of computer simulations. The specific values ​​can be selected according to the actual application scenario, and are not specifically limited here. Preferably, in this embodiment, α1=0.4, α2=0.2, and α3=0.4. The above calculations need to be normalized.

[0078] In step S103, based on the score of the point cloud data in each grid, grid point cloud data with a score greater than a preset score are selected, and the pose of the lidar is calculated based on the grid point cloud data.

[0079] The preset score can be set by the user, obtained through a limited number of experiments, or obtained through a limited number of computer simulations; no specific limitations are made here.

[0080] Specifically, in this embodiment of the application, after obtaining the score of the point cloud data in each grid, the grid point cloud data with the larger score is selected, and the score of the lidar is calculated based on the grid point cloud data with the score greater than the preset score.

[0081] Optionally, in some embodiments, calculating the pose of the LiDAR based on grid point cloud data includes: performing planar fitting on the grid point cloud data according to the X direction of the vehicle coordinate system to obtain a first plane and a second plane; obtaining the initial pose value of the LiDAR based on the first plane and the second plane, and optimizing and iterating the initial pose value to obtain the pose output value of the LiDAR; filtering the pose output value, and calculating the pose of the LiDAR based on the pose output value when the filtered pose output value meets the preset stability conditions.

[0082] As one possible implementation, embodiments of this application select high-scoring grid point clouds and calculate the plane using the Ransac algorithm. The scores of each grid are obtained through the above calculations, and then suitable grids are selected according to their scores. Finally, the grids are determined based on the distance in the X direction (in the LiDAR coordinate system as shown in the image). Figure 3 As shown), the grid is divided into two parts:

[0083]

[0084] Among them, the raster point cloud with x < 20 is fitted to the first plane using Ransac; the raster point cloud with 20 ≤ x ≤ 40 is fitted to a plane using Ransac.

[0085] This yields two planes, from which the initial pitch angle can be calculated using geometric relationships. init and the initial roll angle value init Two planes are fitted using Ransac, and then the initial values ​​of the pitch and roll angles are solved using geometric relationships.

[0086] The equation of the plane is:

[0087] ax + y + cz + = 0; (9)

[0088] The plane normal vector is:

[0089] q = (a, b, c); (10)

[0090] The ground normal vector is:

[0091] n = (0, 0, 1); (11)

[0092] The angle between the plane equation and the ground normal vector in the vehicle coordinate system is:

[0093]

[0094]

[0095] This leads to the formula for calculating the pitch angle, while the calculation for the roll angle is as follows:

[0096] The y-axis vector of the lidar is:

[0097] l = (0, 1, 0); (14)

[0098]

[0099]

[0100] Pitch init =;(17)

[0101] roll init =;(18)

[0102] This leads to the formula for the roll angle, and the pitch can be obtained through the above calculations. init and roll init (Initial value).

[0103] Optimize pitch and roll using optimization methods and calculate the z-value. Optimize the initial values ​​(pitch) using Ceres. init and roll init Get pitch opt and roll opt The optimization problem is described below.

[0104] The plane equation (9) can be expressed as follows:

[0105]

[0106] The transformation of points in the plane by the transformation matrix can be represented as follows:

[0107]

[0108] Where T is the transform matrix, which represents the transformation matrix from the vehicle coordinate system to the ground coordinate system, and can be 4*4;

[0109] The offset between the ground in the vehicle coordinate system and the ground in the ground coordinate system is (e.g.) Figure 4 As shown):

[0110]

[0111] Where z is the offset in the z-axis direction.

[0112] Therefore, T is:

[0113]

[0114] It is the transformed matrix.

[0115] The equation of the plane after transformation by the transformation matrix is:

[0116]

[0117] Where the matrix [A,B,C,D] represents the coefficients of the transformed plane equation, and the relationship between this matrix and the matrix before the transformation is as follows:

[0118] [A,B,C,D]=[a,b,c,d]· -1 ;(twenty four)

[0119] The optimized residual variables are:

[0120] Res = ;(25)

[0121] After 2-3 iterations, the optimized value and z-value (pitch) can be obtained. opt and roll opt ).

[0122] Optionally, in some embodiments, the pose output value includes the vehicle coordinate system pitch angle, roll angle, and Z-direction offset. The pose output value is filtered, and when the filtered pose output value meets a preset stability condition, the pose of the LiDAR is calculated based on the pose output value. This includes: filtering the pitch angle, roll angle, and Z-direction offset respectively to obtain the filtered pitch angle, the filtered roll angle, and the filtered Z-direction offset; if the filtered pitch angle is in a first preset interval, the filtered roll angle is in a second preset interval, and the filtered Z-direction offset is in a third preset interval, then it is determined that the filtered pose output value meets the preset stability condition, and the pose of the LiDAR is obtained based on the pitch angle, roll angle, and Z-direction offset.

[0123] Specifically, after calculating the pitch angle, roll angle, and Z-direction offset of the lidar based on grid point cloud data that meets preset conditions, a Kalman filter is used to adjust the pitch. opt , roll opt and z opt The value is filtered to obtain the pitch. f , roll f and z f .

[0124] x k =*x k-1 (26)

[0125] z k =*x k (27)

[0126] in,

[0127]

[0128]

[0129]

[0130]

[0131] The above is the model of the Kalman filter for pose estimation. The model is simple in principle and effective.

[0132] It should be noted that the embodiments of this application can also verify the pitch. f ,roll f and z f The accuracy of the value. By statistically analyzing the true pose values ​​under different scenarios, we can obtain the pose changes under different scenarios. This allows us to determine the limit value of pose change for each scenario. By comparing the limit value with the currently calculated pose, we can determine whether the current value is correct.

[0133]

[0134]

[0135]

[0136]

[0137]

[0138]

[0139] in, It is the upper limit of the pitch angle. This is the lower bound of the pitch angle. When the pitch and roll angles exceed the upper and lower bounds, this embodiment outputs a calibration value to ensure that the calculation results meet the usage requirements as much as possible.

[0140] Therefore, by preprocessing the laser point cloud data acquired by LiDAR and processing it through the ground detection module, ground and non-ground points are obtained. Based on the ground detection module, the ground points are processed to filter out the ground points in the region of interest. Then, the region of interest is divided into grids, and different point clouds are assigned to corresponding grids. Next, the points in each grid are randomly downsampled to obtain a sparser point cloud. Then, the distribution of each grid point is statistically analyzed from multiple dimensions to obtain a statistical score, which is used as a condition for subsequent grid selection. Then, the grids with high scores are selected, and the grid points with high scores are extracted. Then, the plane is fitted using Ransac, and the initial values ​​of Roll and Pitch are calculated. Finally, the z-value is calculated using the Ceres optimization algorithm and optimized to obtain Pitch_opt and Roll_opt. This solves the problem of unstable ground pose estimation and large error in different scenarios, and can effectively filter out ground noise points, thereby ensuring high accuracy of the fitted ground.

[0141] To enable those skilled in the art to further understand the pose estimation method of the embodiments of this application, a detailed description is provided below with reference to specific embodiments.

[0142] like Figure 5 As shown, Figure 5This is a flowchart of the pose estimation method proposed according to an embodiment of this application.

[0143] S501: Activate the vehicle-mounted LiDAR to collect laser point clouds of the surrounding environment of the vehicle, and obtain ground points from the ground detection module after preprocessing.

[0144] S502 filters out the laser point cloud of the region of interest (ROI).

[0145] S503, and divide the area into grids, and downsample the laser point cloud within the grid.

[0146] S504: Calculate the score of the laser point cloud in each grid cell, select the grid point clouds with high scores, and use the Ransac algorithm to calculate the plane.

[0147] S505, calculates Pitch_init and Roll_init using geometric relationships.

[0148] S506 uses optimization methods to optimize pitch and roll, and calculates the z-value.

[0149] The S507 uses a Kalman filter to filter the pitch, roll, and z values.

[0150] S508 verifies the correctness of pitch, roll, and z values.

[0151] According to the pose estimation method proposed in this application, ground points within the region of interest are filtered out, and the region of interest is divided into grids. Different point clouds are assigned to corresponding grids. Then, the points in each grid are randomly downsampled to obtain a sparser point cloud. The distribution of points in each grid is statistically analyzed from multiple dimensions to obtain a statistical score. The grids with high scores are then selected, and the pose of the LiDAR is calculated based on the point cloud data of the grids with high scores. Thus, by downsampling the grids, the algorithm efficiency is improved, and by performing statistical analysis from multiple dimensions, noise points on the ground can be effectively filtered out. By using the LiDAR pose calculated from the selected grid point cloud, the problem of unstable ground pose estimation and large error in different scenarios is solved, ensuring high accuracy of the fitted ground.

[0152] Next, the pose estimation device proposed according to the embodiments of this application is described with reference to the accompanying drawings.

[0153] Figure 6 This is a block diagram of a pose estimation device according to an embodiment of this application.

[0154] like Figure 6 As shown, the pose estimation device 10 includes: an acquisition module 100, a filtering module 200, and a calculation module 300.

[0155] The acquisition module 100 is used to acquire laser point cloud data of the environment surrounding the vehicle.

[0156] The filtering module 200 is used to filter ground point cloud data from laser point cloud data, divide the ground point cloud data into grids, and calculate the score of the point cloud data in each grid after downsampling the point cloud data in each grid.

[0157] The calculation module 300 is used to filter out grid point cloud data with scores greater than a preset value based on the score of point cloud data in each grid, and calculate the pose of the LiDAR based on the grid point cloud data.

[0158] Optionally, in some embodiments, the calculation module 300 further includes a fitting unit, an optimization unit, and a filtering unit.

[0159] The fitting unit is used to perform planar fitting on the grid point cloud data according to the X direction of the vehicle coordinate system to obtain the first plane and the second plane.

[0160] The optimization unit is used to obtain the initial pose value of the lidar based on the first plane and the second plane, and to optimize and iterate the initial pose value to obtain the pose output value of the lidar.

[0161] The filtering unit is used to filter the pose output value and calculate the pose of the lidar based on the pose output value when the filtered pose output value meets the preset stability conditions.

[0162] Optionally, in some embodiments, the pose output values ​​include the vehicle coordinate system pitch angle, roll angle, and Z-direction offset, and the filtering unit includes: a filtering subunit and a decision subunit.

[0163] The filtering subunit is used to filter the pitch angle, roll angle and Z-direction offset respectively to obtain the filtered pitch angle, the filtered roll angle and the filtered Z-direction offset.

[0164] The determination subunit is used to determine that the filtered pose output value meets the preset stability conditions when the filtered pitch angle is in the first preset range, the filtered roll angle is in the second preset range, and the filtered Z-direction offset is in the third preset range, and obtains the pose of the lidar based on the pitch angle, roll angle and Z-direction offset.

[0165] Optionally, in some embodiments, the screening module further includes a second fitting unit and a scoring unit.

[0166] The second fitting unit is used to fit the plane within each grid cell and calculate the angle score between the plane normal vector of each grid cell and the Z direction of the vehicle coordinate system, the first covariance score of the point cloud data in the Z direction of each grid cell, and the second covariance score of the point cloud data in the X direction of the vehicle coordinate system.

[0167] The scoring unit is used to score the point cloud data in each grid cell based on the included angle score, the first covariance score, and the second covariance score.

[0168] Optionally, in some embodiments, the scoring unit includes: obtaining a score for the point cloud data in each grid cell based on the included angle score, the first covariance score, and the second covariance score, according to a preset scoring calculation formula, wherein the preset scoring calculation formula is:

[0169] s = α1s1 + α2s2 + α3s3

[0170] Where s is the preset scoring formula, s1 is the angle score, s2 is the first covariance score, s3 is the second covariance score, and α1, α2 and α3 are weighting coefficients.

[0171] It should be noted that the foregoing explanation of the pose estimation method embodiment also applies to the pose estimation device of this embodiment, and will not be repeated here.

[0172] According to the pose estimation device proposed in this application, ground points within the region of interest are filtered out, and the region of interest is divided into grids. Different point clouds are assigned to corresponding grids. Then, the points in each grid are randomly downsampled to obtain a sparser point cloud. The distribution of points in each grid is statistically analyzed from multiple dimensions to obtain a statistical score. The grids with high scores are then selected, and the pose of the LiDAR is calculated based on the point cloud data of the grids with high scores. Thus, by downsampling the grids, the algorithm efficiency is improved, and by performing statistical analysis from multiple dimensions, noise points on the ground can be effectively filtered out. The LiDAR pose is calculated from the LiDAR point cloud data of the selected grids, which solves the problem of unstable ground pose estimation and large error in different scenarios, ensuring high accuracy of the fitted ground.

[0173] Figure 7 A schematic diagram of the structure of a vehicle provided in an embodiment of this application. The vehicle may include:

[0174] The memory 701, the processor 702, and the computer program stored on the memory 701 and executable on the processor 702.

[0175] When the processor 702 executes the program, it implements the pose estimation method provided in the above embodiments.

[0176] Furthermore, the vehicle also includes:

[0177] Communication interface 703 is used for communication between memory 701 and processor 702.

[0178] The memory 701 is used to store computer programs that can run on the processor 702.

[0179] The memory 701 may include high-speed RAM (Random Access Memory) memory, and may also include non-volatile memory, such as at least one disk storage.

[0180] If the memory 701, processor 702, and communication interface 703 are implemented independently, then the communication interface 703, memory 701, and processor 702 can be interconnected via a bus to complete communication between them. The bus can be an ISA (Industry Standard Architecture) bus, a PCI (Peripheral Component Interconnect) bus, or an EISA (Extended Industry Standard Architecture) bus, etc. The bus can be divided into address bus, data bus, control bus, etc. For ease of representation, Figure 7 The bus is represented by a single thick line, but this does not mean that there is only one bus or one type of bus.

[0181] Optionally, in a specific implementation, if the memory 701, processor 702, and communication interface 703 are integrated on a single chip, then the memory 701, processor 702, and communication interface 703 can communicate with each other through an internal interface.

[0182] The processor 702 may be a CPU (Central Processing Unit), an ASIC (Application Specific Integrated Circuit), or one or more integrated circuits configured to implement the embodiments of this application.

[0183] This application also provides a computer-readable storage medium storing a computer program thereon, which, when executed by a processor, implements the pose estimation method described above.

[0184] In the description of this specification, the references to terms such as "one embodiment," "some embodiments," "example," "specific example," or "some examples," etc., indicate that a specific feature, structure, material, or characteristic described in connection with that embodiment or example is included in at least one embodiment or example of this application. In this specification, the illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples. Moreover, without contradiction, those skilled in the art can combine and integrate the different embodiments or examples described in this specification, as well as the features of different embodiments or examples.

[0185] Furthermore, the terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of technical features indicated. Thus, a feature defined as "first" or "second" may explicitly or implicitly include at least one of that feature. In the description of this application, "N" means at least two, such as two, three, etc., unless otherwise explicitly specified.

[0186] Any process or method described in the flowchart or otherwise herein can be understood as representing a module, segment, or portion of code comprising one or more N executable instructions for implementing custom logic functions or processes, and the scope of the preferred embodiments of this application includes additional implementations in which functions may be performed not in the order shown or discussed, including substantially simultaneously or in reverse order depending on the functions involved, as should be understood by those skilled in the art to which embodiments of this application pertain.

[0187] It should be understood that the various parts of this application can be implemented using hardware, software, firmware, or a combination thereof. In the above embodiments, the N steps or methods can be implemented using software or firmware stored in memory and executed by a suitable instruction execution system. For example, if implemented in hardware, as in another embodiment, it can be implemented using any one or a combination of the following techniques known in the art: discrete logic circuits having logic gates for implementing logical functions on data signals, application-specific integrated circuits (ASICs) having suitable combinational logic gates, programmable gate arrays (FPGAs), field-programmable gate arrays (FPGAs), etc.

[0188] Those skilled in the art will understand that all or part of the steps of the methods in the above embodiments can be implemented by a program instructing related hardware. The program can be stored in a computer-readable storage medium, and when executed, the program includes one or a combination of the steps of the method embodiments.

[0189] Although embodiments of this application have been shown and described above, it is understood that the above embodiments are exemplary and should not be construed as limiting this application. Those skilled in the art can make changes, modifications, substitutions and variations to the above embodiments within the scope of this application.

Claims

1. A pose estimation method, characterized in that, Includes the following steps: Collect laser point cloud data of the environment surrounding the vehicle; Ground point cloud data is selected from the laser point cloud data, and the ground point cloud data is divided into grids. After downsampling the point cloud data in each grid, the score of the point cloud data in each grid is calculated. as well as Based on the score of the point cloud data in each grid, the grid point cloud data with a score greater than a preset value are selected, and the pose of the lidar is calculated based on the grid point cloud data. The step of calculating the pose of the lidar based on the grid point cloud data includes: Plane fitting is performed on the grid point cloud data according to the X direction of the vehicle coordinate system to obtain the first plane and the second plane; The initial pose value of the lidar is obtained based on the first plane and the second plane, and the initial pose value is optimized and iterated to obtain the pose output value of the lidar. The pose output value is filtered, and when the filtered pose output value meets the preset stability condition, the pose of the lidar is calculated based on the pose output value.

2. The method according to claim 1, characterized in that, The pose output value includes the vehicle coordinate system pitch angle, roll angle, and Z-direction offset. The process of filtering the pose output value and calculating the LiDAR pose based on the filtered pose output value when it meets preset stability conditions includes: The pitch angle, roll angle, and Z-direction offset are filtered respectively to obtain the filtered pitch angle, the filtered roll angle, and the filtered Z-direction offset; If the filtered pitch angle is in the first preset range, the filtered roll angle is in the second preset range, and the filtered Z-direction offset is in the third preset range, then the filtered pose output value is determined to meet the preset stability condition, and the pose of the lidar is obtained based on the pitch angle, the roll angle, and the Z-direction offset.

3. The method according to claim 1, characterized in that, The calculation of the score for the point cloud data in each grid cell includes: Fit the plane within each grid cell, and calculate the angle score between the plane normal vector of each grid cell and the Z direction of the vehicle coordinate system, the first covariance score of the point cloud data in the Z direction of each grid cell, and the second covariance score of the point cloud data in the X direction of the vehicle coordinate system. The score of the point cloud data in each grid is obtained based on the included angle score, the first covariance score, and the second covariance score.

4. The method according to claim 3, characterized in that, The step of obtaining a score for the point cloud data in each grid cell based on the included angle score, the first covariance score, and the second covariance score includes: Based on a preset scoring formula, the score of the point cloud data in each grid cell is obtained according to the included angle score, the first covariance score, and the second covariance score, wherein the preset scoring formula is: ； in, The preset scoring formula, To score for the included angle, The first covariance score, The second covariance score. , and These are the weighting coefficients.

5. A pose estimation device, characterized in that, include: The acquisition module is used to acquire laser point cloud data of the environment surrounding the vehicle; The filtering module is used to filter out ground point cloud data from the laser point cloud data, divide the ground point cloud data into grids, and calculate the score of the point cloud data in each grid after downsampling the point cloud data in each grid. as well as The calculation module is used to filter out grid point cloud data with scores greater than a preset value based on the score of the point cloud data in each grid, and calculate the pose of the lidar based on the grid point cloud data. The computing module includes: The fitting unit is used to perform planar fitting on the grid point cloud data according to the X direction of the vehicle coordinate system to obtain a first plane and a second plane; An optimization unit is configured to obtain the initial pose value of the lidar based on the first plane and the second plane, and to optimize and iterate the initial pose value to obtain the pose output value of the lidar. The filtering unit is used to filter the pose output value and calculate the pose of the lidar based on the pose output value when the filtered pose output value meets the preset stability conditions.

6. The apparatus according to claim 5, characterized in that, The pose output values ​​include the vehicle coordinate system pitch angle, roll angle, and Z-direction offset. The filtering unit includes: The filtering subunit is used to filter the pitch angle, the roll angle and the Z-direction offset respectively to obtain the filtered pitch angle, the filtered roll angle and the filtered Z-direction offset; The determination subunit is used to determine that the filtered pose output value meets the preset stability conditions when the filtered pitch angle is in the first preset range, the filtered roll angle is in the second preset range, and the filtered Z-direction offset is in the third preset range, and obtains the pose of the lidar based on the pitch angle, the roll angle, and the Z-direction offset.

7. The apparatus according to claim 5, wherein the screening module comprises: The second fitting unit is used to fit the plane within each grid cell and calculate the angle score between the plane normal vector of each grid cell and the Z direction of the vehicle coordinate system, the first covariance score of the point cloud data in each grid cell in the Z direction, and the second covariance score of the point cloud data in each grid cell in the X direction of the vehicle coordinate system. The scoring unit is used to obtain a score for the point cloud data in each grid cell based on the included angle score, the first covariance score, and the second covariance score.

8. The apparatus according to claim 7, characterized in that, The scoring unit is specifically used for: Based on a preset scoring formula, the score of the point cloud data in each grid cell is obtained according to the included angle score, the first covariance score, and the second covariance score, wherein the preset scoring formula is: ； in, The preset scoring formula, To score for the included angle, The first covariance score, The second covariance score. , and These are the weighting coefficients.

9. A vehicle, characterized in that, include: A memory, a processor, and a computer program stored in the memory and executable on the processor, the processor executing the program to implement the pose estimation method as described in any one of claims 1-4.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that, The program is executed by the processor to implement the pose estimation method as described in any one of claims 1-4.

Images