[HEX] Inverse Kinematics

This is the second post on my Hexapod Project series. The point of this project is to build a robot that allows me to try out a few robotics concepts. For a listing of each post in this series, click here. In this post, I'll go over the steps (!) needed to get a hexapod robot walking.

At this point, I have a robot with six legs, three servos per leg, and the electronics and code to control each servo independently. But with no guidance for how to move the servos, the hexapod is useless. With their above-average leg count and sixfold symmetry, hexapods can move around in all kinds of unique ways. While dancing around is certainly a possibility for my hexapod, I'm really only interested in getting it to walk around. So to begin the process of getting it mobile, let's start with the basics of getting a robot to walk.

Inverse Kinematics

In order for the hexapod to walk, it needs to be able to independently move each foot up and down, forward and back. But how can we tell it to move its foot? All we have control over are the three servos for each leg, none of which independently determine where the foot ends up. The position (and angle) that any given foot is at is determined by the angles of each of the three leg servos together. We can describe the relationship between leg position and servos angles as such:
\[ r = l_c + l_f \cos(\theta_f) + l_t \cos(\theta_f+\theta_t) \] \[ x_f = x_0 + r \cos(\theta_c+\theta_0) \] \[ y_f = y_0 + r \sin(\theta_c+\theta_0) \] \[ z_f = z_0 + l_f \sin(\theta_f) + l_t \sin(\theta_f+\theta_t) \]
Here, $\theta_c$, $\theta_f$, and $\theta_t$ are the servo angles for the coxa, femur, and tibia joints, respectively, and $l_c$, $l_f$, and $l_t$ are the distances between the joints. The position and angle at which the leg is connected to the body are represented by $x_0$, $y_0$, $z_0$, and $\theta_0$. This set of equations represent the forward kinematics of the leg. Each leg has an identical set of equations, but with different values for the initial position and angle.

These equations can tell us where the foot is, given the angles of the servos, but we need to do the opposite. Unfortunately, there isn't any way to rearrange the equations above so that we can plug in the foot position and solve for the servo angles (go ahead and try!). Fortunately, this doesn't mean that it's an impossible task! The process of inverting these equations is called inverse kinematics, and I've done a project on it before. My other post explains how to go about solving an inverse kinematics problem, so if you're interested in the details, check that out.

In short, the inverse kinematic solver takes a target foot position and outputs the servo angles that it thinks are appropriate. Starting with the servo angles as they are, the algorithm uses the forward kinematic equations to see which way each servo needs to turn so that the foot ends up slightly closer to the target. It takes many small steps like this until the forward kinematics equations say the new set of servo angles put the foot in the right place. This kind of procedure has its flaws, though. Imagine you tell it to find the servo angles that put the foot a mile away? The algorithm has no way to achieve this since the legs aren't nearly that long. In situations like this, it often goes haywire, giving you a nonsensical result for the servo angles. So careful attention to the iteration procedure is important.

The code I've written for the hexapod inverse kinematics solver is in my LIB_HEXAPOD library. The library also contains code used in the rest of this post along with some other bits. The procedure for determining foot position is as follows:


This bit of code gets called once for each leg, giving it a target position for that leg and getting back the optimal servo angles to use. Assuming I give it reasonable target positions, it can spit out servo angles quickly enough to solve each leg in just a few milliseconds (running on the Cortex-A9). The inverse kinematics procedure allows me to effectively ignore the fact that each foot is positioned by three servos, and instead concentrate on where I want the feet to actually go. A simple test is to just tell the feet to move up and down repeatedly:

Impressing Alan the cat with robot weightlifting.

Smooth Stepping

Six legs seems to be more stable than two or four (or an odd number?), but that doesn't make my hexapod impervious to falling over. An object will fall over (or at least be unstable) if it is not supported by at least three points of contact that surround the center of mass. If we assume the center of mass of the hexapod is roughly in the center of the body, this means that we need at least three feet touching the ground at all times. Further, if we were to draw a triangle between where the feet touching the ground are, the center of mass needs to be directly above anywhere within this triangle. Since we have six legs in all, these rules lead us to one of the simplest gaits for a hexapod, the tripod gait:

The six legs are broken up into two groups which trade off being the support for the body. The legs within each group lower to the ground in unison, move towards the back of the body, then lift up and move back to the front. The two groups do this exactly out of phase with each other so that there are always exactly three feet on the ground at any one point. For my hexapod, I've modified this a bit so that the three legs within each group hit the ground at slightly different times. I've done this to reduce the repetitive jolting that occurs from moving each leg simultaneously.

Even so, an issue I ran into while designing the gait is that whenever a foot hits the ground, the sudden transition from free-moving to weight-bearing caused the whole hexapod to jerk around. To specify the position of each foot as a function time, I was using a cosine bell with a flat bottom:

Notice the sharp change in direction at the start and end of when the foot is in contact with the floor. The transition to weight-bearing doesn't happen instantaneously (imagine carpeted floors), so the sudden transition when the foot goes from moving down to moving back creates problems. To create a smoother path for the feet to follow, I turned to Bezier curves.

Bézier curves are smooth functions that are completely determined by a sequence of points that I will call anchors. These anchors specify generally what shape the curve has, so tweaking the shape of the curve just involves moving around the anchor points. Going from a set of anchor points to the resulting Bezier curve involves a series of linear interpolations. Given some intended distance along the total path between 0 and 1, we start by linearly interpolating between each adjacent anchor points. So if we want to know where the Bezier curve is halfway along the path, we start by linearly interpolating halfway between each pair of adjacent anchors. If we have $N$ anchors, this gives us $N-1$ interpolated points. We then linearly interpolate again halfway between these $N-1$ points to get $N-2$ doubly-interpolated points. We continue this procedure until we are left with a single interpolated point, and this is the position of the Bezier curve at the halfway point.

The procedure for generating Bezier curves is a little difficult to describe, so I've made a little interface to help explain it. Drag the grey anchor points around to see how the Bezier curve changes, then increase the Guide Level slider to see the various levels of linear interpolation.

Canvas not working!

To make sure the hexapod stays steady when walking, I've kept the straight part of the foot path where the foot touches the ground, but set the return path to be a Bezier curve. I wrote a simple Bezier curve class to handle the computations on the hexapod.

Turntables

Applying this Bezier curve stepping method to each leg in an alternating pattern gets the hexapod to walk forwards, but it can't yet handle turning. To implement turning, my first instinct was to simply adjust the amount by which each foot sweeps forward and back on each side of the body differently. This would cause one side to move forward more than the other, and the hexapod would turn. The problem with this method is that it isn't particularly physical. If you try it, you'll find that the hexapod has to drag its feet sideways to compensate for the fact that it is turning sidways but the feet only move forward and back. In order to let the hexapod turn naturally, you need to go into a different frame of reference.

If you tell all of the feet to move up towards the sky, the hexapod moves closer to the ground. Tell the feet to move down, the hexapod moves up. It can get confusing to figure out how to move the feet to get the body to move a certain way. I've found it's best to think that the hexapod body stays still in space and the ground just moves around relative to it. Then all you need to do is make sure the feet are in contact with that moving floor and they don't slide on it. For straight walking, we can just see it as a treadmill-like floor that continually moves backwards, and the feet are just trying to match the treadmill speed. For turning, we can think about the hexapod sitting above a giant turntable. How close the hexapod body sits to the axis of rotation determines how sharp of a turn it makes, or what the turning radius is. In order to keep the feet from sliding around on the turntable, we need to make sure each foot travels along a curve of constant radius from the axis of rotation. If we set it so the axis of rotation is directly underneath the body, the hexapod will stay in one place and just turn around and around. If the axis of rotation is set very very far away, there will barely be any curvature to the foot-paths, and the hexapod will basically walk forward in a straight line.

To help explain this concept, I've made another little interface for seeing how the hexapod feet need to move. Move the bottom slider around to change the turning radius, and enable the helper lines to see how each foot follows a specific path relative to the axis of rotation.

Canvas not working!

To incorporate the Bezier curve method from above into this view of walking, I convert the foot positions into polar coordinates around the axis of rotation and use the Bezier curve to pick the $\theta$ and $z$ coordinates as a function of time. In the hexapod code, I've parameterized the turning by a single parameter that relates to the turning radius. Between the turning parameter and a single speed parameter, I have full control over the movement of the hexapod.

At every time step, the code considers the current values for speed and turning, and decides where along the Bezier curve each foot should be. It then computes the actual positions in space from the curves and feeds these positions into the inverse kinematic solver. The solver outputs servo angles for each joint of each leg, and the main code packages these up and sends them off to the lower-level processors. This whole procedure is fairly quick to compute, so I can update the servo positions at about 50Hz.

I wrote a test code that takes input from an Xbox 360 controller and turns it into speed and turning values. This allowed me to treat the whole hexapod like a remote-control car. After driving it around and chasing my cats, I made a little video to show of the build:

At this point, the hexapod can walk around freely, but does not know where to go. In the next post, I'll go into giving the hexapod a sense of awareness through laser ranging.