Discrete-Time Optimal Trajectory Generation Through Multiple Waypoints

An algorithm for discrete-time trajectory generation through a set of waypoints specified in three spatial dimensions is presented. Constructing a desired trajectory that passes through a set of given waypoints is important in several real-life applications such as autonomous guidance of Unmanned Aerial Vehicles (UAV). Commonly, tree search algorithms or polynomial spline-like extrapolations are employed for generating trajectories through a given set of waypoints, but they have not been demonstrated to be optimal or the most computationally efficient. This paper formulates the trajectory generation problem through a given set of waypoints as a discrete-time optimal control problem and provides an optimal trajectory that minimizes a cost that is quadratic in the position vector and its first three derivatives. The 'artificial control input' for the optimal control problem is taken to be the third derivative of the position vector, commonly referred to as the jerk vector. The generated trajectory is shown to pass through the set of specified waypoints and give a smooth position trajectory with continuous jerk. Numerical examples are provided to illustrate the trajectory generation scheme.