Materiály k 3. přednášce: Inverzní úloha kinematiky, Jacobián - FBMI

Přednáška 3
Inovace výuky předmětu Robotika v lékařství
Inverzní úloha kinematiky, Jacobián kinematického řetězce, Newtonova
iterační metoda s Jakobiánem
Jacobián kinematického řetězce je matice, která transformuje limitně malou změnu orientace
a polohy koncového bodu kinematického řetězce na limitně malou změnu zobecněných
souřadnic v jednotlivých kin. dvojicích řetězce a naopak. Platí tedy následující vztahy :
( q)
q&
x&
= J ⋅
( q)
x
q&
= J ⋅
T &
(5.1)
x& je vektor kartézských rychlostí koncového bodu kinem.
řetězce
q& je vektor zobecněných rychlostí v jednotlivých kinem.
dvojicích řetězce
Můžeme také říci, že Jakobián transformuje zobecněné rychlosti (vyjádřené v systému
souřadnic dané kin. dvojice) v kinematických dvojicích řetězce na kartézské rychlosti jeho
koncového bodu (vyjádřené v systému souřadnic prvního členu řetězce- rámu).
Způsob získání jakobiánu :
Vypočteme matice rychlostí základních pohybů kin. dvojic řetězce :
V
V
V
V
V
10
1
= 10
ω ⋅ DRZ
21 = ω
2
21
32 = ω
3
32
43 = ω
4
43
54 = ω
5
54
⋅ DRX
⋅ DRX
⋅ DRX
⋅ DRZ
(5.2)
Poté všechny matice rychlostí základních pohybů převedeme do systému souřadnic poslední
kinem. dvojice, abychom je mohli sečíst.
V
V
V
10
5
21
5
32
5
= T ⋅T
⋅T
⋅T
⋅ V ⋅T
⋅T
⋅T
⋅T
−1
54
−1
54
−1
43
−1
43
−1
32
−1
32
−1
21
21
2
10
1
= T ⋅T
⋅T
⋅ V ⋅T
⋅T
⋅T
= T ⋅T
⋅ V ⋅T
⋅T
−1
54
−1
43
32
3
= T ⋅ ⋅T
− V V
, tj. platí
43
5
1
54
43
4
54
43
32
54
21
43
32
54
43
54
Stránka 1 z 6

Inovace výuky předmětu Robotika v lékařství
V = V + V + V + V + V
50
5
10
5
21
5
32
5
43
5
54 .
5
(5.3)
Tuto celkovou matici rychlosti mezi tělesem 5 vůči tělesu 0 v systému souřadnic tělesa 5, tj.
V převedeme do systému souřadnic tělesa 0 následovně :
50
5
V
50
0
= T ⋅T
⋅T
⋅T
⋅T
⋅ V ⋅T
⋅T
⋅T
⋅T
⋅T
−1
10
−1
21
−1
32
−1
43
−1
54
Matice rychlosti V 50 vyjadřuje rotační a translační rychlost tělesa 5 vůči tělesu 0 vyjádřenou
0
v systému souřadnic tělesa 0. Dále musíme vyjádřit vektor translačních rychlostí koncového
bodu M kinematického řetězce vůči tělesu 0 (rámu), tj.
v = T ⋅r
= T ⋅ V ⋅r
&
M
1
n1
M
n
n1
v obecné podobě, ale v našem příkladu
v = T ⋅ V ⋅ r ,
M
0
50
50
5
M
5
n1
n
M
n
.
50
5
přičemž pro celkovou transformační matici platí vztah
T = T ⋅ T ⋅ T ⋅ T ⋅ T
50
10
Matici rychlosti
21
50
0
32
43
54
,
54
43
32
21
10
(5.5)
(5.4)
V tak i vektor translačních složek rychlosti koncového bodu M
v
použijeme pro získání Jacobiánu paže. Nejdříve si z obou dvou sestavíme vektor translační a
rotační složky kartézské rychlosti následovně :
⎡v
⎢
⎢v
⎢
⎣
v
M
x 0
M
y 0
M
z 0
⎡ω
x
⎢
⎢ω
y
⎢
⎣
ω z
rychlostí je
⎤
⎥
⎥ = T50 ⋅ V50
⋅r
5
⎥
⎦
500
500
500
⎤ ⎡V
⎥ ⎢
⎥ = ⎢V
⎥ ⎢
⎦ ⎣
V
500
500
500
( 3,
2)
( 1,
3)
M
5
⎤
( )⎥ ⎥⎥ ,
2,
1
⎦
(5.6)
⎡ 0 − ωza
ωya
⎤
O
⎢
⎥ ⎡
b Ω ⎤
ba v
Ω ba = ⎢ ωza
0 − ωxa
⎥ a a
a
V .
a
ba = a ⎢ ⎥
T
⎢
⎥ ⎢ 0 0 ⎥
⎣−
ωya
ωxa
0
⎣ ⎦
⎦
(5.7)
protože platí že submatice úhlových
0
Stránka 2 z 6

Inovace výuky předmětu Robotika v lékařství
Pokud z výrazů 5.6 a 5.7 na pravých stranách vytkneme zobecněné rychlosti, získáme rovnici
v maticovém tvaru, kde matice na pravé straně představuje Jacobián paže, tj.:
⎡ vx
⎢
⎢ v y
⎢ vz
⎢
⎢ω
x
⎢
ω
⎢ y
⎢
⎣
ω z
M
0
M
0
M
0
500
500
500
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎦
=
[ J ( q)
]
⎡q&
1 ⎤
⎢ ⎥
⎢
q&
2 ⎥
⋅ ⎢q&
⎥ 3
⎢ ⎥
⎢q&
4 ⎥
⎢
⎣q&
⎥
5 ⎦
, kde q& n je n-tá zobecněná rychlost n-té kin. dvojice
Princip newtonovi iterační metody s Jakobiánem, která řeší úlohu inverzní kinematiky je
patrný z následujícího diagramu :
kde:
X
f(g) – X = g(q)
g(q) ≤ error
Yes
qk equal to
f(g) – X = g(q)≅0
No
qk+1 = qk – J -1 (qk)g(qk)
X Žádaná poloha koncového bodu kinematického řetězce v kart. souřadnicích.
f ( q)
Aktuální hodnota koncového bodu řetězce, která je funkcí zobecněných
souřadnic jednotlivých kinematických dvojic
J ( q)
Jakobián, který transformuji rychlosti zobecněných souřadnic (v kin. dvojicích)
na rychlosti koncového bodu řetězce (v kart. souřadnicích). Platí tedy X& = J(
q)
q&
a
−1
&
q&
= J ( q)
X .
Stránka 3 z 6

Inovace výuky předmětu Robotika v lékařství
q k vektor zobecněných souřadnic v posledním kroku iterace, kde platí že
f ( qk
) − X = g(
q)
≅ 0
g (q)
chybová hodnota mezi žádanou polohou konc. bodu řetězce a aktuální
hodnotou koncového bodu řetězce při daných zobecněných souřadnicích q .
Vstupem pro newtonovu iterační metodu je žádaná poloha konc. bodu řetězce a jejím
výstupem je odpovídající vektor zobecněných souřadnic, který této požadované poloze
odpovídá. Řešení může být nekonečně mnoho, nebo jen jedno, ale také nemusí existovat,
například pokud je žádaná poloha konc. bodu mimo manipulační prostor paže robota.
Využití symbolic toolboxu Matlabu pro výpočet Jacobiánu kinematického řetězce v laboratoři
vysvětluje následný okomentovaný kód :
clear all
clc
% konstanty R21,R32,R43,R54,R5M
R10 = sym ('R10');
R21 = sym ('R21');
R32 = sym ('R32');
R43 = sym ('R43');
R54 = sym ('R54');
R5M= sym ('R5M');
% promennne
% zobecněné polohy v kin. dvojicích
fi10 = sym ('fi10');
fi21 = sym ('fi21');
fi32 = sym ('fi32');
fi43 = sym ('fi43');
fi54 = sym ('fi54');
% zobecněné rychlosti v kin. dvojicích
om10 = sym ('om10');
om21 = sym ('om21');
om32 = sym ('om32');
om43 = sym ('om43');
om54 = sym ('om54');
r21=[0; R21; 0; 1]; % pruvodice mezi kinem dvojicemi
r32=[0; 0; R32; 1];
r43=[0; 0; R43; 1];
r54=[0; 0; R54; 1];
r5M=[0; 0; R5M; 1];
% transformacni matice
TT10=[cos(fi10) -sin(fi10) 0 0; sin(fi10) cos(fi10) 0 0; 0 0 1 0; 0 0 0 1];
% rotace kolem z-ove osy o fi10
TT21=[1 0 0 0; 0 cos(fi21) -sin(fi21) 0; 0 sin(fi21) cos(fi21) 0; 0 0 0 1];
% rotace kolem x-ove osy o fi21
Stránka 4 z 6

Inovace výuky předmětu Robotika v lékařství
TT32=[1 0 0 0; 0 cos(fi32) -sin(fi32) 0; 0 sin(fi32) cos(fi32) 0; 0 0 0 1];
% rotace kolem x-ove osy o fi32
TT43=[1 0 0 0; 0 cos(fi43) -sin(fi43) 0; 0 sin(fi43) cos(fi43) 0; 0 0 0 1];
% rotace kolem x-ove osy o fi43
TT54=[cos(fi54) -sin(fi54) 0 0; sin(fi54) cos(fi54) 0 0; 0 0 1 0; 0 0 0 1];
% rotace kolem z-ove osy o fi54
TT50=TT10*TT21*TT32*TT43*TT54; % výsledná transformacni matice z prostoru
telesa 5 do prostoru telesa 0
% poloha koncového bodu M vůči rámu v homogennim zobrazení
rM0=TT50*r5M;
% Rychlosti
% diferencialni operatory zakladnich pohybu
DTX = [0 0 0 1;0 0 0 0;0 0 0 0;0 0 0 0];
DTY = [0 0 0 0;0 0 0 1;0 0 0 0;0 0 0 0];
DTZ = [0 0 0 0;0 0 0 0;0 0 0 1;0 0 0 0];
DRX = [0 0 0 0;0 0 -1 0;0 1 0 0;0 0 0 0];
DRY = [0 0 1 0;0 0 0 0;-1 0 0 0;0 0 0 0];
DRZ = [0 -1 0 0;1 0 0 0;0 0 0 0;0 0 0 0];
% rychlosti zakladnich pohybu
V10S1=om10*DRZ;
V21S2=om21*DRX;
V32S3=om32*DRX;
V43S4=om43*DRX;
V54S5=om54*DRZ;
% inverzni transformacni matice
ITT10=inv(TT10);
ITT21=inv(TT21);
ITT32=inv(TT32);
ITT43=inv(TT43);
ITT54=inv(TT54);
V10S5=ITT54*ITT43*ITT32*ITT21*V10S1*TT21*TT32*TT43*TT54;%transformace
rychlosti V10S1 (v prostoru 1) na V10S5 (v prostoru 5)
V21S5=ITT54*ITT43*ITT32*V21S2*TT32*TT43*TT54;
V32S5=ITT54*ITT43*V32S3*TT43*TT54;
V43S5=ITT54*V43S4*TT54;
% vysledna matice rychlosti soustavy (z telesa 5 do telesa 0 v prostoru
souradnic 5)
V50S5=V10S5+V21S5+V32S5+V43S5+V54S5;
simV50S5=simplify(V50S5); % zjednoduseni formalni uprava
% rychlost bodu M vuci ramu
Stránka 5 z 6

vM0=TT50*simV50S5*r5M;
Inovace výuky předmětu Robotika v lékařství
simvM0=simplify(vM0); % zjednoduseni formalni uprava, odsud beru translacni
cast Jakobianu
% vysledna matice rychlosti soustavy (z telesa 5 do telesa 0 v prostoru
% souradnic 0)
V50S0=ITT10*ITT21*ITT32*ITT43*ITT54*simV50S5*TT54*TT43*TT32*TT21*TT10;
simV50S0=simplify(V50S0);
%Vypocet J
omega500=[simV50S0(3,2);simV50S0(1,3);simV50S0(2,1)]; %odsud beru rotacni
cast Jakobianu
v500=[simvM0(1,1);simvM0(2,1);simvM0(3,1)]; %odsud beru translační cast
Jakobianu
vkr(1,1)=v500(1,1); %sestaveni vektoru kart.rychlosti konc bodu
vkr(2,1)=v500(2,1);
vkr(3,1)=v500(3,1);
vkr(4,1)=omega500(1,1);
vkr(5,1)=omega500(2,1);
vkr(6,1)=omega500(3,1);
%jacobian([x*y*z; y; x+z],[x y z]) vypočet Jakobianu pomocí fce "jacobian"
J = jacobian([vkr(1,1); vkr(2,1); vkr(3,1); vkr(4,1); vkr(5,1);
vkr(6,1)],[om10 om21 om32 om43 om54])
%ověření správnosti jakobiánu, rozdíl mezi kart. rychlostmi konc. bodu a
%Jakobiáne násobeným vektorem zobecněných rychlostí musí být nulový
"overeni=0"
A=vkr-J*[om10;om21;om32;om43;om54];
overeni=simple (A);
Získaný a ověřený Jacobián lze pak implementovat do řídících struktur pro ovládání paže
robota.
Stránka 6 z 6