DRAFT

PID Closed Loop Control for roboticsBy Alex Brown rbirac@home.com 5/1/00

IntroductionPID (Proportional, Integral, Derivative) control laws will give you more control over your robot’s

performance. While the most immediate robot applications would probably be for steering and speed control, the techniques can be applied to any kinds of motion.

There are two basic types of control we can use: dead reckoning, in which we estimate the commands necessary to achieve a goal ..such as applying 45% PWM to go forward at 6 inches per second and hope that the robot will travel at this speed; or we can monitor the results of the robot’s action and correct the commands to ensure that the goal is achieved. The more time the robot is correcting its performance rather than just charging ahead, the more accurate performance you’re going to get.

PID control systems provide continuous control. Hence, whenever we can operate under PID control, the performance will be much more reliable. The goal should be to operate under PID type control as much as possible and to save dead reckoning for only those times when there isn’t adequate sensor information available to do PID.

Control systemsThere are two basic types of control systems. Open-loop control systems are those in which your

controller tells your system to do something, but doesn’t use the result of that action to verify the results or modify the commands to see that the job is done properly. Closed-loop control continuously monitors the performance of your system and changes the commands as necessary to stay on track.

Open-loop control

Examples of open-loop control might be sending a PWM signal to your drive motor which you know will normally cause it to go forward at 6 inches per second, but not measuring the actual speed and therefore not knowing that the robot ran into a chair, or is climbing a slope, and isn’t really managing to go 6 inches per second. So you tell the robot to go forward for 10 seconds then turn right, expecting it to go 5 feet and enter a doorway. But it doesn’t make 5 feet and crashes into the wall. Or telling the robot to go straight forward for a distance by setting the steering position to center, and the robot slowly veers off to the side and crashes into the wall again.

The above block diagram shows the components of an open-loop control system. The “input reference” is the signal sent to the controller saying, perhaps, proceed forward at 6 inches per second. The “controller” block represents the calculations done to control some servo or motor or something on the robot. “Plant” is a control systems term that means everything that is affected by the controller. This may include a servo motor, the steering system of the robot, the motion of the robot in response to the steering and even the resulting position of the robot in its world. The “controlled variable” is whatever part of the plant your input reference is trying to control to. It can be the servo position, the steering angles or the

position in the world. On a robot, the input reference, the controller function and parts of the plant may all be done by your microcontroller. From the earlier example, the input reference might be a command to go 6 inches/second, and the controlled variable can be the actual speed achieved which may, or may not be 6 inches/second.

Open-loop control is what dead reckoning is all about. Unfortunately, no matter how carefully we calibrate our motor and steering and other actuation devices of the robot such that the input reference should provide the desired result, there will always be errors which will cause the result to be different than what was planned. And since errors are usually cumulative over time, the longer you run in dead reckoning mode, the farther you’ll be from what you planned.

But, if open loop operations are short enough (and nothing keeps the robot from operating normally, like a chair), open loop control may provide adequate performance for many tasks.

Closed-loop control

Closed-loop control starts out like open-loop in that you send an input reference to the system, but then feedback is used to monitor the performance resulting from the command; and if the performance is different than desired, changes are made to the command to get back on track. This is done by comparing the input reference to the feedback signal and computing an error from the reference. The controller then has equations which adjust the commands to the plant to reduce the error.

Closed loop control is often approximated by doing long dead reckoning actions, then stopping and looking around, or finding a wall or corner in a known location, and using that reference to figure out how to get back on track. True closed loop control is continuously checking on performance (location, speed, whatever) so that the robot never gets significantly off track. For instance, to do wall following, the distance to the wall would be measured continuously and the steering adjusted continuously to maintain that distance. Or, in the speed control example above, if the reference speed is 6 inches per second, and the feedback signal indicates the robot is only moving at 5 ½ inches per second, the controller would see the error signal and increase the power to the motor.

To pick an example we can all identify with, steering a real car along a road is like a wall or line following exercise. You are fairly continuously changing the steering to keep the car centered in the lane. Imagine steering by aiming the car then closing your eyes and only opening them for a quick glimpse every 5 seconds or so to make a correction. It’s not only difficult, it’s scary. Because you’re afraid you might run into something during your dead reckoning phase. Same thing for a robot.

There are two aspects to this continuous control: one is the word continous, and the second is the equations to do the control. I’ll talk about continuous first.

“Continuous” really only exists in an analog world. Most of us are doing our robot control with digital computers, which can only calculate a command after some time period. But if you calculate the output often enough, it’s close enough to continuous to give the same effect.

For example, my current robot computes its steering commands 20 times a second. Effectively, it’s doing 1/20 of a second of dead reckoning followed by a control command update. Fortunately, it’s hard to get in much trouble dead reckoning for 1/20 of a second.

A sophisticated set of control equations will have many functions which are dependent on the rate at which they are calculated. For example: integrators, derivatives and filters are all time dependent. For this reason, it is necessary to have a software program (or at least part of the program) which executes at the desired repetition rate. This takes some type of a real-time operating system. This doesn’t have to be something you buy, or even something complicated. It can be done by polling (executing your software everytime a timer says the desired time has come), or by a real time interrupt which automatically runs your software when the time comes.

The interrupt is more flexible as you can have the software which is required to execute at a specific rate run when the interrupt occurs; and run other, non-time critical software in the background.

Polling software:

Start: Initialize anything that needs itSet up timer to trip at desired rate.

Loop: Wait for timer to trip(reset timer if necessary)

Do real time software tasksInput sensorsCalculate commandsOutput commands

Endloop

Interrupt driven software:

Start Initialize anything that needs itSet up Interrupt to occur at desired rate.

Loop: Do background tasks, if any

Endloop

Interrupt: Do real time software tasksInput sensorsCalculate commandsOutput commands

Return

Either of the two schemes above will do the job. Effectively what you are doing is implementing a multi-tasking system. Each time the real time software is called, a small slice of many jobs may be performed. You may be controlling motor speed, controlling steering, controlling sonar and several other things, all at 20 times a second.

You will find that writing software where subroutines are entered repetitively to do a single job is a little different than usual. For example, when a job is first started, it may be necessary to initialize the equations. But, not to initialize them on subsequent entries. This may require other higher level software to set a flag indicating first pass (which the subroutine can reset after doing the intialization) or for the higher level software itself to do the intialization. I’ll try to put some examples in at the end to show how this can be done.

This sounds more complicated, and it is, but it’s worth it.

So, what about the equations?

PID equations come out a a branch of mathematics/engineering called “feedback and control theory”. The study of this is highly mathematical and gets really into differential equations, LaPlace transforms, Nyquist, Root-Locus and Bode analysis and other wonderful things. All this math is necessary to achieve optimal performance with good system stability.

However, I believe that for most simple systems (and most of our systems are simple), a good understanding of what the three equations (P, I and D) do will allow you to hack an adequate solution by trial and error.

PID stands for Proportional, Integral, and Derivitive. These are three basic calculations that can be used to control a system. You may find you can use just one, two or three to do a job.

Proportional control

The first and most basic part is the P, for proportional control. All this means is that if you have a reference you are trying to control to, you provide a control output proportional to the error from your reference.

(note: K is commonly used to represent such gain factors, and is usually given with a subscript indicating which gain is meant. E.g . Ksteering )

As a standard robot example (assuming a front wheel steering robot), if you are trying to follow a wall using a sensor that can measure the distance to the wall (e.g. sonar), you would turn your steering system to return you to the reference. If you are 2 inches to the right, you might turn your steering 4 degrees to the left. If 4 inches to the right, then turn the steering 8 degrees to the left. That is the proportional part.

A block diagram of this operation might look like:

The drawing indicates that the reference distance (the distance from the wall that you want the robot to stay at) is subtracted from the sonar distance. This gives you an error signal showing the distance from where you want to be in inches (or feet or whatever you want). The “O” where the two signals come together is called a summation point. The “+” indicates that the reference signal is added; and the “-“ means that the sonar sensor is subtracted to get the error signal. That is:

error = reference distance – sonar distance.

The steering gain block represents just a multiplier to get the desired amount of steering per inch of error. The whole thing can be written as:

Steering angle command = Ksteering * (reference distance – sonar distance)

So what this equation does is to steer the robot back toward the reference distance. Sounds like a good idea, but in reality, this isn’t enough. What will happen if we try to steer this way?

If the robot starts out dead center on the reference, it will continue to roll straight ahead. Very good, except in the real world, it won’t stay exactly on center. It will get off a bit to one side or the other. When it does it will turn back toward the reference; and will continue to keep turning until it gets back to the reference. And while the amount of steering gets smaller as it approaches the reference, it’s still turning in the same direction. So when it gets back to the reference, its now pointed in a direction to deviate onto the other side. As it goes out the other side, it will cause steering commands to turn back again, but it will just lead to the same problem of overshooting onto the original side.

If you make your gain fairly high, to get fast accurate performance, this simple proportional control law will probably be unstable causing the robot to go back and forth from side to side following a path similar to a sinusoid.

Now if you make your gains low enough, it may be that the robot will stay “close” to the reference. But in reality the robot will be continuously dithering back and forth from one side to the other. It may be a small acceptable amount of dither. The tradeoff would be that with low gains, your robot will probably be slow to return to the reference if errors occur, and may even go unstable with large deviations. (actually, as it was dithering back and forth, it never was really stable anyway.)

Later in this paper, there will be a computer simulation of a robot steering system where you can see this performance.

There is a cure for this instability. It is the “D” part of the PID; the Derivative.

Derivative control

A derivative is the rate of change of a signal. For example, if you are traveling to a town 10 miles down the road; and you are driving there at 65 miles per hour (maybe 70 if you can get away with it!). Your error is 10 miles, and your derivative of that error is 65 miles/hour. The derivative (or what I’m probably going to call rate most of the time) tells you how fast your are approaching or departing from your reference.

To get back to the robot. If you a trying to follow a wall at a distance of 36 inches, the rate signal might say that the robot is approaching the wall at 2 inches per second. Since the goal is to travel parallel to the wall, we want the rate to be zero. Hence, this rate signal is another type of error signal. If the robot is moving closer to the wall, we want the steering to turn away from the wall. If the robot is moving away from the wall , we want to steer back towards the wall.

In block diagram form, this is even simpler than the proportional control above.

You don’t need a separate rate sensor to get this signal, or special hardware to provide it. You can derive rate from your measured signal by subtracting two consecutive measurements and dividing by the time between the measurements.

If you are making measurements at a 20 hz rate,

Loop Rate = (current measurement – previousmeasurement) /.05 Previous measurement = currentmeasurement ‘save for next loopEndloop

For example, if the robot is currently 36.5 inches from the wall, and on the previous measurement (.05 seconds ago), it was 36.6 inches from the wall; then the rate is (36.5-36.6)/.05 or 2 inches per second toward the wall.

A block diagram for the robot steering case follows;

This says that the steering will turn two degrees for each inch per second that the robot is deviating from a path parallel to the wall. Note that the gain is negative. This causes the robot to turn back toward a path parallel to the wall. And clearly, as the rate goes to zero (meaning parallel), the steering will go to zero also, so it will tend to stay parallel.

The equation for this would be:

Steering command = Krate * rate

The following figure shows the effect of this control on a robot which starts out pointed toward a reference line. The robot turns to be parallel to the line (zero rate), but doesn’t care if it is on either side of the line or even if it passes through the line.

Another charactistic of the rate control is that it is much more stable than the proportional gain since the robot must be parallel to the wall when the steering is zero. The proportional gain was unstable since the steering went to zero when the robot was the right distance from the wall, but there was not guarantee (or even likelihood) that robot would be pointed in the right direction.

This equation provides good guidance to drive parallel to the wall but doesn’t really care if it is a the desired distance.

So, now for the good news. These two equations (proportional and derivative) can be combined to provide a stable control which will track the wall at the desired distance.

The following block diagram and equations show how the two are put together:

steering command = Ksteering * (reference distance – sonar distance) + Krate * rate

What this equation provides is guidance to make an assymptotic approach to the reference. Both the distance error signal and the rate error signal are trying to drive the steering to make themselves equal zero.

What will happen is that the steering will turn such that a proportion is held between distance and rate. The proportional gain is trying to make the robot turn toward the reference line and the derivative gain is trying to make the robot turn to be parallel. So if the robot is approaching the line from the left, proportional is trying to make a right turn, and derivative is trying to make a left turn. At some point the two will balance, and if this balance point is followed, the robot ends up on the line. For example in the block diagram above, where the steering gain and the rate gain are both the same magniture, the steering will balance out when the rate is equal to the distance. If the robot is 12 inches to the right of the reference, and is moving to the left at 12 inchs/second, the steering will be zero. As the robot continues to the left until it is 6 inches away, the rate signal, now being proportionately larger than the distance will cause the steering to turn to the right until the rate is just 6 inches per second when the steering command would again be zero. Similarly, as the robot approaches to 2 inches on the right of the reference, the rate will be reduced to two inches per second; and as the distance error reaches zero, the rate will also be zero. Hence the robot will be travelling parallel to the wall at the right distance. As the control equations are computed many times a second, the resulting path will be smooth rather than the occassional turn implied above.

Integral control

Now, if the proportional and derivative gain worked so great, as described above, why do you need an integral term in the equation?

Mainly because of offsets and biases in the system. For example, if you are steering with a hobby servo and the digital zero point you have in software (the approximately 1.5 millisecond point) does not result in the robot running straight ahead, the equation is going to have to provide a continuous offset signal to the servo to keep the robot going straight. In the previous examples, with a gain of 2 degrees of steering per inch of distance error, an offset of 5 degrees on the servo steering would require 2.5 inches of distance offset to cancel it out and go straight . Maybe for your purpose 2.5 inches of error is OK. But sometimes you will want better accuracy…that is the system should compensate for any such errors. That’s what the integral term can do.

Let’s say we already have a proportional/derivative system which has turned to robot toward the line; but for whatever reason , the error signal doesn’t go to zero . The integrator will create a signal to cancel out that bias and return the error signal to zero.

An integrator is a device (or equation) which calculates the integral of the input over time. The way an integrator works is to use the input to create an output which will continue to grow until the input is reduced to zero. This is implemented on a computer by adding the input to the previous output of the integrator each time the integrator equation is performed. This would be something like:

IntegratorOutput = Integrator Output + IntegratorInput

To clarify, the next figure shows how an integrator responds to step inputs over time.

The integrator is effectively calculating the offset distance (error) times the Time that that error exists for . If the input stays at 5 inches for 2 seconds, the integrator output will be 10 inch-seconds. If the error then goes to zero, the integrator will remain at 10, if the error goes negative, the integrator will start to reduce its output.

An integrator is easy to implement in software. All you do is take the current value of error and multiply it by the period of each calculation cycle (e.g. if doing control laws at 20 times per second, the period would be 0.05 seconds). You then keep a running summation of these calculations.

Loop Integrator = Integrator + (error * 0.05)Endloop

The only thing you have to do is initialize the integrator to a desired value (usually zero) before you start running the loop.

steering = Kintegral * integrator output

Note that this new integrator circuit is using the reference error as its input. That’s because the reference is the signal you’re really trying to track (not the rate signals, which is the only other signal in this example. So if the error signal is not averaging zero, that is, it’s staying off to one side, the integrator will slowly increase in value. That integrator output is summed into the steering command with a sign that will cause the steering to move in the direction to reduce the error. Eventually, when the integrator output gets large enough to cancel out whatever bias was causing the error, the error will go to zero and the integrator output will remain constant and the robot will proceed with zero error.

Putting it all together

The following figure shows all three terms put together. It shows that by having just a single measured input signal, you can have proportional, derivative and integral components to your control. It becomes just a matter of selecting the right gains for each path. And we’ll go into that more later.

Using the control law simulator

Attached (or enclosed or something) is a program written to run under Qbasic which I assume most all of you have on your PC.

This program is a simple simulation of a robot which is capable of doing line following. It has an undefined sensor which provides the error from the line in inches (this distance is referred to as Y in the program).

The robot proceeds across the screen as a graph where it is proceeding forward at 2 feet per second and the two plots show the distance from the line (and therefore its path), and the steering angle of the wheels which are set up to be front wheel steering like a car. And since car wheels don’t turn very far, these steering wheels are limited to 20 degrees each direction.

You have the ability to position the robot at a starting position on or offset from the line and at an angle parallel to, converging, or diverging from the line to the left or right.

You can set gains for Ksteering, K rate and Kint and see what the results are and get a feel for what the gains do.

The following is a recommendation of experiments to do to evaluate each gain path and the effect of their combination.

Bring up Qbasic and load the program. If you space down to about the 21st line you’ll find a group of lines that look like:

Y = 0Angle = 0Ksteering = 0Krate = 0Kint = 0Bias = 0

These are the numbers you can change. (Please excuse the user un-friendliness, but I’m too lazy to add a menu to change parameters; and you are all programmers (or will be) and can handle it.)

As a first sample, edit the program to make Y = 12 and Ksteering = -2. Then select Run\start.

You should get a couple plots come up on the screen showing the robot starting 12 inches to the right of the centerline, and oscillating back and forth. The second plot shows the angle of the steering which also moves back and forth from left to right and hits the 20 degree steering limits at its extreme.

Your initial setup values are also listed on the upper part of the screen.

This program implements a PID control just as was explained, so lets just go thru each gain and see how it works.

1. Set Y = 6, K steering = -0.01 (everything else at zero) and run

You’ll see that the robot slowly turns back toward the line.

2. Set Ksteering = -0.1 and run

You’ll see that the robot turns more rapidly back toward the line and overshoots to the other side. Note that very little steering motion is going on. Example 1 would have also overshot if we gave it more time. Note that raising the gain cause an increase in the response speed. And an increase in the frequency of the oscillation.,

3. Repeat using gains of –1.0 and –10.0

Notice that with the gain of -1.0, the response is getting pretty quick and the steering is moving about 10 degrees which is half of the maximum available. When you get to the gain of –10, the response is about as fast as it is going to get and the steering is going from full right to full left and back. Even if you raise the gain more, the steering might get a bit more abrupt, but the response will be about the same.

So what the above demonstrates is that (at least for this steering case), a proportional only gain is always going to be oscillatory; and that raising the gain increases the speed of the response.

4. So lets try the Krate gain. Set Y = 6, Ksteering = 0 and K rate = -1. Go run it.

Note the robot just goes straight. That’s because the rate is zero, so it’s happy.

5. Set the angle to 10. That starts the robot out at an angle to the line.

Notice it steers until it is parallel to the line about 15 inches away.

6. Try again with an angle of –10.

Again it turns to be parallel, but still doesn’t care that it’s not on the line. And notice that not a lot of steering is used so it could be more responsive. It takes almost 10 seconds to get to parallel.

7. So do it with a gain of –10. Now it responds very quickly, getting to parallel within 1 second. Probably faster than you need.

8. So try it with a gain of –5. It uses full steering initially, but then smoothly turns onto a parallel path within two seconds. Not too bad.

9. Now, lets add some of the proportional gain back in to see how the two work together. Set Ksteering to -.1, notice how now it doesn’t quite go to parallel, but slowly starts going back to the line.

10. Set Ksteering up to –1. Now it gets onto the line within 15 seconds, but still not too impressive.

11. Setting Ksteering to –10, you’ll find that it gets back to the center quite rapidly. In fact, too rapidly. It now goes past the line and has to turn back to get on the line. This going past the line is called “overshoot”. In general, anytime your signal goes past its desired value and returns, it is called overshoot.

12. Overshoot is generally not desireable, in this case, the Ksteering gain is too high. So reduce it to –5 and see what happens. Looks a lot better.

13. Just to see the effect, try a much higher Ksteering gain (-30) and note that the system oscillates much as it did with only a proportional gain, however the rate gain does cause the oscillation to damp out eventually.

14. So lets take these gains (Ksteering = -5 and Krate = -5 and see how they handle some test conditions:

Y = Angle =6 012 00 -100 -206 -10

Performance of all these conditions looks pretty good since the robot returns to the line with little overshoot.

Play around using different combinations of gains for these test cases and see the effect on performance.

15. In a steering system like this, you aren’t likely to need an integral gain, but let’s see what it would do. Set the offset Y and the angle to zero, and set bias to 5. You’ll see that the steering starts at 5 degrees and the robot starts on the line. But the robot moves over about an inch or so to the right which causes enough signal thru the proportional gain to center the steering and control the robot parallel to the line. If you want to experiment, change the Ksteering gains up and down and see how it affects the amount of the standoff (as standoff is a displacement from the reference which is held over time). The standoff should be proportional to the Ksteering gain.

16. Now add some integral gain: set Kint to –0.5. You’ll see that while the steering bias causes the same initial motion to the standoff position, the integral gain slowly causes the robot to control back to the line.

17. You can experiment with increasing the integral gain to see if you can get the robot to return to the line more quickly. One thing to watch for is that integral gains, if too large, can cause oscillations. Set Kint to –15 and note that a divergent oscillation occurs.

IMPLEMENTING A SYSTEM USING THESE METHODS:

Before starting, I’d like to point out one requirement for this system that may be apparent, but I haven’t stated explicitly. The input signals have to provide linear measurements. Like they have to provide the distance in inches, not the logic that a distance is greater or less than a reference value. Fortunately such sensors are readily available. E.g. sonar, IR, tachometers, encoders etc.

Another proviso might be that the computer simulation you played with does not have the same kind of errors, mechanical slop and noise that you will find in a real world system. The gains you can use in the real world probably won’t be quite as high as you could use in the simulation. But, now that you know what the gains do, you can adjust them in your robot to get the best performance possible.

So, the only things you need to get the performance you want is the value of the three gains, Ksteering and Krate and Kint. Now, there are entire books and courses in control law analysis to determine optimal gains for best performance. Fortunately, as long as the system is pretty simple, a good hacker can find some good gains by trial and error and maybe a little thought.

First, to get very fast performance in returning to the reference, , you need high displacement (distance) gains. To get more stability, you want high rate gains. Sounds like you just make them both high and go with it? It’s not THAT easy. You saw with the simulation that raising gains improves performance for awhile, but too much can cause oscillation to occur.

Sensor noise (caused by inaccuracies in the sensor or irregularities in the wall being sensed) can, if sent through large displacement and rate gains, cause the steering to be making large motions from left to right trying to follow the noise values. While this superfluous steering activity will probably average out and follow the desired path. It can look terrible and is wasteful of energy to do all the steering.

So, noise is one reason to keep gains as low as practical while still meeting your desired performance. By the way, if sensor noise is a real problem, you may be able to filter out some of the noise, hence keeping steering excess motion under control.

And there are often limits as to how quickly your system can respond. For instance, the computer simulation limited the steering to +\- 20 degrees and also limited the rate the steering could move from one position to another. This showed up in a lot of the oscillatory cases as the steering just going back and forth between the limits as fast as it could. This is sometimes called a “limit cycle” since the system is just cycling back and forth between to limiting values.

Lets do a wall following routine first .

First, I’m going to assume that you have a sonar subroutine that can reliably measure the distance to the wall. Also, I’ll assume you have a steering system which can direct the robot to turn left or right proportionally to a command signal. In a robot with a car type steering system (like I have), this would be the angle of the front wheels. In a robot which steers by differential turning of the two drive wheels, it might be the difference in the commanded speeds of the two wheels. If you have a robot that walks, you figure out what signal defines how much it turns. Anyway, the equations can work.

Obviously we get the proportional error signal by subtracting the sonar value from the reference. But where does the rate come from? You can calculate the rate by computing the difference between two consequtive sonar readings and dividing by the time between readings. For example, if one sonar reading says you are 24 inches from the wall, and the next one (50 milliseconds later) says you are 24.5 inches from the wall, then the robot is moving away from the wall at 10 inches per second. (24.5 –24) / .050 .

Rate = (CurrentSonarValue – LastSonarValue)/.05

Note that this is one of those equations that must be executed at the proper repetition rate, otherwise the rate signal will be wrong if the time interval was not 0.05 seconds.

(Note: doing a simple calculation like this for rate can result in a noisy rate signal if the input sonar signal has much noise on it. The rate signal gets particularly noisy since you are dividing by a short time interval and effectively multiplying the noise by 20. In my own robot, the sonar signal is pretty good and I can use the simple equation written above. If you find that you do have so much noise that it is affecting your steering control too much, you can always filter or smooth your rate signal, maybe by something as simple as just averaging the last few values. )

Sbasic example:yd = (sonardist - sonardistold)*20 'lateral rate B8 fpssonardistold = sonardist

The code sample assumes only that you have previously run a sonar subroutine that returned the distance as “sonardist”, and that sonardistold was saved on the previous iteration of this code. Both measuring the distance and running this code must occur at 20 times per second to make the “*20” work correctly. Other iteration rates can be used by changing the “*20”. Yd is the lateral rate in feet per second. The comment of “B8 fps” is to remind me that the Binary point is 8 bits from the right.

OK, now that you have the two input signals (error and rate) and have a steering subroutine that can accept the output, nothing remains except to determine the gains for the three signal paths: Ksteering, Krate and Kintegral.

Start with both the Ksteering and Kintegrator gains set to zero. Then take a guess at how much the steering gain should be. For instance, if your robot was moving forward at 1 foot per second, and moving toward the wall at 3 inches per second, how much do you think you’d want to turn the steering to direct the robot to a path parallel to the wall? This depends a bit on the geometry of your robot. By simple math, you know that if the robot is going 12 inches per second forward, and 3 inches per second towards the wall, that it is approaching the wall at about a 15 degree angle. So put your robot on the ground pointed 15 degrees toward a wall and turn its steering wheels until when you push the robot forward, it makes a nice turn until it is parallel. You probably don’t want to use either the full steering capability (or else you can not make control corrections in both directions) or so little that the robot takes forever to turn. There’s no magic answer. Just an estimate. So let’s say you guess the steering should be 10 degrees for this situation. This would give a gain of :

Krate = 10 deg steering / 3 inches per second = 3.33

Note: those of you who are saying, “I don’t have floating point math in my computer, how do I do 3.3?”, hang in there and I’ll show you a way to do fractions using integer arithmetic later.

So, program the Krate gain of 3.33 into your robot and then program him to go forward at 1 ft/sec. Then set it down about parallel to the wall and start it moving. It should maintain a line roughly parallel to the wall. Now try it starting from directions pointed slightly toward or away from the wall. The robot should turn to become parallel. You can now increase and/or decrease the gain until the turning performance looks good to you. Basically, increasing the gain will just cause it to turn more quickly to reach parallel; and reducing the gain will cause it to turn more slowly and smoothly.

Note: The gain derived above assumed the speed was 1 foot/second to calculate the 15 degree angle. That angle would obviously change depending on the robot’s speed. The angle would be smaller as speed increased. Hence the amount of correction you would want to do would also decrease as speed increase. One easy way to account for this is to make the gain, Krate, a function of speed.

Krate = 3.33 * 1.0 fps/actual speed

Experiment using different gains and angles until you think you have a gain that will work for all the angles you expect to see in real life. Try some very large angles, 60 deg, 90 deg 120 deg. See that maybe the results won’t be very good for large values. Note that PID control laws generally don’t respond well to very large errors. In general, you should only get very large errors at the transition to a new

reference, e.g. after a turn. You can either ensure that your turning accuracy (which may be open loop) is good enough to provide a close to the reference starting point; or revise the control laws to handle large errors, at least temporarily, until the optimized equations can perform adequately.

Now, to get it to track to the reference distance, you have to program in a Ksteering gain. Again, you can make an estimate. Since the proportional signal and the rate signal work together to guide the robot to the reference, it becomes a question of how fast do you want the robot to return to the reference? Quickly? Or slowly and smoothly? For example, if the robot is 6 inches away from the reference, how soon do you want him to get back to the reference. Probably a couple seconds is a good place to start. If so, when the distance is 6 inches, you want the rate to be 3 inches per second. So, you want the rate gain to be twice as high as the proportional gain (so that the sum of the two terms will be zero).

So with the Krate gain set to 3.33, set the Ksteering gain to 1.66. Now put your robot back down near the wall and run some tests to see how these gains work. Some good tests might be:

Put the robot down on the reference distance pointed parallel to the wall and see if it stays there.

Put the robot down parallel to the wall but 6 inches from the reference on either side. See if it smoothly moves to the reference.

Put the robot down angled slightly (5 to 15 degrees) toward or away from the wall and see it it straightens out and moves back to the reference.

You’ll probably find that for small angles and small displacements from the reference that the robot manages to turn and get back onto the reference distance. You might also find that for larger angles or displacements that the robot can’t make the turns smoothly onto the reference and maybe oscillates back and forth along the line (or crashes into the wall or other objectionable, but normal, robot behavior).

You have several choices at this point. Since the angles and displacements you’ve been testing are just examples of how accurately your previous robot tracking commands lined the robot up for this wall following, you may decide that it can handle all the angles and displacements you expect. If so, you can keep these gains and go on to other problems. If you want the robot to be able to handle larger starting errors, you can try changing the Ksteering gain, probably reducing it. This will give a slower return to the reference, but you’ll probably find it can handle larger starting errors.

In a steering control situation like this, you’ll probably find that an integral gain isn’t necessary. If your robot goes pretty straight when you set the steering to zero, the bias error from the reference distance probably won’t even be noticeable.

A second sample implementation – Speed Control

PID control laws for a speed control system will look a little different from a steering system. In particular, a speed control system (at least the one I’m going to go through) doesn’t need a rate gain, and it does need an integral gain. Why is this so?

Basically, it’s because the steering control has more levels of integration in its “Plant” than a speed control sytem does.

A front wheel steering system does not directly control the distance from the wall. It changes the steering angle of the front wheels. As the robot moves forward over time, the steering angle will cause the car to

turn so that it is traveling toward (or away from) the wall. Then, over time, if the car is pointed toward the

wall, it will move closer to the wall. Each of these moving forwards is an integration.

A motor speed control sytem directly controls the speed of the motor. There are no integrationstages.

The trick is that in a closed loop control sytem, integrators are destabilizing. Hence it is harder to stabilize a steering-to-a-wall reference system than to stabilize a motor speed. This requires use of higher order derivative terms to be come stable (proportional is the derivative of integral, and rate the derivative of proportional).

If the steering control system used the steering wheel angle as the controlled signal in the feedback, it would be as simple as the motor speed control. If the steering system controlled the robots angle to the wall, it would be a little more complex than the motor control due to the one level of integration. Since it controls to the wall, after two levels of integration, steering is even more complex.

Most motor control sytems use only the proportional and integral equations. In fact, the system could be stable with only the integral term. Very different from what you saw using the simulation of the steering system.

Lets imagine what would happen if only an integral term was used. When a reference speed was applied to the controller. A large error would exist so the integrator would begin to build up in a direction to make the motor run. Eventually, the motor would start running and slowly accelerate as the integrator continued to build up. As the motor approached the reference speed, the error to the integrator would go to zero and the integrator would remain constant providing just enough command to keep the motor on the reference speed. If the load on the motor changed, causing its speed to change, the integrator would again see an error and change its output until the motor speed was correct again.

So, an integral term alone can control a motors speed, but it doesn’t do a very fast job of it. It is slow because the integrator gain usually has to be fairly low to remain stable. If the gain is so high that it

exceeds the motors ability to respond, the command will overshoot the needed value for steady state operation and the loop will oscillate.

Adding a proportional gain will provide much faster response since there is no time delay for it to build up. It immediately provides a command proportional to the error and can get the motor immediately moving. However, as the motor starts to approach the reference speed, the error gets smaller and hence the proportional term gets smaller. So, in the end, the integrator will still have to build up to hold the whole command so that the proportional term can be zero when the error is zero. In fact, the motor will get up to speed even more slowly than the integral term alone since the motor starts running sooner and the integrator sees less error to make it build up.

There must be a better way. And there is: a new term, often called “feed forward” which is an estimate of how much command is required to run the motor at the desired speed. The feed forward term is an open-loop command to get the motor close to speed, then the proportional and integral terms get it right on and also compensate for load induced speed changes.

So to determine the gains for a motor speed control sytem, you first determine a feed forward gain which will drive the motor to approximately the desired speed. This feed forward term may not necessarily be a simple gain. For my PWM controlled motor, it takes about 40% PWM just to get the motor started, so my feed forward term is: 40% + Kff * Speed Reference. You can determine the feed forward term by trial and error. Just find something that will drive the motor to about the right speed.

Next you want to improve the motors response to loading changes. With just feed forward, if you load the motor more heavily, or lightly, the speed will change and stay at a new value. You can now add a porportional gain which will immediately respond in a direction to drive the motor toward the reference speed. But the proportional gain can’t get the motor all the way back on speed because as the speed approaches the reference speed, the error tends toward zero and the proportional term starts to go away. What this means is that the proportional term can reduce errors but not eliminate them. So to implement a proportional gain, run the motor and change the loading (maybe just slow the wheels down by hand). Increase the proportional gain as far as you can before undesirable effects (like oscillation or jerkiness) start to appear.

Then to take care of the steady state additional command to handle the new loading, increase the integral gain. The integral gain can be similarly increased until bad effects are seen.

Now why was steering command different in the way we set it up. We saw that speed control had no integrators in the plant and steering had two. We saw that the integral term could provide stable control for speed but not for steering. Basically, it takes derivative terms to stabilize each of the integrators in the steering plant. The proportional term is used to stabilize one integrator, and the rate term is used to stabilize the next upstream integrator.

You have to have a stable system to experiment with the gains, so for the motor speed control you can start with either the proportional or the integral. But for the steering you must start with the rate term just to get a stable system.

Appendix A

Fractions using integer arithmetic

A fraction can be represented in an integer word by assigning a “binary point” to a position in the word which you take care of in your code and calculations. The computer never knows it’s there.

For example, I’m using an Motorola 68HC912B32 computer with 16 bit integer words coded in Sbasic. I have defined many of my variable to have a binary point between the first and last 8 bits of the word. A binary point is similar to a decimal point in that all the numbers to the left of the binary point are unit value, and the number to the left are fractional. This means that the range of my word is now +/- 127 with fractional part down to 1/128 of a unit. For example, 5 feet is represented as #0500. 05 for the feet and 00 for the fraction. 5 ½ feet would be $0580.

Now, obviously, this reduces the range of the word considerably, but it does allow fractions. And you can put the binary point at different places if it suits your purposes.

So adding and subtracting fractions is easy; just add or subtract the 16 bit numbers and you will get the right fractional result.

The problem arises when you want to multiply or divide. Multiplying two 16 bit numbers gives a 32 bit result. Multiplying two numbers with 8 bit fractional components means the integer all ends in the top 16 bits, and the bottom 16 bits is all fraction. What you can do with this situation depends on what language you are coding in. If you have access to all 32 bits, you can use bits 23 thru 8 to construct a 16 bit word with the normal 8 bit fraction. If you don’t have access to the top 16 bits (as in Sbasic), you can just multiply by small enough numbers so as not to exceed the max value of +/- 127.

For instance, in wall following, I would never expect an error of more than 2 feet, so I’d feel comfortable multiplying by anything up to 60.

Now I don’t usually want to multiply by large numbers, but I might want to multiply by small numbers with a fraction. The easy way is by multiplying by one integer and dividing by another.

For example, to multiply by 2.75, multiply by 11 then divide by 4. Yes, this may be a bit awkward, but it works. Just make sure your first multiplier doesn’t cause an overflow.

P.S. If you’re using Sbasic, one thing you’ll soon find is that it doesn’t provide the signed multiply and divide necessary to make this scheme work with positive and negative numbers. It assembles to only unsigned multiply and divide (emul and idiv on the HC12). You can work around this by using the find and replace function of your editor to change all emul to emuls and idiv to idivs in the .asm file before assembling. (and if I ever get around to it I’m going to write a batch file or something to automate this…but it’s just easy enough that I don’t seem to get around to it.)

Appendix B

Code for Qbasic Steering control law simulation

'Control system simulation program for small robots.' by Alex Brown rbirac@home.com 3/21/00

'Demonstrates the effects of gain changes in a PID control system using a

'simple simulated robot steering system.' The robot is assumed to be following a straight line and can steer to the'left and right.' The starting point of the robot can be set by changing the values in the'program (no time for user friendly on-line changes).' The changable values are:' Y distance from the line in inches (+ = left of line)' Angle of robot to line in degrees (+ = pointed left)' Ksteering: the "proportional" gain' Krate: the "derivative"gain' Kint: the "integral" gain' Bias: adds an offset to the steering system

' When "Run/Start" is selected, the robot moves forward at 24 inches per'second for 25 seconds..and the results are plotted showing Y and steering.' Limits steering wheels turn angle to 20 degrees

y = 0 'inches (+ is right of line, down on graph)angle = 0 'degrees (+ is pointed to right, down)Bias = 0 'front steering off center biasKsteering = 0 'degrees steering/inchKrate = 0 'degrees steering/inch/secKint = 0 'degrees steering/ inch-second

Speed = 24 'inches per second of forward motionx = 0 'inches starting point traveling forward steering = 0 'steering starts pointed straight ahead

rate = Speed * SIN(angle / 57.3) ' calculate Yrate from angle and speed

SCREEN (11) '640x480 graphics

LOCATE 1, 1: PRINT "initial offset Y = "; yLOCATE 2, 1: PRINT "initial angle = "; angleLOCATE 3, 1: PRINT "Bias = "; BiasLOCATE 4, 1: PRINT "Ksteering = "; KsteeringLOCATE 5, 1: PRINT "Krate = "; KrateLOCATE 6, 1: PRINT "Kint = "; Kint

LINE (80, 104)-(80, 296) 'vertical start of position graph LINE (0, 200)-(600, 200) 'position zero line LINE (70, 104)-(80, 104) '12 inches to left line LINE (70, 296)-(80, 296) '12 inches to right line LOCATE 8, 1: PRINT "12 inch" LOCATE 9, 1: PRINT "Left" LOCATE 17, 1: PRINT "12 inch" LOCATE 18, 1: PRINT "Right"

LINE (80, 310)-(80, 390) 'vertical start of steering graph LINE (0, 350)-(600, 350) 'steering zero line

LINE (70, 310)-(80, 310) '+20 degrees steering LINE (70, 390)-(80, 390) '-20 degrees steering LOCATE 20, 1: PRINT "20 deg" LOCATE 21, 1: PRINT "Left" LOCATE 24, 1: PRINT "20 deg" LOCATE 25, 1: PRINT "Right"

FOR i = 0 TO 25 LINE (80 + i * 20, 400)-(80 + i * 20, 420) NEXT LOCATE 28, 1: PRINT " 0 10 seconds 20"

FOR i = 0 TO 500 'for a 25 second run

Integ = Integ + y / 20steeringtmp = (Ksteering * y + Krate * rate + Kint * Integ) + BiasIF ABS(steeringtmp - steering) > 4 THEN steering = steering + 4 * SGN(steeringtmp - steering)ELSE steering = steeringtmpEND IFIF ABS(steering) > 20 THEN steering = 20 * SGN(steering)

angle = angle + steering / 20rate = Speed * SIN(angle / 57.3)

x = x + Speed / 20y = y + rate / 20

PSET (i + 80, y * 8 + 200) 'draw position locationPSET (i + 80, 2 * steering + 350) 'draw steering angle

NEXT