The path planning and dynamic window integration ensure
efficient and collision-free motion of a mobile robot in
environments with unknown static obstacles. But it can not ensure
collision-free motion in environments populated with moving
obstacles. Many different approaches to mobile robot motion
control in the presence of moving obstacles have been
investigated. While the majority of
approaches put the assumption on the shape of moving obstacles,
our approach does not actually consider moving obstacles, but
moving cells (MCs) of the grid map. Therefore, the robot is trying
to avoid newly occupied cells in its vicinity, which are modelled
as empty in the stored occupancy grid map of the environment. The
motion of an obstacle can be regarded as the motion of these newly
occupied cells. Our algorithm requires velocity vector
(v_{mc},)
and motion heading
for each
moving cell. The estimation of these three quantities is not
considered in this paper, but it is supposed that they are
known.

When a moving obstacle appears in the vicinity of the robot, a set
of predicted obstacle trajectories is generated, each trajectory
starting from a moving cell. Each MC trajectory is a circular arc
defined by velocity vector
(v_{mc},)
which is assumed
to be constant for the next n
intervals. MC trajectories are
recalculated in every sampling instant and are represented by a
discrete set of points. The number of points used to represent the
MC trajectories is equal to the number of points of DW
trajectories calculated for the robot (N_{t}
). The length of a MC
trajectory L_{mc}
is set equal to
L_{mc} = v_{mc}T_{max}
,
where the time constant T_{max}
is the same as for DW
trajectory. By choosing the same number of points N_{t}
and look
ahead time T_{max}
for both DW and MC trajectories it is
achieved that the i
-th point on the MC trajectory has the same
assigned time instant t_{i}
as the i
-th point on the DW
trajectory. This approach greatly simplifies calculation of the
time until collision t_{col}
, since it is only necessary to check
collisions of points on MC and DW trajectories with the same
index i
, as shown in Fig. 10. The couple of colliding points with the smallest index i
determines the time until
collision t_{col}
, which is then used for calculation of the
clearance objective measure
(v,)
according to (6).

Figure 10:
Collision of MC and DW trajectories

If an obstacle is static then MC trajectories have the zero length
and collision calculation turns to be the same as for original integration control method. If the collision is detected
between the i
-th point on the DW trajectory and the i
-th point
on the MC trajectory, then original location of the MC is marked
as unoccupied and cell corresponding to the collision point as
occupied in the map used by FD* algorithm for the global path
recalculation. Therefore, FD* recalculates the path around the
expected collision location, and path objective measure
(v,)
changes the optimal DW trajectory.

The map is continuously changing around moving obstacles, and
consequently a number of FD* pointers changes directions in each
sampling instant. A situation can occur that FD* generates two
mutely distant paths with very similar costs, and that pointer
changes cause continuous switching between these two paths. Such a
situation is illustrated in Fig. 11,
where the controlled robot (right robot in the subfigures) and the
other robot (left robot in the subfigures), which presents the moving
obstacle, are moving straight ahead towards each other. The FD*
algorithm generates pointers on the robot-obstacle connecting line
that alternately determine pair of symmetrical paths around the
obstacle. The robot would alternately try to follow two different
distant paths, which would lead to collision with moving obstacle
after a short time.

Figure 11:
Path switching

In order to avoid the above described problem we propose the
generation of the paths not only from the robot current position
cell, but also from the surrounding cells. Total costs of the
neighbor paths are simply computed by following the pointers from
the neighbor cells to the goal. The paths that are close and
geometrically similar to the path from previous sampling instant
are preferable, since the robot will not oscillate between distant
paths on its traverse. If the path with minimal cost among
neighbor paths is distinct from the previous path (e.g. path1
with cost c_{1}
in Fig. 12) it will be chosen only if
its cost is lower then the cost of the path with minimal cost
among those that are close and geometrically similar to the
previous path (e.g. path2
with cost c_{2}
in Fig. 12)
decreased by the cost of the transition between starting cells of
these two paths (e.g. transition between cells sc_{1}
and sc_{2}
in Fig. 12). Otherwise, the robot will choose the path
with minimal cost among those that are close and geometrically
similar to the previous path (path2
in Fig. 12).

Figure 12:
Path selection

The size of the neighborhood around the robot current position that should be
considered for choosing a suitable path is determined by the
property of DW and FD* integration method explained in the following. Let's assume the worst case where the robot is traversing in a free space area. There, the global path is composed of long straight line segments, and there is one path direction change in the robot vicinity and the reference point on the
path is at maximal distance
R_{max} = s_{l}
from the robot, encouraging maximal translation
velocity v_{max}
.
Robot traverses along sequence of circular and straight line arcs and thus moves away for some distance d
from the point of the path direction change.
The robot moves away from the global path when the reference point is chosen on the next path segment, i.e. when the robot is within the distance R_{max}
to the point of path direction change (see Fig. 13).

Figure 13:
Maximal robot departure from the path

Maximal departure d
is
given by:

d = R_{max} = r,

(12)

where
= 3^{ . }45/2
(since path directions are always
multiples of 45^{o}
) and
r = R_{max}tan
is radius of the circular arc.
Rotational velocity at which robot traverses along the circular arc is
=
.
In reality,
robot gradually increases its rotational velocity due to selection
of reference point further from the point of the path direction change and thus gradually
decreases the radius of the circle arc along which the robot traverses. The smaller is the radius, the smaller is the departure from the global path. Therefore,
the distance d
given with (12) can be declared as maximal
possible robot departure from the global path. Thus, the
surrounding cells that are close to the robot within the distance d
are used as starting cells
for neighbor paths generation.

Obstacle cells are C-space enlarged to account for robot
dimensions (robot mask is two cells in our case). That cells
get prohibitively large safety cost value. But, due to possible
robot departure from the global path it can happen that robot
comes too close to an obstacle and stops in front of it or even
hits it. In order to avoid such situations we propose generation
of so called safety cost masks around the C-space enlarged
obstacles. Their purpose is to push FD* algorithm to compute
global paths as far as possible from the obstacles. The size of
the cost mask is also determined by the distance d
given with
(12) as it is the maximal departure of the robot from the path. Each cell in FD* map within the safety cost mask
around the occupied cell gets corresponding safety cost value,
which depends on its distance from the occupied cell. The outmost
cells get safety cost value for one grater then empty cells out of
safety cost mask, and safety cost value of inner cells
incrementally increase from outmost cells to the occupied cell.
Assigning safety cost values to the cells inside the safety cost
mask is illustrated in Fig. 14, where it can be seen
that the width of the cost mask is four cells in our case.

Figure 14:
Safety cost mask

When the safety cost values of the cells are included in the FD*
map, the FD* algorithm will compute the global path with the
cheapest total cost, which will be out of the safety cost masks around
the obstacles if possible, or through the middle of narrower passages if not. The consequence can be that the computed cheapest
path is not also the shortest one, as can be seen in Fig.
14. But, this price must be paid in order to achieve
collision-free robot motion.