Two-way D^∗ algorithm for path planning and replanning

Abstract

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.

Introduction

An autonomous mobile robot is expected to perform goal-directed tasks in cramped and unknown environments. The task of the path planning algorithm is to compute an optimal path to the given goal and to replan the path in case the previously planned path is blocked by obstacles.

The majority of path planning algorithms produce a graph of possible paths to the goal [1] and then apply a classical graph search algorithm [2], such as the A^∗ [3], $D^{\ast }$[4] or focused $D^{\ast }$ (FD^∗) [5] algorithms, to find the optimal path. The $D^{\ast }$ and FD^∗ algorithms are very often used because of their capabilities of fast replanning in changing environments, i.e. in environments where new obstacles can be added or removed. Two-dimensional (2D) occupancy grid maps are usually used to represent a continuous environment by an equally-spaced grid of discrete points [6]. Grid cells cover the area densely and each grid cell contains information about its occupancy.

The path obtained by a classical graph search algorithm based on a uniform resolution grid is a zigzag line with sharp turns, the angles of which are limited to increments of 45° [4], [5], [7]. A mobile robot cannot follow such a path smoothly due to its kinematic and dynamic constraints. To overcome this limitation a number of algorithms have been developed which produce paths not constrained to a small set of headings [8], [9], [10], [11], [12], [13]. For example, the Field $D^{\ast }$ algorithm [8] extends the standard $D^{\ast }$ algorithm by using linear interpolation to produce straight line segments with continuous headings. Although Field $D^{\ast }$ produces less costly paths than the $D^{\ast }$ algorithm, the path optimality is not guaranteed. A different approach to produce straight line segments with continuous headings is proposed by Gennery in [9]. His algorithm is based on the Witkowski’s algorithm [14], which uses two-way (forward and backward) passes of the breadth-first search (BFS) to find the optimal paths. BFS can find optimal paths only in graphs that have all weights equal [2]. Gennery adapted the Witkowski’s algorithm for finding the paths in graphs with arbitrary weights, but his adaptation does not guarantee the path’s optimality. Besides, the Witkowski’s and Gennery algorithms perform poorly in changing environments as the path must be completely replanned each time the environment changes, which significantly enlarges the computational time of the search algorithm.

This paper presents a new path planning algorithm, called two-way $D^{\ast }$ (TWD^∗) algorithm, which is actually the Witkowski’s algorithm with the two-way breadth-first search replaced by the two-way $D^{\ast }$ search. By applying the two-way $D^{\ast }$ search of the graph, an area of minimal path costs from the start node to the goal node in graphs with arbitrary weights is found and an algorithm for optimal path calculation through that area is developed. The calculated path is the shortest possible path in the geometrical space composed of long straight line segments with continuous headings. The proposed algorithm also performs well in changing environments although it is computationally more demanding than the $D^{\ast }$ algorithm.

The rest of the paper is organized as follows. Section 2 briefly reviews the $D^{\ast }$ and Witkowski’s algorithms. Section 3 presents the new path planning algorithm. In Section 4 test results are given and compared to those obtained by the Witkowski’s and $D^{\ast }$ algorithms under the same conditions. Finally, Section 5 gives the conclusions of the paper.