Sampling-Based Direct Trajectory Generation Using the Minimum Time Cost Function

Abstract

This paper presents a methodology for computationally efficient, direct trajectory generation using sampling with the minimum time cost function, where only the initial and final positions and velocities of the trajectory are specified. The approach is based on a randomized A* algorithm called Sampling-Based Model Predictive Optimization (SBMPO) that exclusively samples in the input space and integrates a dynamic model of the system. The paper introduces an extended kinematic model, consisting of the standard kinematic model preceded by two integrators. This model is mathematically a dynamic model and enables SBMPO to sample the acceleration and provide the acceleration, velocity, and position as functions of time that are needed by a typical trajectory tracking controller. A primary contribution of this paper is the development of an appropriate “optimistic A* heuristic” (i.e, a rigorous lower bound on the chosen cost) based on the solution of a minimum time control problem for the system q̈ = u; this heuristic is a key enabler to fast computation of trajectories that end in zero velocity. Another contribution of this paper is the use of the extended kinematic model to develop a trajectory generation methodology that takes into account torque constraints associated with the regular dynamic model without having to integrate this more complex model as has been done previously. This development uses the known form of the trajectory following control law. The results are initially illustrated experimentally using a 1 degree of freedom (DOF) manipulator lifting heavy loads, which necessitates the development of trajectories with appropriate momentum characteristics. Further simulation results are for a 2 DOF manipulator.