Inverse Kinematics of a 3-DOF Spider Robot Leg

Introduction

Hexapod robots require real-time computation of inverse kinematics (IK) for each of their six legs to achieve coordinated locomotion. Given a desired foot position in Cartesian coordinates, the IK problem is to determine the joint angles that position the foot at that location. For microcontroller-based systems (e.g., Arduino ATmega328P at 16 MHz without FPU), computational efficiency is critical - IK must be computed at 50-200 Hz for all legs simultaneously.

This article derives a closed-form analytical IK solution for the standard 3-degree-of-freedom (3-DOF) spider leg, which consists of three revolute joints: coxa (hip yaw), femur (hip pitch), and tibia (knee pitch). Our derivation prioritizes numerical stability and minimizes expensive operations (trigonometric functions, square roots) while handling singularities gracefully. Importantly, we do not enforce strict reachability constraints - if a target is beyond the leg's workspace, the solution orients the leg toward the target with maximum extension.

Problem Statement and Notation

Coordinate System

We adopt a right-handed Cartesian frame \(\{O; \mathbf{e}_x, \mathbf{e}_y, \mathbf{e}_z\}\) affixed to the spider's body:

• \(\mathbf{e}_z\): vertical (anti-gravity direction)
• \(\mathbf{e}_y\): forward (nominal direction of travel)
• \(\mathbf{e}_x\): lateral right (completing right-hand rule)

Kinematic Chain

The leg consists of three rigid links connected by revolute joints:

All angles follow the right-hand rule (positive counter-clockwise about the rotation axis).

Joint Angle Conventions

• \(\theta_1 = 0\): leg aligned with \(+\mathbf{e}_y\) in the horizontal plane
• \(\theta_2 = 0\): femur horizontal, pointing along \(+\mathbf{e}_y\) after yaw rotation
• \(\theta_3 = \pi\): leg fully extended (femur and tibia collinear)
• \(\theta_3 = 0\): leg fully flexed (knee maximally bent)

Inverse Kinematics Problem

Decomposition via Rotational Symmetry

The key observation is that the coxa joint induces a rotational symmetry about \(\mathbf{e}_z\). Rotating the entire leg by \(\theta_1\) does not affect the radial distance \(h = \sqrt{p_x^2 + p_y^2}\) or the vertical component \(p_z\). This decouples the 3D problem into:

1. Yaw subproblem: Determine \(\theta_1\) to align the leg azimuthally with \(\mathbf{p}\).
2. Planar subproblem: Solve 2D kinematics in the sagittal plane for \(\theta_2, \theta_3\).

Solution of the Yaw Subproblem

Rotation to the Pitch Plane

A rotation by angle \(\alpha\) about \(\mathbf{e}_z\) is represented by:

\[ \mathbf{R}_z(\alpha) = \begin{bmatrix} \cos\alpha & -\sin\alpha & 0 \\ \sin\alpha & \cos\alpha & 0 \\ 0 & 0 & 1 \end{bmatrix} \]

We seek \(\theta_1\) such that \(\mathbf{R}_z(-\theta_1) \mathbf{p}\) has zero \(x\)-component, placing the target in the \(y\)\(-\)\(z\) plane. Applying the transformation:

\[ \mathbf{p}' = \mathbf{R}_z(-\theta_1) \mathbf{p} = \begin{bmatrix} p_x \cos \theta_1 + p_y \sin \theta_1 \\ -p_x \sin \theta_1 + p_y \cos \theta_1 \\ p_z \end{bmatrix} \]

Setting \(p'_x = 0\):

\[ p_x \cos \theta_1 + p_y \sin \theta_1 = 0 \quad \Rightarrow \quad \tan \theta_1 = -\frac{p_x}{p_y} \]

Using the two-argument arctangent to handle all quadrants and division by zero:

\[ \theta_1 = -\arctan2(p_x, p_y) \]

Singularity at the Vertical Axis

When \(p_x = p_y = 0\) (target on \(\mathbf{e}_z\)), the azimuthal angle is undefined. We adopt the convention:

\[ \theta_1 = 0 \quad \text{if } p_x^2 + p_y^2 < \epsilon^2 \]

where \(\epsilon \approx 10^{-6}\) mm is a numerical threshold.

Radial Coordinate

The radial distance from the \(z\)-axis is:

\[ h = \sqrt{p_x^2 + p_y^2} \]

In the rotated frame, the target becomes \(\mathbf{p}' = (0, h, p_z)^T\).

Planar Kinematics in the Sagittal Plane

Coordinate Transformation

The coxa link displaces the hip pitch joint to \(\mathbf{o}_1 = (0, \ell_1, 0)^T\) in the rotated frame. The vector from hip to target is:

\[ \mathbf{d} = \mathbf{p}' - \mathbf{o}_1 = \begin{bmatrix} 0 \\ h - \ell_1 \\ p_z \end{bmatrix} \]

Define the planar coordinates:

\[ d_y = h - \ell_1, \quad d_z = p_z \]

The squared distance from hip to foot is:

\[ r^2 = d_y^2 + d_z^2 \]

Note: We work with \(r^2\) throughout to defer the square root operation.

Triangle Geometry

The femur, tibia, and hip-to-foot segment form a triangle with sides \(\ell_2\), \(\ell_3\), and \(r = \sqrt{r^2}\). Let \(\delta\) denote the interior angle at the hip between the femur and the hip-to-foot vector.

Law of Cosines and Discriminant Method

By the Law of Cosines on the hip triangle:

\[ \ell_3^2 = \ell_2^2 + r^2 - 2\ell_2 r \cos\delta \]

Rearranging:

\[ \cos\delta = \frac{\ell_2^2 + r^2 - \ell_3^2}{2\ell_2 r} \]

Direct computation via \(\delta = \arccos(\cos\delta)\) requires \(r = \sqrt{r^2}\), which we seek to avoid. Instead, we compute both \(\sin\delta\) and \(\cos\delta\) explicitly.

Auxiliary Variables

Define:

\[ C = \ell_2^2 - \ell_3^2 + r^2 \]

Then:

\[ \cos\delta = \frac{C}{2\ell_2 r} \]

From \(\sin^2\delta + \cos^2\delta = 1\):

\[ \sin^2\delta = 1 - \frac{C^2}{4\ell_2^2 r^2} = \frac{4\ell_2^2 r^2 - C^2}{4\ell_2^2 r^2} \]

Define the discriminant:

\[ \Delta = 4\ell_2^2 r^2 - C^2 \]

Then:

\[ \sin\delta = \frac{\sqrt{\Delta}}{2\ell_2 r}, \quad \Delta \ge 0 \]

For numerical safety:

\[ S = \sqrt{\max(0, \Delta)} \]

Elimination of Square Root in Denominator

Using \(\arctan2\) to compute \(\delta\), the common factor \(2\ell_2 r\) cancels:

\[ \delta = \arctan2(S, C) \]

This formulation avoids computing \(r\) explicitly in the denominator.

Femur Angle via Compound Formula

Direction to Target

The angle from \(+\mathbf{e}_y\) to the hip-to-foot vector is:

\[ \varphi = \arctan2(d_z, d_y) \]

Geometric Relationship

The femur angle \(\theta_2\) satisfies:

\[ \theta_2 = \varphi - \delta \]

Single \(\arctan2\) Optimization

Rather than computing two arctangents, we apply the angle subtraction formulas. From standard trigonometric identities:

\[ \sin(\theta_2) = \sin\varphi \cos\delta - \cos\varphi \sin\delta \]

\[ \cos(\theta_2) = \cos\varphi \cos\delta + \sin\varphi \sin\delta \]

Substituting:

\[ \sin\varphi = \frac{d_z}{r}, \quad \cos\varphi = \frac{d_y}{r}, \quad \sin\delta = \frac{S}{2\ell_2 r}, \quad \cos\delta = \frac{C}{2\ell_2 r} \]

We obtain:

\[ \sin(\theta_2) = \frac{d_z C - d_y S}{2\ell_2 r^2}, \quad \cos(\theta_2) = \frac{d_y C + d_z S}{2\ell_2 r^2} \]

The factor \(2\ell_2 r^2\) cancels in \(\arctan2\):

\[ \theta_2 = \arctan2(d_z C - d_y S, \, d_y C + d_z S) \]

Branch Selection (Elbow Configuration)

The planar IK admits two solutions (elbow-up and elbow-down) distinguished by the sign of \(\sin\delta\). We parameterize this with \(\sigma \in \{+1, -1\}\):

\[ \theta_2 = \arctan2(d_z C - d_y \sigma S, \, d_y C + d_z \sigma S) \]

Typically, \(\sigma = +1\) (elbow-down) is preferred.

Knee Angle from Law of Cosines

Applying the Law of Cosines at the knee vertex:

\[ r^2 = \ell_2^2 + \ell_3^2 - 2\ell_2 \ell_3 \cos \theta_3 \]

Solving for \(\theta_3\):

\[ \cos \theta_3 = \frac{\ell_2^2 + \ell_3^2 - r^2}{2\ell_2 \ell_3} \]

To handle numerical round-off:

\[ \theta_3 = \arccos\!\left(\operatorname{clamp}\!\left(\frac{\ell_2^2 + \ell_3^2 - r^2}{2\ell_2 \ell_3}, -1, 1\right)\right) \]

where \(\operatorname{clamp}(x, a, b) = \max(a, \min(x, b))\).

Behavior Beyond Reachability

If \(r^2 > (\ell_2 + \ell_3)^2\) (target too far), the clamped argument becomes \(-1\), yielding \(\theta_3 = \pi\) (full extension). The leg orients toward the target at maximum reach. If \(r^2 < (\ell_2 - \ell_3)^2\) (target too close), the argument becomes \(1\), yielding \(\theta_3=0\) (full flexion).

Complete Algorithm

Computational Complexity

The algorithm requires:

• 2 arctangents: \(\theta_1\), \(\theta_2\)
• 1 arccosine: \(\theta_3\)
• 2 square roots: \(h\), \(S\)

Total: 5 transcendental operations. This is minimal for an analytical 3-DOF IK solution. Arithmetic operations: \(\sim10\) multiplications, \(\sim8\) additions.

C++ Implementation

#include <math.h>

#define L1 30.0f  // Coxa length (mm)
#define L2 100.0f // Femur length (mm)
#define L3 150.0f // Tibia length (mm)

// Elbow configuration: +1 = down, -1 = up
#define SIGMA 1.0f

static inline float clampf(float x, float lo, float hi) {
    return (x < lo) ? lo : ((x > hi) ? hi : x);
}

/**
* Compute 3-DOF spider leg IK
* 
* Inputs:
*   px, py, pz: target foot position, relative to leg mounting
* 
* Outputs:
*   theta1: coxa yaw (rad)
*   theta2: femur pitch (rad)
*   theta3: tibia pitch (rad)
*/
void legIK(float px, float py, float pz, 
           float *theta1, float *theta2, float *theta3) {

    // Step 1: Yaw about Z
    *theta1 = -atan2f(px, py);

    // Step 2: Projection to pitch plane
    float dy = sqrtf(px * px + py * py) - L1;
    float dz = pz;

    float r_sq = dy * dy + dz * dz;

    // Step 3: Hip triangle discriminant
    float C = L2 * L2 - L3 * L3 + r_sq;
    float disc = 4.0f * L2 * L2 * r_sq - C * C;
    float S = sqrtf(fmaxf(disc, 0.0f));

    // Step 4: Femur pitch
    *theta2 = atan2f(dz * C - SIGMA * dy * S, dy * C + SIGMA * dz * S);

    // Step 5: Tibia pitch
    float cos_theta3 = (L2 * L2 + L3 * L3 - r_sq) / (2.0f * L2 * L3);
    *theta3 = acosf(clampf(cos_theta3, -1.0f, 1.0f));
}

Optimization Notes

Memory layout: For multiple legs, use structure-of-arrays (SoA) layout rather than array-of-structures (AoS) to improve cache utilization:

// Efficient for 6 legs
float px[6], py[6], pz[6];
float theta1[6], theta2[6], theta3[6];

for (uint8_t i = 0; i < 6; i++) {
    legIK(px[i], py[i], pz[i], &theta1[i], &theta2[i], &theta3[i]);
}

Fixed-point arithmetic: On 8-bit AVR without FPU, consider Q16.16 fixed-point with lookup tables for atan2 and acos. Interpolated tables with 256 entries provide sufficient accuracy (<0.5°) while reducing computation time by 10-20x.

Fast inverse square root: The Quake III algorithm can approximate \(1/\sqrt{x}\) with 2-3x speedup, but modern sqrtf implementations are typically optimized enough for this application.