When the Connecticut Robotics Society (CRS) ran its first annual odometry and dead-reckoning event this June, the winning entry completed the 24.7-foot (7.5 m) circuit and terminated its run within 2 inches (5 cm) of its starting position. Although Peeves, a Lego Mindstorms™ based robot, came perilously close to the inner wall on the third leg of the course, it managed to recover and navigate four 90-degree turns and 20 feet of corridors to reach a first-place finish.

First, the robot took advantage of the Lego Mindstorms rotation sensors (encoders) which allowed it to measure wheel position with a 1.8-degree resolution (1.3-mm linear displacement). These devices were coupled to the motors and controller in a closed-loop system that allowed the robot to monitor its performance at all times.

Figure 1. The Connecticut Robotics Society Dead-Reckoning Course. Competing robots complete one circuit of the course (shown in gray). The wining robot is the one that finishes closest to its starting position. To do so, it must navigate four 5-foot corridors and four 1.18-foot curves – altogether, a 24.7 foot (7.5m) circuit.

In this article, I will discuss the dead-reckoning techniques used for the competition, and provide details about its implementation in the Lego Mindstorms system. Completing the contest course required two kinds of maneuvers: straight-line traversals and 90-degree turns. I will provide background on Peeves' approach to both. My primary focus will be on the PID algorithm and how it was used to keep the robot on track when following a straight-line path. I will also discuss how the PID control logic was used to absorb orientation errors when the robot executes "not exactly 90-degree" turns.

— John Gallichotte

Figure 2 shows the basic layout of the Peeves platform. The robot was built from the stock Lego Mindstorms kit with the addition of 2 rotation sensors that were purchased separately. The brain of the Mindstorm system is the RCX "brick." The RCX brick is a Lego-interlocking unit that contains batteries, microcontroller unit, LED display, and an IR communications link. The RCX is built on a Hitachi H8/3272 processor with 32 K of external RAM. It features three Pulse-Width Modulation (PWM) outputs that can be used to drive 9-volt DC motors or other devices. Output can be adjusted to 8 levels by increments of 1/8^th, in forward or reverse. The drivers can also be set to brake or free-turning states. The RCX also provides 3 sensor ports based on 10-bit A/D converters. The Mindstorms kit comes with two high-efficiency 9-volt DC gearmotors that feature an internal fly wheel and reduction gearing. Peak, unloaded motor speed is 350 RPM when running on fresh batteries.

One of the most appealing features of the Lego Mindstorms system is the speed with which a robot can be constructed. When the CRS contest was announced early in April, the Mindstorms kit was still in shrink-wrap. Two months later, a completed robot was running the course. That convenience does come with certain constraints. Design options are limited by what can be accomplished using Lego parts (though Lego enthusiasts like to say, "you are limited only by your imagination"). And while individual Lego components are marvels of mass-produced precision, the overall assembly of a Mindstorms system is sometimes less perfect than a robot builder would prefer. This is particularly true with regard to the wheel and drive systems. Peeves is heavy enough that the flex of its plastic axles give the wheels a significant (and asymmetric) inward tilt. When possible, the effects of these limitations were reduced through appropriate mechanical design.

Of course, there are significant limits to how sophisticated a program you can write in a graphical programming environment. The stock Lego utilities were inadequate to the task of coding a dead-reckoning robot, so a different solution was necessary. Dave Baum's NQC is one of a number of alternate programming tools available as open-source software for the RCX. NQC compiles to the byte-code and runs on top of the RCX firmware. Other solutions (such as legOS and lejos) actually replace the firmware providing more efficient and fully featured implementations.

I chose NQC because it appeared to be the simplest, most easily mastered solution given the short turn-around time for my project. However, NQC does have to operate within the limitations imposed by the RCX firmware. For example, the firmware supports a maximum of 32 variables, does not support arrays or floating-point data types, and is limited to two-byte signed integers. Some of these limitations are lifted if you install the new 2.0 release of the RCX firmware, now in beta release.

Figure 3 shows the gear train used in Peeves. The configuration is a compromise between the geometry imposed by the Lego components and the need to obtain as fine a resolution from the wheel encoders as possible. The more accurately the robot can measure wheel rotation, the more accurately it can dead-reckon. Each motor is coupled to an encoder through a 1:1 gear train that ensures they are synchronized. For the Peeves configuration, peak motor speed is about 290 RPM (unloaded, they peak at about 350 RPM). The rotation sensors are rated for a maximum speed of 500 RPM. The gear train provides a 12.5 to 1 reduction ratio from motor to wheels. Thus, for each rotation of the wheel, the encoder rotates 12.5 times allowing the system to measure 12.5x16=200 samples per revolution. Although I experimented with gearing that provided a higher resolution (with less backlash), it exceeded the 500 RPM sample rate supported by the sensors and had to be abandoned.

The placement of the large, 40-tooth gears in the robot's gear train requires that Peeves use the larger wheels provided in the Mindstorms kit. The Lego wheels are rather wide and soft for a dead-reckoning robot (harder, narrow wheels provide better accuracy), but provide good traction. Their 8.16 cm diameter tires means that each "click" of the encoders covered a distance of 8.16x3.1415/200=0.128 cm. This value, the click, is used extensively in the Peeves software.

Table 1. Vital statistics.

The two gearmotors that came with the Mindstorms kit that was used to build Peeves are not perfectly matched. Due to variations in manufacture, one runs about 5 percent slower than the other. An imbalance in motor output is important in a differential steering system because even small variations in wheel speed can result in the robot straying from a straight-line path. The default tendency for Peeves was to curve to its left.

The acronym PID is short for Proportional, Integral, and Derivative. In the PID approach, a controller monitors the error in the system (its deviation from some desired value, or set point) and makes corrections based on three criteria. The Proportional response is based on the magnitude of the observed error, the Integral of that error (error accumulated over time), and the Derivative of the error (the rate at which the error changes over time). The PID approach was the basis of the system used to control Peeves and keep it following a straight line.

To illustrate the idea behind the PID technique, let's apply it to the familiar problem of steering an automobile on highway. We desire to keep the car in the center of its lane. If the car begins to drift to the right, we apply a correction by turning the steering wheel to the left to stop the movement away from the centerline. In this case, the set point is simply for the car to travel in a straight line. We define the speed of the car's lateral movement as our observed rate of error. The faster it moves away from the centerline, the higher the error. Naturally, a competent driver does not overcorrect. So, if the error is small, our response is small. If the error is large, our response is large. In other words, our initial correction is Proportional to the error rate (giving us the ‘P’ in PID).

If we turn the wheel enough, the car will stop moving to the right and drive in a straight line. Although the car will have drifted from the center of the lane, we will have achieved an error rate of zero because we define the error in terms of the lateral speed, not distance off-center. Still, we do care about the car not being in the center of its lane. So we define a second kind of error, the accumulated error. In mathematical terms, this value is simply the Integral of the error over time (giving us the ‘I’ in PID). And, referring back to our driving example, we realize that the correction applied to the steering is often proportional to values: the instantaneous drift rate, and the integrated error.

If all goes well, the car will reverse its rightward drift and return to the center of its lane fairly quickly. Perhaps a little too quickly... If we do not back off from the correction as we approach the center of the lane (zero accumulated error), the car will overshoot the centerline and begin to drift to the left. If that happens, we need to make a second, counter-adjustment so that the car returns to the right. But if we over-correct, we may need another counter-adjustment to the left. If this continues, the system will oscillate. To damp that oscillation, the PID technique adds a third factor based on the rate-of-change for the error function, that is its Derivative (giving us the ‘D’ in PID).

The car analogy illustrates the three basic components of the PID technique. In general, we consider the instantaneous and integrated (accumulated) error and make a proportional adjustment. Although this adjustment is often enough, it sometimes leads to a system that tends to overcorrect and oscillate as it returns back to the set point. In such cases, we add the third correction to help settle the system.

For purposes of this discussion, we will let e(t) represent a general error function and use the following variation of the PID equation:

There are several web sites describing the PID technique. A good place to start can be found at the Control Engineering Tutorials web site or at the joint Carnegie-Mellon/University of Michigan Control Tutorials for Matlab pages. Many books on control theory also discuss the PID technique. At least one of these is available on line: Lee, Newell, and Cameron's Process Management and Control (the discussion of PID tuning techniques in chapter 5 is especially useful).

PID for Peeves: How one robot implements the algorithm

One common use of the PID technique is in controlling motor speed. If I had taken this approach in the Peeves implementation, I could have used two sets of PID calculations to maintain a constant speed for each motor. Because the Lego rotation sensors are coupled directly to the motors, they form a closed-loop system. Feedback from each sensor can be used as input into the PID calculation for the corresponding motor. Additional calculations can combine sensor inputs to track data such as orientation and position, coordinating the motors and adjusting their respective speeds as needed. Unfortunately, implementing such an approach turned out to be more complicated than I could accomplish given my short development time and the coding limitations of the RCX/NQC environment.

Instead, I took a less ambitious approach. Rather than looking at the problem as a matter of controlling two motors, I looked at the robot as a whole. For the CRS contest, it didn't matter how fast the robot traveled. So controlling the absolute speed of each motor was not as important as controlling their speed relative to each other. The Peeves implementation focuses on the robot's path, and disregards its speed. The important thing is simply that the vehicle goes straight.

Figure 4 shows the track of a robot on a Cartesian coordinate plane. If the robot is to follow a straight-line course, it should move along the x-axis. As error accumulates, it tends to move off the axis. There were a couple of different ways that I could have defined the error functions for the PID equation. For example, I could have focussed on the robot's orientation. But, after a bit of trial and error, I settled on the definitions used in the automobile example used above.

Figure 4. The straight-line path is along the x-axis. The error function is defined to be the rates of change of the y coordinate over time (a straight line being zero error). The integrated error is treated as being the distance from the x-axis, and is sometimes referred to as the accumulated error or the lateral offset from center.

		Error term of PID equation: how fast are we drifting from the center?
		Integral term of PID equation: how far are we from centerline?
		Derivative term of PID equation: is the drift rate accelerating or decelerating?

Table 2. Relationships between error functions.

1. Find equations for the error function e(t) = dy/dt, its integral, and its derivative.
2. Find workable parameters for the PID equation.
3. Write code using numerical techniques to solve the PID equation for a differential steering (which involves sine and cosine) while working in the 2-byte integer data types available in the Mindstorms system.

Equations for Differential Steering

Using the PID technique requires that we have some way to compute the error function, its integral, and its derivative. How do we convert data collected by the encoders into a description of the robot's movements? To do so, we need to know something about the behavior of a differential steering system. Fortunately, I had already tackled the details for an earlier article I had written for the Rossum Project (see An Elementary Trajectory Model for the Differential Steering System). From this source, and others, I was able to obtain equations that described the orientation of the robot, and its y-coordinate as functions of wheel velocities and time. These equations made the simplifying assumption that the wheel velocities were held constant. This assumption seemed reasonable since the large reduction ratio of Peeves' gear train provided a high torque relative to the robot's mass. When motor power levels were changed, the wheels would spin "up to speed" quite quickly. So as long as the robot stayed close to its desired straight-line course, the simplifying assumptions would not result in too much error. The Rossum Project web site provided an applet that allowed me to experiment with the consequences of different settings and test this assumption (see MotionApplet).

The following equations were used in the Peeves project to describe the motion of the robot. They can be applied to any differential-steering system as long as you are careful about the simplifying assumptions mentioned above.

Once I had equations for the PID component functions, the next step was to obtain parameters for the PID equation. Because the selection of parameters depends on the physical characteristics of the system, there is no stock set of values that can be applied to every implementation. Instead, the parameters for the equations must be tuned to the particular platform they are intended to control.

The selection of PID parameters is the aspect of the Peeves Project with which I am least satisfied. Although I eventually arrived at values that work, they do not represent an optimal solution.

In general, there are three approaches that can be used to find values for the PID parameters:

Table 3. Three approaches to tuning PID parameters.

Unfortunately, the two-month lead time for the CRS odometry contest did not allow enough time for me to get "up to speed" on the material. In the absence of a good theoretical model of the system, I attempted to use an empirical technique. There are a number of well-known empirical techniques that can be used to tune PID parameters. One of the oldest, and most straightforward, of these is a method promulgated by Zeigler and Nichols in the 1940's. In the Zeigler-Nichols open-loop technique, a developer runs the system "on-the-bench" and observes its response to changes in input level. Test results are then plugged into an series of simple equations that produce estimates for the PID coefficients. Zeigler-Nichols and related empirical methods often yield better results than a simple trial and error approach. They also have the advantage of not requiring knowledge of the process dynamics of a system.

You may read a short article about the Zeigler-Nichols technique in the Control Engineering Archives. The on-line text Process Motion and Control, Chapter 5, discusses PID tuning techniques at length and offers several more recent, but computationally intensive, alternatives to Zeigler-Nichols.

Although I attempted to use the Zeigler-Nichols method to develop PID parameters for my own system, I'm afraid I struck out. My bench tests yielded unsuitable results. I concluded that either I was misapplying the technique or that the dynamics of the differential-steering system did not match some of the basic assumptions of Zeigler-Nichols. With no time left for further research, I resorted to trial and error.

Thanks to tuning guidelines in the Carnegie-Mellon PID Tutorial, I was able to develop a set of parameters that worked reasonably well for Peeves. While the values I obtained were not optimal, an optimal set of parameters was not absolutely required for the odometry contest. Because the robot was expected to stick close to a straight-line path, the error values would be quite small and well within the capabilities of the Peeves implementation. It was only when I tried to stress the system in non-competition testing that shortcomings became apparent.

To see the effect of the sub-optimal selection of parameters, consider Figure 5. The figure shows the results from an experiment where the initial values of the accumulated error (integral error) was set to 40 clicks (51.2 mm, or about 2 inches). The robot was instructed to run a straight-line path and store its accumulated and instantaneous error values in its datalog. Recall that the integrated error is the robot's lateral offset from the straight-line path, or its y-coordinate, and is given by the function   y(t) (a distance here measured in millimeters). And, again, we define its observed error, e(t), to be the rate of change of its y-coordinate with respect to time, or dy/dt (expressed as lateral movement in millimeters per second). Note that the PID algorithm corrects for the integrated (accumulated) error, an offset of 50 millimeters to the left, by steering to the right. The integrated error drops quite quickly, with relatively little oscillation. As the integrated error gets smaller, so does the magnitude of the controller's response. Ordinarily, this behavior is exactly what we would hope to see. Unfortunately, when the integrated error becomes less than 7 millimeters, the magnitude of the system's response is rather too small and it becomes slow to correct for the remaining offset.

While I was disappointed by Peeves' inability to correct for the remaining offset in a timely manner, it was not that serious an issue in the contest. At most, it degrades the final accuracy of the robot by half a centimeter. Still, in a competition where millimeters count, it could become a serious issue. I plan on continuing my research on the subject and hope to offer more useful information in a future article.

Approximation Issues: Scaling Distance, Time, and Angle to Integers

Once the PID parameters were established, it was time to code the equations for the error functions in the NQC (Mindstorms) programming language. As noted above, the only data type supported by the RCX firmware is the 2-byte signed integer. So not only were floating-point variables unavailable, but any integer arithmetic would have to remain within the range of -32768 to 32767. I would also have to work out approximations for the sine and cosine functions used in the equations shown in [6], [7], and [8].

Perhaps the most common way of handling fractional values on a system that supports only integers is to "scale" the values by multiplying them by some constant. The trick in choosing a scalar is to pick one large enough to avoid a loss of precision while finding one small enough that integer overflow doesn't become a problem. In the case of measures of distance, I was lucky enough to avoid the problem of scaling and round-off altogether.

As noted above, each click of the encoder corresponds to a distance of 1.28 mm. Rather than trying to find a way to scale conventional units of distance, I simply defined the "encoder click" as my fundamental unit of distance. By treating all distances in terms of "clicks", the Peeves software was left free of any need to perform conversions or account for loss of precision.

The treatment of time was also easy. For the version of the RCX firmware that I was running, system timers had the resolution of 0.1 second, a fact that presented a natural value for scaling (a newer version of the firmware provides 0.01-second resolution). In cases where the error functions required time values in their denominators, I simply multiplied the numerators by appropriate powers of 10.

Angles were only a little harder. The equations used to describe the differential-steering system treat angles in radians, not degrees. Although it was tempting to scale them by simply converting them to degrees, a better solution suggested itself. In the equations used for Peeves, angles are always used in close association with the parameter b (the distance between wheels, center-to-center), which in Peeves case has a value of 154 "clicks". So, internally, angles were multiplied by b to scale them to integers. For example, to specify a right angle, the NQC code for Peeves uses the value , or about 242 clicks.

NQC does not provide utilities for computing the sine and cosine functions that occur in the differential-steering equations. Fortunately, the trig functions in equations [6], [7], and [8] are used to operate on the angle deviation from the straight-line path. Since the whole point of the PID implementation was to keep the robot traveling in a straight line, I could assume that these angles would be quite small. And, for angles sufficiently close to zero, the Taylor series provides the convenient approximations and . A little algebra and a willingness to disregard terms that were small contributors to the overall value, yielded the following approximations for [6], [7], and [8]:

Keeping to the straight and narrow

The CRS dead-reckoning course can be treated as a sequence of maneuvers, alternating between straight-line and cornering operations. Competing robots begin the course traveling in a straight line down a corridor. Initially, the Peeves code sets to motors to default "go-straight" values that were determined through experimentation. Due to motor-related issues mentioned above, even the best choice of available parameters resulted in the robot traveling a slightly curved path. So as soon as the motors are activated, the program enters a PID-control loop.

During the straight-line operation, the program monitors its sensors to check for deviations from its standard course. Every 0.2 seconds, it performs the PID-related calculations and determines if a correction is required (although the RCX clock is calibrated in 0.1-second intervals, the larger 0.2-second value was chosen to reduce the effects of timing errors). If the PID calculation indicates the need for a correction, the software adjusts the motor output as appropriate. Refer to the source code at peeves.html for more detail.

The robot also measures distance-traveled along the straight-line path to determine when it reaches the end of a corridor. Once the corridor maneuver is completed, it switches to a turning operation.

Turning the Corner

The PID technique described above kept Peeves traveling in a straight line. Although I may have been able to work out an analogous PID methodology for rounding the corners, I elected to use a simpler approach. In the turning phase the course, the robot simply sets the PWM level for its outer wheel at higher value than for the inner wheel. The resulting difference in wheel speed causes the system to follow a nearly circular path as described in the equations in [7] and [9]. During the turn phase, Peeves simply monitors the angle of its orientation and breaks out of the turn when it reaches 90 degrees. Upon completion of the turn the robot immediately sets its wheels to pre-determined, "go-straight" settings and then performs calculations for the next phase of its navigation.

These calculations involve correcting for errors that may have accumulated during the turn. First, there is a certain amount of delay in the robot’s response when it completes the turn. It may not sample its sensors until after it has crossed the 90-degree threshold and even when it sets its PWM output to "go-straight" it will continue to rotate until the motors can overcome the robot’s angular momentum. This error in orientation is typically small and can be corrected by the PID logic when the robot enters its next straight-line maneuver. To do so, the program simply notes the deviation from a perfect right-angle turn and treats it as an initial condition for the algorithm to process.

We can compute the actual turn radius using data from the rotation sensors and equation [5] above. To compensate for how far the robot has advanced into the corridor, we simply reduce the length of the straight-line maneuver it is about to execute.

Unfortunately, the lateral deviation (to the left and right of the centerline) is not so easily corrected. Although we could use the PID logic to adjust for the lateral deviation (treating it as the integral error term in the PID equation), the magnitude of the error might be too large to be absorbed by the compensation logic. Instead, we defer the correction until after the next turn. We can compensate for a lateral error on the current leg of the course by simply shortening or lengthening the distance the robot travels on the next.

Figure 6. The deviation from central path when the turn radius is larger than expected. The vehicle finishes the turn both forward and outside of its intended position. Fortunately the error can be computed accurately. A symmetrical case exists when the turn radius is smaller than expected.

As you can see, the ability to predict the radius of the next turn is an important part of keeping the robot near the centerline of the course. For example, if we estimate that the turn radius is going to be larger than the standard 9-inches (half width of the course), we instruct the robot to begin its turn before it reaches the end of the corridor. Unfortunately, the actual turn radius depends on many different factors including battery age, ambient temperature, and the kind of flooring. That last factor was relevant in the Connecticut Robotics Society event because the competition took place on a different kind of flooring (linoleum) than had been used for developing the Peeves logic (concrete). The program did not have a good turn-radius estimate for the first corner and deviated substantially from the centerline. As it progressed, however, it was able to keep track of its performance on each turn and improve its estimates until it had corrected for the early deviation.

In addition to the offset due to turn radius, the robot also experiences a certain amount of orientation error due to "not-quite-90-degree" turns. Timing and control issues make it very difficult to execute a perfect right-angle turn. Fortunately, the deviation from a 90-degree turn can be measured using the wheel encoders and plugged back into the PID calculations when the robot reenters its straight-line phase. Any error in orientation is quickly absorbed by the PID algorithm.

Things Left Undone

In recent test runs on a concrete floor, Peeves consistently finished within 3 centimeters of its starting position... typically, about 1/2 cm short of the start/finish line and 2 cm to the right of center. While such accuracy is pretty good for a Lego-based robot, there is still room for improvement. Due to the sub-optimal selection of PID parameters mentioned above, I have not successfully implemented a correction for the lateral error on the last turn. Nor have I been completely successful in predicting the radius for the last turn (which would reduce the need for a lateral correction). Even if I addressed these issues, Peeves' internal position tracking can only account for about half the error. The robot knows that it is not dead-on the start/finish position, but consistently underestimates its error. The discrepancy is probably due to a combination of mechanical inaccuracies and effects such as acceleration that are not correctly handled by the software. An imperfect transition from the cornering phase of its operation to the straight-line traversal is also a likely source of inaccuracies.

Conclusion

I believe that the competition in next year's CRS odometry and dead reckoning contest will be far more intense than it was this June. I sincerely hope that Peeves will be beaten by another robot that benefited from the information I've presented in this article. If the success of Peeves illustrates anything, it is the value of harnessing the large body of information available to the roboticist on the web and in the library. I hope that some of the notes that I have presented in this article will lead other robot builders to the resources they need for their own projects.

Acknowledgments

This article would not have been possible without the assistance of many people. I would like to thank Charles Olsen, a talented and knowledgeable engineer who patiently answered my incessant questions about electronics and control theory. Thanks also go to the the members of the Connecticut Robotics Society, in particular Bill Ruehl and Jake Mendelssohn whose good sense and extensive knowledge of robotics are always an inspiration, and to John Gallichotte who organized the CRS odometry event and made important suggestions regarding this article. I'd also like to thank Kenneth Maxon whose earlier Encoder article Reaching the Next Step in Motion Control and Sensor-Software Fusion for Mobile Robots helped jump start the Peeves development effort... without the information he provided, I wouldn't have known where to begin.

The author gratefully acknowledges the assistance of Sonalysts, Inc. in the production of the graphics and photographs used in this article. Sonalysts is a Connecticut-based company providing services including software engineering for systems ranging from embedded processors to corporate intranets, from rapid prototyping to life-cycle maintenance.

And, finally, I would like to thank my wife, Kathy Lucas, for her assistance in the preparation of this document and long-suffering support throughout the Peeves project.

Comments? Write to the author gwlucas@connix.com.

Copyright © 2001 by G.W. Lucas.
Permission is hereby granted to make and distribute verbatim copies of this document
provided that this copyright notice is not removed or modified.