Apparatus for estimating vehicle pose using lidar sensor and method thereof
Disclosed herein is a vehicle including a light detection and ranging (LiDAR) sensor configured to acquire a point cloud for the vehicle, a vehicle control network unit configured to control the vehicle, a motion unit configured to estimate a motion of the vehicle, a controller configured to estimate a ground plane of the LiDAR, and an automatic calibrator configured to calibrate a pose of the vehicle. The LiDAR sensor acquires time information; the vehicle control network unit processes a speed and a yaw rate of the vehicle; the motion unit calculates motion information and estimated information about the vehicle; the controller calculates ground plane information and estimated information about the vehicle; and the automatic calibrator calculates pose information and estimated information about the vehicle.
1. A vehicle comprising:
a light detection and ranging (LiDAR) sensor configured to acquire a point cloud for the vehicle; and
a processor configured to:
control the vehicle;
process a speed and a yaw rate of the vehicle received from an inertial measurement unit (IMU);
estimate motion information of the vehicle;
estimate ground plane information of the LiDAR sensor; and
update pose parameters of the vehicle based on at least one of the estimated motion information, the estimated ground plane information, the processed speed and yaw rate of the vehicle by performing one or more selected from a group of calibration operations, the group of calibration operations including:
in response to the speed of the vehicle being less than or equal to a first specific speed, updating z-axis information in a Cartesian coordinate system, roll information, and pitch information about the Cartesian coordinate system among the pose parameters of the vehicle,
in response to the vehicle rectilinearly traveling, and the roll information and the pitch information being available, updating yaw information among the pose parameters of the vehicle;
in response to the vehicle turning at a speed greater than or equal to a second specific speed and the roll information, the pitch information, and the yaw information being available, updating x-axis information and y-axis information in the Cartesian coordinate system among the pose parameters of the vehicle, wherein the first specific speed and the second specific speed are same or different; and
automatically calibrate a pose of the vehicle based on the updated pose parameters.
2. The vehicle of claim 1 ,
wherein the processor is configured to propagate variances of the estimated motion information and the estimated ground plane information using an online incremental weighted average filter.
3. The vehicle of claim 1 , wherein the estimated motion information includes an estimated motion of the vehicle and first certainty information representing that a reliability of the estimated motion of the vehicle is greater than a first threshold, and the estimated motion information includes an estimated ground plane of the LiDAR sensor and second certainty information representing that a reliability of the estimated ground plane of the LiDAR sensor is greater than a second threshold.
4. The vehicle of claim 1 , wherein the processor is further configured to:
extract the z-axis information, the roll information, and the pitch information based on the estimated ground plane information; and
calculate the z-axis information, the roll information, and the pitch information based on coefficients of a plane equation.
5. The vehicle of claim 1 , wherein the processor is further configured to:
update the yaw information from transformation values of the roll information and the pitch information.
6. The vehicle of claim 1 , wherein
wherein the processor is configured to:
update the x-axis information and the y-axis information based on a transformation value and rotation value of a vehicle reference frame obtained from the processed speed and yaw rate of the vehicle, a LiDAR transformation value, and a rotation value for yaw calibration.
7. The vehicle of claim 1 , wherein the LiDAR sensor is configured to acquire time information related to the point cloud.
8. A method for controlling a vehicle, the method comprising:
acquiring a point cloud for the vehicle;
processing a speed and a yaw rate of the vehicle received from an inertial measurement unit (IMU);
estimate motion information of the vehicle;
estimate ground plane information of a light detection and ranging (LiDAR) sensor configured to acquire a point cloud for the vehicle; and
update pose parameters of the vehicle based on at least one of the estimated motion information, the estimated ground plane information, the processed speed and yaw rate of the vehicle by performing one or more selected from a group of calibration operations, the group of calibration operations including:
in response to the speed of the vehicle being less than or equal to a first specific speed, information in a Cartesian coordinate system, roll information, and pitch information about the Cartesian coordinate system among the pose parameters of the vehicle,
in response to the vehicle rectilinearly traveling, and the roll information and the pitch information being available, updating yaw information among the pose parameters of the vehicle;
in response to the vehicle turning at a speed greater than or equal to a second specific speed and the roll information, the pitch information, and the yaw information being available updating x-axis information and y-axis information in the Cartesian coordinate system among the pose parameters of the vehicle, wherein the first specific speed and the second specific speed are same or different; and
automatically calibrate a pose of the vehicle based on the updated pose parameters.
9. The method of claim 8 ,
wherein the method comprises: propagating variances of the estimated motion information and the estimated ground plane information using an online incremental weighted average filter.
10. The method of claim 8 ,
wherein the estimated motion information includes an estimated motion of the vehicle and first certainty information representing that a reliability of the estimated motion of the vehicle is greater than a first threshold, and the estimated motion information includes an estimated ground plane of the LiDAR sensor and second certainty information representing that a reliability of the estimated ground plane of the LiDAR sensor is greater than a second threshold.
11. The method of claim 8 , wherein the updating of the z-axis information, the roll information, and the pitch information comprises:
extracting the z-axis information, the roll information, and the pitch information based on the estimated ground plane information; and
calculating the z-axis information, the roll information, and the pitch information based on coefficients of a plane equation.
12. The method of claim 8 , wherein the updating of the yaw information comprises:
updating the yaw information from transformation values of the roll information and the pitch information.
13. The method of claim 8 , wherein the updating of the x-axis information and the y-axis information is performed based on a transformation value and rotation value of a vehicle reference frame obtained from the speed and yaw rate of the vehicle, a light detection and ranging (LiDAR) transformation value, and a rotation value for yaw calibration.
14. An apparatus for controlling a vehicle, comprising
a light detection and ranging (LiDAR) sensor configured to acquire a point cloud for the vehicle;
a processor configured to:
control a vehicle;
process a speed and a yaw rate of the vehicle received from an inertial measurement unit (IMU);
estimate motion information of the vehicle, the estimated motion information including an estimated motion of the vehicle and first certainty information representing that reliability of the estimated motion of the vehicle is greater than a first threshold;
estimate ground plane information of a light detection and ranging (LiDAR) sensor configured to acquire a point cloud for the vehicle, the estimated motion information including an estimated ground plane of the LiDAR sensor and second certainty information representing that reliability of the estimated ground plane of the LiDAR sensor is greater than a second threshold; and
update pose parameters of the vehicle based on at least one of the estimated motion information, the estimated ground plane information, the processed speed and yaw rate of the vehicle by performing one or more selected from a group of calibration operations, the group of calibration operations including:
in response to the speed of the vehicle being less than or equal to a first specific speed, information in a Cartesian coordinate system, roll information, and pitch information about the Cartesian coordinate system among the pose parameters of the vehicle,
in response to the vehicle rectilinearly traveling, and the roll information and the pitch information being available, updating yaw information among the pose parameters of the vehicle;
in response to the vehicle turning at a speed greater than or equal to a second specific speed and the roll information, the pitch information, and the yaw information being available, updating x-axis information and y-axis information in the Cartesian coordinate system among the pose parameters of the vehicle, wherein the first specific speed and the second specific speed are same or different; and
automatically calibrate a pose of the vehicle based on the updated pose parameters.
15. The apparatus of claim 14 ,
wherein the processor is configured to propagate variances of the estimated motion information and the estimated ground plane information using an online incremental weighted average filter.
16. The apparatus of claim 14 , wherein the estimated motion information includes an estimated motion of the vehicle and first certainty information representing that a reliability of the estimated motion of the vehicle is greater than a first threshold, and the estimated motion information includes an estimated ground plane of the LiDAR sensor and second certainty information representing that a reliability of the estimated ground plane of the LiDAR sensor is greater than a second threshold.
17. The apparatus of claim 14 ,
wherein the processor is configured to:
extract the z-axis information, the roll information, and the pitch information based on the estimated ground plane information; and
calculate the z-axis information, the roll information, and the pitch information based on coefficients of a plane equation.
18. The apparatus of claim 14 ,
wherein the processor is configured to:
update the yaw information from transformation values of the roll information and the pitch information.
19. The apparatus of claim 14 ,
wherein the processor is configured to:
update the x-axis information and the y-axis information based on a transformation value and rotation value of a vehicle reference frame obtained from the processed speed and yaw rate of the vehicle, a LiDAR transformation value, and a rotation value for yaw calibration.
20. The apparatus of claim 14 , wherein the LiDAR sensor is configured to acquire time information related to the point cloud.