USV motion planning