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. ► The two-way D ∗ algorithm finds optimal paths in weighted graphs. ► TWD ∗ produces shorter and more natural low-cost paths through the grid. ► The TWD ∗ path consists of straight line segments with continuous headings. ► The TWD ∗ path is the shortest possible path in geometrical space. ► TWD ∗ performs well in changing environments.