Professional Documents
Culture Documents
i-Pendulum
Part 1 –
modeling, control laws, Kalman filter
When I presented
my prototype of
the inverted
pendulum, a
colleague
asked me:
“And so you
just woke
up one morning
and said to yourself
‘today I’m going to make
an upside-down pendulum!’?”
This project ought to elicit the
same astonishment among your more, the movement equations are
far from being linear, leading to com-
own family and friends, because this plex control laws. To keep it simple, I’ve
‘pendulum’ is capable of not only remaining gone from a 3D cube to a 2D square, of
balanced, but also raising itself up on its own. course with a certain thickness, capable
of balancing around a single axis. And
since this square didn’t need to actually
“roll”, I’ve cut it in half, to end up with
this triangular shape.
By Jean-Sébastien Gonsette (Belgium) a moving trolley, a quadcopter carrying
a rod with a glass balanced on top of it. Anatomy of an inverted
The specific feature of an inverted pendu- But none of those matches my design pendulum
lum is that its center of gravity is higher criteria, namely: Figure 1 is an exploded view of the var-
than its rotational or pivoting axis. So • Compact (around 4” / 10 cm) and ious elements of the pendulum; it has a
it’s a pendulum with its head at the bot- light, so I can display it on my desk. limited number of parts. Its skeleton is
tom, and all it wants to do is fall over – • Completely self-contained, i.e. the made up of two plastic half-shells, pro-
like the human body. However, the brain pendulum carries its own battery. duced using a 3D printer (we’ll come back
works in conjunction with the eyes and • Simple, so as to demystify the gen- to this in Part 2 of this article). The first
ears to send adjusting nerve impulses to eral principle without adding unnec- half shell is used to support the PCB, while
the muscles when our torso moves with essary complexity. the second contains the motor and its
respect to our ankles (pivoting point). inertia wheel. The brains of the pendulum
Inverted pendulums are based on this I found a recent project by the scientists are a PIC microcontroller that controls
principle. The video [1] shows my pen- at the Federal Polytechnic in Zurich, spe- and powers the motor. The Lipo battery
dulum in action. On the Internet, you’ll cializing in dynamic systems and con- (common in modeling) slips directly into
find a great many videos on this recurring trol: Cubli. It’s a cube that balances on its holder behind the PCB. The motor, the
university subject. It allows students to one of its edges or corners and jumps to pendulum’s sole control device, is held in
test all sorts of regulating techniques, to move from one position to another (video a housing provided for this purpose in the
get to grips with a multi-discipline sub- [2]). Cubli does meet my first two cri- plastic shell. An inertia wheel is directly
ject — and has a surefire “Wow!” factor teria, but maybe not the last one. With coupled to this motor. This inertia wheel
to boot. There’s something in it for every- three degrees of movement so as to move consists of a plastic shaft, also 3D printed,
one: on two wheels, as in the Segway, a and keep its balance in all directions, the onto which is crimped a metal ring whose
vertical bar (single, double, or triple) on modeling is pretty challenging. What’s mass provides the desired inertia.
Figure 2. Pleasing to the eye: the simulation of the stabilization of the Figure 3. Dynamics of the pendulum and its inertia wheel.
pendulum.
that the total angular momentum does left, sometimes to the right, but won’t and then to dimension the various ele-
not change, i.e. it remains zero (2nd prin- fall over. ments correctly: size of the pendulum,
ciple). And this is indeed the case: the battery weight, motor torque, electronic
sum “inertia wheel angular momentum And the jump? circuit power, etc.
(“+”) + angular momentum of the rest The pendulum’s ability to jump up (go Since I am starting off from nothing, I
of the satellite (“−”)” never varies if the from the lying down position to the bal- choose to define certain parameters arbi-
only forces involved are those between ancing position) is surprising, but easy trarily and use simulation to verify that
the internal elements. to explain. All we have to do is acceler- my model stands up, before building the
ate the inertia wheel in the direction in prototype (see Figure 2, the result of a
For the pendulum, the situation is which we want it to jump, until it gets up regulation tested on a PC). The model lim-
practically the same, except that the sufficient speed. At this point, the wheel its the risk of failure, but it is also essen-
external force is not zero because must be locked so the whole pendulum tial for writing the pendulum’s onboard
of the action of gravity. If you try will lift off. Let’s come back to Newton: algorithms (motor control, analyzing the
to keep the pendulum balancing on data received from the detectors, and
its tip, gravity will turn the slightest a. When the pendulum is on the floor, estimating the state of the pendulum).
imbalance into a non-zero angular with the wheel stopped, its angular To obtain the pendulum dynamics equa-
acceleration until it hits the baseplate. momentum is zero. tions, we need to determine the number
To counter this, the pendulum’s center b. It accelerates its inertia wheel and of degrees of movement in the system,
of gravity would have to be brought thereby gains angular momentum draw up an exhaustive list of the forces
back to exactly above its pivot point. by pushing against this floor. In the and torques acting on each of the ele-
As this point is tiny, in practice this absence of a support, this operation ments, describe the dynamic effects of
is impossible. However, it is perfectly is impossible; this wouldn’t work in each of them, then sift through every-
possible to bring the pendulum’s center space. thing, keeping just those parameters that
of gravity above the pivot, on average. c. When the rotational speed is high are of interest.
Thus the pendulum is pulled back to enough, blocking the wheel amounts The pendulum has two degrees of
the left when it moves to the right, to reducing the angular momentum movement:
and vice-versa, making it possible to to zero. Since the angular momen- • the body, which pivots around its
counter the action of gravity and keep tum of an isolated system is con- point of contact with the floor, with
it balancing. This is where the motor served and cannot disappear, it is an angle θ
and its inertia wheel come into it. Just transferred somewhere else: to the • the rotation of the motor and its iner-
as for the satellite, the body of the pendulum itself! The total angular tia wheel, with an angle φ.
pendulum accelerates in one direction momentum does not vary.
when the inertia wheel goes off in The object of the exercise is to find how
the other direction. It is necessary to Modeling the pendulum dynamics the second derivatives of these two
constantly detect in which direction In order to regulate a physical system like parameters change, i.e. their accelera-
the pendulum is falling and deduce this pendulum, you need to be familiar tion, as a function of the torque deliv-
from this the motor torque value to with the laws governing its movements ered by the motor. Figure 3 lists all the
be applied in order to compensate according to the commands being applied. parameters acting on the pendulum body
this imbalance. If the drive is done So it is necessary to model the pendu- and the wheel:
correctly, the pendulum will oscillate lum’s dynamics, which lets you study the • Wheel: the motor torque itself τm, the
by a small amount, sometimes to the problem to be solved in greater detail, frictional couple between the rotor
Motor Torque
stabilisation (to 0)
Figure 4. The eigenvalue, master of the universe. Figure 5. State feedback control..
and the stator τf, and also the two dish. n: birth rate of the organisms; The equations governing the behavior
forces that hold the rotor in place fwx d: their death rate. of the pendulum are a bit more compli-
and fwy. • If both these rates are propor- cated than in this example. The various
• Pendulum: the forces and torques tional to the size of this population, states of the system, like the angle and
acting on the wheel, but in the oppo- then y’ = ny − dy. velocity of the pendulum or the motor
site direction; the two forces between speed, have to be converted into vec-
the pendulum and the floor fbx and fby If these rates are constant, it is easy to tors, while the multiple differential equa-
that maintain the pendulum pivot in integrate this equation – the result is the tions must be transposed into matrices.
place. population growth rate. If the initial pop-
ulation is y0, we obtain: At the end of the day, we are calculating
Gravity g acts on the mass of both the the eigenvalues of a matrix – hence it is
pendulum mb and wheel mw. I’ll spare you y(t) = y0 ∙ e(n–d)∙t = y0 ∙ eλ∙t less easy to see why the system is sta-
the calculations; we obtain the following ble or not. If one of the eigenvalues is
two dynamic equations: The exponential function shows that the positive, then something in the form of
M ⋅ g ⋅ sin θ − τ m + τ f population will develop in a radically dif- eλ∙t appears somewhere as soon as you
θ!! = ferent way, depending on the sign of perform an integration on your system,
I
n − d. If the death rate is higher than the and everything goes awry.
(I + Iw )⋅ (τ m − τ f ) M ⋅ g ⋅ sin θ
ϕ!! = − birth rate, the exponent is negative: the
I ⋅ Iw I population gradually dies out. In the other Regulation
where M = m b ∙ l b + m w ∙ lw and case, the population grows endlessly and PID regulation is best suited to a sys-
I = Ib + mb ∙lb2 + mw ∙ lw2 colonizes the universe (at least, if the tem with one control input and one out-
equations still have any meaning at this put variable. Now our pendulum has two
The terms mb, Ib, mw and Iw are the mass scale). The exponent λ in this equation is degrees of movement: the angle of its
and the inertia of the pendulum body called the eigenvalue of the system. The body and the rotational speed of its iner-
and inertia wheel respectively mb ; lb and sign of this value is what determines if the tia wheel. These two dimensions have
lw are the height of the center of grav- system is stable (value “−”) or unstable to be both controlled using a single con-
ity of the pendulum body and the height (value “+”) (Figure 4). trol input: the motor torque. The tricky
of the inertia wheel with respect to the Modifying the pH in the dish makes it point here is that a motor can only deliver
floor. These equations are used not only possible to control the rate of growth/ torque over a given speed range. If I fail
for simulating the pendulum’s behavior, decline. If this control is proportional to to regulate the rotational speed, it will
but also for establishing the motor con- the quantity of individuals, the develop- drift until it reaches unacceptable values
trol laws. ment equations become: and it will no longer be possible to control
the pendulum. Out goes PID regulation!
Stability of the pendulum y’ = ny − dy + ky; So I am going for state feedback con-
In order to mathematically study the y(t) = y0 ∙ e(n–d+k)t trol, which is based on the value of the
pendulum’s instability, I’m suggesting an system’s internal states. In concrete
analogy with biology. A Petri dish makes This is where the operation is smart. As terms, the loop is created by introducing
it possible to grow micro-organisms in the parameter k can be freely chosen a real gain factor for each output-to-input
order to study their development. Here (within the physical limits of the system), feedback possibility. In the case of the
are my assumptions: it is possible to force a positive eigen- pendulum, the states being controlled are
value to become negative, which allows the angle of the pendulum, the rotational
• y: number of organisms in the the system to recover its stability. speed of the pendulum, and the rotational
Control
Measure
Instant t+1
Sensors
Figure 6. Gaussian probability density function. Figure 7. Principle of the Kalman filter.
speed of the inertia wheel — hence three pendulum speed, and motor speed? Either of the pendulum will be determined in
outputs. So the feedback loop is based on predict these states, or measure them. accordance with the model and the mea-
the sum of three gains applied to a single If the initial state is known, I can predict surements from the detectors, weighted
control input (Figure 5). It is possible to the way it will change depending on the according to their respective validities.
calculate gains such that the eigenvalues torque setpoint applied to the motor. I This principle lies at the heart of the Kal-
of the pendulum are all negative, giving integrate the differential equations in the man filter: a states filter that performs
a stable pendulum. model in order to find out where the pen- a weighted average of the various mea-
dulum will be an instant later, and I do the surements and predictions in a dynamic
Different approaches are possible, but same again for the next instant… It’s like process. This filter removes as far as pos-
it’s fairly convenient to apply the Lin- trying to drive a car with your eyes shut sible the noise affecting the measure-
ear Quadratic Regulator (LQR) theory. on the basis of a mental image of the road ments and predictions in order to obtain
This provides an algorithm for calculating ahead and the behavior of the car. That an optimum estimate of the state of the
these gains automatically, in accordance will work OK… for a few seconds! Due to process. It breaks down into two steps:
with the system equations and a series modeling errors, the prediction departs
of weighting factors. These weightings, from reality. So measurement seems to 1. Using the model of the system,
defined by the designer, correspond to the be a more reliable method. However, all together with the commands pro-
importance given to the different states detectors have a certain degree of inac- duced at an instant t, to predict the
of the system and to the control during curacy and are subject to measurement state of the system at t + dt. The
regulation. The higher a weighting is, the noise. This means that two successive prediction also estimates how the
greater the extent to which the algorithm measurements of an identical state will current uncertainty about the state
will optimize the associated state or con- not yield the same result twice. What’s of the system changes depending on
trol; it will converge faster towards its more, it’s not always possible to measure the uncertainty on the model itself.
set value. everything. Depending on the process, All the uncertainties in a Kalman
The most important thing here is to re-es- certain states will be deduced indirectly. filter are the subject of Gaussian
tablish balance using minimum torque, So on the one hand I have a method that approximations. For a system hav-
whether this takes one second or three. In is accurate in the short term, but whose ing only a single state, the classic
point of fact, the motor torque is limited: error increases rapidly, and on the other, Gaussian curve is shown in Fig-
hence the aim is to get the maximum a method with a fixed but non-negligible ure 6. In other cases, this becomes
out of the motor, while compensating for error range. The ideal is to use the one a multi-Gaussian curve with several
the largest possible imbalance. I’m not that offers the lowest error at a given dimensions.
going to say any more about the control moment. Let’s take an example: I mea- 2. Measuring the state of the sys-
theory. The subject is too huge to be sure a resistor using a top-end multime- tem at t + dt. These measurements
fully discussed here. Go and take a look ter (68 Ω), then using one costing only too have their own share of uncer-
at the University of Michigan’s series of a few dollars (70 Ω). The first measure- tainties, once again modeled by a
tutorials about controlling systems using ment seems to be the most reliable. In number of Gaussian curves. All the
Matlab, including one on the regulation fact, I need to take the weighted average measurements are weighted by their
of an inverted pendulum [3]. of both measurements using weightings respective uncertainties, as in the
calculated in proportion to the accuracy multimeter example.
Pendulum state after passing of each of the instruments. The more
through the Kalman filter accurate the instrument, the higher the Then the result of the measurements is
How can we find out the states of the pen- weighting of its measurement. I apply merged with the prediction, here again
dulum at all moments: pendulum angle, this principle to the pendulum. The state taking the respective uncertainties of the
two estimates into account. Figure 7 only the first two are used in the Kalman • The accelerometer measures pri-
offers a summary of the process. If you filter. The latter is rather rudimentary, but marily how the acceleration due to
are interested in this subject, read the yields a result that is more than sufficient. gravity is projected onto the X and Y
book referred to at [4]. How is the information from the two axes. Thus by trigonometric inver-
Here, a gyroscope and an accelerometer detectors merged? sion, it gives a direct measurement
measure the angle of the pendulum to the • the gyroscope provides the pendu- of the angle the pendulum makes
floor. The gyroscope is only interested in lum’s rotational speed. By integrat- with the floor.
a single axis, oriented in the Z direction, ing over a time increment dt, we
perpendicular to the PCB. The acceler- can predict the future angle of the The prediction and the measurement are
ometer has three axes, X, Y, and Z, but pendulum. combined in proportion to the errors in
+3V3 +3V3
+3V3
K2
R1 MCLR 1 2
C8 C9 C10 C11
3 4
10k
AVDD
VDD
MCLR 1
100n MCLR +3V3
130R
130R
130R
C1 5
VSS
VSS
C7 C2+
7
8 19 27 V-
10u 6
100n C2- C5
1 12 100n
EN FORCEON
GND
14
+3V3 +3V3A +3V3 +12V
VDD VS
4 33 10n D2 220n
IC4 14 SPI CLK 50V 50V
ADX INT 8 SCL/SCK VSA VSB
INT1 12 SPI MISO BDLC FWD 26 7
SDO/ALT ADDR FWD/REV VCP
7 ADX CS BDLC BRAKE 29 30 1N4148W K3
9 CS BRAKE VBOOT
INT2 13 BDLC TACHO 22 11
SDA/SDI/SDIO TACHO
IC5 BDLC DIAG 11 IC3 9 10
ADXL345 1 DIAG H3
GND GND GND 4 10 9
XY
=1 2 R7 H1
2 4 5 BDLC EN 27 8 8
Accelerometer 100k EN H2
SN74AHC1G32 7
R12
SPI MOSI BDLC REF 28 32 6
56k VREF OUT3
+3V3A +3V3 5 5
PWM to DC C16 OUT2
tau = 0.11ms 15 4
L6235 OUT1
5n6 3
C23 C24 13 12 2
RCOFF SENSE A
24 25 1
RCPULSE SENSE B
100n 10n R13 R14 R15 R8 R9 R10 R11 R16 R17 R18
C17 C18 C19
GND
GND
GND
GND
16 1 Motor
24k9
11k
20k
4k7
4k7
4k7
1R
1R
1R
1R
10 8
F1 VBAT SYNC/SHDN EXT
100n
C32 IC7
1u FDS6680
8A R20 1 6 AS VBAT +3V3
LDO CS+ 1 5
K4 IN IC9 OUT
MAX668
20k
2 4
REF
LP2992-33
3 4
BAT SENSE 2 5 ON/OFF BYPASS
1 C33 FREQ FB
R21 220n R25 R24 R26 GND
C26 C31 GND PGND C29 C34
C38 2 C39 C40
0R015
160k
24k9
10k
3 7
100n 100n 68u 16V 100p
1u 10n 10u
150175 - 11
each of the detectors. The speed of the why particular care must be taken when control the torque delivered, since this
wheel is estimated following the same choosing this capacitor. motor’s torque is proportional to the cur-
principle. The torque demanded from the rent drawn.
motor allows us to make a prediction of Motor
the wheel’s acceleration or deceleration, I’ve chosen a brushless motor from the Detectors
and hence of its speed after a time incre- EC Flat series by Maxon. Compared to The pendulum can only remain balanced
ment dt. This prediction is then associ- a traditional DC motor, the absence of if it measures its position in space and
ated with the speed measurement per- brushes avoids the associated electro- corrects it accordingly. A pair of detec-
formed every time a tachometer pulse magnetic noise and premature mechan- tors enable it to measure its angle with
is received. ical wear. What’s more, this model allows respect to the floor: a single-axis gyro-
better power conversion. However, its scope (ISZ-2510, IC6) and a 3-axis accel-
Pendulum electronics wiring and control logic are more com- erometer (ADXL345, IC4). These two ana-
That’s enough theory, now for the prac- plex. You can’t just hook this motor up log detectors are each housed in a sepa-
tice. The pendulum’s onboard electronics to a power source: it must be powered rately-powered digital IC. In this way, the
are very conventional, and have to ful- in three phases, switching their mag- analog measurement is polluted as little
fill four major functions that are brought netic flux at a precise rotational rate of as possible by the digital noise from the
together in the schematic in Figure 8: the rotor. Hence an electronic system is rest of the circuit. The two ICs dialogue
necessary to supervise the sequence in with the µC via an SPI bus.
1. Converting the voltage from the Lipo which the windings are powered. This is
battery to power the brushless motor the role of the L6235 (IC3), which con- Microcontroller
2. Delivering the motor drive signals tains all the logic for driving this motor, All the pendulum’s brain-power is con-
3. Measuring the dynamic parameters including the power transistors. It has a tained within a 16-bit dsPIC microcontrol-
of the pendulum via an accelerome- set of simple I/Os: ler. Compared to a 32-bit PIC, the 16-bit
ter and a gyroscope family is ideal for a project like this, where
4. Implementing the motor control law • Fwd/Rev: rotation direction wanted responsiveness is more important than
(µC) according to the values returned • Brake: enables the electromagnetic calculating power. What’s more, the RAM
by the detectors. braking function (this is what enables and ROM memory space is fairly generous
the pendulum to jump) and makes it possible to develop more
Powering • En: activates the rotation of the complex applications than on an 8-bit
The voltage provided by the Lipo battery motor µC. And lastly, the dsPICs also have the
varies between 7 V and a little over 8 V, • Vref: analog input for selecting the advantage of PWM outputs, used here to
depending on its state of charge. The limiting value for the current supplied control the motor. There’s nothing special
Maxon motor requires a nominal voltage to the motor about the programming of the µC: ICD/
of 12 V. The switch-mode power supply • Diag: signal when internal protection ICP interface wired to connector K2. One
based on a MAX668 chip performs the is tripped of the UARTs is used to produce a serial
necessary power adaptation (boost reg- • Tacho: delivers tachometer pulses. port via the SN65C3221 (IC2).
ulator). The motor draws a current of 2 A
for a torque of 55 mNm; the power supply The conversion of a PWM signal into an The next installment is devoted to more
will supply at least double this current. analog voltage via the low-pass filter concrete tasks like assembling the cir-
When pin 8 of the MAX668 is high, power formed by R12, R13, and C17 makes it cuit, the mechanical construction, and
transistor T1 conducts and the current possible to define the limit value for the calibrating the detectors.
through power inductor L1 increases. torque. The connection to the motor’s (150175)
Diode D3 prevents the output capacitor three phases is made via outputs Out1 to
C29 from discharging at the same time. Out3, while the position signals from the
When pin 8 goes low, the transistor is motor’s Hall-effect sensors are decoded
turned off, forcing the magnetic energy via inputs H1 to H3. The IC checks the
accumulated in L1 to flow into C29 via current drawn by the motor by measuring
the diode. The MAX668 regulates the it across resistors R8–R11 and compar-
cycles of this process at a configurable ing it with the setpoint defined on Vref.
frequency, here set to 300 kHz by R25. This circuit makes it possible to correctly
This regulation is based on the output
voltage via divider R23 / R26, together
Web Links
with the current flowing in the inductor,
measured across R24. The +12 V output [1] www.youtube.com/watch?v=6xe19XnX5L0
voltage inevitably contains high-frequency [2] www.youtube.com/watch?v=bMuCACqwI4s
ripple due to the repeated charging and [3] http://ctms.engin.umich.edu/CTMS/index.
discharging of C29. This ripple is however php?example=InvertedPendulum§ion=SystemModeling
dominated by the value of its equivalent
[4] Dan Simon:
series resistance (ESR) and the high-fre-
Optimal State Estimation: Kalman, H Infinity, and Nonlinear Approaches
quency currents flowing through it. This is