Robot Inverse Kinematics: A Non-Convex Optimization Challenge

Robotics presents fascinating optimization challenges that combine geometry, control theory, and numerical methods. In this post, I explore a project from my optimization course at NTNU where we tackled the inverse kinematics problem for planar robot arms—a fundamental challenge in robot control that’s both theoretically interesting and practically relevant.

Project materials:

• Project description (PDF) — The original assignment from TMA4180 Optimization I
• Project delivery (PDF) — My complete solution with implementations and analysis

The Robot Kinematics Problem

Consider a planar robot consisting of $n$ rigid segments of lengths $\ell_i$, connected by revolute joints. The first segment is fixed to the origin, and each subsequent segment can rotate freely relative to the previous one. The joint angles $\vartheta = (\vartheta_1, \ldots, \vartheta_n)$ fully describe the robot’s configuration.

Forward Kinematics: The Easy Direction

The forward kinematics problem asks: given the joint angles, where is the end-effector (the “hand”)? This is straightforward. The position of the end-effector is given by:

$F(\vartheta) = \sum_{i=1}^{n} \ell_i \begin{pmatrix} \cos\left(\sum_{j=1}^{i} \vartheta_j\right) \\ \sin\left(\sum_{j=1}^{i} \vartheta_j\right) \end{pmatrix}$

This is a simple composition of trigonometric functions—easy to compute, no surprises.

Inverse Kinematics: The Hard Direction

The inverse kinematics problem reverses the question: given a target position $p \in \mathbb{R}^2$, what joint angles $\vartheta$ will place the end-effector at $p$? In other words, solve:

$F(\vartheta) = p$

This is where things get interesting—and difficult. The equation is:

• Nonlinear (trigonometric functions)
• Under-determined (for $n > 2$, infinitely many solutions may exist)
• Ill-posed (adding $2\pi$ to any joint angle doesn’t change the configuration)

Rather than trying to solve this equation directly, we reformulate it as an optimization problem:

$\min_{\vartheta \in \mathbb{R}^n} d(\vartheta) \quad \text{where} \quad d(\vartheta) := \frac{1}{2}\|F(\vartheta) - p\|^2$

The idea: find joint angles that minimize the squared distance between the end-effector and the target.

Mathematical Analysis

Computing the Gradient

To use gradient-based optimization, we need $\nabla d(\vartheta)$. Let $F_1$ and $F_2$ denote the $x$ and $y$ components of $F$. The $i$-th component of the gradient is:

$\left(\nabla d(\vartheta)\right)_i = \frac{\partial F_1}{\partial \vartheta_i}\left(F_1(\vartheta) - p_1\right) + \frac{\partial F_2}{\partial \vartheta_i}\left(F_2(\vartheta) - p_2\right)$

where

$\frac{\partial F_1}{\partial \vartheta_i} = -\sum_{k=i}^{n} \ell_k \sin\left(\sum_{j=1}^{k} \vartheta_j\right), \quad \frac{\partial F_2}{\partial \vartheta_i} = \sum_{k=i}^{n} \ell_k \cos\left(\sum_{j=1}^{k} \vartheta_j\right)$

This gradient has a beautiful interpretation: it shows how rotating joint $i$ affects the end-effector position through all subsequent segments.

Proving Non-Convexity

A natural question: is $d(\vartheta)$ convex? Unfortunately, no. To see this, consider the one-dimensional case with a single segment of length $\ell$:

$d(\vartheta) = \frac{1}{2}\left(\ell^2 - 2p_1\ell\sin\vartheta - 2p_2\ell\cos\vartheta + p_1^2 + p_2^2\right)$

For $p = (0, 1)$, choose $x = 2\pi$, $y = 0$, and $\alpha = 1/2$. Then:

$d(\pi) = \frac{1}{2}(\ell^2 + 2\ell + 1) > \frac{1}{2}d(2\pi) + \frac{1}{2}d(0) = \frac{1}{2}(\ell^2 - 2\ell + 1)$

This violates the convexity condition. The non-convexity arises from the periodic nature of the trigonometric functions—there are multiple local minima corresponding to different robot configurations that reach the same target.

Numerical Solution: BFGS with Line Search

Given the non-convexity, finding the global minimum is generally intractable. Instead, we use quasi-Newton methods to find local minima, specifically the BFGS algorithm with line search satisfying the Wolfe conditions.

The BFGS Algorithm

BFGS (Broyden-Fletcher-Goldfarb-Shanno) builds an approximation to the Hessian $H_k \approx \nabla^2 d(\vartheta_k)$ iteratively:

1. Start with $H_0 = I$ (identity matrix)
2. Compute search direction: $p_k = -H_k \nabla d(\vartheta_k)$
3. Line search: find $\alpha_k$ satisfying Wolfe conditions
4. Update: $\vartheta_{k+1} = \vartheta_k + \alpha_k p_k$
5. Update Hessian approximation using rank-2 update formula

The Wolfe conditions ensure sufficient decrease while preventing steps that are too short:

$d(\vartheta_k + \alpha p_k) \leq d(\vartheta_k) + c_1 \alpha \nabla d(\vartheta_k)^T p_k$

$\nabla d(\vartheta_k + \alpha p_k)^T p_k \geq c_2 \nabla d(\vartheta_k)^T p_k$

with $0 < c_1 < c_2 < 1$. I used $(c_1, c_2) = (0.1, 0.4)$ in my implementation.

Numerical Results

For a 4-segment robot with lengths $\ell = (3, 2, 1, 1)$ targeting the origin $p = (0, 0)$:

• Solution found: $\vartheta = (1.5342, 3.7950, 4.5470, 0.7437)$
• Iterations: 9 iterations to reach gradient norm $\approx 10^{-10}$
• Convergence: Exponential decrease in gradient norm (typical BFGS behavior)

The method’s efficiency is remarkable—achieving machine precision in under 10 iterations despite the problem’s non-convexity.

Optimal Control: Trajectory Planning

Finding a single configuration is useful, but robots need to move smoothly between configurations. This leads to an optimal control problem.

Problem Formulation

Given initial joint angles $\vartheta$ and target position $q$, find the velocity $\varphi$ that:

1. Reaches the target: $F(\vartheta + \varphi) = q$
2. Minimizes energy: $\frac{1}{2}\|\varphi\|^2$

This is a constrained optimization problem:

$\min_{\varphi \in \mathbb{R}^n} \frac{1}{2}\|\varphi\|^2 \quad \text{subject to} \quad F(\vartheta + \varphi) = q$

Quadratic Penalty Method

To handle the equality constraint, we use the quadratic penalty method. Replace the constrained problem with:

$\min_{\varphi \in \mathbb{R}^n} Q(\varphi; \mu) = \frac{1}{2}\|\varphi\|^2 + \frac{\mu}{2}\|F(\vartheta + \varphi) - q\|^2$

As $\mu \to \infty$, the penalty term forces the constraint to be satisfied. The gradient is:

$\nabla Q(\varphi; \mu) = \varphi + \mu J_F(\vartheta + \varphi)^T \left(F(\vartheta + \varphi) - q\right)$

where $J_F$ is the Jacobian of $F$.

The algorithm iteratively:

1. Solve the unconstrained problem for current $\mu$
2. Increase $\mu$ (typically $\mu \gets 2\mu$)
3. Repeat until constraint violation is small

Multi-Stage Trajectory Planning

We can generalize to piecewise constant velocities by dividing the time interval $[0, 1]$ into $s$ subintervals, each with velocity $\varphi^{(i)}$:

$\min_{\varphi^{(1)}, \ldots, \varphi^{(s)}} \frac{1}{2s}\sum_{i=1}^{s} \|\varphi^{(i)}\|^2 \quad \text{subject to} \quad F\left(\vartheta + \frac{1}{s}\sum_{i=1}^{s} \varphi^{(i)}\right) = q$

Equivalence Theorem

A beautiful theoretical result: the multi-stage problem reduces to the single-stage problem.

Theorem: The sequence $(\varphi^{(i)})_{i=1}^{s}$ solves the multi-stage problem if and only if $\varphi^{(i)} = \varphi^{(j)}$ for all $i, j$, and each $\varphi^{(i)}$ solves the single-stage problem.

Proof Sketch: Fix the mean $x_i = \frac{1}{s}\sum_{j=1}^{s} \varphi_j^{(i)}$ of the $i$-th coordinates. Let $a_j^{(i)} = \varphi_j^{(i)} - x_i$ be the deviations. Then:

$\sum_{j=1}^{s} a_j^{(i)} = 0 \quad \text{and} \quad \frac{1}{s}\sum_{j=1}^{s} (\varphi_j^{(i)})^2 = x_i^2 + \frac{1}{s}\sum_{j=1}^{s} (a_j^{(i)})^2$

With $x_i$ fixed, this is minimized when all $a_j^{(i)} = 0$, i.e., when all velocities are equal. $\square$

This result has practical implications: the optimal trajectory has constant velocity—smoother than piecewise trajectories and easier to compute.

Numerical Verification

For $n = 3$, $\ell = (3, 2, 2)$, $\vartheta = (\pi/2, \pi/2, \pi/2)$, $q = (2, 1)$, $s = 20$:

All velocity vectors converged to $\varphi^{(i)} \approx (4.0550, 1.5588, 3.1778)$ with relative error $< 10^{-10}$, confirming the theoretical equivalence.

Joint Constraints and Barrier Methods

Real robots have physical limits on joint angles. Suppose each joint must satisfy:

$-\frac{3\pi}{4} \leq \psi_j \leq \frac{3\pi}{4}$

For the multi-stage problem, this gives constraints:

$-\frac{3\pi}{4} \leq \vartheta_j + \frac{1}{s}\sum_{i=1}^{k} \varphi_j^{(i)} \leq \frac{3\pi}{4}, \quad k = 1, \ldots, s, \quad j = 1, \ldots, n$

Logarithmic Barrier Function

To enforce these inequality constraints, we use a logarithmic barrier:

$Lb((\varphi^{(i)})_{i=1}^{s}) := \sum_{k=1}^{s}\sum_{j=1}^{n}\left[\log\left(\frac{3\pi}{4} + \vartheta_j + \frac{1}{s}\sum_{i=1}^{k} \varphi_j^{(i)}\right) + \log\left(\frac{3\pi}{4} - \vartheta_j - \frac{1}{s}\sum_{i=1}^{k} \varphi_j^{(i)}\right)\right]$

This function is:

• Finite only when constraints are strictly satisfied
• Tends to $-\infty$ as any constraint boundary is approached

The complete problem becomes:

$\min Q((\varphi^{(i)})_{i=1}^{s}; \mu, \beta) = \frac{1}{2s}\sum_{i=1}^{s} \|\varphi^{(i)}\|^2 + \mu D((\varphi^{(i)})_{i=1}^{s}) - \beta Lb((\varphi^{(i)})_{i=1}^{s})$

where $D$ is the penalty for the equality constraint, $\mu > 0$ is large, and $\beta > 0$ is small.

Gradient of the Barrier Function

The gradient with respect to $\varphi_a^{(b)}$ is:

$\frac{\partial Lb}{\partial \varphi_a^{(b)}} = \frac{1}{s}\sum_{k=b}^{s}\left[\frac{1}{\frac{3\pi}{4} + \vartheta_a + \frac{1}{s}\sum_{i=1}^{k} \varphi_a^{(i)}} - \frac{1}{\frac{3\pi}{4} - \vartheta_a - \frac{1}{s}\sum_{i=1}^{k} \varphi_a^{(i)}}\right]$

Implementation Challenges

The barrier method introduces numerical challenges:

• Step size restrictions: Line search must ensure the barrier stays finite
• Ill-conditioning: As $\mu \to \infty$ and $\beta \to 0$, the Hessian becomes poorly conditioned
• Initialization: Starting point must satisfy all constraints strictly

In my implementation, the line search occasionally failed when $\alpha$ became too small or converged to a fixed value, causing the BFGS update to fail. This highlights the practical difficulty of barrier methods compared to penalty methods.

Key Takeaways

This project demonstrates several fundamental concepts in nonlinear optimization:

1. Non-convexity is ubiquitous in real problems—robot kinematics is inherently non-convex due to periodic joint angles

2. Quasi-Newton methods are powerful even for non-convex problems, providing superlinear convergence to local minima

3. Penalty methods convert constrained problems to unconstrained ones, but require careful parameter tuning and can become ill-conditioned

4. Barrier methods naturally enforce inequality constraints but are numerically delicate

5. Theory and practice diverge: The equivalence theorem is beautiful, but numerical stability matters more in implementation

6. Problem structure matters: Understanding the geometry (e.g., that optimal trajectories have constant velocity) leads to simpler, more robust algorithms

Extensions and Applications

The techniques developed here extend to:

• 3D robot arms (spatial kinematics with orientation constraints)
• Redundant manipulators (more joints than degrees of freedom)
• Obstacle avoidance (adding inequality constraints for collisions)
• Real-time control (using warm starts from previous solutions)
• Humanoid robots (inverse kinematics for full-body motion)

Modern robotics uses these methods extensively, combined with:

• Sequential Quadratic Programming (SQP) for better constraint handling
• Interior-point methods as more robust alternatives to barrier methods
• Model Predictive Control (MPC) for real-time trajectory optimization

---

This post summarizes my optimization project at NTNU (Spring 2016), where I implemented and analyzed various numerical methods for robot kinematics and trajectory planning. All implementations were done in MATLAB using custom BFGS and penalty method codes.