DRAFT PID Closed Loop Control for robotics

locketparachuteElectronics - Devices

Nov 15, 2013 (3 years and 7 months ago)

65 views

DRAFT


PID Closed Loop Control for robotics






By Alex Brown
rbirac@home.com

5/1/00


Introduction


PID (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 necess
ary 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 systems


There are two basic types o
f 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
-
loo
p co
ntrol 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 secon
ds 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 of
f 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
“cont
roller” 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 t
he 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 a
ctuation 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 yo
u 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 tas
ks.




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 desire
d, 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 th
e 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 lo
op 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 cont
inuously 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 st
eering 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 pha
se. 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 o
f 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 c
omputes 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 p
art 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
everyti
me 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 it





Set up timer to trip at desired rate.




Loop:


Wait for timer to trip





(reset timer if necessary)






Do

real time software tasks






Input sensors






Calculate commands






Output commands



Endloop



Interrupt driven software:




Start


Initialize anything that needs it





Set up Interrupt to occur at desired rate.




Loop:


Do background tasks, if an
y






Endloop





Interrupt:

Do real time software tasks






Input sensors






Calculate commands






Output 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 softwa
re 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 re
quire 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
t
his 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 a
nd 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 sim
ple 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 basi
c 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 . K
steeri
ng

)




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 draw
ing 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 distan
ce


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 wha
t 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 cont
inue 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 ou
t 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 w
ill 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 p
er 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 fr
om 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 wan
t 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 wal
l.


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 conse
cutive 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 loop

Endloop



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 w
all.


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 follo
wing 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 d
istance.



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 t
ogether:




steering command = K
steering

* (reference distance


sonar distance) + K
rate

* 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 try
ing 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 der
ivative 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 thi
s 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 robo
t 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 c
ause 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 secon
d; 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 stra
ight . 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/derivativ
e 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 previ
ous 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 integrat
or 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 th
e 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 sig
nal 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 ou
tput 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 t
o
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 progr
am 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 dist
ance 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
whee
ls 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 exper
iments 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 21
st

line you’ll find a
group of lines that look like:


Y = 0

Angle = 0

Ksteering = 0

Krate = 0

Kint = 0

B
ias = 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 a
nd 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 ea
ch 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 respons
e 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 r
esponds 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, b
ut 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

0

12

0

0

-
10

0

-
20

6

-
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 fo
r 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 experi
ment,
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 ga
in: 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 di
stance 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, mec
hanical 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 g
et the best performance possible.



So, the only things you need to get the performance you want is the value of the three gains,
K
steering

and K
rate
and Kint. Now, there are entire books and courses in control law analysis to determine
optimal gains fo
r 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 displacemen
t
(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 os
cillation 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 t
he 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 stil
l 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 betw
een 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 subrouti
ne 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
cons
equtive 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 i
nterval 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 alway
s 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 fps



sonardistold = sonardist




The code sample assumes only that you hav
e 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” wo
rk
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 (erro
r and rate) and have a steering subroutine that
can accept the output, nothing remains except to determine the gains for the three signal paths: K
steering
,
K
rate
and K
integral
.



Start with both the K
steering

and K
integrator

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 p
arallel 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 :




K
rate

= 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 dir
ections 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 mor
e 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 robo
t’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 ve
ry 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 ac
curacy (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 K
steering

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 rob
ot 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 K
rate

gain set to 3.33, set the K
steering

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 paralle
l 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 test
ing 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 pro
blems. If you want the robot to be able to handle larger starting
errors, you can try changing the K
steering

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 se
cond 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 wal
l. 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 integration

stages.




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 deriv
ative 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 re
ference 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 woul
d 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 rem
ain 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 te
rm 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 d
esired 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 contr
ol 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 star
ted, 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
spee
d. 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 red
uce 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 jerk
iness) 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 th
e 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 c
ontrol 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 ass
igning 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. Thi
s 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 re
sult.


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 f
raction. 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 bi
ts (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.


N
ow 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 sche
me 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 bias

Ksteering = 0 'degrees steeri
ng/inch

Krate = 0 'degrees steering/inch/sec

Kint = 0 'degrees steering/ inch
-
second


Speed = 24 'inches per second of forward motion

x = 0 'inches starting point tra
veling
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 = "; y

LOCATE 2, 1: PRINT "initial angle = "; angle

LOCATE 3, 1: PRINT "Bias = "; Bias

LOCATE 4, 1: PRINT "Ksteering = "; Ksteering

LOCATE 5, 1: PRINT "Krate = "; Krate

LOCATE 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"


LOCAT
E 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


L
INE (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: PR
INT " 0 10
seconds 20"


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


Integ = Integ + y / 20

steeringtmp = (Ksteering * y + Krate * rate + Kint * Integ) + Bias

IF ABS(steeringtmp
-

steering) > 4 THEN



steering = steering + 4 * SGN(steeringtmp
-

steering)

ELSE


steering = steeringtmp

END IF

IF ABS(steering) > 20 THEN steering = 20 * SGN(steering)


angle = angle + steering / 20

rate = Speed * SIN(angle / 57.3)


x = x + Speed / 20

y = y + rate / 20


PS
ET (i + 80, y * 8 + 200) 'draw position location

PSET (i + 80, 2 * steering + 350) 'draw steering angle


NEXT