Motion planning under perception and control uncertainties with Space Exploration Guided Heuristic Search

Reliability and safety are extremely important for autonomous driving in real traffic scenarios. However, due to imperfect control and sensing, the actual state of the vehicle cannot be flawlessly predicted or measured, but estimated with uncertainty. Therefor, it is important to consider the execution risk advance in motion planning for a solution with a high success rate. The Space Exploration Guided Heuristic Search (SEHS) method is extended to deal with perception and control uncertainty in its two planning stages. First, the localization uncertainty is evaluated with a simple probabilistic robot model by the Space Exploration to find a path corridor with sufficient localization quality for the desired motion accuracy. Then, a trajectory controller is modeled with nonholonomic kinematics for the belief propagation of a robot state with primitive motions. The dynamic model and the control feedback are approximated in a close neighborhood of the reference trajectory. In this case, the Heuristic Search can propagate the state uncertainty as a normal distribution in the search tree to guarantee a high probability of safety and to achieve the required final accuracy. The belief-based SEHS is evaluated in several simulated scenarios. Compared to the basic SEHS method that assumes perfection, motions with higher execution successful rate are produced, especially the human-like behaviors for driving through narrow passages and precise parking. This confirms the major contribution of this work in exploiting the uncertainties for motion planning in autonomous driving.