## 3 Modeling of Path Planning
### Optimization Criteria  
The optimization cost functions are the functions related to the B-spline path. Given a curve path, the cost function can be used to calculate a comprehensive cost value that combines the time cost,  
Figure 3: The Lamb vortex diagram. the path curvature, the environment map, and the ocean current. A discrete B-spline path \(\wp\) can be expressed as the set comprised by the discrete points shown in the formula (4). \[\wp=\left\{\wp^{1},\ldots\wp^{i},\ldots\wp^{h}\right|1\leq i\leq h,\wp^{i}= \left(X^{i},Y^{i},\psi^{i}\right),\wp^{i}\in\mathbf{R}^{3}\right\} \tag{4}\]  
where \(h\) is the number of discrete points on the path; \(X^{i},Y^{i}\) are the position of the AUV at the \(i\) discrete point; \(\psi^{i}\) is the direction of the AUV at the \(i\) discrete point, also named as the yaw angle, which can be calculated using the following equation:  
\[\psi^{i}=\tan^{-1}\left(\frac{\left|Y^{i+1}-Y^{i}\right|}{\left|X^{i+1}-X^{i} \right|}\right) \tag{5}\]  
**The time cost** is one optimization criteria that requires the shortest sailing time, and usually is proportional to energy consumption. The time cost is obtained by dividing the total length of the accumulated path by the average velocity, as shown in the Equation (6). \[T_{\text{cost}}=\sum_{i=1}^{h-1}\frac{\left|\wp^{i+1}_{x,y}-\wp^{i}_{x,y} \right|}{\left|v\right|} \tag{6}\]  
where \(h\) is the total number of points in the discrete path, and \(v\) is the average velocity. The time cost is used to constrain the traveling time. Minimizing the time cost under certain conditions can result in a path that has the shortest distance and satisfies certain conditions. **The curvature cost** is obtained by accumulating the approximate curvature at each discrete point on the path, as shown in the Equation (7). \[\rho_{\text{cost}}=\sum_{i=1}^{h-1}\frac{\left|\wp^{i+1}_{\psi}-\wp^{i}_{\psi }\right|}{\left|\wp^{i+1}_{x,y}-\wp^{i}_{x,y}\right|} \tag{7}\]  
The curvature cost is used to constrain the spatial curvature of the path, that is, the turning radius (curvature radius) on the path. The curvature and the turning radius are inversely proportional. A legal path should not have a turning radius less than the minimum turning radius, and the path that satisfies the AUV turning radius constraint can be obtained by minimizing the curvature cost. **The map cost** is obtained by the map information at each discrete point on the path, as shown in the Equation (8). If the point is on a map danger area (land, island, etc. ), the map cost for that point is 1, otherwise 0. Minimizing the cost of the map is used to constrain the path from colliding with the danger area of the map and ensure the non-collision of the path. \[M_{\text{cost}}=\sum_{i=1}^{h}\left\{ \begin{array}{ll}1&\wp^{i}_{x,y}\cap Danger Area\\ 0&\text{otherwise}\end{array} \right. \tag{8}\]  
**The ocean current cost** is one optimization criteria obtained by accumulating the dot product of the ocean current vector and the path direction vector at each discrete point on the path, as shown in Equation (9)  
\[O_{\text{cost}}=\sum_{i=1}^{h}\left|\overrightarrow{v_{x,y}}\right|\cdot \theta\left(\overrightarrow{v_{y,y}},\overrightarrow{\tau}_{\wp^{i}_{x,y}}\right) \tag{9}\]  
where \(\overrightarrow{v_{y,y}}\) is the ocean current vector at the point \(\wp^{i}_{x,y}\); \(\overrightarrow{\tau}_{\wp^{i}_{x,y}}\) represents the path unit direction vector at the point \(\wp^{i}_{x,y}\); \(\theta(\cdot)\) is the angle between the two vectors \(\overrightarrow{v_{y,y}}\) and \(\overrightarrow{\tau}_{\wp^{i}_{x,y}}\). The path planning is a multi-objective optimization problem. The optimal path evaluation criterion includes the shortest path time \(T_{\text{cost}}\), the smallest cumulative curvature \(\rho_{\text{cost}}\), the map cost \(M_{\text{cost}}\), and the ocean current cost \(O_{\text{cost}}\).