Upload
em330
View
234
Download
0
Embed Size (px)
Citation preview
8/9/2019 2010_Inverted Pendulum_
1/38
Software PID Control of an InvertedSoftware PID Control of an Inverted
Pendulum Using the PIC16F684Pendulum Using the PIC16F684
2010/7/28 1
8/9/2019 2010_Inverted Pendulum_
2/38
INTRODUCTIONINTRODUCTION
y The purpose of this application note is to describe how a
PIC16F684 can be used to implement a positional
Proportional-Integral-Derivative (PID) feedback control in an
inherently unstable system.
y
The inverted pendulum consists of three main parts: the base
platform, the pendulum and the controller board
2010/7/28 2
8/9/2019 2010_Inverted Pendulum_
3/38
BASE PLATFORMBASE PLATFORM(())
y The base platform is a 3-point platform, 2 wheels (one of
which is geared and attached to a DC motor) and an audio
jack. When the DC motor is turned on, the base platform will
rotate around in a circle with the center of the axis of rotation
being the audio jack.
2010/7/28 3
8/9/2019 2010_Inverted Pendulum_
4/38
BASE PLATFORM(BASE PLATFORM())
y The audio jack serves 2 purposes
1.It is used as the axis of rotation for the base platform
2.It is used to bring commutated power to the controller board.
2010/7/28 4
8/9/2019 2010_Inverted Pendulum_
5/38
PENDULUM(PENDULUM())
y The pendulum is attached to the base platform by a 360 free
rotating potentiometer. The pendulums base is attached to the
potentiometer in such a fashion that when the pendulum is
balanced (completely vertical), the potentiometer center tap is
biased to VREF/2.
2010/7/28 5
8/9/2019 2010_Inverted Pendulum_
6/38
CONTROLLER BOARD(CONTROLLER BOARD())
y The controller board has 2 main functions, to measure and
to drive the DC motor. The power supply needed to run the
system is dictated by the selection of the motor. The motor is
controlled by an H-bridge which is driven by the PIC16F684
Enhanced Capture/Compare/PWM Module (ECCP). The
outputs of the ECCP are connected to FET drivers that
produce the proper drive voltages and reduce the transition
times for the FETs in the H-bridge.
2010/7/28 6
8/9/2019 2010_Inverted Pendulum_
7/38
Potentiometers(Potentiometers())
y There are 5 potentiometers located on the controller board, 3
of which are used for adjusting the PID constants (Kp, Ki and
Kd) and one to measure .
y The fifth potentiometer is used in conjunction with the input
filters reference. The input filter is a low-pass Bessel filter
with a cut-off frequency of 60 Hz and has a voltage gain of 6.
2010/7/28 7
8/9/2019 2010_Inverted Pendulum_
8/38
Potentiometers(Potentiometers())
y A low-pass filter is needed to eliminate any high frequency
noise on the angle measurement which the derivative term of
the PID controller is extremely sensitive to. The Bessel filter
is used because it has the best response to a step function.
2010/7/28 8
8/9/2019 2010_Inverted Pendulum_
9/38
Potentiometers(Potentiometers())
y The cut-off frequency was chosen to be at least twice the
expected frequency of the pendulum. The gain of the filter
was chosen to increase the resolution of the Analog- to-
Digital (A/D) converter. With the 360potentiometer and a
10-bit A/D converter, with no gain, one LSb equals 0.35.
With the gain set to 6, the displacement angle is limited to
30 which gives a resolution of 0.059per LSb.
2010/7/28 9
8/9/2019 2010_Inverted Pendulum_
10/38
Potentiometers(Potentiometers())
y The fifth potentiometer controls the input filters reference to
produce a true 0 displacement angle when the pendulum is
vertical. Without this potentiometer, any slight offset angle
will cause the base to slowly increase its speed and eventually
take the system into an unstable state.
2010/7/28 10
8/9/2019 2010_Inverted Pendulum_
11/38
2010/7/28 11
8/9/2019 2010_Inverted Pendulum_
12/38
PIDPID
The desired set point R(t) of this system occurs when = 0. In this
state, the pendulum is balanced. Since the desired response of the system
is 0, any angle measured other than 0 is the error or Y(t) = E(t).
2010/7/28 12
8/9/2019 2010_Inverted Pendulum_
13/38
y In implementing the PID controller, there are 3 terms which
are based off the error measurement:
y Proportional Term:
y Integral Term:
y Derivative Term:
t
0I dttEK
tEK
td/tdEK
2010/7/28 13
8/9/2019 2010_Inverted Pendulum_
14/38
y In this system, the sign of the controllers output, C(t),will
determine the direction in which the motor will turn.The
magnitude of C(t) directly corresponds to the duty cycle of
the PWM in the ECCP module, determining the speed at
which the motor will turn.
1 dt/tdEKdttEKtEKtt
0
I !
2010/7/28 14
8/9/2019 2010_Inverted Pendulum_
15/38
PID INADIGITAL SYSTEMPID INADIGITAL SYSTEM
y Converting over to a digital system, Y(t) is measured by an
A/D converter. In order to implement the PID controller,
the PICmicro microcontroller will have to do some
approximations of integral and derivative terms. Starting
with the derivative term, we can use the following
difference equations for our approximation.
2010/7/28 15
8/9/2019 2010_Inverted Pendulum_
16/38
y Where E(n) is the current error, E(n-1) is the previous error
and Ts is our sampling period. Equation 2 is the approximate
slope of the tangent line at E(t) (rise/run). For the integralterm use the approximation in Equation 3.
2
(3)
2010/7/28 16
8/9/2019 2010_Inverted Pendulum_
17/38
y With these approximations we can rewrite C(t) as shown in
Equation 4.
4
2010/7/28 17
8/9/2019 2010_Inverted Pendulum_
18/38
MODELING THE INVERTEDMODELING THE INVERTED PENDULUMPENDULUM
y In order to properly implement the control algorithmHere
are some of the variables which need to be looked at in order
to model the system:
y Bases: positionvelocity accelerationmoment of
inertiacoefficient of frictionmasslength
y Pendulum: position velocitymoment of inertiacoefficient of friction mass length
y Earths gravitational constant
2010/7/28 18
8/9/2019 2010_Inverted Pendulum_
19/38
y In order to simplify all this, use one simple rule of thumb.
Select a motor (with proper torque, rpms and gear ratio to
the drive wheel) that can accelerate the base platform as
fast as the pendulum can fall. The angular acceleration of
the pendulum with respect to the displacement angle is:
(5)UU
!dd
R
g
2010/7/28 19
8/9/2019 2010_Inverted Pendulum_
20/38
DERIVATION OF EQUATIONDERIVATION OF EQUATION
Using 2 unit vectors, i and j to
represent the horizontaland vertical vectors
respectively, yields the following equations for the
pendulum.
2010/7/28 20
8/9/2019 2010_Inverted Pendulum_
21/38
F = ma
6
7
8
9
2010/7/28 21
8/9/2019 2010_Inverted Pendulum_
22/38
y With a free body diagram we can see that there are 2 forces
acting on the pendulum, the tension from the rod and the
force of gravity. This free body diagram yields the following
equation
10
Equate Equation 9 and Equation 10:
11
2010/7/28 22
8/9/2019 2010_Inverted Pendulum_
23/38
y Now separate into 2 different vectors equations and eliminate
the vector notation.
(i Vector)
(j Vector)
Use these two simultaneous equations to eliminate the unknown T.
12
13
2010/7/28 23
8/9/2019 2010_Inverted Pendulum_
24/38
1. Multiply Equation 12 by cos.
14
2. Multiply Equation 13 by sin, moving -mgsin to the other side.
15
3. Equate Equation 14 and Equation 15
16
2010/7/28 24
8/9/2019 2010_Inverted Pendulum_
25/38
4.Divide by each side by mR.
which yields Equation 18.
17
19
18
20
Using the small angle approximation where sin = yields.
1sincos 22 ! UU
2010/7/28 25
8/9/2019 2010_Inverted Pendulum_
26/38
y Since the acceleration of the pendulum is not constant, use
the maximum acceleration of the pendulum when using this
rule of thumb. The maximum acceleration of the pendulum
will occur when is at the largest angle . MAX is controlled
by both hardware and software.
y The hardware boundary for MAX is set by the gain of the
Bessel Filter; with a gain of 6, the limit is 30 or0.523
radians. This can be further limited in the software. The
software, accompanying this application note, further limits
MAX to 20 or0.349 radians. This is done to eliminate
the possibility of hitting the hardware boundary.
2010/7/28 26
8/9/2019 2010_Inverted Pendulum_
27/38
With a pendulum length of 0.5 meters and MAX set to 20With a pendulum length of 0.5 meters and MAX set to 20 oror
0.349 radians.0.349 radians.
y From Equation 5:
UU
!dd
R
g
349.05.0
81.9
!ddU
c2radians/se6.845!ddU
2meters/sec3.425!ddU
2010/7/28 27
8/9/2019 2010_Inverted Pendulum_
28/38
y If finding a motor that meets this criterion is difficult, there
are a few solutions:
1. One solution is to decrease MAX in the software or increase
the length of the pendulum; this will reduce the maximum
acceleration of the pendulum.
2. Another possible solution is to increase the coefficient of
friction between the drive wheel and the base.
3.Change the coefficient of friction by changing the drive wheel
to a different material or add an abrasive surface to the
platform.
2010/7/28 28
8/9/2019 2010_Inverted Pendulum_
29/38
C CODE FLOW CHARTC CODE FLOW CHART
y The following flow charts show a simplified version ofthe C
code for the inverted pendulum.
2010/7/28 29
8/9/2019 2010_Inverted Pendulum_
30/38
2010/7/28 30
8/9/2019 2010_Inverted Pendulum_
31/38
SUBTLETIES IN THE SOFTWARESUBTLETIES IN THE SOFTWARE
y The interrupt service routine is used to control the speed of
the PID loop. The interrupt service routine is set to run off the
Timer0 Interrupt.
y Timer0 is an 8-bit timer .When the TMR0 register overflows,
the Timer0 Interrupt Flag is set. The speed at which the
interrupt should occur is every 3.9 milliseconds (1/256 Hz).
2010/7/28 31
8/9/2019 2010_Inverted Pendulum_
32/38
y The derivative term is crucial in order to bring the inherently
unstable system into stability.
y In any PID control the derivative terms acts as an anticipator.
By checking the current error against the previous error, the
controller can tell if the error term is getting bigger or
smaller.
y If the error term is getting larger, the derivative term adds to
the output of the controller.
y If the error term is getting smaller, this term will subtract
from the output of the control in anticipation of an overshoot
condition.
2010/7/28 32
8/9/2019 2010_Inverted Pendulum_
33/38
y Without the derivative term the system will always be
unstable because there is no way to compensate for the
overshoot condition.
2010/7/28 33
8/9/2019 2010_Inverted Pendulum_
34/38
TUNING THE PID CONTROLLERTUNING THE PID CONTROLLER
1.Turn the Kp, Ki and Kdpotentiometers counter clockwise as
far as they will turn. This sets all the constants to zero.
2. Power the device using a 12V (minimum of a 3 amp) power
supply.
3. Holding the Reset button down, lift the pendulum to the
vertical position and release the Reset button and pendulum
at the same time.
4. The pendulum should free fall and the base will not move.
This verifies that all constants are properly read as zeros.
2010/7/28 34
8/9/2019 2010_Inverted Pendulum_
35/38
5. Increase the Kp constant by turning the potentiometer counter
clockwise and repeat step 3.
6. Keep repeating steps 3-5 until there is a little oscillation in the
base. If the Kp term is too small, the base platform will chase
the top of the pendulum while continues to increase. KP
will be too large if the drive wheel breaks free or the base
oscillates at a high rate of speed.
2010/7/28 35
8/9/2019 2010_Inverted Pendulum_
36/38
7.Start increasing the Ki the same way as Kp until the pendulum can be
balanced for several seconds under a constant oscillating condition. When
the Ki is added, the base will now accelerate faster than the pendulum
causing to change from a positive angle to a negative angle (or vice
versa). The pendulum will begin to fall backwards. The base should change
directions and, again, accelerate faster than the pendulum until changes
signs and the whole cycle repeats. This is known as the Overshoot
condition.
8. Increase Kd in the same manner as Kp and Ki until the
Overshoot condition is gone and the pendulum remainsbalanced.
9. Once all overshoot is gone, the PID controller is tuned.
2010/7/28 36
8/9/2019 2010_Inverted Pendulum_
37/38
CONCLUSIONCONCLUSION
1.By using the PIC16F684 devices ECCP and A/D modules we
are able to demonstrate how to implement a positional PID
controller to bring an inherently unstable system into stability.
2. The keys to implementing this control is to have a basic
understanding of the mechanical system, and identifying
the derivative term would be a critical factor in the overall
stability of the system.
2010/7/28 37
8/9/2019 2010_Inverted Pendulum_
38/38
3.The other keys, with respect to the software, were making sure
our registers never overflow, and picking the frequency of the
PID loop that is a power of 2 so we could have a fast multiply
and divide routine using the left and right shift.
2010/7/28 38