Inverted pendulum

[From Bill Powers (980624.1142 MDT)]

This post concerns the program for an inverted pendulum control system that
I presented at the ECSG meeting just finished. The source code for the
program itself and all the units used with it are attached as a series of
separate .pas files. Also attached is the executable .exe code, and in case
it's needed, the graphics interface file egavga.bgi; all attachments are
MIME-encoded. If anyone has difficulty receiving these files get in touch
with me directly and I'll see if I can help. If I can't, someone else
probably can. The program is strictly for PCs; no Macs, unfortunately. If
anyone wants to port these programs to Macs, a lot of people would be
grateful. Of course they will run on Power Macs which can emulate the PC
environment. There is no fancy programming; the units and program should
compile with all versions of Turbo Pascal from 3.0 upward (I am using 7.0).
The program is not windows-aware, although it will run perfectly well under
Windows 3.1 and Windows 95.

Also attached is an encapsulated postscript file, "invpend.eps," containing
the main diagram from the paper to be published in the proceedings.

First I'll describe the strategy of the model and how to run it, and then I
will offer a proposal to members of our group who know something about
"modern control theory" (not Hans Blom's version). A more detailed
discussion will appear in the Proceedings of the conference.

                           An overview of the model

In this program it is assumed that the control systems can sense the
position, velocity, and acceleration of both the cart and the pendulum bob.
It is assumed that in the real system this sensing is direct, through
accelerometers, tachometers, and position sensors, or there are position
(or angle) sensors from which the two derivatives can be computed in real
time.

The cart is controlled at the lowest level by a system which senses
horizontal velocity and controls it by varying a force applied to the cart.
The cart position control system at the next level up senses horizontal
cart position and controls it by varying the reference signal for
horizontal cart velocity. The cart position reference signal is the means
for the bob control system to adjust the cart position.

The first (lowest-level) bob control system senses acceleration, roughly,
by sensing the difference in x position between the bob and the cart. This
is an approximate measure of the lean angle of the bob, which is converted
by gravity to a horizontal acceleration of the bob. This control system
acts by varying the reference signal for cart position; the actual cart
position is very rapidly made equal to the reference position by the cart
control systems.

To prevent operating outside the range of achievable control, the reference
signal of the acceleration control system is limited to +/- 0.5 units
(which turn out to be meters), thus limiting the lean of the pendulum and
limiting the acceleration. In the paper presented at the meeting, the limit
was placed at the velocity control level, but subsequently it was found
that performance was far better if acceleration was limited instead.
Without limiting, the composite control system works perfectly well, but
sudden changes in the position reference level can easily drive it out of
the range of control, leading to runaway and a program halt.

The next higher level of bob control senses bob velocity and controls it by
adjusting the reference signal for bob acceleration. And the highest level
of bob control senses bob position in laboratory space, controlling it by
adjusting the reference signal for bob velocity. For optimum performance,
these two highest-level systems have strict gain limits, 0.5 in the
velocity control system and 2.0 in the position control system. This has to
do, I suspect, with the fact that there is a maximum possible horizontal
acceleration due to gravity acting on the leaning pendulum.

                          Running the model

When the program is started, the reference position of the bob is set to
zero, which is screen center, and the pendulum appears balanced in this
position. A red mark above the bob indicates where the reference position
is set. This reference position can be moved by moving the mouse
horizontally. This varies the reference signal entering the highest level
of control system, the bob position control system. When the reference
position is moved rapidly to a new position, the cart moves rapidly in the
opposite direction to make the pendulum lean in the direction of the
required acceleration. Then as the leaning pendulum accelerates, the cart
accelerates with it, keeping the angle of lean and the acceleration
constant at the maximum level until the distance of the bob from the new
reference position becomes small. Then the cart moves ahead of the bob,
making the pendulum lean backward to decelerate the bob. Finally, the cart
moves underneath the bob just as the bob comes to a stop at the new
reference position.

These complex sequences of movements are not programmed; that is, there are
no instructions that say "now lean, now accelerate, now lean the other way,
and now return to the initial configuration." These accelerations and
decelerations emerge from the simple organization of five analog control
systems in a hierarchical organization, without any instructions specifying
the observed movements.

To show how robust this control process is, provision has been made for
applying a steady force or an impulse force to the bob. Pressing 'i'
produces a 1 newton-second impulse, and pressing 'f' produces (and toggles
off) a 1-newton steady force. After the impulse disturbance, the bob
returns to the reference position very quickly. After the steady force
disturbance is applied, the bob moves to the right, and the cart moves
farther to the right, to produce a steady force the other way, with the
pendulum in equilibrium with the applied force.

Also, the control systems can all be turned off and on with the 'z' key.
When control is turned off, the pendulum eventually departs from perfect
balance (there is a random-number generator which puts enough dither in the
system to assure that you don't have to wait for an hour). It then falls to
the left or right, swinging back and forth with only a very small rate of
decay. If you wait until the pendulum is almost at the top of its swing and
then turn control back on by pressing the 'z' key again, you will see that
balance is restored immediately.

                           Future developments.

There is some leeway in the position and velocity of the free pendulum
within which control can be recaptured. This will allow adding a simple
program for erecting the pendulum from a dangling starting position.

The only remaining challenge, to complete matching the abilities of this
model with those achieved by conventional design methods, is that of adding
a second pendulum on top of the first, so the two segments are balanced by
moving the cart. From a preliminary analysis, it seems that this will be
achievable by adding three more levels for controlling the second segment
of the bob by varying the reference signal of the first bob position
control system. Since the speed with which the first bob can be moved
depends strictly on the maximum force achievable by leaning the pendulum in
a one-gravity field, either the second segment can be controlled only over
a more limited range of initial positions and velocities, or the second
segment must be longer than the first and have a lower natural period.

Some experiments have been done with controlling a normal pendulum by this
hierarchical means. Essentially the same behavior can rather easily be
achieved -- the bob moves as quickly as possible to the specified reference
position, where it stops smoothly with no overshoot. This simulates a
control system for a linearly moving crane which moves dangling loads from
one position to another.

The performance of this model suggests a practical application. Instead of
building a bridge across a chasm, an inverted pendulum ferry could be
provided which is stabilized on a long shaft over a cart running on rails
at the bottom of the chasm. Cars would drive onto the "bob" part of the
pendulum, and would then be transported to the other side, balance and
position control being maintained by moving the cart by the means used
here. The only possible difficulty would be persuading car-owners to trust
this means of crossing the chasm while balanced several hundred feet in the
air. Perhaps it is not yet time to buy stock in this venture.

                    A comparison with other approaches

This multilevel design method is largely intuitive, with little
mathematical depth. Before any general conclusions can be drawn about its
usefulness, the mathematics must be understood. I'm hoping that Wolfgang
Zocher, Christian Wenner, and Christian Heider, as well as any others with
the required mathematical skills such as Richard Kennaway, will be willing
to undertake a comparison with methods currently in use for the synthesis
of control systems, and an analysis of the mathematics of the hierarchical
method.

As an example of the kind of control engineering now being taught at one
university (Carnegie-Mellon), visit this web page:

http://www-dii.ing.unisi.it/research/control/ctm/examples/pend/invpen.html

The page is titled "Control tutorials for MatLab", and is sponsored by
Carnegie-Mellon. The text says

"For these sections we will assume that the system starts at equilibrium,
and experiences an impulse force of 1N. The pendulum should return to its
upright position within 5 seconds, and never move more than 0.05 radians
away from the vertical.

"The design requirements for this system are:

    Settling time of less than 5 seconds.
    Pendulum angle never more than 0.05 radians from the vertical."

It seems to me that these criteria are not generally achievable; they
depend on the mass of the pendulum. Since the pendulum is moved by moving
the cart to create a deviation from the vertical, the speed of the response
is linked to the departure from the vertical. If the maximum departure from
vertical is 0.05 radians, the maximum corrective acceleration will be about
0.05 gravities, which determines how far from the desired position the
pendulum will be moved by a given impulse. The pendulum must be decelerated
to a stop, accelerated back toward the reference position, and declerated
to a stop. With a maximum possible acceleration of 0.05 gravities, the
minimum settling time is determined by the magnitude of the impulse. I
suspect that these criteria were arrived at after some trial and error with
actual implementations.

The page continues:

"However, with the state-space method we are more readily able to deal with
a multi-output system. Therefore, for this section of the Inverted Pendulum
example we will attempt to control both the pendulum's angle and the cart's
position. To make the design more challenging we will be applying a step
input to the cart. The cart should achieve it's desired position within 5
seconds and have a rise time under 0.5 seconds. We will also limit the
pendulum's overshoot to 20 degrees (0.35 radians), and it should also
settle in under 5 seconds."

This seems strange to me, since the cart position is the means of
determining pendulum position. If the system is to end up in equilibrium,
the final cart position is determined by the desired (horizontal) position
of the pendulum. Thus there are not two independent outputs being
controlled; once the desired pendulum position is specified, the final cart
position is also specified: it will be directly below the pendulum. The
description above seems to imply that we could set a desired pendulum
position independently of the desired cart position. I believe this is
impossible.

What seems to be meant is that the cart will resist the application of
forces to it, while the pendulum is brought to a desired final position. My
hierarchical approach automatically achieves this result, in fact greatly
exceeding the speed and overshoot requirements listed above.

My general impression is that while there are new computational techniques
in use in this modern approach, the conceptual approach has not changed
much since my first encounter with control systems in the 1950s. This is
not surprising, since the basic principles of operation of control systems
are fixed by nature. I find the engineering analysis of control very
non-intuitive.

But all that is just my subjective impression. What we need is a formal
mathematical analysis of my method and a mathematical comparison with the
methods now being taught in engineering schools. Is there anything that can
be done with the "modern" methods that cannot be done with the hierarchical
control method? And if there is, can the hierarchical control method be
modified to eliminate any deficiencies?

Best,

Bill P.program cntcart2;

uses crt,dos,graph,grutils,mouse;

type objecttype = record x, y, fx, fy, vx, vy, mass: double end;
     csystype = record p,r,e,o:double; end;

var bob,cart: objecttype;
     cartpos, cartvel, bobpos, bobvel, bobaccel: csystype;
     deltax, deltay, dt, impulse: double;
     l,l0,f,g,ke,r0: double;
     iters: longint;
     time,level: integer;
     control: boolean;
     ch: char;

procedure initpendulum;
begin
dt := 1e-4; { sec }
ke := 1e7; { newton/meter}
l0 := 1.0; { meter}
g := 9.8; { meter/sec^2}
iters := 0;
control := true;

with bob do
begin
  vx := 0.0; {m/sec}
  vy := 0.0;
  x := l0*sin(0);
  y := l0*cos(0); {meter}
  mass := 0.5; {kg}
end;

with cart do
begin
  vx := 0.0;
  vy := 0.0;
  x := 0.0;
  y := 0.0;
  mass := 0.5; {kg}
end;
end;

procedure showpendulum;
const oldx: integer = 0;
      oldy: integer = 0;
      oldcx: integer = 0;
      oldcy: integer = 0;
begin
if iters > 0 then
begin
  setcolor(black);
  rectangle(hcenter - 30 + oldcx,
            vcenter,hcenter + 30 + oldcx,vcenter- 15);
  circle(hcenter + oldx, vcenter - oldy,10);
  circle(hcenter + oldcx - 20, vcenter - oldcy,7);
  circle(hcenter + oldcx + 20, vcenter - oldcy,7);
  line(hcenter + oldcx,vcenter - oldcy,
       hcenter + oldx,vcenter - oldy);
end;

oldx := round(200*bob.x);
oldy := round(200*bob.y);
oldcx := round(200*cart.x);
oldcy := round(200*cart.y);

setcolor(white);
rectangle(hcenter - 30 + oldcx,
            vcenter,hcenter + 30 + oldcx,vcenter- 15);
circle(hcenter + oldx, vcenter - oldy,10);
circle(hcenter + oldcx - 20, vcenter - oldcy,7);
circle(hcenter + oldcx + 20, vcenter - oldcy,7);
line(hcenter + oldcx - 20,vcenter + 7,hcenter + oldcx + 20,vcenter+ 7);
line(hcenter + oldcx,vcenter - oldcy,
      hcenter + oldx,vcenter - oldy);
line(0, vcenter + 7,hsize,vcenter + 7);

end;

procedure showref;
const oldref: integer = 0;
begin
setcolor(black);
line(oldref,25,oldref,35);
line(oldref-1,25,oldref-1,35);
oldref := hcenter + round(mousex*0.2);
setcolor(lightred);
line(oldref,25,oldref,35);
line(oldref-1,25,oldref-1,35);
setcolor(white);
end;

procedure dynamics;
begin
  with bob do
  begin
   deltax := bob.x - cart.x;
   deltay := bob.y - cart.y;
   l := sqrt(sqr(deltax) + sqr(deltay));
   f := (l0 - l)*ke;
   fx := f*deltax/l + 0.001*vx + impulse;
   fy := f*deltay/l - mass*g + 0.001*vy;
   vx := vx + fx/mass* dt;
   x := x + (vx + 0.5*fx*dt/mass)*dt;
   vy := vy + fy/mass* dt;
   y := y + (vy + 0.5*fy*dt/mass)*dt;
  end;
  with cart do
  begin
   if control then fx := -bob.fx + cartvel.o
   else fx := -bob.fx;
   vx := vx + fx/mass*dt;
   x := x + (vx + 0.5*fx/mass*dt)*dt;
  end;
end;

procedure cartvelcont;
begin
with cartvel do
begin
  p := cart.vx;
  r := cartpos.o;
  e := r - p;
  o := 200.0* e;
end;
end;

procedure cartposcont;
begin
with cartpos do
begin
  p := cart.x;
  r := bobaccel.o;
  e := r - p;
  o := 200.0*e;
end
end;

procedure bobaccelcont;
begin
with bobaccel do
begin
  p := bob.x - cart.x;
  r := bobvel.o;
  e := r - p;
  o := -200*e;
end
end;

procedure bobvelcont;
begin
with bobvel do
begin
  p := bob.vx;
  r := bobpos.o;
  e := r - p;
  o := o + 0.001*(0.5*e - o);
  if o > 0.4 then o := 0.4
  else
  if o < -0.4 then o := -0.4;
end;
end;

procedure bobposcont;
begin
with bobpos do
begin
  r := mousex*0.001;
  p := bob.x;
  e := r - p;
  o := 2.0*e;
{ if o > 0.7 then o := 0.7
  else
  if o < -0.7 then o := -0.7;}
end;
end;

procedure showlegend;
begin
setcolor(white);
outtextxy(0,0,'PRESS "q" TO QUIT');
outtextxy(0,10,'PRESS "z" TO TURN OFF CONTROL');
outtextxy(0,vsize - 24,'REF POS');
setcolor(yellow);
outtextxy(0,vsize - 37,'BOB POS');
setcolor(lightred);
outtextxy(0,vsize - 50,'CART POS');
setcolor(white);
outtextxy(100,vsize-50,'impulse = i');
outtextxy(100,vsize-37,'force on/off = f');
outtextxy(300,0,'USE MOUSE TO SET REF POSITION OF BOB');
ch := chr(0);
end;

procedure checkcommand;
const x: integer = 0;
      y: integer = 0;
begin
  if keypressed then ch := readkey;
  case ch of
   'z': control := not control;
   'i': begin
         impulse := 2.0;
         time := 1000;
        end;
   'f': if impulse = 0.0 then impulse := 1.0 else impulse := 0.0;
   '1': level := 1;
   '2': level := 2;
   '3': level := 3;
  end;
  if ch <> 'q' then ch := ' ';
  if time > 0 then dec(time);
  if time = 1 then impulse := 0.0;
  if time > 800 then
   begin
    setcolor(lightred);
    x := hcenter + round(200*bob.x);
    y := vcenter -200*round( bob.y);
    line(x-50,y,x-10,y);
   end;
  if time = 800 then
  begin
   setcolor(black);
   line(x-50,y,x-10,y);
end;
end;

begin
initgraphics;
initpendulum;
if not initmouse then exit;
l := l0; {L-nought, not 10.0}
showlegend;
showpendulum;
level := 5;
ch := ' ';
while ch <>'q' do
begin
  dynamics;
  if control then
  begin
   bobposcont;
   bobvelcont;
   bobaccelcont;
   cartposcont;
   cartvelcont;
  end;
  if (iters mod 100) = 0 then
  begin
   showpendulum;
   showref;
   if iters < 320000 then
   begin
    putpixel(iters div 500, vcenter + 120 - round(70*bobpos.r),white);
    putpixel(iters div 500, vcenter + 120 - round(70*cart.x),lightred);
    putpixel(iters div 500, vcenter + 120 - round(70*bob.x),yellow);
    putpixel(iters div 500, vcenter + 120,white);
   end
   else if iters = 320000 then
    begin
     clearviewport;
     iters := 0;
     showlegend;
    end;
   readmouse;
  end;
  inc(iters);
  checkcommand;
end;
closegraph;
end.

unit mouse;

interface
uses dos,crt;

var mousex,mousey: integer;

function initmouse: boolean;
procedure readmouse;
function readbutton: integer;

implementation

var MouseR : registers;
    dx,dy: real;

{ ---------------------- Mouse Functions -----------------------------------}
function initmouse: boolean;
begin { false if mouse not found }
mousex := 0; mousey := 0;
dx := 0; dy := 0;
MouseR.ax := 0;
intr ($33, mouser);
if Mouser.ax <> $ffff then
  begin
   writeln('MOUSE NOT INSTALLED');
   delay(1000);
  end;
Initmouse := (MouseR.ax = $ffff);
end;

procedure readmouse;
begin
mouser.ax := 11;
intr ($33, mouser);
dx := dx + 0.7*(integer(MouseR.cx) - dx);
dy := dy + 0.7*(integer(MouseR.dx) - dy);
mousex := mousex + round(dx);
mousey := mousey - round(dy);
end;

function readbutton: integer; { returns 1,2,or 4}
begin
MouseR.ax := 3;
intr ($33, MouseR);
readbutton := MouseR.bx and 3;
end;

end. { of unit }

unit GrUtils;
{ Graphics Utilities Unit }

interface

uses
  Graph, bgidriv;

var
  GraphDriver, GraphMode, Error: integer;
  hsize, hcenter, vsize, vcenter: integer;

procedure InitGraphics;
procedure Retrace;

implementation

const BGIDIR = '\tp\bgi';

procedure retrace;
begin
case Graphdriver of
  Ega,Vga,Ega64,EgaMono: begin
           while (port[$3da] and 8) = 8 do ;
           while (port[$3da] and 8) = 0 do ;
          end;
  HercMono: begin
           while (port[$3ba] and $80) = 0 do ;
           while (port[$3ba] and $80) = $80 do ;
           end;
  ELSE begin
          while (port[$3da] and 8) = 0 do ;
          while (port[$3da] and 8) = 8 do ;
       end;
end;
end;

procedure Abort(Msg : string);
begin
  Writeln(Msg, ': ', GraphErrorMsg(GraphResult));
  Halt(1);
end;

procedure InitGraphics; {ADAPTS TO HARDWARE}
begin
  { Register all the drivers }
  if RegisterBGIdriver(@CGADriverProc) < 0 then
    Abort('CGA');
  if RegisterBGIdriver(@EGAVGADriverProc) < 0 then
    Abort('EGA/VGA');

  GraphDriver := Detect; { autodetect the hardware }
  InitGraph(GraphDriver, GraphMode, ''); { activate graphics }
  if GraphResult <> grOk then { any errors? }
  begin
    Writeln('Graphics init error: ', GraphErrorMsg(GraphDriver));
    Halt(1);
  end;
  GraphMode := getmaxmode;
  setgraphmode(GraphMode);
  vsize := getmaxy; hsize := getmaxx;
  vcenter := (vsize + 1) div 2;
  hcenter := (hsize + 1) div 2;
end;

begin
end.

Egavga1.bgi (60 Bytes)

Cntcart2.exe (61 Bytes)

invpend.EPS (60 Bytes)

1 Like

[From Rupert Young (2013.10.16 21:00 BST)]

Here's a few videos, I thought might be of interest, of some work I've being doing to implement the inverted pendulum model in the real world.

···

--

Regards,
Rupert

[From Rick Marken (2013.10.16.1640)]

···

Rupert Young (2013.10.16 21:00 BST)

RY: Here’s a few videos, I thought might be of interest, of some work I’ve being doing to implement the inverted pendulum model in the real world.

This looks really interesting Rupert. Could you say a bit more about what’s going on? What did you build it with? What is being controlled? How? The title of the videos is “Two Wheeled Gyroscope”. Is a gyroscope used for balancing? If so, that’s a bit different that the inverted pendulum situation, but cool nevertheless.

Nice going!!

Best

Rick

http://youtu.be/fDz2SbS8nmo

http://youtu.be/WeMuqZ4Mpg4

http://youtu.be/9Sw5HzihZrQ

Regards,

Rupert


Richard S. Marken PhD
rsmarken@gmail.com
www.mindreadings.com

[From Rupert Young <tel:%282013.10.16%2021>(2013.10.17 11:00 BST) ]

There is a brief description on the youtube description. But basically the gyro gives bob angular velocity from which the integral gives bob angle then along with motor speed and position the control systems are similar to the computer model.

Btw, thanks to Richard Kennaway for clarifying some points about the computer model.

I have been trying a hinged pendulum but so far it is too unstable, I hope to get back to it at some point.
Regards,
Rupert

···

On 17/10/2013 02:31, Richard Marken wrote:

[From Rick Marken (2013.10.16.1640)]

Rupert Young <tel:%282013.10.16%2021>(2013.10.16 21:00 BST)

RY: Here's a few videos, I thought might be of interest, of some work I've being doing to implement the inverted pendulum model in the real world.

This looks really interesting Rupert. Could you say a bit more about what's going on? What did you build it with? What is being controlled? How? The title of the videos is "Two Wheeled Gyroscope". Is a gyroscope used for balancing? If so, that's a bit different that the inverted pendulum situation, but cool nevertheless.
Nice going!!
Best
Rick

<http://youtu.be/fDz2SbS8nmo&gt;&gt;&gt; http://youtu.be/fDz2SbS8nmo

<http://youtu.be/WeMuqZ4Mpg4&gt;&gt;&gt; http://youtu.be/WeMuqZ4Mpg4

<http://youtu.be/9Sw5HzihZrQ&gt;&gt;&gt; http://youtu.be/9Sw5HzihZrQ

--

Regards,
Rupert

--
Richard S. Marken PhD
<mailto:rsmarken@gmail.com>> rsmarken@gmail.com
<http://www.mindreadings.com>> www.mindreadings.com

[From Adam Matic 2013.10.17. 13:40 cet]

···

Looks great. Could you give more details? I’m thinking of making a similar thing soon. Did you need much smoothing of the gyro signals? Are the motors already PID servos or did you have to write control loops for them?

Adam

[From Rupert Young (2013.10.18 20.30 BST)]

Yes the gyro was smoothed to counter drift. The motors aren't PID servos. I'll send you the code.

Regards,
Rupert

···

On 17/10/2013 12:41, Adam Matic wrote:

[From Adam Matic 2013.10.17. 13:40 cet]

Looks great. Could you give more details? I'm thinking of making a similar thing soon. Did you need much smoothing of the gyro signals? Are the motors already PID servos or did you have to write control loops for them?

Adam