Field Testing of a Stochastic Planner for ASV Navigation Using Satellite Images

• •

  $G=(V,E)$ is an undirected graph.

• •

  $c:E\rightarrow\mathbb{R}_{\geq 0}$ is the cost function for an edge, which is the length of the shortest waterway between two points.

• •

  $p:E\rightarrow[0,1]$ is the blocking probability function. An edge with 0 blocking probability is deterministic; otherwise, it is stochastic.

• •

  $k$ is the number of stochastic edges.

• •

  $s\in V$ is the start and return node.

• •

  $J\subseteq V$ is the subset of target nodes to visit. There are $|J|\leq|V|$ goal nodes.

• •

  $I=\{\text{A},\text{T},\text{U}\}^{k}$ is an information vector that represents the robot’s knowledge of the status of all $k$ stochastic edges. A, T, and U stand for ambiguous, traversable, and untraversable, respectively.

• •

  $S\subseteq J$ is the subset of target nodes that the robot has visited.

• •

  $a$ is the current node the robot is at.

• •

  $x=(a,S,I)$ is the state of the robot. $a$ is the current node, $S$ is the set of visited targets, and $I$ is the current information vector.

• •

  $\pi^{*}$ is the optimal policy that minimizes the cost $\mathbb{E}_{w\sim p(w)}\left[\phi\left(\pi\right)\right]$, where $\phi$ is cost functional of the policy $\pi$ and $w$ is a possible world of stochastic graph, where each stochastic edge is assigned a traversability state.

We extend the AO* search algorithm [101] used in CTP to find exact solutions to our problem. AO* is a heuristic, best-first search algorithm that iteratively builds an AO tree to explore the state space until the optimal solution is found. In this section, we will first explain how to use an AO tree to represent a PCCTP instance, then break down how to use AO* to construct the AO tree containing the optimal policy.

AO Tree Representation of PCCTP The construction of the AO tree is a mapping of all possible actions the robot can take and all possible disambiguation outcomes at every stochastic edge. Following [101], an AO tree is a rooted tree $T=(N,A)$ with two types of nodes and arcs. A node $n\in N$ is either an OR node or an AND node; hence the node set $N$ can be partitioned into the set of OR nodes $N_{O}$ and the set of AND nodes $N_{A}$. Each arc in $A$ represents either an action or a disambiguation outcome and is not the same as $G$’s edges ($A\neq E$). For all $n\in N$, a function $c:A\to\mathbb{R}_{\geq 0}$ assigns the cost to each arc. Also, for all $n\in N_{A}$, a function $p:A\to[0,1]$ assigns a probability to each arc. A function $f:N\to\mathbb{R}_{\geq 0}$ is the cost-to-go function if it satisfies the following conditions:

• •

  if $n\in N_{A}$, $f(n)=\sum_{n^{\prime}\in N(n)}[p(n,n^{\prime})\times(f(n^{\prime})+c(n,n^{\prime}))]$,

• •

  if $n\in N_{O}$, $f(n)=\min_{n^{\prime}\in N(n)}[f(n^{\prime})+c(n,n^{\prime})]$,

• •

  if $n\in N$ is a leaf node, $f(n)=0$.

Now, we can map each node and edge such that the AO tree represents a PCCTP instance. Specifically, each node $n$ is assigned a label $(n.a,n.S,n.I)$ that represents the state of the robot. $n.a$ is the current node, $n.S$ is the set of visited targets, and $n.I$ is the information vector containing the current knowledge of the stochastic edges. The root node $r$ is an OR node with the label $(s,\emptyset,\mathrm{AA...A})$, representing the starting state of the robot. An outgoing arc from an OR node $n$ to its successor $n^{\prime}$ represents an action, which can be either visiting the remaining targets and returning to the start or going to the endpoint of an ambiguous edge via some target nodes along the way. An AND node corresponds to the disambiguation event of a stochastic edge, so it has two successors describing both possible outcomes. Each succeeding node of an OR node is either an AND node or a leaf node. A leaf node means the robot has visited all reachable target nodes and has returned to the start node. Each arc $(n,n^{\prime})$ is assigned a cost $c$, which is the length of travelling from node $n.a$ to node $n^{\prime}.a$ while visiting the subset of newly visited targets $n^{\prime}.S\setminus n.S$ along the way. For all outgoing arcs of an AND node, the function $p$ assigns the traversability probability for the stochastic edge. The cost of disambiguating that edge is its length.

Once the complete AO tree is constructed, the optimal policy is the collection of nodes and arcs included in the calculation of the cost-to-go from the root of the tree, and the optimal expected cost is $f(r)$. For example, the optimal action at an OR node $n$ is the arc $(n,n^{\prime})$ that minimizes the cost-to-go from $n$, while the next action at an AND node depends on the disambiguation outcome. However, constructing the full AO tree from scratch is not practical since the space complexity is exponential with respect to the number of stochastic edges. Instead, we use the heuristic-based AO* algorithm, explained below.

PCCTP-AO* Algorithm Our PCCTP-AO* algorithm (Algorithm 1 and 2) is largely based on the AO* algorithm [110, 165]. AO* utilizes an admissible heuristic $h:N\to\mathbb{R}_{\geq 0}$ that underestimates the cost-to-go $f$ to build the AO tree incrementally from the root node until the optimal policy is found. The algorithm expands the most promising node in the current AO tree based on a heuristic and backpropagates its parent’s cost recursively to the root. This expansion-backpropagation process is repeated until the AO tree includes the optimal policy.

One key difference between AO* and PCCTP-AO* is that the reachability of a target node may depend on the traversability of a set of critical stochastic edges connecting the target to the root. If a target $j\in J$ is disconnected from the current node $a$ when all the stochastic edges from a particular set are blocked, then this set of edges is critical. For example, the two stochastic edges in the top-right graph of Fig. 3 are critical because target node 1 would be unreachable if both edges were blocked. Thus, a simple heuristic that assumes all ambiguous edges are traversable may overestimate the cost-to-go if skipping unreachable targets reduces the overall cost.

Alternatively, we can construct the following relaxed problem to calculate the heuristic. If a stochastic edge is not critical to any target, we still assume it is traversable. Otherwise, we remove the potentially unreachable target for the robot and instead disambiguate one of the critical edges of the removed target. The heuristic is the cost of the best plan that covers all definitively reachable targets and disambiguates one of the critical stochastic edges. For example, consider computing the heuristic at starting node 0 in Fig. 5. The goal is to visit both nodes 1 and 2 if they are reachable. Node 1 is always reachable; hence we assume it is traversable in the relaxed problem. Node 2 may be unreachable, so we remove the stochastic edge (4, 2) and ask the boat to visit Node 4 instead in the relaxed problem. This heuristic is always admissible because the path to disambiguate a critical edge is always a subset of the eventual policy. We can compute this by constructing an equivalent generalized travelling salesman problem [170] and solve it with any optimal TSP solver.

Fig. 4 shows the result of applying PCCTP-AO* to the example problem in Fig. 3. The returned policy (coloured in green nodes) tries to disambiguate the closer stochastic edge $(2,3)$ to reach target node 1. Note that the AO* algorithm stops expanding as soon as the lower bound of the cost of the right branch exceeds that of the left branch. This guarantees the left branch has a lower cost and, thus, is optimal.

We will now explain our procedure to estimate the high-level stochastic graph from satellite images.

Water Masking Our first step is to build a water mask of a water area across a specific period (e.g., 30 days). We use the Sentinel-2 Level 2A dataset [120], which has provided multispectral images at 10 m by 10 m resolution since 2017. Each geographical location is revisited every five days by a satellite. We then select all satellite images in the target spatiotemporal window and filter out the cloudy images using the provided cloud masks. For each image, we calculate the Normalized Difference Water Index (NDWI) [166] for every pixel using green and near-infrared bands. However, the distribution of NDWI values varies significantly across different images over time. Thus, we separate water from land in each image and aggregate the indices over time. We then fit a bimodal Gaussian Mixture Model on the histogram of NDWIs to separate water pixels from non-water ones for each image. We average all water masks over time to calculate the probabilistic water mask at the target spatiotemporal window. Each pixel on the final mask represents the probability of water coverage on this 10 m by 10 m area. If the probability of water for a pixel is greater than 90%, we treat it as a deterministic water pixel. We then classify pixels with a probability lower than 90% but greater than 50% as stochastic water pixels. Finally, we identify the boundary of all deterministic water pixels. Fig. 7 shows an overview of these steps.

Stochastic Edge Detection: Pinch Points We can now identify those stochastic water paths (i.e., narrow straits, pinch points [125]) that are useful for navigation. A pinch point (e.g. Fig. 6(a)) is a sequence of stochastic water pixels connecting two parts of topologically far (or distinct) but metrically close water areas. Essentially, this edge is a shortcut connecting two points on the water boundary that are otherwise far away or disconnected. To find all such edges, we iterate over all boundary pixels, test each shortest stochastic water path to nearby boundary pixels, and include those stochastic paths that are shortcuts. The blocking probability of a stochastic edge is one minus the minimum water probability along the path. Since this process will produce many similar stochastic edges around the same narrow passage, we run DBSCAN [123] and only choose the shortest stochastic edge within each cluster.

Stochastic Edge Detection: Windy Edges The second type of stochastic edges are those with strong wind. In practice, when an ASV travels on a path far away from the shore, there is a higher chance of running into a strong headwind or wave, making the path difficult to traverse. Hence, we define a water pixel to be a windy area if it is at least 200m away from any points of the water boundary. An edge is then treated as a windy edge if it crosses the windy area at some point and we assign a small probability for the event where the wind blocks the edge. An example of a windy area and an associated windy edge is shown in Fig. 6(b).

Path Generation The next step is to construct the geo-tagged path and calculate all edge costs in the high-level graph. The nodes in the high-level graph are composed of all sampling targets, endpoints of stochastic edges, and the starting node. We run A* [137] on the deterministic water pixels to calculate the shortest path between every pair of nodes except for the stochastic edges found in the previous step. Since the path generated by A* connects neighbouring pixels, we smooth them by randomized shortcutting [134]. Then, we can discard any unnecessary stochastic edges if they do not reduce the distance between a pair of nodes. For every stochastic edge, we loop over all pairs of nodes and check if setting the edge traversable would reduce the distance between the pair of nodes. Finally, we check if each deterministic edge is a windy edge and obtain the high-level graph used in PCCTP.

In summary, we estimate water probabilities from historical satellite images with adaptive NDWI indexing and build a stochastic graph connecting all sampling locations and pinch points. The resulting compact graph representing a PCCTP instance can be solved optimally with an AO* heuristic search.

We evaluate our mission-planning framework on Canadian lakes selected from the CanVec Series Ontario dataset [168]. Published by Natural Resources Canada, this dataset contains geospatial data of over 1.1 million water bodies in Ontario. Considering a practical mission length, lakes are filtered such that their bounding boxes are 1-10 km by 1-10 km. Then, water masks of the resulting 5190 lakes are generated using Sentinel-2 imagery across 30 days in June 2018-2022 [120]. We then detect any pinch points on the water masks and randomly sample five different sets of target nodes on each lake, each with a different number of targets. The starting locations are sampled near the shore to mimic real deployment conditions.

Furthermore, we generate high-level graphs and windy edges from the water mask. Graphs with no stochastic edges are removed as well as any instances with more than ten stochastic edges due to long run times. Ultimately, we evaluate our algorithm on 2217 graph instances, which come from 1052 unique lakes.

The simplest baseline is an online greedy algorithm that always goes to the nearest unvisited target node assuming all ambiguous edges are traversable. For a graph with $k$ stochastic edges, we simulate all $2^{k}$ possible worlds, each with a different traversability permutation, and evaluate our greedy actor on each one. The greedy actor recomputes a plan at every step and queries the simulator if it encounters a stochastic edge to disambiguate it. Also, it checks the reachability of every target node upon discovering an untraversable edge and gives up on any unreachable targets.

A more sophisticated baseline is the optimistic TSP algorithm. Instead of always going to the nearest target node, it computes the optimal tour to visit all remaining targets assuming all ambiguous edges are traversable. Similar to the greedy actor, TSP recomputes a tour at every step and may change its plan after encountering an untraversable edge. The expected cost is computed via a weighted sum on all $2^{k}$ possible worlds. In contrast to PCCTP, both baselines require onboard computation to update their optimistic plans, whereas PCCTP precomputes a single optimal policy that is executed online.

Lastly, we modify the CR algorithm, originally a method for CCTP [157], to solve PCCTP. CR precomputes a cyclic sequence to visit all target nodes using the Christofides algorithm [117] and tries to visit all target nodes in multiple cycles while disambiguating stochastic edges. If a target node turns out to be unreachable, we allow CR to skip this node in its traversal sequence.

Fig. 8(a) compares our algorithm against all baselines. To measure the performance across various graphs of different sizes, we use the average expected regret over all graphs. The expected regret of a policy $\pi$ for one graph $G$ is defined as

where $\pi^{p}$ is a privileged planner with knowledge of the states of all stochastic edges, $\phi$ is the cost functional, and $w$ is a possible state of (the stochastic edges of) the graph. The cost $\phi(\pi^{p},w)$, calculated using a TSP solver for each state, serves as a lower bound to the costs incurred by the policy $\phi(\pi,w)$. A low expected regret indicates that the policy $\pi$ will find efficient paths to visit all target locations and disambiguate the stochastic edges without prior knowledge of their states.

PCCTP precomputes the optimal policy in about 50 seconds on average in our evaluation, and there is no additional cost online. Compared to the strongest baseline (TSP), our algorithm saves the robot about 1%(50m) of travel distance on average and 15%(1.8km) in the extreme case. Although the advantage is not statistically significant on average, our planner still offers advantages in many specific scenarios and edge cases, such as those with high blocking probabilities or long stochastic edges. The performance of PCCTP may be further enhanced if the estimated blocking probabilities of the stochastic edges are refined based on historical data.

We also find that the performance gap between our algorithms and baselines becomes more significant with more windy edges. In fact, if the only type of stochastic edges in our graph is pinch points (i.e., the number of windy edges is 0), the performance gap is almost negligible between PCCTP and the optimistic TSP baseline. The main reason is that most pinch points only reduce the total trip distance by hundreds of meters on a possible state of the graph. Pinch points are most likely to be found either on the edges of a lake or as the only water link connecting two water bodies. In the first case, these pinch points are unlikely to be a big shortcut. As for the latter case, if the pinch point is the only path connecting the starting location to a target node, disambiguating this edge has to be part of the policy. On the other hand, windy edges passing through the centre of a lake are often longer, and the gap between the optimal and suboptimal policy is much more significant.

Computational Complexity The worst-case complexity of our optimal search algorithm is $O(|J|!\times k!\times 2^{k})$, which depends on number of stochastic edges $k$ and number of target nodes $|J|$. The complexity is exponential in nature because there are $2^{k}$ possible states of the stochastic edges, and all possible orders to visit all nodes and disambiguate stochastic edges need to be enumerated without a good heuristic in the worst case.

In practice, however, our implementation performs efficiently on standard laptop CPUs. The median runtime of our algorithm, implemented in Python, is less than one second, and 99% of the instances run under 3 minutes. Nonetheless, approximately 0.5% of instances with eight nodes and 4.2% with ten nodes require more than five minutes to process. We believe the runtime can be considerably improved by rewriting in a more efficient language, such as C++. More importantly, we argue that this one-time cost can occur offline before deploying the robot into a water-sampling mission. Although the worst-case runtime of the AO* algorithm can increase exponentially as the graph increases in size, the number of target locations in each graph cannot grow infinitely for real-world water-sampling missions. Hence, the runtime of PCCTP is not a concern for practical applications.

One crucial aspect required for fully autonomous policy execution is the capacity to disambiguate stochastic edges. Our approach is to build a robust autonomy framework (Fig. 9) that relies less on lower-level components such as perception and local planning to execute a policy successfully. In more general terms, the mission planner precomputes the navigation policies from satellite images given user-designated sampling locations. During a mission, the robot will try to follow the global path published by the policy. Sensor inputs from a stereo camera and sonar scans are processed and filtered via a local occupancy-grid mapper. The local planner then tries to find a path in the local frame that tracks the global plan and avoids any obstacles detected close to the future path of the robot. When the robot is disambiguating a stochastic edge, the policy executor will independently decide the edge’s traversability based on the GPS location of the robot and a timer. A stochastic edge is deemed traversable if the robot reaches the endpoint of the prescribed path of this edge within the established time limit. If it fails to do so, the edge is deemed untraversable. There is no explicit traversability check on an ambiguous stochastic edge, such as a classifier or a local map. The timer allows us to address complications we cannot directly sense, such as heavy prevailing winds or issues with the local planner. Following this, the executor branches into different policy cases depending on the outcome of the disambiguation.

We made significant improvements to our local navigation framework compared to our previous work in [142]. Similar to before, the traversability assessment uses a timer and GPS locations to classify an edge’s traversability without directly relying on the result of obstacle detections or local mapping. Instead, we made design decision changes to the local planning architecture and improvements to individual modules. Below, we will explain all the important components and highlight any changes we made.

An experienced human paddler or navigator can easily estimate the traversability of a lake by visually distinguishing water from untraversable terrains, obstacles, or any dynamic objects. In our previous work in [142], the video stream collected from the stereo camera is processed geometrically by estimating the water surface from point clouds and clustering point clouds above the water surface as obstacles. This process was prone to stereo-matching errors due to sun glares and calm water surfaces, and could not detect any obstacles on the water surface such as a shallow rock. To address these shortcomings, we use semantic information from RGB video streams and neural stereo disparity maps to estimate traversable waters in front of the robot and identify obstacles. We learn a water segmentation network and bundle it with a temporal filter to estimate the waterline in image space and remove outliers. The estimated waterline is then projected to 3D using the disparity map and used to update the occupancy grid. We provide more details in the following sections.

Sonar is commonly used as a sensor in maritime applications for both ships and submarines. A specific type, the Blue Robotics Ping360 mechanical scanning sonar, serves as our primary sensing module underwater. It is mounted underwater and operates by emitting an acoustic beam within a forward-facing fan-shaped cone. This beam has a consistent width (1°) and height (20°). The sonar then records the echoes reflected by objects, with the reflection strength relating directly to the target’s density. By measuring the return time and factoring in the speed of sound in water, the range of these echoes can be determined. The sonar’s transducer can also be rotated to control the horizontal angle of the acoustic beam. Configured to scan a 120° fan-shaped cone ahead of the boat, the sonar can complete these scans up to a range of 20m in approximately 3.5 seconds. Additionally, we also have a Ping1D Sonar Echosounder from Blue Robotics that measures water depth. The echosounder is mounted underwater and is bottom-facing. Each sonar scan yields a one-dimensional vector that corresponds to the reflection’s intensity along the preset range. If an obstacle impedes the path of the acoustic beam, it prevents the beam from passing beyond the obstruction, leading to an acoustic shadow. This phenomenon facilitates obstacle detection via sonar scanning.

Fig. 15(a) illustrates a typical sonar scan cycle that detects obstacles. A single sonar scan’s raw and processed data with the resulting detected obstacle are shown in Fig. 15(b). The process begins with the removal of noisy reflections within a close range ($<$2.5m) before smoothing the scan using a moving-average filter. Following this, all local maxima above a specific peak threshold (50) are detected. An obstacle is identified at the first local maxima, where the average intensity post-peak falls below the shadow threshold (5). These thresholds are tuned by hand on data collected from previous field tests in Nine Mile Lake and the stormwater management pond at the University of Toronto.

A post-processing filter removes detections that do not persist across a minimum of $n$ scans (with $n=2$ in our configuration). This is accomplished by calculating the cosine similarity between the current intensity vector and its predecessor. If an obstacle is consistently detected $n$ times, and the cosine similarity across these successive intensity vectors exceeds 0.9, along with spatial proximity, this detected obstacle point is included. In other words, any detections occurring in isolation, either spatially or temporally, are excluded. In our previous work [142], sonar was only used for data collection purposes and not for local planning or navigation. Using scanning sonar, we can significantly improve our ability to detect shallow or underwater obstacles even if sonar operates at a much lower frequency than the stereo camera.

Upon receiving detections from the sonar scans and the stereo camera, they are fused into a coherent local representation to facilitate local path planning and robot control. We utilize the classic occupancy map [122] for our local mapping representation. Unlike a 2D naive cost map used in our previous work [142], an occupancy grid maintains a local map in a principled fashion and naturally filters and smoothes sensor measurements temporally and spatially. The traversability of each cell is determined by naively summing the separately maintained log-odds ratios for sonar and camera. Our occupancy grid is 40m x 40m, with a cell resolution of 0.5m x 0.5m, its centre moving in sync with the robot’s odometry updates. Waterline points, as detected by the stereo camera, are ray-traced in 3D back to the robot, thus lowering the occupied probability of cells within the ray-tracing range. Cells containing or adjacent to waterline points have their occupied probabilities increased. However, points exceeding a set maximum range do not affect occupied probabilities beyond the maximum range due to the decreasing reliability of depth measurements with increasing range. The protocol for updating the log-odds ratios for sonar is similar. Each sonar scan is ray-traced to clear the occupancy grid and marks any cells containing or close to the obstacles. The log-odds ratios of existing cells are decayed with incoming measurement updates, enhancing the map’s adaptability to noisy localizations, false positives, and dynamic obstacles. Finally, we apply a median filter to the occupancy grid to smooth out and remove outliers.

A limitation of this system is that the scanning sonar and the stereo camera observe different sections of the environment. The sonar may detect underwater obstacles invisible to the camera and vice versa for surface-level objects. Fig. 16 provides an example where a shallow rock in the front-right of the ASV is detected by the sonar but missed by the stereo camera. Without ample ground-truth data on the marine environment, reconciling discrepancies between these sensors proves challenging. Traversability estimation, especially in shallow water, is also complicated due to the potential presence of underwater flora (e.g. Fig. 1(c)) or terrain. As a solution, we opt for the simplest fusion method: directly summing the log-odds ratio in each cell. Additionally, we adjust the occupancy grid dilation based on the echosounder’s water depth measurements, increasing the dilation radius when the ASV is in shallower water. The workflow of this strategy is shown in Fig. 16. While this strategy may only present a coarse traversability estimate, it still reliably detects the shoreline despite possible undetected smaller obstacles such as lily pads or weeds. The dilation adjustment employed in shallow water allows the ASV to navigate safely, avoiding prevalent aquatic plants near the shore.

Local path tracking is essential to ensure that the robot adheres to the global mission plan while navigating around obstacles on the local map. The desired controller should run in real-time and work well in obstacle-rich environments. The generated local paths must also be easy for the robot to follow. In our previous work, the robot’s velocities were directly controlled using the Dynamic Window Approach (DWA) [130] to avoid local obstacles. However, DWA only samples a single-step velocity and may fail in cluttered environments with obstacles or cul-de-sacs. Direct tracking of the global path with model predictive control (MPC) is another popular option [145, 119], but solving the optimization problem can be costly because the obstacle avoidance constraint is nonconvex in general. In this paper, we use an alternative strategy that uses a separate path planner to find a collision-free path that connects to the global path and then tracks the collision-free path with an MPC. We employ a modified version of Lateral BIT*, as proposed by [183], to serve as both our local planner and controller. Our approach provides a stronger guarantee as BIT* is probabilistically complete and asymptotically optimal.

This optimal sampling-based planner, set within the VT&R [131] framework, follows an arbitrary global path while veering minimally around obstacles. Lateral BIT* builds upon BIT* [132] by implementing a weighted Euclidean edge metric in the curvilinear planning domain, with collision checks performed against the occupancy grid in the original Euclidean space. Samples are pre-seeded along the whole global path in the curvilinear coordinates before random sampling in a fixed-size sampling window around the robot. The planner operates backward from a singular goal node to the current robot location without selecting any intermediate waypoints. Lateral BIT* is also an anytime planner and can be adapted for dynamic replanning. Once an initial solution is found, an MPC tracking controller can track the solution path. The MPC optimizes the velocity commands in a short horizon to minimize the deviation from the planner solution while enforcing robot kinematic models and acceleration constraints. Adopted from [183], the MPC solves the following least-squares problem:

s.t.

where $\mathbf{T}\in SE(3)$ are poses and $\mathbf{u}=[v\;\omega]^{T}$ are velocities. The objective function minimizes the pose error between the reference trajectory $\mathbf{T}_{\text{ref},k}$ and the predicted trajectory $\mathbf{T}_{k}$ while keeping the control effort $\mathbf{u}_{k}$ minimum. The two constraints are the generalized kinematic constraint and actuation limits. We tune the cost matrices $\mathbf{Q}$ and $\mathbf{R}$ to balance the cost between different degrees of freedom. We refer readers to Sec. V of [183] for more details.

If a newly detected obstacle obstructs the current best solution path, the planner will truncate its planning tree from the obstacle to the robot, triggering a replan or rewire from the truncated tree to the robot’s location. Because the resolution of the satellite map is low (10m/cell), our global path could be blocked by large rocks and terrains, especially the pinch points. Hence, we adjust the maximum width and length of the sampling window and tune the parameters balancing lateral deviation and the path length. If there are no viable paths locally within the sampling window and the planner cannot find a solution after 1 second, the controller will stop the ASV and stabilize it at its current location.

In practice, we cannot directly control the ASV’s linear velocity due to the primary source of translational velocity estimates, GPS data, is noisy and unreliable. Consequently, we map the linear velocity commands to motor thrusts through a linear relationship and close the control loop using the MPC tracking controller. Fig. 17 illustrates an example from the field test where our robot detected obstacles and effectively replanned its trajectory. In the middle image, the lateral BIT* planner finds a smooth path around the obstacle while deviating minimally from the global path. The estimated robot path on the right appears jagged due to significant GPS noise (up to 1m), wind and current influences, and the occupancy grid’s time decay, causing the ASV’s heading to oscillate and repeatedly rediscover the same obstacle. However, the robot successfully bypasses the obstacle without requiring manual intervention. This highlights the robustness and adaptability of our architecture in dynamic, noisy, and unpredictable environments.

Our system’s computational load is divided into offline and online processes (Figure 9). Prior to the mission, we precompute the high-level graph and optimal policy, which are loaded onto the onboard PC. The online tasks are distributed between two onboard computers: the Atom PC and the Jetson. An Ethernet switch connects these computers, the sonar, and Heron’s WiFi Radio. The GPS and IMU are connected to the Atom PC via USB, while the echo-sounder sonar and the stereo camera are connected to the Jetson via USB. The switch allows remote SSH access and data transfer between the Atom and the Jetson. We use the ROS framework [178] to implement our autonomy modules in C++ and Python. To synchronize time between the Jetson and the Atom PC, we employ Chrony for network time protocol setup. The Atom PC acts as the ROS master, responsible for vehicle interface, localization, updating the occupancy grids, running the local planner, and MPC controller. The Jetson handles resource-intensive tasks such as depth map processing, semantic segmentation, sonar obstacle detection, and data logging. Additionally, a ROS node hosting a web visualization page is served on the Atom PC. We also provide a Rviz visualizer to display the occupancy grid and outputs of the local planner and MPC. During the mission, the web server publishes the robot’s locations and policy execution states in real-time on a web page served on the local network, using pre-downloaded satellite maps. The web visualization and Rviz can be accessed in the field from a laptop connected to Heron’s WiFi. The policy executor publishes the global plan to the local planner and starts a timer when navigating a stochastic edge, but importantly does not incur any additional compute cost for planning. We periodically save the status of policy execution online, enabling easy policy reloading in case of a battery change during testing.

We tune the update rates and resolutions of our sensing, perception, and planning modules based on the computational capacities of our Heron and Jetson systems. Specifically, we aim to maintain a sustainable compute load within the thermal and power limits. We avoid pushing our CPUs and GPUs to their absolute limits because doing so can lead to system unreliability and sudden frame rate drops in the field.

Therefore, we set the ZED stereo camera and the neural depth pipeline to publish at 5Hz with a resolution of 640 x 480. The semantic segmentation network, optimized using Nvidia’s TensorRT framework, runs with a latency of less than 50ms. Sonar obstacle detection operates at 20Hz, synchronized with the arrival rate of new sonar scans. The occupancy grid map, with a resolution of 0.5m per cell, updates at 10Hz. The lateral BIT* local planner runs asynchronously in a separate thread, sampling 400 points initially and 150 points in each subsequent batch. The maximum dimensions of the sampling window are 40m in length and 30m in width. The MPC retrieves the planned path and calculates the desired velocity at 10Hz, using a step size of 0.1s and a 40-step lookahead horizon.

Our planning algorithm was evaluated at Nine Mile Lake in McDougall, Ontario, Canada. Detailed test sites and the three executed missions can be found in Fig. 19. The Lower Lake Mission in Fig. 19(a) repeats the field test from our prior work [142], involving a 3.7 km mission with five sampling points, three of which are only reachable after navigating a stochastic edge. The stochastic edge at the bottom-left compels the ASV to manoeuvre through a thin opening amid substantial rocks not discernible in the Sentinel-2 satellite images. Besides repeating the old experiment from our prior work, we added two additional missions in the lake’s upper areas. Also, to assess our local mapping and planning stack’s capabilities, an ablation mission was executed to see if the robot could safely navigate the stochastic edge at the bottom-left of the Lower Lake Mission. The policy in Upper Lake Mission (Short) was directly generated from the Fig. 3 water mask. In fact, the high-level graph in Fig. 3 and policy in Fig. 4 is a simplified toy version of our testing policy in the Upper Lake Mission (Short). The expected length of the policy is 1.0km long. We observed that our NovAtel GPS receiver’s reliability was impaired by large trees on the left stochastic edge in Fig. 19(b). On the right stochastic edges of the same subfigure, shallow regions, lily pads, and weeds were numerous. Lastly, we extended this short mission to include three additional sampling sites and another stochastic edge at the lake’s farthest point. The expected length of this Upper Lake Mission (Long) in Fig. 19(c) is approximately 3.3 km Despite having only 5 nodes, the Upper Lake Mission (Long) is still significant due to the complexity introduced by stochastic edges, resulting in 54 contingencies and a policy tree depth of 12. This demonstrates that even small-scale missions require our proposed approach to generate a robust global policy, and the local planner must effectively manage large uncertainties in traversability to execute the mission safely.

The aim of our field experiments is to test if our autonomy stack can successfully execute a global mission policy correctly and fully autonomously, without any manual interventions. Results are summarized in Table 1 and we provide the overview and analysis of our results below.

Lower Lake Mission We undertook the lower lake mission twice - first, using both sonar and camera and second, using only the camera. The ASV successfully reached 4/5 targeted locations during both trials, with the exception of the bottom-left location. This was due to the ASV’s inability to autonomously navigate through large rocks within the designated time frame. When contrasted with prior experiments noted in [142], our trials showed marked improvement, with only a single manual intervention required due to algorithmic failure during the first run, and none during the second. The intervention was necessitated by the ASV’s collision with a tree trunk (the one in Fig. 1(b)) it failed to identify, resulting in manual manoeuvring to remove the obstruction. In both trials, the policy executor deemed the bottom-left stochastic edge untraversable because the local planner did not find a path through large rocks within the time limit. The ASV was then safely directed back to the last sampling location and starting location. Moreover, these trials demonstrated a significant improvement in the stability of our navigational autonomy compared to the same field test conducted last year. These can be attributed to several factors. First, the inclusion of a new semantic segmentation network for the stereo camera allowed the ASV to navigate confidently even in conditions of high sunlight glare or calm water. This was in contrast to the geometric approach in our previous work, which resulted in both numerous false positives and missing obstacles. Second, sonar detection capabilities facilitated the identification and avoidance of underwater rocks by the local planner. We were also able to fuse both sonar and stereo camera inputs with a local occupancy grid map. Third, through the incorporation of a Model Predictive Control (MPC) tracking controller, the reliance on GPS velocity estimates was removed. Lastly, the decision to use an 88Wh battery on the ASV markedly improved the Jetson’s battery life, thereby negating the need for battery changes during each mission. In Table 2, we show that the Jetson and onboard PC are very power-hungry during one of the testing trials. A microcontroller inside our ASV measures the power of the onboard PC, and we use the jetson-stats tool to log the power of the Jetson. Although the measurement is anecdotal and the exact power consumption can depend on other factors, such as the state of the battery and operating temperatures, the 88WH battery powering the Jetson can certainly last through a two-hour-long experiment.

Upper Lake Mission (Short) We performed four successful tests of this new policy on the upper lake to determine if our robot could execute different policy branches and navigate both sides of the central island, which were visibly passable based on aerial observations. The expected length is 1.0km. The success criteria were defined as either safely traversing the stochastic edges on either side within the assigned time limit or safely returning to the starting point without collisions. Initially, we executed the policy twice without modifications. As depicted in Fig. 19(b), the policy guided the ASV to navigate and return along the left stochastic edge, which had a lower expected cost than the right edge. For the subsequent two trials, we deliberately triggered an early timeout to block the left edge in the policy, forcing the ASV to navigate the right edge.

Throughout the four trials, the ASV executed the mission-level policy fully autonomously, except for a battery change. Navigating the left side was straightforward despite occasional GPS signal disruptions. On the right side, the ASV successfully reached the target area once. However, in the second attempt, it travelled too slowly in a shallow area with many aquatic plants (see Fig. 1(c)) and eventually reached the time limit, rendering the right stochastic edge untraversable. Despite this, the ASV safely returned to the starting node. Importantly, we considered a trial a success despite the ASV not reaching the designated target if the overall policy was executed autonomously. No collisions occurred during any trial.

Upper Lake Mission (Long) We expanded the previous policy to a more extensive mission, covering a larger area of Nine Mile Lake’s upper parts with the same starting point as the shorter mission. The expected length is significantly longer at 3.3km. First, the boat navigated the stochastic edges on the island’s left side to reach the sample point, and it returned using the same path. Despite this, significantly deteriorated GPS signals were observed at the edge’s end, preventing the mission-level policy executor from detecting the completion of the edge traversal due to GPS solution noise. Consequently, a manual restart of the policy executor was necessary. Thereafter, the ASV proceeded upward to the next sample point before making a left turn to go through a shortcut pinch point, visiting two more sample points. Following a brief stop for battery replacement, the ASV completed the remaining mission.

In evaluation, our local perception pipeline performed commendably in this area despite having never previously collected data here. In particular, the synergy of sonar obstacle detection and the stereo camera’s semantic waterline estimation showed high reliability in close-range shoreline and obstacle detection with very minimal false positives. Along with the previous four trials for the shorter mission, we demonstrate that our autonomous navigation architecture is effective not only in familiar environments but also in previously unseen conditions.

A main contribution of our current work is the new perception and local planner modules that can safely disambiguate stochastic edges and navigate safely and autonomously in obstacle and terrain-rich waterways without high-resolution prior maps. To verify this, we tested the local planner on a stochastic edge ten times with the exact same parameter, five times each in either direction. Success was demonstrated by either reaching the stochastic edge’s other endpoint within a set time frame or returning to the starting point upon timeout of the policy executor. Without intervention, the ASV accomplished this 70% of the time. However, in three instances, it collided with or became trapped by obstacles, such as rocks and a tree trunk.

The global path extracted from the Sentinel-2 Image was interrupted by a large rock, with only two narrow openings between the rocks, manually traversable, as demonstrated in Fig. 20 (b). One of the narrow openings is visible from the aerial view in Fig. 20 (d). Our ASV can detect these rocks; however, the over-aggressive dilation parameter obstructs the local planner from charting a path through the central passageway (see Fig. 21). There is another wider opening on top of the visible narrow opening, but it is over 30m away from the nearest point on the global path and thus exceeds the maximum corridor width of our local planner’s curvilinear space.

Relying exclusively on GPS/IMU for location and a local occupancy grid centred around the ASV poses considerable challenges in this terrain, due to imprecision in localizing obstacles relative to the robot and issues controlling tight turns and precise path tracking, escalating the collision risk in confined spaces. In order to mitigate noise and path plan conservatively, occupancy values were decayed over time, and substantial dilation was applied around occupied cells. As such, the ASV would not construct and finetune a consistent local map, but would instead overlook previously encountered obstacles. Consequently, the local planner oscillates between two temporarily obstacle-free paths in the occupancy grid, while the ASV stops and unsuccessfully searches for a traversable path locally until the timer limit is reached, as shown in Fig. 20 (a) and Fig. 20 (c).

Another key reason for the low quality of the occupancy grid is the difficulty of fusing sonar and stereo camera measurements, especially at longer ranges. Since sensor fusion occurs solely within the occupancy map, both sensors need to detect an obstacle simultaneously at the same location in the map for accurate fusion. This can be challenging due to a variety of reasons. For instance, depth measurements produced by the stereo camera tend to be noisier over a larger range. Our camera is not capable of detecting underwater obstacles detected by sonar. Additionally, our system lacks effective uncertainty measures for updating sonar and stereo observations within the occupancy map, especially when the two sources provide conflicting data. For example, the ASV simply did not detect the tree trunk. Thus, our sensor fusion mechanism proves effective only over shorter ranges where the sonar and camera are more likely to align. If it is possible to extend the range of our perception modules, the ASV could formulate more optimized navigation paths, preventing collisions with obstacles such as rocks.