NUMERICALLY STABLE SOLUTION TO THE 6R PROBLEM OF INVERSE KINEMATICS

In this paper, a stable and accurate algorithm to compute all solutions of the inverse kinematics problem of a 6 revolute manipulator chain is presented. A system of equations is constructed based on the fundamental closure conditions, leading to a closed algebraic system of 20 equations involving 16 quantities, composed of trigonometric functions of five among the six unknown joint angles. Two among these five are stably eliminated using singular value decomposition (SVD) avoiding the need to consider special cases. The resulting system of equations involving three unknowns is solved by conversion to a generalized eigenvalue problem. The remaining three unknown angles are obtained using the previously computed pseudoinverse. In this formulation we exploit the inherently complex form of the system reducing it to 10 complex equations in 9 quantities, which substantially accelerates the SVD computation. The method's robustness is demonstrated through a comparison to current methods and several examples including known problematic cases where some axis or link lengths vanish, or some joint angles are 180 degrees, as well as cases where multiple eigenvalues arise.