Warszawa,2019 Wykład2-KinematykaMechanizmówWieloczłonowychdrinż.JakubMożaryn,mgrinż.JanKlimaszewski SterowanieMechanizmówWieloczłonowych

(1)

Sterowanie Mechanizmów Wieloczłonowych

Wykład 2 - Kinematyka Mechanizmów Wieloczłonowych

dr inż. Jakub Możaryn, mgr inż. Jan Klimaszewski

Instytut Automatyki i Robotyki

Warszawa, 2019

(2)

Zadanie proste kinematyki

Zadanie proste kinematyki MW - obliczenie współrzędnych kartezjańskich (zewnętrznych) punktu na MW znając jego współrzędne uogólnione. Za- zwyczaj chodzi o jeden z punktów ostatniego członu MW w układzie bazowym.

P_Am = T^m_kP_Ak, k > m (1)

gdzie: P_Ak =





 xAk

yAk

zAk

1







- współrzędne punktu A w k-tym układzie

P_Am=





 x_Am y_Am z_Am 1







- współrzędne punktu A w m-tym układzie

T^m_k = T^m_m+1T^m+1_m+2. . . T^k−1_k =R^m_k D_k^m

0 1

(2)

(3)

Zadanie proste kinematyki - ujęcie ogólne

Mając dane współrzędne uogólnione q1, · · · , qnnależy wyznaczyć położe- nie i orientację ostatniego członu MW.

Wykorzystuje się do tego macierz przekształceń

H = T⁰_n(q1, · · · , qn) =R D

0 1

(3)

gdzie: gdzie: R ∈ R^3×3 - macierz obrotu, D ∈ R^3×1 - wektor opisujący przesunięcie.

Macierz przekształcenia pomiędzy układami współrzędnych (bazowy / koń- cówka MW)

H = T⁰_n(q1, · · · , qn) = T⁰₁(q1)T¹₂(q2) · · · Tⁿ⁻¹_n (qn) (4) Dla zadania prostego kinematyki zawsze istnieje rozwiązanie.

(4)

Zadanie proste kinematyki - przykład

Rysunek:Schemat nogi z DH.

Rysunek:Wizualizacja 3D robota czteronożnego

i a_i α_i d_i θ_i 1 l₁ 0 0 θ₁ 2 l2 0 0 θ2

Tablica:Parametry DH.

(5)

Zadanie proste kinematyki - przykład

Dla omawianego modelu z rys. 1 macierz transformacji z układu 0 do układu 2 jest równa:

T⁰₂= T⁰₁· T¹₂ (5) gdzie

T⁰₁=







c₁ −s₁ 0 l₁c₁ s1 c1 0 l1s1

0 0 1 0

0 0 0 1





 , T¹₂=







c₂ −s₂ 0 l₂c₂ s2 c2 0 l2s2

0 0 1 0

0 0 0 1







(6)

przyjmując oznaczenia snm= sin(θn+ θm), cnm= cos(θn+ θm) otrzymujemy:

T⁰₂=







c12 −s12 0 l1c1+ l2c12

s12 c12 0 l1s1+ l2s12

0 0 1 0

0 0 0 1







(7)

(6)

Zadanie odwrotne kinematyki - ujęcie ogólne

Mając dane położenie i orientację ostatniego członu MW w postaci transformacji

H = T⁰_n(q₁, · · · , q_n) =R D

0 1

(8) rozwiąż równanie (in. znajdź współrzędne uogólnione q1, · · · , qn)

H = T⁰_n(q1, · · · , qn) = T⁰₁(q1)T¹₂(q2) · · · Tⁿ⁻¹_n (qn) (9) Otrzymujemy układ równań skalarnych:

Tij(q1, · · · , qn) = hij, i = 1, . . . , 3, j = 1, . . . , 4 (10) Układ może mieć wiele rozwiązań lub nie mieć żadnego.

(7)

Zadanie odwrotne kinematyki - metody

Rozwiązanie analityczne w formie zamkniętej

qk = fk(h11, · · · , h34), k = 1, · · · , n (11)

Rozwiązanie algebraiczne.

Rozwiązanie geometryczne.

Rozwiązanie numeryczne. Na przykład całkując równanie różniczkowe

v ω

= J(Q) · ˙Q, (12)

gdzie: ˙Q = [ ˙q₁, · · · , ˙q_n]^T, J ∈ R^6×n - Jakobian.

(8)

Zadanie odwrotne kinematyki - uwagi

UWAGA 1: Rozwiązanie analityczne daje dokładniejszy wynik niż rozwią- zanie numeryczne, ale jest trudne do obliczenia.

UWAGA 2: Istnieją metody upraszczające rozwiązywanie zadania odwrot- nego. Np. odsprzęganie kinematyczne - dla manipulatorów o 6-ciu stawach, jeśli osie 3 ostatnich przecinają się w jednym punkcie, możliwe jest roz- dzielenie problemu na dwa prostsze: zadanie odwrotnej kinematyki pozycji i zadanie odwrotnej kinematyki orientacji.

(9)

Prędkość członów MW

Każdy człon MW posiada prędkość kątową ω i prędkość liniową V . Prędkość kątowa członu i wyrażona w układzie bazowym 0 wynosi

ω⁰_i =

i

X

j =1

ω⁰_j =

i

X

j =1

ρjq˙jZ_{j −1}⁰ (13)

gdzie

ρ_j =

(1 jeżeli przegub j jest obrotowy

0 jeżeli przegub j jest przesuwny (14) oraz

Z_{j −1}⁰ = R⁰_{j −1}0 0 1^T (15) Prędkość liniową środka układu współrzędnych członu i wyrażona w układzie bazowym 0 wynosi:

V_i⁰= ˙D_i⁰=

i

X

j =1

∂D_i⁰

∂qj

˙

q_j (16)

(10)

Zadanie proste kinematyki prędkości - jakobian

Wyznaczenie prędkości V_n⁰i ω⁰_nostatniego członu MW na podstawie znanych prędkości współrzędnych uogólnionych ˙Q = [ ˙q₁, · · · , ˙q_n]^T.

V_n⁰ ω_n⁰

= J(Q) · ˙Q (17)

gdzie: J(Q) - jakobian.

Równanie (9) można przedstawić jako

V_n⁰ ω⁰_n

=J_D(Q) J_R(Q)

· ˙Q (18)

gdzie: JD - jakobian przemieszczenia, JR - jakobian obrotu.

(11)

Jakobian przemieszczenia i obrotu

Jakobiany JD(Q) oraz JR(Q) można wyznaczyć na podstawie znanych prędkości współrzędnych uogólnionych ˙Q = [ ˙q1, · · · , ˙qn]^T.

JD(Q) =h

∂D⁰_n

∂q₁

∂D⁰_n

∂q₂ · · · ^∂D_∂q⁰ⁿ

n

i

(19)

JR(Q) =ρ₁Z₀⁰ ρ2Z₁⁰ · · · ρnZ_n−1⁰

(20) gdzie Z₀⁰=0 0 1^T.

(12)

Zadanie proste kinematyki prędkości - przykład

Dla omawianego modelu z rys. 1 położenie ostatniego układu współrzęd- nych:

(dx = l1c1+ l2c12

dy = l1c1+ l2c12

(21) różniczkując po czasie otrzymujemy

(vx = ˙dx = −l1s1θ˙1− l2s12( ˙θ1+ ˙θ2)

vy = ˙dy = l1c1θ˙1+ l2c12( ˙θ1+ ˙θ2) (22) ostatecznie

V =v_x v_y

=

d˙_x d˙y

=(−l₁s₁− l₂s₁₂) (−l₂s₁₂) (l₁c₁+ l₂c₁₂) (l₂c₁₂)

θ˙₁ θ˙2

= J_D(θ)

θ˙₁ θ˙2

(23)

(13)

Zadanie odwrotne kinematyki prędkości

Wyznaczenie prędkości współrzędnych uogólnionych ˙Q = [ ˙q1, · · · , ˙qn]^T na podstawie znanych prędkości V i ω końcówki MW.

Q = [J(Q)]˙ ⁻¹V ω

(24)

Rozwiązanie takiego problemu występuje tylko, gdy J jest macierzą nie- osobliwą, tzn. det(J(Q)) 6= 0

(14)

Sterowanie Mechanizmów Wieloczłonowych

Wykład 2 - Kinematyka Mechanizmów Wieloczłonowych

dr inż. Jakub Możaryn, mgr inż. Jan Klimaszewski

Instytut Automatyki i Robotyki

Warszawa, 2019