Latombe [14], and the resulting navigation function is called NF 2The Voronoi method also tries to follow a maximum clearance map [15]. To obtain a safe path, it is necessary to add a component that repels the robot from obstacles, as proposed by Khatib [16]. In addition, this repulsive potential and its associated vector field should have good properties such as those of electrical fields. If we consider that the robot has an electrical charge of the same sign as obstacles, then the robot would be pushed away from obstacles. The properties of this electric field are very good because it is smooth and there are no singular points in the space of interest (Cfree). The main problem of attractive and repulsive potentials is how to mix the two fields together. This union between an attractive
and a repulsive field has been the biggest problem of potential fields in path planning since the works by Khatib [16]. The problem is that the sum, difference, or similar operations provoke the appearance of local minima.
In the proposed algorithm, this problem has been solved in the same way as Nature does: the electromagnetic waves, such as the light, have a propagation velocity that depends on the medium due to the refraction index. For example, a flint glass has a refraction index of 1.6, while in air it is approximately 1. This refraction index of the medium is the quotient between the light velocity in vacuum and its velocity in the medium, i.e., the slowness index of the wave front propagation in a medium. This idea of mixing the two potentials as Nature does is the main contribution of the VFM and FM2 methods [11–13].
According to Fermat’s principle, the path taken by light to go om one point to another is the path that minimizes the time. In he case of a homogeneous medium, in which the light velocity is
onstant, the light follows a straight line. The set of points achieved a fixed time is a circle. Consider two media with different fractive indexes, e.g., air and water. If a ray of light passes from
he first medium to the second one, the phenomenon known as fraction occurs. The beam appears to bend when the medium hanges. The light seems to prefer to stay in the medium with eater light velocity, as shown in Fig. 1(a). If there is a continuous hange in the refraction index of the medium, the path obtained is curve that goes away from area swith lower light velocity (higher fractive index), as shown in Fig. 1(b) and Fig. 2. This is the case the well-known hot road surface mirage, in which there appearsbe ‘‘fake water’’ on the road which is produced by the light rays ending due to a change of the refraction index in air, with higher mperature (and lower refraction index) near the road surface.
Fig. 1. (a) Propagation of a wave and the correspondingminimumtime path when there are two media of different slowness (refraction) index; (b) the same with a vertical gradient of refraction index.
Fig. 2. Hot road surface mirage: light rays bending due to change of the refraction index in air with higher temperature (and lower refraction index) near the road surface.
Fig. 3. (a) Repulsive potential. (b) Its associated vector field. This potential can be considered as themetrics, the refraction index, or a viscosity index. (c) Attractive potential.(d) Its associated vector field and typical trajectory obtainedwith the attractive potential alone. (e)Union of the two potentials, the second one having the first one as refractive index. (f) Associated vector field and typical trajectory obtained with this metho
In the proposed methods, the repulsive potential is used as the refraction index of the medium where the wave emitted from the goal point propagates, as shown in Fig. 3(a) and (b). Fig. 3(c) and (d) Show the behavior of the attractive potential used alone.When the attractive potential uses the repulsive potential as the refraction index, a unique field is obtained and its associated vector field is attracted to the goal point and repulsed from the obstacles, as shown in Fig. 3(e) and (f). This method inherits the properties of the electromagnetic field, i.e., it is C∞ if the refraction index is C∞.Intuitively, the FM method gives the propagation of a wave front in non-homogeneous media.
This repulsive potential can be obtained using the Extended Voronoi Transform (EVT) of the binary image of the map. The EVT computes the Euclidean distance of the binary image. In a binary image, a pixel is referred to as background if corresponds to complete absence of obstacles and its value is one. The value of a pixel is zero if it corresponds to an obstacle or a wall. For a given distance metric, the EVT of an image produces a distance map of the same size. For each pixel in the image, the EVT assigns a number which is the distance to the nearest zero pixel of the image. For each pixel inside the objects in the binary image, the corresponding pixel in the distance map has a value equal to the minimum distance to the background. This is the solution adopted in the VFM method.
Clearly, the EVT is closely related to the Voronoi diagram. The Voronoi diagram concept is involved in many EVT approaches, either explicitly or implicitly. Another possibility is to build the repulsive potential using FM. This is the solution adopted in the FM2 method. In this case, a wave is propagated starting from the points representing the obstacles and walls. This wave propagation is achieved through the FM method. The result is a potential map,which can be interpreted as a velocity map (or slowness map) because it gives a clear idea of the robot’s permissible velocity at each environmental cell. This
potential map is represented in grey scale, where the walls and obstacles are black and the cells become lighter as long as the distance to these obstacles increases. 2.3. Implementation of a smooth slowness potential This implementation starts with the calculation of the loga- rithmof the inverse of the EVT of the 2D environment updated map (a priori + sensor data) (or the inverse of the EVT in the case of 3D maps). Each white point of the initial image (which represents free cells in the map) is associated to a level of grey that is the logarithm of the inverse of the 2D distance to the nearest obstacles. As a result of this process, a potential is obtained, proportional to the distance to the nearest obstacles, for each cell.
This function introduces a potential similar to a repulsive electric one (in 2D), that can be expressed
as φ = c1 log(1/r) + c2, where r represents the distance to the charge and c1, c2 are constants. If n > 2, the potential is φ = c3rn?2 + c4, where c3, c4 are constants. These expressions of the potential φ correspond to the electric potentials in 2D and higher dimensional cases. 2.4. Fundamentals of the method
Maxwell’s laws govern the propagation of electromagnetic waves.We can use the most typical simplification of the problem in isotropic and possibly non-homogeneous media for a monochromatic wave, considering the so-called almost flat waves. In this case, the optical wave propagates at wavelengths much smaller than the image objects, and therefore the ray optics approximates wave optics. These equations along with the mentioned approach allow us to develop the theories based on rays, such as the geometrical optics, the theory of sound waves, etc. The eikonal equation is derived from these equations. In the Sethian [17] notation.
where D(x) represents the distance to the initial set, R(x) is the refraction index of the medium, and x = (x, y) in 2D or x =(x, y, z) in 3D. In geometrical optics, Fermat’s least time principle for light propagation in a medium with space varying refractive index R(x) is equivalent to the eikonal equation. The eikonal solution D(x) is a scalar function whose isolevel contours are normal to the light rays, and the refractive index of a medium is the quotient of the light velocity in the vacuum and its velocity in the given medium. This equation is also known as the Fundamental Equation of geometrical optics. The most important aspect in this equation, from the path planning point of view, is that the solution is an exponential function of the refractive index
is the angular frequency, which is constant because
the considered wave is monochro matic, T is the period, R(x) = c v(x) is the refraction index, c is the light velocity in vacuum, λ is the wavelength, v(x) is the light velocity in the anisotropic medium, and v = λT .In this expression, ω and c are constants and the refraction index is a function of the position, i.e., the considered medium is anisotropic.
Since this solution is exponential, if the refraction index or first potential R(x) is C∞ then the second potential D(x) is also C∞, and therefore the trajectories calculated by the gradient method over this potential would be of the same class. 3. Robot formations and Fast Marching
Robot formation motion planning deals with the problem of how to find the paths and postures between robots so that the whole formation can adapt to obstacles and other environmental characteristics.
In this work, we have considered two types of formations: (1) leader-following, where two robots (followers) follow the leader as if they were bodyguards, forming a triangle among the three of them; (2) le formation (leader). The leader will not be represented in the leader-protecting figures for the sake of clarity and simplicity.ader-protecting, a formation of six robots in hexagonal configuration protecting the centre of the
As stated before, the FM method is based on a potential function without local minima that provides smooth trajectories. The main problem to deal with is how to apply this method to the robot formation’s motion, keeping its good properties. To solve this problem a repulsive force is needed between the formation robots so that they keep a security distance and do not end up crashing into each other. Furthermore, an internal attractive force is also required for maintaining the formation while adapting to the environment. The advantage of using the VFM method, as proposed here, is that each robot, at each time, has one single potential which is attractive to the objective and repulsive from walls, obstacles, and the other robots of the formation, while keeping the desired nominal inter-robot distances.
Our first approach to implement the repulsive force was to add Gaussian functions to the distance potential Dt i of each robot, centred with the other formation robots, so as to keep inter-robot desired distances balanced with the need to avoid obstacles and reach the goal. This approach is fast and works for a wide range of cases, though the behaviour is not natural in some special situations such as the ones presented in Figs. 8 and 9, for the corridor at the bottom of the images.
Taking this into account, the best solution for a robot to keep its distance from the others is to set them as obstacles in the environment map, expressed in a matrix Woti. The computational cost of this method is higher, since similar matrices Woti, WtiandDti (see next section) have to be computed at
each step. However, this approach preserves the good properties of FM and avoids the appearance of local minima.
The solution adopted for the implementation of the repulsive force is detailed next and summarized in Fig. 4.
Fig. 4. Flowchart of the proposed algorithm.
3.1. Description of the algorithmof robot formationmotion using Fast Marching
The objective of this algorithm is to integrate in a potential without local minima a force that attracts the robots to the goal,a force that repels the robots from the obstacles and a force that maintains the formation.