Autonomous Navigation in Strong Currents

Motion PlanningOptimal ControlFlow FieldsRoboticsMatlab
View on GitHub

FlowFMT*

Motion planning for robots navigating in environments where the flow is stronger than the vehicle itself.

The Problem

Autonomous vehicles operating in natural flows — drones in wind, underwater gliders in ocean currents, aerobots in planetary atmospheres — face an unusual challenge: the environment can push them faster than they can push back.

When the flow speed exceeds the vehicle’s maximum speed relative to the fluid, the standard assumption behind most motion planners breaks down. Large parts of the space become unreachable from certain positions, and naive cost functions produce infeasible trajectories. Traditional sampling-based planners like RRT* struggle to handle this because rewiring a search tree under a strong drift is geometrically hard.

This project develops FlowFMT* and Policy-FlowFMT*, a family of algorithms based on the Fast Marching Tree (FMT*) that explicitly account for the reachability constraints imposed by strong environmental flows, in both 2D and 3D environments.

The motivation comes partly from a NASA-sponsored problem: planning trajectories for an aerobot in the atmosphere of Venus, where winds can exceed 100 m/s and the vehicle cannot counteract the flow everywhere.

The Core Idea: Reachability Cones

The key geometric insight is that when a flow velocity vector c is present, the set of positions reachable from a given point in a fixed time is not a sphere — it is a cone-shaped region aligned with the flow.

Given two positions x(t_A) and x(t_B) separated by a displacement vector d, a straight-line connection is feasible only if the angle between d and c is smaller than a maximum cone angle β_max, which is determined by the ratio of the flow speed to the vehicle’s maximum relative speed. If the flow is weaker than the vehicle (||c|| < v_r^max), any direction is reachable and the cone degenerates to the full sphere. As the flow strengthens, the cone narrows, and once ||c|| ≥ v_r^max, a forbidden region appears — directions from which the vehicle cannot return.

This reachability cone is used in two places:

  1. Neighborhood sets — FlowFMT* splits the standard FMT* neighborhood into a posterior set (vertices reachable from the current node) and an anterior set (vertices that can reach the current node). Only geometrically valid connections are tested, avoiding the need to check feasibility for every candidate edge individually.

  2. Cost functions — The exact minimum-time cost between two waypoints under a constant flow is derived analytically by solving a quadratic equation from the vehicle kinematics. For the minimum-energy problem, the same cone defines the minimum feasible relative speed v_r^min for each edge, which minimizes the drag power for that segment.

FlowFMT* (Path Planning)

The standard version grows a tree from the start configuration toward the goal, just like FMT*, but uses flow-aware posterior and anterior neighborhoods instead of ball neighborhoods. At each iteration, the lowest-cost open vertex expands by connecting to unvisited posterior neighbors through the best anterior neighbor already in the open set. Collision is only checked after the locally optimal connection is found, keeping the number of collision queries low.

The algorithm handles strong flows naturally: if a candidate connection falls outside the reachability cone, it is never added to the neighborhood set and never tested.

Policy-FlowFMT* (Feedback Policy)

By rooting the tree at the goal and swapping the posterior and anterior neighborhoods, the algorithm expands outward to cover the entire feasible space. The result is a tree that encodes the optimal cost-to-go from every sampled configuration to the goal.

This tree is then used to generate a continuous vector field policy via inverse-distance-weighting interpolation of the tree’s velocity vectors. Given any robot position, the field returns the optimal relative velocity command. The policy is inherently robust to disturbances: if actuation is lost and the vehicle drifts, the field guides it back onto an optimal path as soon as control is recovered — without any replanning.

References

  • B. Martinez Rocamora Jr. and G. A. S. Pereira, “Optimal Policies for Autonomous Navigation in Strong Currents Using Fast Marching Trees,” Autonomous Robots, vol. 48, no. 27, 2024. https://doi.org/10.1007/s10514-024-10179-z