CN-116804745-B - Pose estimation method, pose estimation device, vehicle and storage medium

CN116804745BCN 116804745 BCN116804745 BCN 116804745BCN-116804745-B

Abstract

The application relates to the technical field of automatic driving, in particular to a pose estimation method, a pose estimation device, a vehicle and a storage medium, wherein the pose estimation method comprises the steps of collecting laser point cloud data of the surrounding environment of the vehicle, screening ground point cloud data from the laser point cloud data, carrying out grid division on the ground point cloud data, carrying out downsampling on the point cloud data in each grid, calculating the score of the point cloud data in each grid, screening out grid point cloud data with the score larger than a preset score based on the score of the point cloud data in each grid, and calculating the pose of a laser radar according to the grid point cloud data. Therefore, the algorithm efficiency is improved by downsampling the grids, noise points on the ground can be well filtered through statistics from multiple dimensions, the problem that the ground pose estimation is unstable and the error is large under different scenes is solved by calculating the laser radar pose through the screened grid point cloud, and the fitted ground precision is ensured to be higher.

Inventors

LIU LEI
QIAN SHAOHUA

Assignees

重庆长安汽车股份有限公司

Dates

Publication Date: 20260505
Application Date: 20230629

Claims (10)

1. The pose estimation method is characterized by comprising the following steps of: Collecting laser point cloud data of the surrounding environment of the vehicle; Screening out ground point cloud data from the laser point cloud data, performing grid division on the ground point cloud data, performing downsampling on the point cloud data in each grid, calculating the score of the point cloud data in each grid, and Screening out grid point cloud data with the score larger than a preset score based on the score of the point cloud data in each grid, and calculating to obtain the pose of the laser radar according to the grid point cloud data; the calculating the pose of the laser radar according to the grid point cloud data comprises the following steps: performing plane fitting on the grid point cloud data according to the X direction of a vehicle body coordinate system to obtain a first plane and a second plane; Obtaining a pose initial value of the laser radar according to the first plane and the second plane, and optimizing and iterating the pose initial value to obtain a pose output value of the laser radar; And filtering the pose output value, and calculating the pose of the laser radar according to the pose output value when the filtered pose output value meets a preset stable condition.
2. The method according to claim 1, wherein the pose output values include a pitch angle, a roll angle and a Z-direction offset of a vehicle body coordinate system, the filtering the pose output values, and when the filtered pose output values meet a preset stability condition, calculating a pose of the laser radar according to the pose output values, including: Respectively carrying out filtering treatment on the pitch angle, the roll angle and the Z-direction offset to obtain the filtered pitch angle, the filtered roll angle and the filtered Z-direction offset; And 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, judging that the filtered pose output value meets a preset stability condition, and obtaining the pose of the laser radar according to the pitch angle, the roll angle and the Z-direction offset.
3. The method of claim 1, wherein said calculating a score for point cloud data in said each grid comprises: Fitting planes in each grid, and calculating an included angle score of a normal vector of each grid plane and the Z direction of a vehicle body coordinate system, a first covariance score of point cloud data in each grid in the Z direction and a second covariance score of point cloud data in each grid in the X direction of the vehicle body coordinate system; and obtaining the score of the point cloud data in each grid according to the included angle score, the first covariance score and the second covariance score.
4. The method of claim 3, wherein said scoring of cloud data in each grid based on said included angle score, said first covariance score, and said second covariance score comprises: Obtaining a score of the point cloud data in each grid according to the included angle score, the first covariance score and the second covariance score based on a preset score calculation formula, wherein the preset score calculation formula is as follows: ; Wherein, the For the preset score calculation formula, For the score of the included angle, For the first covariance score, For the second covariance score, , And Is a weight coefficient.
5. A pose estimation device, characterized by comprising: the acquisition module is used for acquiring laser point cloud data of the surrounding environment of the vehicle; The screening module is used for screening the ground point cloud data from the laser point cloud data, carrying out grid division on the ground point cloud data, carrying out downsampling on the point cloud data in each grid, calculating the score of the point cloud data in each grid, and The calculation module is used for screening out grid point cloud data with the score larger than a preset score based on the score of the point cloud data in each grid, and calculating to obtain the pose of the laser radar according to the grid point cloud data; the computing module comprises: the fitting unit is used for carrying out plane fitting on the grid point cloud data according to the X direction of the vehicle body coordinate system to obtain a first plane and a second plane; The optimizing unit is used for obtaining the pose initial value of the laser radar according to the first plane and the second plane, and optimizing and iterating the pose initial value to obtain the pose output value of the laser radar; And the filtering unit is used for filtering the pose output value and calculating the pose of the laser radar according to the pose output value when the filtered pose output value meets the preset stable condition.
6. The apparatus of claim 5, wherein the pose output values include a vehicle body coordinate system pitch angle, roll angle, and Z-direction offset, the filtering unit comprising: the filtering subunit is used for respectively carrying out filtering treatment on the pitch angle, the roll angle and the Z-direction offset to obtain the filtered pitch angle, the filtered roll angle and the filtered Z-direction offset; And the judging subunit is used for judging that the filtered pose output value meets a preset stable 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 obtaining the pose of the laser radar according to the pitch angle, the roll angle and the Z-direction offset.
7. The apparatus of claim 5, the screening module comprising: the second fitting unit is used for fitting the plane in each grid, and calculating an included angle score of a normal vector of each grid plane and the Z direction of the vehicle body coordinate system, a first covariance score of the point cloud data of each grid in the Z direction and a second covariance score of the point cloud data of each grid in the X direction of the vehicle body coordinate system; And the scoring unit is used for scoring the point cloud data in each grid according to the included angle score, the first covariance score and the second covariance score.
8. The apparatus according to claim 7, wherein the scoring unit is specifically configured to: Obtaining a score of the point cloud data in each grid according to the included angle score, the first covariance score and the second covariance score based on a preset score calculation formula, wherein the preset score calculation formula is as follows: ; Wherein, the For the preset score calculation formula, For the score of the included angle, For the first covariance score, For the second covariance score, , And Is a weight coefficient.
9. A vehicle comprising a memory, a processor and a computer program stored on the memory and executable on the processor, the processor executing the program to implement the pose estimation method according to any of claims 1-4.
10. A computer-readable storage medium, on which a computer program is stored, characterized in that the program is executed by a processor for implementing the pose estimation method according to any of claims 1-4.

Description

Pose estimation method, pose estimation device, vehicle and storage medium Technical Field The application relates to the technical field of automatic driving, in particular to a pose estimation method, a pose estimation device, a vehicle and a storage medium. Background Vehicle-mounted lidar is one of the important sensors in the automotive field, which is mainly used to provide real-time location of the vehicle and depth information of the surrounding environment. The laser radar has the advantages of being capable of obtaining point three-dimensional information, high in measurement accuracy, independent of external illumination conditions and the like, so that more and more host factories carry the laser radar. Meanwhile, a stricter requirement is put forward on a laser radar sensing algorithm, however, the existing laser radar has the problems of instability and large error in estimating the pose relative to the ground. The method comprises the steps of acquiring an image, determining a ground area in the image, estimating a depth value of the image, fitting a three-dimensional ground by using the depth value, and finally confirming the posture of a camera relative to the ground, and dividing the point cloud data into first target point cloud data representing a calibration surface and second target point cloud data representing a non-calibration surface by acquiring the point cloud data of the laser radar irradiated on the calibration body, and finally calculating the three-dimensional coordinates of the target point under a coordinate system of the laser radar according to the intersection relation between the first target point cloud data and the second target point cloud data. However, the first ground segmentation is prone to noise in the edge region (especially in a congested scene), is prone to influence on subsequent ground fitting, and the depth estimation algorithm is relatively large in influence and low in accuracy, another requires a specific field, cannot output estimated pose values in real time, and sometimes requires manual feature screening. Disclosure of Invention The application provides a pose estimation method, a pose estimation device, a vehicle and a storage medium, which are used for solving the problems of unstable ground pose estimation and large error in different scenes and can obtain more accurate pose estimation. An embodiment of the first aspect of the application provides a pose estimation method, which comprises the steps of collecting laser point cloud data of a vehicle surrounding environment, screening out ground point cloud data from the laser point cloud data, carrying out grid division on the ground point cloud data, carrying out downsampling on the point cloud data in each grid, calculating the score of the point cloud data in each grid, screening out grid point cloud data with the score larger than a preset score based on the score of the point cloud data in each grid, and calculating the pose of a laser radar according to the grid point cloud data. According to the technical means, the method and the system can acquire the point cloud data around the vehicle, divide the ground point data in the point cloud data into grids, perform downsampling on the point cloud data in the grids, score each grid after downsampling, screen out the grid point cloud data with higher scores, calculate the pose of the laser radar, solve the problems of unstable ground pose estimation and large error under different scenes, and obtain more accurate pose estimation by scoring the grids and filtering out the grid point cloud with large noise, and improve algorithm efficiency. Optionally, in some embodiments, the calculating the pose of the laser radar according to the grid point cloud data includes performing plane fitting on the grid point cloud data according to an X direction of a vehicle body coordinate system to obtain a first plane and a second plane, obtaining a pose initial value of the laser radar according to the first plane and the second plane, optimizing and iterating the pose initial value to obtain a pose output value of the laser radar, filtering the pose output value, and calculating the pose of the laser radar according to the pose output value when the filtered pose output value meets a preset stable condition. According to the technical means, plane fitting can be performed according to the grid point cloud data and the X direction of the vehicle body coordinate system to obtain a first plane and a second plane, so that the pose initial value of the laser radar is obtained according to the first plane and the second plane, the pose initial value of the laser radar is optimized to obtain the pose output value of the laser radar, filtering is performed, when the filtered pose output value meets the preset stable condition, the pose of the laser radar is obtained through calculation according to the pose output value, more accurate pose estimation can be obtained, an