Multi-Sensor Perception and Dynamic Motion Planning in City Environments

In this paper we describe a state lattice based motion planning approach, which we have successfully applied to large, cluttered, but quasi-static environments. Our approach produces smooth and complex maneuvers through the use of a multi-resolution state lattice, where the resolution is adapted based on the environment, and distance from the robot. We also describe a framework for detecting dynamic obsta- cles such as pedestrians and cars using a multisensor laser- camera detection and tracking method. Image detection is based on several extensions to the Implicit Shape Model technique; laser detection is instead achieved through the use of a Conditional Random Fields reasoning. Objects are tracked through the use of multiple motion model Kalman filters in order to cope with several different motion dynamics. Urban environments, are complex, cluttered, and dynamic scenes, however. We therefore propose to extend our dynamic obstacle detection and tracking method with a short-term motion prediction functionality based on the same models used for tracking, effectively generating time based cost or risk maps. We further propose to implement these cost maps into our high-dimensional (5D to 6D) lattice planner to generate time-optimal trajectories in dynamic, cluttered environments. A D* implementation is envisioned to speed up re-planning dramatically.