Symulacja dynamiki pªaskich ªa«cuchów kinematycznych

Transkrypt

Symulacja dynamiki
pªaskich ªa«cuchów kinematycznych
Raport nr 05-06 Instytutu Automatyki i Informatyki Stosowanej
Paweª Wawrzy«ski
listopad 2005
Wersja zremasterowana w sierpniu 2009
Spis tre±ci
1 Sformuªowanie problemu i zaªo»enia
3
I Endogeniczne przyspieszenia k¡towe
4
2 Oznaczenia
4
3 Symulacja
5
4 Siªy dziaªaj¡ce na obiekt poza jego dynamik¡, obliczanie Fie
6
5 Iteracja
7
4.1
4.2
4.3
5.1
5.2
Grawitacja . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Przykªadane momenty . . . . . . . . . . . . . . . . . . . . . .
Opór . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Krok 1. Funkcja fei . . . . . . . . . . . . . . . . . . . . . . . . . 8
Krok 2. Funkcja e
ai . . . . . . . . . . . . . . . . . . . . . . . . . 10
6 Przypadki szczególne
6.1
6.2
6.3
6.4
Zª¡cze
Zª¡cze
Zª¡cze
Zª¡cze
7 Zderzenia
7.1
7.2
6
6
7
prowadzone . . . . . . . . . . . . . .
ko«cz¡ce . . . . . . . . . . . . . . . .
poruszaj¡ce si¦ po prostej . . . . . .
ko«cz¡ce i poruszaj¡ce si¦ po prostej
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
10
10
11
12
13
14
Wbicie si¦ w punkt . . . . . . . . . . . . . . . . . . . . . . . . 14
Odrywanie si¦ od punktu . . . . . . . . . . . . . . . . . . . . . 16
8 Wyznaczanie siª powoduj¡cych zadane przyspieszenie
16
II Egzogeniczne przyspieszenia k¡towe
20
9 Zª¡cze aktywne
20
10 Iteracja
22
25
12 Symulacja z ograniczeniami na k¡ty
33
III
40
8.1
8.2
Zale»no±¢ momenty przyspieszenia k¡towe . . . . . . . . . . 18
Wymuszenie przyspieszenia k¡towego . . . . . . . . . . . . . . 18
11.1 Zª¡cze prowadzone i aktywne nast¦pne . . . . . . . . . . . . . 26
11.2 Zª¡cze prowadzone i nieaktywne nast¦pne . . . . . . . . . . . . 29
11.3 Zª¡cze nieprowadzone i nieaktywne nast¦pne . . . . . . . . . . 31
12.1 K¡t traa na ograniczenie, zwarcie . . . . . . . . . . . . . . . 33
12.2 Anomalie . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34
Przykªadowe obiekty
13 Wahadªa z jednym nieruchomym zª¡czem
40
14 Wahadªa z jednym zª¡czem na prostej
44
Literatura
50
13.1 Jedno ogniwo . . . . . . . . . . . . . . . . . . . . . . . . . . . 40
13.2 Dwa ogniwa . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41
13.3 Trzy ogniwa . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42
14.1 Jedno ogniwo . . . . . . . . . . . . . . . . . . . . . . . . . . . 44
14.2 Dwa ogniwa . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46
14.3 Trzy ogniwa . . . . . . . . . . . . . . . . . . . . . . . . . . . . 47
2
1 Sformuªowanie problemu i zaªo»enia
W niniejszym dokumencie zostanie przedstawiony pewien algorytm symulowania pªaskich ªa«cuchów kinematycznych. Jest on oparty na metodzie
Eulera-Newtona. Przyjmuje si¦, »e w ci¡gu kwantu czasu przyspieszenia
w obiekcie s¡ staªe. Istot¡ algorytmu jest zatem znalezienie tych przyspiesze«.
Koszt obliczeniowy tej operacji jest liniowy w liczbie elementów obiektu.
Rozwa»amy pªaski ªa«cuch kinematyczny w postaci pr¦tów (ogniw) poª¡czonych obrotowymi stopniami swobody (zª¡czami). Ogniwa s¡ sztywne,
a caªa masa obiektu jest rozlokowana w zª¡czach. Przeanalizowane s¡ zª¡cza
kilku typów: poruszaj¡ce si¦ bez ogranicze«, poruszaj¡ce si¦ po prostej,
poruszaj¡ce si¦ z zadanym przyspieszeniem. Ponadto k¡t mi¦dzy ogniwami
s¡siaduj¡cymi ze zª¡czem mo»e by¢ staªy lub zmienia¢ stosowanie do indukowanych w ªa«cuchu przyspiesze« zª¡cz.
Napisanie niniejszego dokumentu byªo stymulowane potrzeb¡ sformuªowania metodologii pozwalaj¡cej symulowanie zªo»onych obiektów dynamicznych
do testowania algorytmów uczenia si¦ przez wzmacnianie. Powstaje pytanie
dlaczego tymi obiektami maj¡ by¢ dwuwymiarowe ªa«cuchy kinematyczne.
Czy nie lepiej aby byªy to ogólne trójwymiarowe obiekty? Otó» po pierwsze, symulowanie ogólnych trójwymiarowych ukªadów dynamicznych jest
znacznie bardziej skomplikowane. Po drugie, dla mechanizmu uczenia si¦
przez wzmacnianie nie jest istotne, czy ma on do czynienia z obiektami dwuczy trójwymiarowym o ile ich dynamika ma podobn¡ zªo»ono±¢.
3
Cz¦±¢ I
Endogeniczne przyspieszenia
k¡towe
W Cz¦±ci I b¦dziemy rozwa»a¢ problem symulacji obiektu, w którym nie
istniej¡ ograniczenia na k¡ty mi¦dzy ogniwami. Istnienie takich ogranicze«
tworzy nowe problemy, które przedyskutujemy w Cz¦±ci II.
2 Oznaczenia
Indeksy:
i = 1, 2, . . . , n indeksy zª¡cz,
Staªe:
li odlegªo±¢ mi¦dzy i-tym a i + 1-szym zª¡czem, dªugo±¢ i-tego ogniwa,
mi masa i-tego zª¡cza.
Stan obiektu na pocz¡tku kwantu czasu:
xi poªo»enie i-tego zª¡cza w staªym ukªadzie odniesienia (SUO),
ri jednostkowy wektor wskazuj¡cy kierunek od i-tego do i+1-szego zª¡cza,
vi pr¦dko±¢ i-tego zª¡cza w SUO,
Fie siªa zewn¦trzna, dziaªaj¡ca na zª¡cze poza dynamik¡ obiektu: grawitacja, siªy oporu, przykªadane momenty itd.,
Fid siªa napr¦»enia przykªadana do i-tego zª¡cza poprzez i−1-sze ogniwo;
poniewa» ka»da siªa indukuje siª¦ dziaªaj¡c¡ przeciwnie, wi¦c caªkowita
d
.
siªa dziaªaj¡ca na i-te zª¡cze wynosi Fie + Fid − Fi+1
Funkcje oddziaªywa« w obiekcie:
e
ai (·) wektor przyspieszenia i-tego zª¡cza jako funkcja siªy dziaªaj¡cej
z i − 1-szego zª¡cza,
fei (·) wektor siªy, któr¡ i-te zª¡cze dziaªa na i + 1-sze jako funkcja siªy
dziaªaj¡cej z i − 1-szego zª¡cza. Zachodzi
d
Fi+1
= fei (Fid ).
4
Caªy problem polega w istocie na tym, aby wyprowadzi¢ e
ai oraz fei jako
funkcje liniowe:
e
ai (F ) = Ai F + bi
fei (F ) = Ci F + di
(1)
(2)
B¦dzie si¦ to odbywaªo iteracyjnie: e
ai oraz fei b¦dzie wyznaczane na podstawie e
ai+1 oraz fei+1 .
Rzutowanie wektora na kierunek.
Niech r jest wektorem o jednostkowej dªugo±ci. Wyznacza on pewien
kierunek. Dªugo±¢ rzutu dowolnego wektora y na ten kierunek wynosi
y T r = rT y.
Wart¡ uwagi zale»no±ci¡ jest
(y T r)r = rrT y,
która wyznacza rzut wektora y na wektor jednostkowy r. Pozwala ona
niekiedy zamieni¢ mno»enie przez skalar na mno»enie czysto wektorowe.
3 Symulacja
Celem symulacji jest obliczenie aproksymacji poªo»e« i pr¦dko±ci zª¡cz w dyskretnych momentach czasu 0, δ, 2δ, . . . , gdzie δ to g¦sto±¢ dyskretyzacji czasu.
Poªo»enia i pr¦dko±ci w kolejnej chwili obliczane s¡ w nast¦puj¡cej p¦tli:
1. Dla i = n, n − 1, . . . , 1 wyznacz e
ai , fei czyli Ai , bi , Ci , di .
d
2. Przyjmij F1d = 0 i dla i = 1, 2, . . . , n − 1 oblicz Fi+1
= fe(Fid ).
3. Dla i = 1, 2, . . . , n oblicz
1
xi := xi + δvi + δ 2e
ai (Fid )
2
vi := vi + δe
ai (Fid ).
4. Dla i = 1, 2, . . . , n − 1, oblicz ri := (xi+1 − xi )/kxi+1 − xi k.
5. Ewentualnie popraw bª¦dy numeryczne.
5
Niedokªadno±ci numeryczne mog¡ sprawi¢, »e przestan¡ by¢ speªnione
warunki, które stan obiektu musi speªnia¢, mianowicie
kxi−1 − xi k = li ,
(vi+1 − vi )T (xi+1 − xi ) = 0.
W zasadzie nale»aªoby po ka»dej iteracji przywraca¢ speªnienie powy»szych
równa«. Jest to jednak kosztowne obliczeniowo. Dlatego rozwi¡zaniem, które
zazwyczaj wystarcza jest modykacja xi oraz vi tak aby bª¦dy powy»szych
równo±ci nieco zmniejszy¢ np. w przybli»eniu 2-krotnie. Systematyczne
podej±cie do zagadnienia bª¦dów numerycznych zostanie zaproponowane w sekcji 12.2.
4 Siªy dziaªaj¡ce na obiekt poza jego dynamik¡,
obliczanie Fie
Poza dynamik¡ obiektu istnieje kilka ¹ródeª siª dziaªaj¡cych na i-te zª¡cze.
Te ¹ródªa to grawitacja, przykªadane momenty, opór i by¢ mo»e inne, zaprojektowane przez konstruktora. Fie jest sum¡ takich siª.
4.1 Grawitacja
Siªa grawitacyjna oddziaªuj¡ca i-te zª¡cze wynosi
·
¸
0
−gmi
gdzie g = 9.81ms−2 to ziemskie przyspieszenie grawitacyjne.
4.2 Przykªadane momenty
Zdeniujmy
ri∗
jako taki wektor jednostkowy, »e k¡t mi¦dzy ri∗ a ri wynosi π/2. ri∗ jest to
wi¦c wektor ri skr¦cony o k¡t prosty w lewo. Jednocze±nie zdeniujmy
Li = ri∗ /li .
Przyªo»enie momentu τi do i-tego zª¡cza jest równoznaczne z przyªo»eniem
nast¦puj¡cych siª (Góra Rysunku 1):
1. Do zª¡cza i − 1, siªa ri∗ τi /li = Li−1 τi .
2. Do zª¡cza i, siªa −ri∗ τi /li = −Li−1 τi .
3. Do zª¡cza i, siªa −ri∗ τi /li = −Li τi .
4. Do zª¡cza i + 1, siªa ri∗ τi /li = Li τi .
6
4.3 Opór
Zdeniujmy pr¦dko±¢ k¡tow¡ i-tego ogniwa jako
ωio = (ri∗ )T (vi+1 − vi )/li = LTi (vi+1 − vi ).
Pr¦dko±¢ k¡towa i-tego zª¡cza jest ró»nic¡ pr¦dko±ci k¡towych s¡siednich ogniw i wynosi
o
.
ωiz = ωio − ωi−1
Mo»na rozwa»a¢ siª¦ oporu tocznego zwi¡zan¡ z pr¦dko±ci¡ k¡tow¡ i-tego
zª¡cza. Jest ona równoznaczna przyªo»eniu do tego» zª¡cza momentu o wielko±ci
−kω ωiz
gdzie kω jest pewnym dodatnim skalarem.
Mo»na tak»e rozwa»a¢ opór zwi¡zany z bezwzgl¦dn¡ pr¦dko±ci¡ i-tego
zª¡cza, np. opór powietrza. Siªa taka wynosi
−kv vi kvi k
gdzie kv to dodatni skalar.
5 Iteracja
Caªe poni»sze wyprowadzenie opiera si¦ na poni»szych obserwacjach:
1. Moment przykªadany w zª¡czu mo»na przetªumaczy¢ na zwykªe siªy
dziaªaj¡ce na dane zª¡cze i zª¡cza s¡siednie (Góra Rysunku 1). W zwi¡zku z tym, je±li w zª¡czach przykªadane s¡ znane momenty, mo»emy
stosownie do nich wyznaczy¢ siªy Fid i dalej, niejako, nie zajmowa¢ si¦
tymi momentami.
2. Siªa przykªadana do zª¡cza z jednej strony ukªadu propaguje si¦ na
drug¡ stron¦ jedynie wzdªu» ogniwa z drugiej strony zª¡cza (Dóª Rysunku 1).
d
jed3. i-te zª¡cze dziaªaj¡c poprzez i-te ogniwo na zª¡cze i + 1 siª¡ Fi+1
d
nocze±nie jest przedmiotem dziaªania siªy −Fi+1 (wynika to z III prawa
dynamiki Newtona, Rysunek 2).
Powiedzmy zatem, »e znamy funkcj¦
d
d
+ bi+1
) = Ai+1 Fi+1
e
ai+1 (Fi+1
Zadanie polega na wyznaczeniu funkcji e
ai (1) oraz fei (2), których argumentem
d
jest wektor Fi oznaczaj¡cy arbitraln¡ siª¦ dziaªaj¡c¡ na zª¡cze i. Najpierw
zostanie wyznaczona funkcja fei a potem e
ai .
7
Rysunek 1: Siªy dziaªaj¡ce na zª¡cza. Góra: Przyªo»enie momentu siªy do
i-tego zª¡cza jest równoznaczne z przyªo»eniem zwykªych siª do danego
zª¡cza i s¡siadów. Siªy te, wraz siªami grawitacyjnymi skªadaj¡ si¦ na siªy
zewn¦trzne oznaczane przez F e . Dóª: Poza siªami z zewn¡trz (grawitacj¡
i przykªadanymi momentami), zª¡cza zachowuj¡ si¦ jakby byªy zupeªnie swobodne. Oznacza to, »e siªa przyªo»ona do i − 1-szego zª¡cza oddziaªuje na
i-te zª¡cze jedynie skªadow¡ równolegª¡ do i − 1-szego ogniwa.
5.1 Krok 1. Funkcja fei .
d
Siªa Fi+1
z jak¡ i-te zª¡cze dziaªa na i + 1-sze jest równolegªa do ri i wynosi
d
= ri φi .
Fi+1
gdzie φi to pewien skalar, którego b¦dziemy teraz poszukiwa¢ jako funkcji
argumentu Fid . Przyjmijmy dla uproszczenia oznaczenie
Fid = F.
Dªugo±¢ rzutu przyspieszenia i-tego zª¡cza na kierunek ri wynosi (Rysunek
2)
d
F + Fie − Fi+1
F + Fie − ri φi
riT
= riT
.
mi
mi
8
Rysunek 2: Siªy dziaªaj¡ce na i-te zª¡cze.
Dªugo±¢ rzutu przyspieszenia i + 1-szego zª¡cza na ri wynosi za±
¡
¢
d
riT Ai+1 Fi+1
+ bi+1 = riT (Ai+1 ri φi + bi+1 ) .
Ró»nica pr¦dko±ci s¡siednich zª¡czy sprawia, »e poruszaj¡ si¦ one wzgl¦dem siebie z przyspieszeniem wzdªu» ª¡cz¡cego je ogniwa. Dªugo±¢ rzutu
tej ró»nicy przyspiesze« na ri wynosi1
ei = kvi+1 − vi k2 /li .
(3)
Poni»ej zapiszemy, kluczowe dla naszych rozwa»a«, równanie ogniwa. Jest
ono konsekwencj¡ faktu, »e dªugo±¢ ogniwa pozostaje staªa. W zwi¡zku z tym
(i) ró»nica vi+1 − vi jest prostopadªa do ri , oraz (ii) dªugo±¢ rzutu ró»nicy
przyspiesze« s¡siednich zª¡cz na ri jest równa ei . Mamy
riT
F + Fie − ri φi
− riT (Ai+1 ri φi + bi+1 ) = ei .
mi
(4)
Z równania ogniwa wynika natychmiast
φi =
riT (F + Fie )/mi − riT bi+1 − ei
.
riT ri /mi + riT Ai+1 ri
1 Krótkie
(5)
preliminaria zyczne: Powiedzmy, »e punkt porusza si¦ po okr¦gu o promieniu
l, przy czym warto±¢ bezwzgl¦dna jego pr¦dko±ci nie zmienia si¦ i wynosi kvk. Jakie jest
przyspieszenie takiego punktu? Otó» jest ono skierowane do ±rodka okr¦gu a jego wielko±¢
wynosi kvk2 /l. Wzór (3) jest prostym rozszerzeniem tych rozwa»a« na sytuacj¦, w której
dwa punkty poruszaj¡ si¦ wzgl¦dem siebie ale odlegªo±¢ mi¦dzy nimi jest staªa.
9
d
Bior¡c pod uwag¦, »e Fi+1
= ri φi oraz riT ri = 1, powy»szy wyraz deniuje
funkcj¦ fei , mianowicie
ri riT
1 + mi riT Ai+1 ri
ri riT Fie − mi ri (riT bi+1 + ei )
mi ri (riT bi+1 + ei )
e
di =
=
C
F
−
.
i i
1 + mi riT Ai+1 ri
1 + mi riT Ai+1 ri
Ci =
(6)
(7)
5.2 Krok 2. Funkcja eai .
Aby wyznaczy¢ funkcj¦ e
ai , zauwa»amy, »e przyspieszenie i-tego zª¡cza wynosi
F + Fie − ri φi
m
µ i
¶
I
ri riT /mi
=
−
F
mi riT ri + mi riT Ai+1 ri
µ e
¶
Fi
ri riT Fie /mi − ri (riT bi+1 + ei )
+
−
.
mi
riT ri + mi riT Ai+1 ri
A zatem funkcj¦ e
ai deniuj¡ macierz i wektor
I
ri riT /mi
−
mi 1 + mi riT Ai+1 ri
ri (riT bi+1 + ei )
Fie ri riT Fie /mi − ri (riT bi+1 + ei )
e
−
= Ai Fi +
.
bi =
mi
1 + mi riT Ai+1 ri
1 + mi riT Ai+1 ri
Ai =
(8)
(9)
W niniejszej sekcji rozwa»ymy zª¡cza, które s¡ w jakim± sensie wyj¡tkowe.
6.1 Zª¡cze prowadzone
W niniejszym podrozdziale zajmiemy si¦ zª¡czem, którego przyspieszenie jest
okre±lone poza ukªadem. Zª¡cze takie b¦dziemy nazywali prowadzonym. Najprostszy przykªad to zª¡cze nieruchome, np. tkwi¡ce sztywno w jakim± obiekcie. Wyniki, które tu wyprowadzimy posªu»¡ tak»e do modelowania takich
zjawisk jak zderzenia zª¡cz z innymi obiektami.
Niech wymuszon¡ warto±ci¡ przyspieszenia b¦dzie aw
i . Oczywi±cie mamy
i
wobec
tego
e
ai ≡ aw
i
Ai = 0
bi = aw
i .
10
Jednocze±nie pozostaje w mocy zale»no±¢ mi¦dzy przyspieszeniami s¡siednich
zª¡cz, mianowicie dªugo±¢ rzutu ich ró»nicy na ri musi by¢ równa ei , czyli
T
d
riT aw
i − ri (Ai+1 Fi+1 + bi+1 ) = ei .
d
Poniewa» Fi+1
równa si¦, jak dotychczas, φi ri , to ªatwo wywie±¢, »e
d
= ri
Fi+1
T
riT aw
i − ri bi+1 − ei
riT Ai+1 ri
A wobec tego
Ci = 0
di =
T
ri (riT aw
i − ri bi+1 − ei )
.
riT Ai+1 ri
Nast¦pne zª¡cze prowadzone lub zª¡cze ko«cz¡ce
Powstaje pytanie co si¦ dzieje je±li prowadzone jest zª¡cze i-te oraz i + 1sze. Ich przyspieszenia mog¡ wówczas pozostawa¢ w konikcie w tym sensie,
»e mog¡ generowa¢ zmian¦ dªugo±ci i-tego ogniwa. Problem ten zmusza
nas do okre±lenia priorytetów: czy, w razie koniktu, wa»niejsze jest wymuszone przyspieszenie czy dªugo±¢ ogniwa. B¦dziemy zakªadali, »e to pierwsze.
Dla i-tego zª¡cza prowadzonego, je±li zª¡cze i + 1-sze jest tak»e prowadzone,
b¦dziemy przyjmowali
di = 0.
Oczywi±cie b¦dziemy post¦powali podobnie je±li i + 1-szego zª¡cza nie ma
wcale czyli zª¡cze i-te jest ko«cz¡ce.
6.2 Zª¡cze ko«cz¡ce
Zª¡cze i-te rozumiemy jako ko«cz¡ce wówczas gdy nie istnieje i-te ogniwo.
Wtedy oczywi±cie nie ma sensu mówi¢ o sile dziaªaj¡cej na i + 1-sze zª¡cze
i mo»na ±miaªo przyj¡¢
fei ≡ 0,
Ci = 0,
di = 0.
Tymczasem przyspieszenie i-tego zª¡cza wynosi
e
ai (F ) =
a zatem
Ai =
F + Fie
,
mi
I
,
mi
11
bi =
Fie
.
mi
6.3 Zª¡cze poruszaj¡ce si¦ po prostej
W niniejszej podsekcji analizujemy dynamik¦ zª¡cza, którego ruch jest ograniczony do prostej. Takie zª¡cze mo»e by¢ modelem cz¦±ci obiektu, która je¹dzi,
czy ±lizga si¦, po pewnym torze.
Niech i-te zª¡cze porusza si¦ po prostej prostopadªej do jednostkowego
wektora si .2 Zª¡cze jest utrzymywane na prostej dzi¦ki dodatkowej sile
Fis = si ζi
prostopadªej do prostej; ζi jest skalarem. Wektor przyspieszenia i-tego zª¡cza
jest wyznaczany przez wyra»enie
d
Fie + F − Fi+1
+ Fis
F e + F − ri φi + si ζi
= i
.
mi
mi
d
Poszukujemy teraz dwóch siª: Fis oraz Fi+1
. Wynikaj¡ one z rozwi¡zania
nast¦puj¡cego ukªadu równa«
(
F e +F −ri φi +si ζi
− riT (Ai+1 ri φi + bi+1 ) = ei
riT i
mi
e
(10)
F +F −ri φi +si ζi
=
0.
sTi i
mi
Pierwsze z powy»szych to równanie ogniwa odpowiadaj¡ce (4) a drugie mówi
tylko tyle, »e przyspieszenie i-tego zª¡cza jest prostopadªe do si . Je±li ksi k =
1, z drugiego równania dostajemy
ζi
ri φi − Fie − F
= sTi
mi
mi
co pozwala na eliminacj¦ ζi z ukªadu i wyznaczenie φi jako
φi =
riT (I − si sTi )(Fie + F ) − mi (riT bi+1 + ei )
.
mi riT Ai+1 ri + 1 − (sTi ri )2
d
Skalar φi okre±la Fi+1
, zatem z powy»szego wynika natychmiast funkcja fei :
ri riT (I − si sTi )
ri riT (I − si sTi )Fie − mi (riT bi+1 + ei )ri
di =
mi (riT bi+1 + ei )ri
= Ci Fie −
.
Ci =
2W
przestrzeni 2-wymiarowej mo»na mówi¢ o prostej prostopadªej do wektora. Gdyby
liczba wymiarów byªa wi¦ksza mówiliby±my raczej o ruchu zª¡cza po pewnej hiperpªaszczy¹nie prostopadªej do si .
12
Z równa«
Fie + F − ri φi + si ζi
= Ai F + bi
mi
wynika denicja funkcji e
ai , mianowicie
e
ai (F ) =
I − si sTi
(I − si sTi )ri riT (I − si sTi )/mi
−
mi
(I − si sTi )Fie (I − si sTi )ri riT (I − si sTi )Fie /mi − (riT bi+1 + ei )(I − si sTi )ri
−
bi =
mi
(riT bi+1 + ei )(I − si sTi )ri
e
= Ai Fi +
.
Ai =
6.4 Zª¡cze ko«cz¡ce i poruszaj¡ce si¦ po prostej
W niniejszej podsekcji modelujemy dynamik¦ zª¡cza poruszaj¡cego si¦ po
prostej, za którym nie ma ju» »adnego innego zª¡cza.
Niech i-te zª¡cze porusza si¦ po prostej prostopadªej do jednostkowego
wektora si . Jak w poprzedniej sekcji, zª¡cze jest utrzymywane na prostej
dzi¦ki dodatkowej sile
Fis = si ζi
prostopadªej do prostej; ζi jest skalarem. Wektor przyspieszenia i-tego zª¡cza
jest wyznaczany przez wyra»enie
Fie + F + Fis
Fie + F + si ζi
=
mi
mi
przy czym jest to wektor prostopadªy do si , czyli
sTi
Fie + F + si ζi
= 0.
mi
Z powy»szego wynika, »e
e
ζi
T Fi + F
= −si
mi
mi
sk¡d dostajemy
ai =
(I − si sTi )F + (I − si sTi )Fie
Fie + F + si ζi
=
.
mi
mi
A zatem
I − si sTi
mi
(I − si sTi )Fie
bi =
= Ai F e .
mi
Ai =
13
Oczywi±cie
fei ≡ 0,
Ci = 0,
di = 0.
7 Zderzenia
W rozdziale omówimy co si¦ dzieje kiedy zª¡cze zderza si¦ z jakim± nieruchomym przedmiotem lub odrywa si¦ od niego po zderzeniu, które miaªo
miejsce wcze±niej. B¦dziemy zakªadali, »e zderzenia nie s¡ spr¦»yste, a zatem zª¡cze, niejako, wbija si¦ w punkt, w którym styka si¦ z przedmiotem.
7.1 Wbicie si¦ w punkt
Rozwa»amy teraz sytuacj¦, w której i-te zª¡cze wbija si¦ w pewien punkt.
B¦dzie to oznaczaªo, »e zª¡cze porusza si¦ do punkt z pewn¡ niezerow¡ pr¦dko±ci¡ by chwil¦ pó¹niej znajdowaªo si¦ w tym punkcie i jego pr¦dko±¢ byªa
zerowa. To co si¦ dzieje w mi¦dzyczasie b¦dziemy rozumieli w ten sposób,
»e w ci¡gu bardzo krótkiego kwantu czasu na to zª¡cze dziaªa siªa, która
wyhamowuje jego ruch. Powiedzmy, »e dªugo±¢ kwantu czasu to ² a hamuj¡ce przyspieszenie zª¡cza jest staªe. Je±li pierwotna pr¦dko±¢ wynosi vi , to
wektor przyspieszenia jest równy
aw
i = −vi /².
W dalszej analizie zastanawiamy si¦ co si¦ dzieje kiedy ² d¡»y do zera. Poj¦cie
o tym daje zmiana skali czasu. Powiedzmy, »e jednostk¡ czasu jest teraz
² sekundy. Warto±¢ pr¦dko±ci k -tego zª¡cza wyra»ana dotychczas w [m/s]
wynosi teraz vk0 = ²vk . Przyspieszenia do±rodkowe, mierzone dotychczas w
[m/s2 ] wynosi e0k = ²2 ek , za± siªa dziaªaj¡ca poza dynamik¡ obiektu, mierzona
dotychczas w [kg · m/s2 ] wynosi teraz Fke 0 = ²2 Fke .
W zmienionej skali czasu jedyn¡ wielko±ci¡ nie zmnimalizowan¡ przez
² jest wymuszone zderzeniem przyspieszenie i-tego zª¡cza, którego warto±¢
wynosi teraz −vi . Dla dostatecznie maªej wielko±ci ² warto±ci e0k oraz Fke 0
w formuªach na Ak , bk , Ck , dk b¦d¡ nie do odró»nienia od bª¦du numerycznego
(inne warto±ci w formuªach b¦d¡ relatywnie tak du»e). A zatem, im krótszy
jest czas wbijania si¦ zª¡cza w punkt, tym bardziej pomijalne s¡ siªy dziaªaj¡ce na zª¡cza poza dynamik¡ ukªadu oraz pr¦dko±ci zª¡cz. Mo»na wi¦c
zastosowa¢ nast¦puj¡c¡ metod¦ przeliczania konsekwencji wbicia si¦ i-tego
zª¡cza:
e
Z1. Przyjmij aw
i = −vi oraz Fk = 0, ek = 0 dla k = 1, . . . , n (przej±cie na
skal¦ czasu o jednostce ² d¡»¡cym do zera).
14
Z2. Oblicz Ak , bk , Ck , dk a nast¦pnie przyspieszenia ak dla k = 1, . . . , n
zgodnie z dotychczas wyprowadzonymi wzorami.
Z3. (Powrót do pierwotnej skali czasu) Oblicz vk := vk +ak dla k = 1, . . . , n.
Wbicie si¦ zª¡cza w punkt nast¦puje w pewnym momencie mi¦dzy dyskretnymi chwilami czasu symulacji. Na ogóª ªatwo jest wykry¢ post factum, »e
do wbicia si¦ doszªo. W tej sytuacji nale»y wykona¢ nast¦puj¡ce czynno±ci:
a) cofn¡¢ si¦ w czasie o do chwili wbicia si¦, powiedzmy, »e staªo si¦ to
wcze±niej o ²,
b) wyznaczy¢ pr¦dko±ci po zderzeniu, np. wg. kroków Z1, Z2, Z3,
c) unieruchomi¢ zª¡cze, które si¦ wbiªo, czyli przyj¡¢ aw
i = 0,
d) przesymulowa¢ wydarzenia, które nast¦puj¡ potem w ci¡gu czasu o dªugo±ci ².
Kiedy doszªo do wbicia si¦? Powiedzmy, »e tym w co mo»e wbi¢ si¦ zª¡cze
jest prosta zaczepiona w punkcie z 0 prostopadªa do wektora z .3 Powiedzmy,
»e z wskazuje t¡ dobr¡ stron¦ prostej, po której zª¡cze mo»e si¦ bez przeszkód porusza¢. Do wbicia si¦ dochodzi kiedy przeliczamy wspóªrz¦dne zgodnie
z krokami przedstawionymi w sekcji 3 i okazuje si¦, »e
z T (xi − z 0 ) < 0.
Chcemy obliczy¢ dodatni¡ wielko±¢ ², która mówi jak dawno temu doszªo do
wbicia si¦. Speªnia ona zale»no±¢
z T (xi − z 0 − ²vi − 0.5²2 ai ) = 0,
w której ai jest przyspieszeniem zª¡cza obliczonym przed wbiciem si¦, za± vi
oraz xi to, odpowiednio, pr¦dko±¢ i poªo»enie zª¡cza po kwancie czasu (czyli
ju» po wbiciu si¦). Powy»sze równanie ma dwa rozwi¡zania:
√
√
T
T
−z
v
+
−z
v
−
∆
∆
i
i
,
²00 =
,
²0 =
T
T
z ai
z ai
gdzie
∆ = (z T vi )2 − 2z T ai z T (z 0 − xi ).
Uwa»na analiza ujawnia, »e dobrym rozwi¡zaniem jest jednak tylko ² = ²00 .
3 Podobnie
jak w przypadku poruszania si¦ po prostej, w dwóch wymiarach mamy do
czynienia z prost¡ prostopadª¡ do wektora. Gdyby za± liczba wymiarów byªa wi¦ksza byªaby to hiperpªaszczyzna.
15
7.2 Odrywanie si¦ od punktu
Procesem odwrotnym do wbijania si¦ w punkt jest odrywanie si¦ od punktu.
Ró»ne zjawiska mog¡ temu towarzyszy¢, np. zª¡cze mogªo, w jakim± sensie,
ugrz¦zn¡¢ w punkcie. Zajmiemy si¦ najprostszym wariantem polegaj¡cym na
tym, »e zª¡cze jest wbite w powierzchni¦ i odrywa si¦ od niej je±li tylko suma
siª dziaªaj¡cych na nie odpycha zª¡cze od powierzchni. Je±li powierzchnia jest
wyznaczana przez wektory z 0 (zaczepienie) i z (wektor prostopadªy) a suma
siª wynosi Fi , to oderwanie si¦ nast¦puje wówczas gdy
z T Fi > 0.
8 Wyznaczanie siª powoduj¡cych zadane
przyspieszenie
W niniejszej sekcji przeanalizujemy szczegóªowo zale»no±¢ mi¦dzy siªami Fje
a przyspieszeniami ai dla dowolnych i, j . Znajomo±¢ tej zale»no±ci pozwala
w szczególno±ci wyznacza¢ takie siªy aby uzyskiwa¢ zadane przyspieszenia.
Umo»liwia tak»e zadawanie momentów siª aby uzyskiwa¢ zadane przyspieszenia k¡towe, cho¢ akurat ten problem zostanie lepiej rozwi¡zany w Cz¦±ci II
dokumentu.
Punktem wyj±cia do rozstrzygni¦cia jak Fje wpªywa na ai b¦dzie wyprowadzenie ddFaie , przy czym wyraz ten b¦dzie rozumiany jako macierz Jakobiego
j
speªniaj¡ca równanie
dai
∆ai =
∆Fje
dFje
w jego najbardziej oczywistym rozumieniu.
Rysunek 3 pokazuje kolejno±¢ dziaªa« prowadz¡cych od Fje do ai . Z zestawienia tego wynika natychmiast sposób obliczania dai /dFje oparty na propagacji wstecznej:
1. Oblicz
∂ai
dai
=
= Bi .
d
dF i
∂Fid
2. Dla k = n, n − 1, . . . , i,
dai
dFkd
= 0 za± dla k = i − 1, i − 2, . . . , 2 oblicz
d
dai
dai ∂Fk+1
dai
=
=
Ck .
d
d
d
d
dF k
dFk+1 ∂Fk
dFk+1
16
Rysunek 3: Kolejno±¢ dziaªa« prowadz¡cych od Fje do ai .
3. Dla k = n, n − 1, . . . , i,
dai
ddk
= 0, za± dla k = i − 1, i − 2, . . . , 1 oblicz
d
dai
dai ∂Fk+1
dai
.
=
=
d
d
ddk
dFk+1 ∂dk
dFk+1
4. Dla k = 1, 2, . . . , j , oblicz
dai
dai ∂dk−1
dai ∂bk−1
∂ak
= [k > 1]
+ [k > 1]
+ [k = i]
.
db k
ddk−1 ∂bk
dbk−1 ∂bk
∂bk
Pewnym uproszczeniem jest fakt, »e ∂ak /∂bk = I , za± dla wszystkich
dotychczas rozwa»anych przypadków zachodzi ∂bk−1 /∂bk = Ck−1 .
5. Oblicz
dai ∂dj
dai ∂bj
da i
da i
da i
=
+
=
Cj +
Aj .
e
e
e
dF j
ddj ∂Fj
dbj ∂Fj
ddj
dbj
Warto zauwa»y¢, »e aby wyznaczy¢ komplet warto±ci dai /dFje dla j =
1, . . . , n, nie potrzeba powtarza¢ caªej procedury n-krotnie. Wystarczy, »e
krok 4 zostanie wykonany dla j = n i jedynie krok 5 zostanie wykonany
n-krotnie dla j = 1, 2, . . . , n.
17
8.1 Zale»no±¢ momenty przyspieszenia k¡towe
Niekiedy interesuj¡ce jest obliczenie pochodnej przyspieszenia k¡towego itego zª¡cza po j -tym momencie przykªadanym do i-tego zª¡cza. Pami¦tamy,
»e
αiz = LTi ai+1 − (Li + Li−1 )T ai + LTi−1 ai−1 .
e
e
,
, Fje i Fj+1
Droga wynikania jest tu nast¦puj¡ca: Moment τj wpªywa na Fj−1
z
które wpªywaj¡ na ai−1 , ai oraz ai+1 , które z kolei wpªywaj¡ na αi . Ze wzorów
na siªy wynikaj¡ce z przykªadanych momentów (Rozdziaª 4) ªatwo wynika,
i»
µ
¶
dαiz
dai−1
dai−1
dai−1
T
= Li−1
Li−1 −
(Li−1 + Li ) +
Li
e
e
dτj
dFi−1
dFie
dFi+1
µ
¶
dai
dai
da i
T
− (Li + Li−1 )
Li−1 −
(Li−1 + Li ) +
Li
e
e
dFi−1
dFie
dFi+1
µ
¶
dai+1
dai+1
dai+1
T
+ Li
Li−1 −
(Li−1 + Li ) +
Li .
e
e
dFi−1
dFie
dFi+1
8.2 Wymuszenie przyspieszenia k¡towego
W podsekcji 6.1 zajmowali±my si¦ wymuszaniem przyspieszenia i-tego zª¡cza.
Niniejsza podsekcja zawiera analogiczny materiaª z tym, »e teraz rozwa»amy
zestaw momentów siª wymuszaj¡cy odpowiednie przyspieszenia k¡towe. Innemu rozwi¡zaniu tego samego problemu b¦dzie po±wi¦cona Cz¦±¢ II niniejszego dokumentu.
Rozwa»my zª¡cza i1 , i2 , . . . , im . Chcemy, aby ich przyspieszenia k¡towe
wynosiªy αiw1 , αiw2 , . . . , αiwm . Zajmiemy si¦ zagadnieniem wyznaczenia momentów siªy τi1 , τi2 , . . . , τim tak, aby osi¡gn¡¢ zadane przyspieszenia k¡towe. Dzi¦ki wyprowadzeniom z poprzedniej podsekcji wiemy w jaki sposób zmiany
przykªadanych momentów skutkuj¡ zmianami przyspiesze« k¡towych. Z ksztaªtu odpowiednich pochodnych wiemy, »e zale»no±ci te s¡ w ka»dej chwili
liniowe. Pozwala to sformuªowa¢ nast¦puj¡c¡ procedur¦ wyznaczania momentów τi1 , τi2 , . . . dla znanych przyspiesze« k¡towych.
1. Przy pomocy standardowej procedury (Rozdziaª 3) wyznacz przyspieszenia zª¡cz a nast¦pnie przyspieszenia k¡towe αiz1 , . . . , αizn−1 przy zaªo»eniu, »e nie s¡ przykªadane dodatkowe momenty.
18
2. Przy pomocy metody z poprzedniej podsekcji wyznacz macierz
 dαz
dαzi 
i1
1
.
.
.
dτi1
dτim
z

dα
..
.. 
..
=
.
. 
 .z

dτ
dαim
dαzim
. . . dτi
dτi
1
m
3. Wyznacz momenty τi1 , . . . , τim rozwi¡zuj¡c równanie
£
αiw1 . . . αiwm
¤T
£
¤T d α z
= αiz1 . . . αizm +
[τi1 . . . τim ]T .
dτ
4. Wyznacz przyspieszenia zª¡cz ponownie, tym razem przykªadaj¡c momenty τi1 , . . . , τim .
Kluczowym zastosowaniem dla wymuszania przyspieszenia k¡towego zª¡cza jest sytuacja, w której k¡t zª¡cza zbli»a si¦ do swojego ograniczenia,
np. maksymalnego wygi¦cia stawu. W takim razie jego pr¦dko±¢ k¡towa
jest wyzerowana (zajmiemy si¦ w tym w kolejnej podsekcji) i taka pozostaje.
Gdy k¡ty s¡ utrzymywane na swoich ograniczeniach, zale»y nam aby αiwm =
· · · = αiwm = 0. W Cz¦±ci II zajmiemy si¦ tymi zagadnieniami dokªadniej.
19
Cz¦±¢ II
Egzogeniczne przyspieszenia
k¡towe
W Cz¦±ci I jedynie wspomniane zostaªy zjawiska, które wymuszaj¡ okre±lone
przyspieszenia k¡towe. Tymczasem zjawiska takie s¡ bardzo typowe. Dla
przykªadu zaªó»my, »e ogniwa i zª¡cza modeluj¡, odpowiednio, ko±ci i stawy.
Stawy nie mog¡ obraca¢ si¦ w sposób nieograniczony. Kiedy k¡t mi¦dzy ogniwami (ko±¢mi) dochodzi do swojej granicznej warto±ci, pojawia si¦ wymuszone przyspieszenie k¡towe (dziaªaj¡ ±ci¦gna), które wyhamowuje jego dalszy obrót. Kiedy nast¦pnie k¡t ten pozostaje na granicy, pojawia si¦ wymuszone przyspieszenie k¡towe o zerowej warto±ci (znowu ±ci¦gna). W Cz¦±ci
II zajmiemy si¦ wi¦c symulacj¡ uwzgl¦dniaj¡c¡, »e w niektórych zª¡czach
obiektu istniej¡ wymuszone przyspieszenia k¡towe.
9 Zª¡cze aktywne
Problem omawiany w Cz¦±ci II jest jedynie wzbogacon¡ wersj¡ problemu
dyskutowanego w Cz¦±ci I. Zdecydowana wi¦kszo±¢ oznacze« jest taka jak
poprzednio. Znaczenie wyrazów i, n, li , mi , xi , ri , vi , Fie pozostaje. W stosunku do problemu omawianego w Cz¦±ci I, rozwa»amy tutaj jeden dodatkowy element. Mianowicie, w kwancie czasu istnieje zestaw indeksów
i1 , . . . , im takich, »e przyspieszenia k¡towe zª¡cz wskazywanych przez te indeksy jest dane i wynosi αiw1 , . . . , αiwm . Zª¡cze, którego przyspieszenie k¡towe
jest ustalone b¦dzie nazywane aktywnym.
Zdeniujmy teraz dokªadnie przyspieszenia k¡towe. Przypomnijmy, »e
wektor ri∗ zostaª zdeniowany jako wektor jednostkowy powstaªy z ri przez
skr¦cenie w lewo. Stosowane byªo tak»e oznaczenie
Li = ri∗ /li .
(11)
Przyspieszenie k¡towe i-tego ogniwa deniujemy jako
αio = (ri∗ )T (ai+1 − ai )/li = LTi (ai+1 − ai ).
Tymczasem przyspieszenie k¡towe i-tego zª¡cza to ró»nica przyspiesze« k¡towych s¡siaduj¡cych ogniw:
o
.
αiz = αio − αi−1
20
(12)
Zachodzi
αiz = LTi ai+1 − (Li + Li−1 )T ai + LTi−1 ai−1
(13)
Zaªó»my zatem, »e przyspieszenie k¡towe i-tego zª¡cza jest dane i wynosi
Sposobem na jego wymuszenie b¦dzie pojawienie si¦ tam wymuszaj¡cego
momentu τiw . Aby uwzgl¦dni¢ τiw , musimy nieco inaczej ni» w Cz¦±ci I spojrze¢ na dynamik¦ ukªadu. Do ustalenia co si¦ dzieje ze zª¡czem i + 1-szym
nie wystarczy nam wiedza o sumie siª dziaªaj¡cych na zª¡cze i-te. Potrzebna
jest nam tak»e wiedza o momencie indukowanym w tym zª¡czu.
W ogólno±ci, b¦dziemy rozwa»ali nast¦puj¡cy model propagacji dynamiki
obiektu. B¦dzie to komplet czterech funkcji opisuj¡cych dziaªanie i-tego ogniwa. Argumentem wszystkich tych funkcji jest siªa przykªadana do i-tego
ogniwa i indukowany tam moment.
αiw .
• Przyspieszenie i-tego zª¡cza:
w
ai = e
ai (Fid , τiw ) = Ai Fid + bw
i τi + bi .
• Przyspieszenie k¡towe i-tego ogniwa:
αio = α
eio (Fid , τiw ) = pTi Fid + qiw τiw + qi .
• Siªa, któr¡ i-te zª¡cze dziaªa na i + 1-sze poprzez i-te ogniwo:
d
w
Fi+1
= fei (Fid , τiw ) = Ci Fid + dw
i τi + di .
• Moment wymuszaj¡cy w i + 1-tym zª¡czu:
w
w
τi+1
= gei (Fid , τiw ) = giT Fid + hw
i τi + hi .
Obliczanie przyspiesze« w danym kwancie czasu zachodzi wedªug nast¦puj¡cej procedury:
w
1. Dla i = n, n−1, . . . , 1 wyznacz gei , fei , e
ai oraz α
eio czyli gi , hw
i , h i , C i , di , di ,
w
A i , bw
i , bi oraz pi , qi , qi .
2. Przyjmij zerowe F1d i τiw . Dla i = 1, 2, . . . , n − 1 oblicz
d
= fei (Fid , τiw )
Fi+1
w
τi+1
= gei (Fid , τiw )
3. Dla i = 1, 2, . . . , n oblicz
ai = e
ai (Fid , τiw ).
21
10 Iteracja
Przeanalizujemy sposób wyznaczania funkcji gei , fei , α
eio oraz e
ai w najbardziej
typowej sytuacji gdy zª¡cze i-te nie jest prowadzone za± zª¡cze i + 1-sze
istnieje i jest aktywne. Powiedzmy zatem, »e znamy funkcje
w
d
w
d
+ bw
) = Ai+1 Fi+1
, τi+1
e
ai+1 (Fi+1
i+1 τi+1 + bi+1 ,
o
d
w
d
w
w
α
ei+1
(Fi+1
, τi+1
) = pTi+1 Fi+1
+ qi+1
τi+1
+ qi+1 .
Poszukujemy funkcji opisuj¡cych dynamik¦ i-tego zª¡cza, których argumentami s¡ wektory Fid , τiw wyra»aj¡ce arbitraln¡ siª¦ i moment dziaªaj¡ce na
zª¡cza i-te. Oznaczmy
F = Fid ,
τ = τiw .
Rozwa»my siª¦ napr¦»enia oddziaªuj¡ce na i + 1-sze zª¡cze poprzez i-tego
ogniwo. Jest to wypadkowa dwóch siª: (a) równolegªej do ri , odpowiedzialnej za to, »e zª¡cza i-te oraz i + 1-sze nie zmieniaj¡ odlegªo±ci od siebie,
(b) równolegªej do ri∗ , zwi¡zanej z pojawieniem si¦ momentów wymuszaj¡w
cych τiw oraz τi+1
. Ostatecznie
d
w
w
Fi+1
= ri φi + (ri∗ /li )(τiw − τi−1
) = ri φi + Li (τ − τi+1
).
w
gdzie φi oraz τi+1
to pewne skalary, które teraz musimy wyznaczy¢.
Wyznaczmy caªkowit¡ siª¦ Fi dziaªaj¡c¡ na i-te ogniwo. Jest to suma siªy
zewn¦trznej i siª przenoszonych przez s¡siednie ogniwa. Zatem
d
Fi = Fie + Fid − Fi+1
w
=Fie + F − ri φi − Li τ + Li τi+1
w
Do wyznaczenia φi oraz τi+1
posªu»y nam ukªad dwóch równa«. Pierwsze
z nich, równanie ogniwa, mówi, »e zª¡cza i-te oraz i + 1-sze nie oddalaj¡ ani
nie przybli»aj¡ si¦ do siebie.
ei = riT (ai − ai+1 )
¢
¡
w
d
)
, τi+1
ai+1 (Fi+1
= riT Fi /mi − e
³ 1 ¡
¢
w
= riT
Fie + F − ri φi − Li τ + Li τi+1
mi
¢´
¡
w
w
.
− Ai+1 (ri φi + Li τ − Li τi+1
) + bw
τ
+
b
i+1
i+1 i+1
22
Warto±¢ ei = kvi+1 − vi k2 /li to przyspieszenie do±rodkowe zª¡cz. Je±li przyjmiemy oznaczenia
k0 = −1/mi − riT Ai+1 ri
riT Ai+1 Li
k1 = 0 +
k2 = ri /mi
−
(14)
riT bw
i+1
(15)
(16)
k3 = 0 − riT Ai+1 Li
k4 =
riT Fie /mi
−
(17)
riT bi+1
(18)
− ei ,
to otrzymamy równanie
w
+ kT2 F + k3 τ + k4 .
0 = k0 φi + k1 τi+1
Drugie równanie wynika z (12) i wyznacza pr¦dko±¢ k¡tow¡ i-tego zª¡cza
tak, aby byªa ona równa zadanej warto±ci αiw . Mamy
w
o
αi+1
=αi+1
− αio
o
d
w
d
w
=e
αi+1
(Fi+1
, τi+1
) − LTi (e
ai+1 (Fi+1
, τi+1
) − ai )
d
w
w
d
w
=pTi+1 Fi+1
+ qi+1
τi+1
+ qi+1 − LTi (Ai+1 Fi+1
+ bw
i+1 τi+1 + bi+1 − Fi /mi )
w
w
w
=pTi+1 (ri φi + Li τ − Li τi+1
) + qi+1
τi+1
+ qi+1
w
w
− LTi (Ai+1 (ri φi + Li τ − Li τi+1
) + bw
i+1 τi+1 + bi+1 )
¢
1 ¡ e
w
Fi + F − ri φi − Li τ + Li τi+1
+ LTi
mi
Je±li przyjmiemy oznaczenia
k5 = pTi+1 ri − LTi Ai+1 ri − 0
−pTi+1 Li
k6 =
k7 = Li /mi
+
w
qi+1
+
LTi Ai+1 Li
(19)
−
LTi bw
i+1
2
+ kLi k /mi
(20)
(21)
k8 = pTi+1 Li − LTi Ai+1 Li − kLi k2 /mi
(22)
w
k9 = qi+1 − LTi bi+1 + LTi Fie /mi − αi+1
,
(23)
dostaniemy równanie
w
+ kT7 F + k8 τ + k9
0 = k5 φi + k6 τi+1
i, w rezultacie, ukªad równa«
¸
¸ · T
¸·
·
k2 F + k3 τ + k4
φi
k0 k1
,
+ T
0=
w
k7 F + k8 τ + k9
k5 k6 τi+1
23
którego rozwi¡zaniem jest
·
¸
·
¸−T · T
¸
φi
k0 k1
k2 F + k3 τ + k4
=−
w
τi+1
k5 k6
kT F + k8 τ + k9
· 7T
¸
1
k6 (k2 F + k3 τ + k4 ) − k1 (kT7 F + k8 τ + k9 )
=
k1 k5 − k0 k6 k0 (kT7 F + k8 τ + k9 ) − k5 (kT2 F + k3 τ + k4 )
· T ¸
· ¸
· ¸
k12
k14
k16
= T F+
τ+
k13
k15
k17
gdzie
k11
k12
k13
k14
k15
k16
k17
= k5 k1 − k0 k6
= (k2 k6 − k7 k1 )/k11
= (k7 k0 − k2 k5 )/k11
= (k6 k3 − k1 k8 )/k11
= (k0 k8 − k5 k3 )/k11
= (k4 k6 − k1 k9 )/k11
= (k0 k9 − k5 k4 )/k11 .
W ostatnim kroku wyznaczamy funkcje gei , fei , e
ai oraz α
eio . B¦dziemy je
wyznacza¢ w takiej wªa±nie kolejno±ci. Funkcja gei jest oczywista
w
τi+1
= kT13 F + k15 τ + k17
czyli
gi = k13 ,
hw
i = k15 ,
hi = k17 .
Dalej,
d
w
Fi+1
=ri φi + Li τiw − Li τi+1
=(ri kT12 − Li kT13 )F + (ri k14 + Li − Li k15 )τ + ri k16 − Li k17
=Ci F + dw
i τ + di .
Zatem
Ci =ri kT12 − Li kT13
dw
i =ri k14 + Li − Li k15
di =ri k16 − Li k17 .
Funkcja e
ai b¦dzie wynikaªa z fei .
d
)/mi = Ai F + bw
ai = e
ai (F, τ ) = Fi /mi = (F e + F − Fi+1
i τ + bi
24
czyli
Ai = (I − Ci )/mi
w
bw
i = −di /mi
bi = (Fie − di )/mi .
Funkcja α
eio b¦dzie wynikaªa z e
ai oraz e
ai+1 , mianowicie
αio =LTi (ai+1 − ai )
d
w
=LTi (e
ai+1 (Fi+1
, τi+1
)−e
ai (Fid , τiw ))
³
´
¡
¢
=LTi e
ai+1 fei (F, τ ), gei (F, τ ) − e
ai (F, τ )
³
=LTi Ai+1 Ci F + Ai+1 dw
i τ + Ai+1 di
T
w
w
w
+ bw
i+1 gi F + bi+1 hi τ + bi+1 hi + bi+1
´
− Ai F − bw
τ
−
b
i
i
=pTi F + qiw τ + qi .
sk¡d mamy
T
T
T
pi =(LTi Ai+1 Ci + LTi bw
i+1 gi − Li Ai )
T w
w
T w
qiw =LTi Ai+1 dw
i + Li bi+1 hi − Li bi
T
T
qi =LTi Ai+1 di + LTi bw
i+1 hi + Li bi+1 − Li bi .
Podsumowuj¡c, sposób wyznaczania gei , fei , e
ai oraz α
eio jest nast¦puj¡cy:
Najpierw obliczamy Li , Li−1 , potem wielko±ci oznaczane przez k lub k, w takiej kolejno±ci jak na to wskazuj¡ ich indeksy. Nast¦pnie obliczane s¡ gi , hw
i , hi ,
w
w
w
Ci , di , di , Ai , bi , bi oraz pi , qi , qi .
Wyprowadzenia byªy oparte na zaªo»eniu, »e zarówno zª¡cze i-te nie
jest prowadzone, natomiast zª¡cze i + 1-sze istnieje i jest aktywne. Dalej
rozwa»ymy co si¦ dzieje je±li który± z tych warunków nie jest speªniony.
W niniejszej sekcji rozwa»ymy przypadki szczególne. Musimy rozwa»y¢ rozmaite cechy zª¡cza i-tego oraz i + 1-szego. Poniewa» ka»de z nich mo»e by¢
i nie by¢ aktywne oraz mo»e by¢ i nie by¢ prowadzone, to w zasadzie mo»na
rozwa»a¢ (2 · 2)2 = 16 ró»ne przypadki. Na szcz¦±cie nie wszystkie z nich
musz¡ by¢ dyskutowane odr¦bnie.
25
11.1 Zª¡cze prowadzone i aktywne nast¦pne
Niech i-te zª¡cze b¦dzie prowadzone w odró»nieniu od i+1-szego, które niech
jednak b¦dzie aktywne. Powiedzmy zatem, »e znamy funkcje
d
w
d
w
e
ai+1 (Fi+1
, τi+1
) = Ai+1 Fi+1
+ bw
i+1 τi+1 + bi+1 ,
w
w
d
w
d
o
+ qi+1 .
τi+1
+ qi+1
) = pTi+1 Fi+1
, τi+1
(Fi+1
α
ei+1
Zadanie polega na wyznaczeniu funkcji gei , fei , α
eio oraz e
ai , których argumend
w
tami s¡ wektory Fi , τi wyra»aj¡ce arbitraln¡ siª¦ i moment dziaªaj¡ce na
zª¡cza i-te. Oznaczmy
τ = τiw .
F = Fid ,
(b) równolegªej do ri∗ , zwi¡zanej z pojawieniem si¦ momentów wymuszaj¡w
cych τiw oraz τi+1
. Ostatecznie
d
w
w
Fi+1
= ri φi + (ri∗ /li )(τiw − τi−1
) = ri φi + Li (τ − τi+1
).
w
gdzie φi oraz τi+1
to pewne skalary, które teraz musimy wyznaczy¢.
w
Do wyznaczenia φi oraz τi+1
posªu»y nam ukªad dwóch równa«. Pierwsze
z nich, równanie ogniwa, mówi, »e zª¡cza i-te oraz i + 1-sze nie oddalaj¡ ani
nie przybli»aj¡ si¦ do siebie.
¡
¢
d
w
= riT aw
ai+1 (Fi+1
, τi+1
)
i −e
³
¡
¢´
T
w
w
w
w
= ri ai − Ai+1 (ri φi + Li τ − Li τi+1 ) + bi+1 τi+1 + bi+1 .
k0 = −riT Ai+1 ri
k1 =
k3 =
k4 =
riT Ai+1 Li − riT bw
i+1
T
−ri Ai+1 Li
T
riT aw
i − ri bi+1 − ei ,
to otrzymamy równanie
w
+ k3 τ + k4 .
0 = k0 φi + k1 τi+1
26
(24)
(25)
(26)
(27)
Drugie równanie wynika z (12) i stabilizuje pr¦dko±¢ k¡tow¡
w
o
αi+1
=αi+1
− αio
o
d
w
d
w
=e
αi+1
(Fi+1
, τi+1
) − LTi (e
ai+1 (Fi+1
, τi+1
) − ai )
d
w
w
d
w
w
=pTi+1 Fi+1
+ qi+1
τi+1
+ qi+1 − LTi (Ai+1 Fi+1
+ bw
i+1 τi+1 + bi+1 − ai )
w
w
w
+ qi+1
τi+1
) + qi+1
=pTi+1 (ri φi + Li τ − Li τi+1
w
w
− LTi (Ai+1 (ri φi + Li τ − Li τi+1
) + bw
i+1 τi+1 + bi+1 )
+ LTi aw
i
k5 = pTi+1 ri − LTi Ai+1 ri
k6 =
k8 =
w
−pTi+1 Li + qi+1
+ LTi Ai+1 Li
pTi+1 Li − LTi Ai+1 Li
(28)
−
LTi bw
i+1
w
k9 = qi+1 − LTi bi+1 + LTi aw
i − αi+1 ,
(29)
(30)
(31)
dostaniemy równanie
w
0 = k5 φi + k6 τi+1
+ k8 τ + k9 .
Ostatecznie dostajemy ukªad równa«
·
¸·
¸ ·
¸
k0 k1
φi
k3 τ + k4
0=
+
,
w
k5 k6 τi+1
k8 τ + k9
którego rozwi¡zaniem jest
·
¸
·
¸−T · T
¸
φi
k0 k1
k2 F + k3 τ + k4
=−
w
τi+1
k5 k6
kT F + k8 τ + k9
· 7
¸
k6 (k3 τ + k4 ) − k1 (k8 τ + k9 )
1
=
k k − k k k (k τ + k9 ) − k5 (k3 τ + k4 )
· 1 5 ¸ 0 ·6 0¸ 8
k16
k14
τ+
=
k17
k15
gdzie
k11
k14
k15
k16
k17
= k5 k1 − k0 k6
= (k6 k3 − k1 k8 )/k11
= (k0 k8 − k5 k3 )/k11
= (k4 k6 − k1 k9 )/k11
= (k0 k9 − k5 k4 )/k11 .
27
(32)
W ostatnim kroku wyznaczamy funkcje gei , fei , e
ai oraz α
eio . B¦dziemy je
wyznacza¢ w takiej wªa±nie kolejno±ci. Funkcja gei jest oczywista
w
τi+1
= k15 τ + k17
czyli
gi = 0,
hw
i = k15 ,
hi = k17 .
Dalej,
w
d
Fi+1
=ri φi + Li τiw − Li τi+1
=(ri k14 + Li − Li k15 )τ + ri k16 − Li k17
=Ci F + dw
i τ + di ,
sk¡d wynika funkcja fei , mianowicie
Ci =0
dw
i =ri k14 + Li − Li k15
di =ri k16 − Li k17 .
Funkcja e
ai b¦dzie wynikaªa z fei .
ai = aw
ai (F, τ ) = Ai F + bw
i =e
i τ + bi
czyli
Ai = 0
bw
i = 0
bi = aw
i .
Funkcja α
eio b¦dzie wynikaªa z e
ai oraz e
ai+1 , mianowicie
αio =LTi (ai+1 − ai )
d
w
=LTi (e
ai+1 (Fi+1
, τi+1
) − aw
i )
´
³
¡
¢
=LTi e
ai+1 fei (F, τ ), gei (F, τ ) − aw
i
³
=LTi Ai+1 Ci F + Ai+1 dw
i τ + Ai+1 di
w
w
w
w
T
+ bw
i+1 gi F + bi+1 hi τ + bi+1 hi + bi+1 − ai
=pTi F + qiw τ + qi .
28
´
sk¡d mamy
pi =0
T w
w
qiw =LTi Ai+1 dw
i + Li bi+1 hi
T
T
qi =LTi Ai+1 di + LTi bw
i+1 hi + Li bi+1 − Li bi .
W omawianym przypadku wyprowadzili±my funkcje gei , fei , e
ai oraz α
eio
w taki sposób, »e wektory je opisuj¡ce zale»¡ od siebie. Nale»y je wi¦c wyznacza¢ w takiej wªa±nie kolejno±ci jak podana powy»ej.
Sytuacje wyj¡tkowe
W szczególnych przypadkach ukªad równa« (32) mo»e nie mie¢ rozwi¡za«
lub mie¢ ich niesko«czenie wiele. Z pewno±ci¡ równanie pierwsze nie b¦dzie
miaªo rozwi¡zania je±li zª¡cze i + 1-sze jest prowadzone. Je±li dodatkowo
prowadzone jest zª¡cze i + 2-gie, wówczas tak»e równanie drugie nie b¦dzie
miaªo rozwi¡zania. Je±li zª¡cze i + 2-gie b¦dzie prowadzone ale zª¡cze i + 1sze nie, wówczas oba równania b¦d¡ miaªy rozwi¡zania, tyle, »e b¦d¡ one
sprzeczne. W podobnych przypadkach, których listy bynajmniej tu nie wyczerpali±my, nale»y zamieni¢ równania na takie, których rozwi¡zaniami s¡
w
zerowe φi (przyjmuj¡c k0 = 1, k1 = k3 = k4 = 0) i/albo τi+1
(przyjmuj¡c
k6 = 1, k5 = k8 = k9 = 0). Wszystkie takie sytuacje mo»emy wykry¢
badaj¡c czy wspóªczynnik k11 jest na poziomie bª¦du numerycznego. Je±li
tak, musimy rozwa»y¢ z osobna ka»d¡ z nast¦puj¡cych mo»liwo±ci:
1. Warto±¢ |k0 |+|k1 | jest na poziomie bª¦du numerycznego lub |k0 |+|k1 | <
|k5 | + |k6 |. Zmieniamy pierwsze równanie na daj¡ce zerowe φi .
2. Warto±¢ |k5 |+|k6 | jest na poziomie bª¦du numerycznego lub |k0 |+|k1 | ≥
w
|k5 | + |k6 |. Zmieniamy drugie równanie na daj¡ce zerowe τi+1
.
11.2 Zª¡cze prowadzone i nieaktywne nast¦pne
Zaªó»my, »e zª¡cze i-te jest prowadzone, za± i + 1-sze jest nieaktywne. Zª¡cze
i-te mo»e by¢ przy tym aktywne lub nie. W takim razie przyspieszenie
w
i-tego zª¡cza wynosi aw
i i nie istnieje moment wymuszaj¡cy τi+1 . Wobec tego
ei ≡ 0. Musimy jednak wyznaczy¢ pozostaªe funkcje opisuj¡ce
e
ai ≡ aw
i za± g
dynamik¡ i-tego zª¡cza. Znamy funkcj¦
d
w
d
w
e
ai+1 (Fi+1
, τi+1
) = Ai+1 Fi+1
+ bw
i+1 τi+1 + bi+1 .
29
Zadanie polega na wyznaczeniu funkcji fei i α
ei0 , których argumentami s¡ wekd
w
tory Fi , τi wyra»aj¡ce arbitraln¡ siª¦ i moment dziaªaj¡ce na zª¡cza i-te.
Oznaczamy
F = Fid ,
τ = τiw .
(b) równolegªej do ri∗ , zwi¡zanej z pojawieniem si¦ momentu wymuszaj¡cego
τiw . Mamy
d
Fi+1
= ri φi + (ri∗ /li )τiw = ri φi + Li τ.
gdzie φi to pewien skalar, który teraz musimy wyznaczy¢.
Do wyznaczenia φi posªu»y nam równanie ogniwa, które mówi, »e zª¡cza
i-te oraz i + 1-sze nie oddalaj¡ ani nie przybli»aj¡ si¦ do siebie.
¡
¢
d
w
= riT aw
ai+1 (Fi+1
, τi+1
)
i −e
³
¡
¢´
w
−
A
(r
φ
+
L
τ
)
+
b
·
0
+
b
.
= riT aw
i+1 i i
i
i+1
i
i+1
Z powy»szego dostajemy
φi =
T
riT (aw
i − bi+1 ) − ei − ri Ai+1 Li τ
riT Ai+1 ri
a poniewa»
d
Fi+1
= fei (F, τ ) = Ci F + bw
i τ + bi = ri φi + Li τ
ªatwo dostajemy
Ci = 0
ri riT Ai+1 Li
dw
=
L
−
i
i
rT A r
¢
¡ T iw i+1 i
ri ri (ai − bi+1 ) − ei
di =
.
riT Ai+1 ri
Funkcje gei i e
ai , jak wspomniano wy»ej, maj¡ tym razem proste postaci
gei (F, τ ) = giT F + hw
i τ + hi ≡ 0
w
e
ai (F, τ ) = Ai F + bw
i τ + bi ≡ ai
30
w
co wyja±nia warto±ci wspóªczynników gi , hw
i , hi , Ai , bi , bi . W ko«cu, funkcj¦
α
eio (F, τ ) = pTi F + qiw τ + qi
mo»na wyznaczy¢ stosuj¡c wzory na pi , qiw , qi z poprzedniego rozdziaªu bior¡c
jednak pod uwag¦, »e bw
i+1 = 0. Dostajemy zatem
pi =0
T w
qiw =LTi Ai+1 dw
i − L i bi
qi =LTi Ai+1 di + LTi bi+1 − LTi bi .
Sytuacje wyj¡tkowe
W szczególnym przypadku przyspieszenie i+1-szego zª¡cza nie zale»y od siªy
do« przykªadanej (np. gdy zª¡cze to jest prowadzone). Dostajemy wówczas
riT Ai+1 ri na poziomie bª¦du numerycznego i obliczanie Fid mija si¦ z celem.
d
Nale»y wówczas przyj¡¢ Fi+1
= fei (F, τ ) ≡ 0.
11.3 Zª¡cze nieprowadzone i nieaktywne nast¦pne
Zaªó»my, »e zª¡cze i + 1 nie jest aktywne. W takim razie nie istnieje moment
w
wymuszaj¡cy τi+1
i mamy gei ≡ 0. Musimy jednak wyznaczy¢ pozostaªe
funkcje. Powiedzmy zatem, »e znamy funkcj¦
d
w
d
w
e
ai+1 (Fi+1
, τi+1
) = Ai+1 Fi+1
+ bw
i+1 τi+1 + bi+1 .
Jak zwykle oznaczamy
F = Fid ,
τ = τiw .
Tym razem siªa napr¦»enia oddziaªuj¡ce na i+1-sze zª¡cze poprzez i-te ogniwo jest wypadkow¡ dwóch siª: (a) równolegªej do ri , odpowiedzialnej za to,
»e zª¡cza i-te oraz i + 1-sze nie zmieniaj¡ odlegªo±ci od siebie, (b) równolegªej
do ri∗ , zwi¡zanej z pojawieniem si¦ momentu wymuszaj¡cego τiw . Ostatecznie
d
Fi+1
= ri φi + (ri∗ /li )τiw = ri φi + Li τ.
gdzie φi to pewien skalar, który teraz musimy wyznaczy¢.
Wyznaczmy caªkowit¡ siª¦ Fi dziaªaj¡c¡ na i-te zª¡cze. Jest to suma siªy
zewn¦trznej i siª przenoszonych przez s¡siednie ogniwa. Zatem
d
Fi = Fie + Fid − Fi+1
=Fie + F − ri φi − Li τ
31
Do wyznaczenia φi posªu»y nam równanie ogniwa, które mówi, »e zª¡cza
i-te oraz i + 1-sze nie oddalaj¡ ani nie przybli»aj¡ si¦ do siebie.
mi ei = mi riT (ai − ai+1 )
¢
¡
d
w
ai+1 (Fi+1
, τi+1
)
= riT Fi − mie
³¡
¢
= riT Fie + F − ri φi − Li τ
¡
¢´
− mi Ai+1 (ri φi + Li τ ) + bw
·
0
+
b
.
i+1
i+1
Z powy»szego dostajemy
φi =
riT F − (riT Li + mi riT Ai+1 Li )τ + riT Fie − (riT bi+1 + ei )mi
riT Ai+1 ri mi + 1
a poniewa»
d
Fi+1
= fei (F, τ ) = Ci F + bw
i τ + bi = ri φi + Li τ
ªatwo dostajemy
ri riT
riT Ai+1 ri mi + 1
ri (0 + riT Ai+1 Li mi )
dw
=
L
−
i
i
riT Ai+1 ri mi + 1
ri riT Fie − ri (riT bi+1 + ei )mi
di =
riT Ai+1 ri mi + 1
Ci =
Funkcja e
ai jest, jak zwykle, nieco bardziej kªopotliwa
d
ai = e
ai (F, τ ) = Fi /mi = (F e + F − Fi+1
)/mi = Ai F + bw
i τ + bi
czyli
Ai = (I − Ci )/mi
w
bw
i = −di /mi
bi = (Fie − di )/mi .
W ko«cu, funkcj¦
α
eio (F, τ ) = pTi F + qiw τ + qi
mo»na wyznaczy¢ stosuj¡c wzory na pi , qiw , qi z poprzedniego rozdziaªu bior¡c
jednak pod uwag¦, »e bw
i+1 = 0. Dostajemy zatem
pi =(LTi Ai+1 Ci − LTi Ai )T
T w
qiw =LTi Ai+1 dw
i − L i bi
qi =LTi Ai+1 di + LTi bi+1 − LTi bi .
32
12 Symulacja z ograniczeniami na k¡ty
W niniejszym rozdziale zajmiemy si¦ symulowaniem obiektu, w którym k¡ty
mi¦dzy s¡siaduj¡cymi ogniwami s¡ ograniczone do pewnego przedziaªu. Zdeniujmy k¡t w i-tym ogniwie, θi , jako k¡t mi¦dzy ri a ri−1 . Przyjmujemy,
»e θi ∈ [θim , θiM ]. Dopóki θi ∈ (θim , θiM ), nie maj¡ miejsca »adne zjawiska,
które nale»aªoby byªo modelowa¢ przez wymuszone przyspieszenie k¡towe.
Konieczno±¢ taka pojawia si¦, kiedy (i) θi dochodzi do θim lub θiM , oraz (ii) gdy
θi pozostaje na θim lub θiM . Zdarzenia te omówimy w kolejnym podrozdziale.
12.1 K¡t traa na ograniczenie, zwarcie
W podsekcji 7.1 rozwa»ali±my zjawiska towarzysz¡ce zderzeniu zª¡cza z nieruchomym przedmiotem. W niniejszej sekcji dyskutujemy zjawisko analogiczne
polegaj¡ce na tym, »e k¡t mi¦dzy i − 1-szym a i-tym ogniwem natraa na
swoje ograniczenie. To co wówczas nast¦puje b¦dziemy nazywali zwarciem
k¡ta z jego ograniczeniem. W ci¡gu bardzo krótkiego kwantu czasu dziaªa
moment siªy, który wyhamowuje pr¦dko±¢ k¡tow¡ zª¡cza. Analiza wyst¦puj¡cych tu zjawisk jest identyczna do dyskusji tego co si¦ dzieje gdy zª¡cze
wbija si¦ w punkt. Tam w krótkim kwancie czasu dziaªaªa na zª¡cze bardzo
du»a siªa. Tutaj na takie zderzaj¡ce si¦ ograniczeniem zª¡cze dziaªa przez
krótk¡ chwil¦ du»y moment siªy. Procedura jego obliczania jest analogiczna
do opisanej w podsekcji 7.1 i wygl¡da w sposób nast¦puj¡cy
Z1. Przyjmij Fke = 0, ek = 0 dla k = 1, . . . , n oraz αiw = −ωiz (przej±cie na
skal¦ czasu o jednostce ² d¡»¡cym do zera).
ak , τek a nast¦pnie przyspieszenia ak dla k = 1, . . . , n zgodZ2. Wyznacz fek , e
nie z dotychczas wyprowadzonymi wzorami.
Z3. (Powrót do pierwotnej skali czasu) Oblicz vk := vk +ak dla k = 1, . . . , n.
Zwarcie k¡ta z jego ograniczeniem nast¦puje w pewnym momencie mi¦dzy
dyskretnymi chwilami czasu symulacji. Na ogóª ªatwo jest wykry¢ post factum, »e do zwarcia doszªo. Wtedy bowiem dla pewnego i mamy θi ∈
/ [θim , θiM ].
W tym momencie nale»y podj¡¢ szereg czynno±ci rozpoczynaj¡cy si¦ od ustalenia jaki kwant czasu ² temu doszªo do zwarcia. Powiedzmy, »e θi > θiM .
Mamy
θi − θiM = ωiz ² − 0.5αiz ²2
co daje
√
z
−
ω
∆
,
²0 = i z
αi
√
z
+
ω
∆
²00 = i z
,
αi
33
∆ = (ωiz )2 − 2αiz (θi − θiM ).
Nale»y przyj¡¢ jako ² mniejsz¡ dodatni¡ spo±ród liczb ²0 , ²00 .
Nast¦pnie nale»y wykona¢ nast¦puj¡ce czynno±ci:
a) cofn¡¢ si¦ w czasie o do chwili zderzenia,
b) wyznaczy¢ pr¦dko±ci po zderzeniu, np. wg. kroków Z1, Z2, Z3,
c) uaktywni¢ zª¡cze, które si¦ zderzyªo i przypisa¢ mu αiw = 0.
d) przesymulowa¢ wydarzenia, które nast¦puj¡ potem w ci¡gu czasu o dªugo±ci ².
Po zwarciu, zª¡cze pozostaje aktywne tak dªugo, jak dªugo konieczny jest
moment τiw odpychaj¡cy k¡t θi od ograniczenia. Z chwil¡ gdy okazuje si¦,
»e taki moment w istocie dociska k¡t θi do ograniczenia, nale»y takie zª¡cze
uczyni¢ nieaktywnym. W szczególnym przypadku zª¡cze staje si¦ aktywne
tylko na kwant czasu kiedy jego pr¦dko±¢ k¡towa zostaje wyhamowywana
i natychmiast potem znowu staje si¦ nieaktywne. Tak si¦ dzieje, gdy siªy
egzogeniczne odpychaj¡ k¡t od ograniczenia, jednak dochodzi do zwarcia
przez inercj¦ jego ruchu.
12.2 Anomalie
Oprogramowuj¡c symulacj¦ z ograniczeniami na k¡ty mi¦dzy ogniwami nale»y
pami¦ta¢ o kilku nietypowych zjawiskach, które mog¡ mie¢ miejsce w jej trakcie. Omówimy je jako sztywno±¢, szuranie, klekotanie oraz problemy
z dyskretyzacj¡ czasu.
Sztywno±¢
Powiedzmy, »e zª¡cza n − 1-sze i n-te s¡ prowadzone, natomiast zª¡cza od
i-tego do n − 1-szego s¡ aktywne. W takim razie obiekt mi¦dzy zª¡czami
i − 1-szym a n-tym zachowuje si¦ jak sztywno osadzona w przestrzeni bryªa.
W szczególno±ci, przyspieszenie zª¡cza i − 1-ego nie zale»y od tego jaka
zostanie przyªo»ona do niego siªa. Propagacja funkcji opisuj¡cych dynamik¦
zª¡cz ujawni, »e Ai−1 = 0. W zasadzie wi¦c, powinni±my ze zª¡czem tym
post¦powa¢ jak z prowadzonym. Je±li np. zª¡cze i − 2-gie jest prowadd
tak aby zachowa¢ odpowiednie
zone, wówczas próba znalezienia siªy Fi−1
przyspieszenie mi¦dzy i − 2-gim a i − 1-szym zª¡czem mo»e sko«czy¢ si¦
jedynie dzieleniem przez zero.
Powy»szy przykªad mo»na zgeneralizowa¢ w nast¦puj¡cy sposób: na skutek faktu, »e dwa zª¡cza s¡ prowadzone za± zª¡cza mi¦dzy nimi s¡ aktywne,
34
Rysunek 4: Szuranie. Obiekt jest nieruchomy; zª¡cze 1 jest przy ziemi, k¡t
mi¦dzy ogniwami osi¡gn¡ª swoj¡ najwi¦ksz¡ warto±¢. Zª¡cze 3 jest ci¡gni¦te
siª¡ F .
mamy do czynienia z ukªadem, który jest w pewnej cz¦±ci sztywny. Przykªadanie siª do zª¡cz nale»¡cych do tej cz¦±ci nie zmienia ich przyspiesze«.
Wyranowane remedium na tego rodzaju trudno±ci polega na tym, aby wykrywa¢ sztywne cz¦±ci obiektu i dziaªa¢ tak, jakby nale»¡ce do nich zª¡cza
byªy prowadzone. Rozwi¡zanie mniej eleganckie, jednak bardziej uniwersalne polega na tym, aby przy wyznaczaniu funkcji dynamiki i-tego zª¡cza
za ka»dym razem czy aby Ai+1 nie ma bardzo maªych warto±ci. W poprzednich podrozdziaªach opisujemy jak nale»y reagowa¢ na tak¡ sytuacj¦.
Zaªó»my teraz, »e n-te zª¡cze jest prowadzone, natomiast zª¡cza od itego do n − 1-szego s¡ aktywne. Wówczas obiekt mi¦dzy zª¡czami i-tym
a n-tym zachowuje si¦ jak sztywne wahadªo. Siªa dziaªaj¡ca na i-te zª¡cze
wpªywa na tylko jeden wymiar jego przyspieszenia. Powiedzmy teraz, »e
zª¡cze i − 2-gie jest prowadzone. W takim razie przy zachowanej dªugo±ci
i−1-szego ogniwa nie jeste±my w stanie kontrolowa¢ przyspieszenia k¡towego
i-tego zª¡cza. Wysiªek w tym kierunku musi sko«czy¢ si¦ dzieleniem przez
zero. Poprzez analiz¦ manipulatora mo»emy wykrywa¢ tego rodzaju sytuacj¦
poªowicznej sztywno±ci. Rozwi¡zaniem mniej eleganckim jest sprawdzanie,
czy przyspieszenie k¡towe ka»dego i+1-szego w ogóle zale»y do przykªadanego
w nim momentu. Je±li nie zale»y, trzeba zrezygnowa¢ z jego wyznaczania.
Szuranie
Zjawisko szurania pojawia si¦ przy okazji odrywania zª¡cz od punktów, w których s¡ utkwione lub od ogranicze« na k¡ty. Mo»e si¦ wtedy okaza¢, »e nie da
si¦ tak okre±li¢ przyspiesze« aby odpowiadaªy one temu, które ograniczenia
s¡ aktywne.
Przykªad szurania jest przedstawiony na Rysunku 4. Algorytm próbuj¡cy
obliczy¢ przyspieszenia w tym ukªadzie mo»e wpa±¢ w poni»sz¡ niesko«czon¡
35
p¦tl¦. Powiedzmy, »e na pocz¡tku zª¡cze 1 nie stoi na ziemi za± zª¡cze 2 jest
nieaktywne.
1. Algorytm stwierdza, »e zª¡cze 1 porusza si¦ z przyspieszeniem w dóª
i w takim razie nale»y je postawi¢ czyli uczyni¢ prowadzonym.
2. Zª¡cze 2 ma teraz dodatnie przyspieszenie k¡towe, nale»y je uczyni¢
aktywnym.
3. Teraz zª¡cze 1 jest ci¡gni¦te do góry i wobec tego powinno przesta¢ by¢
prowadzone.
4. W tym momencie okazuje si¦, »e zª¡cze 1 swoj¡ mas¡ skªada zª¡cze
2. Aby teraz w zª¡czu 2 mogªo by¢ utrzymane zerowe przyspieszenie
k¡towe, musi tam zadziaªa¢ moment, który dopycha je do ograniczenia.
W tym momencie nale»y zrezygnowa¢ z przykªadania takiego momentu
i p¦tla zamyka si¦.
W rzeczywistym ukªadzie zycznym zª¡cze 1 poruszaªoby si¦ po podªo»u,
czyli wªa±nie szuraªo po nim.
Recept¡ na opisywane zjawisko jest pozostawianie ogranicze« aktywnymi
nawet je±li nie ma to zycznego sensu. W powy»szym przykªadzie oznaczaªoby to pozostawienie zª¡cza 1 prowadzonym w momencie stwierdzenia zap¦tlenia.
Klekotanie
Klekotanie polega na tym, »e odlegªo±ci czasowe miedzy traeniami obiektu
na ró»ne ograniczenia s¡ coraz mniejsze. W ten sposób symulator musi analizowa¢ coraz wi¦cej wydarze« w kwantach czasu tej samej dªugo±ci. Sytuacja
taka mo»e towarzyszy¢ nast¦puj¡cej kombinacji okoliczno±ci:
• Obiekt jest w pobli»u co najmniej dwóch ogranicze«.
• Zetkniecie obiektu z ka»dym z ogranicze« odbija obiekt od niego i przybli»a do innego.
• Droga do kolejnego ograniczenia zajmuje coraz krótszy przedziaª czasu.
Idea klekotania zostaªa zaprezentowana w lewej cz¦±ci Rysunku 5. Cz¦±¢
prawa dostarcza ilustracji. Dochodzi tam do nast¦puj¡cej sekwencji zdarze«:
1. Zewn¦trzny moment τ w zª¡czu 3 przybli»a zª¡cze 2 do zª¡cza 4.
36
Rysunek 5: Klekotanie. Lewo: idea. Prawo: ilustracja. Zª¡cze 1 jest blisko
ziemi, k¡t zª¡cza 3 jest blisko swojej najmniejszej warto±ci. Dodatkowo w
zª¡czu 3 dziaªa zewn¦trzny moment dopychaj¡cy to zª¡cze do ograniczenia.
Obiekt porusza si¦ w stron¦ ziemi z pr¦dko±ci¡ v .
2. W pewnym momencie ograniczenie na k¡t 3 staje si¦ aktywne; w tym
momencie p¦d zª¡cza 2 jest przekazany do zª¡cza 4.
3. Obiekt ci¡gle opada, wi¦c zª¡cze 1 w pewnym momencie styka si¦
z ziemi¡. Ze wzgl¦du na zewn¦trzny moment τ , zaraz po zetkni¦ciu z ziemi¡ zª¡cze to jest ±ci¡gane w stron¦ zª¡cza 4. W momencie
zderzenia z ziemi¡, pr¦dko±¢ zª¡cza 4 zmienia si¦ i porusza si¦ ono w
stron¦ zª¡cza 2 i p¦tla zdarze« zamyka si¦.
Poniewa» caªy obiekt jest coraz ni»ej, odcinki czasu mi¦dzy zdarzeniami s¡
coraz krótsze. W rzeczywisto±ci przyniosªoby to taki efekt, »e niesko«czona
sekwencja zdarze« odbyªaby si¦ w sko«czonym czasie i obiekt klekocz¡c
wyl¡dowaªby na ziemi. Nie jest natomiast mo»liwe aby tak¡ niesko«czon¡
p¦tl¦ zdarze« zasymulowa¢.
Remedium na klekotanie polega na pozostawieniu ogranicze« aktywnymi
je±li s¡ dezaktywowane zbyt wiele razy w ci¡gu krótkiego przedziaªu czasu.
Bª¦dy numeryczne
Kªopoty zwi¡zane z bª¦dami numerycznymi zostaªy zasygnalizowane ju» w sekcji 3. Przypomnijmy: na skutek niedokªadno±ci prowadzonych oblicze« ogniwa mog¡ si¦ wydªu»a¢ lub skraca¢. Dodatkowo mo»e nam zale»e¢ aby k¡ty
mi¦dzy niektórymi ogniwami pozostawaªy staªe. Niedokªadno±ci numeryczne
mog¡ temu przeszkadza¢. Proponujemy systematyczne remedium na te problemy oparte na idei regulatora PD. Rozwa»my δ² > 0 i równanie ró»niczkowe
²̈ = −²/δ²2 − 2²̇/δ² .
37
(33)
Rysunek 6: Problemy z dyskretyzacj¡ czasu. Zª¡cza 1 i 3 stoj¡ na ziemi.
Niewielkie przyspieszenia stabilizuj¡ce dªugo±¢ ogniw przekªadaj¡ si¦ na
dowolnie du»e przyspieszenie zª¡cza 2.
Z teorii równa« ró»niczkowych zwyczajnych wiemy, »e bez wzgl¦du na warunki
pocz¡tkowe funkcja ² opisana powy»szym równaniem zbiega do zera, przy
czym jej zbie»no±¢ asymptotyczna jest wykªadnicza ze staª¡ czasow¡ δ² .
Powiedzmy, »e pewna dynamiczna wielko±¢ (dªugo±¢ ogniwa lub k¡t)
powinna mie¢ staª¡ warto±¢ ȳ . Tymczasem jej faktyczna warto±¢ w danej
chwili to y . Powiedzmy ponadto, »e jeste±my w stanie obliczy¢ pr¦dko±¢
zmian y , czyli ẏ . Staªo±¢ y jest wymuszana przez odpowiednie równanie,
w którym parametrem jest przyspieszenie (lub przyspieszenie k¡towe). Proponowane tu remedium na bª¦dy numeryczne polega na tym aby potraktowa¢
² jako bª¡d wielko±ci y , czyli przyj¡¢
² = y − ȳ,
²̇ = ẏ,
oraz zmodykowa¢ przyspieszenie (przyspieszenie k¡towe) w odpowiednim
równaniu o praw¡ cz¦±¢ (33). W ten sposób pojawiaj¡ce si¦ bª¦dy numeryczne powinny zanika¢ wykªadniczo ze staª¡ czasow¡ δ² . Pozostaje pytanie jak dobra¢ t¡ staª¡. Otó» powinna ona by¢ kilkukrotnie (np. 5-krotnie)
wi¦ksza ni» staªa δ u»ywana jako odlegªo±¢ mi¦dzy dyskretnymi momentami
symulacji. Chodzi o to, aby wykªadniczy zanik bª¦dów mógª by¢ dobrze
przybli»any kawaªkami, w których przyspieszenia s¡ staªe.
Problemy z dyskretyzacj¡ czasu
Okre±lenie odlegªo±ci mi¦dzy dyskretnymi chwilami symulacji jest zagadnieniem, które znalazªo systematyczne rozwi¡zanie, np. w postaci metody
Rungego-Kutty numerycznego rozwi¡zywania ukªadów równa« ró»niczkowych.
38
Tutaj proponujemy podej±cie znacznie prostsze, które mo»na nazwa¢ metod¡
Eulera z cofaniem si¦ przy detekcji bª¦dów. Przyjmujemy mianowicie, »e
odlegªo±¢ mi¦dzy dyskretnymi chwilami symulacji jest staªa dopóki pewne
oszacowanie bª¦du jest dostatecznie maªe. Je±li oszacowanie to przekracza
pewien próg, wówczas przedziaª czasu jest dzielony na dwie poªowy. Obie
poªowy s¡ tak»e poddawane testowi na bª¦dy i ewentualnie dzielone (i tak
rekurencyjnie).
Najpierw zademonstrujemy na przykªadzie sk¡d mog¡ si¦ bra¢ bardzo
du»e bª¦dy numeryczne. Przyjrzyjmy si¦ Rysunkowi 6. Zaªó»my, »e na skutek
bª¦dów numerycznych ogniwa s¡ nieco krótsze ni» ich nominalne dªugo±ci.
Powiedzmy, »e bª¦dy te s¡ kompensowane przyspieszeniami od±rodkowymi
ko«ców ogniw. Nawet je±li te przyspieszenia s¡ maªe, przyspieszenie zª¡cza
2 jest tym wi¦ksze, im k¡t mi¦dzy ogniwami jest bli»szy π i mo»e by¢ dowolnie du»e. W rezultacie, w ci¡gu kwantu czasu o z góry zadanej dªugo±ci
zª¡cze 2 mo»e zosta¢ rozp¦dzone do dowolnej pr¦dko±ci, po czym poci¡gnie
za sob¡ pozostaªe zª¡cza. Daªoby to zabawny efekt wystrzeliwania obiektu
w gór¦. Efekt ten nie miaªby miejsca w rzeczywisto±ci, poniewa» przyspieszenie zostaªoby zredukowane kiedy tylko bª¦dy dªugo±ci ogniw zostaªyby skompensowane a to staªoby si¦ bardzo szybko.
Powy»szy przykªad mo»na uogólni¢ w nast¦puj¡cy sposób: kiedy k¡ty
mi¦dzy kilkoma s¡siednimi ogniwami s¡ bliskie π , za± zewn¦trzne zª¡cza
takiego ukªadu s¡ w jaki± sposób usztywnione, wówczas umiarkowane przyspieszenia zª¡cz stabilizuj¡ce dªugo±ci ogniw mog¡ si¦ przeªo»y¢ na bardzo
du»e przyspieszenia tych samych zª¡cz prostopadªe do ogniw. Przyspieszenia
te dziaªaj¡ jednak bardzo krótko i je±li symulator zaªo»y ich staªo±¢ przez
do±¢ dªugi kwant czasu, mo»e to doprowadzi¢ do absurdalnych przesuni¦¢
i pr¦dko±ci zª¡cz.
Jako remedium na trudno±ci podobne do opisanych powy»ej proponujemy
cofanie czasu przy stwierdzeniu znacznych bª¦dów numerycznych. Powiedzmy,
»e na pocz¡tku czasu przyspieszenie i-tego zª¡cza wynosi ai za± po tym kwancie czasu wynosi a0i . Nie wiemy jak przyspieszenie zmieniaªo si¦ w tym czasie.
Posªu»ymy si¦ wi¦c heurystyk¡, która mówi, »e bª¡d oszacowania pr¦dko±ci
pod koniec kwantu czasu zwi¡zany ze zmian¡ przyspieszenia jest na poziomie
ka0i − ai kδ.
Odpowiada to sytuacji, w której przyspieszenie zmieniªo si¦ na a0i natychmiast po rozpocz¦ciu kwantu czasu i takie ju» pozostaªo. Je±li dla pewnego i
powy»szy bª¡d przekracza pewien próg (np. równy 1), wówczas przywracany
jest stan obiektu sprzed kwantu czasu, po czym symulacja wydarze« z tego
kwantu odbywa si¦ w dwóch etapach równej dªugo±ci.
39
Cz¦±¢ III
Przykªadowe obiekty
Niektóre pªaskie ªa«cuchy kinematyczne odgrywaj¡ rol¦ obiektów testowych
sªu»¡cych do ewaluacji i porównywania rozmaitych algorytmów. Zwykle dynamika takich obiektów jest znana w postaci zwartych wzorów na drugie
pochodne (po czasie) wszystkich zmiennych stanu. Prezentujemy kilka takich przykªadowych obiektów. W znacznej cz¦±ci nie speªniaj¡ one zaªo»enia,
które towarzyszyªo nam w poprzednich cz¦±ciach niniejszego dokumentu ich masa mo»e by¢ rozlokowana w ogniwach a nie jedynie w zª¡czach.
13 Wahadªa z jednym nieruchomym zª¡czem
Niniejszy rozdziaª po±wi¦cony jest pªaskim ªa«cuchom kinematycznym których jedno z kra«cowych zª¡cz (powiedzmy, »e pierwsze) jest nieruchome. S¡
one sterowane przez przykªadanie momentów siª do zª¡cz; zwykle zarówno
do ruchomych jak i do nieruchomego. Obiekty tego rodzaju s¡ zwykle interpretowane jako wahadªa (and. pendulum). Problem sterowania z nimi
zwi¡zany polega na tym aby je stabilizowa¢ pionowo w górze lub doprowadzi¢
do ustawienia pionowo w górze i wtedy stabilizowa¢.
13.1 Jedno ogniwo
Jednym z najprostszych obiektów dynamicznych jest pojedyncze wahadªo [6],
skªadaj¡ce si¦ ze sztywnego ogniwa o jednostkowej dªugo±ci i jednostkowej
masie skupionej w jego ko«cu, Rys. 7. Stan obiektu opisywany jest przez k¡t
θ mi¦dzy wahadªem a pionem oraz pr¦dko±¢ k¡tow¡ θ̇. Dynamika obiektu
Rysunek 7: Pojedyncze wahadªo: a«cuch z jednym zª¡czem nieruchomym
i jednym ogniwem.
40
Rysunek 8: Podwójne wahadªo: a«cuch z jednym zª¡czem nieruchomym
i dwoma ogniwami.
opisana jest równaniem
θ̈ = sin θ + τ
przy czym τ jest to moment przykªadany do nieruchomego zª¡cza o takim
kierunku jak k¡t θ. Na ogóª przyjmuje si¦, »e τ ∈ [−0.225, 0.225] a celem
sterowania wahadªem jest ustawienie wahadªa w górze i jego stabilizacja.
Osi¡gni¦cie tego celu jest o tyle kªopotliwe, »e maksymalny moment nie
wystarczy aby przenie±¢ wahadªo z doªu do góry i wobec tego nale»y je
rozbuja¢.
13.2 Dwa ogniwa
Manipulator z jednym zª¡czem nieruchomym i dwoma ogniwami nosi nazw¦
Acrobot i jest interpretowany jako akrobata, który buja si¦ na dr¡»ku operuj¡c
jedynie momentem siªy w biodrach. W poni»szym opisie zaªo»ymy, »e mo»e
tak»e przykªada¢ moment siªy w nadgarstku.
Mamy 4 zmiennych opisuj¡cych stan: k¡ty θ = [θ1 , θ2 ]T mi¦dzy s¡siednimi
ogniwami oraz ich pochodne po czasie θ̇ = [θ̇1 , θ̇2 ]T . Sterowanie ªa«cuchem
odbywa si¦ poprzez przykªadanie momentów siªy τ1 , τ2 do zª¡cz. Momenty
te maj¡ taki kierunek jak k¡ty θ1 , θ2 , odpowiednio. Przyspieszenia k¡towe
zª¡cz wynikaj¡ z momentów jako rozwi¡zanie nast¦puj¡cego równania.
B(θ)θ̈ + c(θ, θ̇) + h(θ) = τ
41
gdzie
B1,2
¡
¢
2
2
B1,1 = m1 lc1
+ m2 l12 + lc2
+ 2l1 lc2 cos(θ2 ) + I1 + I2
¡2
¢
= B2,1 = m2 lc2
+ l1 lc2 cos(θ2 ) + I2
2
+ I2
B2,2 = m2 lc2
c1 = − m2 l1 lc2 θ̇22 sin(θ2 ) − 2m2 l1 lc2 θ̇1 θ̇2 sin(θ2 )
c2 =m2 l1 lc2 θ̇12 sin(θ2 )
h1 =(m1 lc1 + m2 l1 )g cos(θ1 ) + m2 lc2 g cos(θ1 + θ2 )
h2 =m2 lc2 g cos(θ1 + θ2 ).
Staªe pojawiaj¡ce si¦ w powy»szych wzorach maj¡ warto±ci i interpretacje
przedstawione w poni»szej tabeli.
staªa
l1 , l2
lc1 , lc2
m1 , m2
I1 , I2
opis
dªugo±ci ogniw
±rodki masy ogniw
masy ogniw
inercje ogniw
warto±¢
1.0
0.5
1.0
1.0
13.3 Trzy ogniwa
Manipulator z jednym zª¡czem nieruchomym i trzema ogniwami zostaª wykorzystany do demonstracji pewnego algorytmu uczenia si¦ w [7]. Miaª tam
interpretacj¦ ramienia podnosz¡cego ci¦»arek przymocowany na ko«cu ostatniego ogniwa (Rys. 9).
Mamy 6 zmiennych opisuj¡cych stan: k¡ty θ = [θ1 , θ2 , θ3 ]T mi¦dzy s¡siednimi ogniwami oraz ich pochodne po czasie θ̇ = [θ̇1 , θ̇2 , θ̇3 ]T . Sterowanie
ªa«cuchem odbywa si¦ poprzez przykªadanie momentów siªy τ1 , τ2 , τ3 do zª¡cz.
Przyspieszenia k¡towe zª¡cz wynikaj¡ z momentów jako rozwi¡zanie nast¦puj¡cego równania.
B(θ)θ̈ + c(θ, θ̇) + h(θ) = τ
42
Rysunek 9: Manipulator z jednym zª¡czem nieruchomym i trzema ogniwami.
gdzie
B1,2
B1,3
B2,3
B1,1
= B2,1
= B3,1
B2,2
= B3,2
B3,3
= 3 + 2 cos θ2 + m(3 + 2 cos θ2 + 2 cos(θ2 + θ3 ) + 2 cos θ3 )
= 1 + cos θ2 + m(2 + cos θ2 + cos(θ2 + θ3 ) + 2 cos θ3 )
= m(1 + cos(θ2 + θ3 ) + cos θ3 )
= 1 + m2(1 + cos θ3 )
= m(1 + cos θ3 )
=m
43
c1 = − θ̇1 θ̇2 2(sin θ2 + m(sin θ2 + sin(θ2 + θ3 )))
− θ̇1 θ̇3 m2(sin(θ2 + θ3 ) + sin θ3 )
− θ̇22 (sin θ2 + m(sin θ2 + sin(θ2 + θ3 )))
− θ̇2 θ̇3 m2(sin(θ2 + θ3 ) + sin θ3 )
− θ̇32 m(sin(θ2 + θ3 ) + sin θ3 )
c2 =θ̇12 (sin θ2 + m(sin θ2 + sin(θ2 + θ3 )))
− θ̇1 θ̇3 m2 sin θ3
− θ̇2 θ̇3 m2 sin θ3
− θ̇32 m sin θ3
c3 =θ̇12 m(sin(θ2 + θ3 ) + sin θ3 )
+ θ̇1 θ̇2 m2 sin θ3
+ θ̇22 m sin θ3
h1 =g((2 + m) cos θ1 + (1 + m) cos(θ1 + θ2 ) + m cos(θ1 + θ2 + θ3 ))
h2 =g((1 + m) cos(θ1 + θ2 ) + m cos(θ1 + θ2 + θ3 ))
h3 =gm cos(θ1 + θ2 + θ3 ).
We wzorach na h, staªa g = 9.81 oznacza ziemskie przyspieszenie grawitacyjne.
14 Wahadªa z jednym zª¡czem na prostej
Niniejszy rozdziaª po±wi¦cony jest takim obiektom, »e ruch jednego z ich
kra«cowych zª¡cz jest ograniczony do prostej. Obiekty tego rodzaju s¡
zwykle interpretowane jako wielostopniowe wahadªa przymocowane do wózka
(owo szczególne zª¡cze). Sterowanie obiektem polega na przykªadaniu siªy
do wózka a jego celem jest stabilizacja wahadªa pionowo w górze.
14.1 Jedno ogniwo
Rozwa»amy zª¡cze, którego ruch jest ograniczony do prostej, a które jest
zwi¡zane z pojedynczym ogniwem. Obiekt ten jest bardzo cz¦sto bywa u»ywany do testowania algorytmów uczenia si¦ przez wzmacnianie. Jest interpretowany jako wózek z wahadªem, przy czym sterowanie wahadªem odbywa
si¦ wyª¡cznie przez przykªadanie siªy do wózka. Istniej¡ dwa warianty celu
44
Rysunek 10: Pojedyncze wahadªo na wózku.
sterowania. Wariant pierwszy (inverted pendulum [2, 1]) polega na stabilizacji wahadªa w górze. Drugi (Cart-Pole Swing-Up [4, 5]) polega na tym aby
wahadªo rozbuja¢, ustawi¢ pionowo w górze i dopiero wtedy stabilizowa¢.
Zª¡cze, którego ruch jest ograniczony do prostej b¦dziemy dalej nazywa¢
wózkiem za± jedyne ogniwo wahadªem.
Mamy cztery zmienne stanu: pozycja wózka x, k¡t θ mi¦dzy wahadªem
a pionem oraz ich pochodne po czasie ẋ, θ̇. Zmienn¡ steruj¡c¡ jest siªa F
przykªadana do wózka. Dynamika obiektu jest opisywana przez nast¦puj¡ce
równania.
Ã"
#
! ·
¸−1
−F − mp lθ̇2 sin θ + µc sgn ẋ
µp θ̇ −1 4 mp cos2 θ
θ̈ =
cos θ + g sin θ +
l
−
mc + mp
mp l
3 mc + mp
F + mp l[θ̇2 sin θ − θ̈ cos θ] − µc sgnẋ
ẍ =
.
mc + mp
Pojawiaj¡ce si¦
x
xmax = 2.4
θ
F ∈ [−10, 10]
g = 9.81
mc = 1
mp = 0.1
l = 0.5
µc = 0.0005
µp = 0.000002
powy»ej staªe maj¡ nast¦puj¡ce interpretacje i warto±ci:
pozycja wózka,
max. akceptowalna warto±¢ |x|,
k¡t wahadªa,
siªa aplikowana do wózka (w przypadku problemu
stabilizacji stosuje si¦ raczej F ∈ {−10, 10}),
ziemskie przyspieszenie grawitacyjne,
masa wózka,
masa wahadªa,
poªowa dªugo±ci wahadªa,
wspóªczynnik tarcia w ruchu wózka,
wspóªczynnik tarcia w ruchu obrotowym wahadªa.
45
Rysunek 11: Podwójne wahadªo na wózku.
14.2 Dwa ogniwa
Obiektem dynamicznym stanowi¡cym wi¦ksze ni» wózek z wahadªem wyzwanie dla algorytmów sterowania jest wózek z podwójnym wahadªem [3], Rys.
11. Obiekt jest sterowany przez siª¦ u przykªadan¡ do wózka. Celem sterowania jest stabilizacja wahadªa pionowo w górze. Stan obiektu opisywany jest
przez poªo»enie wózka, x, k¡ty θ1 , θ2 i pochodne tych wielko±ci po czasie.
Je±li oznaczmy
q = [x, θ1 , θ2 ]T
to dynamika obiektu jest opisywana przez równanie
B(q)q̈ + C(q, q̇)q̇ + h(q) = f (u)
gdzie4


d1
d2 cos θ1
d3 cos θ2
d4
d5 cos(θ1 − θ2 )  ,
B(q) =  d2 cos θ1
d3 cos θ2 d5 cos(θ1 − θ2 )
d6


0
−d2 sin(θ1 )θ̇1
−d3 sin(θ2 )θ̇2
C(θ, θ̇) =  0
0
d5 sin(θ1 − θ2 )θ̇2  ,
0 −d5 sin(θ1 − θ2 )θ̇1
0


 
0
u
h(θ) =  −f1 sin θ1  ,
f (u) =  0  ,
−f2 sin θ2
0
4 Wzory
zostaªy przepisane z [3] prawdopodobnie bez bª¦du. Poniewa» jednak w tym
przypadku trudno wykluczy¢ bª¦dy, materiaª z sekcji 14.2 nale»y raczej traktowa¢ jako
niedoskonaªe tªumaczenie odpowiedniej cz¦±ci raportu [3] i to do niego zainteresowany
czytelnik powinien si¦gn¡¢ implementuj¡c ten obiekt.
46
Tabela 1: Staªe i wspóªczynniki zwi¡zane z wózkiem z podwójnym wahadªem
staªe
d1
d2
d3
d4
d5
d6
f1
f2
= m0 + m1 + m2
= m1 l1 + m2 L1
= m2 l2
= m1 l12 + m2 L21 + I1
= m2 L1 l2
= m2 l22 + I2
= (m1 l1 + m2 L1 )g
= m2 l2 g.
g
m0
m1
m2
L1
L2
li
Ii
wspóªczynniki
9.81m/s , przyspieszenie grawitacyjne
1.5kg , masa wózka
0.5kg , masa 1-szego ogniwa
0.75kg , masa 2-go ogniwa
0.5m, dªugo±¢ 1-szego ogniwa
0.75m, dªugo±¢ 2-go ogniwa
Li /2, odlegªo±¢ mi¦dzy pocz¡tkiem
a ±rodkiem ci¦»ko±ci i-tego ogniwa
Mi L2i /12, inercja i-tego ogniwa.
2
Wspóªczynniki oraz staªe pojawiaj¡ce si¦ w powy»szych wzorach maj¡ znaczenie i warto±ci przedstawione w Tabeli 1.
14.3 Trzy ogniwa
Obiektem dynamicznym stanowi¡cym du»e wyzwanie dla algorytmów sterowania jest potrójne wahadªo na wózku [8], Rys. 12. Obiekt jest sterowany przez
siª¦ u przykªadan¡ do wózka. Celem sterowania jest stabilizacja wahadªa pionowo w górze. W odró»nieniu od poprzednio omawianych obiektów typu
wahadªo na wózku, w tym przypadku masa obiektu rozmieszczona jest w ogniwach nierównomiernie. Stan obiektu opisywany jest przez poªo»enie wózka,
x, k¡ty θ1 , θ2 , θ3 i pochodne tych wielko±ci po czasie. Je±li oznaczmy
q = [x, θ1 , θ2 , θ3 ]T
to dynamika obiektu jest opisywana przez równanie
B(q)q̈ + C(q, q̇)q̇ + h(q) = f (q, u)
gdzie5


A1
A2 cos(θ1 )
A3 cos(θ2 )
A4 cos(θ3 )
 A9 cos(θ1 )
A10
A11 cos(θ1 − θ2 ) A12 cos(θ1 − θ3 ) 
,
B(q) = 
 A18 cos(θ2 ) A19 cos(θ1 − θ2 )
A20
A21 cos(θ2 − θ3 ) 
A28 cos(θ3 ) A29 cos(θ1 − θ3 ) A30 cos(θ2 − θ3 )
A31
5 Wzory
zostaªy przepisane z [8] prawdopodobnie bez bª¦du. Poniewa» jednak w tym
przypadku trudno wykluczy¢ bª¦dy, materiaª z sekcji 14.3 nale»y raczej traktowa¢ jako
niedoskonaªe tªumaczenie odpowiedniej cz¦±ci artykuªu [8] i to do niego zainteresowany
czytelnik powinien si¦gn¡¢ implementuj¡c ten obiekt.
47
Tabela 2: Staªe pojawiaj¡ce si¦ we wzorach opisuj¡cych dynamik¦ wózka
z potrójnym wahadªem.
Staªa
A1
A2
A3
A4
A5
A6
A7
A8
A9
A10
A11
A12
A13
A14
A15
A16
A17
A18
A19
A20
warto±¢
M + m1 + m2 + m3
m1 l1 + (m2 + m3 )L1
m2 l2 + m3 L2
m3 l3
Cc
−m1 l1 − (m2 + m3 )L1
−(m2 l2 + m3 L2 )
−m3 l3
m1 l1 + (m2 + m3 )L1
I1 + m2 l12 + (m2 + m3 )L21
(m2 l2 + m3 L2 )L1
m3 l3 L1
C1 + C2
(m2 l2 + m3 L2 )L1
−C2
m3 l3 L1
−g(m1 l1 + m2 L1 + m3 L1
m2 l2 + m3 L2
(m2 l2 + m3 L2 )L1
I2 + m3 L22 + m2 l22
Staªa
A21
A22
A23
A24
A25
A26
A27
A28
A29
A30
A31
A32
A33
A34
A35
A36
A37
A38
A39
A40
warto±¢
m3 l3 L2
−(m2 l2 + m3 L2 )L1
−C2
C2 + C3
m3 l3 L2
−C3
−g(m2 l2 + m3 L2 )
m3 l3
m3 l3 L1
m3 l3 L2
I3 + m3 l32
C3
−m3 l3 L1
−gm3 l3
−m3 l3 L2
−C3
1.3
0.506
0.219
0.568


A5
A6 sin(θ1 )θ̇1
A7 sin(θ2 )θ̇2
A8 sin(θ3 )θ̇3

 0
A13
A14 sin(θ1 − θ2 )θ̇2 + A15
A16 sin(θ1 − θ3 )θ̇3
,
C(q, q̇) = 
 0 A22 sin(θ1 − θ2 )θ̇1 + A23
A24
A25 sin(θ2 − θ3 )θ̇3 + A26 
0
A33 sin(θ1 − θ3 )θ̇1
A35 sin(θ2 − θ3 )θ̇2 + A36
A32


0
 A17 sin(θ1 ) 

h(q) = 
 A27 sin(θ2 )  ,
A34 sin(θ3 )

Ks u − sign(x)µx A37
 − sign(θ1 )µ1 A38 

f (q, u) = 
 − sign(θ2 )µ2 A39  .
− sign(θ3 )µ3 A40

Staªe pojawiaj¡ce si¦ w powy»szych wzorach zostaªy zestawione w tabeli 2
oraz wspóªczynniki zestawione w tabeli 3.
48
Tabela 3: Warto±ci i znaczenie wspóªczynników rz¡dz¡cych dynamik¡ wózka
z potrójnym wahadªem.
g
M
m1
m2
m3
l1
l2
l3
L1
L2
L3
I1
I2
I3
Cc
C1
C2
C3
Ks
µx
µ1
µ2
µ3
9.8m/s2 , przyspieszenie grawitacyjne
1.014kg , masa wózka
0.4506kg , masa pierwszego ogniwa
0.219kg , masa drugiego ogniwa
0.0568kg , masa trzeciego ogniwa
0.37m, odlegªo±¢ od pocz¡tku 1-szego ogniwa do ±rodka jego masy
0.3m, odlegªo±¢ od pocz¡tku 2-go ogniwa do ±rodka jego masy
0.05m, odlegªo±¢ od pocz¡tku 3-go ogniwa do ±rodka jego masy
0.0042kgm2 , intercja 1-szego ogniwa wzgl. jego ±rodka masy
0.0012kgm2 , intercja 2-go ogniwa wzgl. jego ±rodka masy
0.00010609kgm2 , intercja 3-go ogniwa wzgl. jego ±rodka masy
5.5N ms, wpóªczynnik tarcia dynamicznego wózka o podªo»e
0.00026875N ms, wsp. tarcia dynamicznego 1-szego zª¡cza
0.00026875N ms, wsp. tarcia dynamicznego 2-go zª¡cza
0.00026875N ms, wsp. tarcia dynamicznego 3-go zª¡cza
24.7125N/V , wzmocnienie sterowanie/siªa przykªadana do wózka
0.07, wsp. tarcia coulombowskiego wózka
0.003, wsp. tarcia coulombowskiego 1-szego ogniwa
0.003, wsp. tarcia coulombowskiego 2-go ogniwa
0.003, wsp. tarcia coulombowskiego 3-go ogniwa
Rysunek 12: Potrójne wahadªo na wózku.
49
Literatura
[1] C. W. Anderson, Learning to Control an Inverted Pendulum Using
Neural Networks, IEEE Control System Magazine, vol. 9, pp. 31-37,
April 1989.
[2] A. G. Barto, R. S. Sutton, and C. W. Anderson, Neuronlike Adaptive
Elements That Can Learn Dicult Learning Control Problems, IEEE
Transactions on Systems, Man, and Cybernetics, vol. 13(5), pp. 834-846,
1983.
[3] A. Bogdanov, Optimal Control of a Double Inverted Pendulum on a
Cart, Technical Report CSE-04-006, December 2004.
[4] K. Doya, Ecient Nonlinear Control with Actor-Tutor Architecture,
Advances in Neural Information Processing Systems, vol. 9, pp. 10121018, MIT Press: Cambridge, MA, 1997.
[5] K. Doya, Reinforcement Learning in Continuous Time and Space, Neural Computation, 12:243-269, 2000.
[6] T. J. Perkins, A. G. Barto, Lyapunov Design for Safe Reinforcement
Learning, Journal of Machine Learning Research 3 (2002) 803-832.
[7] M. T. Rosenstein and A. G. Barto, Robot Weightlifting By Direct Policy Search, Proceedings of The 17th International Joint Conference on
Articial Intelligence, Seattle, Washington, August 2001, pp. 839-846.
[8] J. Si and Y.-T. Wand, On-Line Learning Control by Association and
Reinforcement, IEEE Transactions on Neural Networks, vol. 12, pp.
264-276, March 2001.
50

Symulacja dynamiki pªaskich ªa«cuchów kinematycznych

Transkrypt

Podobne dokumenty

Wnęka w s³upach aluminiowych

LEKARZ cHoRoB WEWN ĘTRZNYCH

tabela parametrów

3. Moc w słabości

Płyty Główne : H81M-P33

5_Dlaczego brak miłości.musx

Prędkość łącza i pewne niuanse

poznajemy cz