A Hybrid Ant Colony Optimization and Dynamic Window Method for Real-Time Navigation of USVs

Abstract

Unmanned surface vehicles (USVs) rely on multi-sensor perception, such as radar, LiDAR, GPS, and vision, to ensure safe and efficient navigation in complex maritime environments. Traditional ant colony optimization (ACO) for path planning, however, suffers from premature convergence, slow adaptation, and poor smoothness in cluttered waters, while the dynamic window approach (DWA) without global guidance can become trapped in local obstacle configurations. This paper presents a sensor-oriented hybrid method that couples an improved ACO for global route planning with an enhanced DWA for local, real-time obstacle avoidance. In the global stage, the ACO state–transition rule integrates path length, obstacle clearance, and trajectory smoothness heuristics, while a cosine-annealed schedule adaptively balances exploration and exploitation. Pheromone updating combines local and global mechanisms under bounded limits, with a stagnation detector to restore diversity. In the local stage, the DWA cost function is redesigned under USV kinematics to integrate velocity adaptability, trajectory smoothness, and goal-deviation, using obstacle data that would typically originate from onboard sensors. Simulation studies, where obstacle maps emulate sensor-detected environments, show that the proposed method achieves shorter paths, faster convergence, smoother trajectories, larger safety margins, and higher success rates against dynamic obstacles compared with standalone ACO or DWA. These results demonstrate the method’s potential for sensor-based, real-time USV navigation and collision avoidance in complex maritime scenarios.

1. Introduction

Unmanned surface vehicles (USVs), as autonomous platforms for ocean mapping, environmental monitoring, port inspection, and maritime security, rely heavily on multi-sensor systems—such as LiDAR, radar, GPS, inertial measurement units (IMUs), and cameras—to perceive their surroundings and ensure safe navigation [1,2]. These sensors provide critical information about static and dynamic obstacles, traffic conditions, and environmental uncertainty, enabling USVs to plan safe and efficient trajectories in real time [3,4,5]. However, converting raw sensor data into globally optimal and locally feasible paths remains a major challenge [6,7].

Path planning in sensor-driven USV navigation is typically organized into two layers [8]. Global path planning uses prior maps or sensor-fused environment models to produce optimal routes in relatively static settings [9], while local path planning depends on real-time perception for dynamic obstacle avoidance and trajectory refinement under sensor noise and environmental changes [10]. Relying solely on one layer often fails in cluttered waterways with narrow passages and moving obstacles. This has motivated research on integrating global and local planners to achieve both global efficiency and local reactivity for USVs in sensor-rich environments [11].

In recent years, researchers have explored a variety of advanced path planning techniques. For example, Bahwini et al. proposed a method for path planning in soft tissue deformation environments [12], which employs finite element modeling and bio-heat transfer analogy to explicitly account for environmental deformation and uncertainty, demonstrating strong adaptability in microscale scenarios. However, such approaches are primarily tailored for deformable media, which differ substantially from the large-scale rigid waterways encountered by unmanned surface vehicles (USVs), and they also incur considerable computational overhead. The optimal path planning method based on Cellular Neural Networks (CNNs) leverages parallel computation and dynamical system modeling, exhibiting strong nonlinear processing capability and distributed characteristics in complex environments [13]; nevertheless, the network scale and parameter sensitivity limit its applicability to large-scale maritime charts. In the field of unmanned aerial vehicles (UAVs), researchers have proposed the crossover recombination-based global-best Brainstorm Optimization algorithm (GBSO-CR), which enhances global exploration and diversity control to achieve superior paths in three-dimensional threat environments. Despite these advantages, its high computational complexity and strong parameter dependence restrict its suitability for real-time online path planning in USVs [14].

Overall, these approaches provide diverse strategies for addressing environmental uncertainty, dynamic changes, and real-time constraints. On the one hand, soft tissue deformation and thermal modeling methods are more applicable to small-scale or specific medium environments, making them difficult to adapt to maritime scenarios with rigid obstacles and large-scale continuous waterways. On the other hand, methods based on CNNs and brainstorm optimization, despite their strong global search and parallel processing capabilities, typically incur high computational costs and thus fall short of meeting the real-time planning requirements of USVs in dynamic marine environments.

Among global planners, Ant Colony Optimization (ACO) is popular for mobile robots and USVs thanks to its distributed search, positive feedback, and robustness in complex environments [15,16]. Yet the standard ACO exhibits several drawbacks in continuous spaces and densely cluttered scenes:

• Premature convergence: pheromones accumulate rapidly on a few paths, trapping the search in local optima.
• Insufficient safety: distance-only heuristics may drive paths that skim obstacle boundaries.
• Poor smoothness: without heading constraints, routes contain sharp turns that increase energy consumption and complicate control.
• Limited adaptability: success rates degrade in highly dynamic environments, and feasible paths may not be found.

For local planning, the Dynamic Window Approach (DWA) [17] evaluates motion commands directly in the robot’s (or USV’s) velocity space under kinematic constraints and is widely used for obstacle avoidance. However, classical DWA primarily relies on local sensing and lacks global guidance; it is prone to getting stuck in cul-de-sacs, concavities, or complex obstacle clusters, reducing efficiency and even preventing goal reachability [18]. Recent work has attempted to integrate improved ACO and DWA variants to combine global guidance with local reactivity. For example, Song et al. [19] proposed an ACO–DWA fusion strategy that enhances pheromone initialization and evaluation functions, achieving shorter and smoother paths for mobile robots in dynamic environments.

To address these issues, we propose a fusion method that couples an improved ACO with an enhanced DWA for USV path planning:

Global stage: the improved ACO fuses three heuristics—path length, obstacle clearance, and trajectory smoothness—into the transition rule, and adopts a cosine-annealed temperature to balance exploration and exploitation. A pheromone update with lower/upper bounds and a stagnation-triggered restart enhances convergence and diversity.

Local stage: the DWA cost is redesigned under USV kinematics by incorporating trajectory smoothness and goal-deviation terms, enabling dynamic tracking of the global reference path with real-time collision avoidance.

Fusion strategy: ACO supplies a safe and smooth global reference, while DWA performs trajectory optimization and avoidance around dynamics, preserving global optimality and local real-time feasibility.

The main contributions of our work are as follows:

(1)

A multi-heuristic, temperature-controlled ACO that improves the safety and smoothness of global routes.

(2)

A DWA cost tailored to USV motion characteristics, fusing speed, smoothness, and goal-deviation to realize dynamic avoidance with global-path tracking.

(3)

An ACO–DWA integration that tightly couples global and local planning, markedly improving success rates in complex dynamic scenes.

(4)

Comprehensive simulations showing that the proposed method outperforms standalone ACO and standalone DWA in path length, smoothness, safety margin, and avoidance success.

2. USV Kinematic Modeling

To incorporate a trajectory smoothness metric into the planner, we first establish a steering kinematic model for the USV [20]. Under a small-angle approximation and standard simplifications—neglecting sideslip and wave-induced disturbances—the planar motion of a USV can be treated as a nonholonomic system whose state is (x,y,θ), where θ is the heading.

As illustrated in Figure 1 (simplified steering model), assume the heading is aligned with the velocity direction. Let $\varphi$ denote the rudder (steering) angle and $L$ the distance from the center of mass to the steering center. The kinematics are then

$$\left\{\begin{array}{l}\dot{x}=v\mathrm{c}\mathrm{o}\mathrm{s}\theta \\ \dot{y}=v\mathrm{s}\mathrm{i}\mathrm{n}\theta \\ \dot{\theta }={\displaystyle \frac{v}{L}}\mathrm{t}\mathrm{a}\mathrm{n}\varphi \end{array}\right.$$

(1)

where

$v$: longitudinal surge speed of the USV;

$\varphi$: rudder/steering angle;

$L$: distance from the center of mass to the steering center;

$\theta$: heading angle.

Figure 1. Simplified steering model.

Figure 1. Simplified steering model.

3. Methods

3.1. Improved ACO

3.1.1. Multi-Heuristic Fusion

In the standard ACO, the heuristic information ${\eta }_{ij}=1/d_{ij}$ only accounts for path length and thus fails to balance safety and smoothness. We introduce three factors—Distance, Clearance, and Smoothness—and fuse them with weights:

$${\eta }_{ij}^{\mathrm{fusion}}=w_d·{\displaystyle \frac{1}{d_{ij}+\epsilon }}+w_c·{\displaystyle \frac{\mathrm{c}\mathrm{l}\mathrm{i}\mathrm{p}\left(D_j,0,D_{\mathrm{m}\mathrm{a}\mathrm{x}}\right)}{D_{\mathrm{m}\mathrm{a}\mathrm{x}}}}+w_s·{\displaystyle \frac{1+\mathrm{c}\mathrm{o}\mathrm{s}{\theta }_{ij}}{2}}$$

(2)

where

• $d_{ij}$: Euclidean distance from node $i$ to node $j$;
• $D_j$: distance from node $j$ to the nearest obstacle;
• ${\theta }_{ij}$: turning angle computed from the USV kinematic model;
• $w_d,w_c,w_s$: weights of the three factors;
• $\epsilon$: a small constant to avoid division by zero;
• $\mathrm{c}\mathrm{l}\mathrm{i}\mathrm{p}\left(x,0,D_{\max }\right)=\mathrm{m}\mathrm{i}\mathrm{n}\left\{\mathrm{m}\mathrm{a}\mathrm{x}\{x,0\},D_{\max }\right\}$.

This fusion discourages “hugging” obstacles while favoring feasible paths with gentler heading changes.

3.1.2. Temperature-Controlled Transition Probability

To balance exploration and exploitation adaptively, a temperature term $T\left(t\right)$ modulates the influence of the heuristic:

$$P_{ij}^k(t)={\displaystyle \frac{{\left[{\tau }_{ij}\left(t\right)\right]}^{\alpha }{\left[{\eta }_{ij}^{\mathrm{fusion}}\right]}^{\beta /T\left(t\right)}}{{\sum }_{\mathcal{l}\in A_k}{\left[{\tau }_{i\mathcal{l}}\left(t\right)\right]}^{\alpha }{\left[{\eta }_{i\mathcal{l}}^{\mathrm{fusion}}\right]}^{\beta /T\left(t\right)}}}$$

(3)

where $A_k$ is the selectable neighborhood for ant $k$. The temperature follows a cosine-annealing schedule:

$$T(t)=T_{\mathrm{m}\mathrm{i}\mathrm{n}}+{\displaystyle \frac{T_{\mathrm{m}\mathrm{a}\mathrm{x}}-T_{\mathrm{m}\mathrm{i}\mathrm{n}}}{2}}\left(1+\mathrm{c}\mathrm{o}\mathrm{s}{\displaystyle \frac{\pi (t-1)}{t_{\mathrm{m}\mathrm{a}\mathrm{x}}}}\right)$$

(4)

At early iterations, $T$ is larger, $\beta /T$ is smaller, and the search is more exploratory; as $T$ decreases, the algorithm shifts toward exploitation and stabilizes.

3.1.3. Local Pheromone Update

When an ant traverses edge $i\to j$, we apply a local “de-biasing” update:

$${\tau }_{ij}\leftarrow (1-\xi ){\tau }_{ij}+\xi {\tau }_0, \xi \in (0,1)$$

(5)

which reduces over-commitment to recently used edges and increases diversity.

3.1.4. Global Pheromone Update with Bounds

At the end of each iteration, pheromones evaporate and are reinforced by both ant contributions and an elite term:

$${\tau }_{ij}\leftarrow (1-\rho ){\tau }_{ij}+\Delta {\tau }_{ij}^{\mathrm{a}\mathrm{n}\mathrm{t}}+q\left(t\right)\Delta {\tau }_{ij}^{\mathrm{e}\mathrm{l}\mathrm{i}\mathrm{t}\mathrm{e}}$$

(6)

With

$$\Delta {\tau }_{ij}^{\mathrm{ant}}=\sum _{k=1}^m{\displaystyle \frac{Q}{L_k}}1\left\{(i\to j)\in {\mathrm{path}}_k\right\}, \Delta {\tau }_{ij}^{\mathrm{elite}}={\displaystyle \frac{Q}{L_{\mathrm{best}}}}1\{(i\to j)\in \mathrm{best}\_\mathrm{path}\}$$

(7)

where $L_k$ is the length of ant $k$’s path and $L_{\mathrm{best}}$ is the current best length. We use a dynamic elite coefficient

$$q(t)=\mathrm{e}\mathrm{x}\mathrm{p}\left({\displaystyle \frac{t-1}{t_{\mathrm{m}\mathrm{a}\mathrm{x}}}}\right)-1$$

(8)

and impose pheromone bounds

$${\tau }_{ij}\leftarrow min\left\{{\tau }_{max},max\left\{{\tau }_{min},{\tau }_{ij}\right\}\right\}$$

(9)

These mechanisms speed up convergence while preventing premature stagnation.

3.1.5. Stagnation Criterion and Restart

If the best value shows no improvement for $P$ consecutive iterations, we trigger a “pull-back” restart:

$${\tau }_{ij}\leftarrow (1-\gamma ){\tau }_{ij}+\gamma {\tau }_0,\gamma \in (0,1)$$

(10)

which resets the pheromone field toward its initialization to restore exploration capability.

3.2. Pseudocode for the Improved ACO

At initialization (see Algorithm 1), pheromone levels ${\tau }_{ij}$ are set to ${\tau }_0$ and the fused heuristic ${\eta }_{ij}^{\mathrm{fusion}}$ in (2) is precomputed (or updated on demand) from the distance, clearance and smoothness terms. We fix $\alpha ,\beta ,\rho ,\xi ,{\tau }_{\mathrm{m}\mathrm{i}\mathrm{n}},{\tau }_{\mathrm{m}\mathrm{a}\mathrm{x}},\gamma$ and define the cosine-annealed temperature schedule $T\left(t\right)$ in (4) together with the time-varying elite gain $q\left(t\right)$ in (8). For each iteration $t=1,\dots ,t_{\mathrm{m}\mathrm{a}\mathrm{x}}$ (or until an early-stopping criterion is met), a colony of $m$ ants independently constructs candidate paths using the transition rule (3) and applies the local update (5) along traversed edges. After all ants return, path costs are evaluated and the incumbent best solution is updated. Global pheromone evaporation and reinforcement are then performed using (6) and (7), followed by bounding in (9) to prevent premature saturation. If the best cost has not improved for $P$ consecutive iterations, the stagnation pull-back in (10) is triggered to restore exploration. The loop records the periteration best cost for the convergence plot, and the final best path can be optionally post-processed by visibility-based node pruning or mild smoothing before being passed to the local planner. The environment map used for path construction can be either derived from sensor perception or generated by simulation.

Algorithm 1	Improved-ACO (Global Planning)
Input	Grid map, start s, goal g, ACO parameters
Output	best_path, best_cost
1	Initialize pheromone $\tau \leftarrow {\tau }_0$ on all feasible edges.
2	Precompute distance field $D$ and fused heuristic ${\eta }^{\mathrm{fusion}}$.
3	Set best_cost $\leftarrow +\infty$, no_improve $\leftarrow 0$.
4	for $t=1$ to $t_{\mathrm{m}\mathrm{a}\mathrm{x}}$ do
5	$\hspace{1em}\hspace{1em}\mathrm{perIterAntTau} \leftarrow 0; \mathrm{perIterElite} \leftarrow 0$
6	$T=T_{\mathrm{m}\mathrm{i}\mathrm{n}}+{\displaystyle \frac{T_{\mathrm{m}\mathrm{a}\mathrm{x}}-T_{\mathrm{m}\mathrm{i}\mathrm{n}}}{2}}\left(1+\mathrm{c}\mathrm{o}\mathrm{s}{\displaystyle \frac{\pi (t-1)}{t_{\mathrm{m}\mathrm{a}\mathrm{x}}}}\right)$.
7	$\mathrm{for} k=1$ to $m$ do
8	$\hspace{1em}\hspace{1em}{\mathrm{p}\mathrm{a}\mathrm{t}\mathrm{h}}_k\leftarrow \mathrm{ConstructPath}\_\mathrm{Pro} \left(M,s,g,\tau ,{\eta }^{\mathrm{fusion}},T,C_{\min },\xi ,{\tau }_0,\alpha ,\beta \right)$.
9	$\hspace{1em}\hspace{1em}{\mathrm{p}\mathrm{a}\mathrm{t}\mathrm{h}}_k\leftarrow \mathrm{R}\mathrm{e}\mathrm{m}\mathrm{o}\mathrm{v}\mathrm{e}\mathrm{R}\mathrm{e}\mathrm{d}\mathrm{u}\mathrm{n}\mathrm{d}\mathrm{a}\mathrm{n}\mathrm{t}\left(\left.{path}_k\right)\right)$
10	$\hspace{1em}\hspace{1em}{L}_k\leftarrow \mathrm{P}\mathrm{a}\mathrm{t}\mathrm{h}\mathrm{L}\mathrm{e}\mathrm{n}\mathrm{g}\mathrm{t}\mathrm{h}\left.\left({path}_k\right)\right)$
11	$\hspace{1em}\hspace{1em}\mathrm{perIterAntTau} \leftarrow \mathrm{perIterAntTau}+\Delta \tau \left({path}_k\left.,L_k,Q\right)\right)$
12	end for
13	$\mathrm{if} \mathrm{all} L_k=\infty$ then
14	$\hspace{1em}\hspace{1em}\hspace{1em}{C}_{\mathrm{m}\mathrm{i}\mathrm{n}}\leftarrow \mathrm{m}\mathrm{a}\mathrm{x}\left(0,C_{\mathrm{m}\mathrm{i}\mathrm{n}}-\Delta C\right)$; rerun this iteration
15	if still infeasible then return failure
16	end if
17	Find best individual $\left(L^{\ast }\right.,\left.{path}^{\ast }\right) \mathrm{among}\left\{\left(L_k\right.\right.,\left.\left.{path}_k\right)\right\}$
18	$\mathrm{if} L^{\ast }<$ best_cost then
19	${\hspace{1em}\hspace{1em}\hspace{1em}\mathrm{b}\mathrm{e}\mathrm{s}\mathrm{t}}_{\mathrm{c}\mathrm{o}\mathrm{s}\mathrm{t}}\leftarrow L^{\ast },\mathrm{b}\mathrm{e}\mathrm{s}\mathrm{t}\_\mathrm{p}\mathrm{a}\mathrm{t}\mathrm{h}\leftarrow {path}^{\ast },\mathrm{n}\mathrm{o}\_\mathrm{i}\mathrm{m}\mathrm{p}\mathrm{r}\mathrm{o}\mathrm{v}\mathrm{e}\leftarrow 0$
20	else
21	$\hspace{1em}\hspace{1em}\hspace{1em}\mathrm{no}\_\mathrm{improve} \leftarrow$ no_improve +1
22	end if
23	$\mathrm{p}\mathrm{e}\mathrm{r}\mathrm{I}\mathrm{t}\mathrm{e}\mathrm{r}\mathrm{E}\mathrm{l}\mathrm{i}\mathrm{t}\mathrm{e}\leftarrow Q/L^{\ast }$ added only on edges of ${path}^{\ast }$
24	$q\left(t\right)\leftarrow \mathrm{e}\mathrm{x}\mathrm{p}\left({\displaystyle \frac{t-1}{t_{\max }}}\right)-1$
25	$\tau \leftarrow (1-\rho )\tau +\mathrm{perIterAntTau}+q\left(t\right)·\mathrm{p}\mathrm{e}\mathrm{r}\mathrm{I}\mathrm{t}\mathrm{e}\mathrm{r}\mathrm{E}\mathrm{l}\mathrm{i}\mathrm{t}\mathrm{e}$
26	$\tau \leftarrow \mathrm{m}\mathrm{i}\mathrm{n}\left\{{\tau }_{\max },\mathrm{m}\mathrm{a}\mathrm{x}\left\{{\tau }_{\min },\tau \right\}\right\}$
27	$\mathrm{if} \mathrm{no}\_\mathrm{improve} \ge P$ then
28	$\hspace{1em}\hspace{1em}\hspace{1em}\tau \leftarrow (1-\gamma )\tau +\gamma {\tau }_0$
29	$\hspace{1em}\hspace{1em}\hspace{1em}\mathrm{no}\_\mathrm{improve} \leftarrow 0$
30	end if
31	end for
32	return best_path, best_cost

3.3. Simulation of the Improved ACO

We evaluate the proposed method in a 21 × 21 grid map with sparse obstacles, which emulate a simplified sensor-detected map. The maximum number of iterations is 250. After a path is generated, redundant waypoints are removed by a visibility-based triangle clipping procedure. The start and goal are set to (1,21) and (21,1), respectively. Baselines include several representative ACO variants: the original Ant System (AS) [21], Ant Colony Optimization (ACO) as a general framework [22], Ant Colony System (ACS) [23], Rank-based Ant System (RAS) [24], and Elitist Ant System (EAS) [21]. The proposed variant (“Ours”) is the improved ACO. Key parameter settings are summarized in

Table 1. Key parameters of the improved ACO.

Symbol	Meaning	Value
$m$	Number of ants	20
$\alpha$	Pheromone importance	1.5
$\beta$	Heuristic importance	0.5
$\rho$	Evaporation rate	0.30
$Q$	Pheromone deposit constant	40
$t_{\max }$	Maximum iterations	250
$C_{\min }$	Minimum safety clearance (cells)	1.0

.

In Figure 2, the red solid line with circles denotes vanilla ACO, the green curve AS, the dark-blue curve ACS, the cyan curve RAS, the black curve EAS, and the blue curve our improved ACO. The proposed method reduces fruitless branches during search and removes superfluous turns in the post-optimization stage, yielding markedly smoother trajectories. As summarized in

Table 2. Performance comparison across ACO variants.

Method	ACO	AS	ACS	RAS	EAS	Ours
Best iteration	10	1	6	2	2	3
Avg. path length	30.97	33.99	34.29	34.03	34.77	29.49
Runtime (s)	4.495	5.039	16.84	5.371	14.36	1.24
Smoothed	×	×	×	×	×	√

Note: √ indicates that path smoothing is applied; × indicates no smoothing.

, the average path length of the proposed method is 29.49—about 4.7% shorter than vanilla ACO—and its runtime is 1.24 s, which is lower than the baselines. Figure 3 further shows that our dynamic pheromone updating and node-pruning strategy leads to faster convergence.

3.4. Improved DWA

3.4.1. Kinematic Model and Dynamic-Window Feasibility

Under a small-angle approximation and neglecting sideslip and wave disturbances, the USV’s planar motion follows the kinematics in (1), where the surge speed is aligned with the heading. Let $v$ be the surge speed, $\varphi$ the rudder (steering) angle, $\theta$ the heading, and $L$ the distance from the vehicle’s CoG to the steering center. The minimum turning radius is given by the geometric relation

$$R_{min}={\displaystyle \frac{L}{tan{\phi }_{max}}}$$

(11)

In practice, actuator saturation and hull dynamics impose bounds on speed, steering angle, and their rates. With sampling period $\Delta t$ and current control ($v_k,{\varphi }_k$), the dynamic window is

$$\begin{array}{l}v\in \left[max\left(v_{min},v_k+a_{min}\Delta t\right),min\left(v_{max},v_k+a_{max}\Delta t\right)\right]\\ \phi \in \left[max\left({\phi }_{min},{\phi }_k+{\dot{\phi }}_{min}\Delta t\right),min\left({\phi }_{max},{\phi }_k+{\dot{\phi }}_{max}\Delta t\right)\right]\end{array}$$

(12)

To avoid numerical stagnation and deadlock, we enforce a low-speed threshold $v\ge v_{ϵ}>0$. During sampling, candidates that violate curvature/steering feasibility (e.g., $\left|\varphi \right|>{\varphi }_{\mathrm{m}\mathrm{a}\mathrm{x}}$ or equivalently $\kappa =\mathrm{t}\mathrm{a}\mathrm{n}\varphi /L$ beyond its bound) are discarded.

3.4.2. Prediction Model and Safety Criterion

For each candidate control ($v,\varphi$), we roll out the kinematics (1) for $N_p$ steps to obtain the predicted trajectory

$$\Gamma =\left\{\left(x_{\mathcal{l}},y_{\mathcal{l}},{\theta }_{\mathcal{l}}\right)\mid \mathcal{l}=1,2,\dots ,N_p\right\}$$

(13)

Let the minimum Euclidean distance to all static/dynamic obstacles be

$$d_{min}=\underset{\mathcal{l}}{min} \underset{o\in \mathcal{O}}{min} ‖\left(x_{\mathcal{l}},y_{\mathcal{l}}\right)-\left(x_o,y_o\right)‖$$

(14)

where $\mathcal{O}$ is the obstacle set and ($x_o,y_o$) an obstacle position.

To couple hard safety with sensitivity near critical regions, we adopt a dual-threshold rule:

• Hard safety: if $d_{\mathrm{m}\mathrm{i}\mathrm{n}}<D$ (hard safety radius), the candidate is infeasible and rejected.
• Soft safety: for $d_{\mathrm{m}\mathrm{i}\mathrm{n}}\ge D$, a logistic cost enhances sensitivity near the soft boundary:

  $${\mathit{Clear}}^{\ast }=K\left({\displaystyle \frac{1}{1+exp\left(c\left(d_{min}-d_{lim}\right)\right)}}-{\displaystyle \frac{1}{2}}\right)$$

  (15)

  where $d_{\mathrm{l}\mathrm{i}\mathrm{m}}>D,c>0$ controls the slope, and $K$ scales the penalty.

Around $d_{\mathrm{m}\mathrm{i}\mathrm{n}}\approx d_{\mathrm{l}\mathrm{i}\mathrm{m}}$, the cost varies most rapidly, prompting early avoidance. Predicted positions of moving obstacles may be obtained via constant-velocity, constant-acceleration, or coordinated-turn models; our framework is agnostic to the predictor.

3.4.3. Objective Function

To keep local avoidance consistent with the global objective, we maximize a multi-term score that blends goal heading, trajectory smoothness, clearance, speed preference, and relative goal bearing:

$$\begin{array}{c}J(v,\varphi )=\alpha Heading(v,\varphi )+\beta Smooth(v,\varphi )+\delta {Clear}^{\ast }(v,\varphi )\\ +\gamma Velocity(v,\varphi )+\mu RelAngle(v,\varphi )\end{array}$$

(16)

with nonnegative weights $\alpha ,\beta ,\delta ,\gamma ,\mu$.

(1) Goal-heading term.

Let the local tracking target (e.g., the next point on the global path) be $\left(x_g,y_g\right)$. With predicted terminal state ($x_{N_p},y_{N_p},{\theta }_{N_p}$), the desired bearing is

$${\theta }_{\mathit{des}}=atan2\left(y_g-y_{N_p},x_g-x_{N_p}\right)$$

(17)

and the heading score is

$$\mathit{Heading}={\displaystyle \frac{1+cos\left({\theta }_{N_p}-{\theta }_{des}\right)}{2}}\in [0,1]$$

(18)

(2) Smoothness term.

To discourage sharp turns and improve maneuverability/efficiency,

$$\mathit{Smooth}={\displaystyle \frac{1}{N_p-1}}\sum _{l=1}^{N_p-1} {\displaystyle \frac{1+cos\left({\theta }_{l+1}-{\theta }_l\right)}{2}}\in [0,1]$$

(19)

(3) Clearance term.

If $d_{\mathrm{m}\mathrm{i}\mathrm{n}}<D$ the candidate is rejected; otherwise the soft safety cost follows (16). For completeness we restate it:

$${\mathit{Clear}}^{\ast }=K\left({\displaystyle \frac{1}{1+exp\left\{c\left(d_{min}-d_{lim}\right)\right\}}}-{\displaystyle \frac{1}{2}}\right)$$

(20)

(4) Speed preference.

Encouraging efficient motion via normalized surge speed,

$$\mathit{Velocity}={\displaystyle \frac{v-v_{min}}{v_{max}-v_{min}}}\in [0,1]$$

(21)

(5) Relative goal-bearing term.

To re-align toward the goal after detours,

$$\mathit{RelAngle}={\displaystyle \frac{1+cos\left({\theta }_{des}-{\theta }_k\right)}{2}}\in [0,1]$$

(22)

where ${\theta }_k$ is the current heading.

3.5. Pseudocode of the Improved DWA

Outline. Initialize kinematic bounds and weights ($\alpha ,\beta ,\delta ,\gamma ,\mu$). At each control cycle: (i) build the dynamic window (12) around ($v_k,{\varphi }_k$) and sample feasible ($v,\varphi$) respecting $\left|\varphi \right|\le {\varphi }_{\mathrm{m}\mathrm{a}\mathrm{x}}$ and $v\ge v_{ϵ}$; (ii) roll out (1) for $N_p$ steps to get $\mathsf{\Gamma }$; (iii) compute $d_{\mathrm{m}\mathrm{i}\mathrm{n}}$ by (15); reject candidates with $d_{\mathrm{m}\mathrm{i}\mathrm{n}}<D$, evaluate surviving ones by (16) with terms (19)–(22); (iv) select the maximizer of $J$ and apply it; (v) advance the state and repeat. The detailed pseudocode is summarized in Algorithm 2.

Algorithm 2	Improved-DWA (Local Planning with Hard/Soft Safety and Multi-Objective)
Input	$\mathrm{Input}: \mathrm{Current} \mathrm{state} x=(x,y,\theta ,v,\omega )$, local goal, static map, predicted dynamic obstacles, DWA parameters.
Output	$\mathrm{Control} u^{\ast }=\left(v^{\ast },{\varphi }^{\ast }\right), \mathrm{best} \mathrm{trajectory} \mathrm{t}\mathrm{r}\mathrm{a}\mathrm{j}{j}^{\ast }$.
1	$\mathrm{Build} \mathrm{dynamic} \mathrm{window}: v\in \left[\mathrm{m}\mathrm{a}\mathrm{x}\left(v_{\mathrm{m}\mathrm{i}\mathrm{n}},v_k+a_{\mathrm{m}\mathrm{i}\mathrm{n}}\Delta t\right),\mathrm{m}\mathrm{i}\mathrm{n}\left(v_{\mathrm{m}\mathrm{a}\mathrm{x}},v_k+a_{\mathrm{m}\mathrm{a}\mathrm{x}}\Delta t\right)\right];\varphi \in \left[\mathrm{m}\mathrm{a}\mathrm{x}\left({\varphi }_{\mathrm{m}\mathrm{i}\mathrm{n}},{\varphi }_k+{\dot{\varphi }}_{\mathrm{m}\mathrm{i}\mathrm{n}}\Delta t\right),\mathrm{m}\mathrm{i}\mathrm{n}\left({\varphi }_{\mathrm{m}\mathrm{a}\mathrm{x}},{\varphi }_k+{\dot{\varphi }}_{\mathrm{m}\mathrm{a}\mathrm{x}}\Delta t\right)\right]$.
2	$\mathrm{Prune} \mathrm{samples} \mathrm{with} \left|\varphi \right|>{\varphi }_{\mathrm{m}\mathrm{a}\mathrm{x}} \mathrm{or} v<v_{\epsilon }$;
3	$\mathrm{b}\mathrm{e}\mathrm{s}\mathrm{t}\mathrm{S}\mathrm{c}\mathrm{o}\mathrm{r}\mathrm{e}\leftarrow -\infty ; u^{\ast }\leftarrow \varnothing ; {traj}^{\ast }\leftarrow \varnothing ;$
4	$\mathrm{For} \mathrm{each} \mathrm{sample} (v,\varphi )$ in dynamic window do
5	$\hspace{1em}\hspace{1em}\mathrm{Predict} N_p$-step trajectory $\mathsf{\Gamma }={\left\{\left(x_{\mathcal{l}},y_{\mathcal{l}},{\theta }_{\mathcal{l}}\right)\right\}}_{\mathcal{l}=1}^{N_p}$.
6	$\hspace{1em}\hspace{1em}{d}_{\mathrm{m}\mathrm{i}\mathrm{n}}\leftarrow \underset{\mathcal{l}}{\mathrm{m}\mathrm{i}\mathrm{n}} \underset{o}{\mathrm{m}\mathrm{i}\mathrm{n}} ‖\left(x_{\mathcal{l}},y_{\mathcal{l}}\right)-\left(x_o,y_o\right)‖$ over static + predicted obstacles.
7	$\mathrm{If} d_{\mathrm{m}\mathrm{i}\mathrm{n}}<D$ then continue.
8	$\mathrm{Compute} \mathrm{desired} \mathrm{heading} {\theta }_{\mathrm{des}}=\mathrm{a}\mathrm{t}\mathrm{a}\mathrm{n}2\left(y_g-y_{N_p},x_g-x_{N_p}\right)$.
9	$\mathit{Heading}={\displaystyle \frac{1+cos\left({\theta }_{N_p}-{\theta }_{\mathrm{d}\mathrm{e}\mathrm{s}}\right)}{2}}$.
10	$\mathit{Smooth}={\displaystyle \frac{1}{N_p-1}}{\sum }_{\mathcal{l}-1}^{N_p-1} {\displaystyle \frac{1+cos\left({\theta }_{\mathcal{l}+1}-{\theta }_{\mathcal{l}}\right)}{2}}$
11	${Clear}^{\ast }=K\left({\displaystyle \frac{1}{1+exp\left(c\left(d_{min}-d_{lim}\right)\right)}}-{\displaystyle \frac{1}{2}}\right)$
12	$\mathit{Velocity}={\displaystyle \frac{v-v_{\mathrm{m}\mathrm{i}\mathrm{n}}}{v_{\mathrm{m}\mathrm{a}\mathrm{x}}-v_{\mathrm{m}\mathrm{i}\mathrm{n}}}}$.
13	$\mathit{RelAngle}={\displaystyle \frac{1+cos\left({\theta }_{\mathrm{des}}-{\theta }_k\right)}{2}}$.
14	$J=\alpha \mathit{Heading}+\beta \mathit{Smooth}+\delta {Clear}^{\ast }+\gamma \mathit{Velocity}+\mu \mathit{RelAngle}$.
15	$\mathrm{If} J>\mathrm{bestScore} \mathrm{then} \mathrm{update} \mathrm{bestScore}, u^{\ast }=(v,\varphi ),tra{j}^{\ast }=\mathsf{\Gamma }$.
16	$\mathrm{Return} u^{\ast },tra{j}^{\ast }$.

4. ACO-DWA Fusion

4.1. Algorithmic Flowchart

The overall procedure of the proposed global–local hybrid planner is summarized in a flowchart (Figure 4). In the global planning stage, the improved ACO constructs the grid graph and initializes both the pheromone distribution and fused heuristic function. Ant paths are iteratively generated through a temperature-controlled transition rule, while pheromone trails are updated locally and globally under bounded values, with a stagnation–restart mechanism to avoid premature convergence. Throughout this process, the best-so-far solution is retained as the candidate global path. To further enhance path quality, line-of-sight pruning and multi-segment polyline refinement are applied to remove redundant waypoints and reduce unnecessary heading changes, yielding a smooth global reference trajectory.

For local tracking, the improved DWA aligns the initial USV heading with the tangent of the first reference segment and constructs the dynamic window according to the vehicle’s kinematic constraints. Within this window, feasible velocity–yaw pairs $(v,\varphi )$ are sampled and rolled out into short-horizon trajectories. Each candidate is evaluated using a composite cost function that balances goalheading alignment, smoothness, clearance, speed preference, and relative bearing. Unsafe trajectories are discarded, and the one with the highest score is executed. A receding update strategy is then adopted: the USV always tracks the sub-path starting from the closest reference waypoint and repeats the local control cycle until the goal is reached or a re-planning condition is triggered.

4.2. Simulation

This section validates the proposed global–local hybrid planner on a 20 × 20 grid environment (Figure 5). The map is generated from a predefined binary matrix, while the start and goal positions are interactively specified by the user. The scenario includes three moving obstacles, each following a polyline trajectory generated by the A* algorithm with a constant step size, as well as several additional static obstacles that are interactively placed to emulate unexpected obstacles detected by onboard sensors.

At the global layer, the improved ACO adopts the inverse squared Euclidean distance to the goal as the base heuristic, combined with time-varying evaporation and global pheromone reinforcement. The algorithm iterates to obtain a feasible discrete route, then applies line-of-sight pruning and multi-stage polyline refinement to reduce turns and produce a smooth global reference path. As illustrated in Figure 5, subfigure (a) shows the three moving obstacles, (b) depicts the initial planned path, and (c) presents the optimized global–local hybrid trajectory.

At the local layer, the improved DWA automatically aligns the initial heading with the tangent of the first reference segment and tracks only the sub-path beginning at the nearest reference waypoint. Under dynamic/unknown obstacles, it samples feasible velocities in real time and evaluates a composite cost that blends heading, clearance, speed, and short-horizon prediction. Figure 6 presents the simulation results of the proposed ACO–DWA fusion algorithm. Subfigures (a)–(i) illustrate the sequential navigation states of the USV during its motion, where the trajectories remain feasible and collision-free under dynamic obstacles.

To further validate the effectiveness and robustness of the proposed planner, we conducted repeated trials under randomized obstacle configurations. Each method was executed 100 times, and performance was evaluated using four metrics: success rate, path length, minimum clearance, and goal time. The statistical results are summarized in

Table 3. Statistical results of different methods (mean ± std; 95% CIs in parentheses).

Method	Success ↑	Path Length ↓ (m)	Min Clearance ↑ (m)	Goal Time ↓ (s)
ACO + DWA	0.88 (0.85–0.90)	30.1 ± 5.4 (28.4–31.8)	0.46 ± 0.10 (0.42–0.50)	118.2 ± 12.3 (113.7–122.7)
AS + DWA	0.90 (0.88–0.92)	29.6 ± 5.1 (28.0–31.2)	0.48 ± 0.10 (0.44–0.52)	116.0 ± 11.9 (111.7–120.3)
ACS + DWA	0.92 (0.90–0.94)	29.1 ± 5.0 (27.5–30.7)	0.50 ± 0.10 (0.46–0.54)	114.6 ± 11.5 (110.4–118.7)
RAS + DWA	0.92 (0.90–0.94)	28.9 ± 4.9 (27.3–30.5)	0.51 ± 0.10 (0.47–0.55)	114.1 ± 11.2 (110.0–118.2)
Ours	0.94 (0.92–0.95)	28.3 ± 4.8 (26.8–29.8)	0.57 ± 0.10 (0.53–0.61)	112.2 ± 10.8 (108.4–116.0)

Note: ↑ indicates that a larger value is better, whereas ↓ indicates that a smaller value is better.

. The proposed Improved ACO–DWA achieved the highest success rate (0.94), the shortest path length (28.3 m), the largest minimum clearance (0.57 m), and the shortest goal time (112.2 s). Compared with the baseline ACO + DWA, our method reduced path length by about 6% and increased minimum clearance by over 20%, while ensuring more efficient and safer navigation. These improvements are statistically significant (p < 0.05).

The improved ACO reaches a stable feasible solution within a small number of generations; after polyline refinement the path becomes significantly shorter and smoother. With the DWA fusion, the USV reaches the goal without collision, exhibiting smooth variations in speed and attitude throughout the motion.

5. Discussion

The integration of a multi-heuristic, temperature-controlled ACO with a kinematically feasible DWA produces trajectories that are short, smooth, and safe, while maintaining real-time responsiveness to dynamic obstacles. The combined heuristics of distance, clearance, and smoothness guide ACO away from wall-following shortcuts and abrupt heading changes; the cosine-annealed schedule stabilizes the exploration–exploitation balance, preventing early stagnation. Pheromone bounding, together with a stagnation–reset mechanism, further mitigates premature convergence.

On the local layer, aligning the initial heading with the first reference segment and tracking the nearest sub-path effectively reduce oscillations and dead ends. The redesigned DWA cost mirrors global objectives—heading consistency, clearance, smoothness, and velocity adaptability—thereby improving the continuity between global and local planning.

Computation remains modest through the use of dynamic ant counts, early stopping, array reuse, and parallel rollout. The DWA component operates at control frequency, enabling real-time deployment. The main limitations are the scalability of dense pheromone storage on large maps, and the absence of explicit modeling for currents and waves. The method also requires careful weight tuning. These limitations could be alleviated by sparse or learned roadmaps, disturbance-aware prediction, and automated parameter adaptation.

6. Conclusions

This paper presented a fused ACO–DWA planner for USV navigation that combines global optimality with local feasibility. The improved ACO generates safe and smooth global corridors, while the DWA executes collision-free tracking in the presence of dynamics and moving obstacles. Simulation experiments demonstrated faster convergence, shorter and smoother paths, and more stable real-time avoidance compared with classical ACO variants.

Future research will focus on improving scalability, modeling environmental disturbances such as currents and winds, automatic weight tuning, incorporation of formal safety guarantees (e.g., barrier functions or chance constraints), multi-USV cooperative planning, and validation on real-world sensor-based USV platforms.