## 2 Materials and Methods
### Path Smoothing and Set Trajectory Calculation  
For the classical algorithms, an approach was adopted in which the computed path was processed further to apply smoothing and impose speed limits. These limits were determined based on the path curvature, the maximum velocity achievable by the AUV model, and the maximum allowable lateral acceleration to maintain maneuverability and ensure the feasibility of the tracked trajectory. ```
1:Input: binary map, start \((x_{p},y_{p})\), goal \((x_{k},y_{k})\), number of waypoints noPoints, swarm size SwarmSize, max stall iterations maxStallIter, penalty \(\mathcal{P}\). 2:Cost function\(\mathtt{distanceCostPSO(x)}\): interpret each waypoint \((x_{i},y_{i})\in[0,1]\), scale to the map size, and build \(path=[\mathtt{start};\ (x_{1},y_{1}),\ldots,(x_{\mathtt{noPoints}},y_{\mathtt{noPoints}}); \mathtt{goal}]\); then, \(cost=\sum_{i=1}^{L-1}\mathtt{subCost}(path_{i},path_{i+1})\). 3:\(\mathtt{subCost}(p,q)\): subdivide the segment \((p\to q)\); each small step \(\Delta\) adds \(\Delta\) if properPoint is true; otherwise, \(\mathcal{P}\times\Delta\). 4:\(\mathtt{properPoint}(r)\) checks whether \(1\leq r_{x}\leq\dim_{x}\), \(1\leq r_{y}\leq\dim_{y}\), and \(\mathtt{map}(r_{x},r_{y})=0\); returns false if outside boundaries or is an obstacle. 5:Initialize swarm of size SwarmSize by placing each particle \(\mathbf{x}_{i}\) randomly in \([0,1]\). Set the velocity \(\mathbf{v}_{i}\gets 0\) for all \(i\). 6:Evaluate \(\mathtt{distanceCostPSO(x_{i})}\) for each particle. Let \(\mathit{pbest}_{i}\leftarrow\mathbf{x}_{i}\) (the particle's best position), and choose \(\mathit{gbest}\) as the particle with the minimal cost in the swarm. 7:\(\mathit{stallCount}\gets 0\). 8:while\(\mathit{stallCount}<\mathtt{maxStallIter}\)do
9:\(\mathit{improved}\leftarrow\) false
10:for each particle \(i=1,\ldots,\mathtt{SwarmSize}\)do
11:\(\mathbf{v}_{i}\gets w\,\mathbf{v}_{i}+c_{1}\,\mathbf{r}_{1}\odot( \mathit{pbest}_{i}-\mathbf{x}_{i})+c_{2}\,\mathbf{r}_{2}\odot(\mathit{gbest}- \mathbf{x}_{i})\)
12:\(\mathbf{x}_{i}\leftarrow\mathbf{x}_{i}+\mathbf{v}_{i}\); clamp to \([0,1]\)
13:\(\mathtt{costNew}\leftarrow\mathtt{distanceCostPSO(x_{i})}\)
14:if\(\mathtt{costNew}<\mathtt{distanceCostPSO(\mathit{pbest}_{i})}\)
15:\(\mathit{pbest}_{i}\leftarrow\mathbf{x}_{i}\)
16:if\(\mathtt{costNew}<\mathit{bestCost}\)
17:\(\mathit{gbest}\leftarrow\mathbf{x}_{i}\); \(\mathit{bestCost}\leftarrow\mathtt{costNew}\)
18:\(\mathit{improved}\leftarrow\) true
19:endfor
20:if improvedthen
21:\(\mathit{stallCount}\leftarrow\) 0 else\(\mathit{stallCount}\leftarrow\mathit{stallCount}+1\)
22:endwhile
23:Reconstruct the final path: scale \(\mathit{gbest}\) into map coords, prepend start, and append goal. 24:Output:path and \(\mathit{cost}\). ```  
**Algorithm 7**Particle swarm optimization  
Each of the classical algorithms returns a path as a set of \(N\) waypoints:  
\[\big{\{}\ (x_{1},y_{1}),\ (x_{2},y_{2}),\ \ldots,\ (x_{N},y_{N})\big{\}}, \tag{24}\]  
representing the desired path for the vehicle. To avoid abrupt heading changes, linear interpolation was performed between consecutive waypoints at a spacing \(\Delta s=10\) m. Next, the heading angles were computed to identify large changes in direction. The difference was examined for each pair of consecutive headings. Any segment for which the absolute heading change exceeded the threshold \(\theta_{\mathrm{th}}=30^{\circ}\) was designated as a \"sharp turn\". Consecutive sharp-turn indices were grouped into blocks and then extended by one point on each side to ensure smoother transitions. Within each block, a cubic spline was employed to fit the points, thus creating a smoothly varying local path. Outside these blocks, linear interpolation was retained. Overlapping spline segments were merged to form a continuous, piecewise-smooth path with norepeated points.