REKLAMA

1.pdf

Jak wczytać dane z ADXRS150 i MMA7260 oraz sterować silnikami PID?

Witam buduję balansującego robota (coś w stylu segwaya). Znalazłem taki opis(poniżej udostępniam). W tym projekcie został użyty żyroskop ADXRS150 i akcelerometr MMA7260. Pozbywanie się zakłóceń załatwia filtr kalmana, a silnikami steruje prosty regulator PID. Chciałem wykorzystać udostępnione przez autora fragmenty kodu ale mam pewne niejasności. 1)Jak wygląda procedura wczytywania informacji z akcelerometru i żyroskopu? 2)Po regulatorze PID wychodzą tylko dwie zmienne: ster1 i ster2. Jak przy ich użyciu można sterować silnikami? Fragmenty kodu zaczynają się od 79 strony. Bardzo proszę kogoś kto się na tym dobrze zna o odpowiedź na te pytania. Dziękuję i pozdrawiam

Pobierz plik - link do postu

POLITECHNIKA WROCŁAWSKA
WYDZIAŁ ELEKTRONIKI
Kierunek:
Specjalność:

Automatyka i Robotyka (AIR)
Robotyka (ARR)

PRACA DYPLOMOWA
MAGISTERSKA

Robot balansujący - model i sterowanie.
Balancing robot - modeling and control.

Autor:
Jan Kędzierski

Prowadzący pracę:
dr inż. Alicja Mazur, I-6

Ocena pracy:

WROCŁAW 2008

Pracę tą dedykuje rodzicom oraz
przyszłej żonie.

Składam serdeczne podziękowania
Pani dr inż. Alicji Mazur
za pomoc, wyrozumiałość oraz
czas poświęcony tej pracy. Chciałem również podziękować dr
inż. Markowi Wnukowi za
pomoc w uruchomieniu jednostki MPC555 oraz wszelkie
uwagi techniczne. Dziękuje także
Kołu Naukowemu Robotyków
„KoNaR” za wsparcie ﬁnansowe oraz zaplecze techniczne
udostępnione w trakcie realizacji
projektu.

Spis treści
1 Wstęp

3

2 Model robota balansującego
2.1 Model dynamiki robota balansującego
we współrzędnych uogólnionych. . . . . . . . . . . . . . . . . . . .
2.1.1 Energia kinetyczna kół. . . . . . . . . . . . . . . . . . . . .
2.1.2 Energia kinetyczna odwróconego wahadła. . . . . . . . . .
2.1.3 Energia potencjalna. . . . . . . . . . . . . . . . . . . . . .
2.1.4 Dynamiki napędów kół. . . . . . . . . . . . . . . . . . . .
2.1.5 Ograniczenia nieholonomiczne. . . . . . . . . . . . . . . . .
2.1.6 Równania dynamiki mobilnego odwróconego wahadła. . . .
2.2 Model dynamiki robota balansującego we współrzędnych fazowych
2.3 Dane platformy przyjęte podczas symulacji. . . . . . . . . . . . .

5

3 Sterowanie liniowe
3.1 Przybliżenie liniowe systemu . . . . . . . . . . . . . . . .
3.1.1 Wyznaczenie przybliżenia liniowego . . . . . . . .
3.1.2 Stabilność przybliżenia liniowego . . . . . . . . .
3.1.3 Sterowalność przybliżenia liniowego . . . . . . . .
3.2 Projektowanie sterownika liniowego . . . . . . . . . . . .
3.2.1 Sterownik minimalno-kwadratowy (LQR) . . . . .
3.2.2 Sterownik rozmieszczający bieguny (PP). . . . . .
3.2.3 Zastosowanie liniowego sterownika do nieliniowego
4 Nieliniowy sterownik
4.1 Projektowanie sterownika nieliniowego
4.1.1 Częściowa linearyzacja globalna
4.1.2 Nieliniowy sterownik . . . . . .
4.2 Symulacje . . . . . . . . . . . . . . . .

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

5 Praktyczna realizacja układu sterowania
5.1 Konstrukcja mechaniczna . . . . . . . . . . . . . .
5.1.1 Budowa korpusu robota . . . . . . . . . .
5.1.2 Napędy robota . . . . . . . . . . . . . . .
5.2 Konstrukcja elektroniczna . . . . . . . . . . . . .
5.2.1 Czujniki pomiaru prędkości obrotowej kół
5.2.2 Prosty moduł INS . . . . . . . . . . . . .
5.2.3 Sterownik napędów . . . . . . . . . . . . .
5.2.4 Zasilanie . . . . . . . . . . . . . . . . . . .

.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

. . . . . . . . .
. . . . . . . . .
. . . . . . . . .
. . . . . . . . .
. . . . . . . . .
. . . . . . . . .
. . . . . . . . .
modelu robota
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

6
6
7
9
9
11
14
17
19

.
.
.
.
.
.
.
.

20
20
20
21
22
22
23
27
30

.
.
.
.

33
33
33
34
35

.
.
.
.
.
.
.
.

40
43
43
45
46
46
46
50
52

SPIS TREŚCI

5.3

5.4
5.5

5.6

5.2.5 Główny sterownik . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Pomiar odchylenia kątowego . . . . . . . . . . . . . . . . . . . . . . . . . .
5.3.1 Wyznaczenie odchylenia od pionu na podstawie pomiaru przyspieszeń
5.3.2 Filtr Kalmana . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
5.3.3 Pomiar odchylenia kątowego, a Filtr Kalmana . . . . . . . . . . . .
Regulator PID . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Oprogramowanie . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
5.5.1 Deﬁnicje . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
5.5.2 Konﬁguracja UART . . . . . . . . . . . . . . . . . . . . . . . . . . .
5.5.3 Konﬁguracja modułu USIU . . . . . . . . . . . . . . . . . . . . . .
5.5.4 Konﬁguracja modułu MIOS1 . . . . . . . . . . . . . . . . . . . . . .
5.5.5 Konﬁguracja modułów TPU . . . . . . . . . . . . . . . . . . . . . .
5.5.6 Konﬁguracja QSMCM . . . . . . . . . . . . . . . . . . . . . . . . .
5.5.7 Konﬁguracja QADC64 . . . . . . . . . . . . . . . . . . . . . . . . .
5.5.8 Obsługa przerwań w MPC555 . . . . . . . . . . . . . . . . . . . . .
5.5.9 Filtr Kalmana . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
5.5.10 Sterownik . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Wyniki badań . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

2

54
61
61
62
65
66
67
67
69
69
70
71
72
73
75
76
79
81

6 Podsumowanie

86

Bibilograﬁa

87

Rozdział 1
Wstęp
Od kilku lat znacząco wzrosło zainteresowanie robotami balansującymi. Odwrócone wahadło już od dawna służy w uczelnianych laboratoriach jako przykład nieliniowego, niestabilnego systemu dynamicznego. Dzięki implementacji odpowiednich systemów sterowania
możliwe jest badanie jego własności. Robot balansujący jest w uproszczeniu niczym innym jak odwróconym, mobilnych wahadłem. W konstrukcji nośnej mocuje się dwa napędy
wraz z kołami. To dzięki nim możliwe jest utrzymywanie równowagi oraz przemieszczanie
się robota. Idea takiego rozwiązania pozwala lepiej zrozumieć skomplikowane, kroczące
układy lokomocji robotów humanoidalnych. Roboty balansujące przyczyniły się także
do rozwoju różnego rodzaju platform transportowych. Najpopularniejszą tego typu konstrukcją jest ”Segway” - Human Transporter [22]. Doczekał się już swojej drugiej, znacznie
bardziej dopracowanej wersji. Rozważa się także zastosowanie balansującego podwozia w
wózkach inwalidzkich. Dwukołowe podwozie łatwo pokonuje różne nierówności. Ponadto
zarówno sterowanie jak i planowanie ruchu nie wymaga stosowania skomplikowanych algorytmów, gdyż w przypadku tej klasy robotów mobilnych możliwa jest zmiana orientacji
w miejscu.
W większości popularnych algorytmów sterowania zakłada się, że wszystkie przeguby
są napędzane. W przypadku odwróconego wahadła, odchylenie spowodowane jest działaniem siły grawitacji. Przeciwdziałać temu zjawisku można jedynie poprzez odpowiednie
wysterowanie kół. Te same napędy tworzą także układ lokomocji robota. Dostępnych
jest wiele prac opisujących systemy sterowania robota utrzymującego równowagę [8], [23].
Sposób postępowania w większości przypadków jest podobny. Matematyczny opis platformy traktuje się jak liniowy obiekt, a następnie implementuje się liniowe sterowniki.
W pracy skupiono się głównie na części symulacyjnej. Wyliczono model dynamiki robota balansującego zarówno we współrzędnych uogólnionych jak i pomocniczych. Wyznaczono przybliżenie liniowe w punkcie równowagi, a także podjęto próbę wyznaczenia
sterownika liniowego w oparciu o dwie metody LQR oraz PP. Przeprowadzono szereg
symulacji w celu weryﬁkacji poprawnego działania sterownika. Następnie zaprojektowano
i przebadano nieliniowy sterownik, działający w oparciu o model dynamiki opisujący zachowanie robota. Wyniki symulacji zaprezentowano na wykresach.
W praktycznej części pracy podjęto próbę budowy opisywanej konstrukcji. Z uwagi
na to, że tematem pracy jest głównie model matematyczny robota oraz jego sterowanie,
zdecydowano o szczegółowym opisaniu tylko niektórych elementów składowych robota.
Pominięto te elementy, które nie są konieczne do powtórzenia praktycznego eksperymentu.
Niestety w trakcie budowy robota okazało się, że niektóre parametry ﬁzyczne znacznie
się różniły od tych przyjętych w symulacjach. Użyte niskiej jakości napędy przyczyniły się
do tego, że autor nie zdążył ponownie przeprowadzić symulacji. Aby jednak przetestować

1. Wstęp

4

poprawność działania wszystkich modułów robota, zaimplementowano w sterowniku algorytm rekursywnej ﬁltracji Kalmana oraz prosty podwójny sterownik PID. Wyniki eksperymentu przedstawiono na wykresach, na podstawie danych z robota wysłanych do komputera poprzez interfejs RS232.

Rozdział 2
Model robota balansującego
Robot balansujący przedstawiony schematycznie na rysunku 2.1 może być rozważany jako
odwrócone, mobilne wahadło. Opisywana konstrukcja różni się od klasycznego odwróconego wahadła tym, że nie posiada wyróżnionej platformy jezdnej [1]. Konstrukcja
nośna jest jednocześnie wahadłem, do której mocuje się dwa napędy wraz z kołami.
To dzięki nim możliwe jest utrzymywanie równowagi oraz przemieszczanie się robota.
Przedstawiona konstrukcja jest nazywana robotem balansującym, gdyż celem sterowania
jest utrzymywanie robota w pozycji pionowej, ponieważ jego środek ciężkości znajduje
się znacznie wyżej niż podwozie. W dalszej części pracy rozważono jedynie dynamikę
wahadła, kół oraz napędów robota.

Rysunek 2.1 Odwrócone wahadło na kołach - robot balansujący

2. Model robota balansującego

2.1
2.1.1

6

Model dynamiki robota balansującego
we współrzędnych uogólnionych.
Energia kinetyczna kół.

Opisywany robot balansujący można potraktować jako manipulator mobilny z niedosterowanym ramieniem manipulacyjnym [11]. W konstrukcji tej można wyróżnić dwa
układy lokalne. Jeden z nich jest stowarzyszony z osią łączącą koła Xp Yp Zp , drugi zaś z
odwróconym wahadłem Xw Yw Zw . Układ lokalny stowarzyszony z podsystemem mobilnym
Xp Yp Zp jest umiejscowiony na osi łączącej koła, w połowie odległości między nimi. Transformacja opisująca położenie i orientację robota względem globalnego układu odniesienia
X0 Y0 Z0 jest wyrażona wzorem


cos θ − sin θ 0 x
 sin θ cos θ 0 y 
p

T0 (x, y, θ) = T rans(X, x)T rans(Y, y)Rot(Z, θ) = 
 0
0
1 0 
0
0
0 1
Opisywana konstrukcja nie posiada wyróżnionego nadwozia. Napędy wraz z kołami
są dołączone bezpośrednio do wahadła, które spełnia rolę nadwozia, dlatego w tym przypadku nie oblicza się energii kinetycznej nadwozia. Wyznaczamy jedynie energię kinetyczną kół. Do wyliczenia energii poszczególnych kół konieczne jest wyznaczenie położenia
układów lokalnych Xi Yi Zi stowarzyszonych z obracającymi się kołami. Transformacja dla
pierwszego koła ma postać
π
p
k1
T0 = T0 Rot(Z, − )T rans(X, b)Rot(X, −φ1 ).
2

(2.1)

Z pracy [3] wiadomo, że dla transformacji (2.1) energia kinetyczna pierwszego koła będzie
równa
1
1
1
˙
˙ ˙
˙
˙
˙
K1 = Ixx1 φ2 + Izz1 θ2 + Mk1 (x2 + y 2 + b2 θ2 ) + Mk1 bθ(y sin θ + x cos θ).
˙
˙
1
2
2
2
Podobnie dla drugiego koła transformacja przekształcająca układ lokalny X2 Y2 Z2 do
układu podstawowego X0 Y0 Z0 jest następująca
π
p
k2
T0 = T0 Rot(Z, )T rans(X, b)Rot(X, φ2 ),
2
a energia kinetyczna tego koła wynosi
1
1
1
˙
˙
˙
˙ ˙
K2 = Ixx2 φ2 + Izz2 θ2 + Mk2 (x2 + y 2 + b2 θ2 ) − Mk2 bθ(y sin θ + x cos θ),
˙
˙
˙
2
2
2
2
gdzie poszczególne symbole oznaczają:
2
• Ixxi = 1 Mki Ri - moment bezwładności i-tego koła względem osi Xi ,
2

• Izzi =

1
2
Mki wi
12

2
+ 1 Mki Ri - moment bezwładności i-tego koła względem osi Zi ,
4

• Mki - masa i-tego koła,

2. Model robota balansującego

7

• Ri - promień i-tego koła,
• wi - grubość i-tego koła,
• b - odległość i-tego koła od początku układu lokalnego robota Xp Yp Zp .
W dalszych rozważaniach przyjmujemy, że oba koła są jednakowe, a więc mają takie same
wszystkie parametry. Po zsumowaniu energii kinetycznej obydwu kół
Kk = K1 + K2
otrzymujemy

1
˙
˙
˙
Kk = Ixx (φ2 + φ2 ) + Ip θ2 + (x2 + y 2 )Mk ,
˙
˙
2
1
2

gdzie
Ip = Izz + Mk b2 .
Energię kinetyczną kół można wyrazić w postaci formy kwadratowej jako
1 T
Kk = qk Qk (qk )qk ,
˙
˙
2

qk = (x, y, θ, φ1 , φ2 )T

z diagonalną macierzą bezwładności

2Mk
0
0
0
0
 0
2Mk 0
0
0

 0
0
2Ip 0
0
Qk = 
 0
0
0 Ixx 0
0
0
0
0 Ixx

2.1.2




.



Energia kinetyczna odwróconego wahadła.

Aby obliczyć energię kinetyczną wahadła, należy wyznaczyć transformację kinematyczną
końca wahadła Xw Yw Zw względem układu podstawowego [4]. Wahadło jest zaczepione
w środku lokalnego układu robota Xp Yp Zp . Wówczas transformację układu Xw Yw Zw
względem układu podstawowego można wyrazić jako
p
w
T0 = T0 Rot(X, 90o )Rot(Z, 90o − α)T rans(X, l)

gdzie l to długość wahadła, a α to odchylenie wahadła od pionu, jak oznaczono na rysunku
2.1. Transformacja ta jest równa


cos θ sin α − cos θ cos α sin θ x + l cos θ sin α
 sin θ sin α − sin θ cos α − cos θ y + l sin θ sin α 
w
.
T0 = 
 cos α

sin α
0
l cos α
0
0
0
1
Do wyznaczenia energii kinetycznej potrzebna jest także macierz pseudoinercji wahadła
(ogniwa L) względem układu lokalnego. Macierz ta jest zdeﬁniowana w następujący
sposób


x2 · dm L xy · dm L xz · dm L x · dm
L

xy · dm L y 2 · dm L zy · dm L y · dm 
.
Jw =  L

xz · dm L zy · dm L z 2 · dm L z · dm 
L
x · dm
y · dm
z · dm
dm
L
L
L
L

2. Model robota balansującego

8

Przyjęto założenie, że wahadło ma postać pręta jednorodnego, którego długość jest ponad
10 razy większa niż średnica przekroju. Wówczas macierz pseudoinercji wynosi
 M l2

w
0 0 − M2w l
3
 0
0 0
0 


Jw = 
0
0 0
0 
− M2w l 0 0 Mw
Energię kinetyczną wahadła można wyliczyć z następującego wzoru
1
˙w
˙w
Kw = tr T0 Jw (T0 )T .
2
Energia ta jest równa
1
˙˙
˙
Kw = − M l2 (−3x2 + 3θxl sin θ sin α − 3αxl cos θ cos α − θ2 l2 +
˙
˙˙
6
˙
˙˙
+θ2 l2 cos α2 − 3y 2 − 3θyl cos θ sin α − 3αyl sin θ cos α − α2 ).
˙
˙˙
˙
Energię wahadła (2.2) można również przedstawić w postaci formy kwadratowej
1
Kw = q T Qw (q)q,
˙
˙
2

q = (x, y, θ, φ1 , φ2 , α)T

z diagonalną macierzą bezwładności

Q11 0 Q13
 0 Q22 Q23

 Q
Q23 Q33
Qw (q) =  13
 0
0
0

 0
0
0
Q16 Q26 0

0
0
0
0
0
0

0 Q16
0 Q26
0 0
0 0
0 0
0 Q66

przy czym poszczególne elementy tej macierzy są równe
Q11 = Mw ,
Q22 = Mw ,
1
Q33 = − Mw l2 (cos2 α − 1, )
3
1
Q66 = Mw l2 ,
3
1
Q13 = − Mw l sin θ sin α,
2
1
Q23 = Mw l cos θ sin α,
2
1
Q16 = Mw l cos θ cos α,
2
1
Q26 = Mw l sin θ cos α.
2





,




(2.2)

2. Model robota balansującego

2.1.3

9

Energia potencjalna.

W opisywanej konstrukcji uwzględniono jedynie energię potencjalną dla podsystemu wahadła ponieważ koła poruszają się po powierzchni ekwipotencjalnej. Energię potencjalną
wahadła można obliczyć ze wzoru
w
Vw = −Mw & lt; g, T0 Rw & gt;

(2.3)

gdzie Rw to wektor opisujący położenie środka masy konstrukcji nośnej (wahadła) względem układu lokalnego Xw Yw Zw . Wektor grawitacji jest wyrażony


 1 
0
−2l
 0 
 0 



g=
(2.4)
 −g  , R =  0  .
0
1
Ze wzorów (2.3) i (2.4) otrzymuje się energię potencjalną odwróconego wahadła
1
Vw = Mw lg cos α.
2

2.1.4

Dynamiki napędów kół.

Opisywany robot jest napędzany dwoma silnikami prądu stałego wraz z dwustopniową
przekładnią planetarną. Rysunek 2.2 przedstawia model silnika DC, który zostanie uwzględniony w modelu robota. Wejściem silnika jest przyłożone napięcie, wyjściem - moment
potrzebny do tego, aby robot utrzymywał równowagę.

Rysunek 2.2 Model silnika prądu stałego.
Napięcie przyłożone do elektrod silnika generuje prąd i płynący przez jego cewki.
Z kolei, prąd wytwarza moment obrotowy proporcjonalny do wartości prądu ze stałym
współczynnikiem km równy
τm = km i.
Cewki silnika, poza indukcyjnością L, posiadają także rezystancję. Obydwa te parametry
modeluje się jako elementy połączone szeregowo. Należy także zwrócić uwagę na fakt, że
cewki wirnika obracają się w polu magnetycznym, które jest wytworzone przez magnesy
stałe. Wskutek tego pojawi się, skierowana przeciwnie do napięcia zasilania, zwrotna
siła elektromotoryczna Ve . Można przyjąć, że jest to liniowa funkcja prędkości obrotowej
silnika
Ve = ke ω.

2. Model robota balansującego

10

Korzystając z pierwszego prawa Kirchhoﬀa, można zapisać równanie różniczkowe dla
prądu płynącego w obwodzie silnika jako
V a − Rs i − L

di
− Ve = 0.
dt

(2.5)

Następnie należy skorzystać z Zasad Dynamiki Newtona, które mówią o tym, że suma
wszystkich momentów sił jest liniowo zależna od przyspieszenia osi pomnożonej przez
jej bezwładność. Tarcie elementów ruchomych można przybliżyć jako liniową funkcję
prędkości obrotowej ze współczynnikiem tarcia kf , jak następuje
M = τm − kf ω − τa = IR

dω
,
dt

(2.6)

gdzie IR to bezwładność wirnika, zaś τa to moment przyłożony do osi napędu. Z powyższych
równań uzyskuje się dwa równania różniczkowe pierwszego rzędu, które są liniowymi
funkcjami prądu i prędkości obrotowej
di
=
dt
dω
=
dt

Va
L

−

km
i
IR

Rs
i
L

−

−

kf
ω
IR

ke
ω,
L

(2.7)

τa
.
IR

(2.8)

−

W praktyce często pomija się indukcyjność oraz tarcie w małych silnikach DC. Ponieważ
rozważana konstrukcja jest wyposażona właśnie w takie niewielkie silniki, to w modelu
konstruowanego robota balansującego powyższe (2.5) i (2.6) upraszczają się do postaci
i=
dω
=
dt

Va
Rs

−

km
i
IR

ke
ω,
Rs

(2.9)

τa
.
IR

(2.10)

−

Dokonując podstawienia równania (2.9) do (2.10) otrzymano
dω
km
ke km
IR =
Va −
ω − τa .
dt
Rs
Rs

(2.11)

Równanie (2.11) można przedstawić w ogólnej postaci modelu przestrzeni stanów, gdzie
stanem jest położenie osi koła θ i jej prędkość ω. Wejścia systemu to napięcia przyłożone
do elektrod silnika Va oraz moment przyłożony do osi napędu τa
˙
θ
ω
˙

=

0
1
ke
0 − IRkm
Rs

θ
ω

0

+

km
IR Rs

0
− I1
R

Va
τa

,

zaś wyjściem jest położenie osi θ
y=

1 0

θ
ω

.

W przedstawionej konstrukcji robota użyto dwóch jednakowych silników prądu stałego
i ich identyczne modele włączono do modelu robota balansującego. Oznaczenie pozycji θ,
prędkości kątowej ω i napięcia Va w modelu i-tego silnika dla zgodności z modelem kół w
˙
robocie zastąpiono oznaczeniami φ, φ oraz Vi . Moment τa przyłożony do osi k-tego silnika
to moment pochodzący od kół. Wówczas równanie (2.11) przyjmuje postać
km
ke km ˙
¨
φi =
Vi .
τa + IR φi +
Rs
Rs

2. Model robota balansującego

11

We współrzędnych uogólnionych q robota balansującego, model silników można zapisać
jako
V1
Qs q + T q = B
¨
˙
,
V2
gdzie macierz bezwładności silnika to

0
 0

 0
Qs = 
 0

 0
0
zaś macierz tarcia to





T =




0
0
0
0
0
0

0
0
0
0
0
0

0
0
0
0
0
0
0
0
0
0
0
0

0 0 0
0 0 0
0 0 0
0 IR 0
0 0 IR
0 0 0
0
0
0
ke km
Rs

0
0

0
0
0
0
ke km
Rs

0

0
0
0
0
0
0





,




0
0
0
0
0
0





.




W niniejszej pracy rozważono dwa przypadki oddziaływania napędów na model dynamiki
robota balansującego. W pierwszym z nich przyjęto, że zamocowane do wahadła napędy
będą oddziaływały na nie z taką samą siłą, jak na koła, tylko że skierowaną w przeciwnym
kierunku. Macierz wejścia ma wtedy postać


0
0
 0
0 


 0
0 
 km

B=
(2.12)
0 .
 Rs

km 
 0
Rs
− km − km
Rs
Rs
Drugi przypadek to taki, w którym przyjęto, że napędy oddziałują jedynie na koła robota. Utrzymywanie wahadła w pozycji pionowej uzyskuje się w sposób pośredni czyli np
poprzez odpowiednie przemieszczanie się podwozia. Macierz wejścia ma wtedy postać


0
0
 0
0 


 0
0 
.
(2.13)
B =  km

0 
 Rs km 
 0

Rs
0
0
Pierwszy przypadek wykorzystano dla sterowników liniowych, drugi zaś dla nieliniowych.

2.1.5

Ograniczenia nieholonomiczne.

Odwrócone mobilne wahadło napędzane jest dwoma kołami ustalonymi. Przyjmując założenie, że robot porusza się bez poślizgów kół, można wyprowadzić tzw. ograniczenia
fazowe [1] w postaci Pfaﬀa
A(qk )qk = 0.
˙
(2.14)

2. Model robota balansującego

12

Dla opisywanej dwukołowej konstrukcji przyjęto założenie, że prędkości w punkcie kontaktu kół z podłożem są równe zero. Zatem nie występuje poślizg boczny, poślizg wzdłużny
oraz buksowanie kół.
Ruch bez poślizgu bocznego oznacza brak składowej prędkości prostopadłej do kierunku
ruchu. Równania składowych prostopadłych mają postać

Rysunek 2.3 Ruch robota po płaszczyźnie.
VL = x sin θ
˙
VP = y cos θ,
˙
a po podstawieniu
VL − VP = x sin θ − y cos θ = 0.
˙
˙
Tak wyprowadzone ograniczenie w postaci Pfaﬀa wygląda następująco
(sin θ, − cos θ, 0, 0, 0)qk = 0.
˙
Brak poślizgu i buksowania kół oznacza brak prędkości w punkcie styku koła z podłożem,
co przedstawiono na rysunku 2.4. Zgodnie z rys. 2.4, ograniczenie to zapisano w postaci
ogólnej dla i-tego koła jako
˙
Vi = Rφi ,
xi = Vi cos θ,
˙
yi = Vi sin θ.
˙
W przypadku niespełnienia pierwszej z powyższych równości następuje zjawisko

(2.15)
(2.16)
(2.17)

2. Model robota balansującego

13

Rysunek 2.4 Ruch robota po płaszczyźnie.
˙
• V & lt; Rφ - buksowanie
˙
• V & gt; Rφ - poślizg
Prędkość V to suma prędkości wzdłuż osi x, y oraz prędkości związanej z orientacją robota
θ. Dla koła pierwszego wyprowadzenie ograniczenia wygląda następująco
x1 = x + b sin θ,

y1 = y − b cos θ,

˙
x1 = x + θb cos θ,
˙
˙

y1 = y + θb sin θ.
˙
˙ ˙

a po zróżniczkowaniu

Następnie podstawiając do składowych prędkości x1 oraz y1 równanie (2.15) otrzymuje
˙
˙
się
˙
˙
x cos θ + θb cos2 θ − Rφ1 cos2 θ = 0
˙
˙
˙
−y sin θ − θb sin2 θ + Rφ1 sin2 θ = 0
˙

(2.18)

oraz przyrównując do siebie równania (2.18) otrzymano ograniczenie mówiące o braku
poślizgu wzdłużnego dla pierwszego koła
˙
˙
x cos θ + y sin θ + bθ − Rφ1 = 0.
˙
˙
Ponieważ drugie koło kręci się w przeciwnym kierunku do koła pierwszego ograniczenie
dla tego koła ma postać
˙
˙
−x cos θ − y sin θ + bθ + Rφ2 = 0.
˙
˙
Ostatecznie można zapisać wszystkie ograniczenia w postaci macierzowej Pfaﬀa


x
˙


sin θ − cos θ 0 0 0  y 
 ˙ 
˙
sin θ b −R 0   θ  = 0.
A(qk )q˙k =  cos θ


˙
− cos θ − sin θ b 0 R  φ1 
˙
φ2

2. Model robota balansującego

14

Z warunku Pfaﬀa (2.14) wynika, że prędkości kół należą do jądra macierzy A(qk ), czyli
qk ∈ KerA(qk ). Należy znaleźć wektory rozpinające te jądro, czyli
˙
qk = G(qk )η,
˙
gdzie η ∈ Rm , m = n − l, n-liczba zmiennych stanu, l-liczba ograniczeń. Macierz G(qk )
spełnia zatem warunek
A(qk )G(qk ) ≡ 0
i można ją wybrać w następujący sposób

cos θ cos θ
 sin θ sin θ

−1
G(qk ) =  1
b
b
 2

0
R
2
0
R




,



η=

η1
η2

,

gdzie η1 ma sens przeskalowanej prędkości koła pierwszego, a η2 na sens przeskalowanej
prędkości koła drugiego. Prędkości η noszą nazwę prędkości pomocniczych.

2.1.6

Równania dynamiki mobilnego odwróconego wahadła.

Po uzyskaniu wzorów na energię kinetyczną i potencjalną wahadła należy funkcję Lagrange’a równą
L(q, q) = Kw + Kk − Vw
˙
wstawić do zasady d’Alemberta w celu uzyskania równań dynamiki
Qw (q)¨ +
q

Qk 0
0 0

q + Qs (q)¨ + Cw (q, q)q + Ck (q, q)q + Cs (q, q)q
¨
q
˙ ˙
˙ ˙
˙ ˙
T

+ T (q)q + D(q) = A (q)λ + Bu
˙
gdzie
• q = (x, y, θ, φ1 , φ2 , α)T - wektor zmiennych stanu,
• Qw (q) - macierz bezwładności wahadła,
• Qk - macierz bezwładności kół,
• Qs - macierz bezwładności silników,
• Cw (q, q) - macierz Coriolisa wahadła,
˙
• Ck (q, q) - macierz Coriolisa kół,
˙
• Cs (q, q) - macierz Coriolisa silników,
˙
• T - macierz tarcia pochodząca od silników,
• D(q) - wektor grawitacji.

(2.19)

2. Model robota balansującego

15

Aby uprościć zapis, macierze Qw , Qk i Qs dodano do siebie w modelu jako
Qwks (q)¨ + Cw (q, q)q + T q + D(q) = AT (q)λ + Bu
q
˙ ˙
˙
i otrzymano macierz bezwładności całości

Q11 0 Q13 0
0 Q16
 0 Q22 Q23 0
0 Q26

 Q
Q23 Q33 0
0
0
Qkws (q) =  13
 0
0
0 Q44 0
0

 0
0
0
0 Q55 0
Q16 Q26 0
0
0 Q66










(2.20)

gdzie
Q11 = 2Mk + Mw ,
Q22 = 2Mk + Mw ,
1
Q33 = 2Ip − Mw l2 (cos2 α − 1),
3
Q44 = Ixx + IR ,
Q55 = Ixx + IR ,
1
Q66 = Mw l2 ,
3
1
Q13 = − Mw l sin θ sin α,
2
1
Q23 = Mw l cos θ sin α,
2
1
Q16 = Mw l cos θ cos α,
2
1
Q26 = Mw l sin θ cos α,
2
oraz
• Ixx = 1 Mk R2 ,
2
• Izz =

1
Mk w2
12

+ 1 Mk R2 ,
4

• Ip = Izz + Mk b2 .
Wektor grawitacji ma postać




D(q) = 




0
0
0
0
0
1
− 2 Mw lg sin α










Ponieważ macierze bezwładności Qk (q) oraz Qs (q) są stałe, to macierze Coriolisa
Ck (q, q) oraz Cs (q, q) będą równe 0. Zatem należy obliczyć jedynie macierz Cw (q, q).
˙
˙
˙
Korzystając ze wzoru na symbole Christoﬀela
C ij =

i
Ckj qk ,
˙
k

2. Model robota balansującego

16

gdzie

∂Qij ∂Qik ∂Qjk
+
−
∂qk
∂qj
∂qi

1
2

i
Ckj =

wyznaczono następującą macierz




Cw (q, q) = 
˙




0
0
0
0
0
0

0
0
0
0
0
0

C13
C23
C33
0
0
C63

0
0
0
0
0
0

0 C16
0 C26
0 C36
0 0
0 0
0 0





,




której poszczególne elementy są równe
1
˙
C13 = − Mw l(cos θ sin αθ + sin θ cos αα),
˙
2
1
˙
C16 = − Mw l(sin θ cos αθ + cos θ sin αα),
˙
2
1
˙
C23 = − Mw l(sin θ sin αθ − cos θ sin αα),
˙
2
1
˙
C26 = Mw l(cos α cos θθ − sin α sin θα),
˙
2
1
C33 = Mw l2 cos α sin αα,
˙
3
1
˙
C36 = Mw l2 cos α sin αθ,
3
1
˙
C63 = − Mw l2 cos α sin αθ.
3

(2.21)

Pozostałe macierze pozostają bez zmian




T =








B=




0
0
0
km
Rs

0
0
0
0
0
0

0
0
0
0
0
0

0
0
0
0
0
0

0
0
0

0
0
0
0

ke km
Rs

ke km
Rs

0
0

0





0
0
0
0

km
0
Rs
km
− Rs − km
Rs




,




u=

0
0
0
0
0
0





,




0
0
0




lub B =  km

 Rs
 0
0
V1
V2

.

(2.22)

0
0
0
0
km
Rs





,




(2.23)

0

(2.24)

2. Model robota balansującego

17

Wektor stanu q ma postać




q=




2.2



x
y
θ
φ1
φ2
α




.




(2.25)

Model dynamiki robota balansującego we współrzędnych fazowych

Wyprowadzony w podrozdziale 2.1.6 model dynamiki robota we współrzędnych uogólnionych można wyrazić również w postaci modelu we współrzędnych pomocniczych [4].
Dzięki temu zabiegowi zmienne stanu q zostaną zastąpione zmiennymi sterowalnymi η1 i
η2 (prędkościami pomocniczymi).
Zależność pomiędzy wektorem stanu qk , a η jest następująca
˙
qk = G(qk )η.
˙

(2.26)

Model dynamiki we współrzędnych uogólnionych
Qkws (q)¨ + Cw (q, q)q + D(q) + T q = AT λ + Bu
q
˙ ˙
˙

(2.27)

gdzie A to macierz ograniczeń Pfaﬀa można zapisać jako
Q11 Q12
Q21 Q22
0
Dα

+

+

qk
¨
α
¨
Tk
0

C11 C12
C21 C22

+
qk
˙
α
˙

=

Bk
Bα

qk
˙
α
˙

+
(2.28)

AT λ
0

u+

Aby wyrazić powyższy model we współrzędnych pomocniczych należy wyliczyć wektor q ,
¨
korzystając z zależności (2.26)
˙
qk = Gη + Gη.
¨
˙

(2.29)

Podstawiając do modelu (2.28) zależności (2.26) i (2.29) i po przemnożeniu przez GT
pierwszego równania macierzowego, otrzymano nowe równanie macierzowe
GT Q11 G GT Q12
Q21 G
Q22
+

η
˙
α
¨

+

˙
GT (Q11 G + C11 G) GT C12
˙ + C21 G
Q21 G
C22

η
α
˙

+

GT Tk G
0

η
α
˙

=

+

0
Dα

+

(2.30)
GT Bk
Bα

u

Ostatecznie model dynamiki we współrzędnych pomocniczych przyjmuje postać
Q∗ (q)
gdzie

η
˙
α
¨

+ C ∗ (q, q)
˙

η
α
˙

+ D∗ + T ∗

η
α
˙

= B ∗ u,

(2.31)

2. Model robota balansującego

• macierz bezwładności to

18




Q∗ Q∗ Q∗
11
12
13
Q∗ (q) =  Q∗ Q∗ Q∗  ,
21
22
23
Q∗ Q∗ Q∗
31
32
33

(2.32)

a współczynniki tej macierzy to
Q∗ = −
11

Q∗
12
Q∗
13
Q∗
21
∗
Q22
Q∗
23
Q∗
31
∗
Q32
Q∗
33

1
3b2 R2

(−3b2 R2 Mw − 6b2 R2 Mk − R2 Mw l2 + R2 Mw l2 cos2 α,

− 6R2 Ip − 12b2 Ixx − 12b2 Ir),
1
= 2 (3b2 Mw + 6b2 Mk − Mw l2 + Mw l2 cos2 α − 6Ip),
3b
1
= Mw l cos α,
2
=Q12 ,
=Q11 ,
=Q13 ,
=Q13 ,
=Q13,
1
= M l2
3

• macierz sił odśrodkowych i sił Coriolisa
 ∗

∗
∗
C11 C12 C13
∗
∗
∗
C ∗ (q, q) =  C21 C22 C23  ,
˙
∗
∗
∗
C31 C32 C33

(2.33)

(2.34)

ze współczynnikami
Mw l2
sin(2α)α,
˙
6b2
Mw l
˙
= − 2 sin α(l cos αα − 3bθ,
˙
3b
Mw l
˙
=
sin α(2l cos αθ − 3bα),
˙
6b
Mw l
˙
= − 2 sin α(l cos αα + 3bθ),
˙
3b
∗
=C11 ,
Mw l
˙
sin α(2l cos αθ + 3bα),
˙
=−
6b
Mw l2
˙
=−
sin(2α)θ,
6b
∗
= − C31 ,
=0

∗
C11 =
∗
C12
∗
C13
∗
C21
∗
C22
∗
C23
∗
C31
∗
C32
∗
C33

• wektor grawitacji


0
,
0
D∗ (q) = 
1
− 2 Mw lg sin α

(2.35)



(2.36)

2. Model robota balansującego

19

• macierz tarcia

T∗ = 

• macierz wejściowa
 2
B∗ = 

k
RRs m
0
− km
Rs

4
kk
R2 Rs e m

0
0


0
2
k ,
RRs m
km
− Rs


0
0
4
kk
0 ,
R2 Rs e m
0
0


lub B ∗ = 

2
k
RRs m

0
0

(2.37)


0
2
k ,
RRs m
0

(2.38)

gdzie
u=

2.3

V1
V2

.

(2.39)

Dane platformy przyjęte podczas symulacji.

W opisywanej konstrukcji można wyróżnić trzy główne składowe elementy: koła oraz część
nośną. W części nośnej zainstalowano dwa napędy wraz z przekładniami planetarnymi.
Do osi napędów zamocowano koła.
UWAGA! Poniższe dane przyjęto jedynie w symulacjach i niektóre z nich
mogą sie różnić od parametrów modelu wykonanego w praktyce.
oznaczenie wartość

jednostka opis

Mk
Mw
R
l
w
b
km

0,2
2
0,06
0,6
0,005
0,15
0,006123

kg
kg
m
m
m
m
N· m/A

ke

0,00692

V· s/rad

Rs
g
Ixx
Izz
IR
Ip

3
9,81
0,00036
0,00018
0,005
0,0047

Ω
m/s2
kg· m2
kg· m2
kg· m2
kg· m2

masa koła
masa wahadła
promień koła
długość wahadła
szerokość koła
odległość koła od środka platformy
współczynnik wytworzonego momentu
do prądu płynącego przez cewki silnika
współczynnik wytworzonego napięcia
w obracających cewkach silnika
rezystancja cewek silnika
grawitacja
moment bezwładności koła względem osi X
moment bezwładności koła względem osi Z
moment bezwładności napędu
moment platformy wraz z kołami względem osi Z

Rozdział 3
Sterowanie liniowe
Jak wspomniano we wstępie, większość rozwiązań praktycznych w literaturze opartko na
sterowaniu dla układów liniowych. Aby zbadać teoretycznie rozwiązania innych autorów
znane z literatury, należy wyznaczyć przybliżenie liniowe robota balansującego w pobliżu
punktu równowagi. Następnie do otrzymanych równań przybliżonych można zastosować
liniowe regulatory LQR i PP, co pokazano w kolejnych podrozdziałach.
Należy podkreślić, że aby móc zastosować sterowniki liniowe, macierz B
musiała mieć postać (2.12). Zatem w dalszych rozważaniach zakłada się, że
napędy oddziaływają zarówno na koła jak i na wahadło.

3.1

Przybliżenie liniowe systemu

Do utrzymywania robota w pozycji pionowej podjęto próbę zaimplementowania sterownika liniowego. Konstrukcja sterowników jest oparta na pierwszej zasadzie Lapunowa [2]
i wykorzystuje metody projektowania liniowych sterowników rozmieszczających bieguny
oraz minimalno-kwadratowych. Do zaprojektowania sterowników niezbędne jest znalezienie przybliżenia liniowego w pobliżu punktu równowagi systemu o postaci
x = f (x) + g(x)u.
˙

3.1.1

(3.1)

Wyznaczenie przybliżenia liniowego

Nieliniowy model dynamiki we współrzędnych pomocniczych ma postać
Q∗ (q)

η
˙
α
¨

+ C ∗ (q, q)
˙

η
α
˙

+ D∗ + T ∗

η
α
˙

= B ∗ u,

Następnie przekształcono równanie w następujący sposób
η
˙
α
¨

= Q∗−1 −(C ∗ + T ∗ )

η
α
˙

− D∗

+ Q∗−1 B ∗ u = f (x) + g(x)u,

(3.2)

gdzie x jest nowym wektorem stanu równym
x1 =φ1 ,
˙
x2 =φ1
x3 =φ2 ,
˙
x4 =φ2
x5 =α,
x6 =α
˙

(3.3)

3. Sterowanie liniowe

21

Widać, że f (x) i g(x) ∈ I 3 . Ponieważ w modelu (3.2) korzystamy ze współrzędnych
R
˙
pomocniczych η, to do poprawnej interpretacji prędkości kół φ należy pierwsze dwa wiersze
równania odpowiednio przeskalować
2
˙
φi = ηi .
R

(3.4)

W nowych współrzędnych x równania dynamiki (3.2) maja postać

 

 
x1
˙
0
x2
 x2   2 f1 (x)   2 g1 (x) 

 ˙   R
  R

 x3   x 4  

 ˙ = 2
+ 2 0
 x4   f2 (x)   g2 (x)  u = F (x) + G(x)u
˙   R


  R

 x5   x 6  
0
˙
g3 (x)
f3 (x)
x6
˙

(3.5)

Następnie dokonano przybliżenia liniowego równań wokół punktu równowagi
˙
˙
x0 = [φ1 , φ1 , φ2 , φ2 , α, α]T = [0, 0, 0, 0, 0, 0]T
˙

i u=0

(3.6)

∂G
|x
∂x 0

(3.7)

w celu otrzymania ogólnej liniowej postaci równania stanu
x = Ax + Bu, gdzie A =
˙
Obliczeń dokonano w
następujące wartości

0
1
 0 −0, 0723

 0
0
x=
˙ 
0 0, 0048

 0
0
0 0, 0051

3.1.2

∂F
|x ,
∂x 0

B=

środowisku Matlab korzystając z funkcji jacobian. Otrzymano
0
0
0
0
0 0, 0048 −63, 2450 0
0
1
0
0
0 −0, 0723 −63, 2450 0
0
0
0
1
0 0, 0051
34, 0117 0











x + 







0
0
0, 3352
0, 0011
0
0
0, 0011
0, 3352
0
0
−0, 0337 −0, 0337





 u (3.8)




Stabilność przybliżenia liniowego

Według pierwszej metody Lapunowa układ dynamiczny jest stabilny wtedy i tylko wtedy,
gdy wszystkie wartości własne λi macierzy systemowej A przybliżenia liniowego mają nie
dodatnie części rzeczywiste, a każda wartość własna o zerowej części rzeczywistej jest
wartością własną jednokrotna [2].
W przypadku wyznaczonego przybliżenia liniowego system nie jest stabilny. Obliczenia
wykonano w środowisku Matlab przy użyciu funkcji eig
eig(A)
λ1
λ2
λ3
λ4
λ5
λ6

=0
=0
= 5.8226
= −5.8415
= −0.0486
= −0.0771

Jak widać Re(λ3 ) & gt; 0 zatem potwierdza się hipoteza o niestabilności układu.

(3.9)

3. Sterowanie liniowe

3.1.3

22

Sterowalność przybliżenia liniowego

Układ dynamiczny jest sterowalny wtedy i tylko wtedy, gdy rank(Ω) = n. Macierz
Ω (Ω = [B; AB; . . . ; An−1 B]) to macierz sterowalności Kalmana [2]. Jej wyznaczenia
dokonano w środowisku Matlab przy użyciu funkcji ctrb
ctrb(A,B)

rank([B; AB; . . . ; An−1 B]) = 6

(3.10)

Jak widać, system (3.8) jest sterowalny.

3.2

Projektowanie sterownika liniowego

Na potrzeby sterowania robotem balansującym zaprojektowano sterownik liniowy korzystając z dwóch metod. Pierwszy z nich to sterownik minimalno-kwadratowy, drugi to
sterownik rozmieszczający bieguny. Teoria regulacji korzysta z idei sprzężenia zwrotnego.
Wektor stanu przemnożony przez odpowiednią macierz wzmocnień K podawany jest na
wejście systemu, jak pokazano na rysunku 3.1. Prawo sterowania ma postać
u = −Kx.

(3.11)

Rysunek 3.1 Schemat blokowy sterownika dla liniowej wersji systemu
Przybliżenie liniowe robota balansującego ma postać (3.7)
x = Ax + Bu.
˙
Stosując prawo sterowania (3.11) do modelu (3.7) otrzymano
˜
x = (A − BK)x = Ax,
˙

(3.12)

Aby układ z zamknięta pętlą sprzężenia zwrotnego był stabilny, musi byc spełniony
˜
warunek ∀i eigi (A) & lt; 0.

3. Sterowanie liniowe

3.2.1

23

Sterownik minimalno-kwadratowy (LQR)

Jednym z najczęściej używanych sterowników liniowych jest LQR. Podczas sterowania
zakładamy, że system liniowy znajduje się w stanie równowagi. Celem sterowania jest
utrzymanie systemu w położeniu równowagi, pomimo oddziałujących na niego zakłóceń.
Zakładamy ponadto, że znany jest jego stan początkowy oraz wszystkie zmienne są
mierzalne.
Należy wyznaczyć sterowanie u, które minimalizuje kwadratowy wskaźnik jakości:
J=

T

1
2

xT (t)Qx(t) + uT (t)Ru(t) dt,

0

Q =QT ≥ 0,

(3.13)

R =RT & gt; 0,
gdzie Q i R są stałymi diagonalnymi nastawami regulatora. W praktyce dobiera się
wartości elementów macierzy Q i R w następujący sposób:
Qii = 1/max. akceptowalna wartość [x2 ]
i
Rii = 1/max. akceptowalna wartość [u2 ]
i

(3.14)

Forma kwadratowa xT Qx we wskaźniku jakości J odpowiada kosztowi, jaki ponosi układ
ze względu błąd położenia x (opisuje błąd położenia i prędkości w stanie braku równowagi
układu). Człon uT Ru opisuje koszt energrtyczny zastosowanego sterowania.
(UWAGA! Oznaczenia Q,R nie są tymi samymi oznaczeniami jakie występują w poprzednich rozdziałach. Zostały użyte tylko przy opisie sterownika
LQR.)
Prawo sterowania w sterowniku LQR ma postać
u = −R−1 B T P,

(3.15)

gdzie P uzyskuje się poprzez rozwiązanie następującego równania Riccatiego
0 = P A + AT P − P BR−1 B T P + Q.

(3.16)

Macierz wzmocnień K opisana jest równaniem
K = R−1 B T P

(3.17)

i określa w całości postać sprzężenia zwrotnego dla opisywanego systemu.
W pracy do wyznaczenia macierzy K użyto środowiska Matlab i dostępnej w nim
funkcji lqr.
[K, P, ev] = lqr(A,B,Q,R)
gdzie
K
P
ev
Przykład wyliczenia

−macierz wzmocnień sterownika
−macierz Lapunowa,
−wektor wartości własnych układu zamkniętego.
nastaw sterownika jest następujący. Za Q i R przyjęto macierze


aQ 0 0 0 0 0
 0 bQ 0 0 0 0 


 0 0 cQ 0 0 0 


(3.18)
Q=
0 0 0 dQ 0 0 


 0 0 0 0 eQ 0 
0 0 0 0 0 fQ

3. Sterowanie liniowe

24

gdzie aQ = bQ = cQ = dQ = eQ = fQ = 1, zaś
R=

aR 0
0 bR

(3.19)

gdzie aR = bR = 1
Otrzymana macierz wzmocnień miała postać
K = 1.0e3 ∗

−0.0000 −0.0006 −0.0010 −0.0030 −1.1322 −0.2003
−0.0010 −0.0030 −0.0000 −0.0006 −1.1322 −0.2003

(3.20)

Wartości własne układu z zamkniętą pętlą sprzężenia (3.7) są następujące
λ1
λ2
λ3
λ4
λ5
λ6

= −5.9015,
= −5.7678,
= −0.4432 + 0.3710i,
= −0.4432 − 0.3710i,
= −0.3427 + 0.3054i,
= −0.3427 − 0.3054i.

(3.21)

Wyniki symulacji przedstawiono na rysunku 3.2. Aby zaobserwować jak szybko sterownik
doprowadza układ do stanu równowagi, przyjęto początkową wartość wychylenia równą
α(0) = 0.1

(3.22)

Zaobserwowano, że wraz ze wzrostem współczynników macierzy Q malał czas powrotu do
stanu równowagi. Wzrastało także przeregulowanie.

Rysunek 3.2 Wychylenie wahadła, przy użyciu sterownika LQR dla parametrów Q = I6
i R = I2

3. Sterowanie liniowe

25

Rysunek 3.3 Wychylenie wahadła,przy użyciu sterownika LQR dla parametrów aQ =
cQ = eQ = 10, bQ = dQ = fQ = 1 oraz R = 0.1 · I2

Rysunek 3.4 Wychylenie wahadła, przy użyciu sterownika LQR dla parametrów aQ =
cQ = eQ = 100, bQ = dQ = fQ = 1 oraz R = 0.1 · I2

3. Sterowanie liniowe

26

Rysunek 3.5 Wychylenie wahadła, przy użyciu sterownika LQR dla parametrów aQ =
cQ = eQ = 1000, bQ = dQ = fQ = 1 oraz R = 0.1 · I2

Rysunek 3.6 Wychylenie wahadła, przy użyciu sterownika LQR dla parametrów aQ =
cQ = eQ = 3000, bQ = dQ = fQ = 1 oraz R = 0, 1 · I2

3. Sterowanie liniowe

3.2.2

27

Sterownik rozmieszczający bieguny (PP).

Metoda sterowania polega na wyznaczeniu takiej macierzy K danej równaniem (3.7), aby
˜
wynikowa macierz A, równa
˜
A = A − BK
˜
miała wartości własne spełniające warunek ∀i eigi (A) & lt; 0. Ich optymalne położenia wyznacza się metodą prób i błędów. Oceny dokonuje się poprzez obserwację sygnału wyjściowego, który powinien mieć najmniejsze odkształcenie od sygnału wejściowego. Jeżeli
celem jest szybka odpowiedź, to bieguny powinny leżeć daleko od osi urojonej charakterystyki fazowo-częstotliwościowej. Trzeba pamiętać, że szybka odpowiedź na zadane
wymuszenie będzie wymagać silnych napędów robota.
Macierz K wyznaczono w środowisku Matlab przy użyciu funkcji place.
K = place(A,B,p)
gdzie
p wektor biegunów otrzymanych przy sprzężeniu zwrotnym (p=eig(A-BK))
Przykład wyliczenia nastaw sterownika jest następujący. Za p przyjęto
p = [a b c d e f ].

(3.23)

gdzie a = −1, b = −2, c = −3, d = −4, e = −5, f = −6.
Stabilność systemu będzie zagwarantowana tak długo, jak długo część rzeczywista
˜
eig A będzie mniejsza od zera. Macierz wzmocnień K dla powyższych parametrów ma
postać:
K = 1.0e3

0.0168
0.0106 −0.0120 −0.0093 −0.7699 −0.1272
−0.0299 −0.0298 0.0035 −0.0081 −2.3509 −0.4674

(3.24)

Wyniki symulacji przedstawiono poniżej. Aby zaobserwować jak szybko sterownik doprowadza układ do stanu równowagi, przyjęto wychylenie początkowe
α(0) = 0.1

(3.25)

Zaobserwowano, że wraz z oddalaniem biegunów od osi urojonej malał czas powrotu do
stanu równowagi. Wzrastało także przeregulowanie.

3. Sterowanie liniowe

28

˜
Rysunek 3.7 Wychylenie wahadła przy użyciu sterownika PP dla biegunów A równych
a = −1, b = −2, c = −3, d = −4, e = −5, f = −6

˜
Rysunek 3.8 Wychylenie wahadła przy użyciu sterownika PP dla biegunów A równych
a = −3, b = −6, c = −9, d = −12, e = −15, f = −18

3. Sterowanie liniowe

29

˜
Rysunek 3.9 Wychylenie wahadła przy użyciu sterownika PP dla biegunów A równych
a = −5, b = −10, c = −15, d = −20, e = −25, f = −30

3. Sterowanie liniowe

3.2.3

30

Zastosowanie liniowego sterownika do nieliniowego modelu
robota

Po wyznaczeniu optymalnych sterowników dla systemów liniowych podjęto próbę przetestowania ich na nieliniowym modelu dynamiki robota. Założono, że przy niewielkich
odchyleniach wahadła od pionu sterownik powinien pracować prawidłowo.
Na wykresach przedstawiających odchylenie wahadła od pionu zaobserwowano, że strojenie sterownika w kierunku zwiększenia szybkości działania powodowało wzrost przeregulowań. Wzrasta także zapotrzebowanie na moment napędowy aktuatorów. Sterownik
rozmieszczający bieguny działał zdecydowanie szybciej. Po czasie 1s robot z niezerowego
stanu początkowego odzyskiwał równowagę.

Rysunek 3.10 Schemat blokowy sterownika dla nieliniowego systemu
Zgodnie z rysunkiem 3.10 przetestowano sterowniki liniowe na nieliniowym modelu
robota balansującego. Wybrano po jednej, najszybciej działającej wersji sterownika zaprojektowanej metodą PP oraz LQR. Aby zaobserwować czy sterownik poprawnie doprowadza układ do stanu równowagi, przyjęto wychylenie początkowe równe
α(0) = 0.1

(3.26)

Jak widać z rysunków 3.11, 3.12 oraz 3.13 sterowniki działają poprawnie dla niewielkich odchyleń korpusu robota od pionu. Przy większych odchyleniach np. α(0) = π , robot
3
nie jest w stanie osiągnąć stanu równowagi co pokazano na rysunku 3.13.

3. Sterowanie liniowe

31

Rysunek 3.11 Wychylenie wahadła nieliniowego systemu przy użyciu sterownika LQR dla
parametrów aQ = cQ = eQ = 3000, bQ = dQ = fQ = 1, aR = bR = 0, 1, α(0) = 0.1

Rysunek 3.12 Wychylenie wahadła nieliniowego systemu przy użyciu sterownika PP dla
˜
biegunów A równych a = −5, b = −10, c = −15, d = −20, e = −25, f = −30

3. Sterowanie liniowe

32

Rysunek 3.13 Wychylenie wahadła nieliniowego systemu przy użyciu sterownika LQR dla
parametrów aQ = cQ = eQ = 3000, bQ = dQ = fQ = 1, aR = bR = 0, 1, α(0) = π
3

Rozdział 4
Nieliniowy sterownik
4.1

Projektowanie sterownika nieliniowego

W poprzednim rozdziale zaproponowano sterowniki liniowe, które działają poprawnie jedynie w okolicy punktu równowagi. Założono, że w punkcie tym wszystkie prędkości i
przyspieszenia mają wartość zerową. Nie mniej jednak model dynamiki robota balansującego jest jak najbardziej nieliniowy. W rozdziale tym zaprezentowano nieliniowy system sterowania, który pozwoli osiągać punkt równowagi nawet wtedy, gdy korpus robota
znacznie się odchyli od pionu przy niezerowych wartościach prędkości i przyspieszeń. Zaprojektowany sterownik umożliwi także śledzenie trajektorii przez odwrócone mobilnego
wahadło.
Ponownie rozważmy model dynamiki robota, tym razem jednak przyjęto, że
jest to manipulator mobilny z jednym pasywnym przegubem (stopień swobody
pozbawiony napędu) (2.13). Oznacza to, że teraz traktujemy system tak, że
napędy oddziaływają jedynie na koła.
Wprowadzając macierz B pochodzącą z równań (2.13), otrzymamy w ten sposób nieco
inny model, częściowo niedosterowany
Q∗ Q∗
11
12
Q∗ Q∗
21
22

4.1.1

η
˙
α
¨

+

∗
F1
∗
F2

=

B∗
0

τ
0

.

(4.1)

Częściowa linearyzacja globalna

W rozważanym modelu rolę pasywnego przegubu spełnia odwrócone wahadło. Dla tego
typu obiektów możliwe jest zastosowanie tzw. częściowej linearyzacji globalnej [10], transformującej model, z punktu widzenia sterowania, do wygodniejszej postaci.
Przekształcając drugie równanie (4.1) w celu wydzielenia wyrażenia na α
¨
∗
α = −(Q∗ )−1 (Q∗ η + F2 )
¨
22
21 ˙

(4.2)

i wstawiając do pierwszego równania [11] uzyskamy zapis
¯ ˙ ¯
Q(q)η + F (q, q) = B ∗ τ,
˙
α = −(Q∗ ),
¨
22
gdzie
¯
Q(q) = Q∗ (q) − Q∗ (q)Q∗ (q)−1 Q∗ (q),
11
12
22
21
¯ (q, q) = F ∗ (q, q) − Q∗ (q)Q∗ (q)−1 F ∗ (q, q).
F
˙
˙
˙
1
12
22
2

(4.3)
(4.4)

4. Nieliniowy sterownik

34

Podstawiając do części sterowanej układu prawo sterowania
¯
¯
τ = (B ∗ )−1 Q(q)u + F (q, q) ,
˙

(4.5)

otrzymujemy model (4.1) częściowo zlinearyzowany system o postaci
∗
α = −(Q∗ )−1 (Q∗ η + F2 ),
¨
22
21 ˙
η = u.
˙

(4.6)

Z punktu widzenia sterowania, dynamika została zlinearyzowana, a przekształcone
równanie dla wahadła jest rodzajem ograniczeń. System ten jest punktem wyjścia do
projektowania nieliniowego algorytmu sterowania pozwalającego nie tylko na utrzymywanie położenia równowagi, ale także na śledzenie zadanej trajektorii.

4.1.2

Nieliniowy sterownik

W celu zaprojektowania sterownika należy rozważyć model jako dwa podsystemy, którymi
będziemy sterować na poziomie kinematyki i dynamiki. Na poziomie kinematyki uzyskujemy prędkości referencyjne ηr , które zapewnią śledzenie zadanej trajektorii αd (t) dla
korpusu (odwróconego wahadła). Na poziomie dynamicznym uzyskujemy częściowo zlinearyzowane sterowanie u, które reguluje prędkością η w taki sposób, aby śledzić prędkości
referencyjne ηr uzyskanych w sterowniku kinematycznym.
Sterownik kinematyczny
Niech równanie sterownika ma następująca postać
∗
−(Q∗ )−1 (Q∗ ηr + F2 ) = αd − Kd (α − αd ) − Kp (α − αd ),
¨
˙
˙
22
21 ˙

Kd , Kp & gt; 0.

(4.7)

Niestety, powyższe równanie jest skalarne, natomiast ηr jest dwuwymiarowe. Jednakże
można przyjąć założenie, że robot utrzymując równowag porusza się tylko po linii prostej.
Zatem relacja pomiędzy prędkościami jest następująca η1r = η2r . Przy takim założeniu
z równania (4.7) można wyznaczyć przyspieszenia referencyjne ηr . W tym celu należy
˙
najpierw wyliczyć η1r = η2r z równania analitycznego
˙
˙
η1r =
˙

Q∗ Kp (α − αd ) + Q∗ Kd (α − αd ) − Q∗ α − F2
˙
˙
22
22
22 ¨
∗
2Q21

(4.8)

Sterownik dynamiczny
W praktyce rzeczywiste prędkości kół różnią się od prędkości referencyjnych uzyskanych
w sterowniku kinematycznym.Zastosujmy nowy algorytm sterowania, który zagwarantuje
asymptotyczne śledzenie trajektorii we wszystkich współrzędnych mobilnego odwróconego
wahadła. Prawo sterowania ma postać
u = ηr − Km eη ,
˙

Km & gt; 0,

(4.9)

gdzie Km jest pewną macierzą regulacji, zaś
eη = η − ηr =

η1 − η1r
η2 − η2r

jest błędem występującym na poziomie dynamicznym, kiedy prędkości kół nie są równe
prędkościom referencyjnym, np. w pierwszej fazie ruchu, na ogół zachodzi η(0) = ηr (0).

4. Nieliniowy sterownik

4.2

35

Symulacje

Zaprojektowany nieliniowy sterownik dynamiczny w przeciwieństwie do liniowej wersji,
powinien działać poprawnie także przy dużych wartościach kąta α. Liniowe sterowniki
doprowadzały wahadło do równowagi ze stanu nie większego niż π . Na wartość graniczną
4
α miały także wpływ nastawy regulatorów.
Na rysunku 4.1 przedstawiono wyniki symulacji systemu dla nastaw i warunków początkowych równych
Kp
Kd
Km
αd

= 40,
= 10,
= 10,
= 0,
pi
α(0) =
3,
η1r (0) = 0,
η2r (0) = 0.

(4.10)

Rysunek 4.1 Wychylenie wahadła (system nieliniowy) przy użyciu nieliniowego sterownika
Na rysunkach 4.2 oraz 4.3 wyniki symulacji, w których śledzona jest niezerowa wartość
wychylenia wahadła αd = π . Symulacje przeprowadzono dla nastaw i warunków początko3

4. Nieliniowy sterownik

36

wych równych
Kp = 40,
Kd = 10,
Km = 10,
π
αd = ,
3
α(0) = 0,
η1r (0) = 1,
η2r (0) = 1.

Rysunek 4.2 Błąd śledzenia zadanej stałej wartości kąta αd

(4.11)

4. Nieliniowy sterownik

Rysunek 4.3 Błąd śledzenia prędkości referencyjnych eη1

37

4. Nieliniowy sterownik

38

Zaprojektowany sterownik pozwala także na śledzenie kąta αd zmiennego w czasie
(śledzenie trajektorii). Na rysunkach 4.4 i 4.5 zaprezentowano wyniki symulacji opisywanego przykładu dla nastaw i warunków początkowych równych
Kp = 40,
Kd = 10,
Km = 10,
αd = 0, 05 sin(

t
),
10

(4.12)

α(0) = 0,
η1r (0) = 1,
η2r (0) = 1.

Rysunek 4.4 Błąd śledzenia zadanej stałej wartości kąta αd
Przeprowadzając powyższe symulacje pokazano, że zastosowanie nieliniowego sterownika jest znacznie szersze. Pozwala on nie tylko na utrzymywanie równowagi robota
balansującego, ale także można go stosować do utrzymywania wychylenia (αd = 0)
lub śledzenia trajektorii (αd (t) korpusu. Ponadto zaprezentowany w pracy nieliniowy
sterownik bez problemu radził sobie z osiągnięciem równowagi nawet, gdy wychylenie
początkowe było równe α(0) = π .
3

4. Nieliniowy sterownik

Rysunek 4.5 Błąd śledzenia prędkości referencyjnych eη1

39

Rozdział 5
Praktyczna realizacja układu
sterowania
W pracy podjęto próbę budowy zamodelowanego robota balansującego. Zmierzono się
z wieloma trudnościami w realizacji poszczególnych elementów składowych robota, jak:
opracowanie i wykonanie konstrukcji nośnej, odpowiedni dobór napędów, użycie dostatecznie wydajnego sterownika silników prądu stałego, zapoznanie się i uruchomienie wydajnej jednostki centralnej MPC555 ﬁrmy Freescale, a także opracowanie odpowiedniego
zestawu sensorów, niezbędnych do poprawnej pracy urządzenia. Dzięki temu eksperymentowi, udało się zweryﬁkować poprawność działania, rozważanego dotychczas jedynie
teoretycznego modelu robota. Doświadczenie to dowodzi jak ważna, w procesie urzeczywistnienia teoretycznego modelu jest poprawna identyﬁkacja ﬁzycznego obiektu. Trudności z pomiarem rzeczywistych wartości wektora stanu, a także niska jakość użytych do
budowy robota napędów, pozwoliły jedynie na implementację prostych regulatorów PID.

Rysunek 5.1 Robot „Kosmos”
Budowa robota ma charakter modułowy. Główny sterownik został tak zaprojektowany,

5. Praktyczna realizacja układu sterowania

41

aby umożliwić łatwą rozbudowę robota o dodatkowe urządzenia peryferyjne. W pracy
zdecydowano o wyposażeniu urządzenia w następujące moduły:
• główna jednostka sterująca CPU MPC555 ﬁrmy Freescale wraz z dotykowym, graﬁcznym wyświetlaczem LCD,
• układ konwertera napięć z przetwornikiem cyfrowo-analogowym,
• podwójny mostek typu H z regulacją prądu, wyposażony w przetwornice napięcia
+/- 5v,
• moduł zasilacza +5V oraz +6V,
• moduł INS,
• moduł oświetlenia RGB oraz kamery,
• klawiaturę 24-klawisze,
• pakiet zasilający, składający się z czterech ogniw litowo-polimerowych o pojemności
4800mAh,
• dodatkowo istnieje możliwość podłączenia sześciu serwomechanizmów.

Rysunek 5.2 Rozmieszczenie poszczególnych modułów na robocie (widok z tyłu i z przodu)
Schemat blokowy robota „Kosmos” przedstawiono na rysunku 5.3. Warto zwrócić
uwagę na to, że do modułu głównego sterownika, doprowadzono aż trzy źródła zasilania.
Zabieg ten pozwolił uniknąć propagacji zakłóceń pomiędzy poszczególnymi modułami
zainstalowanymi w robocie.
Z uwagi na to, że tematem pracy było głównie zamodelowanie i przeprowadzenie
symulacji zdecydowano szczegółowo opisać tylko niektóre moduły robota skonstruowanego
na potrzeby eksperymentów. W tekście zamieszczono fragmenty kodu oprogramowania
głównego sterownika, które zdaniem autora są najciekawsze. Część praktyczna jest wciąż
rozwijana i udoskonalana.

5. Praktyczna realizacja układu sterowania

Rysunek 5.3 Schemat blokowy robota „Kosmos”

42

5. Praktyczna realizacja układu sterowania

5.1
5.1.1

43

Konstrukcja mechaniczna
Budowa korpusu robota

Do budowy robota użyto w głównej mierze kątowników aluminiowych w kształcie „L”.
Całość poskręcano śrubami o średnicy 3mm. Poszczególne moduły zamocowano na stalowych tulejach dystansowych. W celu chronienia konstrukcji, na stelaż nawinięto odbojniki wykonane z mikrogumy. W dolnej części korpusu zamocowano wszystkie moduły z
elektroniką, tj. główny sterownik, moduł przetwornika wraz z konwerterem napięć, moduł
INS, moduł zasilacza, podwójny mostek typu H. Najniżej zamocowano napędy wraz z
kołami a w połowie korpusu umiejscowiono akumulator. Konstrukcja okazała się bardzo
stabilna i wytrzymała na wielokrotne upadki. Wymiary korpusu prezentują rysunki 5.4 i
5.5.

Rysunek 5.4 Rysunek poglądowy (widok z przodu).
Dane ﬁzyczne skonstruowanego modelu nieco się różnią od parametrów przyjętych

5. Praktyczna realizacja układu sterowania

44

w trakcie symulacji w poprzednich rozdziałach. Wyniki symulacji są bardzo podobne,
a ich formatowanie jest dość pracochłonne, autor zdecydował o pozostawieniu wyników
symulacji, przeprowadzonych na podstawie pierwotnie przyjętych parametrów ﬁzycznych.

Rysunek 5.5 Rysunek poglądowy (widok z boku i z tyłu).
Zmierzone nowe parametry ﬁzyczne platformy (wahadła) oraz kół, które należy uwzględnić w modelu matematycznym:
oznaczenie wartość jednostka opis
Mk
Mw
R
l
w
b

0,176
3.9
0,06
0,68
0,035
0,15

kg
kg
m
m
m
m

masa koła
masa wahadła
promień koła
długość wahadła
szerokość koła
odległość koła od środka platformy

5. Praktyczna realizacja układu sterowania

5.1.2

45

Napędy robota

Robot balansujący został wyposażony w dwa silniki prądu stałego niemieckiej ﬁrmy
Graupner. Użyte modele to Graupner 500E (rysunek 5.6). Jak większość silników modelarskich, charakteryzują się dużą sprawnością. Egzemplarz oznaczony literą „E” należy do
silników typu ekonomicznego, czyli takiego, który dostarcza sporych momentów sił przy
niewielkim zużyciu energii elektrycznej. Silniki te pracują na znamionowym napięciu 12V.
Parametry silnika zamieszczono w tabeli poniżej.

Rysunek 5.6 Silnik 500E ﬁrmy Graupner
opis
napięcie znamionowe
prędkość
prąd bez obciążenia
prąd przy max sprawności
prąd w zwarciu
sprawność
grubość wału
masa
wymiary (długość x średnica)

wartość
6...12
12000
0,4
2
10
67
3,17
158
50x35,8

jednostka
V
obr/min
A
A
A
%
mm
g
mm

Tabela. 5.1 Parametry silnika
Napęd na koło został przeniesiony poprzez podwójną przekładnię planetarną. Przekładnie użyte w robocie, wymontowano z tanich wkrętarek akumulatorowych. Jak się
okazało, ich niska jakość znacząco wpłynęła na jakość sterowania platformą i w dużym
stopniu uniemożliwiła implementację większości rozważanych teoretycznie sterowników.
Zmierzone nowe parametry ﬁzyczne pojedynczego napędu wraz z przekładnią, który
należy uwzględnić w modelu matematycznym:
oznaczenie wartość jednostka opis
km
ke
Rs
IR
n

0,725
0,33
3
0,0031
1:36

N· m/A
V· s/rad
Ω
kg· m2
−

współczynnik wytworzonego momentu
współczynnik wytworzonego napięcia
rezystancja cewek silnika
moment bezwładności napędu
przełożenie

5. Praktyczna realizacja układu sterowania

46

Rysunek 5.7 Pojedynczy napęd.

5.2
5.2.1

Konstrukcja elektroniczna
Czujniki pomiaru prędkości obrotowej kół

Na tylnej ściance każdego silnika zainstalowano magnetyczny enkoder AS5040 ﬁrmy Austriamicrosystems (rysunek 5.9 i 5.8) [14] [28]. Czujnik ten doskonale nadaje się do zastosowań w robotyce, do pomiarów prędkości obrotowej oraz położenia osi napędowych.
Jest jednym z najmniejszych magnetycznych czujników obrotu na świecie. Wewnątrz
struktury układu znajduje się procesor, połączony z matryca czujników pola magnetycznego (czujników Hall’a). Dzięki specjalnemu algorytmowi, procesor potraﬁ określić
położenie linii sił pola magnetycznego (biegnących pomiędzy biegunami magnesu) względem owej struktury. Zapewniają bardzo precyzyjny bezkontaktowy pomiar obrotów,
gdyż ich rozdzielczość wynosi 1024 impulsy na obrót. W czujniku wykorzystano wyjścia kwadraturowe i podłączono je do głównego sterownika. .

5.2.2

Prosty moduł INS

Do budowy modułu INS (Inertial Navigate System) użyto dwóch sensorów: trzyosiowy
akcelerometr, jednoosiowy żyroskop, a także szybki, 10-cio bitowy przetwornik A/C. Pierwszy z czujników produkuje ﬁrma Freescale i jest to model MMA7260 [24]. Drugi czujnik
produkowany jest przez ﬁrmę Analog Devices i jest to model ADXRS150 [25]. Do zamiany
sygnałów analogowych na cyfrowe wykorzystano przetwornik A/C TLV2556 ﬁrmy Texas
Instruments [26]. Poniżej przedstawiono główne parametry czujników oraz przetwornika.
opis
czułość
wybór czułości
napięcie znamionowe
szumy
typ obudowy

wartość
800
1,5/2/4/6
3,3
4,7
QFN

jednostka
mV
g
V
mV rms
-

Tabela. 5.2 Parametry akcelerometru MMA7260
Żyroskop, akcelerometr oraz przetwornik umieszczono na jednej płytce drukowanej.
Niestety jeden z czujników wymaga zasilania o napięciu 3V3. Konieczne zatem było
użycie dodatkowego stabilizatora, obniżającego napięcie zasilania modułu. Moduł ten

5. Praktyczna realizacja układu sterowania

47

Rysunek 5.8 Mocowanie enkodera
opis
czułość
max częstotliwość
napięcie znamionowe
szumy
typ obudowy

wartość
12,5
40
5
0,05
BGA

jednostka
mV/o /s
Hz
V
± /svHz
-

Tabela. 5.3 Parametry żyroskopu ADXRS150
przedstawiono na rys. 5.10. Całość przymocowano do korpusu przy pomocy wkrętów i
sprężyn, tak aby możliwa była regulacja kąta, pod którym moduł został zamocowany.

5. Praktyczna realizacja układu sterowania

48

Rysunek 5.9 Schemat ideowy czujnika obrotów
opis
max częstotliwość
rozdzielczość
napięcie znamionowe
liczba kanałów
interfejs
max częstotliwość interfejsu
napięcie referencyjne

wartość
200
12
5
12
SPI
12
wbudowane 4,098

jednostka
KSPS
bitów
V
MHz
V

Tabela. 5.4 Parametry przetwornika A/C TLV2556

Rysunek 5.10 Widok modułu nawigacyjnego INS

5. Praktyczna realizacja układu sterowania

Rysunek 5.11 Schemat modułu nawigacyjnego INS

49

5. Praktyczna realizacja układu sterowania

5.2.3

50

Sterownik napędów

Do sterowania napędami zdecydowano o użyciu podwójnego gotowego mostka H [12], [9].
W pracy przedstawiono jedynie schemat blokowy mostka.

Rysunek 5.12 Widok skonstruowanego podwójnego mostka H
Największą zaletą opisywanego mostka jest możliwość pracy z regulacją średniej wartości
napięcia na zaciskach silnika lub praca z regulacją prądu przepływającego przez cewki
silnika. Ten drugi pozwala na kontrolę momentu siły wytwarzanego przez oś napędu. Dodatkowo w strukturze mostka znajduje się zasilacz napięcia symetrycznego ± 5V. Napięcie dodatnie wytwarzane jest przez zintegrowaną z mostkiem przetwornicę. Parametry
mostka przedstawiono w tabeli poniżej.
opis
max napięcie zasilania
min napięcie zasilania
max prąd wyjściowy na kanał
wyj. pomiaru prądu
rodzaj wejścia

wartość
40
12
5
tak
analogowy lub/i PWM

jednostka
V
V
A
-

Tabela. 5.5 Parametry podwójnego mostka typu H
W mostku można wyróżnić następujące istotne sygnały:
• wejściowe:
– napięcie zasilania 12-40V,
– sygnały PWM (w pracy wykorzystano jako załączenie mostka),
– sygnał sterujący analogowy, bipolarny,

5. Praktyczna realizacja układu sterowania

51

– masa,
• wyjściowe:
– dwa wyjścia do zasilania silników,
– dwa analogowe wyjścia do pomiaru prądu płynącego przez silniki,
– napięcie +5V,
– napięcie -5V,
– masa.

Rysunek 5.13 Schemat blokowy podwójnego mostka H
Więcej o mostku można przeczytać w pracach [12], [9]. Niestety mostek posiada
kilka wad. Jedną z nich jest dość spora złożoność konstrukcji. Ponadto do poprawnego
sterowania silnikiem w obie strony wymagane jest bipolarne napięcie sterujące. Ten drugi
fakt wpłyną na skonstruowanie dodatkowego modułu, wyposażonego w przetwornik C/A
oraz konwerter napięć z unipolarnego na bipolarny. Zastosowany przetwornik C/A to
model MAX534 ﬁrmy MAXIM [27]. Istotne parametry przetwornika zestawiono w tabeli
poniżej.
opis
rozdzielczość
czas przetwarzania
napięcie znamionowe
liczba kanałów
interfejs
max częstotliwość interfejsu
wyjścia

wartość
8
8
5
4
SPI
10
Rail-to-rail

jednostka
bitów
µs
V
MHz
-

Tabela. 5.6 Parametry przetwornika C/A MAX534

5. Praktyczna realizacja układu sterowania

52

Opisany powyżej mostek wyposażono w wyjścia napięcia symetrycznego co wykorzystano w module konwertera. Widok konwertera przedstawiono na rysunku 5.14.

Rysunek 5.14 Widok modułu z przetwornikiem i konwerterem napięć
Z uwagi na to, że konwerter jest zasilany napięciami z mostka, w którym napięcie +5V
jest generowane przez zintegrowaną przetwornicę, zdecydowano o wyprowadzeniu tego
źródła na płytce oraz podłączeniu go do głównego sterownika. Schemat modułu konwertera pokazano na rysunku 5.15. Zaznajamiając się z wyprowadzeniami mostka opisanymi
w jego dokumentacji można zaobserwować, że złącze konwertera jest w pełni zgodne
ze złączem mostka. Na wyjściu przetwornika C/A uzyskuje się napięcia 0...2,5V, gdyż
wyjścia te sa typu Rail-to-rail, a napięcie referencyjne wynosi 2,5V. Zasilany symetrycznie
układ TL082 to podwójny wzmacniacz operacyjny, na którym zrealizowano przesunięcie
napięcia, czyli zamianę z unipolarnego na bipolarne ±1,25V. Konwerter, oprócz złącza
głównego posiada także trzy mniejsze złącza 5-cio pinowe, którymi podłącza się moduł
do głównego sterownika. Jedno z nich to interfejs SPI, drugie służy do doprowadzenia
napięć do pomiaru prądu, płynącego przez silniki oraz linii mostka załączających stopnie mocy. Trzecie złącze to pozostałe dwa wyjścia analogowe, które wyprowadzono z
czterokanałowego przetwornika.

5.2.4

Zasilanie

Głównym źródłem zasilania całego robota jest wysokiej jakości pakiet akumulatorów
litowo-polimerowy o pojemności 4800mAh i napięciu 14,8V ﬁrmy KOKAM. Głównymi
jego zaletami są możliwy duży prąd rozładowywania oraz relatywnie mały ciężar ok.
0,5kg. Warto zwrócić uwagę na to, że napięcie akumulatora przy maksymalnym naładowaniu wynosi 16,8V. Pozwala to na nieprzerwaną, prace robota do 3-4h. Robot został
wyposażony w tzw. balancer, który w trakcie ładowania czuwa nad tym, aby każde ogniwo
było równo naładowane. Akumulatory tego typu są bardzo drogie, a ich nieodpowiednie
ładowanie może doprowadzić do trwałego uszkodzenia jednego z ogniw.
Jak już wspomniano na wstępie, główny sterownik wymaga, aż trzech źródeł zasilania.
Jedno z nich (+5V) dostarczone jest z przetwornicy zintegrowanej w sterowniku napędami,

5. Praktyczna realizacja układu sterowania

53

Rysunek 5.15 Schemat modułu z przetwornikiem i konwerterem napięć
dwa pozostałe (+5V oraz +6V) dostarczone są z zasilacza. Wszystkie urządzenia peryferyjne podłącza się jedynie do głównego sterownika. Zasilanie dostarczane jest razem z
przewodami sygnałowymi. Dzięki temu zminimalizowano liczbę przewodów w układzie.
Napięcie (+5V), które dostarczone jest z mostka służy głównie do zasilania klawiatury,
modułu INS, pery-ferii podłączanych do magistral I2 C oraz SPI. Pierwsze napięcie (+5V)
dostarczone z zasilacza zasila układy, które wymagają większej wydajności prądowej,
np. serwomechanizmy, moduły oświetlenia, czujniki odległości. Drugie zasilanie dostarczone z modułu zasilacza (+6V) służy jedynie do zasilenia płytki mikrokontrolera wraz z
dotykowym wyświetlaczem.
Schemat zasilacza przedstawiono na rysunku 5.18. Układ L1084-5V to liniowy stabilizator dużej mocy typu low drop. Maksymalny prąd obciążenia tego układu wynosi
5A. Układ LM7806 to zwykły liniowy stabilizator 6V o wydajności prądowej 1A. Za
bezpiecznikami na wyjściu każdego układu zastosowano diodę zenera, której zadaniem
jest szybkie spalenie bezpiecznika, na wypadek uszkodzenia któregoś ze stabilizatorów.
Dla zwiększenia bezpieczeństwa głównego sterownika wyposażono go w dodatkowe stabilizatory. W zasilaczu wyprowadzono wyjście napięcia 0...4V, które dzięki dwóm diodom
zenera jest zredukowaną wartością napięcia akumulatora. Dostarcza ono informacji do
głównego sterownika o stanie naładowania akumulatora. Napięcie akumulatora nigdy nie

5. Praktyczna realizacja układu sterowania

54

Rysunek 5.16 Akumulator Li-Poly 4800mAh 14,8V

Rysunek 5.17 Widok modułu zasilacza
powinno spaść poniżej 10V. Dla bezpieczeństwa ogniw w oprogramowaniu robota zaimplementowano odłączanie stopni mocy, jeżeli napięcie w układzie spadnie poniżej 12,5V.

5.2.5

Główny sterownik

Główny sterownik składa się z trzech piętrowo na sobie zainstalowanych płytek drukowanych. Pierwsza z nich zawiera graﬁczny wyświetlacz LCD na którym zainstalowano
także panel dotykowy. Druga płytka zwiera głównie jednostkę centralną, a trzecia to
komplet złącz sterownika.
Płytka wyświetlacza to gotowy moduł, który posiada zintegrowaną przetwornicę ujemnego napięcia, które jest niezbędne do poprawnej pracy wyświetlacza. Istotną cechą całego
modułu jest to, że zarówno kontroler jak i układ przetwornicy pracują na napięciu 3,3V,
czyli takim jak jednostka centralna. Kontroler sterujący wyświetlaczem to popularny
układ T6963 ﬁrmy Toshiba [15]. Rozdzielczość wyświetlacza wynosi 240x128 pikseli.
Sposób podłączenia wyświetlacza do głównego sterownika pokazano w tabeli 5.2.5.

5. Praktyczna realizacja układu sterowania

55

Rysunek 5.18 Schemat modułu zasilacza
Głównym elementem środkowej płytki [17] jest moduł ﬁrmy PHYTEC phyCOREMPC555 [18]. Jest to wielowarstwowa płytka drukowana, która zawiera mikrokontroler
PowerPC MPC555 ﬁrmy Freescale [20], układy pamięci ROM 1MB, pamięci RAM 1MB,
pamięć FLASH, układ zegara czasu rzeczywistego, sterowniki magistrali RS232 oraz CAN.
Poniżej przedstawiono główne cechy układu PowerPC MPC555:
• 32-bitowa architektura RISC PowerPC,
• zegar 40MHz,
• 64-bitowa jednostka zmiennoprzecinkowa,
• 26 kB SRAM,
• 448 kB FLASH,
• podwójny interfejs UART,
• interfejs QSPI z (kolejka 32 słowa),
• podwójny interfejs CAN 2.0B,
• dwa układy TPU3 po 16 kanałów,
• układ MIOS1 dwa 16 bitowe systemy liczników ,8 kanałów, 16-bitowych układów
PWM,
• dwa 10-bitowe QADC (7µs) po 16 wejść,
• uniwersalne, wielofunkcyjne sygnały I/O,

5. Praktyczna realizacja układu sterowania

56

Rysunek 5.19 Wyświetlacz graﬁczny LCD
• port testowo-uruchomieniowy JTAG/BDM.
Interfejs wyświetlacza jest typu i80 czyli taki, który posiada osobno linie wyboru zapis i
linię wyboru odczyt. Interfejs mikrokontrolera ma typ M68 z pojedynczą linią wyboru odczytu RDNWR, dlatego na płytce zastosowano układ 74HC14, w którym wykorzystano
pojedynczy negator. Wyświetlacz zgodnie z tabela, 5.2.5 podłączono tak, aby wykorzystać sprzętowy blok wyboru (dekoder adresów). Dodatkowo na płytce umieszczono
cztery diody podłączone poprzez klucze do linii I/O MPIO[11:14]. W panelu dotykowym
wykorzy-stano jedynie oś X, gdyż oprogramowanie wyświetlania graﬁki napisano tak,
że wyświetla „klawisze” (pola) jedynie w poziomie. Moduł z mikrokontrolerem wymaga
dwóch napięć zasilania 5V i 3,3V. Na płytce znalazł się także zasilacz, który do poprawnej
pracy sterownika musi być zasilony napięciem min +6V. Dodatkowo na płytce umieszczono dwa złącza RS232, złącze BDM oraz złącza którymi podłącza się trzecią płytkę
sterownika.
Trzecia płytka sterownika zawiera wszystkie złącza dla peryferii robota. To do niej
podłącza się moduł INS, klawiaturę, moduł sterownika silnikami, moduł oświetlenia, serwomechanizmy oraz czujniki odległości. Z uwagi na to, że płytka ta odpowiada także za
doprowadzenie napięć zasilania dla poszczególnych peryferii, doprowadzono do niej dwa
napięcia +5V.

5. Praktyczna realizacja układu sterowania

płytka LCD
A
K
Vout
V0
PD
GND
VDD
VEE
/WR
/RD
C/E
C/D
/RST
DB0...DB9
FS

płytka z mikrokontrolerem
+5V
GND
do potencjometru
do potencometru
nie podłączono
GND
+3V3
nie podłączono
RDNWR
/RDNWR
CS2
A31
MPI015
D0...D7
jumper

57

opis
zasilanie podświetlenia
masa zasilania podświetlenia
wyj. ujemnego napięcia -15V
wej. napięcia ujemnego (np. z dzielnika)
pin kontrolny
masa kontrolera
zasilanie kontrolera
aktywny zapis
aktywny odczyt (negator)
wybór wyświetlacza
wybór rejestru (danych/sterujący)
reset wyświetlacz
linie danych
wybór czcionki

Tabela. 5.7 Podłączenie wyświetlacza LCD

Rysunek 5.20 Widok środkowej płytki sterownika głównego

5. Praktyczna realizacja układu sterowania

Rysunek 5.21 Widok dolnej płytki sterownika głównego

58

5. Praktyczna realizacja układu sterowania

Rysunek 5.22 Schemat środkowej płytki sterownika głównego

59

5. Praktyczna realizacja układu sterowania

Rysunek 5.23 Schemat dolnej płytki sterownika głównego

60

5. Praktyczna realizacja układu sterowania

5.3

61

Pomiar odchylenia kątowego

W części pracy poświęconej symulacjom zakładano, że pomiar odchylenia kątowego i
jego prędkości kątowej są informacjami dobrze mierzalnymi i pozbawionymi znaczących
błędów. W praktyce pozyskanie tych informacji należy do jednych z najtrudniejszych
zadań.
Zwykle klasyczne odwrócone wahadło zaczepione jest na ruchomej platformie, która
przemieszcza się równolegle do powierzchni, np. wózek na czterech kołach. Zakładając,
że porusza się ona jedynie po powierzchniach płaskich, wystarczy zastosować enkoder
o dostatecznie dużej rozdzielczości. Robot balansujący nie posiada wyróżnionej platformy, której orientacja względem podłoża byłaby stała, gdyż koła robota są zamocowane bezpośrednio do wahadła (korpusu). Dobrym rozwiązaniem, pozwalającym zmierzyć
pożądaną wielkość jest odpowiedni system sensoryczny. W części opisującej praktyczny
eksperyment, przedstawiono prosty miniaturowy moduł INS (Inertial Navigation System),
który wyposażono w dwa czujniki, trzyosiowy akcelerometr i żyroskop.
Fuzji pomiarów z wymienionych czujników dokonuje się w sposób niezwykle efe-ktywny
przy użyciu algorytmu Filtracji Kalmana [16]. Akcelerometry mierzą zarówno przyspieszenia
dynamiczne jak i statyczne, np grawitacja. Zdolność tą wykorzystuje się do wyznaczania
odchylenia od pionu. Tanie, dostępne na rynku modele charakteryzują się sporymi szumami na wyjściu, dlatego zaleca się stosowanie jeszcze jednego czujnika. Żyroskopy, jak
wiadomo, dostarczają informacji o prędkości obrotowej. Niestety ich wadą jest fakt, że
z czasem ulegają dryftowi, pomiary stają się więc błędne. Filtr Kalmana [5] doskonale
niweluje niepożądane zjawisko dryftu oraz dokonuje przy tym fuzji pomiarów, dostarczając dostatecznie dokładnej i nie zaszumionej informacji o odchyleniu oraz prędkości
obrotowej.

5.3.1

Wyznaczenie odchylenia od pionu na podstawie pomiaru
przyspieszeń

Czujniki przyspieszenia dostarczają informacji o kierunku działania siły grawitacji. Jak
wiadomo, robot znajduje się w ciągłym ruchu. Odczytane z osi poziomej i pionowej
przyspieszenia będą składowymi mierzonej siły grawitacji oraz przyspieszeń robota. Należy
ten fakt uwzględnić w trakcie obliczeń trygonometrycznych. Jedną z metod wyłuskania
kąta odchylenia ilustruje rysunek 5.24
Po odczytaniu czujników przyspieszeń, dokonuje się następujących obliczeń
a = +/ −

x2 + y 2 − 1

(5.1)

„+” jeżeli a & gt; 0, „-” jeżeli a & lt; 0. Następnie dokonuje się obliczeń
x−y·a
• dla a & gt; 0 α = arcsin( x2 +y2 )
x+y·a
• dla a & lt; 0 α = arcsin( x2 +y2 )

• dla a=0 α = arcsin(x)
Wszystkie wartości są znormalizowane do działania siły grawitacji (g=1). Jak widać z
powyższych równań, kłopotliwe jest oszacowanie kierunku przyspieszenia całego robota
a. Można tego dokonać znając znak wartości sterowania napędów.

5. Praktyczna realizacja układu sterowania

Rysunek 5.24
akcelerometru

5.3.2

62

Uzyskanie kąta odchylenia od pionu na podstawie pomiarów z

Filtr Kalmana

W 1960 r. R.E. Kalman opublikował słynną pracę, w której przedstawił metodę ﬁltracji dynamicznej, która stała się jedną z najpopularniejszych metod estymacji statycznie
optymalnej. Posługując się tym narzędziem, można wyznaczyć pomiarowo niedostępne
zmienne jedynie na podstawie bieżących wartości wielkości pomiarowo dostępnych oraz
znajomości modelu matematycznego łączącego ze sobą obydwie te grupy pomiarów.
Filtry Kalmana [7] [5] (z ang. Kalman Filter w skrócie KF) znalazły zastosowanie w
wielu dziedzinach nauki. Najczęściej są one stosowane w inżynierii, głównie w układach
sensorycznych: robotów, samolotów oraz promów kosmicznych. Stosuje się je także w
technikach komputerowych do przetwarzania obrazów oraz w ekonomii do prognozowania
wskaźników gospodarczych. Metoda ta jest chętnie wykorzystywana w robotyce, gdzie
układy percepcji otoczenia odgrywają istotna rolę. Są często jedynym źródłem informacji
o środowisku, w którym znajduje się urządzenie.
W ramach wprowadzenia warto wspomnieć o własnościach ﬁltru [7].
• KF jest optymalnym estymatorem, gdyż przy konkretnych założeniach może on
spełniać pewne kryterium np minimalizacja błędu średniokwadratowego.
• Kolejną cechą, która świadczy o optymalności ﬁltru to fakt, że korzysta on z wszystkich dostępnych pomiarów bez względu na to, z jaką dokładnością i precyzją zostały
one wykonane. Ostatecznie na ich podstawie dokonuje najlepszej estymacji stanu.
• FK jest algorytmem typu rekursywnego. Nie przechowuje on wszystkich danych
z przeszłości i nie dokonuje on w każdym korku ich przeliczenia. Informacje są
przetwarzane sukcesywnie, bazując na wartościach obliczonych w poprzednim kroku.
• Jest to algorytm przetwarzania danych. Znając wejście i wyjście systemu można
uzyskać niedostępne (niemierzalne) wartości na podstawie dostępnych (mierzalnych)

5. Praktyczna realizacja układu sterowania

63

danych, np z sensorów. W teorii systemów liniowych mówimy o obserwowalności
układu.
• Metodę nazywamy ﬁltrem, gdyż jest on optymalnym estymatorem stanu tzn, że
uzyskamy możliwie optymalna wartość, na podstawie wielu pomiarów pochodzących
z zaszumionego środowiska.
Filtr Kalmana jest dwufazowym rekursywnym algorytmem (Rysunek 5.25). Pierwsza
faza algorytmu nazywana jest predykacją (z ang. predict). Równania wykonywane w
trakcie tej fazy nazywane są aktualizacją czasową (z ang. time update). Druga faza
nazywa się korekcją (z ang. correct), a jej równania to aktualizacja pomiarowa (z ang.
measurement update). W trakcie predykcji, bazując na stanie z poprzedniego kroku,
wyznacza się estymowaną wartość stanu x oraz jego kowariancję i są to wartości a priori.
ˆ
Pomiar z w drugiej fazie jest pewną formą sprzężenia zwrotnego. Na jego podstawie
dokonuje się wyznaczenia wartości a posteriori dla stanu i jego kowariancji.

Rysunek 5.25 Cykl dwufazowego algorytmu FK.
Do opisu zarówno procesu jak i systemu pomiarowego stosuję się modele matematyczne.
xk = Axk−1 + Buk−1 + wk−1
zk = Hxk + vk

(5.2)
(5.3)

Pierwsze równanie różnicowe to model procesu, który częściowo jest deterministyczny a
częściowo losowy. Jest to powiązanie poprzedniego stanu z aktualnym poprzez macierz
A. Macierz B to wymuszenie stanu (sterowanie), wk to tzw. szum procesu (część losowa).
Drugie równanie to model pomiaru, gdzie H jest macierzą wiążącą pomiar ze stanem
(wyjście ﬁltru), a vk to zakłócenia. Zarówno wk jak i vk reprezentują białe szumy Gausa.
p(w) ∼ N (0, Q)
p(v) ∼ N (0, R)

(5.4)

Podobnie zapis prawdopodobieństw warunkowych dla powyższych modeli będzie wyglądał
następująco:
p(xk |xk−1 ) ∼ N (Axk−1 + Buk , Q)
p(zk |xk ) ∼ N (Hxk , R)

(5.5)
(5.6)

Teraz zdeﬁniujmy błędy szacowania, w których x− to estymowany stan a priori uzyskany
ˆ
z procesu, a x to estymowany stan a posteriori uwzględniający pomiar zk .
ˆ
ˆk
e− ≡ xk − x−
k
e k ≡ xk − xk
ˆ

(5.7)
(5.8)

5. Praktyczna realizacja układu sterowania

64

gdzie e− to błąd a priori, a ek to błąd a posteriori. Są to różnice pomiędzy rzeczywistym
k
stanem, a wartością estymowaną. W praktyce rzeczywiste wartości stanu xk nie są znane.
Macierze kowariancji (zależności wariancji składowych wektora stanu) to:
−
Pk = E[e− , e− ]
k
k
Pk = E[ek , ek ]

(5.9)
(5.10)

−
gdzie Pk to macierz kowariancji a priori, a Pk to macierz kowariancji a posteriori.

Rysunek 5.26 Kompletny algorytm FK.
Równania pierwszej fazy FK ma postać:
x− = Aˆk−1 + Buk−1
ˆk
x
−
Pk = APk−1 AT + Q

(5.11)

−
gdzie x− i Pk to prognozowane wartości stanu i kowariancji a priori, xk−1 i Pk−1 to
ˆk
ˆ
optymalne szacowane wartości a posteriori wykonane w poprzednim kroku. Wzmocnienie
Kalmana jest czymś w rodzaju wagi z jaką wpłynie faza korekcji na estymowany stan:
−
−
Kk = Pk H T (HPk H T + R)−1

(5.12)

Równanie, które mówi jakie jest optymalne skorygowanie prognozy w czasie k, bazujące
na wszystkich dotychczasowych pomiarach będzie miało postać:
ˆk
xk = x− + Kk (zk − H x− )
ˆ
ˆk

(5.13)

gdzie zk to pomiar, a różnica (zk − H x− ) nazywa się measurement innovation. Pozostaje
ˆk
jeszcze skorygować macierz kowariancji:
−
Pk = (I − Kk H)Pk

gdzie I to macierz jednostkowa.

(5.14)

5. Praktyczna realizacja układu sterowania

5.3.3

65

Pomiar odchylenia kątowego, a Filtr Kalmana

Jest to jeden z najczęściej spotykanych przykładów wykorzystania KF [16]. W obiektach,
które nie mają stałego punktu odniesienia pomiarów odchylenia dokonuje się przy pomocy
inklinometrów, akcelerometrów lub żyroskopów. Niestety żaden z sensorów nie dostarcza
dostatecznie dokładnej informacji o odchyleniu kątowym. W praktyce wykorzystuje się
kilka czujników dla jednego systemu. Najciekawszym przypadkiem jest inklinometr lub
akcelerometr wraz z żyroskopem.
Akcelerometry mierzą przyspieszenia statyczne, np grawitacja, zdolność tą wykorzystuje się do wyznaczenia odchylenia od pionu. Tanie, dostępne na rynku modele charakteryzują się sporymi szumami na wyjściu, dlatego zaleca się stosowanie jeszcze jednego
czujnika. Żyroskopy, jak wiadomo, dostarczają informacji o prędkości obrotowej. Niestety
ich wadą jest fakt, że z czasem ulegają dryftowi, pomiary stają się więc błędne. Filtr
Kalmana doskonale niweluje niepożądane zjawisko dryftu oraz dokonuje przy tym fuzji
pomiarów dostarczając dostatecznie dokładnej i nie zaszumionej informacji o odchyleniu
(obrocie). Równanie takiego systemu ma postać:
αk = αk−1 + (ωk−1 − gbias )dt + wk

(5.15)

zk = Hαk + vk

(5.16)

oraz równanie pomiaru

gdzie α to odchylenie kątowe, ω to prędkość kątowa, gbias to dryft żyroskopu, wk to
szum żyroskopu, a vk to szum akcelerometrów. Pomiar prędkości kątowej ω odczytany
z żyroskopu uwzględniony zostanie już w fazie predykcji jako wymuszenie u, a pomiar
odchylenia uzyskany z akcelerometrów w fazie korekcji.
Można więc zapisać jeszcze raz równania algorytmu KF:
Predykcja:
x− = Axk−1 + Bu
ˆk
−
Pk = APk−1 AT + Q

(5.17)

Korekcja:
−
Pk H T
−
−
Kk =
= Pk H T (HPk H T + R)−1
− T
HPk H + R
xk = x− + Kk (zk − H x− )
ˆ
ˆk
ˆk
−
Pk = (I − Kk H)Pk

(5.18)

gdzie poszczególne macierze są równe



1 0 −dt
A =  0 0 −1  ,
0 0 1


dt
B =  1 ,
0

wektor stanu

(5.19)




α
x= ω 
gbias

(5.20)

5. Praktyczna realizacja układu sterowania

66

wyjście ﬁltru
H=

1 0 0

(5.21)

R to macierz 1 × 1 (szum akcelerometru), macierz kowariancji Q ma wymiar 3 × 3


a 0 0
Q= 0 b 0 
(5.22)
0 0 c
Zarówno Q oraz R należy wyznaczyć eksperymentalnie. Wymiar macierzy K jest 1 × 3.

5.4

Regulator PID

Po skonstruowaniu robota okazało się, że niektóre parametry ﬁzyczne elementów składowych nieco się różniły od tych, które przyjęto w trakcie symulacji. W trakcie budowy, wielokrotnie zmieniano koncepcję, która przyczyniła się do wymieniany napędów
na zdecydowanie mocniejsze. Symulacje przeprowadzono zanim jeszcze powstał ﬁzycznie
robot. Niestety autor nie zdołał ponowne wyznaczyć optymalnych sterowników oraz
przeprowadzić symulacje. Aby jednak przetestować konstrukcję i zbadać poprawność działania wszystkich modułów, zdecydowano o napisaniu prostego podwójnego regulatora
PID.

Rysunek 5.27 Schemat blokowy prostego podwójnego regulatora PID
Głównym celem sterowania było utrzymywanie równowagi. Do tego wystarczyło zastosować prosty regulator PID. Niestety nigdy nie zdarza się tak, aby obydwa napędy

5. Praktyczna realizacja układu sterowania

67

charakteryzowały się takimi samymi parametrami, nawet jeśli są tego samego typu. Niezbędne okazało się zastosowanie drugiego regulatora, który kontrolował, aby obydwa koła
kręciły się z taką samą prędkością.
Równanie pierwszego regulatora
t

e(t)dt + Kd

uα = Kp e(t) + Ki
0

de(t)
dt

gdzie
e(t) = αd − α(t)

αd = 0

Warto zwrócić uwagę na to, że jeżeli αd = 0 to błąd człon różniczkującego, to
prędkością kątową α.
˙
Drugie równanie regulatora jest następujące
t

uφ12 (t) = Kp e(t) + Ki

e(t)dt + Kd
0

gdzie

de(t)
dt

jest

de(t)
dt

˙
˙
e(t) = φ1 − φ2

Można teraz zapisać sterowanie napędów w całości
u1 (t) = uα (t) + uφ12 (t)
u2 (t) = uα (t) − uφ12 (t)

5.5

Oprogramowanie

Program dla mikrokontrolera został w całości napisany w środowisku CodeWarrior IDE
w wersji 5.1.1090 ﬁrmy Metrowerks Corporation [29] w języku ANSI C.
W podrozdziale poświęconemu oprogramowaniu zdecydowano o przedstawieniu tylko
niektórych fragmentów kodu. Głównie skupiono się na konﬁguracji poszczególnych modułów oraz implementacji Filtru Kalmana oraz podwójnego sterownika PID. Wszystkie
przedstawione listingi zostały zaimplementowane i przetestowane.

5.5.1

Deﬁnicje

Do omówienia konﬁguracji poszczególnych modułów niezbędne jest zapoznanie się z niektórymi deﬁnicjami przedstawionymi poniżej.

//
//
//

//
//

#define XTAL 20000000
#define PIT_TB (XTAL/256)
***********************************
Definicje dla USIU
***********************************
#define TPSEC 100 //przerwanie 100Hz
#define PIT_CONST
((PIT_TB+(TPSEC & gt; & gt; 1))/TPSEC) // oblicz stała dla PIT
***********************************
Definicje dla QSM

5. Praktyczna realizacja układu sterowania

68

// ***********************************
#define ADCSEL
0x0E
// ADC aktywny LOW dla PCS0 - moduł INS
#define DACSEL
0x0D
// DAC aktywny LOW dla PCS1 - sterownik silników
#define QsmDT
0x20
#define QsmBITSE
0x40
#define QsmDSCK
0x10
#define DESELECT
0xF
// no SLAVEs selected
// ***********************************
// Definicje dla TPU
// ***********************************
#define CHAN0
0
// TPU channel
#define CHAN1
1
// TPU channel
#define CHAN2
2
// TPU channel
#define CHAN3
3
// TPU channel
#define CHAN4
4
// TPU channel
#define CHAN5
5
// TPU channel
#define CHAN6
6
// TPU channel
#define CHAN7
7
// TPU channel
#define CHAN8
8
// TPU channel
#define CHAN10
10 // TPU channel
#define CHAN11
11 // TPU channel
#define CHAN12
12 // TPU channel
#define CHAN13
13 // TPU channel
#define CHAN14
14 // TPU channel
#define CHAN15
15 // TPU channel
#define FSys
40000000 // 40
MHz
#define A_TCR1CK
((FSys)/4)
// 10
MHz
#define A_TCR2CK
((FSys)/8)
// 5 MHz
#define B_TCR1CK
((FSys)/4)
// 10
MHz
#define B_TCR2CK
((FSys)/8/4) // 0,625
MHz
#define PWM_FRQ_SERW 50
// częstotliwość PWM dla serwomechanizmów [Hz]
#define PWM_PER_SERW ((B_TCR2CK+(PWM_FRQ_SERW & gt; & gt; 1))/PWM_FRQ_SERW)
#define PWM_FRQ_LUX 250
// częstotliwość PWM dla oświetlenia RGB [Hz]
#define PWM_PER_LUX ((B_TCR2CK+(PWM_FRQ_LUX & gt; & gt; 1))/PWM_FRQ_LUX)
// ***********************************
// Definicje dla QADC
// ***********************************
#define QADC_CCW_P 0x0200
#define QADC_CCW_BYP 0x0100
#define QADC_CCW_QCLK2 0x0000
#define QADC_CCW_QCLK4 0x0040
#define QADC_CCW_QCLK8 0x0080
#define QADC_CCW_QCLK16 0x00C0
#define QADC_CCW_AN0 0
#define QADC_CCW_AN1 1
#define QADC_CCW_AN2 2
#define QADC_CCW_AN3 3
#define QADC_CCW_AN48 48
#define QADC_CCW_AN49 49

5. Praktyczna realizacja układu sterowania

#define
#define
#define
#define
#define
#define
#define
#define
#define
#define
#define
#define
#define
#define

5.5.2

69

QADC_CCW_AN50 50
QADC_CCW_AN51 51
QADC_CCW_AN52 52
QADC_CCW_AN53 53
QADC_CCW_AN54 54
QADC_CCW_AN55 55
QADC_CCW_AN56 56
QADC_CCW_AN57 57
QADC_CCW_AN58 58
QADC_CCW_AN59 59
QADC_CCW_VRL 60
QADC_CCW_VRH 61
QADC_CCW_VRM 62 // (VRH-VRL)/2
QADC_CCW_EOQ 63

Konﬁguracja UART

Do komunikacji sterownika z komputerem PC skonﬁgurowano jeden z dwóch dostępnych
portów UART [20]. Wyjścia portów SCI można przy pomocy specjalnych zworek na
module skonﬁgurować jako TTL lub RS232. Pierwszy port (UART1) skonﬁgurowano do
pracy z prędkością 57600kb w trybie RS232.
InitPort1();
InitializeUART(kBaud57600);
printf( " \n\rUART channel 1 initialized\n\r " );

5.5.3

Konﬁguracja modułu USIU

Moduł USIU (Uniﬁed System Interface Unit) [20] jest odpowiedzialny za start mikrokontrolera, jego inicjację, tryby pracy, zabezpieczenia, pracę zewnętrznej magistrali (EBI),
synchronizację zegara, pamięć, debagowanie oraz system przerwań.
Jednym z bloków modułu USIU jest funkcja cyklicznego przerwania PIT (Periodic
Interrupt Timer). W sterowniku skonﬁgurowano je jako główne przerwanie, w którym
realizowane jest sterowanie robota. Częstotliwość tego przerwania ustawiono na 100Hz.
Jak już wspomniano USIU kontroluje także zewnętrzną magistralę (EBI), która wyposażona jest w sprzętowy dekoder adresów. Blok wyboru urządzeń zewnętrznych został
skonﬁgurowany do pracy z wyświetlaczem LCD. W module z mikrokontrolerem linie
wyboru CS0 i CS1 są juz zarezerwowane do pracy z pamięciami. Zatem wybór wyświetlacza skonﬁgurowano dla linii CS2. Dostęp do wyś-wietlacza ustawiono pod adresami:
0x02000000 - rejestr danych, 0x02000001 - rejestr konﬁguracyjny.
void Usiu_Init()
{
/*********************** USIU ******************************/
/*********************** PIT ******************************/
// STEP 1: Inicjacja
USIU.PITC.B.PITC =PIT_CONST; // wartość do zliczenia
USIU.PISCR.B.PITF = 1;
// zatrzymanie PIT jeżeli FREEZE
USIU.PISCR.B.PTE = 1;
// start licznika

5. Praktyczna realizacja układu sterowania

70

// STEP 2: Ustawienie poziomu przerwań
USIU.PISCR.B.PIRQ = 0x20;
// Poziom przerwań dla PIT równy 2
// STEP 3: Włącz/wyłącz przerwanie
USIU.PISCR.B.PIE = 0 ;
// 0-włącz 1-wyłącz
/********************* INTERRUPTS *************************/
USIU.SIMASK.R = 0xFFFF0000;
// maskuj wszystkie przerwania
/********************* CHIP SELECTS ***********************/
/******* LCD - konfiguracja dekodera adresowego ************/
/*********************** BR2/OR2 ***************************/
USIU.BR2.B.BA=0x0400;
//Ustaw adres bazowy na 0x200
//
// UWAGA
// pole BA ma szerokość 17 bitów, a nie 16
// zapisanie liczby 0x0400 odpowiada adresowi 0x0200
//
USIU.BR2.B.PS=1;
// 8 bitów - szerokość danych
USIU.BR2.B.V=1;
// bit skończonej konfiguracji
USIU.OR2.B.AM=0xff80;
// maskuj cały adres
USIU.OR2.B.SCY=0xF;
// szerokość zegara (najdłuższa)
USIU.OR2.B.BSCY=2;
// szerokość impulsu zegara
USIU.OR2.B.ACS=3;
// CS ustaw w w połowie zegara
/*********************** POWER & CLOCK **********************/
USIU.SCCR.B.RTDIV=1;
// Ustaw dzielnik zegara RTC na 256
/******************** END USIU ******************************/
}

5.5.4

Konﬁguracja modułu MIOS1

Moduł MIOS1 (Modular Input/Output Subsystem) [20] zawiera szereg przydatnych funkcji
czasowo-licznikowych. Pozwala na skonﬁgurowanie 32 kanałów jako funkcje PWM, timery,
linie I/O, generatory impulsów, IC, OC, pomiar szerokości impulsu. W sterowniku skonﬁgurowano dwa kanały. Kanał szósty jako cykliczne przerwanie, w którym dokonuje się
rekursywnego przeliczania Filtru Kalmana. Częstotliwość tego przerwania wynosi 500Hz.
Kanał zerowy został skonﬁgurowany jako funkcja PWM. Służy do sterowania oświetleniem
białym.
void Mios1_Init()
{
/*********************** MIOS1 ******************************/
// Inicjacja MIOS Timer MMCSM6
// 6 kanał jako timer z
//
przerwaniem 500Hz
MIOS1.MMCSM6SCR.B.CLS = 0x3;
// ustaw źródło impulsów
//
z dzielnika zegara
MIOS1.MMCSM6SCR.B.CP = 0xEC;
// ustaw dzielnik zegara 20 - 1MHz
MIOS1.MMCSM6CNT.R = 0xFFFF-1000; // ustaw wartość do zliczenia
MIOS1.MMCSM6MLR.R = 0xFFFF-1000; // ustaw wartość do załadowania
// po zliczeniu
// Initialization MPWM0 na kanale 0 PWM
MIOS1.MPWMSM0PERR.R=0x0FFF;
// PWM okres

5. Praktyczna realizacja układu sterowania

71

MIOS1.MPWMSM0PULR.R=0x0800;
// PWM wypełnienie
MIOS1.MPWMSM0SCR.B.EN=1;
// PWM uruchom
// Inicjacja dzielnika częstotliwości
MIOS1.MCPSMSCR.B.PSL = 2;
// dzielnik częstotliwości
//
2 (40MHz/2=20MHz)
MIOS1.MCPSMSCR.B.PREN = 1;
// włącz licznik
// Konfiguracja przerwań dla MIOS1
MIOS1.MIOS1LVL0.B.LVL=1;
// Ustaw poziom przerwań na 1
MIOS1.MIOS1ER0.B.EN6 = 0;
// Przerwanie 1-włącz 0-wyłącz
/******************* END MIOS1 ******************************/
}

5.5.5

Konﬁguracja modułów TPU

Moduły TPU3 [20] [21] są rozwinięciem dobrze już znanej wersji TPU. Są to niezależne moduły mikrokontrolera, które można w dowolny sposób zaprogramować jako różne
funkcje czasowe. Ich największą zaletą jest to, że każda z nich posiada swoją niezależną
jednostkę, co pozwala odciążyć główną jednostkę od czasochłonnych operacji czasowolicznikowych. W sterowniku skonﬁgurowano obydwa dostępne układy TPU. Moduł TPU_A
został wykorzystany głównie do obsługi enkoderów jako dekodery kwadraturowe. Moduł
TPU_B odpowiada za sterowanie serwomechanizmami oraz układem oświetlenia RGB.
void Tpu_Init()
{
/*********************** TPU ******************************/
/*********************** TPU_A ****************************/
TPU_A.TPUMCR3.B.PWOD=1;
// Ten bit trzeba ustawić przed konfiguracją
//
TCR1P/TCR2P ponieważ pola konfiguracji
//
dzielnika w rejestrze MCR domyślnie
//
można ustawiać tylko raz
TPU_A.TPUMCR.B.EMU=1;
// Tryb pracy z indywidualna maska funkcji
TPU_A.TPUMCR.B.T2CG=1;
// Ustaw źródło zegara TCR2 na wew.
TPU_A.TPUMCR.B.PSCK=1;
// Dzielnik źródła zegara TCR1 1=/4 0=/32
TPU_A.TPUMCR.B.TCR1P=0;
// Dzielnik zegara TCR1 0-1 1-2 2-4 3-8
TPU_A.TPUMCR.B.TCR2P=0;
// Dzielnik zegara TCR2 0-1 1-2 2-4 3-8
TPU_A.TICR.B.CIRL = 4;
// Ustaw poziom przerwań na 4
TPU_A.TICR.B.ILBS = 0;
// Wyłącz przerwania
/*********************** TPU_B ************************/
TPU_B.TPUMCR3.B.PWOD=1;
TPU_B.TPUMCR.B.EMU=1;
TPU_B.TPUMCR.B.T2CG=1;
TPU_B.TPUMCR.B.PSCK=1;
// źródło dzielnika zegara TCR1 1=/4 0=/32
TPU_B.TPUMCR.B.TCR1P=0;
// Dzielnik zegara TCR1 0-1 1-2 2-4 3-8
TPU_B.TPUMCR.B.TCR2P=2;
// Dzielnik zegara TCR2 0-1 1-2 2-4 3-8
TPU_B.TICR.B.CIRL = 5;
// Ustaw poziom przerwań na 4
TPU_B.TICR.B.ILBS = 0;
// nie używaj przerwań
/*********************** TPU CHAN CONF ********************/
// konfiguracja sterowań dla serwomechanizmów
//ch0

5. Praktyczna realizacja układu sterowania

72

tpu_pwm_init( & TPU_B,CHAN0,3,PWM_PER_SERW,PWM_PER_SERW/15,TPU_PWM_TCR2);
//ch1
tpu_pwm_init( & TPU_B,CHAN1,3,PWM_PER_SERW,PWM_PER_SERW/15,TPU_PWM_TCR2);
//ch2
tpu_pwm_init( & TPU_B,CHAN2,3,PWM_PER_SERW,PWM_PER_SERW/15,TPU_PWM_TCR2);
//ch3
tpu_pwm_init( & TPU_B,CHAN3,3,PWM_PER_SERW,PWM_PER_SERW/15,TPU_PWM_TCR2);
//ch4
tpu_pwm_init( & TPU_B,CHAN4,3,PWM_PER_SERW,PWM_PER_SERW/15,TPU_PWM_TCR2);
// konfiguracja sterowań dla układu oświetlenia RGB
//ch5
tpu_pwm_init( & TPU_B,CHAN5,3,PWM_PER_LUX,PWM_PER_LUX/2,TPU_PWM_TCR2);
//ch6
tpu_pwm_init( & TPU_B,CHAN6,3,PWM_PER_LUX,PWM_PER_LUX/2,TPU_PWM_TCR2);
//ch7
tpu_pwm_init( & TPU_B,CHAN7,3,PWM_PER_LUX,PWM_PER_LUX/2,TPU_PWM_TCR2);
// konfiguracja obsługi enkoderów
// ch8 & ch9
tpu_qdec_init( & TPU_A,CHAN8,3,0);
// ch10 & ch11
tpu_qdec_init( & TPU_A,CHAN10,3,0);
/****************** END TPU ******************************/
}
Przykład odczytu wartości zliczonych przez enkodery
result0=(tpu_qdec_position( & TPU_A, CHAN8));
result1=(tpu_qdec_position( & TPU_A, CHAN10));

5.5.6

Konﬁguracja QSMCM

Moduł QSMCM ( Qued Serial Multi-Channel Module) [20] odpowiada za kolejkową komunikację przy pomocy interfejsu SPI. Komunikację tą wykorzystują dwa podzespoły robota:
sterownik silników oraz moduł nawigacyjny INS. Przy obydwu układach wykorzystuje się
transmisję o długości 12 bitów.
void Qsmcm_Init()
{
/*********************** QSPI *****************************/
QSMCM.DDRQS.R = 0xfe;
// PCSx, SCK, MOSI - wyj, MISO - wej
QSMCM.PORTQS.R = DESELECT & lt; & lt; 3;
QSMCM.PQSPAR.R = 0x7b;
// Wszystkie piny dla QSPI
QSMCM.SPCR1.R = 0x1717;
// DSCKL=?us, DTL=?us ustaw opóźnienia
QSMCM.SPCR2.R = 0x0700;
// kolejkuj od 0 do 7 (8 words),
QSMCM.SPCR0.R = 0xB00F;
// master, 12bitów, cpol 0, cpha 0, 3MHz
QSMCM.TRANRAM[0].R = (VUINT16)0xF00; // komenda dla kanału 0
QSMCM.COMDRAM[0].R = QsmDT| QsmBITSE | QsmDSCK | ADCSEL;
// 12 bitów QSPI
QSMCM.TRANRAM[1].R = (VUINT16)0x000; // komenda dla kanału 1
QSMCM.COMDRAM[1].R = QsmDT | QsmBITSE | QsmDSCK | ADCSEL;

5. Praktyczna realizacja układu sterowania

73

// 12 bitów QSPI
QSMCM.TRANRAM[2].R = (VUINT16)0x100; // komenda dla kanału 2
QSMCM.COMDRAM[2].R = QsmDT | QsmBITSE| QsmDSCK | ADCSEL;
// 12 bitów QSPI
QSMCM.TRANRAM[3].R = (VUINT16)0x200; // komenda dla kanału 3
QSMCM.COMDRAM[3].R = QsmDT | QsmBITSE | QsmDSCK | ADCSEL;
// 12 bitów QSPI
QSMCM.TRANRAM[4].R = (VUINT16)0x700; // komenda dla kanału 4
QSMCM.COMDRAM[4].R = QsmDT | QsmBITSE | QsmDSCK | ADCSEL;
// 12 bitów QSPI
QSMCM.TRANRAM[5].R = (VUINT16)0x800; // komenda dla kanału 5
QSMCM.COMDRAM[5].R = QsmDT | QsmBITSE | QsmDSCK | ADCSEL;
// 12 bitów QSPI
QSMCM.TRANRAM[6].R = (VUINT16)(0x0300)+(128); // komenda dla kanału 6
QSMCM.COMDRAM[6].R = QsmDT | QsmBITSE | QsmDSCK | DACSEL;
// 12 bitów QSPI
QSMCM.TRANRAM[7].R = (VUINT16)(0x0700)+(128); // komenda dla kanału 7
QSMCM.COMDRAM[7].R = QsmDT | QsmBITSE | QsmDSCK | DACSEL;
// 12 bitów QSPI
/****************** END QSPI *****************************/
}
Poniżej przedstawiono przykład uruchomienia transmisji.
QSMCM.SPCR1.B.SPE=1; // uruchom transmisję
if (QSMCM.SPSR.B.SPIF==1)
{
result1=QSMCM.RECRAM[2].R;
result2=QSMCM.RECRAM[3].R;
result3=QSMCM.RECRAM[4].R;
result4=QSMCM.RECRAM[5].R;
result5=QSMCM.RECRAM[1].R;
QSMCM.SPSR.B.SPIF=0; // zgaś flagę ukończenia
QSMCM.SPCR1.B.SPE=1; // uruchom kolejną transmisję
}

5.5.7

Konﬁguracja QADC64

Moduł QADC ( Qued Analog-To-Digital Converter Module-64) [20] to kolejkowy 10bitowy, 16 kanałowy, przetwornik analogowo-cyfrowy. MPC555 zawiera dwa niezależne
moduły QADC_A oraz QADC_B. W oprogramowaniu sterownika, skonﬁgurowano pierwszy moduł przetwornika do pomiaru napięcia akumulatora oraz trzech czujników odległości. Drugi moduł odpowiedzialny jest za pomiary dotykowego panelu, zainstalowanego
na wyświetlaczu LCD. Wszystkie wyprowadzenia przetwornika mogą także pracować jako
zwykłe linie wejściowe, a połowa z nich także jako wyjściowe.
void Qadc_Init()
{
/*********************** QADC *****************************/
/*********************** QADC_A ***************************/

5. Praktyczna realizacja układu sterowania

QADC_A.QADC64MCR.B.STOP =
QADC_A.QADC64MCR.B.FRZ =
QADC_A.QADC64MCR.B.SUPV =
QADC_A.QADC64INT.B.IRL1 =
QADC_A.QADC64INT.B.IRL2 =
// Control Register 0
QADC_A.QACR0.B.MUX = 0;
QADC_A.QACR0.B.PSH = 11;
QADC_A.QACR0.B.PSL = 7;
// Control Register 1 (queue
QADC_A.QACR1.B.CIE1 = 0;

0;
0;
1;
7;
7;

74

// wlącz zegar dla QADC
// ignoruj sygnał FREEZE
// poziom przerwań dla pierwszej kolejki
// poziom przerwań dla drugiej kolejki
// wybieraj automatycznie
// High QLCK Time = (PHS + 1) / FSys
// Low QLCK Time = (PHL + 1) / FSys

1)

// Wyłącz przerwania od skończonego
//
cyklu pomiarów
QADC_A.QACR1.B.PIE1 = 0;
// Wyłącz przerwania od pauzy
//
cyklu pomiarów
QADC_A.QACR1.B.MQ1 = 0x01;
// pojedynczy skan wyzwalany programowo
// konfiguracja kanałów pomiarowych 0...3
QADC_A.CCW[0].R = QADC_CCW_BYP | QADC_CCW_QCLK16 | QADC_CCW_AN0;
QADC_A.CCW[1].R = QADC_CCW_BYP | QADC_CCW_QCLK16 | QADC_CCW_AN1;
QADC_A.CCW[2].R = QADC_CCW_BYP | QADC_CCW_QCLK16 | QADC_CCW_AN2;
QADC_A.CCW[3].R = QADC_CCW_BYP | QADC_CCW_QCLK16 | QADC_CCW_AN3;
QADC_A.CCW[4].R = QADC_CCW_EOQ;
/*********************** QADC_B ***************************/
QADC_B.QADC64MCR.B.STOP = 0;
QADC_B.QADC64MCR.B.FRZ = 0;
QADC_B.QADC64MCR.B.SUPV = 0;
QADC_B.QADC64INT.B.IRL1 = 7;
QADC_B.QADC64INT.B.IRL2 = 7;
// Control Register 0
QADC_B.QACR0.B.MUX = 0;
QADC_B.QACR0.B.PSH = 11;
QADC_B.QACR0.B.PSL = 7;
// Control Register 1 (queue 1)
QADC_B.QACR1.B.CIE1 = 0;
QADC_B.QACR1.B.PIE1 = 0;
QADC_B.QACR1.B.MQ1 = 0x01;
// konfiguracja kanałów pomiarowych - tylko 0 kanał
QADC_B.CCW[0].R = QADC_CCW_BYP | QADC_CCW_QCLK16 | QADC_CCW_AN1;
QADC_B.CCW[4].R = QADC_CCW_EOQ;
/*********************** QADC *****************************/
Poniżej przedstawiono przykładowy fragment kodu realizujący pojedynczy pomiar
(cykl kolejki).
void read_analog_inputs()
{
QADC_A.QACR1.B.SSE1 = 0x01;
// uruchom pojedynczy skan
while( !(QADC_A.QASR0.B.CF1)) {} // czekaj na koniec przetwarzania
QADC_A.QASR0.B.CF1 = 0;
// zgaś flagę końca przetwarzania
result0=QADC_A.RJURR[0].R;
// kopiuj rezultat do zmiennej pomocniczej
result1=QADC_A.RJURR[1].R;
// kopiuj rezultat do zmiennej pomocniczej

5. Praktyczna realizacja układu sterowania

result2=QADC_A.RJURR[2].R;
result3=QADC_A.RJURR[3].R;

75

// kopiuj rezultat do zmiennej pomocniczej
// kopiuj rezultat do zmiennej pomocniczej

}

5.5.8

Obsługa przerwań w MPC555

Mikrokontroler MPC555 posiada wektory, tzw zdarzeń (Exceptions) [20] [19], a nie tradycyjnie wektory przerwań. W wektorach tych zapisuje się adres, pod którym znajduje
się funkcja obsługująca dane zdarzenie. Wszystkie przerwania od poszczególnych modułów widziane są jako jedno i to samo zdarzenie (External Exception 0x500). Jednym
ze sposobów odróżniania źródeł przerwań zewnętrznych jest nadanie każdemu z nich innego poziomu. Innym sposobem może być czytanie kolejno ﬂag wystąpienia przerwania
poszczególnych modułów. W sterowniku zrealizowano pierwsze rozwiązanie.
Za kontrolowanie przerwań odpowiedzialny jest moduł USIU, którego konﬁgurację
przedstawiono powyżej. Warto wspomnieć, że bloki modułu USIU, takie jak PIT, PLL,
Real-Time Clock itd., mogą zgłaszać przerwania na poziomach 0...7. Dodatkowo dochodzą przerwania od zewnętrznych, linii mikrokontrolera, które nie reprezentują tych
samych poziomów przerwań. Daje w sumie 16 poziomów wszystkich przerwań. Moduły
podłączone do IMB3 BUS takie jak TPU, MIOS1, QADC, TOUCAN, QSMCM mogą
mieć przydzielane poziomy przerwań z zakresu 0...32 przy czym przerwania na poziomie
& gt; 7 są zgłaszane do jednostki jako przerwania o poziomie 7. Ich rozstrzyganiem zajmuje
się moduł UIMB.
W zaimplementowanej obsłudze rozpoznawania źródeł przerwań odczytuje się rejestr
SIPEND.
void InterruptHandler(long cause)
{
UINT32 int_value = 0 ;
//******************************************************
// Włącznie jednostki zmiennoprzecinkowej
//
( w przerwaniach domyślnie jest wyłączana}
//******************************************************
asm{
mfmsr r3
ori r3,r3,0x00002000
mtmsr r3
}
//******************************************************
int_value = USIU.SIPEND.R ;
switch(cause)
{
case 0x500: // zewnętrzne przerwanie
if (int_value & LEVEL1)
{
MIOS1.MIOS1SR0.B.FLG6 = 0; // zgaś flagę przerwania
Interrupt_MIOS();
// funkcja obsługująca przerwanie
//
cykliczne skonfigurowane
//
w module MIOS1
}

5. Praktyczna realizacja układu sterowania

else if (int_value & LEVEL2)
{
USIU.PISCR.B.PS = 1;
Interrupt_PIT();
}
else return;

76

// zgaś flagę przerwania
// funkcja obsługująca przerwanie PIT

}
}
Po rozpoznaniu źródła przerwania następuje zgaszenie ﬂagi oraz skok do funkcji obsługującej dane przerwanie.

5.5.9

Filtr Kalmana

Poniżej przedstawiono przykład implementacji ﬁltra dla dowolnego mikrokontrolera [16].
W tym celu zadeklarowano dwie struktury stan oraz kalm. Pola tych struktur przechowują pomiary, wartości poszczególnych elementów macierzy P , Q oraz wektora stanu
x. Warto zwrócić uwagę na to, że nie ma konieczności obliczania wszystkich elementów
macierzy P . W procesie ﬁltracji biorą udział jedynie elementy P11 , P13 , P21 , P31 , P33 .
//*******************************************************
// Definicja struktury stanu - zmienna globalna
//*******************************************************
struct typ_state {
VFLOAT64 alpha;
// odfiltrowane odchylenie
VFLOAT64 dalpha;
// odfiltrowana prędkość odchylania
VFLOAT64 pomiar_alpha; // pomiar odchylenia
VFLOAT64 pomiar_omega; // pomiar prędkość kątowej
VFLOAT64 phi1;
// obrót pierwszego koła
VFLOAT64 phi2;
// obrót drugiego koła
VFLOAT64 dphi1;
// prędkość obrotowa pierwszego koła
VFLOAT64 dphi2;
// prędkość obrotowa pierwszego koła
};
extern struct typ_state state;
//*******************************************************
// Definicja struktury dla F.Kalmana - zmienna globalna
//*******************************************************
struct typ_kalman {
VFLOAT64 Q;
// Wartości przekątnej macierzy Q
VFLOAT64 R;
// Wariancja pomiaru
VFLOAT64 alpha;
// odchylenie
VFLOAT64 omega;
// prędkość kątowa
VFLOAT64 g_bias; // dryft żyroskopu
VFLOAT64 P11;
// wartości macierzy kowariancji P
VFLOAT64 P13;
VFLOAT64 P21;
VFLOAT64 P31;
VFLOAT64 P33;
VFLOAT64 K1;
// wartości macierzy K

5. Praktyczna realizacja układu sterowania

VFLOAT64
VFLOAT64

77

K2;
K3;

};
extern struct typ_kalman kalm;
W programie głównym należy wszystkie pola zainicjować odpowiednimi wartościami.
void Kalman_Init()
{
// stan
state.alpha=0;
state.dalpha=0;
state.pomiar_alpha=0;
state.pomiar_omega=0;
// kalman
kalm.Q=0.0001;
kalm.R=1;
kalm.alpha=0;
kalm.omega=0;
kalm.g_bias=0;
kalm.P11=kalm.R;
kalm.P13=0;
kalm.P21=0;
kalm.P31=0;
kalm.P33=0;
kalm.K1=0;
kalm.K2=0;
kalm.K3=0;
Teraz można przystąpić do rekursywnej pracy ﬁltra. Do tego celu wykorzystano przerwanie skonﬁgurowane w module MIOS1. Wszystkie operacje arytmetyczne są wykonywane na zmiennych typu f loat. Proces obliczeń przebiega dość szybko, gdyż MPC555
wyposażony jest w 64-bitową jednostkę zmiennoprzecinkową.
#define dt 0.002 // przerwanie co 2ms, f=500Hz

void Interrupt_MIOS()
{
//***********************************************
// POMIAR
//***********************************************
state.pomiar_alpha=odczytaj_akcelerometry();
state.pomiar_omega=odczytaj_żyroskop();
//***********************************************
// FILTR KALMANA
//***********************************************
///////////////////
// PREDYKCJA
//_____________________________________________

5. Praktyczna realizacja układu sterowania

//
|alpha |
|1 0 -dt| |alpha |
| dt|
//
|omega | =|0 0 -1 |*|omega |
+| 1 |*gyro
//
|q_bias |k |0 0 1 | |q_bias |k-1 | 0 |
// xk apriori
//
kalm.alpha=kalm.alpha+(state.pomiar_omega-kalm.g_bias)*dt;
kalm.omega=state.pomiar_omega-kalm.g_bias;
// kalm.g_bias=kalm.g_bias;
//______________________________________________
// Pk=AP_{k-1}A^T+Q a priori
//
kalm.P11=kalm.P11-kalm.P31*dt+kalm.P33*dt*dt-kalm.P13*dt+kalm.Q;
kalm.P13=kalm.P13-kalm.P33*dt;
kalm.P21=kalm.P33*dt-kalm.P31;
kalm.P31=kalm.P31-kalm.P33*dt;
kalm.P33=kalm.P33+kalm.Q;
//
/////////////////////
// KOREKCJA
// ____________________________________________
// Kk=PkH^T(HPkH^T+R)^-1
//
kalm.K1=kalm.P11*(1/(kalm.P11+kalm.R));
kalm.K2=kalm.P21*(1/(kalm.P11+kalm.R));
kalm.K3=kalm.P31*(1/(kalm.P11+kalm.R));
// ____________________________________________
// xk=xk-K(zk-Hxk) a posteriori
//
kalm.apha=kalm.alpha+kalm.K1*(state.pomiar_alpha-kalm.alpha);
kalm.omega=kalm.omega+kalm.K2*(state.pomiar_alpha-kalm.alpha);
kalm.g_bias=kalm.g_bias+kalm.K3*(state.pomiar_alpha-kalm.alpha);
// ____________________________________________
// Pk=(1-KkH)Pk a posteriori
//
kalm.P11=(1-kalm.K1)*kalm.P11;
kalm.P13=(1-kalm.K1)*kalm.P13;
kalm.P21=kalm.P21-kalm.P11*kalm.K2;
kalm.P31=kalm.P31-kalm.P11*kalm.K3;
kalm.P33=kalm.P33-kalm.P13*kalm.K3;
//
/////////////////////////////
// AKTUALIZACJA WEKTORA STANU
//
state.alpha=kalm.theta;
state.dalpha=kalm.omega;
//
}

78

5. Praktyczna realizacja układu sterowania

5.5.10

79

Sterownik

W robocie zaimplementowano jedynie prosty, podwójny regulator PID. Jego implementacja w układach mikroprocesorowych nie jest zbyt skomplikowana. Poniżej przedstawiono fragment kodu, który odpowiada za dostarczenie odpowiednich sterowań do silników.
Zanim przedstawiony zostanie fragment kodu sterownika, konieczne jest zapoznanie
się z niektórymi zmiennymi globalnymi wykorzystywanymi w sterowaniu.
//*******************************************************
// Definicja struktury ustawień PID - zmienna globalna
//*******************************************************
struct Settings_tag {
struct {
VFLOAT64 P;
VFLOAT64 I;
VFLOAT64 D;
VFLOAT64 angle_int_err;
} angle_PID;
struct {
VFLOAT64 P;
VFLOAT64 I;
VFLOAT64 D;
VFLOAT64 diferential_int_err;
VFLOAT64 diferential_old_err;
} diferential_PID;
};
extern struct Settings_tag settings;
//*******************************************************
// Definicja struktury stanu - zmienna globalna
//*******************************************************
struct typ_state {
VFLOAT64 alpha;
// odfiltrowane odchylenie
VFLOAT64 dalpha;
// odfiltrowana prędkość odchylania
VFLOAT64 pomiar_alpha; // pomiar odchylenia
VFLOAT64 pomiar_omega; // pomiar prędkość kątowej
VFLOAT64 phi1;
// obrót pierwszego koła
VFLOAT64 phi2;
// obrót drugiego koła
VFLOAT64 dphi1;
// prędkość obrotowa pierwszego koła
VFLOAT64 dphi2;
// prędkość obrotowa pierwszego koła
};
extern struct typ_state state;
Kod sterownika jest następujący
void Interrupt_PIT()
{
//*******************************************************
//Prosty podwójny regulator PID
//*******************************************************

5. Praktyczna realizacja układu sterowania

80

FLOAT64 ster1=0; //sterowanie pierwszego silnika - zmienna pomocnicza
FLOAT64 ster2=0; //sterowanie pierwszego silnika - zmienna pomocnicza
FLOAT64 angle_err=0;

//błąd odchylenia od pionu

FLOAT64 diferential_err=0;
//błąd różnicy prędkości kół
FLOAT64 diferential_deriv_err=0; //różnica błędu różnicy prędkości kół
////////////////////////////////////
// Pierwszy regulator PID (odchylenie od pionu)
//
angle_err=state.alpha;
//błąd odchylenia od pionu
settings.angle_PID.angle_int_err+=angle_err; // całkowanie błędu
// Sprawdza granice całkowania
if (settings.angle_PID.angle_int_err & gt; 0.1)
settings.angle_PID.angle_int_err=0.1;
if (settings.angle_PID.angle_int_err & lt; -0.1)
settings.angle_PID.angle_int_err=-0.1;
//
////////////////////////////////////
// Drugi regulator PID (różnica prędkości kół)
//
diferential_err=state.dphi1-state.dphi2;
// błąd różnicy
settings.diferential_PID.diferential_int_err+=diferential_err;
// całkowanie błędu
// Sprawdza granice całkowania
if (settings.diferential_PID.diferential_int_err & gt; 10)
settings.diferential_PID.diferential_int_err=10;
if (settings.diferential_PID.diferential_int_err & lt; -10)
settings.diferential_PID.diferential_int_err=-10;
//
diferential_deriv_err=diferential_err
-settings.diferential_PID.diferential_old_err;
// różniczkowanie błędu
//
////////////////////////////////////
// Wyliczanie sterowania dla pierwszego koła
//
ster1=(angle_err*settings.angle_PID.P
+state.dalpha*settings.angle_PID.D
+settings.angle_PID.angle_int_err*settings.angle_PID.I)
+diferential_err*settings.diferential_PID.P
+settings.diferential_PID.diferential_int_err*settings.diferential_PID.I
+diferential_deriv_err*settings.diferential_PID.D;
//
////////////////////////////////////
// Wyliczanie sterowania dla drugiego koła
//
ster2=(angle_err*settings.angle_PID.P
+state.dalpha*settings.angle_PID.D

5. Praktyczna realizacja układu sterowania

81

+settings.angle_PID.angle_int_err*settings.angle_PID.I)
-diferential_err*settings.diferential_PID.P
-settings.diferential_PID.diferential_int_err*settings.diferential_PID.I
-diferential_deriv_err*settings.diferential_PID.D;
//
////////////////////////////////////
// Zadawanie sterowania na silniki
//
Control_LR(ster1,ster2);
}

5.6

Wyniki badań

Na zbudowanym obiekcie przeprowadzono szereg eksperymentów, w celu weryﬁkacji teoretycznych rozważań. Zaimplementowany prosty regulator PID sprawdzał się w miarę dobrze. Utrzymywał równowagę robota, a przy tym lekko wahał się i podjeżdżał cyklicznie
do przodu i do tyłu. Największym problemem w procesie sterowania okazały się spore
luzy na przekładniach. Należałoby w tym przypadku podjąć próbę uwzględnienia tego
zjawiska w procesie sterowania, np. wprowadzając histerezę.
Aby luzy te nie wpływały znacząco na jakość sterowania, w procesie ﬁltracji pomiarów tak dobrano współczynniki, aby luzy te nie były przez system wykrywane. Tak silne
ustawienie parametrów ﬁltracji, niestety prowadziło także do zatracenia istotnych informacji, takich jak szybkie zmiany odchylenia od pionu. Zjawisko to m.in. powodowało,
że robot cały czas się kołysał. Poniżej przedstawiono kilka wykresów, które ilustrują działanie modułu INS oraz sterownika utrzymującego równowagę. Większość parametrów
została dobrana eksperymentalnie.
Układ pomiarowy INS przebadano dla różnych wartości macierzy Q i R. Pierwsze trzy
wykresy (rysunki 5.28, 5.29, 5.30) uzyskano badając moduł przy dużych wychyleniach
od pionu rzędu 0,4 rad. Czwarty wykres (rysunek 5.31) pokazuje jak pracuje ﬁltr przy
bardzo silnych ustawieniach parametrów. Badanie to przeprowadzono w trakcie pracy
robota (utrzymywanie równowagi).
Kolejne wykresy ilustrują działane sterownika. Na pierwszym z nich (rysunek 5.32)
przedstawiono odﬁltrowaną wartość odchylenia od pionu robota w trakcie normalnej
pracy. Można zaobserwować jak robot „kołysze się” wokół zera. Drugi wykres (rysunek
5.33) przedstawia prędkość odchylenia od pionu. Trzeci wykres (rysunek 5.34) to pomiar
prędkości kół. Widać, kiedy prędkość ta jest równa zero. Jak wspominano w części teoretycznej, silniki działają siłą nie tylko na koła, ale i na wahadło (korpus) robota. Czas, w
którym prędkość jest równa zero to chwile, kiedy moment siły jest niejako odbierany przez
wahadło. Dopiero jak wzrośnie odchylenie, następuję obrót koła względem platformy oraz
jej przemieszczenie. Ostatni wykres (rysunek 5.35) ilustruje wartości sterowań w trakcie
pracy robota.

5. Praktyczna realizacja układu sterowania

82

0.8
przed filtracja
po filtracji
0.6

0.4

θ [rad]

0.2

0

−0.2

−0.4

−0.6

0

10

20

30

40

50
t

60

70

80

90

100

Rysunek 5.28 Pomiar odchylenia kątowego dla parametrów Q = I i R = 1

0.6
przed filtracja
po filtracji
0.4

0.2

θ [rad]

0

−0.2

−0.4

−0.6

−0.8

0

20

40

60

80

100
t

120

140

160

180

200

Rysunek 5.29 Pomiar odchylenia kątowego dla parametrów Q = I· 0.01 i R = 10

5. Praktyczna realizacja układu sterowania

83

0.8
przed filtracja
po filtracji
0.6

0.4

θ [rad]

0.2

0

−0.2

−0.4

−0.6

0

50

100

150
t

200

250

Rysunek 5.30 Pomiar odchylenia kątowego dla parametrów Q = I· 0.0001 i R = 1

0.5
przed filtracja
po filtracji

0.4
0.3
0.2

α

0.1
0
−0.1
−0.2
−0.3
−0.4
−0.5

0

200

400

600

800

1000

1200

t

Rysunek 5.31 Pomiar odchylenia kątowego w trakcie pracy robota przy parametrach
Q = I· 0.00001 i R = 1000

5. Praktyczna realizacja układu sterowania

84

0.1
α
0.08
0.06
0.04
0.02
α

0
−0.02
−0.04
−0.06
−0.08
−0.1

0

500

1000
t

1500

2000

Rysunek 5.32 Pomiar odchylenia kątowego w trakcie pracy robota przy parametrach
Q = I· 0.00001 i R = 1000

0.5
dα/dt

0.4
0.3
0.2

dα/dt

0.1
0
−0.1
−0.2
−0.3
−0.4
−0.5

0

500

1000
t

1500

2000

Rysunek 5.33 Pomiar prędkości kątowej odchylenia od pionu

5. Praktyczna realizacja układu sterowania

85

5
dφ1/dt

4

dφ /dt
2

3
2

dφ/dt

1
0
−1
−2
−3
−4
−5

0

200

400

600

800

1000

1200

1400

t

Rysunek 5.34 Pomiar prędkości kół w trakcie pracy robota

50
40
30
20

u

10
0
−10
−20
−30
u1
−40
−50

u2
0

500

1000
t

1500

Rysunek 5.35 Przebiegi sterowań obydwu kół

2000

Rozdział 6
Podsumowanie
Praca dotyczyła modelowania, sterowania i praktycznej implementacji robota balansującego (mobilne odwrócone wahadło). Wyliczony nieliniowy model dynamiki robota, w
porównaniu z modelami innych podobnych konstrukcji wydaje się być prawidłowy. Po
przeprowadzeniu wielu symulacji można stwierdzić, że zarówno sterowniki liniowe jak i
nieliniowe dają podobny efekt. Cel sterowania jest spełniony. Warto jednak podkreślić,
że nieliniowe sterowniki bez problemu radzą sobie nie tylko z utrzymywaniem równowagi,
nawet przy bardzo dużych odchyleniach robota od pionu, ale także pozwalają trajektorię wychylenia. Jednak w praktyce bardzo rzadko doprowadza się do takich skrajnych
sytuacji. Porównując jakość sterowania, nieliniowa wersja sterownika wydaje się znacznie
lepsza, o szerszych możliwościach. Wynika to z faktu, że brany jest pod uwagę pełny
nieliniowy model dynamiki.
Rozważania teoretyczne zilustrowano nie tylko symulacjami, ale również powstał robot
balansujący „KOSMOS”. Z praktycznego punktu widzenia okazuje się, że zarówno wersja sterownika liniowego jak i nieliniowego nie wymaga systemów o dużej wydajności
obliczeniowej. A zatem, wybrana do eksperymentów jednostka centralna MPC555 ﬁrmy
Freescale z pewnością poradziłaby sobie z realizacją nieliniowego sterownika. W trakcie budowy robota wielokrotnie zmieniano koncepcję. Niektóre parametry ﬁzyczne elementów składowych znacznie różniły się od tych, które przyjęto w trakcie symulacji.
Naj-większym mankamentem konstrukcji były użyte do przeniesienia napędu przekładnie.
Posiadały one dość spore luzy, których w żadnym opracowanym sterowniku nie uwzględniono. Luzy napędów powodowały, że robot nieustannie wpadał w wibracje. Po wielu
próbach z zastosowaniem prostego regulatora PID i silnymi nastawami Filtra Kalmana,
udało się doprowadzić do tego, że robot zaczął poprawnie utrzymywać równowagę.
Konstrukcja jest wciąż rozwijana. Obecnie przeprowadzono testy robota z zainstalowanym w korpusie laptopie. W przyszłości zostaną wymienione napędy na precyzyjne
oraz zostaną zaimplementowane zasymulowane algorytmy. W trakcie badań nad modułem
nawigacyjnym INS autor wnioskuje, że zdecydowanie lepiej jest użyć inklinometr zamiast
akcelerometrów. Ten pierwszy czujnik pozbawiony jest błędów związanych z pomiarem
poziomych przyspieszeń platformy. Dodatkowo metoda pomiaru odchylenia, na podstawie
pomiarów przyspieszeń w dwóch osiach, sprawdza się jedynie podczas ruchu platformy
po powierzchniach płaskich lub nachylonych jedynie wzdłuż kierunku jazdy. Gdy robot
przechyla się na boki pomiar ten staje się błędny. Po kilku eksperymentach okazało się,
im niższe ułożenie środka ciężkości, tym lepiej robot utrzymywał pion.
Roboty to konstrukcje, których ludzkość wciąż się obawia. Badania nad robotami
społecznymi wskazują, w którym kierunku powinno się kierować prace nad urzeczywistnianiem mechanicznych towarzyszy. Zbytnie podobieństwo do człowieka nie jest

6. Podsumowanie

87

Rysunek 6.1 Studenci podczas zabawy z robotem przed Wydziałem Elektroniki PWr
wskazane, gdyż czyni maszynę przerażającą [13]. Balansująca platforma pomimo tego, że
nie posiada kończyn, w pewnym sensie przypomina napęd kroczący. Jest wyposażona
w dwa punkty podparcia oraz także utrzymuje równowagę. W trakcie testów skonstruowanego robota okazało się, że tego typu konstrukcja robota jest milej odbierana przez
człowieka. Delikatne ruchy robota, uwidocznione problemy z utrzymywaniem równowagi,
zwłaszcza na nierównych powierzchniach niejako odsłaniały niedoskonałości konstrukcji,
czyniąc ją bardzo naturalną w zachowaniu i nie sprawiająca zagrożenia.

Bibliograﬁa
[1] TCHOŃ K., MAZUR A., DULĘBA I., HOSSA R., MUSZYŃSKI R. Manipulatory i roboty mobilne: modele, planowanie ruchu, sterowanie, Akademicka Oﬁcyna
Wydawnicza PLJ, Warszawa, 2000.
[2] KACZOREK T. Teoria sterowania i systemów Wydawnictwo Naukowe PWN.
[3] HOSSA R. Modele i algorytmy sterowania kołowych robotów mobilnych, rozprawa
doktorska, Politechnika Wrocławska, 1996.
[4] MAZUR A. Model dynamiki i kinematyki manipulatora mobilnego typu RTR Instytut
Cybernetyki Technicznej Politechniki Wrocławskiej, Wrocław, 1999.
[5] WELCH G.,BISHOP G.: An Introduction to the Kalman Filter. University of North
Carolina at Chapel Hill Department of Computer Science Chapel Hill, NC 27599-3175
[6] NEGENBORN R.: Robot Localization and Kalman Filters. On ﬁnding your position
in a noisy world. Institute of Information and Computing Sciences in partial fulﬁlment of the requirements for the degree of Master of Science, specialized in Intelligent
Systems
[7] PETER S. MAYBECK: Stohastic models, estimation and control. Volume 1 Department of Electrical and Computer Engineering, Air Force Institute of Technology,
Wright-Patterson Air Force Base Ohio.
[8] CHI OOI R.Balancing a Two-Wheeled Autonomous Robot The University of Western
Australia School of Mechanical Engineering, Final Year Thesis 2003.
[9] WNUK M. SZLAWSKI R. Konstrukcja dwukołowego robota mobilnego RoBik Raport
serii SPR nr 12/2004. Instytut Cybernetyki Technicznej Politechniki Wrocławskiej.
Wrocław 2004
[10] A. De LUCA, IANNITTI S., ORIOLO G. Stabilization of the PR planar underactuated robot. Proc. IEEE International Conference on Robotics and Automation (ICRA
2001).
[11] RATAJCZAK A., TCHOŃ K. Control of underactuated robotic manipulators: an
endogenous conﬁguration space approach Proc. IEEE Conf. on Methods and Models
in Automation and Robotics MMAR 2007, pp. 985?990, Szczecin, 2007.
[12] SZLAWSKI R. Konstrukcja podwójnego mostka H Raport 15/10/2004. Wrocław
2007.

BIBLIOGRAFIA

89

[13] WALTERS M.L., SYRDAL D.S., DAUTENHAHN K., BOEKHORST R., LEE
KOAY K. Avoiding the uncanny valley: robot appearance, personality and consistency
of behavior in an attention-seeking home scenario for a robot companion Received:
5 February 2007 / Published online: 20 November 2007.
[14] KĘDZIERSKI J. OSTROWSKI E. Enkoder magnetyczny AS5040 Raport Koła
Naukowego Robotyków „KoNaR” 2007.
[15] KĘDZIERSKI J. OSTROWSKI E. Sterowanie wyświetlaczem graﬁcznym z kontrolerem ﬁrmy Toshiba T6963C Raport Koła Naukowego Robotyków „KoNaR” 2007.
[16] KĘDZIERSKI J. Filtr Kalmana - zastosowania w prostych układach sensorycznych
Raport Koła Naukowego Robotyków „KoNaR” 2008.
[17] OSTROWSKI E. MPC555 Development Board Raport Koła Naukowego Robotyków
„KoNaR” 2008.
[18] phyCORE-MPC555 Hardware Manual, Edition July 2005.
[19] DUNLOP J., FUCHS J., MIHALIK S. MPC555 Interrupts Rev. 0, 26 July 2001.
[20] Freescale Semiconductor, Inc. MPC555 / MPC556 Revised 15 October 2000.
[21] DEES R., LOELIGER J. General TPU C Functions for the MPC500 Family Rev.
0, 10/2002.
[22] http://www.segway.com
[23] Segbot - Final project for the Introduction to Mechatronics class at the University of
Illinois http://coecsl.ece.uiuc.edu/ge423 /spring04/group9/index.htm, 2004.
[24] ±1.5g - 6g Three Axis Low-g Micromachined Accelerometer Freescale Semiconductor,
Technical Data, Rev 5, 03/2008.
[25] ±150o /s Single Chip Yaw Rate Gyro with Signal Conditioning Analog Devices Inc.
[26] 12-BIT, 200-KSPS, 11 CHANNEL, LOW POWER, SERIAL ADC WITH INTERNAL REFERENCE Texas Instruments, DECEMBER 2001.
[27] +5V, Low-Power, 8-Bit Quad DAC with Rail-to-Rail Output Buﬀers MAXIM, 191105, Rev 0. 8/96.
[28] www.austriamicrosystems.com. „AS5040 DataSheet Rev 1.6 2006”.
[29] CodeWarrior Development Tools IDE 5.1 Userñs Guide