It's the holiday season, and this is a blog that has a history of talking about RGB LEDs, Arduino controllers, and simple algorithms. As such, I will now spend three hours writing up how I spent 20 minutes hooking up my randomized Christmas lights:
Just kidding, Santa's bringing you coal this year. You get a post on the math behind Inverse Kinematics and Regularized Least Squares along with some code examples applying these concepts to an Arduino-controlled robot arm. Pour yourself a glass of eggnog with an extra shot of bourbon, it's going to be a long night.
Months ago I received a robot arm as a gift. I immediately set out to wire up an Arduino Prop Mini and some H-bridges to control the motors, along with a couple potentiometers to provide live feedback on the motor positions. I did a short write up here. At the time I wrote some basic code to control the arm. All it would do is take a set of target angles and move the motors until the measured angles were close to the targets. You could tell the arm do do things like "point straight up" and it would do it with ease. Since the arm is not capable of rotating very far around any joint, I implemented a check that would prevent the arm from ever moving out of bounds. If it did find itself in an especially contorted state, it would slowly restore itself to a more proper state and then resume whatever it was attempting to do before.
This method works just fine if all you want to do is tell the arm what angles to have. It's not super useful if you want the gripper to move itself to a specific position in space. Since all the arm knows so far is the angle each joint is at, it can't work backwards to determine how to get the gripper to a specific spot. Fortunately, there is a solution to this problem: Inverse Kinematics.
To understand how inverse kinematics works, first we must look at forward kinematics. We know the current angles of each joint based on our live measurements, and we know the shape and connectivity of the arm. Each segment of the arm can be seen as a simple rod of length $l_i$ connected to the previous joint and rotated by some angle $\theta_i$:
From these quantities we can work out the position of the gripper ($\vec{p_{\mathrm{g}}}$) in physical space. For now I will restrict the math to two dimensions ($x$, $z$) since motion in the $y$ direction is determined by the base rotation motor, and is trivial to work out. However, I will soon generalize to vector positions and states so that it can be extended to any number of dimensions and angles.
\[ \vec{p_{\mathrm{g}}} = \vec{f}(\vec{\theta}) = \begin{pmatrix} l_3 cos(\theta_1 + \theta_2 + \theta_3) + l_2 cos(\theta_1 + \theta_2) + l_1 cos(\theta_1) \\ l_3 sin(\theta_1 + \theta_2 + \theta_3) + l_2 sin(\theta_1 + \theta_2) + l_1 sin(\theta_1) \end{pmatrix}. \]
Solving for the position of the gripper based on knowing the angles of the joints is the forward problem. We want the arm to solve the opposite, or inverse problem. Given a target position for the gripper, determine the angles that each joint must be at.
If the function $f$ were simpler, we might have a chance of finding the inverse function and applying it to any target position $\vec{p_t}$ to find the target state $\vec{\theta_t}$. Unfortunately, we can't do this for our case so we must resort to other methods of solving inverse kinematics. Suppose our target position is not very far from our current position so that we may approximate the target as a small step in angle away from the current position:
\[ \vec{p_{\mathrm{t}}} = \vec{p_{\mathrm{g}}} + J \vec{\delta \theta} + \epsilon \]
where the $\epsilon$ indicates additional terms that will be neglected. This is a multi-dimensional Taylor expansion of the forward kinematics equation. The quantity $J$ is the Jacobian matrix containing the partial derivatives of each component of the function $f$ with respect to each component of $\theta$:
\[ J_{ij} = \frac{\partial f_i}{\partial \theta_j}. \]
We know the target position, the current position, and the derivatives of $f$ at the current location, so we are able to rearrange our approximation to get an equation for the step in angle required.
\[ \vec{\delta \theta} = J^{-1} (\vec{p_{\mathrm{t}}} - \vec{p_{\mathrm{g}}}) \]
Since this is just an approximation, we need to apply this formula to our current position many times in order to settle on the target position. The first iteration will hopefully provide a $\delta \theta$ that moves the gripper towards the target position, but will not be exact. Successive applications of a new $\delta \theta$ computed at each new position should cause the gripper position to converge on the target position. This is sometimes called the Jacobian Inverse method.
The robot arm I'm dealing with has 5 motors. I haven't settled on a good way of providing feedback on the gripper motor, so I'm ignoring that one for now. The base rotation motor determines the direction the arm is pointing in the $x$-$y$ plane, but reformulating the math in cylindrical coordinates ($r$, $\phi$, $z$) makes the target angle of this motor trivial ($\theta_{base} = \phi$). This leaves two non-trivial dimensions for the positions ($r$, $z$) and three joint angles that need to be determined ($\theta_1, \theta_2, \theta_3$). For various math and non-math reasons, I added one further constraint on the position vector. To specify the angle that the gripper ends up at, I added a third 'position' $\psi$ defined as
\[ \psi = \theta_1 + \theta_2 + \theta_3 \]
From this, we can formulate our forward kinematic function $f$ as:
\[ \vec{f}(\vec{\theta}) = \begin{pmatrix} l_1 cos(\theta_1) + l_2 cos(\theta_1 + \theta_2) + l_3 cos(\theta_1 + \theta_2 + \theta_3) \\ l_1 sin(\theta_1) + l_2 sin(\theta_1 + \theta_2) + l_3 sin(\theta_1 + \theta_2 + \theta_3) \\ \theta_1 + \theta_2 + \theta_3 \end{pmatrix}. \]
The partial derivatives of each of these three components with respect to each joint angle gives the analytic form of the Jacobian matrix $J$.
I coded this up in C as an IKSolve() function which reads and write to global variables for the current and target positions and angles. The function performs a single iteration of updating the target angle and allows the rest of the code to work on moving the arm to those angles in-between iterations.
The 3-by-3 matrix inversion is performed using a function adapted from a post found on stackexchange.
I added this to my robot arm code and uploaded it to the Arduino Pro Mini. The target position has to be hard-coded, so each new target required uploading the code again. When given a target position that is within it's reach, the arm typically does a good job moving to it. Here's a gif of the arm moving a pen down to touch a piece of paper:
The algorithm performs fairly well. The arm may not have taken the most optimal path to get to the target position, but we have not put any constraints on the path. The arm can also be told to move to a position that is outside it's reach. Here's a gif of the arm trying to place the pen directly above the base, slightly out of it's reach:
As you can see, the algorithm isn't perfect. The arm seems to struggle to rotate itself to the correct angle, and instead of converging on a final resting place it seems to oscillate around the target. Finally, it goes unstable and wanders off. The Jacobian algorithm used in this example is attempting to converge on the exact answer provided, and when it physically cannot, it starts doing strange things. It will get the arm as close as it can get, but will keep iterating, thinking that it will just need a few more steps to get to the answer. While testing the arm I found this instability to show up much more often than I would have liked. If the arm could not move itself to exactly the correct position, it would start to shake and eventually drift away from the target completely. I needed a new approach.
To fix this instability, I resorted to using a technique I am familiar with for my grad school research. I do helioseismology, which involves measuring oscillations on the surface of the sun to infer that is happening on the inside. Specifically, I'm interested in the flow velocities deep in the solar interior. Every flow measurement we make is related to the actual flow inside the sun by some reasonably convoluted math. The process of turning a set of measurements into a flow map of the solar interior involves solving an inverse problem. This is similar to what I have outlined for the robot arm, except the math for the forward problem is very different. We have a few methods of solving the inverse problem, one of which is using Regularized Least Squares.
Least squares is often used in data analysis to 'fit' an analytical model of data to the measured data. The model is reliant on a user-defined set of numerical paramaters that are optimized during the fitting to produce a model that appropriately matches the data. The measure of how appropriate the match is depends on the sum of the difference between the data and the model squared. This way, the optimized parameters are the ones that provide the least deviance between the model and the data. In the case of solving inverse problems, we want to find the angles (the parameter set) which will position the arm (the model) closest to our target position (the data). Additional terms can be added to the model to bias the optimization to find a solution that we prefer (regularization).
The wikipedia page for non-linear least squares provides a nice derivation of how to turn the problem into matrix algebra which can be iterated to find an optimal solution. In the end, you can express the iteration technique in a similar way to the Jacobian Inverse method outlined above:
Jacobian Inverse: \[ \vec{\delta \theta} = J^{-1} (\vec{p_{\mathrm{t}}} - \vec{p_{\mathrm{g}}}) \]
Least Squares: \[ \vec{\delta \theta} = (J^T J)^{-1} J^T (\vec{p_{\mathrm{t}}} - \vec{p_{\mathrm{g}}}) \]
Regularized Least Squares with Weighting: \[ \vec{\delta \theta} = (J^T W J + R)^{-1} J^T W (\vec{p_{\mathrm{t}}} - \vec{p_{\mathrm{g}}}) \]
Without regularization or weighting, the least squares method looks strikingly similar to the Jacobian method. Once the fancy extras are added back in things look a little messier, but the essence is still there. Since I already had code set up to solve the Jacobian method, it was easy enough to modify it to handle a few more matrix operations:
The weighting vector simply weights one or more components of the target position more or less than the others. If you want to make sure the gripper is in the right location but you don't care so much for what angle it ends up at, you can de-weight the third component. I set the regularization to penalize solutions which caused excess bending at any of the joints. This way, if there happened to be multiple ways of the arm positioning itself correctly, this would bias the algorithm to pick the solution that is 'smoothest'.
With this more advanced approach, the robot arm was acting much more sane. I could still confuse it with particularly troublesome targets, but overall it was much more stable than before. If a target position was out of reach or required moving out of bounds for any joint, the arm would settle on a position that was fairly close. With high hopes, I stuck a pen in the gripper and told it to draw circles on a piece of paper.
The code will need a little more tuning before I sent the arm off to art school. For now, the dunce hat remains. I've been able to confirm that the arm generally works, but I think the next step is to see how well I can fine tune both the code and the sensors to achieve a modest level of accuracy in the positioning (<1 cm).
I don't always like to look up the solution to my problems right off the bat. Often I will see if I can solve them myself using what I know, then sometime near the end check with the rest of the world to see how I did. For this project, I was happy to find that different types of modified least squares methods are commonly used to do inverse kinematics. I certainly didn't expect to create a novel method on my own, but even more, I wasn't expecting to land on a solution that the rest of the world already uses.
