Abstract
Derivatives of equations of motion (EOM) describing rigid body dynamics are becoming increasingly relevant for the robotics community, finding many applications in the design and control of robotic systems. Controlling robots—particularly multibody systems with elastic components—requires not only smooth trajectories but also the time derivatives of control forces and torques, and thus the EOM. This paper presents novel $n^{th}$ order time derivatives of the EOM in both closed and recursive forms. While the former provides direct insight into the structure of these derivatives, the latter enables highly efficient implementation for large degree-of-freedom (DOF) robotic systems.
Introduction: The Need for Higher-Order Derivatives
In classical robotics, inverse dynamics is used to calculate the joint forces or torques required to achieve a specific motion. However, modern robotic systems frequently employ Series Elastic Actuators (SEA) or Variable Stiffness Actuators (VSA). These "compliant" joints improve safety and energy efficiency but introduce control complexities. Effective control of such systems requires not just the forces themselves, but their rate of change—specifically, first, second, or even higher-order time derivatives of the control torques.
Historically, researchers have relied on three primary methods for calculating these derivatives:
- Finite Differences: Simple to implement but susceptible to numerical noise.
- Automatic Differentiation: Flexible but often computationally expensive and lacking structural transparency.
- Hand-Derived Formulas: Efficient but limited to low orders (1st or 2nd); manual derivation of higher orders is mathematically intensive and error-prone.
This research bridges the gap by providing a generalized analytical framework to compute derivatives to any order $n$ without the drawbacks of numerical approximation.
Methodology: Analytical Framework
The proposed approach leverages Lie Group and Screw Theory to represent robot kinematics and dynamics. The core methodology involves two primary components:
Leibniz Rule for Products
The generalized Leibniz rule is applied to the product of kinematic and dynamic matrices. This allows the complex derivative of the entire Equation of Motion to be decomposed into a sum of derivatives of its constituent parts, such as the Mass and Coriolis matrices.
Recursive and Closed-Form Solutions
- Recursive Formulation: An $O(n)$ algorithm is developed that propagates kinematic and dynamic quantities from the base to the end-effector and back. This is optimized for computational speed in high-DOF systems.
- Closed-Form Expressions: A matrix-based version of the equations provides structural insight into how derivatives relate to the robot's physical configuration.
A key insight is that the $n^{th}$ derivative of generalized forces is a linear combination of the $(n+2)^{th}$ derivative of joint positions and various lower-order terms. This generality allows researchers to switch between derivative orders (e.g., from 2nd to 4th) by simply adjusting a single variable.
Experimental Validation
The algorithms were evaluated using a Franka Emika Panda industrial manipulator.
The analytical results were validated against numerical differentiation, showing perfect alignment within numerical accuracy. The framework successfully computed high-order derivatives, such as the 3rd time derivative of generalized forces, which are critical for smooth control in complex actuators.
Computational Performance
Benchmarks performed on a standard Intel Core i7 processor for 10,000 evaluations of 2nd-order inverse dynamics yielded the following results:
- Recursive $n^{th}$ order algorithm: 117 seconds.
- Closed-form $n^{th}$ order algorithm: 124 seconds.
While a specialized, hand-crafted 2nd-order algorithm is faster (24 seconds), the proposed general algorithm offers unprecedented flexibility for higher-order requirements.
Conclusion and Future Work
The current formulation is designed for open-chain serial robots. Future work aims to extend these $n^{th}$ order derivatives to general mechanisms with kinematic loops (closed-loop systems). Additionally, while the current MATLAB implementation demonstrates mathematical validity, moving to an optimized C++ environment (such as the HyRoDyn framework) is expected to significantly enhance real-time performance.
Summary updated