Abstract
We present a model-predictive trajectory planning algorithm for following a target boat by an autonomous unmanned surface vehicle (USV) in an environment with static obstacle regions and civilian boats. The planner developed in this work is capable of making a balanced trade-off among the following, possibly conflicting criteria: the risk of losing the target boat, trajectory length, risk of collision with obstacles, violation of the Coast Guard Collision Regulations (COLREGs), also known as "rules of the road", and execution of avoidance maneuvers against vessels that do not follow the rules. The planner addresses these criteria by combining a search for a dynamically feasible trajectory to a suitable pose behind the target boat in 4D state space, forming a time-extended lattice, and reactive planning that tracks this trajectory using control actions that respect the USV dynamics and are compliant with COLREGs. The reactive part of the planner represents a generalization of the velocity obstacles paradigm by computing obstacles in the control space using a system-identified, dynamic model of the USV as well as worst-case and probabilistic predictive motion models of other vessels. We present simulation and experimental results using an autonomous unmanned surface vehicle platform and a human-driven vessel to demonstrate that the planner is capable of fulfilling the above mentioned criteria.