Trajectory Generation Implemented as a Non-linear Filter

An algorithm is presented which, given a prescribed manipulator path and corresponding kinematic solution, computes a feasible trajectory in the presence of kinematic singularities. The resulting trajectory is close to minimum time, subject to explicit bounds on joint velocities and accelerations, and follows the path with precision. The algorithm has complexity O(M log M), with respect to the number of joint coordinates M, and works using "coordinate pivoting", in which the path timing near singularities is controlled using the fastest changing joint coordinate. This allows the handling of singular situations, including linear self-motions (e.g., wrist singularities), where the path speed is zero but other joint velocities are non-zero. To compute the trajectory, knots points are inserted along the input path, with increased knot density near singularities. Appropriate path velocities are then computed at each knot point, and the resulting knot-velocity sequence can be integrated to yield the path timing. Examples involving the PUMA manipulator are shown.