# An Automatic Navigation System for Unmanned Surface Vehicles in Realistic Sea Environments
## 3 The Static Environment Modeling and Path Planning
### The Fast Marching Method  
The FMM is a level set method proposed by [PERSON] in 1996 to solve the wave propagation problem [20]. It can also be used as a kind of the grid search algorithm in path planning with the continuity and smoothness of the path. This method can be divided into two steps. The first step is the exploration process, which establishes the optimal cost value (arrival time) of each grid on the whole map. The last step is the development process. That is, by solving the optimal cost value, the optimal path is formed from the target to the starting point. The exploration process is very similar to the expansion of waves. The path of waves from the source point to the target point can be considered as the optimal path, so we can establish the time function based on the propagation of waves. The FMM calculates the arrival time \(T\) for a wave to reach every point of the space. The wave can be generated by more than one source point, and each point originates one wave. The corresponding time of the source point is \(T=0\). \[\left\{ \begin{array}{l}t=T(x,y)\\ T(x_{0},y_{0})=0\end{array} \right. \tag{1}\]  
where \(T(x,y)\) is the time that the wave interface requires to reach \((x,y)\), and \((x_{0},y_{0})\) is the source point. When Equation (1) is a derivative of \(t\), the inner product of two vectors about \(\
abla T(x,y)\) and speed \(V\) of wavefront can be obtained. \[\left|\
abla T\left(x,y\right)\right|\cdot V\left(x,y\right)=1 \tag{2}\]  
That is, at the point \((x,y)\), the motion of the front is described by the Eikonal equation. The numerical solution of Equation (2) can be obtained via the upwind deferential method when using the FMM. The solving process of the FMM is similar to [PERSON]'s method but in a continuous way. Suppose \((x,y)\) needs to be solved and its neighbor is a point set containing four elements \((x+\Delta x,y)\), \((x-\Delta x,y)\), \((x,y+\Delta y)\), \((x,y-\Delta y)\). The discretization of the gradient \(\
abla T(x,y)\) according to [32] derives to the following equation:  
\[\max\left(D_{(x,y)}^{-x}T,-D_{(x,y)}^{+x},0\right)^{2}+\max\left(D_{(x,y)}^{-y }T,-D_{(x,y)}^{+y},0\right)^{2}=\frac{1}{V_{(x,y)}^{2}} \tag{3}\]  
where  
\[ \begin{array}{l}D_{(x,y)}^{-x}=\frac{T_{(x,y)}-T_{(x-\Delta x,y)}}{\Delta x }\\ D_{(x,y)}^{+x}=\frac{T_{(x+\Delta x,y)}-T_{(x,y)}}{\Delta x}\\ D_{(x,y)}^{-y}=\frac{T_{(x,y-\Delta y)}-T_{(x,y-\Delta y)}}{\Delta y}\\ D_{(x,y)}^{+y}=\frac{T_{(x,y+\Delta y)}-T_{(x,y)}}{\Delta y}\end{array}  \tag{4}\]  
and \(\Delta x\) and \(\Delta y\) are the grid spacing in the \(x\) and \(y\) directions. Substituting (4) in (3) and letting  
\[ \begin{array}{l}T=T_{(x,y)}\\ T_{1}=\min(T_{(x-\Delta x,y)},T_{(x+\Delta x,y)})\\ T_{2}=\min(T_{(x,y-\Delta y)},T_{(x,y+\Delta y)})\end{array}  \tag{5}\]  
the Eikonal Equation can be rewritten for a discrete 2D space as:  
\[\max\left(\frac{T-T_{1}}{\Delta x},0\right)^{2}+\max\left(\frac{T-T_{2}}{ \Delta y},0\right)^{2}=\frac{1}{V_{(x,y)}^{2}} \tag{6}\]Consequently Equation (6) can be solved as the following quadratic:  
\[\frac{1}{V_{(x,y)}}=\left\{ \begin{array}{cc}\sqrt{\left(\frac{T-T_{1}}{\Delta x }\right)^{2}+\left(\frac{T-T_{2}}{\Delta y}\right)^{2}}&T\geq\max(T_{1},T_{2}) \\ \frac{T-T_{1}}{\Delta y}&T_{2}\geq T\geq T_{1}\\ \frac{T-T_{2}}{\Delta y}&T_{1}\geq T\geq T_{2}\end{array} \right. \tag{7}\]  
Assuming that the velocity is positive \((V>0)\), \(T\) must be greater than \(T_{1}\) and \(T_{2}\), that is, the front wave passes over the coordinates \((x,y)\) only once. To further illustrate the FMM algorithm, a simple case representing how to update a \(7\times 7\) grid map is shown in Figure 3. Figure 3a shows the initial configuration of the algorithm with the middle point being the algorithm start point. The interface propagating speed is set to be uniform as 1 at each grip point.