The purpose of this tutorial is to demonstrate the use of software repositories whelk and conch to generate an energy efficient path for an autonomous surface vehicle (ASV).
Consider the following planning mission. You are located on a vessel in Boston Harbor and are going to deploy an ASV that will navigate to a target location. The vehicle is equipped with an onboard controller capable of maintaining a heading and desired speed to reach a target location. To effectively navigate to the target, the controller requires a plan: a sequence of waypoints that form an obstacle-free path from vehicle’s current location to the goal. The following figure shows the ASV's start and goal locations.
(Modified from Google Maps)
Clearly the vessel cannot travel in a straight line to reach the goal - Spectacle Island is in the way. The most obvious planning criteria is that the path should be feasible: the vehicle cannot navigate through obstacles. We also prefer the path to be, at least in some sense, good. That is, a path that is optimal or near-optimal for some criteria. A shortest-distance path is an obvious choice, but often not the best for vehicles operating in the marine environment. Wind, waves, and water currents can substantially impact a vessel's efficiency. Ideally, the vehicle would avoid going against forces that oppose it and instead take advantage of those that are headed toward the goal. Even if this means deviating from the shortest path, it may be much more energy efficient which is highly desirable considering the limited energy of ASVs.
If we have a vector field of the spatio-temporal water currents over the extent of the mission, then we can plan a path that minimizes the energy expenditure of the ASV. The problem is that the data does not exist: we cannot completely predict the state of the complex marine environment. Instead, we rely on forecasts to act as our best guess as to how the currents will behave at least in the near future. For example, the Northeast Coastal Ocean Forecast System (NECOFS). In this tutorial, we will use the whelk repository to download NECOFS data for a specified region and time duration, then convert it to a raster (grid) format useful for path planning.
If we have a map of Boston Harbor and the local water current forecasts, then we should be able to plan a route for energy-efficient and obstacle-free ASV navigation. In this tutorial, we will use planning software in the conch repository to do so.
Now consider why the ASV is going to that goal location. In our fictitious scenario, suppose that the vehicle is going to use its onboard sensors to collect detailed data on an eelgrass habitat at that location. Now suppose that the vehicle's energy efficient route has the vehicle navigating close to other patches of seagrass along the way. Without explicitly targeting those patches, we might get more useful data from the mission if the waypoints were slightly modified so that the ASV passes over and samples those patches. We call this opportunistic reward-based planning. Unlike coverage planners that seek to maximize the sampling reward within time/energy constraints, we are proposing to still perform a point-to-point planning but with slightly relaxed efficiency constraints to take advantage of nearby sampling opportunities. In this tutorial, we will demonstrate using a raster grid of reward values to achieve opportunistic reward-based planning. For simplicity, we will rely on a synthetic reward grid. But it is based on real-world applications. Consider using satellite imagery to get coarse visual data on habitat locations, then following up with the ASV for detailed water measurements and underwater imagery.
This tutorial will explain:
- How to acquire the input data to represent the planning environment (map of obstacles, water current forecasts, & reward)
- How to use Visibility Graphs and Particle Swarm Optimization to generate a solution path
The environment is represented using raster (gridded) data. The values of the cells are used to determine the fitness of a path. The rasters must align! For example, coordinate (100, 75) must refer to the same geographic location in all of the input rasters. This is what enables the planner to determine the obstacles, forces, and reward at a given raster location.
$ mkdir asv_planning
$ cd asv_planning
$ git clone git@github.com:ekrell/whelk.git
$ git clone git@github.com:ekrell/conch.git
Check the whelk and 'conch' repos to see what dependencies they require.
A map of the environment is the only required data input. By itself, you can plan a collision-free, shortest-distance path.
A raster of the Boston Harbor planning environment is included at in the conch repo (test/inputs/full.tiff).
The file format is a GeoTiff:
it contains a georeferenced raster dataset and associated geographic metadata.
For path planning with conch, the values of this raster need to be 0 and 1.
This is called an occupancy grid
where 0 indicates a free cell (here, water) and 1 is an obstacle.
Be careful that the free space includes only water deep enough for your vessel's draft!
How to create one? There are various ways. I manually created the Boston Harbor raster using QGIS. I simply used the polygon tool to outline the coast and islands, then converted to raster.
Another idea: take advantage of the NIR-absorption properties of water to convert satellite imagery to occupancy grids. Just be careful with the depth...
Now we will use whelk to generate a raster of water current forecasts.
So that we can see the entire forecast in a 2D plot, we will use a single forecast time step.
Use the geospatial metadata from the region map to set the correct options to generate compatible forecasts. Specifically, we need to know the world coordinates (lat, lon) of the top-left and bottom-right corners as well as the number of raster rows and columns.
$ gdalinfo conch/test/inputs/full.tif
Driver: GTiff/GeoTIFF
Files: conch/test/inputs/full.tif
Size is 1259, 1134
Coordinate System is:
GEOGCS["WGS 84",
DATUM["WGS_1984",
SPHEROID["WGS 84",6378137,298.257223563,
AUTHORITY["EPSG","7030"]],
AUTHORITY["EPSG","6326"]],
PRIMEM["Greenwich",0],
UNIT["degree",0.0174532925199433],
AUTHORITY["EPSG","4326"]]
Origin = (-71.019278655999997,42.400803791999998)
Pixel Size = (0.000133352949960,-0.000133422306878)
Metadata:
AREA_OR_POINT=Area
Image Structure Metadata:
INTERLEAVE=BAND
Corner Coordinates:
Upper Left ( -71.0192787, 42.4008038) ( 71d 1' 9.40"W, 42d24' 2.89"N)
Lower Left ( -71.0192787, 42.2495029) ( 71d 1' 9.40"W, 42d14'58.21"N)
Upper Right ( -70.8513873, 42.4008038) ( 70d51' 4.99"W, 42d24' 2.89"N)
Lower Right ( -70.8513873, 42.2495029) ( 70d51' 4.99"W, 42d14'58.21"N)
Center ( -70.9353330, 42.3251533) ( 70d56' 7.20"W, 42d19'30.55"N)
Band 1 Block=1259x1 Type=Float64, ColorInterp=Gray
NoData Value=nan
Based on the above, we know that the top-left corner is (-71.0192787, 42.4008038)
and the botton-right is (-70.8513873, 42.2495029). The dimensions of the raster
is 1134 rows and 1259 columns.
We will use a script from the whelk repository to extract the required ocean model data.
The forecast date-time is specified based on the url of the netcdf product.
The following can be used to always access the latest forecast: http://www.smast.umassd.edu:8080/thredds/dodsC/FVCOM/NECOFS/Forecasts/NECOFS_FVCOM_OCEAN_MASSBAY_FORECAST.nc
Generate the forecast rasters:
$ python3 whelk/NECOFS/necofs2raster.py \
--nc http://www.smast.umassd.edu:8080/thredds/dodsC/FVCOM/NECOFS/Forecasts/NECOFS_FVCOM_OCEAN_MASSBAY_FORECAST.nc \
--bounds -71.0192787,-70.8513873,42.2495029,42.4008038 \
--rows 1134 --cols 1259 \
--times 1 \ # Number of time steps. With NECOFS, temporal resolution is hourly
--geotiff_prefix forecast \ # How each of the output raster filenames should begin
--plot forecast.png # Will plot the data for handy reference (make sure you got what you wanted)
List the files to see the outputs.
$ ls
conch forecast_height.tiff forecast.png forecast_salinity.tiff forecast_uwater.tiff forecast_vwater.tiff whelk
What are these?
forecast_height.tiff: terrain height. (With a binary threshold based on boat's draft, should get an occupancy grid... haven't tried it yet).forecast_salinity.tiff: salinity values.forecast_uwater.tiff: u-component of water current vector field.forecast_vwater.tiff: v-component of water current vector field.
View the plot: forecast.png
However, conch expects the water currents rasters to be in terms of magnitude and direction.
I made a utility to handle the conversion. The utility is called magdir2uv.py, which sounds
like the opposite of what we want. But there is the --reverse option that makes it do the opposite.
$ python3 conch/tools/magdir2uv.py \
-u forecast_uwater.tiff -v forecast_vwater.tiff \ # What you have
-m forecast_mwater.tiff -d forecast_dwater.tiff \ # What you need
--reverse
$ ls
conch forecast_dwater.tiff forecast_height.tiff forecast_mwater.tiff forecast.png
forecast_salinity.tiff forecast_uwater.tiff forecast_vwater.tiff whelk
A sample reward raster is included in the conch repo (test/inputs/reward.txt).
This is a simple ASCII txt file where each number represents the reward value.
Again, it is critical that the dimensions (rows, columns) match the region raster.
The reward values are plotted on the map above. The white pixels are lower reward and darker purples are more.
Where you see the ocean, there is no reward.
I made this file manually with a text editor. In real life, reward values could originate from satellite imagery, ocean model outputs, previous ASV missions, etc.
Inputs: region map.
python3 conch/planners/metaplanner.py \
-r conch/test/inputs/full.tif \ # Input region raster
--sy 42.32343 --sx -70.99428 \ # Start coordinates
--dy 42.33600 --dx -70.88737 \ # Goal coordinates
--speed 0.5 \ # Target boat speed
--generations 500 \ # Number of PSO generations
--pool_size 100 \ # PSO pool size
--num_waypoints 5 \ # Number of waypoints
--map dist.png \ # Output plot of path
--path dist.txt # Output path waypoints
Plot the path:
python3 conch/tools/plot_plan.py \
-r conch/test/inputs/full.tif \
--sy 42.32343 --sx -70.99428 \
--dy 42.33600 --dx -70.88737 \
--path dist.txt \
--map figure_distance_plot.png
Inputs: region map, water currents.
The water currents introduce additional challenge. Their complex spatio-temporal structure yields a complex search space. There are likely to be many opportunities for PSO to fall into a local optima. You might get lucky and get a solution that approaches the optimal or end up with a very inefficient path.
In our experiments, we found that PSO convergence was much more reliable when combined with a Visibility Graph. This is a graph representation of the environment where the only paths that can be made are those that touch obstacle corners. This is because shortest-distance paths will touch the corners (unless there is no obstacle blocking the destination, in which case the plan is trivial anyway).
But we are no longer performing shortest distance planning, so how can this help? We have no guarantees that the energy-efficient path will touch corners, in fact it seems unlikely. We need to rich search space of the entire grid to make strategic waypoint selection w.r.t. the water currents.
Metaheuristic search algorithms, like Particle Swarm Optimization, typically start with a random set of candidate solutions. Then, for many iterations, these solutions are modified in some way to optimize the fitness. Instead of using a random initial population, we found that we could generate a set of feasible paths from the Visibility Graph to serve as the initial population. Since these are not the final path, we simplify the shapes of the polygons somewhat to save computation time. Highly complex polygons can yield massive Visibility Graphs.
Solving a set of paths (using A*) using the Visibility Graphs yields a initial population of obstacle-free paths that are a good starting point for PSO. Clearly this step must be fast, or you might as well just solve the problem with a classic algorithm like Dijkstra's.
python3 conch/planners/vgplanner_build.py \
-r conch/test/inputs/full.tif \ # Input region raster
-g vg.graph \ # Output Visibility Graph
-s poly.shp \ # Shapefile of simplified obstacle polygons
-m poly.png \ # Plot of simplified obstacle polygons
-v vg.png \ # Plot of Visibility Graph
-n 4 \ # Number of parallel workers
--build # Actually build it, instead of only showing the polygons
python3 conch/planners/vg2g.py \
-r conch/test/inputs/full.tif \ # Input region raster
-v vg.graph \ # Input Visibility Graph
--sy 42.32343 --sx -70.99428 \ # Start coordinates
--dy 42.33600 --dx -70.88737 \ # Goal coorindates
-o vg.pickle # Output Visibility Graph
python3 conch/planners/getNpaths.py \
-r conch/test/inputs/full.tif \ # Input region raster
--graph vg.pickle \ # Input Visibility Graph
--n_paths 100 \ # Number of paths to solve
--sy 42.32343 --sx -70.99428 \ # Start coordinates
--dy 42.33600 --dx -70.88737 \ # Goal coorindates
--paths initpop.txt
python3 conch/planners/metaplanner.py \
-r conch/test/inputs/full.tif \ # Input region raster
--currents_mag forecast_mwater.tiff \ # Input water current magnitudes
--currents_dir forecast_dwater.tiff \ # Input water current directions
--init_pop initpop.txt \ # Input initial population
--sy 42.32343 --sx -70.99428 \ # Start coordinates
--dy 42.33600 --dx -70.88737 \ # Goal coordinates
--speed 0.5 \ # Target boat speed
--generations 500 \ # Number of PSO generations
--pool_size 100 \ # PSO pool size
--num_waypoints 5 \ # Number of waypoints
--map energy.png \ # Output plot of path
--path energy.txt # Output path waypoints
Plot the path:
python3 conch/tools/plot_plan.py \
-r conch/test/inputs/full.tif \
-u forecast_mwater.tiff \
-v forecast_dwater.tiff \
--sy 42.32343 --sx -70.99428 \
--dy 42.33600 --dx -70.88737 \
--path energy.txt \
--map figure_energy_plot.png
The path takes a longer route to avoid the strong currents that are present along the shortest-distance path.
Inputs: region map, water currents, reward.
Now, we will combine the water currents and reward for a multi-object task: opportunistic reward-based planning.
python3 conch/planners/metaplanner.py \
-r conch/test/inputs/full.tif \ # Input region raster
--currents_mag forecast_mwater.tiff \ # Input water current magnitudes
--currents_dir forecast_dwater.tiff \ # Input water current directions
--reward conch/test/inputs/reward.txt \ # Input reward raster
--init_pop initpop.txt \ # Input initial population
--sy 42.32343 --sx -70.99428 \ # Start coordinates
--dy 42.33600 --dx -70.88737 \ # Goal coordinates
--speed 0.5 \ # Target boat speed
--generations 500 \ # Number of PSO generations
--pool_size 100 \ # PSO pool size
--num_waypoints 5 \ # Number of waypoints
--map reward.png \ # Output plot of path
--path reward.txt # Output path waypoints
Plot the path:
python3 conch/tools/plot_plan.py \
-r conch/test/inputs/full.tif \
-u forecast_mwater.tiff \
-v forecast_dwater.tiff \
--reward conch/test/inputs/reward.txt
--sy 42.32343 --sx -70.99428 \
--dy 42.33600 --dx -70.88737 \
--path reward.txt \
--map figure_reward_plot.png
The path makes minor adjustments to the energy-efficient path to intersect a couple of regions with high reward.
This tutorial demonstrated how to use conch for PSO path planning as well as how to get water currents suitable for energy-efficient planning.
This is not an exhaustive documentation of all that can be done with conch. The metaplanner.py utility has a plethora of options.
The user could modify the weights of the planning objects, for example to increase the planner's emphasis on reward.
Other metaheuristic algorithms are available (via the PaGMO library) such as Genetic Algorithm and Artificial Bee Colony.
You can also use Dijkstra instead, whose reliability (deterministic, guaranteed optimality) gives it a lasting appeal despite high computational cost.
There are many opportunities for enhancements. We use spatio-temporal water currents, but why not doing the same with the region (i.e. changing obstacles with tide) or reward (i.e. drifting scientific sampling targets). Also, we maintain a constant target speed. While common in path planning, we might want to go slower over high-reward sampling regions, but speed up to meet time constraints. We encourage exploration: feel free to make pull requests with your new features.