9990200024

2. Podstawy teoretyczne 17

Równania filtru Kalmana w fazie predykcji są następujące

(2.21)

żk = Alk-1 + ^Buk-1 Ą- = aĄ-i^^t + Q

oraz P*T to estymowanie stanu procesu i macierzy kowariancji a priori, Xk oraz Pk to szacowne wartości stanu kowariancji a posteriori z poprzedniego kroku, natomiast Q jest to macierz wariancji procesu.

W drugiej fazie wyznaczane jest wzmocnienie Kalmana

K_k = P_kH^r(HP_kH^T + R)~\    (2.22)

gdzie R jest to wariancja pomiaru.

Następnie dokonywana jest korekcja estymowanego stanu oraz korekta macierzy kowariancji

(2.23)

x_h =x_k +K_k(z_k - Hx_k) P„ = (I- K_KH)P_k

gdzie Zk to pomiar, natomiast I to macierz jednostkowa.

2.9 Wyznaczanie prędkości oraz przemieszczenia na podstawie przyspieszeń

Określenie położenia oraz orientacji robota względem globalnego układu odniesienia realizowane jest jako złożenie translacji oraz rotacji:

T = Trans{x, y, z) ■ Rot(9_z, 9_y, 6_X)    (2.24)

Ponieważ czujniki rejestrujące przyspieszenie i prędkość kątową robota nie są zorientowane tak samo jak układ globalny, koniecznie jest stosowne przeliczenie tych wartości. Sposób przeliczenia prędkości kątowych przedstawiono w rozdziale 2.7.

Akcelerometry umieszczone w osiach OX, OY, OZ zmieniają orientację wraz z robotem. Aby móc wykorzystać je do pomiaru przyspieszeń w układzie globalnym, konieczna jest wpierw eliminacja przyspieszenia ziemskiego, następnie przeliczenie przyspieszeń z układu lokalnego na układ globalny.

Sposób eliminacji przyspieszenia ziemskiego oraz przyspieszenia odśrodkowego, przedstawiono w pracy [12] i wygląda on następująco:

a_x = A_x + V_yui_z — V_zu_y + g sin 9_y

a_y = A_y — V_xu>_z — V_zu>_x — g cos 9_y sin 9_X.    (2.25)

a_z = A_z + V_xuj_y — V_yuj_x — g cos 9_y cos 9_X

Jak widać, przyspieszenie odśrodkowe zależy od prędkości przemieszczania się robota. Ponieważ badany w pracy robot kroczący porusza się z niewielką prędkością, postanowiono zrezygnować z korekcji przyspieszenia wynikającej z przyspieszenia odśrodkowego. Ostatecznie stosuje się jedynie eliminację przyspieszenia ziemskiego:

a_x = A_x + g sin 9_y

a_y = A_y — g cos 9_y sin 9_X.    (2.26)

a_z = A_z — g cos 9_y cos 9_X