# Moving a robot, part 1

So you decide that you want to move a wheeled robot around because you happen to have a wheeled robot that needs programming.  That’s great!  Let’s talk about that a little.

## Preliminaries

Let’s assume that your robot can obtain a rough estimate of where it is located in some Cartesian coordinate system.  For example, it has wheel encoders that allow you to measure how far it has traveled, and from this, dead reckoning can be used to compute an (x, y) position. The robot can also determine its orientation (i.e., heading), which we’ll refer to as φ (phi).  We’ll reference its orientation to the x-axis; that is, the robot is at zero degrees heading if its “head” is sitting on the x-axis.

The goal: we want to travel from some starting point (x0, y0) to a waypoint (xd, yd).

Should be pretty straightforward, right?  Well, here are some things to keep in mind:

1. The robot’s position is a rough estimate, remember? Therefore, there is uncertainty in the position.
2. This uncertainty will accumulate as the robot travels, and for a simple dead reckoning process without some way of reducing this uncertainty (e.g., a measurement of GPS position fused with your dead reckoning), this uncertainty will grow without bound.
3. There is always uncertainty in the position no matter how hard you try, because of nonlinearities in the way the robot moves.
4. Because of the above, your robot has a very high likelihood of just driving by the goal point without even knowing it.
5. GPS will not save you because there’s always at least 1 meter of error unless you’ve got something crazy like RTK-GPS, which is friggin’ expensive and not something you can mount on your little two-wheeled robot.  Also, you’re probably doing this inside, so forget GPS in general.

## Checkpointing

After all that, you might be saying to yourself, “Well, crap.”

Not to worry! There are some simple things you can do to mitigate this uncertainty.

Here’s the main one we’ll mention: do not attempt to travel from your starting point to your goal point in one fell swoop.  Take it in little steps – generate checkpoints between the start and goal, and drive to each of those checkpoints in turn.  This will prevent positional errors from messing your robot up too much since they won’t accumulate as quickly between points.

An algorithm for doing so is as follows:

1. Pick the maximum distance you want between checkpoints.
2. Pick the maximum number of checkpoints you want between your current position and the goal.
3. Set the initial checkpoint to be the goal.
4. For each potential checkpoint:
1. Compute the distance between the current robot position and the checkpoint.
2. If the distance is less than or equal to the maximum checkpoint distance, you’re done, you have your checkpoint.
3. Otherwise, calculate the midpoint between the current robot position and the checkpoint, and try again.

## A Tale of Two Velocities

Once you have your checkpoint, we need to steer the robot toward it and move there.  Now how exactly do we do that?

As we said before, we have the robot’s heading.  So what we want to do is turn the robot toward the point and move to the point, generally at the same time.  Therefore, we have two degrees of freedom for controlling the robot’s speed: the linear velocity (denoted by v) and the turn rate/angular velocity (denoted by ω).

How do we compute the appropriate velocities to get us to our point smoothly and (mostly) error-free?

We start by mentioning that our robot has certain performance parameters: it likely can’t move faster than some certain speed, and certainly can’t move slower than a certain speed.  These are tied directly to how fast it can turn, as well.

Therefore, to navigate to a point in a nice smooth fashion without building up errors, we can gradually speed up until we reach a maximum speed, and slow down as we approach the point.  We do this by sliding the velocities between the minima and maxima, based on the current distance from a given heading and a given point.

The robot’s current state is:

$\mathbf{x}_{robot}&space;=&space;(x_{current},&space;y_{current},&space;\phi)$

First, we compute the distance between the current position and our destination:

$\Delta&space;x&space;=&space;x_{dest}&space;-&space;x_{current}$

$\Delta&space;y&space;=&space;y_{dest}&space;-&space;y_{current}$

$d&space;=&space;\sqrt{\Delta&space;x^2&space;+&space;\Delta&space;y^2}$

We next compute the heading that the robot must have to move in the direction of the point:

$\phi_{new}&space;=&space;\mathrm{atan2}(\Delta&space;y,&space;\Delta&space;x)$

where atan2() is the four-quadrant arctangent function.

Figure out how far we need to go in angle:

$\Delta&space;\phi&space;=&space;\phi_{new}&space;-&space;\phi$

(There are some subtleties in computing this angle delta; i.e., it’s incredibly easy to get this wrong in some way even though it looks so deadly simple.  We’ll examine these subtleties in a future post.)

We can now compute the appropriate angular velocity based on the robot’s angular velocity limits:

$\omega&space;=&space;\omega_{min}&space;+&space;\frac{|\Delta&space;\phi|}{\pi}&space;(\omega_{max}&space;-&space;\omega_{min})$The weighting factor causes the robot to add increments of angular velocity in terms of π-units.

We can also compute the linear velocity:

$v&space;=&space;v_{min}&space;+&space;\frac{d}{d_{sep}\sqrt{2}}&space;(v_{max}&space;-&space;v_{min})$

The weighting factor in this case has a “separation distance” dsep that is used to control the “units” of speed added, similar to that for angular velocity. The $\sqrt{2}$ is merely an additional weighting factor.

And that’s it! We keep driving the robot and recomputing these velocities as we go, until we reach our destination.*

What we’ve really done here is developed a very simple pair of set-point, proportional, feedback controllers.  Simple, but they get the job done, as you can see.

(* There are subtleties in calculating these velocities as well, much like computing the $\Delta&space;\phi$ …mostly with respect to the what the robot can actually do.  We’ll talk about those later, as well)

## That Waypoint Uncertainty Thing

Of course, once you get to the waypoint, as said before, there’s some uncertainty there.  The robot could drive right past it because of: numerical errors in computation, nonlinearities in the motors, nonlinearities in the encoders, so on and so forth.

Therefore, what we do is establish a region of convergence around a waypoint.  That’s a pretty complicated term for “draw a circle around the waypoint”, or “when the robot gets within a certain distance of the waypoint, it is the same as having arrived right on top of the waypoint.”

Uncertainty problem? Handled.  Not fixed, but handled.  That’s good enough for what we’re trying to do here.

## Conclusion

And there you go, that’s how you can drive a wheeled robot around.  You even learned a little control theory!

Tune in next time when I finally get around to talking about all those subtleties I kept mentioning.  It’s probably good that you do, because these subtleties can really ruin your day if you’re not careful.

Also, we’ll talk about how to translate those velocities into something a robot can actually use at a low level.

This entry was posted in General and tagged , . Bookmark the permalink.