Two-way D? algorithm for path planning and replanning
Inspired by the Witkowskis 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 Witkowskis 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 Witkowskis algorithms by extensive simulations and experimentally on a Pioneer 3DX mobile robot equipped with a laser range finder.
path planning mobile robotics graph search euclidean shortest path