Fast and Robust Algorithm for General Inequality/Equality Constrained Minimum Time Problems

This paper presents a new algorithm for solving general inequality/equality constrained minimum time problems. The algorithm's solution time is linear in the number of Runge-Kutta steps and the number of parameters used to discretize the control input history, with no a priori initial guess needed. At the core of the algorithm is a method for finding and re-solving for a fixed-time feasible solution to the full nonlinear problem. The fixed time solver/re-solver uses a new method of finding a near-minimum-2-norm solution to a set of linear equations and inequalities. This near-minimum-2-norm subproblem solution achieves quadratic convergence to a feasible solution of the full nonlinear problem. The algorithm solves for a feasible solution for large trajectory execution time t_f and then reduces t_f by a small amount and re-solves. The re-solve routine is called repeatedly, giving a sequence of feasible solutions, with smaller and smaller time t_f, until convergence is obtained. The method is being applied to a three link redundant robotic arm with torque bounds, joint angle bounds, and a specified tip path. Our experience with the minimum time program is that it solves case after case within a graphical user interface in which the user chooses the initial joint angles and the tip path with a mouse. Solve times are from 30 to 120 seconds on a hewlett packard 175-125 workstation, for 90 control variables and widely different initial arm positions and tip paths all of which require large angle motions of the arm. A zero torque history is always used in the initial guess (invisibly to the user) and the algorithm has never crashed, indicating its robustness. A representative numerical example is presented in which the solve time was 82 seconds with 150 control variables, and 153 seconds for 300 control variables.