Katedra Robotyki i Mechatroniki

Akademia Górniczo-Hutnicza w Krakowie

Roboty przemysłowe

Wojciech Lisowski

5
Opis położenia i orientacji efektora
w przestrzeni trójwymiarowej

Roboty Przemysłowe KRIM, AGH w Krakowie 1

 z

Położenie:
x, y, z
v

O
y Orientacja:
j, q, y
u

Opis z wykorzystaniem macierzy przekształcenia jednorodnego

Roboty Przemysłowe KRIM, AGH w Krakowie 2

Orientacja: j, q, y Kąty: RPY

RPY (f ,q ,y ) = Rot( z,f )Rot( y,q )Rot( x,y )

OBROTU (Roll) - f
POCHYLENIA (Pitch) - q
SKRĘTU (Yaw) - y

(Fu)

Roboty Przemysłowe KRIM, AGH w Krakowie 3

Transformacja RPY: Rot(z,180°)Rot(y,90°)Rot(x,90°)

180° z
zI yIII
90° yII zII zIII
y
yI xI
90°
x
xII xIII

Odwrócona kolejność przekształceń: x®y®z

Obroty wokół osi pierwszego układu współrzędnych (odniesienia)

z yI
90° zII yIII
180°
y zIII
90° zI
yII
x
xI xII
xIII

Wynik jest taki sam !

Roboty przemysłowe
KRiM, WIMIR AGH Kraków 4
Współrzędne jednorodne: reprezentacja wektora n wymiarowego w
przestrzeni n+1 wymiarowej

Wektor zerowy
p = [ px , p y , pz ] T

[0,0,0, n], n ¹ 0
!
p = ( sp x , sp y , sp z , s ) Wektor Nieokreślony

é3ù é 6 ù é - 30 ù [0,0,0,0]
ê4ú ê 8 ú ê- 40 ú
ê úºê úºê ú Wektor kierunkowy
ê5ú ê10 ú ê - 50 ú
ê ú ê ú ê ú [a , b, c,0]
ë1 û ë 2 û ë - 10 û

Roboty Przemysłowe KRIM, AGH w Krakowie 5

Macierz przekształcenia jednorodnego pozwala określić położenie i
orientację lokalnego układu współrzędnych Puvw w układzie
odniesienia Oxyz
z

éu x vx wx xP ù
êu y P úú
w

vy wy
A=ê y
v êu z vz wz zP ú
ê ú
P
y
ë0 0 0 1û
O

! ! ! ! ! !
x
éu x vx wx ù é ix " iu ix " jv ix " k w ù
ê! ! ! ! ! ! ú
R = êêu y vy w y úú = ê !j y " i!u j y " jv jy " kw ú R -1 = R T
! ! ! !
êëu z vz wz úû ê k z " iu k z " jv k z " k w úû
ë
Roboty Przemysłowe KRIM, AGH w Krakowie 6
Podstawowe macierze rotacji

é1 0 0 ù
ê ú
R( x ,a ) = ê0 Ca - Sa ú
êë0 Sa Ca úû

Roboty Przemysłowe KRIM, AGH w Krakowie 7

4 podstawowe macierze przekształcenia jednorodnego

A = Tra(a, b, c) Rot( x,a ) Rot( y,j ) Rot( z,q )

é1 0 0 0ù é Cj 0 Sj 0ù
ê 0 C a - Sa ê 0 1 0 0úú
0úú
Rot ( x , a ) = ê Rot ( y, j ) = ê
ê 0 Sa C a 0ú ê - Sj 0 Cj 0ú
ê ú ê ú
ë0 0 0 1û ë 0 0 0 1û

éC q - Sq 0 0ù é1 0 0 aù
ê Sq C q ê0 1 0 b úú
0 0úú Tra(a, b, c) = Tra( x, a) × Tra( y, b) × Tra( z, c) = ê
Rot ( z , q) = ê ê0 0 1 cú
ê0 0 1 0ú
ê ú ê ú
ë0 0 0 1û
ë0 0 0 1û

Uwaga! Składanie przekształceń jednorodnych nie jest przemienne:

Rot( x,a )Rot( y,j ) ¹ Rot( y,j )Rot( x,a )

Roboty Przemysłowe KRIM, AGH w Krakowie 8

 z0 Przykład 1 Układ współrzędnych -> Macierz
y1

z1
O1 P =1P = [2,1,0]T
!
1
P = [2,1,0,1]T
O1
O0 5

y0 ! !
3
P 0
P = A1 P
1

é? ? ? 3ù
ê? é2 ù é 0 -1 0 3ù é2 ù
? ? 5úú
x0 x1
ê5ú ê0 0 1 5úú êê1úú
A1 = ê é 0 -1 0 3ù ê ú= ê
ê? ? ? 2ú
ê0 0 ê0ú ê -1 0 0 2 ú ê 0ú
ê ú 1 5úú ê ú ê úê ú
ë0 0 0 1û A1 = ê
ê -1 0 0 2ú ë1û ë0 0 0 1û ë 1û
x1 = [0,0,-1]T ê ú
ë0 0 0 1û
y1 = [-1,0,0]T O0 P= 0P = [2,5,0]T
z1 = [0,1,0]T

Roboty Przemysłowe KRIM, AGH w Krakowie 9

Konwencja orientowania osi chwytaka

n´o = a

(Fu)

Wektory: n – normalny (xe)

o – orientacji (ye)
a – zbliżenia (ze)

Roboty Przemysłowe KRIM, AGH w Krakowie 10

 Wyznaczanie kątów RPY

éN x Ox Ax Px ù éCfCq - SfCy + CfSqSy SfSy + CfSqCy 0ù

êN ê SfCq 0úú
Oy Ay Py úú CfCy + SfSqSy - CfSy + SfSqCy
T =ê y RPY (f ,q ,y ) = ê
êNz Oz Az Pz ú ê - Sq CqSy CqCy 0ú
ê ú ê ú
ë 0 0 0 1û ë 0 0 0 1û

Wyjątki:
Ny
f = arctg q = 90! q = -90!
Nx Roll (z)
-Nz sin (y - f ) = Ox - sin (y + f ) = Ox
q = arctg cos(y + f ) = Oy
1- Nz
2 Pitch (y) cos(y - f ) = Oy
Oz
y = arctg Yaw(x) Ox - Ox
Az y - f = arctg y + f = arctg
Oy Oy
zakresy: 𝜑𝜖 −180°, 180° 𝜃𝜖 −90°, 90° 𝜓𝜖 −180°, 180°

Roboty Przemysłowe KRIM, AGH w Krakowie 11

Funkcja ATAN2
I i IV ćwiartka
y

æ yö æ yö
atan 2ç ÷ = arctgç ÷
II I è xø è xø
y>0 y>0
x<0 x>0

a II i III ćwiartka

x
æ yö æ yö
III IV atan 2ç ÷ = arctgç ÷ ± p
y<0 y<0 è xø è xø
x<0 x>0

Roboty Przemysłowe KRIM, AGH w Krakowie 12

 Przykład 3 Macierz -> Układ współrzędnych

é 3 1 ù u v w
ê- 0 0ú
ê 2 2 ú x ±150° ±90° ±60°
1 3
A1 = ê - 0 - 0úú
ê 2 2
ê 0
ê
-1 0 0ú
ú
y ±120° ±90° ±150°
êë 0 0 0 1úû
z ±90° ±180° ±90°
z

-150° y
120°

-60°
w

-150°

x v

Roboty Przemysłowe KRIM, AGH w Krakowie 15

Przykład – macierz -> układ współrzędnych
1
-
-1
tg (f ) = 2 = f = -150°
3 - 3
-
2
0
tg (q ) = =0 q =0
1- 0

-1
tg (y ) = = -¥ y = -90!
0

Ny
f = arctg
Nx u

-Nz
q = arctg
2
1- Nz é 3 1 ù
ê-
w 0 0ú
Oz ê 2 2 ú
y = arctg 1 3
Az v A1 = ê - 0 - 0úú
ê 2 2
ê 0 -1 0 0ú
ê ú
Roboty przemysłowe KRIM, AGH w Krakowie êë 0 0 0 1úû 16
 z
1
-
-1
tg (f ) = 2 = f = -150°
3 - 3
-
2 -90°
y

0
tg (q ) = =0 q =0
1- 0
x

-1 z¢
tg (y ) = = -¥ y = -90 !
0
x¢ -150°

Ny
f = arctg
Nx u
y¢
-Nz
q = arctg
2
1- Nz é 3 1 ù
ê-
w 0 0ú
Oz ê 2 2 ú
y = arctg 1 3
Az v A1 = ê - 0 - 0úú
ê 2 2
ê 0 -1 0 0ú
ê ú
Roboty Przemysłowe KRIM, AGH w Krakowie êë 0 0 0 1úû 17
 Katedra Robotyki i Mechatroniki
Akademia Górniczo-Hutnicza w Krakowie

Roboty przemysłowe

Wojciech Lisowski

6
Pomiar położenia i orientacji

Roboty przemysłowe KRIM, AGH w Krakowie 18

Zagadnienia:
Klasyfikacja metod pomiaru położenia i orientacji
Zastosowanie interferometru laserowego do
pomiaru położenia i orientacji efektora
Zastosowanie systemu wizyjnego do pomiaru
położenia i orientacji efektora
Zastosowanie teodolitu do pomiaru położenia
i orientacji efektora
Zasada pomiaru położenia i orientacji w metodach
referencyjnych
Czujniki pomiaru odległości stosowane
w metodach referencyjnych
Roboty przemysłowe KRIM, AGH w Krakowie 19
Klasyfikacja metod wyznaczania pozycji efektora

METODY WYZNACZANIA
POZYCJI
CHWYTAKA/NARZĘDZIA

REFERENCYJNE GEODEZYJNE
(LOKALNE) (W CAŁEJ
PRZESTRZENI
ROBOCZEJ)

TRIANGULACYJNE WIZYJNE OPARTE O ŚLEDZENIE

Roboty przemysłowe KRIM, AGH w Krakowie 20

Grupa metod geodezyjnych pozwala na prowadzenie pomiaru
w znacznych obszarach przestrzeni roboczej.
Systemy pomiarowych stosowanych w metodach geodezyjnych
Typ układu pomiaru Pomiar Wyznaczanie Dowolna Pomiar w Dokładność
odległości stykowy orientacji trajektoria ruchu [mm/m]
przemieszczenia

TEODOLIT NIE TAK TAK NIE 0.01

SYSTEM WIZYJNY NIE TAK TAK TAK 0.05

PRZYRZĄD ŚLEDZĄCY NIE TAK TAK TAK 0.01

Tendencja rozwoju systemów pomiarowych:

umożliwienie przeprowadzenia pomiaru:
w możliwie najbardziej zautomatyzowany sposób
w rzeczywistym otoczeniu pracy robota
w czasie ruchu manipulatora.
Roboty przemysłowe KRIM, AGH w Krakowie 21
 Metody interferometryczne (przyrząd śledzący)

Zastosowanie
interferometru laserowego
pozwala wykorzystać w
pomiarze pojedynczy
moduł pomiarowy dzięki
równoczesnemu
pomiarowi zmiany
długości drogi promienia
(Kyle) świetlnego emitowanego
przez laser, odbitego od zwierciadła (ang. retroreflector) zamocowanego
do kołnierza kiści manipulatora i docierającego do interferometru oraz
orientacji zwierciadła pośredniego, odbijającego wiązkę światła w
kierunku zwierciadła zamocowanego do manipulatora.

Roboty przemysłowe KRIM, AGH w Krakowie 22

 Wyznaczanie położenia i orientacji

(Prenninger)
Dokładność pomiaru do 0.5 długości fali.
Zasięg około 20 m
Zakres kątów: poziomo – kąt pełny,
pionowo ±45o
Prędkość śledzenia: promieniowo 1m/s,
stycznie 2 m/s.
Roboty przemysłowe KRIM, AGH w Krakowie 23
Przyrządy pomiarowe
FARO Hexagon Metrology Leica Geosystems

FARO Vantage
IFM 4 µm + 0.8 µm/m AT 901
ADM 16 µm + 0.8 µm/m (A)IFM ±15 µm +6 µm/m
ADM ±5 µm

Roboty przemysłowe KRIM, AGH w Krakowie 24

Automated Precision Inc.
Nikon Metrology

SMR

Radian
IFM ±10 µm
ADM ±10 µm (retro)reflector ± 50° cat eye ± 60°
7g, d 12 mm 730 g, d 75 mm

Roboty przemysłowe KRIM, AGH w Krakowie 25

Metody wizyjne
- komputerowa analiza obrazu z wielu kamer
- do efektora jest mocowany w odpowiedni sposób zespół diod, których
położenie jest śledzone przez zespół kamer o znanym położeniu
i orientacji
Problemy zastosowania:
jak objąć całą przestrzeń roboczą?
rozmiary diod
opóźnienie wprowadzane w czasie obróbki obrazu (w czasie ruchu)

Krypton, REFLINE 2D/3D, dokładność 0.03 mm,

rozdzielczość 0.005 mm, zakres przemieszczeń
20mm´20mm´10m
Możliwość pracy z przetwornikiem cięgnowym (3D) (Raucent)
Roboty przemysłowe KRIM, AGH w Krakowie 26
Nikon Metrology K600, dokładność 0.06 mm/m, rozdzielczość 0.002
mm, obrót w poziomie 360º, w pionie ±60 º, szybkość pracy 600 Hz

Roboty przemysłowe KRIM, AGH w Krakowie 27

Wśród metod geodezyjnych najwcześniej zastosowano metody
triangulacyjne polegające na śledzeniu wybranego punktu
chwytaka/narzędzia z 2 lub 3 modułów pomiarowych, których
położenie wzajemne oraz względem robota jest również wyznaczane
doświadczalnie.

Powszechnie stosowano teodolity,

W ograniczonym zakresie wykorzystywano przetworniki cięgnowe.

Wyznaczenie pozycji wymaga określenia orientacji przestrzennej

osi optycznych teodolitów lub długości pomiarowej cięgien

Roboty przemysłowe KRIM, AGH w Krakowie 28

 Teodolity
z napędami 2 osi obrotu
skomputeryzowane
sterowane w układzie zamkniętym
wyposażone w oprogramowanie
analizujące obraz.

(Meyer)

(Albright)
Roboty przemysłowe KRIM, AGH w Krakowie 29
Pomiar odbywa się w spoczynku.
Przejście do następnego punktu trajektorii jest zautomatyzowane lub
ręcznie przez operatora przy pomocy joystika.
Automatyczne ustawianie ogniskowej może służyć do zgrubnego
oszacowania odległości
Odczyt na podstawie kątów (rozdzielczość około 0.1 sekundy
kątowej) jakie tworzy oś optyczna teodolitu z kierunkami odniesienia
gdy jest ona nakierowana na cel.
Poziomnica pozwala wyznaczyć kierunek pionowy, od którego
odmierzane są kąty (najczęściej oprogramowanie usuwa przechylenie
automatycznie z wyników pomiaru).

Możliwy pomiar orientacji

Pomiar zaczyna kalibracja zespołu

teodolitów (Kyle)

Roboty przemysłowe KRIM, AGH w Krakowie 30

 Przykłady:

Krypton, RODYM LTD 500 3D (Leica), dokładność 0.01 mm/m,

rozdzielczość 0.0012 mm, 0.14 ”, obrót w płaszczyźnie poziom. ±235°
obrót w płaszczyźnie pion. ±45°, zasięg 0-35 m

Leica, Total Station TDA5005

Pomiar położenia: 0.3 mm/m
Pomiar odległości: 0.2 mm/m
Zasięg 500 m

(Leica Geosystems)

Roboty przemysłowe KRIM, AGH w Krakowie 31

 Płaski przetwornik cięgnowy (2D) Przetwornik przestrzenny (3D)

(Driels)

Krypton, RODYM CABLE 2D,

dokładność 0.1 mm,
rozdzielczość 0.02 mm, szybkość
pracy 2000 Hz, długość cięgien
1.5 m

Roboty przemysłowe KRIM, AGH w Krakowie 32

 Przetworniki cięgnowe
Problemy konstrukcyjne:
wydłużanie się cięgien
wpływ bezwładności mas (enkoder,
bęben, cięgno)
oddziaływanie siłowe na efektor
szybkość zwijania cięgna
wyprowadzenie cięgna z głowicy
jak objąć całą PR, niebezpieczeństwo
kolizji cięgna z robotem
i urządzeniami towarzyszącymi

(BEI) Dynalog, Inc., 3D

Roboty przemysłowe KRIM, AGH w Krakowie 33

Metody referencyjne pozwalają na wyznaczenie pozycji chwytaka/
narzędzia lokalnie w obszarze zamocowania modułu odniesienia.
Koncepcje eksperymentalnego wyznaczania pozycji

(Ranky)
Roboty przemysłowe KRIM, AGH w Krakowie 34
Wyznaczanie pozycji w metodzie kostki – 6 czujników

P1
P2

P3
P6 n2

P (Ranky)
O n3
n1

P4 P5
Położenie – OP
Orientacja – n1, n2, n3
Wymagana kalibracja głowicy pomiarowej w celu kompensacji
błędów wykonania kostki odniesienia oraz ustawienia czujników
Zastosowanie 9 czujników pozwala przeprowadzać częściową
autokalibrację (błędy kostki, ale nie czujników).
Roboty przemysłowe KRIM, AGH w Krakowie 35
Do pomiaru odległości stosuje się czujniki:
zegarowe
LVDT (indukcyjne transformatorowe przetworniki położenia)
wiroprądowe, pojemnościowe, magnetostrykcyjne
laserowe (dioda laserowa+element światłoczuły)

TYP CZUJNIKA POMIARU POMIAR ZAKRES ROZDZIELCZOŚĆ

ODLEGŁOŚCI/ STYKOWY POMIAROWY [mm]
PRZEMIESZCZENIA [mm]

MECHANICZNY ZAGAROWY TAK 50 0.001

LVDT TAK 100 0.01

WIROPRĄDOWY NIE 20 0.01

POJEMNOŚCIOWY NIE 20 0.01

LASEROWY NIE 100 0.001

Dokładność pomiaru zależy od własności czujników i od cech

wieloczujnikowych głowic pomiarowych
Roboty przemysłowe KRIM, AGH w Krakowie 36
Przykład

Krypton, REFPOSE 3D
dokładność 0.1 mm
rozdzielczość 0.01 mm
szybkość pracy 100 Hz
zakres 10´10´10 mm

Roboty przemysłowe KRIM, AGH w Krakowie 37

Transformatorowy przetwornik położenia

(Mooring)

Zastosowanie zestawu kul

wzorcowych
(McKerrow)

Roboty przemysłowe KRIM, AGH w Krakowie 38

Zastosowanie: czujników laserowych

Przykład:
(Tang) Krypton REFCUBE 6D, dokładność 0.1 mm,
rozdzielczość 0.02 mm, szybkość pracy 200 Hz,
warstwa przestrzeni pracy o grubości 10 mm

Roboty przemysłowe KRIM, AGH w Krakowie 39

Zastosowanie: czujników laserowych

Przykład: Wiest AG, LASERLAB 3D

5 czujników laserowych, dokładność 0.1 mm, 3D,
39.5mm ×38.5 mm × 36.5 mm
Roboty przemysłowe KRIM, AGH w Krakowie 40
Zastosowanie systemów wizyjnych

Teconsult, 3D, ROSY, 2 kamery CCD

Roboty przemysłowe KRIM, AGH w Krakowie 41

Wykorzystanie czujników wiroprądowych, magnetostrykcyjnych,
pojemnościowych i.t.p – trajektoria płaska

Krypton RODYM 2D, dokładność 0.1 mm, rozdzielczość 0.01 m,

szybkość pracy 100 Hz, powierzchnia 0.5´0.6 m
Roboty przemysłowe KRIM, AGH w Krakowie 42
 Pomiar położenia i orientacji

Roboty przemysłowe KRIM, AGH w Krakowie 43