Optimal policies for autonomous navigation in strong currents using fast marching trees

Several applications require that unmanned vehicles, such as UAVs and AUVs, navigate environmental flows. While the flow can improve the vehicle’s efficiency when directed towards the goal, it may also cause feasibility problems when it is against the desired motion and is too strong to be counteracted by the vehicle. This paper proposes the flow-aware fast marching tree algorithm (FlowFMT*) to solve the optimal motion planning problem in generic three-dimensional flows. Our method creates either an optimal path from start to goal or, with a few modifications, a vector field-based policy that guides the vehicle from anywhere in its workspace to the goal. The basic idea of the proposed method is to replace the original neighborhood set used by FMT* with two sets that consider the reachability from/to each sampled position in the space. The new neighborhood sets are computed considering the flow and the maximum speed of the vehicle. Numerical results that compare our methods with the state-of-the-art optimal control solver illustrate the simplicity and correctness of the method.