Laboratorium Sterowania Robotów - Katedra Sterowania i Inżynierii

Laboratorium Sterowania Robotów
Katedra Sterowania i Inżynierii Systemów
Politechnika Poznańska
KSIS-PP
oprac. M. Michałek
ĆWICZENIE B
Sterowanie pozycyjne ze sprzężeniem od stanu
Celem ćwiczenia jest implementacja na platformie z procesorem DSP
układu sterowania ze sprzężeniem od stanu dla modelu pozycyjnego
manipulatora o jednym stopniu swobody (PM1R). Przeprowadzona
implementacja ma stanowić eksperymentalną weryfikację jakości działnia wybranych technik sterowania dla zadań śledzenia i stabilizacji
dedykowanych dla pojedynczego serwonapędu.
1
Model pozycyjny systemu PM1R
Znajomość i wykorzystanie dobrego modelu sterowanego systemu pozwala na uzyskanie wysokiej
jakości sterowania. Poniżej przedstawiono procedurę wyprowadzenia i krótkiej analizy podstawowych bloków dynamicznych systemu PM1R przy założeniu, że interesującym nas sygnałem
wyjściowym jest pozycja kątowa ogniwa (napędu), a rozkład masy obciążenia (ogniwa) względem
osi obrotu jest równomierny. Poglądowy schemat systemu PM1R przedstawia rys. 1.
Rys. 1: Schemat poglądowy systemu PM1R na stanowisku laboratoryjnym wraz z definicją sygnałów wejściowego u i wyjściowego θ.
2
KSIS-PP: Laboratorium Sterowania Robotów – B
1.1
Model części mechanicznej
Przedmiotem rozważań będzie manipulator PM1R (rys. 1) złożony z silnika prądu stałego połączonego za pośrednictwem przekładni redukcyjnej z ogniwem o masie równomiernie rozłożonej
względem osi obrotu. Równomierne rozłożenie masy ogniwa usuwa problem oddziaływania nieliniowego składnika grawitacyjnego τ g = mgL cos θl . Zatem równanie równowagi momentów sił po
stronie ogniwa przyjmuje postać:
Jl θ̈l + fl (θ̇l ) = τl − τz ,
(1)
gdzie θl jest pozycją kątową ogniwa, Jl jest momentem bezwładności ogniwa względem osi obrotu,
fl (θ̇l ) stanowi moment siły tarcia w przegubie (złączu) wynikający z przyjętego modelu tarcia, a
τl oraz τz stanowią odpowiednio: moment siły napędzający ogniwo oraz zewnętrzny moment siły
zakłócenia (w idealnym przypadku τ z = 0). Równanie równowagi momentów po stronie silnika
można zapisać w następującej postaci:
Jm θ̈m + fm (θ̇m ) = τm − τl∗ ,
(2)
gdzie θm jest pozycją kątową silnika, Jm jest momentem bezwładności napędu względem osi
obrotu, fm (θ̇m ) stanowi model momentu siły tarcia napędu, τ m jest momentem napędowym
silnika, a τl∗ jest całkowitym momentem siły obciążenia oddziaływującym bezpośrednio na wał
silnika.
Równania (1) oraz (2) łączy zależność wynikająca z zastosowania przekładni redukcyjnej
między silnikiem a ogniwem (zakładamy brak luzów w połączeniu mechanicznym):
τl∗ = ητl ,
(3)
gdzie η > 0 jest współczynnikiem przełożenia przekładni zdefiniowanym jako:
∆
η=
θl
<1
θm
⇒
η=
θ̇l
,
θ̇m
η=
θ̈l
.
θ̈m
(4)
Podstawiając (3) do (2) i korzystając z równania (1) oraz zależności (4) otrzymujemy wypadkowe
równanie równowagi momentów sił po stronie silnika:
(Jm + η 2 Jl )θ̈m + fm (θ̇m ) + ηfl (η θ̇m ) = τm − τz∗ ,
τz∗ = η τz ,
(5)
gdzie τz∗ jest wypadkowym momentem zakłócającym widzianym po stronie silnika.
1.1.1
Modele tarcia
W tym momencie musimy zdefiniować model tarcia, który będzie wykorzystywany w dalszej
części ćwiczenia. Rozważymy trzy modele zależne od prędkości napędu, przy czym dla prostoty
zastosowane zostanie oznaczenie ω ≡ θ̇. Pierwszy model ma strukturę liniową i jest tzw. modelem
tarcia wiskotycznego (związanego z ruchem):
∆
f (ω) = b ω,
(6)
gdzie b > 0 jest współczynnikiem tarcia wiskotycznego. Pozostałe modele uwzględniają dodatkowo
składnik tarcia statycznego fC (tzw. tarcie suche lub tarcie Coulomb’a) i mogą mieć dwie postaci:
symetryczną


 fC + b ω dla ω > 0
∆
f (ω) = fC sign(ω) + b ω =
0
dla ω = 0
(7)

 −f + b ω dla ω < 0
C
3
KSIS-PP: Laboratorium Sterowania Robotów – B
i niesymetryczną względem początku układu charakterystyki tarcia:
∆
f (ω) =


 fC1 + b1 ω
dla ω > 0
0
dla ω = 0 ,


fC2 + b2 ω dla ω < 0
(8)
gdzie fC1 , b1 , b2 > 0 oraz fC2 < 0 są elementami odpowiednich składników tarcia. Charakterystyki
wszystkich modeli tarcia1 przedstawiono na rys. 2.
Rys. 2: Charakterystyki trzech modeli tarcia zdefiniowanych odpowiednio w (6), (7) oraz (8).
Podstawiając powyższe definicje modeli tarcia do równania (5) otrzymujemy ostateczną postać
równania modelującego część mechaniczną rozważanego manipulatora:
J θ̈m + f (θ̇m ) = τm − η τz ,
(9)
przy czym J = (Jm + η 2 Jl ) oraz
• dla modelu (6):
f (θ̇m ) ≡ f (ωm ) = b ωm ,
(10)
f (θ̇m ) ≡ f (ωm ) = b ωm + fC sign(ωm ),
(11)
• dla modelu (7):
• dla modelu (8):
f (θ̇m ) ≡ f (ωm ) =
gdzie:


 fC1 + b1 ωm
dla ωm > 0
dla ωm = 0 ,
dla ωm < 0
(12)
fC = (fCm + ηfCl ),
fC1 = (fC1m + ηfC1l ),
fC2 = (fC2m + ηfC2l ).
(13)
0

 f +b ω
C2
2 m
b = (bm + η 2 bl ),
b1 = (b1m + η 2 b1l ),
b2 = (b2m + η 2 b2l ),
Należy zaznaczyć, iż w rozważanym modelu pozycyjnym po stronie silnika, wyjściowym sygnałem dla części mechanicznej jest pozycja kątowa napędu θ m , a sygnałem wejściowym jest
moment napędowy τm .
1.2
Model części elektromagnetycznej
Równanie bilansu napięć obwodu twornika silnika prądu stałego ze wzbudzeniem od magnesów
trwałych ma następującą postać:
di
L + Ri = ua − E,
(14)
dt
1
Są to charakterystyki statyczne.
4
KSIS-PP: Laboratorium Sterowania Robotów – B
gdzie R jest rezystancją uzwojenia wirnika, L indukcyjnością uzwojenia wirnika, i prądem płynącym w uzwojeniu, E napięciem indukowanym w uzwojeniu twornika, a u a stanowi napięcie podawane na zaciski uzwojenia wirnika (sygnał wejściowy).
W rozważanym modelu po stronie silnika, sygnałem wyjściowym dla części elektromagnetycznej jest prąd w uzwojeniu wirnika silnika i, a sygnałem wejściowym jest napięcie na zaciskach
twornika ua .
1.3
Pełny model manipulatora
Oba podsystemy – mechaniczny i elektromagnetyczny – są ze sobą ściśle związane strukturą i
zasadą działania napędu prądu stałego. Zatem pełny opis zjawisk zarówno o charakterze mechanicznym jak i elektromagnetycznym charakteryzujący działanie manipulatora PM1R wymaga
matematycznego powiazania równań (9) oraz (14). Zakładając stałość strumienia magnetycznego
pochodzącego od magnesów trwałych umieszczonych w stojanie silnika możemy zapisać następujące słuszne zależności:
E = E(ωm ) = kE ωm ,
τm = τm (i) = kI i,
(15)
gdzie kE , kI > 0 są stałymi parametrami silnika (często zawartymi w danych katalogowych).
Powyższe równania pozwalają połączyć (9) z (14). Rozważymy dwa przypadki takiego połączenia
związane z wartością indukcyjności L.
1.3.1
Wartość indukcyjności L istotna.
Równanie (14) możemy przepisać na podstawie (15) jako:
L
R
τ̇m + τm = ua − kE ωm
kI
kI
(16)
co w połączeniu z równaniem (9) oraz zależnością θ̇m = ωm daje układ sprzężonych równań
różniczkowych opisujących pełny model manipulatora:
θ̇m = ωm
(17)
J θ̈m + f (θ̇m ) = τm − η τz ,
L
R
τ̇m + τm = −kE ωm + ua .
kI
kI
(18)
(19)
Powyższy układ równań można zapisać w postaci równania stanu w trójwymiarowej przestrzeni
stanu:





ωm
0
θ̇m
   1
  η
1
ω̇m  = − J f (ωm ) + J τm  + − J
τ̇m
− kELkI ωm − R
0
L τm

0 " #
τz
0

ua
kI
⇒
ẋ = f (x) + Bu
(20)
L
gdzie x = [θm ωm τm ]T jest stanem systemu, a u = [τz ua ]T jest wektorem sygnałów wejściowych
(sygnały wejściowe to odpowiednio zakłócenie τ z i sygnał sterujący ua ). Równanie wyjścia w
modelu pozycyjnym przymuje postać:
∆
y = θm
⇒
h
y = Cx,
i
C= 1 0 0 .
(21)
Zakładając liniowy model tarcia (6) składnik f (ω m ) = b ωm i układ równań (20) jest układem
równań liniowych postaci:



0
1
θ̇m
  
− Jb
ω̇m  = 0
τ̇m
0 − kELkI
0



0
θm
  η
1 
J  ω m  + − J
τm
0
−R
L

0 " #
τz
0

u
a
kI
L
⇒
ẋ = Ax + Bu.
(22)
5
KSIS-PP: Laboratorium Sterowania Robotów – B
Korzystając ze znanej zależności G(s) = C(sI − A) −1 B można wyprowadzić model obu torów
dynamiki w postaci następujących transmitancji operatorowych:
Θm (s)
Ua (s)
∆ Θm (s)
G2 (s) =
Tz (s)
∆
G1 (s) =
=
=
kI
,
s[JLs2 + (JR + Lb)s + (Rb + kI kE )]
−η(Ls + R)
,
2
s[JLs + (JR + Lb)s + (Rb + kI kE )]
(23)
(24)
gdzie J oraz b zdefiniowano w (13).
1.3.2
Wartość indukcyjności L pomijalnie mała.
W przypadku, gdy indukcyjność uzwojeń silnika jest pomijalnie mała 2 , czyli dla L ≈ 0, równanie
różniczkowe (14) przechodzi w równanie algebraiczne Ri ≈ u a − E(ωm ) i pozwala (na podstawie
(15)) zapisać algebraiczną zależność wiążącą moment napędowy z napięciem twornika:
τm ≈
kI
[ua − kE ωm ] .
R
(25)
Podstawiając powyższe równanie do modelu części mechanicznej (9) uzyskujemy uproszczone
(przybliżone) równanie dynamiki manipulatora PM1R w dwuwymiarowej przestrzeni stanu:
"
#
"
ωm
θ̇m
=
ω̇m
− J1 f (ωm ) −
kE kI
JR ωm
#
"
0
+
− Jη
0
kI
RJ
#"
τz
ua
#
⇒
ẋ∗ = f (x∗ ) + Bu,
(26)
gdzie x∗ = [θm ωm ]T jest zredukowanym wektorem stanu manipulatora PM1R oraz ponownie u a
jest sygnałem wejściowym o charakterze sterującym, a τ z jest sygnałem wejściowym o charakterze
zakłócenia zewnętrznego (oddziaływującego na ogniwo manipulatora). Zakładając liniowy model
tarcia (6) składnik f (ωm ) = b ωm i zależność (26) jest równaniem liniowym:
"
#
"
0
1
θ̇m
=
Rb+kE kI
0 − JR
ω̇m
#"
#
"
0
θm
+
ωm
− Jη
0
kI
RJ
#"
τz
ua
#
⇒
ẋ∗ = Ax∗ + Bu.
(27)
Można zatem wyprowadzić model obu interesujących nas torów dynamiki w postaci następujących
transmitancji operatorowych:
∆
G1 (s) =
Θm (s)
kI
=
,
Ua (s)
s[JRs + (Rb + kI kE )]
∆
G2 (s) =
Θm (s)
−ηR
=
,
Tz (s)
s[JRs + (Rb + kI kE )]
(28)
gdzie J oraz b zdefiniowano w (13).
Warto podkreślić, iż w pełnym modelu łączącym część mechaniczną i elektromagnetyczną
(zarówno w formie (20)-(21) jak i uproszczonej (26)) wejściowym sygnałem sterującym (fizycznie
realizowalnym) jest napięcie twornika u a , natomiast sygnałem wyjściowym (zmienną regulowaną)
jest pozycja kątowa θm . Model manipulatora PM1R z wydzielonymi blokami mechanicznym i
elektromagnetycznym przedstawia rys. 3. W tab. 1 zebrano ponadto wszystkie wielkości fizyczne
występujące w rozważanym modelu manipulatora wraz z jednostkami układu SI.
W praktyce mamy do czynienia z modelem nieco bardziej złożonym niż ten wynikający z
powyższej idealizowanej analizy. Musimy bowiem dodatkowo uwzględnić występowanie dwóch istotnych zjawisk, a mianowicie efektów nieliniowych wynikających z nasycenia sygnału napięcia
twornika oraz nasycenia wartości prądu twornika. Bloki nieliniowe reprezentujące wspomniane
2
Taka tendencja projektowa dominuje wśród producentów współczesnych napędów pradu stałego.
KSIS-PP: Laboratorium Sterowania Robotów – B
6
Rys. 3: Schemat blokowy modelu manipulatora PM1R dla sterowania pozycyjnego z wyróżnioną
częścią elektromagnetyczną (B1) oraz mechaniczną (B2).
efekty zaznaczono na rys. 4. Rysunek ten ukazuje także blok opóźnienia charakterystyczny dla
wzmacniaczy mocy stosowanych we współczesnych napędach 3 (wartość tego opóźnienia jest jednak pomijalnie mała), a również dodatkową pętlę regulacyjną z blokiem BOP (blok ograniczenia
prądu) spełniającą rolę zabezpieczenia przetężeniowego dla prądu twornika. Pętla ta może być
traktowana jako przezroczysta w trybie rozważanego wymuszenia napięciowego. Dodatkowo należy
zaznaczyć, że w zastosowanym napędzie pomiarowo dostępny jest jedynie sygnał pozycji wału
silnika θm , zatem sygnał prędkości ωm (jeżeli wymaga tego algorytm regulacji) musi być estymowany programowo (patrz kolejny punkt). Wspomniane względy każą traktować wyprowadzone
wcześniej modele jedynie jako przyjęte aproksymacje nieliniowego systemu z rys. 4.
Rys. 4: Schemat blokowy modelu manipulatora PM1R z uwzględnieniem wzmacniacza mocy
(WM ), bloku ograniczenia prądu (BOP) oraz bloków nasycenia (Sat) sygnałów wymuszających.
2
Układ sterowania
Zadaniem sterowania w systemie z manipulatorem PM1R będzie odtwarzanie zadanej (referencyjnej) pozycji kątowej θd definiowanej jako wartość odcinkami stała lub jako trajektoria pozycji
θd (t) (sygnał θd zmienny w czasie w sposób ciągły). Do realizacji tego zadania wykorzystamy
3
Tzw. H-mostków.
7
KSIS-PP: Laboratorium Sterowania Robotów – B
Tab. 1: Wielkości fizyczne modelu manipulatora wraz z jednostkami
Wielkość
J
f (ω)
τ
ω
θ
η
b
fC
ua
E
i
kI
kE
R
L
Opis
moment bezwładności (napędu/ogniwa)
moment siły tarcia
moment siły (napędowy/zakłócenia)
prędkość obrotowa (napędu/ogniwa)
pozycja (napędu/ogniwa)
współczynnik przełożenia przekładni
współczynnik tarcia wiskotycznego
moment siły tarcia statycznego (Coulomba)
napięcie twornika
napięcie indukowane w uzwojeniu twornika
prąd twornika
stała prądowa
współczynnik prędkościowy
rezystancja uzwojenia wirnika
indukcyjność uzwojenia wirnika
Jednostka
[kg m 2 ]
[Nm]
[Nm]
[obr/s]=[Hz]
[obr]
[-]
[Nm s/obr]
[Nm]
[V]
[V]
[A]
[Nm/A]
[V s/obr]
[ohm]
[H]
sterowanie będące sumą dwóch składników: sprzężenia od stanu u e (ściślej: sprzężenia od wektora uchybu śledzenia) oraz sprzężenia wyprzedzającego u F F . Implementacja układu sterowania
odbywać się będzie na platformie z procesorem sygnałowym TMS320F2812, zatem rozważone
zostaną dyskretne wersje poszczególnych składników sterowania obliczanych z przyjętym okresem próbkowania Tp .
2.1
Generator sygnałów referencyjnych
Zadaniem bloku generatora sygnałów referencyjnych (GSR) jest generowanie w czasie rzeczywistym sygnału zadanego pozycji θmd napędu manipulatora PM1R oraz odpowiednich pochodnych tego sygnału.
2.1.1
Sygnał zadany odcinkami stały
Jakkolwiek w zadaniach sterowania pozycyjnego manipulatorów nie stosuje się sygnałów zadanych
pozycji w postaci przebiegów prostokątnych 4 , to ze względów dydaktycznych sygnały takie będą
rozważone. Pozwoli to na porównanie jakości sterowania i realizacji ruchu w sytuacji, gdy sygnał pozycji zadanej posiada bądź nie posiada własności ciągłej różniczkowalności. W przypadku
sygnału zadanego θmd odcinkami stałego (czyli w postaci przebiegu prostokątnego), równania
generatora w dyskretnej dziedzinie czasu mają postać (przebieg symetryczny względem zera):
θmd (nTp ) =
(
+Θmd dla nTp ∈ [0, T2 )
,
−Θmd dla nTp ∈ [ T2 , T )
θ̇md (nTp ) ≡ 0,
(29)
θ̈md (nTp ) ≡ 0,
4
Realizacja przebiegów prostokątnych pozycji może prowadzić do ujawnienia zjawiska rezonansów strukturalnych
elementów manipulatora, a także zwykle skutkuje zwiększeniem kosztu energetycznego sterowania.
8
KSIS-PP: Laboratorium Sterowania Robotów – B
gdzie Θmd [obr] jest parametrem projektowym, a znak wartości tego parametru ulega zmianie co
pół okresu T zadanego przebiegu prostokątnego (T [s] jest drugim parametrem projektowym i
winien stanowić całkowitą wielokrotność okresu próbkowania T p ).
2.1.2
Sinusoidalna trajektoria zadana
W przypadku sygnału zadanego θmd w postaci przebiegu sinusoidalnego równania generatora w
dyskretnej dziedzinie czasu mają postać:
θmd (nTp ) = Θmd sin(wnTp ),
θ̇md (nTp ) = wΘmd cos(wnTp ),
(30)
2
θ̈md (nTp ) = −w Θmd sin(wnTp ),
gdzie Θmd [obr] oraz w[rad/s] są parametrami projektowymi trajektorii.
2.1.3
Wielomianowa trajektoria zadana
Jeżeli sygnał zadany θmd będzie zdefiniowany jako wielomian stopnia trzeciego, wówczas równania
generatora w dyskretnej dziedzinie czasu dla zadanych warunków początkowych θ md (0), θ̇md (0)
oraz zadanych warunków końcowych θ md (Tk ), θ̇md (Tk ) mają postać:
θmd (nTp ) = a0 + a1 · (nTp ) + a2 · (nTp )2 + a3 · (nTp )3 ,
θ̇md (nTp ) = a1 + 2a2 · (nTp ) + 3a3 · (nTp )2 ,
(31)
θ̈md (nTp ) = 2a2 + 6a3 · (nTp ),
gdzie
a0 = θmd (0),
a1 = θ̇md (0),
a2 =
3[θmd (Tk ) − θmd (0)] − [2θ̇md (0) + θ̇md (Tk )]Tk
,
Tk2
a3 =
2[θmd (0) − θmd (Tk )] + [θ̇md (0) + θ̇md (Tk )]Tk
,
Tk3
natomiast θmd (0), θmd (Tk ) [obr], θ̇md (0), θ̇md (Tk ) [obr/s] oraz Tk = kTp , k ∈ N są parametrami
projektowymi trajektorii (Tk [s] jest całkowitym czasem realizacji trajektorii).
2.2
Regulator ze sprzężeniem od stanu + sprzężenie wyprzedzające
Aby zrealizować cel sterowania zastosujemy algorytm sterowania ze sprzężeniem od błędu śledzenia 5
ue uzupełniony o składnik sprzężenia wyprzedzającego u F F :
∆
u = ue + uF F ,
∆
ue = kT e = [k1 k2 ]e,
(32)
gdzie e jest uchybem odtwarzania sygnałów referencyjnych (patrz dalej). Zastosowanie nominalnego sygnału sterującego uF F wynikającego z przebiegu sygnałów zadanych, zwanego sygnałem
sprzężenia wyprzedzającego6 , pozwala na znaczne polepszenie jakości sterowania szczególnie dla
5
Sformułowanie sprzężenie od stanu należy tutaj rozumieć jako sprzężenie od wektora błędu odtwarzania, który
jest wektorem stanu w przestrzeni błędu śledzenia.
6
W języku angielskim: Feed-Forward.
9
KSIS-PP: Laboratorium Sterowania Robotów – B
zadania odtwarzania sygnałów ciągłych i zmiennych w czasie (np. sinusoidalnych). Sposób obliczania sterowania wyprzedzającego uF F wynika z analizy teoretycznej równania błędu śledzenia.
Wyprowadzenie postaci sygnału uF F przeprowadzone zostanie dla zredukowanego rzędu modelu
manipulatora PM1R reprezentowanego równaniem (26), przy założeniu, że na układ nie oddziaływuje sygnał zakłócenia (τz ≡ 0):
"
#
"
ωm
θ̇m
=
1
ω̇m
− J f (ωm ) −
kE kI
JR ωm
#
+
"
0
kI
RJ
#
⇒
u
ẋ = f (x) + bu,
(33)
gdzie przez u oznaczyliśmy wejściowe napięcie sterujące (u ≡ u a , gdy rozważamy idealny wzmacniacz mocy o wzmocnieniu jednostkowym - patrz rys. 4). Zdefiniujmy uchyb odtwarzania jako:
∆
e = xd − x
⇒
" #
"
#
"
#
e1
x − x1
θ − θm
= 1d
= md
.
e2
x2d − x2
ωmd − ωm
(34)
Pochodną powyższego równania można zapisać w następujący sposób:
(33)
ė = ẋd − ẋ = ẋd − f (x) − bu = ẋd − f (xd − e) − bu.
(35)
Podstawiając do powyższego wyrażenia definicję (32), przyjmując nieliniowy model tarcia (7)
oraz rozpisując składnik f (xd − e) jako f (xd − e) = γ(ωmd , e2 ) − He, gdzie:
"
ωmd
γ(ωmd , e2 ) =
− J1 fC sign(ωmd − e2 ) −
Rb+kE kI
ωmd
JR
#
,
"
0
1
H=
I kE
0 − Rb+k
JR
#
(36)
otrzymujemy następujące równanie w dziedzinie błędu śledzenia:
ė = (H − bkT )e + [ẋd − γ(ωmd , e2 ) − buF F ] .
(37)
Składnik uF F sterowania (32) wynika teraz z założenia zerowania wyrażenia w nawiasie po prawej
stronie (37): uF F = (bT b)−1 bT [ẋd − γ(ωmd , e2 )]. Skoro jednak sterowanie wyprzedzające jest definiowane jako sterowanie nominalne wzdłuż trajektorii zadanej, to winno być ono określone dla
e ≡ 0, czyli także dla e2 ≡ 0. Zatem ostatecznie składnik uF F wynosi:
uF F
= (bT b)−1 bT [ẋd − γ(ωmd )] .
(38)
Dla symetrycznego modelu tarcia (7) w dyskretnej dziedzinie czasu mamy:
R
Rb + kI kE
uF F (n) =
θ̇md (n) + fC sign(θ̇md (n)) ,
J θ̈md (n) +
kI
R
(39)
natomiast dla modelu niesymetrycznego (8):
uF F (n) =

R
Rb1 + kI kE


J
θ̈
(n)
+
θ̇
(n)
+
f
sign(
θ̇
(n))

C1
md
md
md

 kI
R


R
Rb2 + kI kE



J θ̈md (n) +
θ̇md (n) + fC2 sign(θ̇md (n))
kI
R
dla θ̇md (n) > 0,
(40)
dla θ̇md (n) < 0.
Podstawienie zależności (38) do równania (37) skutkuje następującym równaniem układu zamkniętego
w dziedzinie błędu śledzenia:
ė = He,
T
H = (H − bk ) =
"
0
1
I k1
− kJR
kE +kI k2
− Rb+kIJR
#
,
(41)
10
KSIS-PP: Laboratorium Sterowania Robotów – B
w którym współczynniki k1 , k2 pozwalają na swobodne lokowanie wartości własnych macierzy
H zgodnie z założeniami projektowymi dotyczącymi jakości dynamicznej sterowania (przebiegu
przejściowego błędu śledzenia e).
Ostatecznie zatem w dyskretnej dziedzinie czasu sygnał sterujący (32) przyjmuje postać:
u(n) = k1 e1 (n) + k2 e2 (n) + uF F (n),
(42)
gdzie składnik uF F (nTp ) wynika z (39) lub (40) w zależności od przyjętego modelu tarcia oraz
(34)
e1 (n) = θmd (n) − θm (n),
2.3
(34)
e2 (n) = θ̇md (n) − θ̇m (n).
(43)
Nasycenie sygnału sterującego
W praktyce nie można fizycznie zrealizować dowolnej wartości sygnału sterującego. Ograniczenia
wynikają z dostępnej mocy zastosowanego wzmacniacza mocy napędu. Aby mieć dodatkową
możliwość programowego nasycania sygnału sterującego (42), czyli sprowadzania go do wartości
uSat , wprowadza się nieliniowy blok ograniczania sygnału, którego działanie definiuje poniższa
funkcja:


u > umax ,
 umax , gdy
∆
u,
gdy umin < u < umax ,
uSat = Sat(u, umax , umin ) =
(44)

 u
u 6 umin ,
min , gdy
gdzie założone wartości umin , umax (stanowiące dolne i górne ograniczenie sterowania) wynikają
z zastosowanego wzmacniacza mocy napędu lub mogą być nałożone sztucznie (programowo).
2.4
Estymator prędkości napędu
Zamknięcie sprzężenia zwrotnego od drugiej zmiennej stanu x 2 = ωm w docelowym układzie
sterowania (42) wymaga pomiaru rzeczywistej prędkości napędu θ̇m ≡ ωm manipulatora PM1R.
W dostępnym zestawie laboratoryjnym bezpośredni pomiar prędkości nie jest możliwy, ponieważ
system nie jest wyposażony w czujnik prędkości. Pomiarowo dostępny jest natomiast sygnał
pozycji wału silnika θm jako wyjście optycznego enkodera przyrostowego. Dysponując pomiarem
pozycji kątowej możemy przeprowadzić prostą estymację prędkości silnika realizując cyfrowe
różniczkowanie pozycji z filtracją dolnoprzepustową (filtracja pozwala na ograniczenie amplitudy szumu w sygnale estymowanym). Blok estymatora prędkości napędu (EPN) można opisać
następującą transmitancją:
∆
GEP N (s) =
Ωmf (s)
sΘmf (s)
s
≡
=
,
Θm (s)
Θm (s)
Tf s + 1
(45)
gdzie ωmf ≡ θ̇mf = L−1 {Ωmf } jest estymowaną (i filtrowaną) prędkością napędu, a T f [s] jest
stałą czasową definiującą pasmo przenoszenia filtru. Zastosowanie transformacji Eulera wstecz
do dyskretyzacji powyższej transmitancji daje w efekcie następujące równanie definiujące sposób
obliczania prędkości napędu:
θ̇mf (n) =
Tf
1
θ̇mf (n − 1) +
[θm (n) − θm (n − 1)].
Tf + T p
Tf + T p
(46)
Należy zwrócić uwagę, iż wprowadzenie bloku estymatora (45) implikuje pojawienie się dodatkowej inercji w torze sprzężenia zwrotnego, którego wpływ na jakość sterowania będzie tym
bardziej istotny, im większa zostanie przyjęta wartość stałej czasowej T f . Fakt ten należy także
uwzględnić podczas parametrycznej syntezy regulatora.
11
KSIS-PP: Laboratorium Sterowania Robotów – B
Wszystkie wyżej opisane bloki funkcjonalne układu sterowania przedstawia rys. 5. Powinny
one stanowić odpowiednie funkcje programu umieszczonego na pokładzie procesora TMS320F2812
wykonywane synchronicznie z założonym okresem pracy T p .
Rys. 5: Schemat blokowy układu sterowania pozycją manipulatora z zaznaczonym generatorem
sygnałów referencyjnych (GSR), sprzężeniem od stanu, sprzężeniem wyprzedzającym (FF), blokiem programowego nasycenia sterowania (Sat) oraz blokiem estymacji prędkości napędu (EPN).
B.1 Programowo wybrać tryb wymuszenia napięciowego. Ustawić ograniczenie prądowe na
wartości odpowiadającej 50% wartości maksymalnej.
B.2 Zaimplementować na pokładzie procesora DSP funkcję GeneratorSR() obliczającą w czasie rzeczywistym aktualne próbki sygnałów referencyjnych θ md , θ̇md , θ̈md dla wszystkich
trzech rodzajów sygnałów: odcinkami stałego, sinusoidalnego oraz wielomianowego – równania (29), (30), (31). Uwaga: przyjąć następujące zakresy parametrów:
|Θmd | 6 36[obr],
|θmd (Tk ) − θmd (0)| 6 36[obr],
w < 4[rad/s],
θ̇md (0) = θ̇md (Tk ) = 0.
B.3 Zaimplementować blok estymatora prędkości napędu
EstymatorVm(). Przyjąć stałą czasową inercji estymatora
(46)
w
postaci
funkcji
Tf ≈ 0.025[s].
Sprawdzić jakość estymacji prędkości napędu (również ze względu na wartość stałej T f ).
B.4 Zrealizować blok obliczeniowy regulatora (42) bez sprzężenia wyprzedzającego (u F F ≡ 0)
w postaci funkcji StateReg().
B.5 Zrealizować blok programowego nasycania sterowania w postaci funkcji USaturation() –
równanie (44). Przyjąć następujące wartości ograniczenia sygnału sterującego:
umin = −12[V],
umax = +12[V].
B.6 Przeprowadzić syntezę parametryczną regulatora ze sprzężeniem od stanu (u = u e = kT e)
w oparciu o wybrane kryterium odpowiedzi skokowej dla układu zamkniętego.
Zamknąć pętlę sprzężenia zwrotnego od wektora błędu śledzenia. Sprawdzić i porównać
jakość odtwarzania wszystkich dostępnych rodzajów sygnałów referencyjnych dla różnych
wartości nastaw regulatora.
12
KSIS-PP: Laboratorium Sterowania Robotów – B
B.7 Zaimplementować blok sprzężenia wyprzedzającego (40) (model tarcia (8)) w postaci
funkcji UFFBlok(), przy czym parametry f C1 , fC2 oraz b1 , b2 wyznaczyć doświadczalnie.
Załączyć sygnał sprzężenia wyprzedzającego u F F w tor sterowania (u = ue + uF F ) i
sprawdzić jakość odtwarzania dla wszystkich dostępnych sygnałów referencyjnych.
Wywołanie wszystkich bloków synchronizować okresem próbkowania
Tp = 0.001[s].
Podczas realizacji ćwiczenia analizować przebiegi
e1 (n), e2 (n), uSat (n), ue (n), uF F (n), θm (n), θmd (n).
następujących
sygnałów:
♦