Risk-Sensitive Online Path Planning for AUVs Under Localization and Sensing Uncertainty

Abstract

Online path planning is a core capability of autonomous underwater vehicles (AUVs). Its effectiveness in underwater environments is affected by uncertainty in localization and obstacle sensing. Insufficient consideration of these uncertainties can lead to risky paths or unnecessary detours. To address this challenge, we proposed a risk-sensitive online path planning (RSO-RRT). A Gaussian process occupancy map is constructed to incorporate localization and sensing uncertainty into collision risk assessment. It generates candidate paths by combining a goal-rooted reverse RRT* with local receding-horizon planning. To reduce computational cost, we reuse the existing search tree during the local planning. Candidate paths are evaluated using path length, mean collision risk, and conditional value-at-risk (CVaR) of collision loss. We conduct separate simulations and experiments to evaluate the contributions of individual terms in the weighted objective. Comparisons are also conducted with RRTX, DRRT, RT-RRT, and CC-RRT show that RSO-RRT achieves higher planning success rates, lower maximum collision risk, and greater minimum obstacle clearance under localization and sensing uncertainty. HoloOcean simulations demonstrate online risk-sensitive planning in underwater environments. Indoor experiments further indicate the algorithm’s applicability to other GPS-denied environments.

1. Introduction

Online path planning enables autonomous underwater vehicles (AUVs) to navigate toward targets while avoiding obstacles. In unknown underwater environments, it relies on onboard sensors such as inertial navigation system and sonars to estimate the position of AUVs and surrounding obstacles, respectively [1]. However, AUV position estimates obtained through inertial navigation systems are inherently subject to drift that accumulates over time due to the integration of sensor errors. Sonar observations are affected by acoustic noise, multipath propagation, and limited angular resolution [2,3]. These uncertainties can lead to inaccurate estimates of the AUV’s position relative to obstacles and the mission goals, increasing collision risk and path cost [4]. Increasing safety margins can reduce the risk, but overly conservative margins may lead to unnecessary detours. Therefore, effective path planning requires collision risk assessment under localization and sensing uncertainty, together with a trade-off between collision risk and path cost.

Several existing planning approaches explicitly account for uncertainty when evaluating path safety, including chance-constrained planning, trajectory optimization subject to safety constraints, and belief-space planning [5]. Chance-Constrained RRT (CC-RRT) incorporates collision-probability constraints into sampling-based planning to enforce probabilistic safety requirements during tree expansion [6]. Risk-Averse RRT* with Local Vine Expansion (RA-RRTV*) combines chance constraints with RRT* and local vine expansion to support risk-averse navigation through narrow passages under localization uncertainty [7]. Hakobyan et al. incorporated conditional value-at-risk (CVaR) constraints into model predictive control (MPC) to limit collision-related risk during trajectory tracking in environments with randomly moving obstacles [8]. CVaR-based risk-averse motion planning has also been investigated for quadrotors [9]. Further studies address nonlinear systems, measurement uncertainty, and other robotic platforms [10,11,12,13,14,15]. These methods provide tools for evaluating and constraining risk under uncertainty. However, in unknown underwater environments, new observations may change the estimated AUV state and perceived obstacle positions, requiring candidate-path risk to be reassessed during navigation. The computational cost of this repeated assessment makes replanning efficiency an important consideration.

To improve replanning efficiency, existing methods often reuse and incrementally maintain search structures. Execution-extended RRT (ERRT) accelerates replanning by caching information from previously generated paths [16]. Dynamic RRT (DRRT) supports rapid replanning by pruning invalid branches and reusing the remaining tree structure [17]. RRTX repairs the existing search graph in response to environmental changes, avoiding the need to rebuild it from scratch [18]. Reverse tree guided RRT (RT-RRT) uses a receding-horizon mechanism to update the local search tree as the robot moves [19]. Online RRT* and online FMT* maintain their search structures through local rewiring and root relocation during robot motion [20]. Persistent search trees have also been used in online informative path planning to reduce the tendency to become trapped in local optima due to limited lookahead [21]. Related approaches have been applied to autonomous underwater navigation in unknown environments [22] and to planning in more complex unknown or dynamic environments [23,24,25,26]. However, accounting for both localization and sensing uncertainty requires uncertainty propagation and collision-risk computation during candidate-path evaluation. These computations must be repeated as new observations become available, adding to the computational demands of online replanning. A remaining challenge in unknown underwater environments is therefore to assess path risk while maintaining timely planning responses.

To address this challenge, we propose RSO-RRT for online AUV path planning in such environments. The framework updates the local probabilistic map using incoming observations and incorporates both localization and sensing uncertainty into candidate-path risk assessment. The main contributions are summarized as follows.

1.

We develop RSO-RRT, a risk-sensitive online path planning algorithm for AUVs in unknown environments. Adjustable risk weights are updated online and enable a trade-off between collision risk and path cost.

2.

We develop efficient methods for candidate path generation and risk assessment. Reusing a retained reverse tree during local forward belief-tree expansion reduces replanning overhead. Risk assessment combines mean collision risk with conditional value-at-risk (CVaR) of collision loss to account for both average risk and high-loss collision outcomes.

3.

We validate RSO-RRT through simulations and real-world experiments. The results show that RSO-RRT reduces the occurrence of high-risk collision events in unknown GPS-denied environments and outperforms state-of-the-art methods in both safety and online planning efficiency.

2. Problem Formulation

We consider AUV path planning under localization and sensing uncertainty using a simplified motion model, without imposing vehicle-specific dynamic constraints. Let $X\subset {\mathbb{R}}^n$ denote the AUV state space. In an unknown environment, the true obstacle set $X_{\mathrm{obs}}\subset X$ and the free space $X_{\mathrm{free}}=X\setminus X_{\mathrm{obs}}$ are not known a priori and must be inferred online from local sensor observations. At time t, the AUV has a local sensing radius $R_s$, which defines the local planning region

$$\mathcal{B}({\mu }_t,R_s)=\{x\in X\mid \parallel x-{\mu }_t\parallel \le R_s\},$$

(1)

where ${\mu }_t$ denotes the mean of the current pose, as defined in the next subsection.

We adopt receding-horizon online planning. As shown in Figure 1, at each time t, a set of candidate path segments ${\Pi }_t={\left\{{\pi }_t^i\right\}}_{i=1}^M$ is generated based on the current AUV state $x_{\mathrm{bot}}$ and the local probabilistic occupancy map ${\mathcal{M}}_t$, where M is the number of candidate paths. The candidate path with the lowest risk-aware cost is then selected as

$${\pi }_t^{★}=\arg \underset{{\pi }_t^i\in {\Pi }_t}{\min }J\left({\pi }_t^i\right).$$

(2)

Figure 1. Online risk-sensitive planning of RSO-RRT in an unknown underwater environment: local planning region $\mathcal{B}({\mu }_t,R_s)$ (green dashed boundary), the path selected using the risk-aware cost ${\pi }_t^{*}$ (red solid line), and the path selected using path length alone (blue dashed line).

The AUV then advances one step along the selected path, and newly acquired local observations are used to determine whether replanning should be triggered. The objective is to support online obstacle avoidance in unknown underwater environments under localization and sensing uncertainty while balancing path length and risk.

3. Modeling of Localization and Sensing Uncertainty

3.1. Modeling of Localization Uncertainty

Without informative localization updates, pose-estimation errors can accumulate as the AUV moves. We represent the AUV pose in the belief space using a Gaussian belief [5]:

$$b_t=({\mu }_t,{\Sigma }_t),x_t\sim \mathcal{N}({\mu }_t,{\Sigma }_t),$$

(3)

where ${\mu }_t\in {\mathbb{R}}^n$ is the mean of the AUV state estimate at time t, ${\Sigma }_t\in {\mathbb{R}}^{n\times n}$ is the corresponding state-estimation covariance matrix, and $x_t\in {\mathbb{R}}^n$ denotes the true AUV state at time t.

The AUV motion and observations are represented by the following stochastic models for state estimation and uncertainty propagation:

$$x_{t+1}=f(x_t,u_t)+w_t,z_t=h\left(x_t\right)+v_t,$$

(4)

where $w_t\sim \mathcal{N}(0,Q_t)$ and $v_t\sim \mathcal{N}(0,R_t)$ are zero-mean noise terms. Using a linearized filtering formulation, the covariance prediction and update can be written in the standard form

$${\Sigma }_{t+1}^{-}=F_t{\Sigma }_t{F}_t^{\top }+Q_t,{\Sigma }_{t+1}=(I-K_t{H}_t){\Sigma }_{t+1}^{-},$$

(5)

where $F_t$ and $H_t$ are the Jacobian matrices, $Q_t$ is the process-noise covariance, and $K_t$ is the Kalman gain.

In the absence of informative external measurement updates, the state estimator performs only the prediction step, and the state-estimation covariance is propagated according to the motion model and process noise. For the local candidate paths considered here, we assume that the state-transition characteristics vary only slightly over short-term motion. For online candidate-path evaluation, we use a local approximation in which covariance grows linearly with traveled arc length:

$$\Sigma \left(l\right)={\Sigma }_t+\kappa lI,$$

(6)

where l denotes the accumulated arc length from the current state along a candidate path, $\kappa >0$ is the uncertainty growth rate calibrated from odometry and inertial-navigation error levels, and I is the identity matrix. This model provides a local approximation of covariance propagation in the absence of external measurement updates and is used to reduce the computational cost of online candidate-path risk evaluation.

3.2. Modeling of Sensing Uncertainty

In unknown environments, obstacle occupancy can only be inferred online from noisy local observations. To account for sensing noise, we construct a local probabilistic occupancy map using Gaussian process occupancy mapping (GPOM) with uncertain inputs. Let $o\left(x\right)\in \{0,1\}$ denote the true occupancy state at location x. The latent occupancy function is assigned a Gaussian process prior:

$$f_{\mathrm{occ}}\left(x\right)\sim \mathcal{GP}\left(0,k(·,·)\right),$$

(7)

where $f_{\mathrm{occ}}\left(x\right)$ denotes the latent occupancy function at location x, $\mathcal{GP}\left(0,k(·,·)\right)$ represents a Gaussian process prior with zero mean, and $k(·,·)$ is a squared-exponential kernel that accounts for input covariance.

Unlike conventional GPOM with fixed input noise [27], we define a time-varying uncertain input for the ith range-bearing observation at time t:

$$z_{i,t}=\left[\begin{array}{c}{r}_{i,t}\\ {\theta }_{i,t}\\ {\mu }_{x,t}\\ {\mu }_{y,t}\\ {\varphi }_t\end{array}\right]\sim \mathcal{N}\left({\overline{z}}_{i,t},{\Sigma }_{z,i,t}\right),{\Sigma }_{z,i,t}=\left[\begin{array}{cc}{R}_{i,t}^{\mathrm{meas}}& 0\\ 0& {\Sigma }_t\end{array}\right],$$

(8)

where $R_{i,t}^{\mathrm{meas}}$ is the measurement-noise covariance associated with the current range-bearing observation and is modeled as heteroscedastic:

$$R_{i,t}^{\mathrm{meas}}=\left[\begin{array}{cc}{\sigma }_r^2(i,t)& 0\\ 0& {\sigma }_{\theta }^2(i,t)\end{array}\right],$$

(9)

with

$${\sigma }_r^2(i,t)=a_r+b_r{r}_{i,t}^2,{\sigma }_{\theta }^2(i,t)=a_{\theta }+b_{\theta }{r}_{i,t}^2.$$

(10)

Equation (10) models range and bearing noise as increasing with observation distance. Input uncertainty therefore varies across observations and over time.

The uncertain inputs are propagated through the unscented transform (UT) to obtain training-sample distributions in the world coordinate frame. The resulting posterior of the latent occupancy function at query point x is

$$f_{\mathrm{occ}}\left(x\right)\mid {\mathcal{D}}_t\sim \mathcal{N}\left({\mu }_f\left(x\right),{\sigma }_f^2\left(x\right)\right),$$

(11)

where ${\mathcal{D}}_t$ denotes the training dataset accumulated up to time t.

To obtain a more conservative estimate in regions of high predictive uncertainty, we further define the conservative occupancy probability as

$$\begin{array}{cc}\hfill p_t^{\xi }\left(x\right)& =\Phi \left(a_p\left({\mu }_f\left(x\right)+z_{\xi }{\sigma }_f\left(x\right)\right)+b_p\right),\hfill \\ \hfill z_{\xi }& ={\Phi }^{-1}\left(\xi \right),\xi \in (0.5,1).\hfill \end{array}$$

(12)

This definition uses the upper $\xi$-quantile of the posterior occupancy score. Compared with the mean occupancy probability used in standard GPOM, (12) shifts the estimate toward higher occupancy in regions with large predictive variance, thereby reducing the underestimation of risk.

Accordingly, we define the local conservative probabilistic occupancy map as

$${\mathcal{M}}_t:x\mapsto \left(p_t^{\xi }\left(x\right),{\nu }_t\left(x\right)\right),{\nu }_t\left(x\right)=p_t^{\xi }\left(x\right)\left(1-p_t^{\xi }\left(x\right)\right),$$

(13)

where ${\nu }_t\left(x\right)$ represents occupancy uncertainty and is approximated by the Bernoulli variance.

The local conservative probabilistic occupancy map is then used with the risk-aware cost function to evaluate path costs, accounting for the effects of localization and sensing uncertainty on path selection.

4. Design of the Risk-Aware Cost Function

The risk-aware cost function combines three terms representing path length, mean collision probability, and tail collision loss.

4.1. Mean Collision Risk Under Uncertainty

Given a candidate path segment ${\pi }_t^i$, we sample K path points ${\left\{{\mu }_k\right\}}_{k=1}^K$ along the path at an arc-length interval $\Delta l$, where ${\mu }_1={\mu }_t$. The corresponding covariance at each path point is obtained from (6) as ${\Sigma }_k=\Sigma \left(l_k\right)$. The belief-space collision risk at each path point is defined as the expected occupancy probability:

$$P_{\mathrm{coll}}({\mu }_k,{\Sigma }_k)={\mathbb{E}}_{x_k\sim \mathcal{N}({\mu }_k,{\Sigma }_k)}\left[p_t^{\xi }\left(x_k\right)\right].$$

(14)

Because a closed-form solution to (14) is generally difficult to obtain, we approximate it using Monte Carlo sampling:

$$P_{\mathrm{coll}}({\mu }_k,{\Sigma }_k)\approx {\displaystyle \frac{1}{N}}{\displaystyle \sum _{j=1}^N}{p}_t^{\xi }\left(x_k^{\left(j\right)}\right),$$

(15)

where N is the number of samples at each path point. As shown in Figure 2, a candidate path consists of a sequence of mean nodes ${\mu }_k$. Querying the occupancy probability of each sample from the local probabilistic occupancy map jointly accounts for the effects of localization uncertainty and occupancy uncertainty on collision probability. States closer to regions of high occupancy probability and with greater localization uncertainty therefore have higher collision probabilities.

Figure 2. Monte Carlo estimation of candidate-path collision risk. The grey dashed ellipses illustrate localization uncertainty associated with ${\Sigma }_k$, while the white circles denote Monte Carlo samples $x_k^{\left(j\right)}$.

The mean collision risk of candidate path ${\pi }_t^i$ is then defined as

$${\overline{P}}_{\mathrm{coll}}\left({\pi }_t^i\right)={\displaystyle \frac{1}{K}}{\displaystyle \sum _{k=1}^K}{P}_{\mathrm{coll}}({\mu }_k,{\Sigma }_k),$$

(16)

which characterizes the overall collision tendency of the candidate path within the local planning region. This metric averages the pointwise collision risks within the local planning region but does not quantify collision severity. We therefore introduce a separate tail-loss metric to account for high-loss collision scenarios.

4.2. Tail Collision Loss: SAA-Based CVaR Estimation

To explicitly penalize low-probability, high-loss collision events, we introduce conditional value-at-risk (CVaR) to quantify the tail collision loss [8]. We first define the path-loss random variable $Z\left({\pi }_t^i\right)$ under a given scenario $\omega$. Penetration depth is used to characterize severe collision outcomes. Each uncertain occupancy-map scenario is generated by sampling from the probabilistic occupancy map $M_t$. The spatial correlation between neighboring cells is preserved through the covariance structure of the GP kernel. Given an occupancy-probability threshold $p_{\mathrm{th}}$, a binary obstacle set is generated for each scenario $\omega$ as

$$X_{\mathrm{obs}}^{\omega }=\left\{x\in \mathcal{B}({\mu }_t,R_s)\mid p_t^{\xi }\left(x\right)\ge p_{\mathrm{th}}\right\}.$$

(17)

The penetration depth at any position x is defined as

$$d_{\mathrm{pen}}^{\omega }\left(x\right)=\left\{\begin{array}{cc}\mathrm{dist}\left(x,X_{\mathrm{free}}^{\omega }\right),\hfill & x\in X_{\mathrm{obs}}^{\omega },\hfill \\ 0,\hfill & x\in X_{\mathrm{free}}^{\omega },\hfill \end{array}\right.$$

(18)

where $\mathrm{dist}(·,·)$ denotes the Euclidean distance to the nearest free region. Under scenario $\omega$, the scenario-wise maximum loss along the path, accounting for localization uncertainty, is defined as

$$Z^{\omega }\left({\pi }_t^i\right)=\underset{k}{\max }{d}_{\mathrm{pen}}^{\omega }\left(x_k^{\omega }\right),x_k^{\omega }\sim \mathcal{N}({\mu }_k,{\Sigma }_k).$$

(19)

As shown in Figure 3, a perturbed state $x_k^{\omega }$ is sampled from the Gaussian belief distribution associated with each path node under scenario $\omega$. Its penetration depth relative to the scenario obstacle set $X_{\mathrm{obs}}^{\omega }$ is then computed using (18). Samples in free space have zero penetration depth, whereas samples inside obstacle regions incur a penetration loss determined by their distance to the nearest free region. Equation (19) defines the scenario loss as the maximum penetration depth among the sampled states along the candidate path.

Figure 3. Maximum penetration depth of a candidate path under a single obstacle scenario. Grey circles with black outlines denote the mean path nodes, whereas grey circles with white outlines denote perturbed states sampled from the corresponding Gaussian belief distributions.

For confidence level $\alpha$, ${\mathrm{CVaR}}_{\alpha }$ is defined as the conditional expectation of the worst $(1-\alpha )$ fraction of the loss distribution:

$${\mathrm{CVaR}}_{\alpha }\left(Z\right)=\mathbb{E}\left[Z|Z\ge {\mathrm{VaR}}_{\alpha }\left(Z\right)\right],\alpha \in (0,1).$$

(20)

For online computation, we use sample average approximation (SAA). Given S sampled scenarios with losses ${\left\{Z^{\omega }\left({\pi }_t^i\right)\right\}}_{\omega =1}^S$, sorted as $Z^1\le \cdots \le Z^S$, CVaR is approximated as

$${\mathrm{CVaR}}_{\alpha }\left(Z\right)\approx {\displaystyle \frac{1}{S-k+1}}{\displaystyle \sum _{\omega =k}^S}{Z}^{\omega },k=⌈\alpha S⌉.$$

(21)

The scenario sampling accounts for both (i) GP map uncertainty through map perturbations induced by ${\nu }_t\left(x\right)$ and (ii) localization uncertainty through pose perturbations induced by ${\Sigma }_k$. The resulting tail-loss term explicitly penalizes severe collision outcomes, thereby improving robustness against high-consequence collision events.

4.3. Risk-Aware Cost Function

To jointly account for path efficiency and risk aversion, we define a normalized risk-aware cost for each candidate path ${\pi }_t^i$ in the candidate set ${\Pi }_t$ generated at the current replanning step. For each candidate path ${\pi }_t^i$, let $L_i$ denote its length, $P_i={\overline{P}}_{\mathrm{coll}}\left({\pi }_t^i\right)$ its mean collision risk defined in (16), and $C_i={\mathrm{CVaR}}_{\alpha }\left(Z\left({\pi }_t^i\right)\right)$ its tail collision loss estimated using (21). These terms are normalized as

$$\begin{array}{cc}\hfill {\widehat{L}}_i& ={\displaystyle \frac{L_i-L_{\min }}{L_{\max }-L_{\min }+ϵ}},\hfill \\ \hfill {\widehat{P}}_i& ={\displaystyle \frac{P_i-P_{\min }}{P_{\max }-P_{\min }+ϵ}},\hfill \\ \hfill {\widehat{C}}_i& ={\displaystyle \frac{C_i-C_{\min }}{C_{\max }-C_{\min }+ϵ}}.\hfill \end{array}$$

(22)

Here, $L_{\min }$ and $L_{\max }$, $P_{\min }$ and $P_{\max }$, and $C_{\min }$ and $C_{\max }$ denote the minimum and maximum values of the corresponding metrics within the current candidate set. The small positive constant $ϵ$ prevents numerical instability when the range of a metric approaches zero. The normalized risk-aware cost of candidate path ${\pi }_t^i$ is then defined as

$$J_i=w_L{\widehat{L}}_i+w_P{\widehat{P}}_i+w_C{\widehat{C}}_i,w_L+w_P+w_C=1,$$

(23)

where $w_L$, $w_P$, and $w_C$ are the relative weights assigned to path length, mean collision risk, and CVaR-based tail collision loss, respectively. The weights are used to balance the trade-offs among the three terms.

5. Algorithms

5.1. Algorithm Description

RSO-RRT adopts a two-layer planning mechanism that combines a goal-rooted reverse RRT* with local receding-horizon replanning. The reverse tree extends from the goal region toward the AUV and preserves connections toward the goal. These connections provide candidates for local replanning and allow the existing search structure to be reused, thereby improving online planning efficiency in unknown environments. The main procedure is given in Algorithm 1, and the major subroutines are presented in Algorithms 2–6.

Before the AUV starts moving, a reverse RRT* tree $T^{-}$ is constructed with the goal state $x_{\mathrm{goal}}$ as its root. Once the tree first connects to the start state $x_{\mathrm{start}}$, the initial committed path ${\pi }_{\mathrm{commit}}$ is extracted. The reverse tree is not rebuilt during online execution; instead, it is retained as a goal-directed global search structure for subsequent local replanning. The AUV then begins moving along ${\pi }_{\mathrm{commit}}$, and the main loop terminates when the AUV reaches the goal region.

Algorithm 1 RSO-RRT

1: $T^{-}\leftarrow$ GoalRootedRRT*$(X,x_{goal},x_{start})$,${\pi }_{\mathrm{commit}}\leftarrow$ ExtractPathToGoal$(T^{-},x_{start})$   2: ${\mathcal{M}}_0\leftarrow$ GPOM_Init$\left(X\right)$,$b_1=({\mu }_1,{\Sigma }_1),{\mu }_1=x_{start},t\leftarrow 1$   3: while  $\parallel {\mu }_t-x_{goal}\parallel >d_{\mathrm{goal}}$  do   4:  ${\varphi }_t\leftarrow$ HeadingFromCommit$({\mu }_t,{\pi }_{\mathrm{commit}})$   5:  ${\mathcal{M}}_t\leftarrow$ GPOM_UpdateWithBelief$({\mathcal{M}}_{t-1},b_t,{\varphi }_t)$   6:  if $\parallel x_{goal}-{\mu }_t \parallel \le R_s$ and ¬SegmentHitsOccProb$({\mu }_t,x_{goal},{\mathcal{M}}_t,p_{\mathrm{th}})$ then   7:   ${\pi }_{\mathrm{commit}}\leftarrow [{\mu }_t,x_{goal}]$   8:  else if EnvironmentChanged$({\mathcal{M}}_{t-1},{\mathcal{M}}_t)$ then   9:   ${\pi }_{\mathrm{new}}\leftarrow$ LocalReplan$(T^{-},{\mathcal{M}}_t,b_t,{\varphi }_t,{\pi }_{\mathrm{commit}})$ 10:   if ${\pi }_{\mathrm{new}}\ne \varnothing$ then ${\pi }_{\mathrm{commit}}\leftarrow {\pi }_{\mathrm{new}}$ 11:   else ${\pi }_{\mathrm{commit}}\leftarrow \mathrm{RecoveryStrategy}\left(\right)$ 12:   end if 13:  end if 14:  $({\mu }_{t+1},{\pi }_{\mathrm{commit}})\leftarrow$ MoveOneStepPolyline$({\mu }_t,{\pi }_{\mathrm{commit}},v\Delta t)$ 15:  $b_{t+1}\leftarrow$ BeliefUpdate$(b_t,{\mu }_{t+1})$,$t\leftarrow t+1$ 16: end while

Algorithm 2 GPOM_UpdateWithBelief$({\mathcal{M}}_{t-1},b_t,{\varphi }_t)$

1: $({\mathbf{r}}_t,{\theta }_t,{\mathbf{hit}}_t)\leftarrow$ AcquireRangeBearing$(b_t,{\varphi }_t)$,${\mathcal{D}}_t\leftarrow {\mathcal{D}}_{t-1}$   2: for each measurement i do   3:  $R_{i,t}^{\mathrm{meas}}\leftarrow$ MeasurementCovariance$\left(r_{i,t}\right)$   4:  $({\mu }_i^f,{\mathbf{S}}_i^f)\leftarrow$ UT_MeasToWorld$(r_{i,t}^f,{\theta }_{i,t},b_t,R_{i,t}^{\mathrm{meas}})$,${\mathcal{D}}_t\leftarrow {\mathcal{D}}_t\cup \left\{({\mu }_i^f,{\mathbf{S}}_i^f,-1)\right\}$   5:  if $hi{t}_{i,t}=\mathrm{true}$ then   6:   $({\mu }_i^o,{\mathbf{S}}_i^o)\leftarrow$ UT_MeasToWorld$(r_{i,t},{\theta }_{i,t},b_t,R_{i,t}^{\mathrm{meas}})$,${\mathcal{D}}_t\leftarrow {\mathcal{D}}_t\cup \left\{({\mu }_i^o,{\mathbf{S}}_i^o,+1)\right\}$   7:  end if   8: end for   9: for each  $x_q\in B({\mu }_t,R_s)$  do 10:  $({\mu }_f\left(x_q\right),{\sigma }_f^2\left(x_q\right))\leftarrow$ GPPredict$(x_q,{\mathcal{D}}_t)$ 11:  $p_t^{\xi }\left(x_q\right)\leftarrow$ Equation (12),${\nu }_t\left(x_q\right)\leftarrow$ Equation (13) 12: end for 13: ${\mathcal{M}}_t\leftarrow \{p_t^{\xi }\left(x\right),{\nu }_t\left(x\right)\}$ 14: return  ${\mathcal{M}}_t$

Algorithm 3 LocalReplan$(T^{-},{\mathcal{M}}_t,b_t,{\varphi }_t,{\pi }_{\mathrm{commit}})$

1: ${\mathcal{E}}_t^{\mathrm{inv}}\leftarrow$ PruneReverseEdgesByGP$(T^{-},{\mathcal{M}}_t,p_{\mathrm{th}})$   2: ${\mathcal{V}}_t^{\mathrm{reach}}\leftarrow$ ReachableFromGoal$(T^{-},{\mathcal{E}}_t^{\mathrm{inv}})$   3: ${\mathcal{A}}_t\leftarrow \{v\in T^{-}.V\mid v\in {\mathcal{V}}_t^{\mathrm{reach}},\parallel x_v-{\mu }_t\parallel \le R_s\}$   4: ${\widehat{\mathcal{A}}}_t\leftarrow$ PoissonDiskSelect$\left({\mathcal{A}}_t\right)$   5: $T_t^{+}\leftarrow$ GrowForwardBeliefTree$(b_t,{\mathcal{M}}_t,{\widehat{\mathcal{A}}}_t)$,${\Pi }_t\leftarrow \varnothing$   6: for each  $v\in {\widehat{\mathcal{A}}}_t$  do   7:  ${\pi }_t^{+,v}\leftarrow$ ConnectForwardTree$(T_t^{+},x_v,{\mathcal{M}}_t,p_{\mathrm{th}})$   8:  if ${\pi }_t^{+,v}=\varnothing$ then continue   9:  end if 10:  ${\pi }_t^{-,v}\leftarrow$ ExtractReversePathToGoal$(T^{-},v)$ 11:  ${\pi }_t^v\leftarrow {\pi }_t^{+,v}\oplus {\pi }_t^{-,v}$,${\Pi }_t\leftarrow {\Pi }_t\cup \left\{{\pi }_t^v\right\}$ 12: end for 13: ${\Pi }_t^{\sec }\leftarrow$ SectorFilter$({\Pi }_t,{\varphi }_t,{\theta }_{\max })$ 14: if  ${\Pi }_t^{\sec }\ne \varnothing$  then  ${\Pi }_t\leftarrow {\Pi }_t^{\sec }$ 15: end if 16: ${\pi }_t^{★}\leftarrow$ EvaluateCost_RSO$({\Pi }_t,b_t,{\mathcal{M}}_t)$ 17: return  ${\pi }_t^{★}$

Algorithm 4 EvaluateCost_RSO$({\Pi }_t,b_t,{\mathcal{M}}_t)$

1: for each  ${\pi }_t^i\in {\Pi }_t$  do   2:  $L_i\leftarrow$ PathLength$\left({\pi }_t^i\right)$   3:  $P_i\leftarrow$ MeanCollisionProb_MC$({\pi }_t^i,b_t,{\mathcal{M}}_t)$   4:  $C_i\leftarrow$ TailCollisionLoss_CVaR$({\pi }_t^i,b_t,{\mathcal{M}}_t)$   5:  $({\widehat{L}}_i,{\widehat{P}}_i,{\widehat{C}}_i)\leftarrow$ NormalizeByEquation (22)   6:  $J_i\leftarrow w_L{\widehat{L}}_i+w_P{\widehat{P}}_i+w_C{\widehat{C}}_i$   7: end for   8: $i^{★}\leftarrow \arg {\min }_i{J}_i$   9: return  ${\pi }_t^{i^{★}}$

At each time t during online execution, the AUV updates the GP occupancy map ${\mathcal{M}}_t$ within the sensing region $\mathcal{B}({\mu }_t,R_s)$ using local observations and the current belief state $b_t=({\mu }_t,{\Sigma }_t)$, with covariance propagated according to (6) (Algorithm 2). If the goal lies within sensing range and the straight-line connection passes the map-based collision check, the planner selects this segment as the current execution path. When a change in the local environmental information is detected, LocalReplan() is triggered to update the current path. The replanning process first prunes locally invalid connections from the reverse tree, then generates local candidate paths (Algorithm 3), and selects the locally optimal path according to the risk-aware cost function in (23) (Algorithm 4). The AUV continues executing the selected path in this receding-horizon manner until the goal is reached. The complete online planning process is illustrated in Figure 4.

Algorithm 5 MeanCollisionProb_MC$({\pi }_t^i,b_t,{\mathcal{M}}_t)$

1: $(\left\{{\mu }_k\right\},\left\{l_k\right\})\leftarrow$ SamplePathInCircle$({\pi }_t^i,{\mu }_t,R_s)$   2: if  $K=0$  then   3:  return 0   4: end if   5: for  $k=1$ to  K  do   6:  ${\Sigma }_k\leftarrow {\Sigma }_t+\kappa l_{k}I$   7:  for $j=1$ to N do   8:   $x_k^{\left(j\right)}\sim \mathcal{N}({\mu }_k,{\Sigma }_k)$   9:  end for 10:  $P_{\mathrm{coll}}({\mu }_k,{\Sigma }_k)\leftarrow {\displaystyle \frac{1}{N}}{\sum }_{j=1}^N{p}_t^{\xi }\left(x_k^{\left(j\right)}\right)$ 11: end for 12: ${\overline{P}}_{\mathrm{coll}}\left({\pi }_t^i\right)\leftarrow {\displaystyle \frac{1}{K}}{\sum }_{k=1}^K{P}_{\mathrm{coll}}({\mu }_k,{\Sigma }_k)$ 13: return  ${\overline{P}}_{\mathrm{coll}}\left({\pi }_t^i\right)$

Figure 4. Online receding-horizon replanning of RSO-RRT in an unknown environment. (a) Construction of the goal-rooted reverse tree and motion along the initial path. (b) Generation of candidate connections after local obstacles are detected. (c) Evaluation of candidate paths and update of the current path. (d) Invalidation of part of the original path as new obstacles are observed. (e) Reselection of candidate nodes and generation of a new obstacle-avoidance path. Red crosses mark connections invalidated by newly observed obstacles, and orange arrows indicate the direction of AUV motion along the selected path. Grey dashed lines show the remaining reverse-tree connections toward the goal and, behind the AUV, the previously traversed trajectory. Green dashed lines indicate the continuation of the selected path toward the goal.

Algorithm 6 TailCollisionLoss_CVaR$({\pi }_t^i,b_t,{\mathcal{M}}_t)$

1: $(\left\{{\mu }_k\right\},\left\{l_k\right\})\leftarrow$ SamplePathInCircle$({\pi }_t^i,{\mu }_t,R_s)$   2: if  $K=0$  then   3:  return 0   4: end if   5: for  $\omega =1$ to  S  do   6:  ${\mathcal{X}}_{\mathrm{obs}}^{\omega }\leftarrow$ SampleMapScenario$({\mathcal{M}}_t,p_{\mathrm{th}})$   7:  $Z^{\omega }\left({\pi }_t^i\right)\leftarrow 0$   8:  for $k=1$ to K do   9:   ${\Sigma }_k\leftarrow {\Sigma }_t+\kappa l_{k}I$ 10:   $x_k^{\omega }\sim \mathcal{N}({\mu }_k,{\Sigma }_k)$ 11:   $d_{\mathrm{pen}}^{\omega }\left(x_k^{\omega }\right)\leftarrow$ Equation (18) 12:   $Z^{\omega }\left({\pi }_t^i\right)\leftarrow \max \left(Z^{\omega }\left({\pi }_t^i\right),d_{\mathrm{pen}}^{\omega }\left(x_k^{\omega }\right)\right)$ 13:  end for 14: end for 15: $Z^{\left(1\right)}\le \cdots \le Z^{\left(S\right)}\leftarrow$ Sort$\left({\left\{Z^{\omega }\left({\pi }_t^i\right)\right\}}_{\omega =1}^S\right)$ 16: $k^{★}\leftarrow \lceil \alpha S\rceil$ 17: $C_i\leftarrow {\displaystyle \frac{1}{S-k^{★}+1}}{\sum }_{\omega =k^{★}}^S{Z}^{\left(\omega \right)}$ 18: return  $C_i$

RSO-RRT triggers replanning based on updates to the local environmental information, allowing the planner to respond to risk variations caused by localization and sensing uncertainty in unknown underwater environments.

To improve robustness in highly constrained environments, a failure recovery mechanism is incorporated into the local replanning process. When the forward belief tree cannot establish valid connections with the retained reverse tree within the current local planning region, the local search region is gradually expanded and additional connection nodes are generated until a feasible path is found or the maximum allowable search region is reached. If no collision-free connection can be established after the expansion procedure, the planner returns a planning failure signal to the mission layer. The mission layer may then command the AUV to surface or return along the previously traversed route, depending on the vehicle-level recovery strategy. This mechanism prevents local sampling failures from causing indefinite replanning loops and improves the operational reliability of RSO-RRT in complex underwater environments.

5.2. Occupancy Probability Map Updating

RSO-RRT updates the occupancy probability map ${\mathcal{M}}_t$ online using the current local observations, as detailed in Algorithm 2. For each range-bearing observation, the measurement-noise covariance is first determined from (9) and (10), and then combined with the current localization covariance ${\Sigma }_t$ to construct the uncertain input in (8). The UT propagates the input uncertainty into the world frame to obtain training samples for free and occupied regions.

Within the sensing region $\mathcal{B}({\mu }_t,R_s)$, the GP posterior is computed according to (11), and the conservative occupancy probability $p_t^{\xi }\left(x\right)$ is obtained from (12). The occupancy uncertainty ${\nu }_t\left(x\right)$ is then computed using (13).

GPOM_Init(): initializes the Gaussian Process Occupancy Map using prior environmental information.

SegmentHitsOccProb(): checks whether the path segment between the current belief state and the goal intersects high-occupancy regions in the map.

UT_MeasToWorld(): transforms uncertain range-bearing measurements from the sensor frame to the world frame using the unscented transform.

5.3. Local Replanning

When a change in the local environmental information is detected, RSO-RRT perzhegforms local replanning within the current sensing region, as shown in Algorithm 3.

Once replanning is triggered, the reverse tree $T^{-}$ is first repaired under the current map ${\mathcal{M}}_t$ through invalid-edge marking and goal-reachability filtering. Specifically, locally invalid edges are identified according to the conservative occupancy probability $p_t^{\xi }\left(x\right)$ and the occupancy threshold $p_{\mathrm{th}}$, and only nodes that remain connected to the goal are retained. The connection-node set ${\mathcal{A}}_t$ within the sensing region $\mathcal{B}({\mu }_t,R_s)$ is then extracted and filtered using Poisson-disk selection to obtain the candidate connection-node set ${\widehat{\mathcal{A}}}_t$.

Poisson-disk selection distributes the selected connection nodes across the local search region while limiting clustering, providing diverse connection candidates for subsequent path generation and risk evaluation. Compared with purely random sampling, this strategy improves the coverage of the local search region and provides more diverse candidate paths for subsequent risk evaluation.

A local forward belief tree $T_t^{+}$ is subsequently grown from the current belief state $b_t$ and bridged to valid reverse-tree nodes in ${\widehat{\mathcal{A}}}_t$, forming the candidate path set ${\Pi }_t$. After sector-based directional filtering, the remaining candidate paths are evaluated using the risk-aware cost, and the path with the minimum cost is selected as the new ${\pi }_{\mathrm{commit}}$.

5.4. Risk-Aware Cost Evaluation and Path Selection

Once the candidate path set ${\Pi }_t$ is generated, the mean collision risk (Algorithm 5) and CVaR-based tail collision loss (Algorithm 6) are computed according to (16) and (21), respectively. For each candidate path ${\pi }_t^i$, let $L_i$, $P_i={\overline{P}}_{\mathrm{coll}}\left({\pi }_t^i\right)$, and $C_i={\mathrm{CVaR}}_{\alpha }\left(Z\left({\pi }_t^i\right)\right)$ denote the path length, mean collision risk, and CVaR-based tail collision loss, respectively. The three metrics are normalized over the current candidate set according to (22). The risk-aware cost is then computed using (23) (Algorithm 4), and the path with the minimum cost is selected. If the maximum and minimum values of a metric are equal within the current candidate set, its normalized value is set to zero.

6. Simulations and Experiments

We evaluate the online planning performance of RSO-RRT under localization and sensing uncertainty through numerical simulations, simulations in a virtual underwater environment built in HoloOcean, and indoor experiments. The numerical simulations include an ablation study of the cost terms and risk weights, performance comparisons with baseline algorithms, and evaluations of online planning in unknown static and dynamic environments. The virtual underwater simulations and indoor experiments further examine path selection and obstacle-avoidance behavior under different risk-weight configurations.

6.1. Simulation and Experimental Setup

6.1.1. Baseline Algorithms and Evaluation Metrics

We compare RSO-RRT with RRTX, DRRT, RT-RRT, and CC-RRT to assess both replanning efficiency and safety under uncertainty. RRTX and DRRT are included for their ability to reuse and repair search structures during online replanning. RT-RRT provides a closely related comparison because it also employs a reverse-tree-guided receding-horizon strategy. CC-RRT serves as a baseline for planning under uncertainty through its use of chance constraints. These methods allow us to evaluate RSO-RRT against both efficient online planners and a planner that explicitly accounts for uncertainty. erformance is evaluated using path length, computation time, planning success rate, maximum collision risk, and minimum obstacle clearance. Path length is calculated as the total distance traveled along the executed trajectory. Computation time includes initial search-tree construction and subsequent environmental updates, path replanning, and risk evaluation. A trial is considered successful if the vehicle reaches the goal without collision. Execution terminates upon goal arrival, collision, or failure to find a feasible continuation. Planning success rate is the proportion of trials completed successfully. For each trial, maximum collision risk is defined as the largest pointwise collision-risk estimate along the executed trajectory. Minimum obstacle clearance is the smallest signed distance between the vehicle and the nearest obstacle during execution, with negative values indicating obstacle penetration. Within each comparison, all algorithms use the same environment, start and goal states, sensing conditions, motion constraints, and uncertainty settings.

6.1.2. Numerical Simulation Setup

The numerical simulations follow the localization and sensing uncertainty models introduced in the preceding sections. The main parameter values are listed in Table 1. Local obstacle observations are generated by perturbing the true range and bearing measurements with zero-mean Gaussian noise whose covariance follows the sensing uncertainty model. Localization errors arise from the process and measurement noise in the stochastic state-estimation model. The planner receives noisy local observations and the estimated AUV pose with its associated covariance, rather than the true vehicle and obstacle states. The simulator retains these true states for observation generation and performance evaluation. The AUV travels at a constant speed of 4 m/s.

Table 1. Main algorithm parameters used in the simulations.

Category	Parameter	Value
Environment	Map size	$100\mathrm{m}\times 100\mathrm{m}$
	Map resolution	$1\mathrm{m}$
Motion and sensing	Sensor range $R_{\mathrm{s}}$	$12\mathrm{m}$
	Number of sensing beams	90
	Range noise ${\sigma }_r$	1.0 m
	Bearing noise ${\sigma }_{\theta }$	$5.0^{\circ }$
Localization uncertainty	Initial position uncertainty	0
	Position measurement noise ${\sigma }_p$	1.0 m
	Heading uncertainty ${\sigma }_{\varphi }$	$5^{\circ }$
GP occupancy mapping	Length scale ℓ	3
	Signal variance $s_f^2$	1.0
	Noise variance $s_n^2$	0.35
	Maximum training samples	25,000
Planning parameters	RRT* maximum iterations	2000
	Goal bias probability	0.15
Risk evaluation	Occupancy threshold $p_{\mathrm{th}}$	0.7
	CVaR confidence level $\alpha$	0.9
	Monte Carlo samples	200
	CVaR scenarios	50
Hardware	CPU	i9-13900K
	GPU	RTX5090

(1)

Ablation Study Setup

An ablation study is conducted to examine the individual and combined effects of the mean collision risk and CVaR-based tail collision loss terms. The aim is to assess how each term influences path selection and whether combining them provides a balance between average collision risk and tail collision loss.

The study uses the environment containing irregular polygonal obstacles of different sizes. The obstacle arrangement provides alternative routes with different lengths and obstacle clearances, allowing the influence of the risk terms on path selection to be examined.We compare four weight configurations: $(w_L,w_P,w_C)=(1,0,0)$, $(0.5,0.5,0)$, $(0.5,0,0.5)$, and $(0.2,0.4,0.4)$. The first uses path length alone as the baseline. The other three add mean collision risk, CVaR-based tail collision loss, or both, respectively.

The main simulation and planning parameters are summarized in Table 1. The environment, start and goal states, uncertainty settings, and all planning parameters other than the cost weights are held fixed across configurations. Representative paths are compared to illustrate differences in obstacle-avoidance behavior. Each weight configuration is evaluated over 1000 randomized trials. The obstacle layout and start and goal states remain fixed, while localization errors, sensing noise, and random samples used in planning and risk evaluation vary between trials. For each trial, mean collision risk and CVaR-based tail collision loss are calculated for the local path segment selected at each replanning step. Each metric is then averaged over these steps to obtain one value per trial.

(2)

Comparative Evaluation Setup

The comparative simulations evaluate whether RSO-RRT improves planning success and reduces collision risk under localization and sensing uncertainty, while examining the associated costs in path length and computation time. RSO-RRT is compared with RRTX, DRRT, RT-RRT, and CC-RRT. The comparison uses the same obstacle layout and start–goal configuration as the ablation study. For the baseline comparison, RSO-RRT uses the full cost function with $(w_L,w_P,w_C)$ = (0.2, 0.4, 0.4). The default search budget is 2000 iterations. For the sampling-count comparison between RSO-RRT and RRTX, this budget is varied from 500 to 10,000 rather than held at the default value, while the remaining settings are unchanged. The main simulation and implementation parameters are summarized in Table 1.

Two statistical comparisons are conducted. First, RSO-RRT and RRTX are evaluated at 20 sampling counts ranging from 500 to 10,000 to examine how sampling count affects planning performance and computational cost. Each setting is independently repeated 50 times, yielding 1000 randomized trials. Second, DRRT, RT-RRT, CC-RRT, and RSO-RRT are each evaluated over 1000 independent trials to compare path length, computation time, planning success rate, maximum collision risk, and minimum obstacle clearance.

Within each comparison, all algorithms use the same environment, start and goal states, sensing conditions, and uncertainty models and parameters. The generated paths are assessed using a common evaluation procedure. Planning success rate is calculated as the number of successful trials divided by the total number of trials. Path length, computation time, maximum collision risk, and minimum obstacle clearance are averaged over successful trials.Minimum obstacle clearance is averaged over all trials, including those that terminate in collision.

(3)

Static Environment Setup

The static-environment simulation examines how RSO-RRT updates its path as previously unknown obstacles are detected and how risk-aware path selection affects obstacle clearance and path length. The environment contains stationary obstacles whose locations are not known to the AUV in advance. The AUV navigates toward a fixed goal using observations available only within its local sensing range. As additional obstacles are detected, the planner updates its local environmental representation and reassesses candidate paths. Paths selected using path length alone are compared with those selected using the risk-aware cost function. Both configurations use the same obstacle layout, start and goal states, and localization and sensing uncertainty settings. The main simulation parameters are provided in Table 1.

(4)

Dynamic Environment Setup

The dynamic-environment simulation evaluates how RSO-RRT responds to moving obstacles through online replanning under limited local sensing. The environment contains several obstacles whose positions and motion states are initially unknown to the AUV. These obstacles move along the x- or y-axis at 1–2 m/s, changing the available passages during navigation. The AUV replans when local observations reveal changes in obstacle states. The main simulation parameters are listed in Table 1.

6.1.3. Virtual Environment Simulation Setup

Simulations in a virtual underwater environment are conducted to evaluate online planning under localization and sensing uncertainty and to examine the effect of risk weights on path selection. A 100 m × 100 m × 35 m underwater reef environment is constructed in HoloOcean, containing irregularly shaped obstacles that constrain the available routes between the start and goal. The AUV navigates from the horizontal position (0 m, 0 m) to (100 m, 100 m) at a fixed depth, with a commanded forward speed of 1 m/s. Environmental observations are obtained from the simulated sonar, and the AUV pose is estimated from inertial measurement unit (IMU) measurements. Sensing and localization uncertainty are represented using the models described in the preceding sections. RSO-RRT uses the sonar observations and IMU-based pose estimates to update the local probabilistic occupancy map and evaluate candidate paths. The simulated AUV follows the selected path at a fixed depth, and local replanning is triggered when new sonar observations reveal changes in the surrounding environment.

Three weight configurations are compared: a path-length-only configuration, $(w_L,w_P,w_C)$ = (1, 0, 0); a moderate risk-weight configuration, (0.5, 0.25, 0.25); and a higher risk-weight configuration, (0.2, 0.4, 0.4). The environment, start and goal states, and all other planning parameters are held fixed across configurations. The main algorithm parameters and computing hardware are listed in Table 1.

6.1.4. Indoor Experimental Setup

Complementary experiments are conducted with a tracked mobile robot in an indoor GPS-denied environment to examine applicability beyond underwater scenarios. The robot is equipped with a LiDAR sensor and an onboard controller, and its pose is estimated using LiDAR-based simultaneous localization and mapping (SLAM). RSO-RRT is implemented within the Robot Operating System (ROS) framework and runs on a host computer that communicates with the robot. The occupancy-map resolution is 0.1 m. The robot is controlled at 10 Hz with a constant linear velocity of 0.2 m/s, and collision checking accounts for its approximately 0.14 m × 0.10 m footprint. Cardboard boxes of different sizes serve as obstacles. The platform and obstacle layout are shown in Figure 5. No external global positioning information is used. The robot constructs its environmental representation from LiDAR observations and pose estimates and generates paths online. The three weight configurations defined in Section 6.1.3 are used, while the start and goal positions, obstacle arrangement, and motion parameters remain unchanged.

Figure 5. Comparison of planned paths under different risk-weight configurations. Black polygons represent obstacles, and the green and red markers indicate the start and goal positions, respectively. The orange curves show the resulting paths, and the grey ellipses illustrate localization uncertainty along the paths. The blue dashed circles indicate the sensing range at the current AUV position.

6.2. Results

6.2.1. Effects of Cost Terms and Risk Weights

Figure 5 compares the paths obtained under the four weight configurations. The path-length-only configuration in Figure 5a produces a relatively short path close to obstacle boundaries. Adding the mean collision risk term increases obstacle clearance, as shown in Figure 5b. The configuration combining path length and CVaR-based tail collision loss produces a similar nominal path, as shown in Figure 5c. With both risk terms included, the selected path takes a larger detour and maintains greater clearance, as shown in Figure 5d.

The similar nominal paths in Figure 5b,c provide limited insight into the differences between the two risk terms. Figure 6 therefore compares their statistical effects, showing CVaR-based tail collision loss in Figure 5a and mean collision risk in Figure 5b.

Figure 6. Statistical ablation of the risk terms under different risk-weight configurations. Red plus signs denote outlying observations in the box plots.

Relative to the path-length-only configuration, both risk-aware configurations reduce the evaluated risk measures. The configuration using the mean collision risk term achieves lower mean collision risk, whereas the configuration using the CVaR term achieves lower tail collision loss. The full cost function does not attain the lowest value for either risk metric, but maintains relatively low values for both.

6.2.2. Performance Comparison with Baseline Algorithms

Figure 7 presents the comparison between RSO-RRT and RRTX at different sampling counts. As the number of samples increases, the path lengths of both algorithms decrease and approach stable levels. RRTX produces shorter paths than RSO-RRT over the tested sampling range. RRTX’s tree construction time increases markedly with sampling count, whereas RSO-RRT requires less time to construct its tree over the tested range. The travel-time metric also increases more rapidly for RRTX. RSO-RRT maintains a more stable planning success rate across the tested sampling counts. RRTX exhibits relatively high maximum collision risk and minimum clearance close to zero, with negative clearance values reported in some trials. RSO-RRT achieves lower maximum collision risk and greater minimum clearance.

Figure 7. Statistical performance comparison between RSO-RRT and RRTX under different sampling counts.

Figure 8 summarizes the planning success rates and mean performance metrics for DRRT, RT-RRT, CC-RRT, and RSO-RRT. DRRT and RT-RRT produce shorter average paths and lower average completion times, but their planning success rates are approximately 0.4, and their maximum collision risk approaches 0.6. CC-RRT achieves a planning success rate of approximately 0.9 and a maximum collision risk of approximately 0.3, with greater minimum clearance than DRRT and RT-RRT. Among these four algorithms, RSO-RRT achieves the highest planning success rate, the lowest maximum collision risk, and the greatest minimum clearance. RSO-RRT achieves a success rate close to 1. Across the evaluated trials, its maximum collision risk averages approximately 0.2, and its minimum obstacle clearance averages approximately 4 m. However, its average path length and time cost exceed those of DRRT and RT-RRT and are slightly higher than those of CC-RRT.

Figure 8. Overall performance comparison with other planning algorithms.

6.2.3. Online Planning in an Unknown Static Environment

Figure 9 shows the planning process in the unknown static environment. The blue path is selected using path length alone, whereas the green path is selected using the risk-aware cost function. As previously unknown obstacles enter the sensing range, the planner updates its local environmental representation and reassesses the candidate paths. Compared with the path-length-only configuration, the risk-aware configuration selects paths farther from obstacle boundaries, with greater clearance and a moderate increase in path length. As additional obstacles are detected, the planner updates the selected path, and the AUV eventually reaches the goal in the illustrated run.

Figure 9. Online path planning in an unknown static environment. The blue solid line shows the path selected using path length alone, while the green solid line shows the path selected using the risk-aware cost.

6.2.4. Online Planning in an Unknown Dynamic Environment

Figure 10 shows the planning process in the unknown environment with moving obstacles. The AUV initially follows the planned path and updates its route when local observations reveal changes in obstacle states. In the illustrated run, these path updates allow the AUV to avoid the moving obstacles and reach the goal using local observations.

Figure 10. Online path planning in an unknown dynamic environment. Black circles represent moving obstacles. The blue line shows the currently planned continuation toward the goal.

6.2.5. Virtual Underwater Simulation Results

Figure 11b–d shows the paths obtained under the three weight configurations. The path-length-only configuration produces a more direct route with smaller obstacle clearance. The moderate risk-weight configuration selects a path farther from obstacle-dense regions. With higher risk weights, the AUV takes a more pronounced detour and maintains greater clearance. All three configurations complete the illustrated navigation task without collision.

Figure 11. Path planning experiment in the HoloOcean underwater environment. (a) Simulation environment and vehicle platform; red and green markers denote the start and goal, respectively. (b–d) Paths obtained with $(w_L,w_P,w_C)=(1,0,0)$, $(0.5,0.25,0.25)$, and $(0.2,0.4,0.4)$, respectively. Yellow curves show the vehicle paths, blue wireframe lines outline the simulation domain, and white dimension lines indicate its size. Red frames and connecting lines identify enlarged views of the vehicle.

6.2.6. Indoor Experimental Results

Figure 12b–d shows the execution results under the three weight configurations. With path length as the only cost term, the robot follows a shorter path close to obstacles. Although the path is collision-free on the nominal map, a collision occurs during execution in the presence of localization drift and sensing errors. The moderate risk-weight configuration produces a path with greater obstacle clearance. Under the higher risk-weight configuration, the robot selects a more open outer passage, increasing path length while maintaining a larger distance from obstacles. A demonstration of the underwater simulations and indoor experiments is provided in Supplementary Video S1.

Figure 12. Path planning experiment in a real-world indoor environment. (a) Experimental setup, robot platform, and obstacle layout. (b) Execution with $(w_L,w_P,w_C)=(1,0,0)$, in which a collision occurs. (c) Execution with $(w_L,w_P,w_C)=(0.5,0.25,0.25)$. (d) Execution with $(w_L,w_P,w_C)=(0.2,0.4,0.4)$. Blue curves show the robot trajectories overlaid on the scene images, while red curves in the occupancy-map insets show the corresponding paths in map coordinates. Red connecting lines identify enlarged views, and red dimension annotations indicate distances in the experimental setup.

Figure 13 compares the ground-truth nearest-obstacle distances along the executed paths. Under the path-length-only configuration, the distance remains relatively small and approaches zero at several locations. The moderate risk-weight configuration increases obstacle clearance, whereas the higher risk-weight configuration maintains larger distances over the middle and later portions of the path. These distance profiles are consistent with the differences in the executed trajectories.

Figure 13. Comparison of ground-truth nearest-obstacle distances under different risk weights.

6.3. Discussion

6.3.1. Effects of the Risk Terms

The two risk terms describe different aspects of uncertain path outcomes. Mean collision risk averages the pointwise risk along a candidate path, whereas CVaR evaluates the high-loss tail of the scenario-wise maximum penetration depth. Consequently, similar nominal paths can differ in their responses to position and obstacle uncertainty. Including both terms allows average collision risk and high-loss outcomes to influence path selection, with their relative importance controlled by the weights.

6.3.2. Comparison with Existing Methods

Table 2 summarizes the planning capabilities explicitly addressed in the cited formulations. Incremental and receding-horizon planners emphasize efficient path updates, probabilistic safety planners incorporate uncertainty through safety constraints, and tail-risk-aware planners account for high-loss outcomes. RSO-RRT combines online search-structure reuse with path evaluation under localization and sensing uncertainty.

Table 2. Qualitative comparison of representative planning methods.

Planning Method	Online Replanning	Uncertainty Handling	Risk-Aware Planning	Tail-Risk Modeling	Unknown Environment
Incremental online planners [17,18]	✓				✓
Probabilistic safety planners [6,7]		✓	✓
Tail-risk-aware planners [8]		✓	✓	✓
Receding-horizon planners [19,20]	✓				✓
RSO-RRT	✓	✓	✓	✓	✓

The comparative results reveal the costs and benefits of this combination. DRRT and RT-RRT generate shorter paths with lower time costs, but exhibit lower success rates and smaller clearances under the tested conditions. CC-RRT improves safety by accounting for uncertainty. RSO-RRT achieves further improvements in the reported safety metrics, while incurring longer paths and additional computation. GP map updates and risk sampling contribute to this overhead. Search-tree reuse reduces replanning time but does not fully eliminate the computational cost of risk evaluation.

6.3.3. Applicability and Limitations

The virtual underwater simulations show that changing the risk weights alters route selection and obstacle clearance. The indoor experiments provide complementary evidence under real sensing and localization errors: the path-length-only configuration encounters a collision, whereas the risk-weighted configurations maintain greater clearance. These observations support the applicability of RSO-RRT to other GPS-denied environments. Further validation is needed for physical AUVs with vehicle-specific dynamics, as the underwater simulations use fixed-depth planning. The current weights are also predefined; adapting them to changing uncertainty and environmental conditions remains a direction for future work.

7. Conclusions

This paper proposes RSO-RRT, a risk-sensitive online path planning algorithm for AUVs in unknown underwater environments. The algorithm combines Gaussian process occupancy mapping with a goal-rooted reverse RRT* and local receding-horizon replanning to update environmental information and reuse the existing search structure. Localization and sensing uncertainty are incorporated into candidate-path risk assessment, and a weighted objective combines path length, mean collision risk, and CVaR-based tail collision loss. Adjustable risk weights allow the planner to select paths that reflect different risk preferences. Comparative simulations show that RSO-RRT reduces collision risk and improves planning success rates under the tested uncertainty conditions. HoloOcean simulations demonstrate online risk-sensitive planning in underwater environments, while indoor mobile robot experiments provide complementary evidence of applicability to other GPS-denied environments under real sensor noise and localization errors. Future work will extend uncertainty propagation and risk assessment to account for vehicle-specific dynamics and more complex marine environments, while investigating adaptive risk-weight adjustment based on local obstacle density, uncertainty levels, and mission requirements to enable the planner to dynamically balance safety and efficiency as environmental conditions change.