9990200020

2. Podstawy teoretyczne 13

Algorytm wymaga, aby nogi wstępnie ustawione były w pozycję neutralną (rysunek 2.5 faza 3). Następnie podnoszone zostają nogi niepodporowe (np. nogi 1,3,5). Pozycje stóp w danej iteracji wyznaczane są na podstawie ich pozycji w iteracji poprzedniej.

Aby wyznaczyć pozycję stopy w danej iteracji stosuje się następujące przeliczenia:

P?-l = O .JH,

(2.11)

(2.12)

f? = T-4ri?

Pi — O^-1- _Pf,

gdzie macierz translacji T ma następującą postać:

• dla nóg podporowych

’cos(Aq:₂) — sin(Aa:₂) 0 —Aa:’ sin(Aa₂)    cos(Aa₂)    0 A y

0    0    10

0    0    0 1    .

• dla nóg niepodporowych

"cos(—Aa₂) sin(—Aq:₂) 0 0

- sin(—Ack₂) cos(—Aa₂) 0 0

Aa: ’ -A y 0 1

(2.13)

Aa:, Ay, Aa_z są to przyrosty drogi w danej iteracji:

Aa: = At • v_x,

Ay = At-v_y,    (2.14)

Aa_z = At • v_az.

Zmiana nóg podporowych dokonywana jest, gdy któraś z nóg wykroczy poza przyjęty zakres roboczy. Podczas zmiany nóg podporowych nogi opuszczane są nezależnie do momentu kontaktu stopy z podłożem. W ten sposób w kolejnych iteracjach algorytmu uwzględniona zostanie nierówność podłoża.