Macierz Jacobianowa – Jacobiany manipulatora
Macierz Jacobianowa – Jacobiany manipulatora
Sterowanie i
Sterowanie i
Programowanie
Programowanie
Robotów
Robotów
Wykład nr 05 / 06
Wykład nr 05 / 06
dr inż. Tomasz Trawiński
Politechnika Śląska, Wydział
Politechnika Śląska, Wydział
Elektryczny
Elektryczny
KATEDRA MECHATRONIKI
KATEDRA MECHATRONIKI
)
(
)
(
)
(
)
(
6
6
1
1
1
1
)
6
(
q
f
q
q
f
q
q
f
q
q
f
q
n
n
n
J
Macierz Jacobianowa
Macierz Jacobianowa
• Z matematycznego punktu widzenia równania
kinematyki prostej określają funkcję między przestrzenią
pozycji i orientacji kartezjańskich a przestrzenią pozycji
przegubów.
• Natomiast związki prędkościowe pomiędzy
przestrzeniami pozycji/orientacji oraz przegubów
określone są przez Jacobiany tej funkcji.
Macierz Jacobianowa
Macierz Jacobianowa
• Załóżmy że mamy manipulator o n-członach
Wektor
Prędkości
Przegubowych
Macierz
Jacobianowa
Wekto
r
Prędko
ści
Liniow
ej i
Kątow
ej
Końcó
wki
Manip
ulatora
n -
wymiarowy
6 -
wymiarowy
6 n -
wymiarowa
Macierz Jacobianowa
Macierz Jacobianowa
• Jako przekształcenie pomiędzy prędkościami
przegubowymi a prędkością liniową i kątową
dowolnego punktu na manipulatorze
Wektor
Prędkości
Przegubowych
Macierz
Jacobianowa
Wekto
r
Prędko
ści
Liniow
ej i
Kątow
ej
Pkt. na
Manip
ulatorz
e
n -
wymiarowy
6 -
wymiarowy
6 n -
wymiarowa
Dla n-członowego manipulatora
Dla n-członowego manipulatora
oraz
1
0
0
0
0
n
n
n
d
R
T
T
n
n
q
q
q
q
1
2
1
q
Ruch
manipulatora
t
d
n
0
t
R
n
0
T
n
n
t
q
t
q
t
q
t
q
t
)
(
),
(
,
),
(
),
(
)
(
1
2
1
q
Poszukujemy relacji określających związek pomiędzy
prędkością liniową i kątową końcówki roboczej (ale
nie tylko końcówki) z prędkościami przegubowymi
prędkość kątową końcówki określa:
Niech:
Niech:
prędkość liniową końcówki określa:
n
n
d
v
0
0
T
n
t
R
t
R
S
)
(
)
(
)
(
0
poszukujemy pewnych wyrażeń postaci:
q
J
v
v
n
0
q
J
ω
n
0
A w zapisie macierzowym:
A w zapisie macierzowym:
q
ω
v
J
J
v
n
n
0
0
n
3
3
J
J
v
1
3
3
0
0
n
n
ω
v
1
n q
Wyprowadzenie macierzy J
Wyprowadzenie macierzy J
• Jeśli przegub „i” jest obrotowy to
zmienna przegubowa „q
i
” jest
równa „
i
”, a osią obrotu jest „z
i-1
”
• Prędkość
kątowa członu
„i” wyrażona w
układzie „i-1”
wynosi:
k
i
i
i
q
1
z
z
0
0
x
x
0
0
y
y
0
0
z
z
1
1
x
x
1
1
z
z
2
2
x
x
2
2
x
x
3
3
z
z
3
3
0
0
2
1
3
1
1
2
2
3
3
Wyprowadzenie macierzy J
Wyprowadzenie macierzy J
• Podstawiając wyrażenie:
k
i
i
i
q
1
do
Wprowadzając zmienną pomocniczą
i
(1-dla przegubu
obrotowego, 0 – dla pryzmatycznego):
otrzymamy:
k
k
k
k
)
(
...
)
(
)
(
1
0
2
0
3
3
1
0
2
2
1
1
0
t
R
q
t
R
q
t
R
q
q
n
n
n
n
z
0
z
1
z
2
Z
n-1
Wyprowadzenie macierzy J
Wyprowadzenie macierzy J
• Prędkość kątowa w bazowym układzie współrzędnych, w
zwięzłej formie, dana jest wyrażeniem
1
1
0
i
i
n
i
i
n
z
q
T
i
i
R
z
1
0
0
,
1
0
1
k
k
0
z
gdzie:
• Macierz J
ma postać:
1
0
1
,
,
n
n
z
z
J
Wyprowadzenie macierzy J
Wyprowadzenie macierzy J
v
v
• Prędkość liniowa końcówki manipulatora:
n
i
i
i
n
n
q
q
d
d
1
0
0
• Tak więc „i” kolumna macierzy J
v
równa się:
i
n
q
d
0
i
n
v
q
d
0
J
•
Rozróżnia się dwa przypadki:
1. Dla przegubu pryzmatycznego
2. Dla przegubu obrotowego
Wyprowadzenie macierzy J
Wyprowadzenie macierzy J
v
v
– przegub pryzmatyczny
– przegub pryzmatyczny
• Różniczkując powyższe wyrażenie
względem zmiennej przegubowej
i
k
i
i
i
i
i
i
a
R
d
d
1
1
1
1
0
1
1
0
0
i
i
i
i
i
i
i
n
z
d
R
d
d
R
d
k
1
0
i
i
n
z
q
d
z
z
0
0
x
x
0
0
y
y
0
0
z
z
1
1
x
x
1
1
z
z
2
2
x
x
2
2
x
x
3
3
z
z
3
3
0
0
2
1
3
1
1
2
2
3
3
z
z
4
4
x
x
4
4
4
4
4
4
d
4
0
d
3
0
d
Wyprowadzenie macierzy Jv – przegub obrotowy
Wyprowadzenie macierzy Jv – przegub obrotowy
n
i
i
i
n
d
R
d
d
1
1
0
1
0
0
n
i
i
i
n
d
R
o
o
1
1
0
1
• Jeśli tylko przegub „i”
jest napędzany to
są stałe.
1
0
1
0
i
i
R
d
i
z
z
0
0
x
x
0
0
y
y
0
0
z
z
1
1
x
x
1
1
z
z
2
2
x
x
2
2
x
x
3
3
0
0
2
1
3
1
1
2
2
3
3
4
z
z
3
3
z
z
4
4
x
x
4
4
5
4
4
5
5
Wyprowadzenie macierzy J
Wyprowadzenie macierzy J
v
v
– przegub obrotowy
– przegub obrotowy
• Różniczkując:
• Ponieważ ruch członu „i”
jest obrotem wokół osi
„z
i-1
” to:
• Stąd:
n
i
i
i
i
d
R
q
R
1
1
0
1
0
k
)
(
1
1
i
n
i
i
o
o
z
q
1
i
z
n
i
i
i
n
d
q
R
d
1
1
0
0
k
1
i
n
o
o
)
(
1
1
0
i
n
i
i
n
o
o
z
q
d
n
i
i
n
i
d
q
d
1
1
k
n
i
i
n
d
R
d
1
1
0
0
Wyprowadzenie macierzy J
Wyprowadzenie macierzy J
v
v
– przegub obrotowy
– przegub obrotowy
• Jacobian J
v
ma postać:
vn
vi
v
v
J
J
J
,
,
,
,
1
J
)
(
1
1
i
n
i
vi
o
o
z
J
1
i
vi
z
J
– dla przegubu obrotowego
– dla przegubu pryzmatycznego
Całkowita postać Jacobianu manipulatora n-
Całkowita postać Jacobianu manipulatora n-
członowego
członowego
1
1
1
i
i
n
i
i
z
o
o
z
J
n
i
J
J
J
,
,
,
,
1
J
0
1
i
i
z
J
– dla przegubu
obrotowego
– dla przegubu
pryzmatycznego
Przykład 1. Manipulator płaski z łokciem 1/7
Przykład 1. Manipulator płaski z łokciem 1/7
z
z
0
0
x
x
0
0
y
y
0
0
x
x
1
1
z
z
1
1
z
z
2
2
x
x
2
2
a
a
1
1
a
a
2
2
1
1
2
2
Człon
Człon
a
a
i
i
i
i
d
d
i
i
i
i
1
1
a
1
0
0
1
2
2
a
2
0
0
2
Przykład 1. Manipulator płaski z łokciem 2/7
Przykład 1. Manipulator płaski z łokciem 2/7
Postać macierzy
Jacobianowej dla
manipulatora płaskiego:
1
0
1
2
1
0
2
0
)
(
)
(
z
z
o
o
z
o
o
z
J
1
1
1
)
(
i
i
n
i
i
z
o
o
z
J
z
z
0
0
x
x
0
0
y
y
0
0
x
x
1
1
z
z
1
1
z
z
2
2
x
x
2
2
a
a
1
1
a
a
2
2
1
1
2
2
Przykład 1. Manipulator płaski z łokciem
Przykład 1. Manipulator płaski z łokciem
3/7
3/7
Macierze przekształceń T:
1
0
0
0
0
1
0
0
sin
sin
0
cos
sin
cos
cos
0
sin
cos
2
1
2
1
1
2
1
2
1
2
1
2
1
1
2
1
2
1
2
0
a
a
a
a
T
0
sin
cos
1
1
1
1
1
a
a
o
1
0
0
0
0
1
0
0
sin
0
cos
sin
cos
0
sin
cos
1
1
1
1
1
1
1
1
1
0
a
a
T
0
0
0
0
o
0
sin
sin
cos
cos
2
1
2
1
1
2
1
2
1
1
2
a
a
a
a
o
Współrzędne środków układów współrzędnych w bazowym ukł.
współrzędnych:
1
0
1
2
1
0
2
0
z
z
o
o
z
o
o
z
J
Przykład 1. Manipulator płaski z łokciem
Przykład 1. Manipulator płaski z łokciem
4/7
4/7
Osie z
i
układów współrzędnych wyrażone w bazowym układzie
współrzędnych
1
0
0
0
0
1
0
0
sin
sin
0
cos
sin
cos
cos
0
sin
cos
2
1
2
1
1
2
1
2
1
2
1
2
1
1
2
1
2
1
2
0
a
a
a
a
T
1
0
0
1
z
1
0
0
0
0
1
0
0
sin
0
cos
sin
cos
0
sin
cos
1
1
1
1
1
1
1
1
1
0
a
a
T
1
0
0
0
z
Współrzędne środków układów współrzędnych w bazowym ukł.
współrzędnych
Przykład 1. Manipulator płaski z łokciem
Przykład 1. Manipulator płaski z łokciem
5/7
5/7
Iloczyny wektorowe:
0
cos
cos
sin
sin
0
sin
sin
cos
cos
1
0
0
det
2
1
2
1
1
2
1
2
1
1
2
1
2
1
1
2
1
2
1
1
0
2
0
k
j
i
k
j
i
a
a
a
a
a
a
a
a
o
o
z
0
cos
cos
sin
sin
2
1
2
1
1
2
1
2
1
1
0
2
0
a
a
a
a
o
o
z
Przykład 1. Manipulator płaski z łokciem
Przykład 1. Manipulator płaski z łokciem
6/7
6/7
Iloczyny wektorowe:
0
cos
sin
0
sin
cos
1
0
0
det
2
1
2
2
1
2
2
1
2
2
1
2
1
2
1
k
j
i
k
j
i
a
a
a
a
o
o
z
0
cos
sin
2
1
2
2
1
2
1
2
1
a
a
o
o
z
Przykład 1. Manipulator płaski z łokciem
Przykład 1. Manipulator płaski z łokciem
7/7
7/7
Macierz Jacobianowa manipulatora płaskiego z łokciem:
1
0
0
1
0
0
0
cos
sin
0
cos
cos
sin
sin
)
(
2
1
2
2
1
2
2
1
2
1
1
2
1
2
1
1
a
a
a
a
a
a
q
J
Przykład 2.
Przykład 2.
Manipulator RRR
Manipulator RRR
1/x
1/x
z
z
0
0
x
x
0
0
y
y
0
0
z
z
1
1
x
x
1
1
z
z
2
2
x
x
2
2
x
x
3
3
z
z
3
3
1
1
2
2
3
3
d
d
1
1
a
a
2
2
a
a
3
3
Czło
Czło
n
n
a
a
i
i
i
i
d
d
i
i
i
i
1
1
0
/2
d
1
1
2
2
a
2
0
0
2
3
3
a
3
0
0
3
2
1
0
2
3
2
1
3
1
0
3
0
)
(
)
(
)
(
z
z
z
o
o
z
o
o
z
o
o
z
J
Przykład 2.
Przykład 2.
Manipulator RRR
Manipulator RRR
2/x
2/x
%% Jacobian przegubu pierwszego
% Pierwszy - czlon 1
% wspolrzedne srodka ukladu O0
O0=[0;0;0];
% wspolrzedne osi z0
z0=[0;0;1];
% wpolrzedne trzeciego srodka (!!)
O3=T30(1:3,4);
% Jacobian predkosci liniowej pierwszego przegubu
Jv1=simple([0 -1 0;1 0 0; 0 0 0]*[O3-O0]);
% Jacobian predkosci katowej pierwszego przegubu
Jw1=[z0];
% JACOBIAN PREDKOSCI Pierwszego Przegubu
J1=simple([[[Jv1];[Jw1]]]);
z
z
0
0
x
x
0
0
y
y
0
0
z
z
1
1
x
x
1
1
z
z
2
2
x
x
2
2
x
x
3
3
z
z
3
3
1
1
2
2
3
3
d
d
1
1
a
a
2
2
a
a
3
3
0
0
3
0
)
(
z
o
o
z
J
Przykład 2.
Przykład 2.
Manipulator RRR
Manipulator RRR
3/x
3/x
%% Jacobian przegubu drugiego
% Przegub obrotowy
% wspolrzedne srodka ukladu O1
O1=T10(1:3,4);
% wspolrzedne osi z1
z1=T10(1:3,3);
% wpolrzedne 3 srodka (!!)
O3=T30(1:3,4);
% Jacobian predkosci liniowej drugiego przegubu
Jv2=simple([0 -z1(3) z1(2);z1(3) 0 -z1(1);
-z1(2) z1(1) 0]*[O3-O1]);
% Jacobian predkosci katowej drugiego przegubu
Jw2=[z1];
% JACOBIAN PREDKOSCI Drugiego Przegubu
J2=simple([[[Jv2];[Jw2]]]);
z
z
0
0
x
x
0
0
y
y
0
0
z
z
1
1
x
x
1
1
z
z
2
2
x
x
2
2
x
x
3
3
z
z
3
3
1
1
2
2
3
3
d
d
1
1
a
a
2
2
a
a
3
3
1
1
3
1
)
(
z
o
o
z
J
Przykład 2.
Przykład 2.
Manipulator RRR
Manipulator RRR
4/x
4/x
%% Jacobian przegubu trzeciego
% Przegub obrotowy
% wspolrzedne srodka ukladu O2 - znajduja sie w macierzy przeksztalcen
jednorodnych
% T20 - sa to trzy wiersze ostatniej kolumny
O2=T20(1:3,4);
% wspolrzedne osi z2 - sa to trzy wiersze trzeciej kolumny macierzy T20
z2=T20(1:3,3);
% wpolrzedne trzeciego srodka - znajdujemy w macierzy T30
O3=T30(1:3,4);
% Jacobian predkosci liniowej drugiego srodka ciezkosci
Jv3=simple([0 -z2(3) z2(2);z2(3) 0 -z2(1); -z2(2) z2(1) 0]*[O3-O2]);
% Jacobian predkosci katowej pierwszego srodka ciezkosci
Jw3=[z2];
% JACOBIAN PREDKOSCI Trzeciego przegubu
J3=simple([[[Jv3];[Jw3]]])
z
z
0
0
x
x
0
0
y
y
0
0
z
z
1
1
x
x
1
1
z
z
2
2
x
x
2
2
x
x
3
3
z
z
3
3
1
1
2
2
3
3
d
d
1
1
a
a
2
2
a
a
3
3
2
2
3
2
)
(
z
o
o
z
J
Przykład 2.
Przykład 2.
Manipulator RRR
Manipulator RRR
4/x
4/x
J =
[ -sin(Q1)*(a3*cos(Q2 + Q3) + a2*cos(Q2)),
-cos(Q1)*(a3*sin(Q2 + Q3) + a2*sin(Q2)), -a3*sin(Q2 +
Q3)*cos(Q1)]
[ cos(Q1)*(a3*cos(Q2 + Q3) + a2*cos(Q2)),
-sin(Q1)*(a3*sin(Q2 + Q3) + a2*sin(Q2)), -a3*sin(Q2 +
Q3)*sin(Q1)]
[ 0,
a3*cos(Q2 + Q3) + a2*cos(Q2), a3*cos(Q2 + Q3)]
[ 0,
sin(Q1), sin(Q1)]
[ 0,
-cos(Q1), -cos(Q1)]
[ 1,
0,
0]
z
z
0
0
x
x
0
0
y
y
0
0
z
z
1
1
x
x
1
1
z
z
2
2
x
x
2
2
x
x
3
3
z
z
3
3
1
1
2
2
3
3
d
d
1
1
a
a
2
2
a
a
3
3