An O(n)-Algorithm for the Higher-Order Kinematics and Inverse Dynamics of Serial Manipulators Using Spatial Representation of Twists
To address the real-time computational demands of optimal robot control—particularly differential flatness-based control—for high-order kinematics and inverse dynamics, this paper proposes a unified algorithm grounded in spatial screw theory and Lie group/Lie algebra formalism. The method achieves linear-time complexity (O(n)) for both forward and inverse kinematics up to the fourth order, as well as second-order inverse dynamics, via recursive forward/backward propagation and rigid-body screw modeling, fully supporting vectorized parameter inputs. The algorithm is compact, analytically exact, and real-time capable. Experimental validation on a Franka Panda 7-DOF manipulator demonstrates significant speedup over conventional approaches, enabling millisecond-level response required for high-order closed-loop control. The core contribution is the first unified O(n) framework integrating fourth-order kinematics and second-order inverse dynamics, establishing a foundational advancement for real-time differential flatness control of serial manipulators.