While forward kinematics is uniquely determined by simple matrix multiplications, Inverse Kinematics (IK) is the inverse mapping: given a desired target pose (T_{sd} in ext{SE}(3)), find the joint configuration ( heta in mathbb{R}^n) such that (T_{sb}( heta) = T_{sd}). Inverse kinematics is non-linear, non-unique (possessing multiple discrete solutions or an infinite continuum in redundant robots), and prone to algorithmic singularities where numerical solvers diverge into catastrophic joint velocities.
1. Analytical Closed-Form Solutions and Pieper’s Criterion
For industrial 6-DoF manipulators (e.g., KUKA, FANUC, Universal Robots), having an analytical closed-form IK solver is highly desirable because it executes in microseconds and returns all possible branch configurations (elbow up/down, wrist flip/no-flip). By Pieper’s Criterion (1968), a 6-DoF serial arm possesses a closed-form analytical solution if and only if:
- Three consecutive joint axes intersect at a single common point (e.g., a spherical wrist where axes 4, 5, and 6 intersect).
- Or three consecutive joint axes are parallel to each other.
Under a spherical wrist decoupling, the inverse kinematics problem separates into two independent 3-DoF subproblems:
- Wrist Center Positioning: The wrist center position (p_w = p_{sd} – d_6 R_{sd} hat{z}) is determined solely by the first three arm joints (( heta_1, heta_2, heta_3)) via geometric trigonometry.
- Wrist Orientation: Once (R_{03}( heta_1, heta_2, heta_3)) is computed, the remaining spherical wrist angles are extracted directly from (R_{36} = R_{03}^T R_{sd}) via Euler angle extraction.
2. Numerical Inverse Kinematics: Newton-Raphson
For general kinematics chains (e.g., 7-DoF redundant humanoids or snake robots) lacking Pieper geometry, solutions must be computed iteratively. Let the task-space error twist be (e = log(T_{sb}^{-1}( heta) T_{sd}) in mathbb{R}^6). Using the manipulator Jacobian (J( heta)), the first-order Taylor expansion yields:
[
e approx J( heta) Delta heta implies Delta heta = J^{-1}( heta) e
]
In standard Newton-Raphson iteration, joint updates follow ( heta_{k+1} = heta_k + J^{-1}( heta_k) e_k).
3. Singularity Explosion and Damped Least Squares (DLS)
When the Jacobian is non-square or loses rank at a kinematic singularity ((det(J J^T) o 0)), using the standard Moore-Penrose pseudo-inverse (J^+ = J^T (J J^T)^{-1}) causes eigenvalues to approach zero. Inverting near-zero singular values causes the joint velocity (dot{ heta} = J^+ mathcal{V}) to explode toward infinity, tripping motor amplifiers or commanding impossible hardware accelerations.
To ensure numerical robustness, Nakamura and Hanafusa (and Levenberg-Marquardt) introduced Damped Least Squares (DLS), minimizing a weighted cost combining tracking error and joint effort:
[
min_{Delta heta} left( |J Delta heta – e|^2 + lambda^2 |Delta heta|^2
ight)
]
Yielding the regularized IK update formula:
[
Delta heta = J^T left( J J^T + lambda^2 I
ight)^{-1} e
]
Where (lambda) is a dynamic damping factor. Far from singularities, (lambda approx 0) yielding exact tracking; near singularities, (lambda > 0) automatically bounds joint velocities by sacrificing a small amount of tracking accuracy.
4. Redundancy Resolution in Null-Space
For redundant manipulators ((n > 6) DoF), the Jacobian has a non-trivial null space (mathcal{N}(J)). The general IK solution incorporates a secondary task vector (z_0 in mathbb{R}^n) projected into the null space without affecting the primary end-effector tracking:
[
dot{ heta} = J^+ mathcal{V} + left( I – J^+ J
ight) z_0
]
By choosing (z_0 =
abla H(q)), the secondary objective can maximize manipulability ((H = sqrt{det(J J^T)})), avoid joint limits ((H = sum rac{(q_i – bar{q}_i)^2}{(q_{i,max}-q_{i,min})^2})), or evade obstacles in cluttered human workspaces.
