Fast Marching-Based Path Generating Algorithm in Anisotropic Environment With Perturbations

Fast Marching is a widely used method in path planning, especially continuity is demanded due to kinodynamic constraints of automatic vehicles. However, its application in real environment with obstacles and perturbations requires simplifications to be made, resulting in non-optimal or even unfeasible path. We introduce a risk function $\zeta $ to describe the effect of obstacles to the safety of a generated path. Combined with the physical law based description of external flow perturbation, we formulate the path planning problem in realistic environment as a wave front propagation in terms of an extended Eikonal equation. Inspired by the underlying mechanism of the numerical recipe of Fast Marching algorithm, we transform the equation into its canonical form based on the physical rules behind. This enables solutions of the equation without any simplification nor introduction of additional complexity. Comparing to the classical fast marching based method in anisotropic environment, numerical simulations show that the proposed method is more efficient and robust to external flow perturbations, especially in the case of weak or strong external flow environment.


I. INTRODUCTION
With the recent development of technologies, applications of unmanned robotics are increasing. For example, autonomous underwater vehicles are widely used in oil and gas exploration, ocean floor mapping, search and rescue, etc. To accomplish these tasks, path planning plays an important role [1]. It figures out a feasible path that can be followed by the autonomous vehicle from one position to another without the assistance of human operation. However, due to the limited power supply and the requirement of long endurance as well as the uncertainties of its surrounding environment, path planning is not an easy task [2], which aims at finding a best path in terms of minimum cost and collision free. Although it has been studied for decades, it is still one of the hot research topics in mobile robotics [3]- [5].
In the past, the path planning problem was typically associated with the shortest length. As a result, methods in graph theory were widely used [6]- [8]. These methods share The associate editor coordinating the review of this manuscript and approving it for publication was Hao Shen . common contributes: the terrain where robotic vehicles plan to traverse is explored by a set of randomly sampled nodes and the cost for traveling between them described as a function of the distance between nodes [9]. As lack of global information of the terrain, path feasibility can not be guaranteed. Further more, the generated paths are typically noncontinuous and might not be feasible for robotics with kinematic and dynamic constraints [10]. In contrast, deterministic path planning approaches divide the whole terrain with grids. Each grid is associated with a cost which describes the distance/risks of traversing it. After computing cost over the entire terrain, applying gradient descent on the cost map to trace back from the goal point leads to a good approximation of the optimal path to the start point.
The artificial potential field path planning method is of this type [11]. Its main idea is to introduce an artificial field that attracts the robot to the goal point and repels it from obstacles [12]. However, it suffers from local minimum and does not take into account the influences from environment (like flows and wind) [13]. The fast marching algorithm introduced by Sethian is another alternative deterministic path planning method [14]. Initially, this method uses a symbolic wavefront expansion to calculate the shortest time path and also determines the departure time of the robot from a starting point. There after, modifications were introduced to cope with influences from environments: in 2007 Pêtrès and co-authors introduced an effective cost function to describe the effect of external flows and gave a feasible path that takes advantages of perturbation flows in anisotropic environment [15]. Although this method shares same order of complexity of the original fast marching method, it generates infeasible path under strong environmental perturbations.
This paper targets the difficulties in applying fast marching to path planning problems in anisotropic environment with strong perturbations. The main contributions of the paper are: 1) we detail a mathematical description of a path planning problem in complex environment with both obstacles and external field perturbations based on the general fast marching principle, and derive an easy-to-implement numerical receipt to find optimal path; 2) we explore parameters that govern the geometrical properties of the generated path, which can be incorporated in the path planning process to over-come kinematic constraints of robots and other unmanned automatic vehicles. To serve the purpose of better description of the proposed method, the rest of paper is organized as following: in Section II, we give a definition of path planning problem which we are going to tackle. It follows an introduction of the general fast marching method in Section III. Section IV presents the way how we cope with obstacles and external field perturbation according to the first-arrival principle. We then give numerical experiments to show the effectiveness of the propose method in Section V and conclude the paper in Section VI.

II. PROBLEM STATEMENTS
Given a configuration space , the path planning problem is to find a curve (defined in Eq. (1)) that minimizes a set of internal and external constraints (time, fuel consumption or danger etc).
where s is a parameterization variable describing the path in the free space. Introducing the cost function τ to represent all the constraints in the configuration space, the index of feasibility ρ of a path can be described as: where C x 0 ,x is the path in the configuration space that connects the starting point x 0 and the target point x . τ is a nonnegative function, representing the difficulties of reaching x . For example, an obstacle in the configuration space is a region with τ ∼ ∞; an automatic vehicle moving upstream to x will have a larger τ comparing to that moving downstream. With these definitions, the path planning can be further formulated into a minimal value problem as: where u(x 0 , x) stands for the minimal cost along a path C starting from point x 0 to the destination x in configuration space . As τ is a non-negative function, there will be only one local minimum which is the starting point x 0 . However, due to the fact that τ is position dependent, solving Eq. (3) is not an easy task.

III. FAST MARCHING PRINCIPLE
It has been proven that the minimization problem defined in Eq. (3) is equivalent to solution of the Eikonal equation [15]: where F(x, y, n) is the propagating velocity of the level sets of u, and n = ∇u ∇u the outwards unit normal vector. For the purpose of easy understanding while without losing generality, we view the cost function u as the first arrival time of an automatic vehicle or robot (other merits can be constructed based on robot's dynamical and kinetic properties, see below for explanation). The solution of Eq.(4) gives the smallest time required to reach any point x from the starting point x 0 . The optimal path can then be found by a simple backtracking on the cost map starting from the goal point.
Exploiting the entire configuration space to build a cost map is not an easy task. One way is through the Huygens principle construction. The solution to Eq. (4) is developed by imagining wavefront propagating from the boundary with a speed F. Since each point on the wavefront can be seen as a wave source, which initiates outward information propagation, the point used to update a nearby neighbor is determined according to the notion of an entropy condition, i.e., a point is updated, it keeps unchanged no matter what is the information coming later [14].
Based on these observations, Sethian proposed to use the upwind approximation for the gradient [14]. At each point in the configuration space (i, j), the unknown u satisfies: where D −· and D +· are standard finite difference notation: where h is the grid size. Considering the fact that the first arrival time u can only grow, an order in selecting the grid points to update is introduced. Algorithm 1 details the implementation of upwind scheme. This Fast-Marching algorithm is an efficient way to approximate the viscosity solution of the Eikonal equation for every position [14], without introducing additional complexity [16]. To proceed, three different queues VOLUME 8, 2020

Algorithm 1 Fast Marching Algorithm 1) Definition
• Dead D is the set of all grid points at which the value of u has been determined and will not change.
• Open O is the closest neighbors of the dead sets, whose u have be estimated using Eq. (5) • Far F is the set of all grid points, whose u are not estimated 2) Initialization

IV. ANISOTROPIC FAST-MARCH ALGORITHM
Different from the isotropic environment, where F i,j are constants, in real applications, an autonomous vehicle might be influenced by external flows. Further more, to guarantee safety, the vehicle should keep a proper distance to obstacles. Incorporating these facts in Eq. (5) is a pre-condition of applying Fast Marching method in real applications. The first-arrival time analogy allows us to extend Eq. (4) into the case of anisotropic environment, where there are obstacles and complex flows v c (x).

A. FORMULATION OF INFLUENCE FROM COMPLEX ENVIRONMENT
Considering the fact that obstacles are un-penetrated, and the closer to them, the higher probability a vehicle collides, we introduce the risk map ζ (x) to the right hand size of Eq. (5), which can be seen as the required time for advancing a unit length and takes the form as: Here d i is the distance of a point x to the closest boundary of obstacle i, and constant d 0 specifies the safe distance a vehicle should keep to avoid collision. λ is a velocity dependent parameter, specifying how sensitive the safety of an automatic vehicle to obstacles|. Typically, high velocity toward obstacles implies large λ in order to avoiding getting too close to obstacles. Fig.1 demonstrates the effect of obstacles to its surrounding environment. Clearly, getting closer to the obstacles, it takes more time for the vehicle to advance, e.g., nodes close to obstacles typically have high values of ζ (x) and u. As a result, the generated path naturally incorporates the criteria of least risk, as the gradient descent method always tries to put the path away from regions of high u. In addition, λ also acts as a parameter specifying the importance of safety in the process of path planning. Large λ emphasizes on the safety, while λ < 1 refers the least cost orientated planning strategy.
Recalling the entropy requirement, an open point x is updated by a point on the wavefront that gives maximum normal velocity F(x, n), with n representing the normal vector of the propagating front. As shown in Fig. 2, according to Huygens principle, any points (O, O , etc) on the wavefront C (A, B) can be the potential point source that updates x.
Taking O for example, due to the influence of the external flow current v c , the vehicle has to move with speed v v in order to give a resultant velocity f (x, n) toward X, which is required to update u(x). However, the entropy requirement fixes the source to be O, at which point maximum F(x, n) is expected. As a consequence, the difference with respect to the isotropic case is to find F(x, n) as [16]: Substituting Eq. (7) and (8) into Eq. (4) gives us the equation governing the update of u in complex environment as: B. NUMERICAL SCHEMES Fast marching algorithm was firstly proposed by Sethian to propagate a wave front to an isotropical configuration space. Petres and co-authors [15] extended this method to anisotropic case. They introduced a scaling parameter to approximate the influence of external current flow as an additional cost. However, this simplification might result in a non-optimal path or even non-feasible path, depending on the strength of external disturbance (See Fig. 5).
Noticing the difference between Eq. (9) and (4), the real challenge to solve Eq. (9) is to extract the effective nominal velocity Eq. (8). To simplify the derivation, we view an automatic vehicle as a point on the wavefront. It moves forward in any direction with speed |v v |. As a consequence, the maximum nominal velocity can be easily determined as: To numerical solve Eq. (9), we partition the configuration space into Cartesian grid and adopt the 4-connexity discrete model. Notice that only dead points are used to estimate u at point x. To update point a grid point (i, j), we denote its two dead neighbors in opposite axis as D x and D y , with their values as u x and u y respectively. This allows us to define the normal direction of the wavefront n as: where ξ x is introduced to denote the relative position of its dead neighbors with respective (i, j). ξ x = 1 when (i, j) is on the left side of its dead neighbor u x , while ξ x = −1 when (i, j) is on the right side of u x and ξ x = 0 when there is no dead neighbors along x-axis. The same goes to ξ y . Substituting it into Eq. (10) gives where v x c and v y c are the components of v c along x− and y− axis respectively. Discretizing Eq. (9) gives us the quadrature equation that updates the point (i, j) as where h is the grid size. After some simple algebra calculation, we end up with a quadratic equation: with Solving Eq. (14, 15) allows us to build an entire cost map in the configuration space. Although these equations are derived from the case of two dead point, it is applicable to the 1-dead neighbor case, simply by setting ξ x = 0 or ξ y = 0.

C. IMPLEMENTATION DETAILS
The key idea to Fast Marching Methods lies on the observation that the previous iteration contains a very specific order.
In the first arrival time problem presented here, a far point should be updated to the possible smallest u from its dead neighbors. As a far point might be surrounded by multiple dead neighbors, to guarantee the entropy condition, a set of trial estimations of the far point using different combination of dead points should be done [15]: • One Dead neighbor Under this case, the update of the far point u(x) is directly given by Eq. (14,15) with ξ y = 0 when the dead neighbor is along x−axis or ξ x = 0 when the dead neighbor is along y-axis (see Fig.3(a) for details).
• Two Dead neighbors If the Dead neighbors are on opposite sides of the far point ( Fig.3 (b) case (1)), estimate the far point u twice with each of the Dead neighbors separately. The smaller is then selected as the value of u at this point. If the Dead neighbors are on different directions, three estimations have to be done: estimate the open point u twice each time using one of these two Dead neighbors; do the estimation using both Dead neighbors. After finishing these estimations, the smallest is chosen (Fig.3 (b) case (2)).
• Three or Four Dead neighbors The method for updating a far point surrounded by three or four Dead is same as for the two Dead points, i.e., estimate u with each Dead neighbors separately, then do the estimation with each couple of two consecutive Dead neighbors. After these trial estimations, choose the smallest.

D. COMPLEXITY ANALYSIS
The proposed method for solving path planning problem in anisotropic environment is derived from the classical Fast Marching method, which is of complexity order of N · log N , with N representing the number of grid points we discretize the configuration space. Comparing to the original fast VOLUME 8, 2020 marching, the main differences are 1) calculating the risk map of each grid and 2) deriving the maximum nominal velocity F(x, n) at each point. These two additional operations are all of complexity order of N . Thus proposed method still keeps the same complexity order of N · log N as the original fast marching method.

V. SIMULATION RESULTS
Based on the analysis given above, the proposed method copes with the collision risks with obstacles and external flow perturbation in a union form. We would expect that the method generates path that take good advantage of external force while maintains low risk of collision with obstacles. This section provides detailed simulation results to verify these merits.

A. EFFECTIVENESS OF PATH GENERATION IN COMPLEX ENVIRONMENT
To verify the effectiveness of the proposed method, we carried out a set of numerical experiments, in which the configuration space was divided into a 200 × 200 lattice; An automatic vehicle initially locating at S (20,20) plans to move to a target point T (117, 156). Two circles with radius 10 centered at (50,120) and (120, 50) were added as obstacles in the configuration space (See Fig.4). Apparently, without the influence of external flow perturbations, one can imagine the optimal path should pass through the region between these two obstacles; if the consideration of least risk of collision is taken into account, the optimal path should not go through any region between these two obstacles. As shown in the figure, the optimal paths generated by applying the proposed method (solid black curves in panel (a) and (b)) exactly match the intuitive analysis results: when a vehicle is not sensitive to risk (panel (a) with λ = 1), the optimal path goes between the obstacles and when λ = 3 (panel (b)), the shortest path is no longer the optimal one.
Beside the ability of ensuring safety, the proposed method is also robust in coping with external flow perturbations. As illustrated in panel (c), under the influence of external flow current shown with little arrows, the generated path can take full use of the propelling force to reach its destination in the case of low safety requirement (λ = 1), although it is not the shortest. When the benefit from taking use of external flow currents is big enough comparing to the risk of collision with obstacles, the generated path goes through the region between the obstacles (see the solid black curve in panel (d)), even though the vehicle is more demanding on safety (λ = 3).

B. ROBUSTNESS UNDER PERTURBATIONS
Comparing to the method proposed by Petres et al. [15], the main difference comes from an exact description of the influence of external perturbation in propagating the wavefront without introducing any simplification. This implies that the proposed method is applicable to all possible external interfering force, no matter how strong it is. To verify this, we focus on a path planning problem that an automatic vehicle initially locating at S(100, 120) plans to move to a target point T (90, 180). A rectangular obstacle with length l = 120 and width w = 40 locates in the center of the configuration space (see Fig. 5). The external current flows with strength v c are constraint in the region labeled by arrows, indicating their directions. To avoid collision with the obstacle, a safe distance d 0 = 20 was introduced in Eq. (7). With out losing generality, the vehicle velocity v v is normalized into 1. Fig. 5(a) shows the cost map and the resulted optimal path generated using the proposed method in the case of v c = 0.6. Clearly shown in the figure, the cost optimal path (solid curve) takes full use of the external flows by following the current. However, the shortest length path (dashed FIGURE 5. Countour plot of the cost function u in the configuration space and the generated path. An obstacle (the rectangle area) is treated as a region with risk factor ζ ∼ ∞, the regions with external flow perturbations are marked with allows, which also indicate the directions of the current flows. (a) Cost optimal path generated using the proposed method with |v c | = 0.6 is shown as the solid curve. The length optimal path generated by fast marching method without taking the external current into consideration is shown as dashed curved. (b) The cost optimal path generated using the proposed method (red solid curve) and that obtained using the method given in [15] (blue solid curve) under weak external perturbation flow |v c | = 0.2. curved in Fig. 5(a)) moves counter-flow and passes over the obstacle.
To qualitatively demonstrate the profits brought by the cost optimal path algorithm, we calculated the fuel consumption E along paths under the assumption that a ship consumes same fuel α to keep velocity |v v | as: where the integration takes along the generated path. Apparently, minimizing fuel consumption can be achieved by reducing path length or taking advantages of external current flow. In the configuration presented in Fig. 5(a), the shortest length path should be a curve above the obstacles (dashed curve in Fig. 5(a)) while the cost optimal path goes through the region below the obstacle (solid curve in Fig. 5(a)). As the cost optimal path generated using the proposed method takes full use of external flow currents (|v c | = 0.6), although it has longer length, following it to reach the destination requires about 40% less fuel as that required by following the shorted length path. However, when the profits from using external current flows can not compensate the cost of taking longer path (in the case of weak flow currents), the algorithm should take the shortest length as the criteria to plan a feasible path. Consistent with our intuitive requirement, the proposed method plans a path (red solid curve) that passes above the obstacle when the external flow is weak (|v c | = 0.2, see Fig. 5(b) for detailed information).
To further prove the merits of our method, we carried out a set of numerical path planning tasks using different Fast Marching methods in a configuration same as in Fig. 5. The classical fast marching method applicable in anisotropic environment is the one proposed by Petres et al. [15] in 2007. As this method is insensitive to the strength of external current flows, the generated path always tries to use the external current flow, no consideration to the real expense(see the FIGURE 6. Comparison between consumed energy E of by following paths generated using the proposed method (red solid curve) and the method in [15] under environmental configuration same as in Fig.5. Under strong current perturbations, both method give paths of same quality, while under weak current, the proposed method bring more benefits. Inner panel shows the extra energy required for taking the path generated from the method in [15] in comparing to our method.

FIGURE 7.
Comparison between the proposed method to that in [15] under environment with obstacles and strong maximum external flow current v c = 1.0. Flow currents are labeled by the arrows with their length indicating the strength relative to the max. The cost maps and resulted paths generated from the method in [15] (a) and our method (b).
dash-dot blue curve in for illustration Fig. 5(b)), where using the small external propelling force (|v c | = 0.2) doesn't bring any profit comparing to the shortest length path. This is clearly shown in Fig. 6, where the dash-dot blue curve shows the required fuel consumption E following paths generated using Petres's method [15] to reach the destination, and the red curve shows that required by following path given by our method under different v c . These two curves overlap in the case |v c | > 0.35, while big gaps are seen under weak v c . The reason is that method in [15] is insensitive to the strength of external flow currents during path planning, while the proposed method assesses the real benefits that it would bring by using external flows.
The robustness of the proposed method is not limited to the case of |v v | |v c |. As mentioned by many of other authors [17], the method in [15] might be incapable of generating feasible path under strong external flow perturbations, we further test the proposed method in these cases. For this purpose, we consider a configuration space given in Fig. 7: a flow of strength |v c | = 1 coming from right to left. Due to the existence of obstacles, the current splits into two parts: one moves directly toward the left, the other first moves upward and comes down after passing the obstacle with a diminished flow velocity |v c | = 0.5. An automatic vehicle plans to move from the starting point S to its destination T . Applying Pêtrès' method to the problem gives a path as shown the solid red curve in the panel (a). Although it uses the part of the current flow, to reach the destination it has to pass through an upstream region of |v c | = 1.0. Clearly this path is infeasible, as the upstream current prevents the vehicle to move. Different from [15], our method calculates the cost map in the configuration space strictly according to the underlying physical rules. As a result, cost values in the region with |v c | > |v v | can not be updated using downstream points. This is the reason why a non-continuous change of cost value seen in the bottom of Fig. 7(b). Backtracking on this cost map gives a path all in the region with |v c | < |v v |, where the vehicle can always overcome these obstruent force and reach its destination.

VI. CONCLUSION AND FUTURE WORKS
Fast marching is a widely used method in path planning which discretizes the isotropic configure space and builds a cost map upon it. The global optimal path is then generated by a descent back-tracking on the cost map from the goal point to the starting point. However, when extending this method to anisotropic case, planning accuracy as well as feasibility is challenged due to required simplifications in order to solve the Eikonal equation.
To solve path planing problems in realistic environment, where there exists obstacles and external flow perturbation, we introduce the risk function ζ to describe the effect of obstacles to the safety of a generated path. We then formulate the influence of external flow current to the propagation of a wavefront in the form of an Eikonal equation. This allows the consideration of safety and efficiency of automatic vehicle during path planning procedure at the same time. As a result, solution to the equation could give a cost map and an optimal feasible path at end. Inspired by the underlying mechanism of numerical recipe of fast marching algorithm, we transform the extended Eikonal equation into the canonical form based on the physical rules behind. It enables us solve the global optimal path generation problem without any simplification nor introduction of additional complexity. Comparing to the classical fast marching based method in anisotropic environment, numerical simulations show that the proposed method is more efficient and robust to external flow perturbations, especially in the case of weak or strong external flow environment.
Comparing to the sampling based path planning method, like RRT [18] or its variations [19], the unique merit of fast marching based algorithm is the continuity of the generated path, which is a direct consequence of the continuity of the cost map in the configuration space. Here we introduce two additional parameters, i.e., safe distance d 0 and scaling parameter λ in the process of the cost map build-up. An open question on how these two parameters effect the maximum curvature of the generated path is untouched. In particular, the feasibility of the generated path to an automatic vehicle with not only kinodynamic constraints but also their own dynamical property, like unmanned cargos, should be considered [20]. Since 2012, he has been with the College of Computer Science and Technology, Zhejiang University of Technology, Hangzhou. His research interests include nonlinear control and optimization theory with applications in output regulation, multiagent systems, intelligent transportation systems, and image processing.
MEIYU ZHANG received the B.S. and M.S. degrees from Zhejiang University, Hangzhou, China, in 1988 and 1997, respectively. She is currently a Full Professor with the Zhejiang University of Technology. Her research interests include image processing and automations. VOLUME 8, 2020