Waypoint Navigation and Sailing Algorithm

This post describes the underlying operation of the Sailing algorithm built into the DroneLink platform.  The functionality is spread across a number of logical modules, allowing for flexible usage in different types of devices, for example, motorboats requiring waypoint navigation, but not sailing logic.

Waypoint Navigation

All our boats (and other mobile robots) require basic waypoint navigation, implemented in the Waypoint module.  The module loads a list of lon/lat coordinates and associated target radii from a CSV stored in the ESP32 flash filesystem (waypoint.csv).  An example CSV and the resulting visualization is shown below for 4 waypoints:

The Waypoint module subscribes to the current lon/lat location, from the GPS module, and uses this to calculate once the boat has come within the target distance of the current waypoint (10m radius for the first waypoint in the example above).  Once the target radius is reached, the next waypoint from the file will be selected.  The current waypoint index is stored in EEPROM on the ESP32 each time it changes, which allows for the course to be resumed in the event of a power outage or unexpected device reset.  

The Waypoint module also has a loop mode, whereby it will automatically return to the first waypoint in the file once the last waypoint is reached.  This is useful for endurance testing, where we want to have a boat repeat a set course indefinitely. 

The Waypoint module publishes the target location of the current waypoint so that other modules can use this for navigation.

Example parameters for a Waypoint module in operation:

Navigating to a Waypoint

The Nav module is responsible for actually navigating to a specific target location.  That target location can be:

• Supplied by subscription to the Waypoint module target parameter for automated navigation
• Manual input from the Web UI
• Supplied by subscription to another boat's GPS location for automated following/tracking behaviour (we use this function for the antenna tracker)

Like the Waypoint module, the Nav module also subscribes to the current lon/lat location from the GPS module.  Using these inputs it calculates and publishes the distance and heading to the target location for other modules to use.

When the target location changes, the Nav module will store the current location as the last location value.  This is used for the compensation features described below, for the sailing algorithm, and also for visualising the navigation progress in the web UI.

The Nav module has all the required functionality for a robotic motorboat (like the Paddles or Kit), but is insufficient for a sailboat, hence the next section describing the Sailor module.

The Nav module also contains some more advanced functionality that can be optionally enabled:

• Drift compensation
• Crosswind compensation
• Alternate navigation modes

Drift Compensation

Drift compensation is intended to keep the boat tracking close to the line between the last and target locations.  The distance from this ideal line is called the crosstrack distance.  The Nav module calculates and publishes this as a normalised value representing the ratio between the crosstrack distance and the target radius.  Thus, when the boat is at the edge of the corridor formed by the ideal path and the width of the target radius (shown in dashed yellow lines below), the crosstrack value is 1.  When the boat is exactly on the line from last to target location, the crosstrack value is 0.

If the compensation parameter is non-zero, then the target heading is adjusted by turning it toward the ideal course as the crosstrack value increases.  Higher compensation values make the correction more aggressive.

Crosswind Compensation

Crosswind compensation accounts for the wind pushing the boat off course and works to turn the target heading into the wind.  The crosswind parameter specifies the strength of this compensating behavior.  In the future, we plan to feed that value from a wind speed sensor.  In order for crosswind compensation to be calculated, the Nav module must also be subscribed to a wind direction value from a Wind vane module.

Alternate Navigation Modes

The default navigation mode is "goto", whereby the distance and heading are the shortest path (technically the great circle path) to the centre of the target location.  

Alternative modes are in development:

• Orbit - generates a path that orbits a target location at a set distance, intended to automate a camera boat tracking one of our sailing boats
• Follow at a distance - will generate a path that heads toward the target, but stops at a set distance away.  Intended for daisy chaining a convoy of boats (acting as radio relays) a set distance apart and automatically following a leader.

Example parameters for a Nav module in operation:

Polar Map and Selecting an Optimal Point of Sail

The Sailor module implements the sailing algorithm that allows for tacking, as opposed to simply heading straight for the target waypoint.  The core functionality for any sailing algorithm is to be able to select an appropriate point of sail given the current wind direction and desired heading.  Knowing how the boat will perform (speed over ground) for a given point of sale is critical information and is traditionally captured in a polar chart.  See background to Polar charts for sailing boats on Wikipedia.

The Sailor module is configured with a simplified polar chart, capturing the expected performance for each of 16 points of sail (representing angles from 0 to 180 degrees) at an ideal wind speed.  The polar is assumed to be symmetrical and thus we only need to explicitly record half the chart.  We are using a single idealised wind speed for simplicity and on the assumption, the relative performance of different points of sail does not change significantly with wind speed.  An example polar chart is shown below:

To use the polar for selecting a point of sail, the Sailor module will check the expected speed for all possible headings given the wind direction, then compute the effective speed in the direction of the target (using a vector dot product) as provided by the Nav module.  This expected speed toward target is shown in red below, with the selected point of sail (having the fastest expected speed toward target) in cyan:

This algorithm to select a point of sail will ensure the boat always makes the best progress possible toward the target, however, it will often deviate far from the ideal line between last and target locations - i.e. the crosstrack distance will grow very large and it will sail using an extremely inefficient course.  Human sailors solve this problem by tacking, which is the second major part of Sailor module logic.

Tacking 

Referring to the following diagram, we want the Sailor module to tack when the crosstrack value exceeds 1, which means the boat has gone outside of the ideal corridor between last and target locations, as defined by the target radius.  I'll refer to this as the tacking corridor from now on.

Selecting an alternate tack means choosing a point of sale where the wind comes over the opposite side of the boat (port vs starboard).  The Sailor module achieves this by storing the current tack (as in which side the wind is currently coming from) and then flipping the target side when it reaches the edge of the tacking corridor.  When evaluating points of sale (see above), only headings that have the wind on the desired side will be considered.  

Put together, this generates points of sale that maintain an optimal speed-over-ground toward the target, whilst zig-zagging back and forth across the tacking corridor.  The algorithm is robust to changing wind conditions and drift.

Example tacking trace from our in-circuit simulator

An example set of published parameters for a Sailor module is shown below along with the more human-friendly interface seen in the web UI:

Setting the Helm (Rudder) & Gybing

The Sailor module publishes a point of sale (the target parameter shown above) which is fed to a TurnRate module.  This implements a PID controller, measuring the difference between the boat's current heading (from its Compass) and the target heading from the Sailor.  The output of the TurnRate module is fed to a Servo module that controls the boat's rudder.

The TurnRate module also implements gybing functionality, whereby it monitors if the desired heading is unachievable (e.g. if the boat stalls trying to turn across the wind) and will switch to gybe mode, turning the boat the opposite way around to reach the target heading.

The Servo module implements a bezier control curve that maps input to output values and allows for fine-tuning of the rudder response.

 

Setting the Sheet

For regular sailing boats, the Sailor module calculates a sheet parameter that indicates the position of the mainsheet.  It's fed to another Servo module that controls the mainsheet servo, whereby the mapping curve can be used to tune the sheet for the full range of relative wind positions.

Our Volantex Compass RG65-class robot sailboat under autonomous control

Setting the Wing

For wing-sail boats, the Sailor generates a wing parameter that tells the wing what position to set the tail servo to.  This value simply flips between -1 and 1 depending on which side the wind is blowing from (port vs starboard) - a very easy calculation!

Future Topics

In future posts, I'll explore:

• How to avoid other ships detected via AIS transponder signals
• How to avoid crashing into landmasses
• How our in-circuit simulator (dreamer) works.  This allows us to perform endurance testing of the firmware (sailing algorithms, etc) whilst the server-side software simulates the sailing physics and virtual sensors like GPS and compass.

References

I've leveraged many sources to understand the concepts mentioned above, but none have been more fundamental/critical than the navigational math described here: https://www.movable-type.co.uk/scripts/latlong.html

That site is gold and should be mandatory reading for anyone attempting autonomous sailing!