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.