Opis kinematyki robotów z wykorzystaniem rachunku kwaternionowego

Opis kinematyki robotów z wykorzystaniem rachunku kwaternionowego

Sebastian Juszczyński, e-mail: sebastian.juszczynski.stud@pw.edu.pl 5 czerwca 2019

1 Wstęp

1.1 Definicja

Liczby zespolone sa to pary liczb rzeczywistych, dzięki którym możemy dowie- dzieć się o geometrii płaszczyzny. Słynny irlandzki matematyk William Rowan Hamilton (1805-1865) spędził sporo czasu na próbach zbudowania analogicz- nego systemu dla trójek liczb rzeczywistych, za pomocą którego chciał badać geometrię przestrzenną. Hamilton doszedł jednak do wniosku, że zamiast trzech współrzędnych należy wziąć cztery i zbudować nowy system liczb - liczby hi- perzespolone, które nazwał kwaternionami.

Te nowe liczby będące generalizacją liczb zespolonych, są definiowane przez cztery liczby rzeczywiste. Jedna z osi tego układu przedstawia liczby rzeczy- wiste, a trzy pozostałe - liczby urojone.

Kwaternion to liczba postaci:

q = (s, m) = s + xi + yj + zk

gdzie s to część skalarna, natomiast jego część urojona jest trójwymiarowym wektorem m = xi + yj + zk.[1]

1.2 Działania na kwaternionach

Dodawanie i mnożenie na kwaternionach wykonujemy jak na wielomianach czte- rech zmiennych, ale do mnożenia należy stosować reguły Hamiltona:

i²= j²= k²= −1

ij = k, jk = i, ki = j

ji = −k, kj = −i, ik = −j ,

przy czym należy pamiętać, że mnożenie kwaternionów Hamiltona nie jest prze- mienne. Spełniają jednak one prawa łączności i rozdzielności.

Zgodnie z definicją kwaternionów, q1 i q2:

q1= (s1, m1) = s1+ x1i + y1j + z1k

q2= (s2, m2) = s2+ x2i + y2j + z2k Dodawanie jest zdefiniowane następująco:

q1+ q2= (s1+ s2, m1+ m2) = (s1+ s2) + (x1+ x2) i + (y1+ y2) j + (z1+ z2) k Mnożenie skalarne (·):

q1· q2= s1· s2+ m1· m2

Mnożenie kwaternionów (⊗):

q1⊗ q2= (s1· s2− m1· m2, s1· m2+ s2· m1+ m1× m2)

Dla każdego kwaternionu istnieje kwaternion sprzężony q^∗ oraz odwrotny q⁻¹:

q^∗= (s, −m)

q⁻¹ = 1

|q|

² q^∗

gdzie (miara kwaternionu):

|q|²= s²+ x²+ y²+ z²

2 Opis kinematyki przy pomocy kwaternionów jednostkowych

2.1 Odniesienie do notacji Denavita-Hartenberga

Tworząc opis kinematyki przy pomocy kwaternionów należy posłużyć się para- metrami znanymi z notacji Denavita-Hartenberga, są to: α - kąt skręcenia członu (obrót wokół osi X), a - długość członu (przesunięcie wzdłuż osi X), d - odsunię- cie przegubu (przesunięcie wzdłuż osi Z) oraz θ - kąt przegubu (obrót wokół osi Z). Każda transformacja pomiędzy sąsiednimi układami współrzędnych opisana przy pomocy macierzy transformacji, znajduje swoje odwzorowanie w notacji kwaternionowej.

2.2 Reprezentacja rotacji

Rotacja definiowana jest za pomocą kwaternionów jednostkowych, dla których

|q| = 1. Iloczyn dwóch kwaternionów jednostkowych będzie również tej samej postaci. Z tego wynika, że za pomocą jednego kwaternionu można przedstawić N rotacji: qr= qr1⊗ qr2⊗ qr3⊗ . . . ⊗ qrN.

Kwaternion reprezentujący rotację przyjmuje postać:

qr=









cos(α/2) v_x· sin(α/2) v_y· sin(α/2) v_z· sin(α/2)









gdzie α jest kątem obrotu, a wektor v = 

v_x v_y v_z T

jest wektorem jed- nostkowym reprezentującym oś obrotu[2].

W szczególności, przyjmując oznaczenia używane przy opisie kinematyki robota możemy wyszczególnić dwa kwaterniony reprezentujące obroty o poszczególne kąty α oraz θ:

qr,θ =









cos(θ/2) 0 0 sin(θ/2)







 , qr,α=









cos(α/2) sin(α/2)

0 0









które po wymnożeniu dają ogólny wzór na kwaternion reprezentujący rotację robota, o określonym α oraz θ[3]:

qr= qr,θ⊗ qr,α=









cos (α/2) cos (θ/2) sin (α/2) cos (θ/2) sin (α/2) sin (θ/2) cos (α/2) sin (θ/2)









2.3 Obrót punktu

Za pomocą wyznaczonego kwaternionu rotacji qrmamy możliwość obrotu punktu określonego w przestrzeni p =

px py pz ^T

. W tym celu należy zdefiniować punkt w notacji kwaternionowej qp = 

0 px py pz ^T

. Wynikiem prze- kształcenia będzie kwaternion q_p⁰ = 

0 p⁰_x p⁰_y p⁰_z ^T

reprezentujący prze- kształcony punkt p⁰=

p⁰_x p⁰_y p⁰_z T

.

q⁰_p= q_r⊗ qp⊗ q_r⁻¹

3 Kwaterniony dualne i pełny opis kinematyki

3.1 Kwaterniony dualne

Dualne kwaterniony zostały zaproponowane przez William’a Kingdom Clifford’a w 1873 roku. Są rozszerzeniem kwaternionów jednostkowych. Reprezentują za-

równo rotacje, jak i translacje, których połączenie definiowane jest jako pełna transformacja. Są one reprezentowane przez wektor składający się z ośmiu ele- mentów, na które składają się dwa kwaterniony jednostkowe odpowiadające od- powiednio za rotację (qr) oraz iloczyn translacji i rotacji (qd). Na bezpośredni kwaternion translacji (qt) składa się wektor t =

t_x t_y t_z T

opisujący prze- sunięcia w danej osi. Dodatkowo ε stanowi operator umożliwiający przejście do operacji na kwaternionach dualnych[4]:

q = q_r+ q_dε

q_d= 1 2q_t⊗ q_r

q_t=







 0 tx

ty

tz







 , q_r=









cos(α/2) vx· sin(α/2) vy· sin(α/2) vz· sin(α/2)









3.2 Działania na kwaternionach dualnych

Dodawanie:

q1+ q2= qr1+ qr2+ (qd1+ qd2) ε Mnożenie:

q1⊗ q2= qr1⊗ qr2+ (qr1⊗ qd2+ qd1⊗ qr2) ε Kwaternion sprzężony:

q^∗ = q_r^∗− q_d^∗ε

3.3 Reprezentacja rotacji i translacji

Dokładny opis kinematyki przy użyciu kwaternionów dualnych można przedsta- wić w następujący sposób:

q = qθz⊗ qαx

qθz = qr,θ+ qd,zε = qr,θ+ (1

2qt,z⊗ qr,θ)ε qαx= qr,α+ qd,xε = qr,α+ (1

2qt,x⊗ qr,α)ε

qr,θ =









cos(θ/2) 0 0 sin(θ/2)







 , qt,z=







 0 0 0 d









q_r,α=









cos(α/2) sin(α/2)

0 0







 , q_t,x=







 0 a 0 0









q_d,z =









−^d₂sin(θ/2) 0 0

d

2cos(θ/2)







 , q_d,x=









−^a₂sin(α/2)

a

2cos(α/2) 0 0









qθz=

























cos(θ/2) 0 0 sin(θ/2)

−^d₂sin(θ/2) 0 0

d

2cos(θ/2)























 , qαx=

























cos(α/2) sin(α/2)

0 0

−^a₂sin(α/2)

a

2cos(α/2) 0 0

























q = q_θz⊗ qαx=

























cos (α/2) cos (θ/2) sin (α/2) cos (θ/2) sin (α/2) sin (θ/2) cos (α/2) sin (θ/2)

−^a₂sin (α/2) cos (θ/2) −^d₂cos (α/2) sin (θ/2)

a

2cos (α/2) sin (θ/2) −^d₂sin (α/2) sin (θ/2)

a

2cos (α/2) sin (θ/2) +^d₂sin (α/2) cos (θ/2)

−^a₂sin (α/2) sin (θ/2) + ^d₂cos (α/2) cos (θ/2)

























Podobnie jak w przypadku kwaternionów jednostkowych, za pomocą jednego kwaternionu dualnego można przedstawić N przekształceń reprezentujących jed- nocześnie rotację i translację: q = q1⊗ q2⊗ q3⊗ . . . ⊗ qN.

3.4 Przekształcenie punktu

Przy pomocy wyznaczonego kwaternionu dualnego q mamy możliwość trans- formacji punktu określonego w przestrzeni p =

px py pz

T

. W tym celu należy zdefiniować punkt w notacji kwaternionu dualnego:

qp =

1 0 0 0 0 p_x p_y p_z T

Wynikiem przekształcenia będzie kwaternion:

q_p⁰ =

1 0 0 0 0 p⁰_x p⁰_y p⁰_z ^T

reprezentujący przekształcony punkt p⁰ =

p⁰_x p⁰_y p⁰_z ^T .

q_p⁰ = q ⊗ qp⊗ q^∗

3.5 Przykład zaimplementowany w środowisku Matlab

% d e f i n i c j a p a r a m e t r ó w

a1 =5; d1 =2; a l f a 1 = pi /2; t e t a 1 = pi /4;

a2 =3; d2 =1; a l f a 2 = pi /4; t e t a 2 = pi /6;

% w s p ó ł r z ę d n e p u n k t u p o c z ą t k o w e g o px =5; py =5; pz =5;

% m a c i e r z e DH

DH1 =[ cos ( t e t a 1 ) - sin ( t e t a 1 ) * cos ( a l f a 1 ) sin ( t e t a 1 ) * sin ( a l f a 1 ) a1 * cos ( t e t a 1 ) ;...

sin ( t e t a 1 ) cos ( t e t a 1 ) * cos ( a l f a 1 ) - cos ( t e t a 1 ) * sin ( a l f a 1 ) a1 * sin ( t e t a 1 ) ;...

0 sin ( a l f a 1 ) cos ( a l f a 1 ) d1 ;...

0 0 0 1];

DH2 =[ cos ( t e t a 2 ) - sin ( t e t a 2 ) * cos ( a l f a 2 ) sin ( t e t a 2 ) * sin ( a l f a 2 ) a2 * cos ( t e t a 2 ) ;...

sin ( t e t a 2 ) cos ( t e t a 2 ) * cos ( a l f a 2 ) - cos ( t e t a 2 ) * sin ( a l f a 2 ) a2 * sin ( t e t a 2 ) ;...

0 sin ( a l f a 2 ) cos ( a l f a 2 ) d2 ;...

0 0 0 1];

% p u n k t p o c z ą t k o w y w n o t a c j i k w a t e r n i o n u d u a l n e g o q_p = [ 1 ; 0 ; 0 ; 0 ; 0 ; px ; py ; pz ];

% k w a t e r n i o n y d u a l n e o r a z k w a t e r n i o n s p r z ę ż o n y q1 = d u a l _ q ( a1 , d1 , alfa1 , t e t a 1 ) ;

q2 = d u a l _ q ( a2 , d2 , alfa2 , t e t a 2 ) ; q = d u a l _ i l o c z y n ( q1 , q2 ) ;

q_s = d u a l _ s p r z ( q ) ;

% w y n i k i o b l i c z e ń w c e l u p o r ó w n a n i a w y n i k _ D H = DH1 * DH2 *[ px ; py ; pz ; 1 ] ;

w y n i k _ q = d u a l _ i l o c z y n ( d u a l _ i l o c z y n ( q , q_p ) , q_s ) ; p u n k t _ D H = w y n i k _ D H ( 1 : 3 )

p u n k t _ q = w y n i k _ q ( 6 : 8 )

% p u n k t p r z e k s z t a ł c o n y p r z y p o m o c y m a c i e r z y DH p u n k t _ D H =

1 4 . 1 4 1 6 2 . 7 2 7 4 6 . 0 0 0 0

% p u n k t p r z e k s z t a ł c o n y p r z y p o m o c y k w a t e r n i o n u d u a l n e g o p u n k t _ q =

1 4 . 1 4 1 6 2 . 7 2 7 4 6 . 0 0 0 0

f u n c t i o n [ qi ] = d u a l _ q ( ai , di , alfai , t e t a i )

% f u n k c j a t w o r z ą c a k w a t e r n i o n d u a l n y qi =[ cos ( a l f a i /2) * cos ( t e t a i /2) ;...

sin ( a l f a i /2) * cos ( t e t a i /2) ;...

sin ( a l f a i /2) * sin ( t e t a i /2) ;...

cos ( a l f a i /2) * sin ( t e t a i /2) ;...

-( ai /2) * sin ( a l f a i /2) * cos ( t e t a i /2) -( di /2) * cos ( a l f a i /2) * sin ( t e t a i /2) ;...

( ai /2) * cos ( a l f a i /2) * cos ( t e t a i /2) -( di /2) * sin ( a l f a i /2) * sin ( t e t a i /2) ;...

( ai /2) * cos ( a l f a i /2) * sin ( t e t a i /2) +( di /2) * sin ( a l f a i /2) * cos ( t e t a i /2) ;...

-( ai /2) * sin ( a l f a i /2) * sin ( t e t a i /2) +( di /2) * cos ( a l f a i /2) * cos ( t e t a i /2) ];

end

f u n c t i o n [ qi ] = d u a l _ s p r z ( q )

% f u n k c j a t w o r z ą c a d u a l n y k w a t e r n i o n s p r z ę ż o n y qi =[ q _ s p r z ( q ( 1 : 4 ) ) ;...

- q _ s p r z ( q ( 5 : 8 ) ) ];

end

f u n c t i o n [ qi ] = q _ s p r z ( q )

% f u n k c j a t w o r z ą c a j e d n o s t k o w y k w a t e r n i o n s p r z ę ż o n y qi =[ q (1) ; - q (2) ; - q (3) ; - q (4) ];

end

f u n c t i o n [ qi ] = d u a l _ i l o c z y n ( q1 , q2 )

% f u n k c j a r e a l i z u j ą c a m n n o ż e n i e k w a t e r n i o n ó w d u a l n y c h qr1 = q1 ( 1 : 4 ) ;

qd1 = q1 ( 5 : 8 ) ; qr2 = q2 ( 1 : 4 ) ; qd2 = q2 ( 5 : 8 ) ;

q r 1 q r 2 = q _ i l o c z y n ( qr1 , qr2 ) ; q r 1 q d 2 = q _ i l o c z y n ( qr1 , qd2 ) ; q d 1 q r 2 = q _ i l o c z y n ( qd1 , qr2 ) ; qi =[ q r 1 q r 2 ;...

q r 1 q d 2 + q d 1 q r 2 ];

end

f u n c t i o n [ qi ] = q _ i l o c z y n ( q1 , q2 )

% f u n k c j a r e a l i z u j ą c a m n o ż e n i e k w a t e r n i o n ó w j e d n o s t k o w y c h

qi =[ q1 (1) * q2 (1) - q1 (2) * q2 (2) - q1 (3) * q2 (3) - q1 (4) * q2 (4) ; q1 (1) * q2 (2) + q1 (2) * q2 (1) + q1 (3) * q2 (4) - q1 (4) * q2 (3) ; q1 (1) * q2 (3) - q1 (2) * q2 (4) + q1 (3) * q2 (1) + q1 (4) * q2 (2) ; q1 (1) * q2 (4) + q1 (2) * q2 (3) - q1 (3) * q2 (2) + q1 (4) * q2 (1) ];

end

4 Bibliografia

[1] Kwaterniony. http://www.math.edu.pl/kwaterniony.

[2] Understanding Quaternions. http://www.chrobotics.com/library/

understanding-quaternions.

[3] Ricardo D. Quinteros Mario A. Storti Alberto Cardona, Paul H. Kohan.

A comparative study of the kinematics of robots manipulators by denavit- hartenberg and dual quaternion. Mec´anica Computacional Vol XXXI, 2012.

[4] Ben Kenwright. A beginners guide to dual-quaternions. School of Computing Science, Newcastle University, 2012.