In the previous modules, after some cautious tests a position control loop has been closed. A variety of nonlinear strategies have been investigated to obtain the performance required of an actuator such as a robot axis.
Most of the feedback decisions were made empirically, experimenting to find values which would give a swift response with no overshoot. For control of an inverted pendulum, we need rather more theoretical background. The module starts with experiments to determine the mathematical model of the system. Practical responses then compare the actual and modelled performance.
The inverted pendulum is now added to the positioned 'trolley' and the sensor for tilt angle is investigated, together with its interface. Digital signal processing is then developed to enable a tilt-rate signal to be estimated.
A loop is closed with tilt and tilt-rate signals. The top of the pendulum is constrained and the result is position control of the trolley by means of the tilt sensor. Trolley position and velocity signals are now added. The sense with which they are added is the opposite of that used for position control. As a result, when the top of the pendulum is slowly moved from side to side the trolley moves to cause the pendulum to lean 'inwards'.
At last the pendulum is released. With the suggested feedback values,
the pendulum is found to be stable. By trial and error adjustment of the
gains, the performance can be much improved.
To make an accurate mathematical model of the system, we need to measure a number of parameters. By modifying some of the earlier programs, we can let the computer do most of the hard work for us.
The key parameter is the acceleration of the trolley under full drive. Another is the effect of the velocity on the acceleration.
What units should we use to measure the position with? The most convenient measure is in terms of the potentiometer voltage. Since we have scaled the ADC output to the range -1 to 1, we will take this range to be two units of position.
Next we need to have some absolute scale of time. We must use the interrupt
method in preference to one which relies on the speed of a program loop.
The following program will calibrate the interrupt process to give the
correct value to use for dt.
CLS
PRINT "Calibrating dt - please wait five seconds"
PLAY "mbl64t255"
ON PLAY(1) GOSUB rates
PLAY ON
PLAY "cde"
t = TIMER
n=0
DO
LOOP UNTIL n>=500
PLAY OFF
PRINT "Use dt =";(TIMER - t)/n
END
rates:
n = n + 1
PLAY "n0"
RETURN
Now for a program to measure acceleration. Edit the program which follows to use the dt value which you have just found.
Load in the program version you used in module 2, section 9. Delete large quantities of it to leave just the skeleton:
CONST adstart = &H2A6 'Or single-chip constants
CONST adhi = &H2A7
CONST adlo = &H2A6
CONST readybit = &H40
CONST port= &H378 '(might be 278)
CONST tmax= 1
SCREEN 12
WINDOW (0,-1) - (tmax,1)
dt = .01
PLAY "mbl64t255"
ON PLAY(1) GOSUB rates
PLAY ON
PLAY "cde"
'A
PLAY OFF
OUT port, 0
END
rates:
v = ADC(0)*5
x = ADC(1)
'B
t = t + dt
IF t > tmax THEN t = 0
PLAY "n0"
RETURN
FUNCTION adc(chan%) 'Or single-chip version
DIM hibyte AS INTEGER
OUT adstart, &H80+chan%
DO
hibyte = INP(adhi)
LOOP UNTIL hibyte AND readybit
adc = (INP(adlo) + 256*(hibyte AND 15) - 2048)/2048
END FUNCTION
To test the acceleration, we first ask for the trolley to be moved by hand to the left hand end of the track. When we press <space> for 'go', the trolley accelerates under full drive. When it reaches the centre, the drive is removed and friction brings the trolley to a halt.
At the point marked 'A', insert
OUT port, 0
PRINT "Move the carriage to the left end and press <space>"
DO
LOOP UNTIL INKEY$ = " " 'Wait for key
x0 = ADC(1)
t=0
OUT port, 1 'Off we go
DO
PSET (t, x), 14
LOOP UNTIL x>=0 'When we get to the middle
t1 = t 'make a note of the time
x1 = x 'and check the position
OUT port, 0 'switch off and coast to a halt
t=0
DO
LOOP UNTIL t >.9 'Should stop before 0.9 seconds
x2 = x 'How far did it coast?
a = 2 * (x1 - x0) / (t1^2) 'using s=1/2 a t^2
b = a * (x1 - x0) / (x2 - x1) 'equating 2 a s
PRINT "Acceleration = "; a
PRINT "Deceleration = "; b
The fact that the stopping distance is if the same order as the 'run up' suggests that the major slowing effect is not viscous drag but friction. On this assumption we can model the trolley with two equations:
dx/dt = v
dv/dt = c u - friction
where u is +1, -1 or zero according to the drive setting. Friction will be of constant magnitude, multiplied by the sign (+/-1) of the velocity. Its magnitude will be given by the deceleration we have just found.
Since the drive has to act on top of the friction, the constant c will be the sum of the acceleration and the deceleration. We are now in a position to make a mathematical model.
At the point marked 'B' in the rates subroutine, add
vm = vm + (30 * u - 15 *SGN(vm)) * dt
xm = xm + vm * dt
OUT port, 1' insert
u = 1
xm = x
u = 0
xm = x
PSET (t, xm), 12
As a result the new program will show the actual and modelled positions superimposed.
Before we move on, let us look at the velocity.
We can see the tacho value, which we input as
v = ADC(0) * 5
PSET (t, v)
PSET (t, vm), 10.
PSET (t, vm/20), 10.
vm really is the time derivative of xm. When we calculate the gains to use, we have to take in a factor of around 100 between ADC(0) and the derivative of x.
Summary
You have measured some important parameters and used them to build a
reasonably accurate model.
The pendulum takes the form of a one metre length of lightweight aluminium tubing (or several choices for the new experiment). At the lower end, a pivot connects it to the 'trolley' of the position control experiment. At this pivot, a sensor detects the tilt angle of the pendulum.
The sensor is a linear Hall effect sensor, a chip the size of a small transistor with three connections. Supply lines of 5 volts and 0 volts are connected to two of these, while the third delivers an output voltage which varies about 2.5 volts in proportion to the normal component of the magnetic field.
The sensor is fixed in the pivot which is locked to the trolley. A small powerful magnet is contained within the pendulum rod that rotates about it. As a result, the sensor voltage represents the sine of the tilt angle - effectively the same as the angle over the small range which is relevant.
Connect the device across the 5 volt output of the power supply.
Connect
the sensor's output to ADC 2. Don't forget to link the ground
terminals! (In the new experiment there are two sensors at right
angles.)
At the point 'A' in the 'skeleton' program above, insert the loop
DO
t = t + dt
IF t > tmax THEN t = 0
v = ADC(0) *5
x = ADC(1)
tilt = ADC(2)
PSET (t, v)
PSET (t, x), 14
PSET (t, tilt), 12
LOOP UNTIL INKEY$ = "q"
To centralise the tilt reading, we have to subtract the equivalent of 2.5 volts. This will be 1 / 6 (about). Replace the 'tilt =' line with
tilt = (ADC(2) - 1/6) * 10
Rotate the pendulum and check that the value swings equally positive and negative. After making sure that a 'lean' to the right gives a positive signal, fine-tune the '1/6' value to give a zero reading when the pendulum is 'straight up'.
Summary
You have interfaced the pendulum sensor to obtain a 'tilt' signal.
Now we use the interrupt routine again to estimate the tilt rate from the tilt signal. Since we are using a substantial gain already, this estimate might be rather noisy.
tiltrate = (tilt - slowtilt) * 20
slowtilt = slowtilt + tiltrate * dt
Add these lines to the 'rates' routine and also add
PSET (t, tiltrate), 13
Now we have all four state variables to hand. We can try to control
the system.
We have two coupled second order systems. One of these is the trolley. If for now we ignore the friction and other nonlinear effects, we can describe it by
d2x/dt2 = a u
where a is a constant with a value of the order of 15.
For the pendulum, since the trolley acceleration is proportional to u we can write:
d2(tilt)/dt2 = b* tilt - c * u
Choose state variables
x2 = dx/dt
x3 = tilt
x4 = tiltrate
Now when we apply feedback so that
u = e x1 + f x2 +g x3 + h x4
we can substitute back to get the closed loop matrix equation. Then we work out the characteristic equation. For stability all the coefficients must be positive and this shows us conditions which e, f, g and h must satisfy.
It is no surprise that g and h must be positive. That means that if the pendulum is leaning to the right or falling to the right, then the trolley must accelerate to the right.
We also find that e and f must be positive - if the trolley is to the right of centre we must accelerate it further to the right! How can this be true?
One explanation is that by accelerating the trolley to the right, we cause the pendulum to lean to the left which in turn will result in the trolley being moved to the left.
From the characteristic equation, if we know the values of a, b and
c we can assign feedback variables for any choice of pole positions. Rather
than go into the mathematical details, however, we will try to build up
the parameter choice progressively.
We start with the final program from section 9 of module 2. We add lines to measure tilt and to compute tiltrate, plus lines to display them. We also lose the lines concerning vdemand. There is no need to display vobs.
CONST adstart = &H2A6 'These constants and ADC routine are for
CONST adhi = &H2A7 'the original experiment.
CONST adlo = &H2A6 'Substitute new ones if necessary.
CONST readybit = &H40
CONST port= &H378 '(might be 278)
CONST tmax=4
dg = .1
CONST vmax = 0.1
k = 1
kv = 10
kt = 10
SCREEN 12
WINDOW (0,-1) - (tmax,1)
dt = .01
PLAY "mbl64t255"
ON PLAY(1) GOSUB rates
PLAY ON
PLAY "cde"
DO
a$ = INKEY$
if a$ = "." THEN xdemand = xdemand + 0.1
if a$ = "," THEN xdemand = xdemand - 0.1
if a$ = "0" THEN xdemand = 0
g = g + dg
IF g > 1 THEN dg = -.1
if g < 0 then dg = .1
IF u > g THEN
OUT port, 1
ELSEIF u < g - 1 THEN
OUT port, 2
ELSE
OUT port, 0
END IF
LOOP UNTIL a$ = "q"
PLAY OFF
OUT port, 0
END
rates:
v = ADC(0)
x = ADC(1)
tilt = ADC(2)
tiltrate = (tilt - tiltslow) * kt
tiltslow = tiltslow + tiltrate * dt
vobs = (x - xlsow) * kt
xslow = xslow + vobs * dt
u = 10 * tilt '***This is the line we work on
t = t + dt
IF t > tmax THEN t = 0
PSET (t, v)
PSET (t, x), 14 'yellow
PSET (t, tilt), 10 'green
PSET (t, tiltrate), 12 'red
PLAY "n0"
RETURN
FUNCTION adc(chan%)
DIM hibyte AS INTEGER
OUT adstart, &H80+chan%
DO
hibyte = INP(adhi)
LOOP UNTIL hibyte AND readybit
adc = (INP(adlo) + 256*(hibyte AND 15) - 2048)/2048
END FUNCTION
Move the trolley to the right by hand and make sure the yellow trace rises on the screen. move it swiftly a little way to the right and make sure the white velocity trace also rises. Lean the pendulum to the right - the green trace should rise. Hold the pendulum straight up and the green trace should be central. Wave the pendulum to the right and the red trace should be positive for a short burst.
If all is well, we are ready to begin.
The whole feedback exercise comes down to finding a suitable expression for the "u =" line. In the listing above we have started with
u = 10 * tilt
You will see that this feedback arrangement acts as a sort of position control. As you move the top of the pendulum to the left, the trolley moves to follow it. There is no damping, however, so the response will be fairly oscillatory. We should add some damping.
Do this by changing the vital line to be
u = 10 * tilt + 10 * tiltrate
Time for some position feedback. Change the line again to
u = ?? * tilt + ?? * tiltrate + ?? * 10 * x
Hold the pendulum again and power up. Now you will see that as you move the pendulum left and right, the trolley follows - but does a little more. It moves so that the pendulum leans inwards - or at least it should if you have the coefficients right. Adjust the coefficient of x so that the pendulum rotates about a point roughly twice as high as the length of the pendulum.
Now give the pendulum another solo run, starting near the centre. The trolley will 'swing' to and fro with increasing amplitude. Catch it before it hits the ends.
Now add yet another term, a constant times v. When you have the right coefficient the 'swing' will be damped and the pendulum will 'rest' near the centre. 'Rest' might not really be the right word, since the trolley will jitter to and fro.
You can instead use a multiple of vobs. It will actually give a smoother response. So now the only sensors used are the potentiometer measuring x and the sensor measuring tilt. The other variables are deduced from these.
We can go a little further by replacing 'x' with (x - xdemand). As the keys are tapped, the trolley will obediently wobble to the left or right as commanded.
Summary
You have followed a systematic process to arrive at feedback coefficients which will stabilise the pendulum. It would make a good exercise to evaluate the characteristic equation with these values substituted into the matrix equation above.
A further exercise is to extend the simulation to include the whole system. For the coefficient 'b' of this section, remove the pendulum and hang it upside down. Measure the time of the swing - timing over ten swings, say. Take the number of swings per second, multiply by 2 pi and square the result. The answer is the value of b.
The value of c is the length of one unit of 'x' divided by two-thirds of the length of the pendulum - a value around 0.5.
To polish up the program for demonstration purposes, you can add some 'safety' lines. The drive is normally set by the value of u in combination with g and gap. If x approaches the values representing the ends of the track, the drive can instead be set to avoid a collision. Similarly overriding constraints can be put on 'outward' velocity near the ends.
It is possible to detect by software when the pendulum has 'toppled'
and to perform a manoeuvre to set it up again.
In the new pendulum, which can hang down, the two sensors make it possible to try to swing it up automatically.