Professional Documents
Culture Documents
Điều Khiển Động Cơ Dc Servo
Điều Khiển Động Cơ Dc Servo
Gii thiu
iu khin ng c DC (DC Motor) l mt ng dng thuc dng c bn nht
ca iu khin t ng v DC Motor l c cu chp hnh (actuator) c dng
nhiu nht trong cc h thng t ng (v d robot). iu khin c DC Motor l
bn c th t xy dng c cho mnh rt nhiu h thng t ng. Khi nim
Servo m ti dng trong bi hc ny ch mt h thng hi tip. DC servo motor
l ng c DC c b iu khin hi tip.
Bi ny l mt bi tng hp nhiu vn ng dng AVR bao gm nhn d liu t
ngi dng, iu khin motor, c encoder, hin th LCD, c gii thut iu khin
PID v mch cng sut cho MotorDo , t nht bn phi nm c cc vn
c bn nh Timer-Counter, TexLCD, mch cu H. Phn cn li ti s gii thch
trong lc hc bi ny. C 2 phng php iu khin ng c DC l analog v
digital. Mc ch chnh ca chng ta l dng AVR iu khin ng c DC nn
phng php s m c th l phng php iu rng xung (PWM) s c gii
thiu. Ngoi ra, khi ni n iu khin ng c DC c 2 i lng iu khin chnh
l v tr (s vng quay) v vn tc. Trong phn gii thch v b iu khin PID ti
s iu khin v tr lm v d, tuy nhin trong phn v d lp trnh cho AVR chng
ta s thc hin iu khin vn tc cho DC Motor. Bng cch ny, bn c th t tin
m rng v d iu khin cho c 2 i lng. V l iu khin mt cch t
ng nn chng ta cn c v i lng iu khin (c th l v tr hoc vn tc
motor) v hi tip (feedback) v hiu chnh PWM cp cho ng c. Chng ta
s dng incremental optical encoder c s vng quay v hi tip v cho AVR.
B iu khin PID s c dng v vn hnh bi AVR. Tng qut, bi hc ny bao
gm:
- AVR pht PWM iu chnh vn tc ng c: phn ny bn xem li bi 4 v
Timer-Counter. iu c bn cn nm l bng cch thay i rng ca xung PWM
chng ta s thay i c vn tc Motor.
- Xung PWM khng trc tip lm quay ng c m thng qua mt mch cng sut
gi l dirver. Driver cho DC Motor chnh l mch cu H m chng ta tm hiu
trong bi Mch cu H. Trong bi hc ny, ti gii thiu mt chip c tch hp sn
mch cu H, chip L298D.
- vic iu khin chip driver L298D d dng, chng ta s to mt mch logic
dng cc cng NOT v AND.
- ng c DC m chng ta s dng c tch hp sn mt encoder 3 ng ra, chng ta
s dng AVR c s xung (hay s vng quay) v tnh ra vn tc ca Motor. Vic
c encoder s c thc hin bng ngt ngoi.
- Mt gii thut PID c xy dng trong AVR hiu chnh vn tc ng c.
- Ngi dng s nhp vn tc cn iu khin vo AVR thng qua cc switches. Vn
tc mong mun v vn tc thc ca ng c c hin th trn Text LCD.
Mch in v d c trnh by trong hnh 1.
F=Kp*e
(1)
Trong Kp l mt hng s dng no m chng ta gi l h s P (Propotional
gain), e l sai s cn iu khin tc khong cch t im O n v tr hin ti ca
xe. Mc tiu iu khin l a e tin v 0 cng nhanh cng tt. R rng nu Kp ln
th F cng s ln v xe rt nhanh chng tin v v tr O. Tuy nhin, lc F qu ln s
gia tc cho xe rt nhanh (nh lut II ca Newton: F=ma). Khi xe n v tr O
(tc e=0), th tuy lc F=0 (v F=Kp*e=F=Kp*0) nhng do qun tnh xe vn tip tc
tin v bn phi v lch im O v bn phi, sai s e li tr nn khc 0, gi tr sai s
lc ny c gi l overshot (vt qu). Lc ny, sai s e l s m, lc F li xut
hin nhng vi chiu ngc li ko xe v li im O. Nhng mt ln na, do Kp
ln nn gi tr lc F cng ln v c th ko xe lch v bn tri im O. Qu trnh c
tip din, xe c mi dao ng quanh im O. C trng hp xe dao ng cng
ngy xng xa im O. B iu khin lc ny c ni l khng n nh. Mt
xut nhm gim overshot ca xe l s dng mt thnh phn thng trong b iu
khin. S rt l tng nu khi xe ang xa im O, b iu khin sinh ra lc F ln
nhng khi xe tin gn n im O th thnh phn thng s gim tc xe li.
Chng ta u bit khi mt vt dao ng quanh 1 im th vt c vn tc cao nht
tm dao ng (im O). Ni mt cch khc, gn im O sai s e ca xe thay
i nhanh nht (cn phn bit: e thay i nhanh nht khng phi e ln nht). Mt
khc, tc thay i ca e c th tnh bng o hm ca bin ny theo thi gian.
Nh vy, khi xe t A tin v gn O, o hm ca sai s e tng gi tr nhng ngc
chiu ca lc F (v e ang gim nhanh dn). Nu s dng o hm lm thnh
phn thng th c th gim c overshot ca xe. Thnh phn thng ny chnh
l thnh phn D (Derivative) trong b iu khin PID m chng ta ang kho st.
Thm thnh phn D ny vo b iu khin P hin ti, chng ta thu c b iu
khin PD nhu sau:
F=Kp*e +
Kd*(de/dt) (2)
Trong (de/dt) l vn tc thay i ca sai s e v Kd l mt hng s khng m
gi l h s D (Derivative gain).
S hin din ca thnh phn D lm gim overshot ca xe, khi xe tin gn v O, lc
F gm 2 thnh phn Kp*e > =0 (P) v Kd*(de/dt) <=0 (D). Trong mt s trng
hp thnh phn D c gi tr ln hn thnh phn P v lc F i chiu, thng xe li,
vn tc ca xe v th gim mnh gn im O. Mt vn ny sinh l nu thnh
phn D qu ln so vi thnh phn P hoc bn thn thnh phn P nh th khi xe tin
gn im O (cha tht s n O), xe c th dng hn, thnh phn D bng 0 (v sai
s e khng thay i na), lc F = Kp*e. Trong khi Kp v e lc ny u nh nn lc
F cng nh v c th khng thng c lc ma st tnh. Bn hy tng tng tnh
hung bn dng sc ca mnh y mt xe ti nng vi chc tn, tuy lc y tn
ti nhng xe khng th di chuyn. Nh th, xe s ng yn mi d sai s e vn
cha bng 0. Sai s e trong tnh hung ny gi l steady state error (tm dch l sai
s trng thi tnh). trnh steady state error, ngi ta thm vo b iu khin mt
thnh phn c chc nng cng dn sai s. Khi steady state error xy ra, 2 thnh
phn P v D mt tc dng, thnh phn iu khin mi s cng dn sai s theo
thi gian v lm tng lc F theo thi gian. n mt lc no , lc F ln
thng ma st tnh v y xe tin tip v im O. Thnh phn cng dn ny chnh
l thnh phn I (Integral - tch phn) trong b iu khin PID. V chng ta iu bit,
tch phn mt i lng theo thi gian chnh l tng ca i lng theo thi
gian. B iu khin n thi im ny y l PID:
F=Kp*e + Kd*(de/dt)
+Ki*edt (3)
(ch : edt l tch phn ca bin e theo t)
Nh vy, chc nng ca tng thnh phn trong b iu khin PID gi r. Ty
vo mc ch v i tng iu khin m b iu khin PID c th c lt bt
tr thnh b iu khin P, PI hoc PD. Cng vic chnh ca ngi thit k b
iu khin PID l chn cc h s Kp, Kd v Ki sao cho b iu khin hot ng tt
v n nh (qu trnh ny gi l PID gain tuning). y khng phi l vic d dng
v n ph thuc vo nhiu yu t. Ti tm tt mt kinh nghim c bn khi chn cc
h s cho PID nh sau:
- Chn Kp trc: th b iu khin P vi i tng tht (hoc m phng), iu
chnh Kp sao cho thi gian p ng nhanh, chp nhn overshot nh.
- Thm thnh phn D loi overshot, tng Kd t t, th nghim v chn gi tr
thch hp. Steady state error c th s xut hin.
- Thm thnh phn I gim steady state error. Nn tng Ki t b n ln gim
steady state error ng thi khng cho overshot xut hin tr li.
C mt phng php rt ph bin dng chn cc h s cho b iu khin PID
gi l ZieglerNichols, bn quan tm c th t tm hiu thm.
iu khin PID s
Cng thc ca b iu khin PID trnh by trong (3) l dng hm lin tc ca bin
e, trong c c thnh phn tuyn tnh, o hm v tch phn. Tuy nhin, h thng
my tnh v vi iu khin li l h thng s. Mun xy dng b iu khin PID trn
my tnh hay trn vi iu khin chng ta phi bit cch xp x phng trnh lin tc
thnh dng ri rc. thc hin s ha b iu khin PID trc ht ti ni s
qua th no l h thng s (digital) so vi h thng lin tc hay h thng tng t
(analog). Hy quan st h thng iu chnh nhit n gin nh trong hnh 7.
(4)
Tng hp cc xp x, cng thc ca b iu khin PID s c trnh by trong
(5)
(5)
Trong u l i lng output t b iu khin. n gin ha vic tnh thnh
phn tch phn, chng ta nn dng phng php cng dn (hay quy):
(6)
Vi I(k) l thnh phn tch phn hin ti v I(k-1) l thnh phn tch phn trc
.
Cc cng thc (5) v (6) rt d dng thc hin bng AVR. Do , n lc ny
chng ta sn sng a tng vo lp trnh cho chip.
VI. iu khin DC Motor bng AVR
Phn ny chng ta s vn dng tt c phn l thuyt gii thiu trn vit
chng trnh cho AVR. Mc ch l iu khin vn tc ca DC Motor bng gii
thut PID. Mch in m phng c trnh by trong hnh 1. M hnh Motor dng
trong v d l loi 12V c vn tc khng ti ti a l 720rpm (revolute per minute)
tc 20 vng/s. Encoder dng cho motor c chn c phn gii 112 pulse/vng.
Knh A ca encoder c ni vi ngt ngoi INT2 m xung, knh B ni vi
chn PB0 (chn 1) ca chip Atmega32 xt hng quay. Bn switches c ni
vi 4 bit cao ca PORTB ci t vn tc mong mun cn iu khin. Mt Text
LCD dng hin th vn tc thc ca motor c t Encoder (Actual speed) v vn
tc ci t (Desired speed). Do Text LCD c ni vi PORTC nn nu bn mun
dng chng trnh ny cho ng dng tht th phi np li fuses v hiu ha
JTAG. Gii thut PID s c vn hnh bi AVR trong thi gian ly mu l 25ms.
Timer 2 c dng to khong thi gian 25ms. Timer 1 (16 bit) l b to PWM
iu khin vn tc ng c. Ton b ni dung chng trnh c trnh by trong
list 1.
List 1. iu khin vn tc ng c DC
Cc dng t 14 n 17 chng ta nh ngha cc chn iu khin DC Motor, chn
DIR iu khin hng v EN kch hot hoc dng Motor (thc ra l dng L298D).
Do mc ch ca chng ta l iu khin vn tc ng c, 2 chn ny ch c
kch mt ln duy nht trong chng trnh chnh (khng cn i hng quay ca
Motor). Dng 18 nh ngha thi gian ly mu, Sampling_time l 25 ms (.025s).
Bin inv_Sampling_time dng 19 l nghch o ca Sampling_time, 1/0.025 =
40, v y cng l hng s, chng ta nh ngha trc sau ny khng cn thc
hin php nghch o trong chng trnh chnh (tit kim thi gian thc thi). PWM
dng iu khin ng c c chn c tn s 1KHz nn chu k la 1ms. Do chng
ta dng ngun xung gi nhp 8MHz, to thi gian 1ms cn 8000 xung, gi tr
ny c nh ngha trong dng 20 v s c gn cho thanh ghi ICR1 (TOP ca
PWM, xem li bi Timer-Counter, Timer1, Fast PWM) trong chng trnh chnh
(dng 81). Cc dng code t 22 n 27 khai bo mt s bin ton cc dng trong
chng trnh chnh. Do cc bin ny s c dng c trong trnh phc v ngt v
chng trnh chnh nn cn khai bo c tnh volatile, kiu bin l long int tc s
nguyn 32 bit ( trnh b trn khi tnh ton sau ny). Bin Pulse v pre_Pulse l s
xung hin ti v ln ly mu trc c t encoder. Cc bin trong dng 23 v 24
dng cho b iu khin PID, bin Ctrl_Speed l vn tc mong mun (set point)
ton cc v bin Output cha gi tr tnh c t b iu khin PID.
Trc khi i tm hiu chng trnh con cha gii thut PID, chng ta s kho st
ni dung chng trnh main v cc trnh phc v ngt trc hiu tng quan cch
thc thc hin. Chng trnh chnh bt u t dng 45 v kt thc dng 103.
Phn u ca chng trnh chnh (ngoi vng lp while) khai bo v khi to cc
module c s dng. 2 dng 49 v 50 ci t hng cho PORTB, do PORT ny
dng c encoder v cc switches chng ta cn set n l input v c in tr ko
ln. Hai dng 52 v 53 set hng cho ng c v s gi hng ny khng i trong
sut qu trnh iu khin sau ny. Hai dng 55 v 56 khai bo ngt ngoi INT2
dng m xung knh A ca encoder. Ch l INT2 ch c 2 mode l cnh xung v
cnh ln nn ch c 1 bit sense ISC2 chn mode. Bit ISC2 khng nm trong
thanh ghi iu khin MCUCR nh cc ngt khc m nm trong thanh ghi iu
khin-trng thi MCUCSR. Khi ISC2=0 th ch ngt cnh xung ca INT2 c
chn (xem dng 55). Sau INT2 c cho php hot ng dng 56. Hy tm
thi di chuyn n dng 109 xem trnh phc v ngt INT2. Chc nng ca INT2
trong bi ny l m xung encoder v th trnh phc v s lm vic ny. Khi c
mt ngt INT2 xy ra tc c 1 xung t encoder vo th trnh phc v ngt
ISR(INT2 vect) t ng c gi ra, dng 110 trong trnh phc v ngt kim tra
trng thi chn PB0, tc kenh B ca encoder. Nu PB0=1 th tng bin xung m
c Pulse ln 1, ngc li nu PB0=0 th gim Pulse i 1 trong dng 111. Quay v
gii thch chng trnh chnh dng 59, y l cc khai bo cho timer 2. Chng ta
s dng timer 2 to ra mt khong thi gian ly mu 25 ms, c sau 25 ms th s c
ngt trn timer2 mt ln v trong trnh phc v ngt trn ca timer2 chng ta thc
hin tnh ton PID. Dng 59 chng ta set cc bit CS chn b chia tn s, b chia
Prescaler=1024 c chn v 25 ms kh ln so vi thi gian 1 chu k xung gi
nhp (1/8 micro giy). Prescaler = 1024 ngha l sau 1024 nhp ca xung gi nhp,
tc sau 128 micro giy (1024 *1/8=128 us) th thanh ghi gi tr TCNT2 mi tng 1
n v. Do chng ta mun to khong thi gian 25 ms tng ng
25000/128=195 n v m ca thanh ghi TCNT2, chng ta s gn gi tr khi to
cho TCNT2 l 255-195=60 (timer 2 s trn mt ln khi TCNT2 m n 255, xem
li bi Timer-Counter). iu ny thc hin dng 60 TCNT2=60. Dng 61 cho
php ngt trn timer2. Hai dng 64 v 65 khi ng Timer 1 dng nh mt b to
xung Fast PWM, mode 14, trong thanh ghi ICR1 cha chu k PWM v 2 thanh
ghi OCR1A, OCR1B cha duty cycle (khong ON) ca PWM. Cc dng t 68 n
70 ghi texts ln LCD. Cc dng t 80 n 83 khi ng PWM cho DC Motor v
cho php ngt ton cc sei();. Trong vng lp while ch yu l cng vic kim tra
v hin th, bin sample_count m s ln ngt trn timer2 xy ra, n c tng 1
n v khi c mt ngt trn (xem dng 106) tc sau 25ms. Dng 86, chng ta kim
tra bin sample_count, vic hin th ch cthc hin mi 250 ms mt ln
(sample_count=10) v vic ny tn kh nhiu thi gian. Trong dng 87 chng ta
kim tra cc swiches xem ngi dng cho mun thay i vn tc tham chiu cho
iu khin. Cc dng tip theo in bin rSpeed l s lng xung m c t
encoder trong vng 25 ms (cho ti hin ti) dong 1 ca LCD v in bin
Ctrl_Speed l s xung/25ms m ngi dng mong mun motor t c. Ni dung
quan trng nht ca list 1, tuy nhin, khng nm trong chng trnh chnh m nm
cc trnh phc v ngt v chng trnh con Motor_Speed_PID(long int
des_Speed).
Trc ht, trnh phc v ngt ISR(TIMER2_OVF_vect) c t ng gi sau mi
25ms, trong trnh ny chng ta cn set li gi tr khi ng cho thanh ghi gi tr
TCNT2 (xem li bi Timer-counter) dng 105. Sau tng bin m
sample_count ln 1 (cng cho vic m thi gian hin th, ni trn). Cui
cng l gi chng trnh con tnh ton gii thut PID Motor_Speed_PID(long int
des_Speed). y l on chng trnh tnh ton gii thut PID v xut gi tr iu
khin Motor. Hy quay li dng 30 tm hiu chng trnh con ny. Do bin
Pulse cha tng s xung c t encode (trong ISR(INT2_vect) ), chng ta ly gi
tr ny tr i gi tr pre_Pulse, tc s lng xung thi im 25 ms trc ,
thu c tng s xung thu c trong 25 ms qua. y chnh l vn tc motor tnh
trn 25 ms: rSpeed=Pulse-pre_Pulse. Sau khi tnh c vn tc rSpeed chng ta
gn li gi tr Pulse cho pre_Pulse ln ly mu sau dng n (dng 32). Sai s
vn tc c t tn l Err, bin ny c tnh bng bng cch ly vn tc mong
mun tr vn tc hin ti: Err=des_Speed-abs(rSpeed) dng 33. Dng 34 tnh
thnh phn P ca b iu khin pPart=Kp*Err. Dng 35 tnh thnh phn D ca b
iu khin, nh chng ta tho lun trong cng thc (2) th thnh phn D c
tnh l: dPart=Kd*(Err-pre_Err)/Sampling_time, trong pre_Err l gi tr sai s
ln ly mu trc c lu li. Do 1/Sampling_time = inv_Sampling_time nn
chng ta c th thay dng tnh dPart bng cng thc trong dng
35: dPart=Kd*(Err-pre_Err)*inv_Sampling_time. Dng 36 tnh thnh phn I
(iPart), s dng phng php cng dn ( quy) chng ta thu c iPart bng
iPart trc cng vi din tch hnh ch nht sai s hin
ti: iPart+=Ki*Sampling_time*Err/1000. Chng ta phi chia iPart cho 1000 v
Sampling_time c tnh theo ms trong khi n v tnh ton chun trong l s. Cng
cc thnh phn ny li chng ta c gi tr Output tng hp trong dng 37. Tuy
nhin, theo l thng th cng thc dng 37 phi l Output=pPart+dPart+iPart
nhng y li l : Output+=pPart+dPart+iPart ( du + trc du =), ngha
l Output c cng dn thay v l tng tc thi nh chng ta tho lun trong
phn gii thut PID. Tht ra vic ny cng d hiu. Trong bi ton iu khin v tr,
khi sai s bng 0 chng ta c th dng b iu khin (u=0) nhng trong bi ton
iu khin vn tc, khi sai s bng 0 th gi tr u vn phi c gi l gi tr trc
.V vy, trong bi ton iu khin vn tc gi tr Output c cng dn thay v
gn trc tip, bn phi ghi nh iu ny trong cc ng dng iu khin ca mnh.
Hai dng 40 v 41 xt trng hp bo ha (saturation) khi Output vt qu gii
hn cho php ca PWM (xn 2 u). Cui cng l gn gi tr tnh ton c t PID
cho thanh ghi OCR1A tng hoc gim duty cycle ca PWM trn chn OC1A
(ni vi PWM ca Motor) v gn gi tr sai s Err cho bin pre_Err cho ln ly
mu sau dng n.
Chy m phng: ton b chng trnh v c mch in m phng c ti to
sn. Ngi c ch cn c hiu v chy m phng mch in. Ch khi chy m
phng hy thay i cc switches thay i vn tc cn iu khin. Gi tr vn tc
thc cht l s xung encoder trong 25 ms, ngi c hy t tnh ra s vng /s. Do
m hnh motor trong phn mm m phng khng hon ho lm nn p ng b
iu khin hi chm, bn c th phi ch mt khong thi gian thy vn tc
Motor t n vn tc yu cu. Hay thay gi tr Kd trong dng 23 thnh 1 hoc 0,
bin dch li chng trnh v m phng quan st v so snh ovetshot (s vt
qu) ca h thng.