Localization in an automated driving system

Localization is a technique to estimate an ego-vehicle pose on a given map. An automated driving system is composed of a lot of modules such as environment recognition, path planning, control, and localization. The localization is quite simple task more than that of other modules. However, the localization is quite important in the system because the automated driving system typically assumes that the localization successfully works always. Therefore, I tried to develop a robust and accurate localization method with 3D LiDAR.

I worked on this project since 2016 to 2017 with Prof. Eijiro Takeuchi who is a professional of 3D normal distributions transform (NDT). I developed 3D NDT-based localization methods with him and conducted automated driving demonstrations in Japanese public roads.

Fusion of 3D NDT with INS based on EKF

I first developed a fusion method of 3D NDT and inertial navigation system (INS) estimates based on extended Kalman filter (EKF). To fuse the NDT estimate in EKF, a covariance of the estimated is needed to be determined. However, the NDT estimate does not have provide the covariance because NDT minimizes the cost function to estimate the ego-vehicle pose. To determine the covariance, I utilized the Hessian matrix that is calculated during the minimization process. The ellipses depicted in the right hand figures are error ellipses estimated based on the covariance. The NDT estimate is fused based on EKF and the vehicle trajectory is smoothly estimated. Consequently, we realized smooth automated driving.

Fusion of NDT and road marker matching based on PF

I extended the above localization method and integrated a road marker matching method to 3D NDT. Additionally, I introduced an uncertainty determine method for the NDT estimate based on pre-experiments and it enables to assign appropriate uncertainty in each area as shown in the right. I fused the NDT estimate with the pre-determined uncertainty and the road marker matching result based on particle filter (PF). I showed the robustness and smooth trajectory estimation performance using the demonstration logs.