A new guidance trajectory generation algorithm for unmanned systems incorporating vehicle dynamics and constraints

Abstract

We present a new trajectory generation algorithm for autonomous guidance and control of unmanned vehicles from a given starting point to a given target location. We build and update incomplete a priori maps of the operating environment in real time using onboard sensors and compute level sets on the map reflecting the minimal cost of traversal from the current vehicle location to the goal. We convert the trajectory generation problem into a finite-time-horizon optimal control problem using the computed level sets as terminal costs in a receding horizon framework and transform it into a simpler nonlinear programming problem by discretization of the candidate control and state histories. We ensure feasibility of the generated trajectories by constraining the solution of the optimization problem using a simplified vehicle model. We provide strong performance guarantees by checking for stability of the algorithm through the test of matching conditions at the end of each iteration. The algorithm thus explicitly incorporates the vehicle dynamics and constraints and generates trajectories realizable by the vehicle in the field. Successful preliminary field demonstrations and complete simulation results for a marine unmanned surface vehicle demonstrate the efficacy of the proposed approach for fast operations in poorly characterized riverine environments.