Two-way D* algorithm for path planning and replanning

Inspired by the Witkowski’s algorithm, we introduce a novel path planning and replanning algorithm – the two-way D* (TWD*) algorithm – based on a two-dimensional occupancy grid map of the environment. Unlike the Witkowski’s algorithm, which finds optimal paths only in binary occupancy grid maps, the TWD* algorithm can find optimal paths in weighted occupancy grid maps. The optimal path found by the TWD* algorithm is the shortest possible path for a given occupancy grid map of the environment. This path is more natural than the path found by the standard D* algorithm as it consists of straight line segments with continuous headings. The TWD* algorithm is tested and compared to the D* and Witkowski’s algorithms by extensive simulations and experimentally on a Pioneer 3DX mobile robot equipped with a laser range finder.