Solving Inverse Kinematics with Non-Linear Least Squares

Question

I want to write my own inverse kinematics solver, and I have been recommended to use Google's Ceres Solver to help. Now, according to the documentation, Ceres Solver is usually used for non-linear least squares problems (http://ceres-solver.org/nnls_tutorial.html). This minimises the sum of squared differences between the measured and target values, over all data. What I am confused about, is how this relates to inverse kinematics.

In inverse kinematics, with the example of a robot arm, the goal is to determine what joint angles the robot should be positioned in, in order to reach a target end effector pose. There exists a single equation which determines the end effector pose, given the set of joint angles, and we want to determine the parameters in that equation (the joint angles).

But how does this relate to the least-squares problem, where there are multiple measurements? Is the problem I am trying to solve essentially the same, except that the number of measurements is one? And in that case, is using Ceres Solver's non-linear least squares solver really necessary?

Thanks!

Answer 1

The inverse kinematics problem can be stated as a nonlinear constrained least-squares optimization, which is in its simplest form as follows:

$$ \mathbf{q}^*=\arg\min_{\mathbf{q} \in \mathbb{R}^n} \left\| \mathbf{x}_d-K \left( \mathbf{q} \right) \right\|^2 \\ \text{s.t.} \quad \mathbf{q}_l<\mathbf{q}<\mathbf{q}_u $$

where $\mathbf{q}$ is the vector of the $n$ independent joint angles, $\mathbf{x}_d$ is the desired Cartesian pose comprising target position and orientation of the end-effector, and $K\left(\cdot\right)$ is the forward kinematic map. The minimization must be carried out without violating the lower and upper joints bounds, $\mathbf{q}_l$ and $\mathbf{q}_u$ respectively.

Therefore, you are right - in a sense - saying that there exists only one measurement you have to account for, if compared with the traditional least-squares setting.

Concerning how we can solve the problem, there's a large number of traditional techniques that do not resort to sophisticated optimizations: Jacobian transposed, Jacobian pseudoinverse, a synergy of the two termed Damped Least-Squares, to cite the most known. Thus, it's not necessary to employ Ceres, for example, as back-end to implement your solver.

Nonetheless, lately there's been an increasing attention in the community to the use of Sequential Quadratic Programming and more generally nonlinear methods: see for example [1] for a extensive survey. The reason for that is the significant amount of benefits you'll get with a nonlinear optimizer:

• robustness against singularities and out-of-reach targets
• better handling of joints bounds
• possibility to deal with higher number of linear/nonlinear constraints
• scalability with the number of DoFs
• and more...

Personally, I developed in the past a nonlinear inverse kinematics solver using Ipopt for a humanoid robot ([2]) and I found that I managed to easily outperform other standard techniques, as those provided by KDL.

Thereby, my warm suggestion is to go with Ceres, Ipopt or other equivalent optimizers.

References

[1] TRAC-IK: An open-source library for improved solving of generic inverse kinematics.

[2] An experimental evaluation of a novel minimum-jerk cartesian controller for humanoid robots.