Heuristic Search in Belief Space for Motion Planning under Uncertainties David Lenz 1 and Markus Rickert 1 and Alois Knoll 2 Abstract— In order to fully exploit the capabilities of a robotic systems, it is necessary to consider the limitations and errors of actuators and sensors already during the motion planning phase. In this paper, a framework for path planning is introduced, that uses heuristic search to build up a search graph in belief space, an extension to the deterministic state space considering the uncertainty associated with this space. As sources of uncertainty actuator errors and map uncertainties are considered. We apply this framework to various scenarios for a non-holonomic vehicle and compare the resulting paths to heuristic state space planners and LQG-MP[1] with the help of simulations. As a result, paths generated with this framework could either not be found with worst-case assumptions or have a higher probability of being successfully executed compared to planners with more relaxed constraints. I. I NTRODUCTION Since the DARPA Grand Challenge in 2005, automated driving has gained more and more focus in academia and with car manufacturers all over the world. All of the research vehicles have in common, that they use very sophisticated sensors like for example the Velodyne Lidar sensor in order to create a very good environment representation around the vehicle. As autonomous driving functions like automated parking, traffic-jam assistants or highway driving are on the verge of becoming available for customers, this approach is not feasible anymore. To keep the costs low and feasible for a product, a cheap and limited sensor set is installed. The mostly noisy signals of the sensors are then usually combined within a probabilistic data fusion framework to get one consistent environment representation and state estimate of the car. The result is a best guess on what the environment might look like and can—depending on which sensors see which obstacle—lead to areas which are not well-known but have a large uncertainty. Furthermore, a car especially with a combustion engine is very hard to model and to control such that it behaves exactly the way it was planned beforehand. Fig. 1 shows an example of an obstacle that is known only with noise and also the possible error distribution due to actuators along the reference. On the other hand, there are motion planning algorithms that need to plan a feasible and safe motion that the vehicle can actually execute. In current systems this is mostly achieved by making conservative worst-case assumptions and adding these to the safety margins used in planning. This limits the true capabilities of a robotic system as most 1 David Lenz and Markus Rickert are with fortiss GmbH, affiliated institute of Technische Universit¨ at M¨ unchen, Munich, Germany 2 Alois Knoll is with Robotics and Embedded Systems, Technische Universit¨ at M¨ unchen, Munich, Germany Fig. 1. Reference path with underlying controller errors (green) and one concrete simulation run trajectory (red-dashed). The obstacle has only been observed briefly and is thus not known very well. of the time a better performance than worst case can be achieved. Thus, in this paper, a probabilistic model of the sensing and control capabilities of a car is used to leverage all of its capabilities. The proposed framework is not limited to autonomous cars but can be applied to arbitrary robotic systems that can be (locally) linearized. II. RELATED WORK In the past, mostly motion planning under deterministic conditions was considered, but in recent years uncertainties and probabilistic modeling of these have gained a lot of interest. There are four main sources for uncertainties in the planning and in the execution steps, namely: • Uncertainty in the map due to sensor noise • Process noise that prevents a perfect execution of mo- tion • Localization uncertainty due to partial observability • Unknown future motion of dynamic obstacles Other sources like modeling errors also exist but can be accounted for in one of these error sources. On the one hand, there are planners that incorporate map uncertainty but treat the robot model as deterministic. For example, Burns and Brock [2] use a utility-guided roadmap planner that minimizes the uncertainty that is encountered along a path and allows utility-guided exploration. Guibas et al. [3] on the other hand consider uncertain corners in a polygonal map and use the success probability in a cost function for a roadmap planner. For motion execution errors, [4] shows a variant of RRT, that uses particles for each tree expansion to represent and calculate the underlying motion uncertainty. Similarly, Alterovitz et al. [5] build a probabilistic roadmap where the edges are associated with a success probability. This is determined by multiple simulations of the execution of the edge with Markov motion uncertainty. Mellinger and Kumar