PODSTAWY ROBOTYKI- J. Craig: Wprowadzenie do robotyki. Mechanika i sterowanie, WNT, Warszawa 1995 -...

PODSTAWY ROBOTYKIPODSTAWY ROBOTYKI

Opracował: dr hab. in ż. Adam Rogowski

Autor wykładu:dr hab. in ż. Adam Rogowskipok. ST 405adam.rogowski@pw.edu.pl

Literatura:- Treść niniejszego wykładu dost ępna na www.cim.pw.edu.pl/lzp- J. Craig: Wprowadzenie do robotyki. Mechanika i ste rowanie, WNT,

Warszawa 1995- J. Honczarenko: Roboty przemysłowe. Budowa i zastos owanie, WNT,

Warszawa 2010

Efekty kształcenia:• Zapoznanie się studentów z wiedzą z zakresu podstaw budowy i programowania

robotów przemysłowych. Poznanie podstaw teoretycznych kinematyki robotów i metod obliczania transformacji prostej i odwrotnej. Nabycie wiedzy na temat sterowania robotów i podstawowych umiejętności programowania.

• Student ma uporządkowaną, pobudowaną teoretycznie wiedzę ogólną w zakresie rodzajów robotów, ich cech charakterystycznych oraz głównych elementów składowych, metod opisu położenia i orientacji brył sztywnych, kinematyki robotów, wyznaczania trajektorii, a także podstaw programowania robotów, nawigacji pojazdami autonomicznymi, języków programowania robotów.

Treść wykładu:• Rozwój i zastosowania robotyki przemysłowej; rys historyczny, przyczyny

stymulujące rozwój robotyki, prawa robotyki.• Definicje podstawowe i terminologia: pojęcie robota, człony i pary kinematyczne,

ruchliwość, manewrowość, przestrzeń robocza, dokładność i powtarzalnośćpozycjonowania, transformacja prosta i odwrotna.

• Klasyfikacja robotów pod względem kinematyki, sterowania, programowania i komunikacji z otoczeniem (generacje robotów). Typowe struktury kinematyczne robotów przemysłowych stacjonarnych.

• Celowość stosowania robotów w przemyśle, typowe zadania robotów przemysłowych, specyfika robotów używanych do różnych zadań technologicznych.

• Układy współrzędnych, różne metody opisu położenia kątowego, macierzowy zapis odwzorowania układów współrzędnych.

• Notacja Denavita-Hartenberga, rozwiązanie prostego zadania kinematyki dla położenia i prędkości.

• Zastosowanie macierzy Jacobiego w robotyce, rozwiązanie odwrotnego zadania kinematyki dla prędkości, osobliwości.

Treść wykładu – c.d.:• Siły statyczne w manipulatorach, zastosowanie macierzy Jacobiego do obliczania

sił i momentów napędowych, osobliwości a siły statyczne.• Generowanie trajektorii, planowanie trajektorii w przestrzeni współrzędnych

konfiguracyjnych, zastosowanie wielomianów 3-go stopnia w planowaniu trajektorii, algorytmy heurystyczne, zachowanie ciągłości przyspieszenia przy planowaniu trajektorii. Wykorzystanie funkcji liniowej łączonej z fragmentami parabolicznymi przy planowaniu trajektorii.

• Opis trajektorii w językach programowania robotów.

Metody i kryteria oceniania:• Warunkiem zaliczenia przedmiotu jest ocena co najmniej 3.0 zarówno z wykładu

(egzamin) jak i z ćwiczeń. Ocena końcowa wystawiana jest wówczas w następujący sposób na podstawie średniej arytmetycznej z tych ocen:3,00 – 3,25 >>> 3,03,26 – 3,75 >>> 3,53,76 – 4,25 >>> 4,04,26 – 4,75 >>> 4,54,76 i więcej >>> 5,0

• Podczas egzaminu nie jest dozwolone korzystanie z urządzeń elektronicznych.• Przy zaliczeniu jednej części składowej przedmiotu (W lub L), można przepisać tę

ocenę na rok następny tylko dla ocen min. 4.0

Rys historyczny rozwoju robotyki

Historia robotyki (budowa mechanizmów próbuj ących na śladowa ćruchy organizmów żywych)• Mechaniczne zabawki (staro żytna Grecja)• Androidy (okres średniowiecza)• Maszyny z nap ędem elektrycznym wyposa żone w „zmysły”

Etymologia słowa „robot”: Karel Čapek – „Roboty Uniwersalne Rossuma”

Prawa robotyki (Isaac Asimov):1. Robot nie mo że ingerowa ć w działanie człowieka, oprócz tych

działań, które szkodz ą człowiekowi2. Robot musi by ć posłuszny człowiekowi, za wyj ątkiem sytuacji, gdy

byłoby to sprzeczne z prawem 13. Robot musi chroni ć swoj ą egzystencj ę, za wyj ątkiem przypadków,

gdy byłoby to sprzeczne z prawem 1 lub 2

Roboty przemysłowe – poj ęcia podstawowe

Robot przemysłowy :• Maszyna manipulacyjna o wielu stopniach swobody, rea lizuj ąca

w pewnym zakresie funkcje charakterystyczne dla gór nych ko ńczynczłowieka, a czasem maj ąca równie ż zdolno ści lokomocyjne

• Maszyna wielozadaniowa (uniwersalna) i programowalna , a więcmogąca realizowa ć wiele ró żnych sekwencji czynno ścimanipulacyjnych

przegub

Manipulator składa si ę z członówtworz ących ła ńcuch kinematyczny TCPotwartySąsiaduj ące ze sob ą człony są połączone za pomoc ą członpar kinematycznych (przegubów)

podstawa

Pojęcia podstawowe – c.d.

Numer klasy pary kinematycznej – liczba wi ęzów nało żonych przez połączenie mi ędzy dwoma członami.Pary kinematyczne V klasy:

przegub obrotowy

przegub translacyjny

Ruchliwo ść – liczba stopni swobody manipulatora z unieruchomion ąpodstaw ą (odpowiada liczbie wi ęzów, które trzeba by nało żyć, aby go całkowicie unieruchomi ć):

5w = 6 · n − ∑ i · p i

i = 1

n – liczba członów ruchomych p i – liczba par kinematycznych klasy „i”

Pojęcia podstawowe – c.d.

Manewrowo ść – liczba stopni swobody manipulatora z unieruchomion ąpodstaw ą i ostatnim członem w ła ńcuchu kinematycznym

Przestrze ń robocza – zbiór punktów przestrzeni, które mo że osi ągnąćkońcówka robocza (efektor)

Przestrze ń robocza manipulacyjna – punkty osi ągane z dowoln ąorientacj ą kątową

Przestrze ń robocza osi ągalna – punkty osi ągane przynajmniej z jedn ąorientacj ą kątową

Powtarzalno ść manipulatora – dokładno ść, z jaką manipulator mo że powróci ć do punktu zapami ętanego poprzednio metod ą nauczania

Dokładno ść manipulatora – dokładno ść, z jaką manipulator mo że osi ągnąć punkt o zadanych współrz ędnych

Pojęcia podstawowe – c.d.

Proste zadanie kinematyki – jego rozwi ązanie polega na okre śleniu poło żenia ko ńcówki roboczej przy zadanej konfiguracji poszczegól nych przegubów. Proste zadanie kinematyki ma zawsze dokł adnie jedno rozwi ązanie

Odwrotne zadanie kinematyki – jego rozwi ązanie polega na okre śleniu konfiguracji wszystkich przegubów przy zało żonym poło żeniu ko ńcówki roboczej. Mo że mieć różną liczb ę rozwi ązań.

Odwrotne zadanie kinematyki

Metody rozwi ązywania odwrotnego zadania kinematyki:

• algebraiczne i geometryczne (możliwe do zastosowania, gdy jest rozwi ązanie w postaci jawnej) – charakteryzuj ą się dużą szybko ściąoblicze ń

• numeryczne – nie musz ą zapewni ć znalezienia wszystkich rozwi ązań

Warunek istnienia rozwi ązania odwrotnego zadania kinematyki wpostaci jawnej dla manipulatora o 6 przegubach obro towych:

Osie trzech s ąsiednichprzegubów przecinaj ą sięw jednym punkciebądź są do siebierównoległe .

Interpolacja

Realizacja interpolacji przez układ sterowania robo ta:

• jest niezb ędna tylko w niektórych zastosowaniach robotów

• jest z reguły znacznie bardziej skomplikowana oblic zeniowo ni ż w przypadku obrabiarek CNC np. implikuje zmienn ą w czasie pr ędkośćobrotow ą przegubu, zsynchronizowan ą z prędkością innych przegubów

Klasyfikacja robotów przemysłowych

• Pod wzgl ędem struktury kinematycznej

• Pod wzgl ędem sterowania

• Pod wzgl ędem sposobu programowania i komunikacji z otoczenie m

Klasyfikacja robotów przemysłowych pod względem struktury kinematycznej

ROBOTY

STACJONARNE MOBILNE

o strukturze o strukturze poruszaj ące się autonomicznekinematycznej kinematycznej po sta łym roboty

szeregowej równoległej torze mobilne

SCARA sferyczne heksapody

cylindryczne tripodywielokorbowe

kartezja ńskie przegubowe

Struktury kinematyczne robotów stacjonarnych

Robot kartezja ński:

(przestrze ń roboczao kształcieprostopadło ścianu)

Robot cylindryczny: Robot sferyczny:

Struktury kinematyczne robotów stacjonarnych –c.d.

SCARA(Selectively Robot wielokorbowy:Compliant Assembly Robot Arm)

Robot przegubowy(antropomorficzny)

Odmiana: PUMA(Programmable UniversalManipulator for Assembly)

Klasyfikacja robotów przemysłowych – c.d.

Pod wzgl ędem sterowania (4 klasy):

• Robot sekwencyjny – wyposa żony w sekwencyjny układ sterowania (wykonuj ący kolejno zaprogramowane ruchy i czynno ści)

• Robot realizuj ący zadane trajektorie – wg instrukcji pozycjonowania określających pozycje i pr ędkość

• Robot adaptacyjny – dostosowuj ący swoje działanie do informacji pochodz ących z ró żnego rodzaju czujników

• Teleoperator – sterowany zdalnie albo przez człowieka -operatora albo przez komputer)

Klasyfikacja robotów przemysłowych – c.d.

Pod wzgl ędem sposobu programowania i komunikacji z otoczeniem (3 generacje):

• Roboty nauczaneNa ogół nie wyposa żone w czujniki. Operuj ą na jednoznacznie określonych obiektach o ściśle okre ślonym poło żeniu. Programowanie sekwencyjne lub przez nauczanie

• Roboty ucz ące si ęWyposa żone w sensory dotyku i/lub obrazu. Operuj ą na ró żnych obiektach (cho ć na ogół obiektach okre ślonego typu), które mog ą sięznajdowa ć w ró żnym poło żeniu. Programowanie przez nauczanie z elementami adaptacyjnymi.

• Roboty inteligentneOperuj ą na zró żnicowanych obiektach o poło żeniu zmiennym w czasie. Wykorzystuj ą także inne sensory ni ż odpowiadaj ące zmysłom wzroku i dotyku. Programowanie w j ęzyku (quasi-) naturalnym.

Celowo ść stosowania robotów w przemy śle

• Zastąpienie człowieka przy pracach niebezpiecznych

• Odsuni ęcie człowieka od pracy w warunkach szkodliwych dla zdrowia

• Zastąpienie człowieka przy pracach wymagaj ących du żego wysiłku fizycznego

• Zwiększenie dokładno ści, a zwłaszcza powtarzalno ści wykonywanych prac

• Zastąpienie człowieka przy realizacji zaj ęć monotonnych i mało twórczych

Typowe zadania robotów przemysłowych

• Zgrzewanie punktowe

• Spawanie łukowe

• Obsługa obrabiarek skrawaj ących

• Obsługa linii pras

• Obsługa stanowisk kucia na gor ąco

• Monta ż

• Paletyzacja

• Lakierowanie i nanoszenie powłok

W każdym z wymienionych przypadków warto si ę zastanowi ć, jakimi cechami powinien si ę charakteryzowa ć układ sterowania robota: czy powinien on by ć w stanie realizowa ć interpolacj ę, dokładne sterowanie pr ędkością bądź sterowanie przyspieszeniem.

Układy współrz ędnych stanowiska zrobotyzowanego

Układy odniesienia:

podstawy → B

stanowiska → S

kiści → W ZW ZT

narzędzia → T (TCP) ZS

XTZB XW

XS

XB

Opis poło żenia za pomoc ą wektora pozycji i macierzy orientacji

Poło żenie liniowe punktu Q w układzie współrz ędnych A:

Poło żenie kątowe układu B w układzie A jest okre ślone przez macierz obrotu:

XA

YA

ZA A PQ Q A PQ =

x Q

y Q

z Q

XA

YA

ZA ZB

XB

YB

Opis poło żenia kątowego za pomoc ą macierzy orientacji - c.d.

AWiersze macierzy R s ą z kolei wersorami osi układu współrz ędnych A

B

w układzie B:

B A TZatem R = R :

A B

Opis poło żenia kątowego za pomoc ą macierzy orientacji - przykład

Zatem:

XA

ZA

XB

YB = YA

ZB

30º

Specjalna grupa euklidesowa

Specjalna grupa euklidesowa SE(3) umo żliwia przedstawienie pozycjiliniowej i orientacji k ątowej (np. chwytaka robota) za pomoc ą jednejmacierzy

XA

YA

ZA A PB ORG

X B

YB

Z B

Opis orientacji za pomoc ą kątów Eulera

XA X’=X’’

YA=Y’Y’

ZA

Z’Z’

X’

Z’’

Y’’

X’’

Z’’=Z’’’

Y’’Y’’’

X’’’

ZB

YB

Można udowodni ć, że dowolnie zorientowany układ współrz ędnych A można zorientowa ć zgodnie z innym, dowolnie zorientowanym układem współrz ędnych B, poprzez zło żenie kolejno po sobie nast ępujących co najwy żej trzech obrotów o odpowiednio dobrane k ąty wokół własnych osi tego układu

XB

Opis orientacji za pomoc ą kątów Eulera – c.d.

Kąty kolejnych takich obrotów mog ą słu żyć do jednoznacznego okre śleniaorientacji układu współrz ędnych B w stosunku do układuwspółrz ędnych odniesienia A

- Kąty te nazywamy k ątami Eulera

- Kolejno ść obrotów wokół poszczególnych osi (X, Y i Z) ma znacz eniedla ko ńcowej orientacji k ątowej obracanego układu, w zwi ązku z czymprzy podawaniu k ątów Eulera nale ży okre ślić kolejno ść tych obrotównp.:

αX=π βY=π/2 γZ=π/4 - obroty kolejno wokół osi X o k ąt π, następniewokół osi Y o k ąt π/2, a na końcu wokół osi Zo kąt π/4

αY=π/2 βZ=π/4 γX=π - obroty kolejno wokół osi Y o k ąt π/2, następniewokół osi Z o k ąt π/4, a na końcu wokół osi Xo kąt π

Opis orientacji za pomoc ą kątów Eulera –przykład

Przykład opisu orientacji układu współrz ędnych XBYBZB w stosunku do układu współrz ędnych XAYAZA za pomoc ą kątów Eulera:

αX=π βY=0 γZ=π/2

YA

XAZA

XB

YB

ZB

Opis orientacji za pomoc ą kwaternionów

Można udowodni ć, że dowolnie zorientowany układ współrz ędnychA można zorientowa ć zgodnie z innym, dowolnie zorientowanymukładem współrz ędnych B, poprzez realizacj ę pojedynczego obrotuo okre ślony k ąt wokół odpowiednio dobranej osi (najcz ęściej niepokrywaj ącej si ę z żadną osi ą tego układu współrz ędnych)

XA

YA

ZA Z’=ZB

X’=XBXB

Y’=YB

YB

ZBOś obrotu

Opis orientacji za pomoc ą kwaternionów – c.d.

Orientacja osi obrotu (okre ślona przez współrz ędne jej wersoranx,ny,nz) oraz warto ść kąta obrotu θ mogą słu żyć do jednoznacznegookreślenia orientacji układu współrz ędnych B w stosunku do układuwspółrz ędnych odniesienia A

XA

YA

ZA Z’=ZB

X’=XBXB

Y’=YB

YB

ZB

θ

Oś obrotu

Wersor osi obrotu [n x,ny,nz]

Równoznaczne z powy ższym jest okre ślenie orientacji układuwspółrz ędnych B w stosunku do układu współrz ędnych A za pomoc ą tzw. kwaternionów, okre ślonych nast ępującymi wzorami:

Q1 = cos ( θ/2) Q2 = nx sin ( θ/2) Q3 = ny sin ( θ/2) Q4 = nz sin ( θ/2)

Opis orientacji za pomoc ą kwaternionów –przykład

Przykład opisu orientacji układu współrz ędnych XBYB ZB

w stosunku do układu współrz ędnych XAYAZA za pomoc ąkwaternionów:

nX=√2 / 2 nY= -√2 / 2

nZ=0

Θ = π

stąd:

Q1 = 0

Q2 = √2 / 2

Q3 = -√2 / 2

Q4 = 0

YA

XAZA

XB

YB ZB

Oś obrotu

Kąt obrotu: π

Wersor osi obrotu

Odwzorowania układów współrz ędnych za pomoc ą specjalnej grupy euklidesowej

Przekształcenie opisu poło żenia punktu przy przej ściu z jednego układuwspółrz ędnych do drugiego:

XA

YA

ZA

A PB ORG XB

YB

ZB

Q

B PQA PQ

Odwzorowania układów współrz ędnych za pomoc ą specjalnej grupy euklidesowej -

przykład

Q (x,y,z)

B PQ

XA

ZA

XB

YB

ZB

30º

YA

A P B ORG

A P B ORG =

25

0

10

Macierze opisuj ące elementarne transformacje

1 0 0 a

0 1 0 b

Tran (a,b,c) = 0 0 1 c

0 0 0 1

1 0 0 0

0 cosφ –sinφ 0

RotX(φ) = 0 sinφ cosφ 0

0 0 0 1

XA

YA

ZAA PB ORG XB

YB

ZB

A P B ORG =

a

b

c

ZA

YA

ZB

XB = XA

YB

φ

Macierze opisuj ące elementarne transformacje –c.d.

cosφ 0 sinφ 0

0 1 0 0

RotY(φ) = –sinφ 0 cosφ 0

0 0 0 1

cosφ –sinφ 0 0

sinφ cosφ 0 0

RotZ(φ) = 0 0 1 0

0 0 0 1

XA

ZA

XB

YB = YA

ZB

φ

YA

XA

YB

ZB = ZA

XB

φ

Macierze opisuj ące elementarne transformacje –c.d.

1 0 0 r

0 cosφ –sinφ 0

SkrętX(r,φ) = 0 sinφ cosφ 0

0 0 0 1

cosφ 0 sinφ 0

0 1 0 r

SkrętY(r,φ) = –sinφ 0 cosφ 0

0 0 0 1XA

YA

ZA

rXB

YB

ZB

φ

XA

YAr

XBYB

ZB

φZA

Macierze opisuj ące elementarne transformacje –c.d.

cosφ –sinφ 0 0

sinφ cosφ 0 0

SkrętZ(r,φ) = 0 0 1 r

0 0 0 1

XA

YA

ZA r

XB

YB

ZB

φ

Notacja Denavita-Hartenberga

Założenie: robot posiada tylko przeguby rotacyjne i tran slacyjne

Geometria pojedynczego członu „i” robota jednoznacznie determinuje wzajemne poło żenie osi „i” oraz „i+1” jego przegubów:

ai – odległo ść między osiami „i” oraz „i+1” (odcinek o długo ści ai jest prostopadły zarówno do osi „i” jak te ż osi „i+1”)

człon „i”oś „i”

oś „i+1”

ai

Sworzeń członu „i+1”

Sworzeń członu „i”

Notacja Denavita-Hartenberga – c.d.

Każdemu członowi przypisany jest układ współrz ędnych XiYiZi zgodnie z następuj ącymi zasadami:

- Oś Zi pokrywa si ę z osi ąprzegubu „i”

- Oś Xi pokrywa si ę z najkrótszymodcinkiem prostej ł ączącym osie„i” oraz „ i+1” (tym, wzdłu ż któregooblicza si ę odległo ść ai)

- Poło żenie osi Yi determinuje si ę napodstawie faktu, że układ współrz ędnychXiYiZi jest układem prawoskr ętnym X i

Z i

X i+1Z i+1

Notacja Denavita-Hartenberga – c.d.

Poło żenie członu „i+1” wzgl ędem członu „i” jest opisane za pomoc ąparametrów Denavita-Hartenberga:

- ai – długo ść członu „i”(odległo ść między Z i i Z i+1 mierzona wzdłu ż Xi)

- αi – kąt skr ęcenia członu „i”(kąt mi ędzy Z i i Z i+1 mierzony wokół X i)

- d i+1 – odsuni ęcie członu „i+1”(odległo ść między X i i X i+1mierzona wzdłu ż Zi+1)

- θi+1 – kąt konfiguracji członów(kąt mi ędzy X i i X i+1 mierzony wokół Z i+1)

W przypadku przegubu obrotowego θ jest zmienn ą konfiguracyjn ą, zaś w przypadku przegubutranslacyjnego zmienn ą konfiguracyjn ą jest d. Pozostałe parametry Denavita-Hartenberga dla danego członu s ą stałymi warto ściami.

X i

Z i

X i+1Z i+1

ai

d i+1αi

θi+1

Notacja Denavita-Hartenberga – c.d.

Dla pierwszego poł ączenia ruchowego w ła ńcuchu (nieruchoma podstawa, czyli człon „0”, oraz człon „1”) zazwycza j przyjmuje si ę:

- Dla pary przesuwnej: a0 = 0 α0 = 0º θ1 = 0º

- Dla pary obrotowej: a0 = 0 α0 = 0º d1 = 0

Poło żenie układu współrz ędnych „1” zazwyczaj pokrywa si ę z poło żeniem układu „0” dla zerowej warto ści zmiennej konfiguracyjnej układu „1”

Notacja Denavita-Hartenberga – przykład 1

θ1

θ2

θ3

L1

L2

i αi-1 ai-1 d i θi

1 0º 0 0 θ1

2 0º L1 0 θ2

3 0º L2 0 θ3

Zmienne konfiguracyjne oznaczono kolorem czerwonym

X 0

Y 0

X 1Y 1

X 2Y 2

X 3

Y 3

Notacja Denavita-Hartenberga – przykład 2

θ1

θ2

θ3

L1L2

i αi-1 ai-1 d i θi

1 0º 0 0 θ1

2 90º 0 L1 θ2

3 0º L2 L3 θ3

X 0

Y 0

X 1

Y 1

X 2

Z 2

X 3Z 3

L3

X 0

Z 0

X 2

Z 2

X 3

X 1

Z 1

X 0

Z 0=Z1

Z 2

Y 2 Z 3

Y 3

Zapis typowych struktur kinematycznych w notacji Denavita-Hartenberga

Robot o strukturze kinema-tycznej kartezja ńskiej:

Robot o strukturze kine-matycznej cylindrycznej:

Inne struktury (SCARA, przegubowa, sferyczna) – pros zę się zastanowi ćsamodzielnie.

i αi-1 ai-1 d i θi

1 (0º) (0) d1 (0)

2 ±90º (0) d2 ±90º

3 ±90º (0) d3 (0)

Z 3

i αi-1 ai-1 d i θi

1 (0º) (0) (0) θ1

2 0/180º (0) d2 (0)

3 ±90º (0) d3 (0)

Określenie poło żenia TCP w układzie współrz ędnych podstawy robota

Transformacja pomi ędzy układami współrz ędnych przegubu „i-1” i „i”:

Określenie poło żenia TCP w układzie współrz ędnych podstawy robota

Transformacja pomi ędzy układami współrz ędnych podstawy i ostatniego członu robota:

Obliczenie poło żenia punktu TCP w układzie współrz ędnych podstawy robota przy jego znanym poło żeniu w układzie współrz ędnych ostatniego członu (ki ści):

Określenie transformacji dla przykładowego robota

θ1

θ2

θ3

L1L2

X 0

Y 0

X 1

Y 1

X 2

Z 2

X 3Z 3

L4

L3 i αi-1 ai-1 d i θi

1 0º 0 0 θ1

2 90º 0 L1 θ2

3 0º L2 L3 θ3

3 P TCP =

L4

0

0

Określenie transformacji dla przykładowego robota – c.d.

i αi-1 ai-1 d i θi

1 0º 0 0 θ1

2 90º 0 L1 θ2

3 0º L2 L3 θ3

Określenie transformacji dla przykładowego robota – c.d.

i αi-1 ai-1 d i θi

1 0º 0 0 θ1

2 90º 0 L1 θ2

3 0º L2 L3 θ3

Określenie transformacji dla przykładowego robota – c.d.

i αi-1 ai-1 d i θi

1 0º 0 0 θ1

2 90º 0 L1 θ2

3 0º L2 L3 θ3

Określenie transformacji dla przykładowego robota – c.d.

Określenie transformacji dla przykładowego robota – c.d.

Określenie transformacji dla przykładowego robota – c.d.

Przykładowo dla θ1=45º θ2=0º θ3=0º :

Transformacja wektora pr ędkości

Wektor poło żenia a wektor pr ędko ści:

XA

YA

ZA

A PB ORG XB

YB

ZB

Q

B PQA PQ

B VQ

Załóżmy (na razie) stałe poło żenie kątowe układu B wzgl ędem układu A. Wtedy:

A VB ORG

Transformacja wektora pr ędkości – c.d.

Teraz uwzgl ędnimy obrót układu B w układzie współrz ędnych A opisany za pomoc ą wektora pr ędkości obrotowej AΩB (macierz obrotu jest wi ęc zmienna w czasie). Na razie zakładamy BVQ=0 oraz AVB ORG = 0 (rozwa żamy tylko pr ędkość punktu Q w układzie A, wynikaj ącą z obrotu układu B)

Dla wektora pr ędkości obrotowej AΩB :- Kierunek determinuje o ś obrotu- Długo ść (oznaczmy j ą jako ω)

determinuje pr ędkość kątową

XA

YA

ZA

A PB ORG XB

YB

ZB

Q

|AΩB|=ω

A PQ

AΩB

Transformacja wektora pr ędkości – c.d.

XA

YA

ZA

A PB ORG

A PQ – A PB ORG

A PQθ

Wektor pr ędkości AVQ jest przedstawiony w układzie współrz ędnych A. Można go wi ęc doda ć bezpośrednio do obliczonego wcze śniej wektora pr ędko ści wynikaj ącej z ruchu liniowego układu B względem układu A oraz z ruchu punktu Q względem układu B. Sumaryczna pr ędkość punktu Q jest wi ęc:

Q

AΩB

A VQ

r

Poniewa ż:

więc:

Ponadto wiadomo, że:

Zatem:

Transformacja wektora pr ędkości: przykład

Wierzchołek narz ędzia przemieszcza si ę w układzie B kiści robota w kierunku osi XB z prędko ścią 5mm/s . Chwilowe poło żenie tego wierzchołka w milimetrach w układzie ki ści jest (10,0,0). Układ B, o pocz ątku wspólnym z układem współrz ędnych stanowiska A, wykonuje wokół osi ZA jeden obrót w ci ągu sekundy ( ω=2π rad/s ). Określić wektor prędkości punktu wierzchołka w układzie stanowiska A w momencie, gdy oba układy maj ą jednakow ą orientacj ę kątową oraz gdy o ś XB

tworzy z osi ą XA kąt 45º

XA

YA

XB

ZB = ZA

YB

45º

v = 510

XA

YA

XBZB = ZA

YB

v = 5

10

ω = 2π

ω = 2π

1) 2)

Transformacja wektora pr ędkości: przykład – c.d.

XA

YA

XBZB = ZA

YB

v = 5

10

ω = 2π

XA

YA

ZA

A VQ

1)

Transformacja wektora pr ędkości: przykład – c.d.

XA

YA

ZA

A VQ

2)

XA

YA

XB

ZB = ZA

YB

45º

v = 510

ω = 2π

Przenoszenie pr ędkości przez człony kinematyczne

X i

Z i

X i+1Z i+1 W wyniku przemieszczania si ę członów

1,2..,i robota, układ współrz ędnych „i”jest w ruchu, który mo żna rozbi ć na ruchy składowe: post ępowy i obrotowyiVi – wektor opisuj ący ruch post ępowy układu współrz ędnych „i” w stosunku do nieruchomego otoczenia (np. do podstawy manipulatora)iΩi – wektor opisuj ący obrót układu współrz ędnych „i”Kierunek wektora iΩi określa oś obrotu, a jego długo ść prędkość kątowąWspółrz ędne wektorów iVi i iΩi podane są w układzie współrz ędnych „i”Opisuj ą one chwilowe parametry tych ruchów

iΩi

iVi

Obliczenie obrotu układu współrz ędnych „i+1”

X i

Z i

X i+1Z i+1

Jeśli przegub „i+1” jest przegubem obrotowym, to obrót i+1Ωi+1 członu „i+1” wynika ze zło żenia obrotu iΩioraz obrotu wokół osi Zi+1 z prędkością ωi+1 = θi+1

Aby doda ć dwa wektory do siebie, muszą być one opisane w tym samym układzie współrz ędnych. Dlatego zapisujemy obrót członu „i+1” w układzie współrz ędnych „i”:

ωi+1iΩi

i+1Ωi+1

Przejście do układu „i+1”:

Obliczenie obrotu układu współrz ędnych „i+1”– c.d.

X i

Z i

X i+1Z i+1

Stąd:

ωi+1iΩi

i+1Ωi+1

Obliczenie pr ędkości liniowej pocz ątku układu współrz ędnych „i+1”

Celem jest okre ślenie, jakim wzorem przedstawia si ę prędkośćliniowa pocz ątku układu współrz ędnych „i+1” przy zadanej prędkości liniowej i pr ędkości kątowej układu „i”

Wykorzystujemy wyprowadzony poprzednio wzór na transformacj ęwektora pr ędkości z układu współrz ędnych A do układu współrz ędnych B:

X i

Z i

X i+1Z i+1

iΩi

iVi

i+1Vi+1

Obliczenie pr ędkości liniowej pocz ątku układu współrz ędnych „i+1” – c.d.

X i

Z i

X i+1Z i+1

iΩi

iVi

i+1Vi+1

Jako punkt Q interpretujemy pocz ątek układu współrz ędnych „i+1”

Jako układ B interpretujemy układ współrz ędnych „i”

Jako układ A interpretujemy nieruchomy wzgl ędem podstawy manipulatora układ współrz ędnych o orientacji k ątowej zgodnej z układem współrz ędnych „i”

X A

Z A

Obliczenie pr ędkości liniowej pocz ątku układu współrz ędnych „i+1” – c.d.

Poniewa ż orientacja k ątowa układu A oraz „i” jest taka sama, wi ęc:

Stąd:

Gdy przegub „i+1” jest obrotowy:

Obliczenie pr ędkości liniowej pocz ątku układu współrz ędnych „i+1” – c.d.

Poło żenie liniowe iPi+1 początku układu współrz ędnych „i+1” wzgl ędem układu „i” okre ślamy na podstawie wcze śniej udowodnionej zale żności:

Obliczenie pr ędkości liniowej pocz ątku układu współrz ędnych „n” w układzie współrz ędnych

podstawy manipulatora1) Dla i = 0, 1… n-1 obliczy ć:

2) Przyjąć:

3) Dla i = 0, 1… n-2 obliczy ć:

4) Dla i = 0, 1… n-1 obliczy ć:

5) Obliczy ć orientacj ę kątową układu „n” wzgl ędem podstawy:

6) Ostatecznie szukana pr ędkość liniowa wynosi:

Przykład obliczenia pr ędkości liniowej chwytaka (dla manipulatora dwuczłonowego)

Manipulator 2-członowy:

θ1

θ2

L1

L2

i αi-1 ai-1 d i θi

1 0º 0 0 θ1

2 0º L1 0 θ2

3 0º L2 0 0

X 0

Y 0

X 1Y 1

X 2Y 2

X 3Y 3 Parametry Denavita-Hartenberga:

Przykład obliczenia pr ędkości liniowej chwytaka manipulatora – c.d.

Przykład obliczenia pr ędkości liniowej chwytaka manipulatora – c.d.

Przykład obliczenia pr ędkości liniowej chwytaka manipulatora – c.d.

Przykład obliczenia pr ędkości liniowej chwytaka manipulatora – c.d.

Przykładowo dla:

L1

vx=−2·L2

X 0

Y 0

vy=L1

θ1

v

θ2 = 90º

L2

θ2

Macierze Jacobiego (jakobiany) w robotyce

Jeśli dana jest funkcja odwzorowuj ąca wektor X na wektor Y:

Y = F (X)

czyli: y1 = f1 (x1, x2, x3 …)y2 = f2 (x1, x2, x3 …)y3 = f3 (x1, x2, x3 …)………

to macierz Jacobiego jest:

Uwaga: w robotyce u żywa si ę terminu „jakobian” jako synonimu macierzy Jacobiego (w matematyce „jakobian” oznacza wyznacznik tej macierzy)

Jakobiany w robotyce – c.d.

Wiadomo, że:

zatem:

czyli:

Różniczkuj ąc po czasie otrzymujemy:

Jakobiany w robotyce – c.d.Przy opisie manipulatorów: X → wektor zmiennych konfiguracyjnych θ

Y → wektor współrz ędnych kartezja ńskichzatem: dx

dy = J(θ) · dθ…

oraz:gdzie: V → wektor pr ędkości liniowej członu roboczego

Ω→ wektor pr ędkości k ątowej członu roboczegoLiczba kolumn jakobianu – liczba poł ączeń ruchowychLiczba wierszy jakobianu – liczba stopni swobody w r ozważanej przestrzeni

kartezja ńskiejJeśli jakobian słu ży tylko do obliczania pr ędkości liniowej członu roboczego, to wystarczy, że będzie miał 3 wiersze (a dla zagadnie ń płaskich – 2 wiersze):

Przykład wyznaczenia jakobianu na podstawie definicji (dla manipulatora dwuczłonowego)

Najpierw nale ży okre ślić funkcje:x = f x (θ1, θ2)y = f y (θ1, θ2)(w układzie współrz ędnych X0Y0Z0)

Do tego potrzebne jest określenie transformacji przejścia z układu 0 do układu 3:θ1

θ2

L1

L2

X 0

Y 0

X 1Y 1

X 2Y 2

X 3Y 3

Przykład wyznaczenia jakobianu na podstawie definicji – c.d.

Przykład wyznaczenia jakobianu na podstawie definicji – c.d.

Przykład wyznaczenia jakobianu na podstawie definicji – c.d.

stąd:

zatem jakobian(w układzie „0”):

Przykład wyznaczenia jakobianu na podstawie definicji – c.d.

Wynika st ąd, że:

Wzory te s ą zgodne z wyznaczonym wcze śniej wektorem pr ędko ści 0V3:

Praktyczna metoda wyznaczania jakobianu

Dla konkretnej konfiguracji robota (dla okre ślonych warto ści zmiennych konfiguracyjnych), jakobian mo żna obliczy ć na podstawie:

• aktualnych wersorów ai poszczególnych osi przegubów

zaś w przypadku przegubów rotacyjnych dodatkowo na pods tawie:

• wektora poło żenia efektora e (punktu TCP)

• wektorów poło żenia punktów pi na poszczególnych osiach tychprzegubów (punkty pi mogą być np. pocz ątkami układówwspółrz ędnych poszczególnych członów):

Praktyczna metoda wyznaczania jakobianu – c.d.

Jakobian przedstawia si ę wzorem:

w1X w2X w3X … wnX

J = w1Y w2Y w3Y … wnY

w1Z w2Z w3Z … wnZ

gdzie: w ix , w iY , w iZ – rzuty wektora wi na osie X,Y,Z układuwspółrz ędnych, w którym chcemy obliczy ćjakobian

przy czym:

wi = ai – dla przegubów translacyjnych

wi = ai xxxx (e – pi) – dla przegubów rotacyjnych

Przykład zastosowania omówionej metody do wyznaczania jakobianu

θ1

θ2

L1

L2

X 0

Y 0

X 1Y 1

X 2Y 2

X 3Y 3 Obliczmy jakobian w układzie 0:

Przykład zastosowania omówionej metody do wyznaczania jakobianu – c.d.

Stąd:

Ostatecznie:

Wyznaczanie jakobianu na podstawie wektora prędkości liniowej

Jakobian mo żna równie ż wyznaczy ć znając prędkość członu roboczego w układzie współrz ędnych podstawy 0Vn. Jakobian ten jest oznaczany 0J(θ).

Mając dany wektor 0Vn = F (θ) oraz znaj ąc zależność 0Vn = 0J(θ) · θ , można dobra ć wyra żenia opisuj ące poszczególne elementy jakobianu.

Jeżeli dany jest wektor pr ędko ści nVn, to obliczony na jego podstawie jakobian oznaczamy nJ(θ).

Przykład wyznaczenia jakobianu na podstawie wektora pr ędkości (dla manipulatora

dwuczłonowego)

Dla manipulatora dwuczłonowego był wyznaczony wektor prędkości:

Poniewa ż:

więc:

Zatem jakobian b ędzie miał 3 wiersze i 2 kolumny:

Zastosowanie jakobianu – rozwi ązanie odwrotnego zadania kinematyki dla pr ędkości

Skoro:

więc:

Rozwiązanie w ten sposób odwrotnego zadania kinematyki dl a prędkości jest mo żliwe, je śli mo żna wyznaczy ć macierz odwrotn ą do jakobianu J-1(θ)

Macierz odwrotna

Niech b ędzie dana macierz A.Macierz odwrotna spełnia warunek: A·A-1 = A-1·A = I

gdzie I – macierz jednostkowa

Macierz odwrotna – c.d.

Macierz odwrotn ą wyznaczamy ze wzoru: A-1 = Aad / det (A)gdzie Aad – macierz doł ączona

Zastosowanie wzoru wymaga spełnienia warunku det (A) ≠0, a zatem :

- Rząd macierzy A jest równy wymiarowi tej macierzy.- Macierz A jest nieosobliwa.

Obliczenie macierzy doł ączonej:

- Obliczamy macierz transponowan ą AT

- Każdy element AT zastępujemy przez dopełnienie algebraiczne:

Dij = (-1)i+j ·Mij

gdzie Mij – minor stopnia n-1 (wyznacznik macierzy powstałej przez wykre ślenie i-tego wiersza i j-tej kolumny)

Przykład macierzy odwrotnej

Dana jest macierz A 2x2:

stąd: gdzie:

Przykład rozwi ązania odwrotnego zadania kinematyki dla pr ędkości

Niech b ędzie dany manipulator dwuczłonowy:

θ1

θ2

L1

L2

X 0

Y 0

X 1Y 1

X 2Y 2

X 3Y 3

Jakobiany uwzgl ędniaj ące tylko składowe vx i vyprędkości (składowa vz=0):

Przykład rozwi ązania odwrotnego zadania kinematyki dla pr ędkości – c.d.

Wyznacznik jakobianu 0J(θ) jest:

zatem macierz odwrotna jest:

Przykład rozwi ązania odwrotnego zadania kinematyki dla pr ędkości – c.d.

Wyznacznik jakobianu 3J(θ) jest:

zatem macierz odwrotna jest:

Przykład rozwi ązania odwrotnego zadania kinematyki dla pr ędkości – c.d.

Rozwiązanie dla przypadku θ1=45º θ2=90º vx=-5m/s L 1=0.5m L2=0.5m

θ1=45º

θ2=90º

L1=0.5m

L2=0.5m

X 0

Y 0

v=5m/s

Przykład rozwi ązania odwrotnego zadania kinematyki dla pr ędkości – c.d.

Dla tego samego przypadku wykorzystamy 3J-1:

θ1=45º

θ2=90º

L1=0.5m

L2=0.5m

X 0

Y 0

X 3

Y 3

v=5m/s

Osobliwo ści

W niektórych obszarach swojej przestrzeni roboczej manipulator mo że mieć tzw. osobliwo ści:

det (J( θ)) = 0

Wyrózniamy:

- Osobliwo ści graniczne przestrzeni roboczej

- Osobliwo ści wn ętrza przestrzeni roboczej

Osobliwo ść jest zawsze zwi ązana z utrat ą jednego lub wi ęcej stopni swobody.

Przykład osobliwo ści dla manipulatora 2-członowego

Gdzie znajduj ą się osobliwo ści takiego manipulatora?

θ1

θ2

L1

L2

X 0

Y 0

X 1Y 1

X 2Y 2

X 3

Y 3det(J( θ)) = L1·L2·sin θ2 = 0

czyli: sin θ2 = 0

zatem: θ2 = 180º lub θ2 = 0º

Ruch jest mo żliwy tylko w kierunku prostopadłym do ramienia o długo ści L 1, czyli mechanizm stracił 1 stopie ń swobody

Prędkości obrotowe przegubów w pobli żu osobliwo ści manipulatora 2-członowego

Końcówka robocza przemieszcza si ę wzdłu ż osi X 0 z prędkością vx

θ1

-θ2

L1

L2

X 0

Y 0

Jak zmieniaj ą się prędko ści obrotowe członów w zale żności od warto ści zmiennych konfiguracyjnych?

vx

Prędkości obrotowe przegubów w pobli żu osobliwo ści manipulatora 2-członowego

Stąd:

W pobli żu osobliwo ści (dla θ2→0) jest:

Siły statyczne w manipulatorach

Z i

Z i+1

iMi

iFi

i+1Fi+1

i+1Mi+1

X i

X i+1

Fi – siła w przegubie „i”Mi – moment siły w przegubie „i”

Z równowagi sumy sił i momentów działaj ących na człon „i”:

Ponadto:

Siły statyczne w manipulatorach – c.d.

Znając sił ę Fn i moment Mn którymi ostatni człon manipulatora oddziałuje na otoczenie, mo żna – korzystaj ąc z wyprowadzonych wzorów – obliczy ć siły Fn-1 , Fn-2 … i momenty Mn-1 , Mn-2 … w przegubach manipulatora, zapewniaj ące równowag ę statyczn ą. Istnienie tych sił jest częściowo rezultatem działania nap ędów. Siły i momenty rozwijane przez nap ędy poszczególnych przegubów oznaczamy τ1, τ2, τ3 … τn.

Dla pary przesuwnej:

(jest to składowa fz siły Fi)

Dla pary obrotowej:

(jest to składowa mz momentu Mi)

Przykład: siły statyczne w manipulatorze dwuczłonowym

θ1

θ2

L1

X 0

Y 0

X 1Y 1

X 2Y 2

X 3

Y 3

3F3

Obliczamy rozwijane momenty siłnapędowych w przegubach „1” i „2”,w warunkach równowagi statycznej.Poprzednio stwierdzono, że:

Człon roboczy niech oddziałuje na otoczenie sił ą 3F3 o składowych fx, fy:

L2

Przykład: siły statyczne w manipulatorze dwuczłonowym – c.d.

Przykład: siły statyczne w manipulatorze dwuczłonowym – c.d.

Zatem potrzebne momenty sił nap ędowych w przegubach:

Można zauwa żyć, że wyst ępuje tu macierz transponowana jakobianu 3J.Czy to przypadek? Przekonamy si ę, że nie.

albo:

Jakobiany w dziedzinie siły

Zasada prac przygotowanych:

F dX = τ dθ (iloczyny skalarne)

F – wektor 6×1 „siła – moment siły” rozwijany przez czło n roboczyΤ – wektor n×1 sił i momentów nap ędowych w poł ączeniach ruchowych

(n – liczba poł ączeń ruchowych)dX – niesko ńczenie małe przemieszczenie kartezja ńskiedθ – niesko ńczenie małe przemieszczenie w poł ączeniach ruchowych

Zastępuj ąc iloczyn skalarny mno żeniem macierzy otrzymujemy:

FT · dX = τT · dθPoniewa ż – jak pokazano wcze śniej: dX = J(θ)·dθ

więc: FT · J ·dθ = τT · dθ czyli: FT · J = τT

Transponuj ąc obie strony równania otrzymujemy:

τ = (FT · J)T = JT · (FT)T = JT ·F

Obliczenie momentów nap ędowych za pomoc ąjakobianu – przykład

Chwytak manipulatora jak na rysunku działa sił ą 5N przeciwnie do osi X 0. Obliczy ć momenty nap ędowe w przegubach, rozwijane w warunkach równowagi statycznej

θ1=45º

X 0

Y 0

F=5N

L1=0.5m

θ2=90º

L2=0.5m

Osobliwo ści a siły statyczne

W osobliwo ściach przeło żenie mechanizmu mo że dążyć doniesko ńczono ści. Rozwa żmy ostatni przykład dla zmiennychkonfiguracyjnych θ1=0º i θ2=0º

L1 L2

X 0

Y 0

F=5N

W pobli żu osobliwo ści momenty τ1 i τ2 bliskie zera umo żliwiaj ąwywarcie siły dowolnie wielkiej

Generowanie trajektorii

Niezależnie od sposobu programowania robota (teach-in, off- line) układ sterowania zazwyczaj musi wygenerowa ć trajektori ę zadaną przez użytkownika.

Generowanie punktów trajektorii zazwyczaj jest real izowane z cz ęstotliwo ścią20–200 Hz (im wi ększa cz ęstotliwo ść, tym dokładniejsza ścieżka, ale równie żtym wi ększa obliczeniochłonno ść).

Pozycja pocz ątkowa

Pozycja ko ńcowa

Trajektoria

Przy generowaniu trajektorii mog ą być uwzgl ędniane ró żne sytuacje i aspekty:• Użytkownik mo że zadawać tylko poło żenie ko ńcowe (pozycj ę i orientacj ę)• Zadawane jest poło żenie ko ńcowe i jedno lub kilka po średnich• Może być określony czas trwania ruchu (po średnie zadawanie pr ędko ści)• Często wyst ępuje wymóg płynno ści ruchu (braku du żych przyspiesze ń)• Może być zadana ściśle okre ślona ścieżka (interpolacja liniowa, kołowa itp.)

Planowanie trajektorii w przestrzeni współrz ędnych konfiguracyjnych

Typowy algorytm działania:

• Użytkownik zadaje poło żenie (pozycj ę i orientacj ę) dla poło żeńpośrednich i poło żenia ko ńcowego

• Dla poło żeń tych rozwi ązywane jest odwrotne zadanie kinematyki (czyli okre śla si ę warto ści zmiennych konfiguracyjnych dla poszczególnych poł ączeń ruchowych)

• Dla każdego z poł ączeń ruchowych wyznacza si ę gładk ą funkcj ęprzemieszcze ń przechodz ącą przez te poło żenia

• Zazwyczaj zakłada si ę, że czas przemieszczania pomi ędzy poszczególnymi poło żeniami jest taki sam dla wszystkich poł ączeńruchowych (wszystkie przeguby rozpoczynaj ą i kończą pracę w tym samym momencie)

Zastosowanie wielomianów 3-go stopnia w planowaniu trajektorii

θ

t

θk

θ0

tk

Różne mo żliwe trajektorie

Trajektorie powinny spełnia ć nast ępuj ące warunki ograniczaj ące:

θ(0) = θ0 θ(tk)=θk

θ(0) = 0 oraz zazwyczaj θ(tk)=0

Cztery warunki oznaczaj ą, że trajektoria mo że być opisana wielomianem stopnia co najmniej trzeciego

stąd:

Z warunków ograniczaj ących wynika:

więc jego pochodna:

Zastosowanie wielomianów 3-go stopnia w planowaniu trajektorii – c.d.

Wielomian 3-go stopnia:

Zastosowanie wielomianów 3-go stopnia w planowaniu trajektorii – c.d.

więc:

Podsumowuj ąc:

Zastosowanie wielomianów 3-go stopnia w planowaniu trajektorii – c.d.

Trajektoria dla pojedynczego przegubu: θ

t

θ0

tk

θk

θ

ttk

θ

t

tk

Przebieg zmiennej konfiguracyjnej:

Przebieg pr ędkości:

Przebieg przyspieszenia:

Wielomiany 3-go stopnia dla trajektorii z pozycjami po średnimi

Wariant 1 – zatrzymanie w pozycji po średniej: zagadnienie sprowadza się do poprzedniego rozwi ązania

Wariant 2 – bez zatrzymywania w pozycji po średniej

prędkość w pozycji po średniej pr ędkość w pozycji po średniejzadawana przez u żytkownika obliczana automatycznie

Wielomiany 3-go stopnia: pr ędkości w pozycjach pośrednich zadawane r ęcznie

Dla tego wariantu trajektori ę rozbijamy na trajektorie elementarne (pozycja początkowa → 1-sza pozycja po średnia, 1-sza pozycja po średnia → 2-ga pozycja po średnia, … , ostatnia pozycja po średnia → pozycja ko ńcowa), ka żdąz nich obliczaj ąc jak poprzednio, cho ć z nieco innymi warunkami brzegowymi:

Wielomiany 3-go stopnia: pr ędkości w pozycjach pośrednich zadawane r ęcznie – c.d.

Wielomiany 3-go stopnia: pr ędkości w pozycjach pośrednich zadawane r ęcznie – c.d.

Podsumowuj ąc:

Algorytm post ępowania:

• Zadaje si ę wektor pr ędko ści V w pozycji pocz ątkowej, ko ńcowej i każdej pozycji po średniej

• Dla wszystkich pozycji oblicza si ę θ = J-1 · V• Dla każdego odcinka oblicza si ę trajektori ę według podanych wzorów• W niektórych przypadkach trzeba uwzgl ędnić ograniczenia dotycz ące

prędkości (zwłaszcza w pobli żu osobliwo ści)

Wielomiany 3-go stopnia: pr ędkości w pozycjach pośrednich obliczane automatycznie

Post ępowanie przy automatycznym obliczaniu pr ędkości w pozycjach pośrednich:

- Przeważnie stosuje si ę algorytmy heurystyczne (z czego wynika uzyskanie rozwi ązania niekoniecznie optymalnego)

- Zazwyczaj d ąży si ę do zapewnienia ci ągłości przyspiesze ń w pozycjach po średnich

Przykładowy algorytm heurystyczny

Dla poszczególnych pozycji wyznacza si ę w układzie współrz ędnych θ – t styczne do trajektorii w zadanych pozycjach:

θ

t

Zasady:• Jeśli nast ępuje zmiana kierunku ruchu, to styczna jest pozioma (prędkość = 0)• Gdy nie ma zmiany kierunku ruchu, to styczna jest n achylona do osi „t” pod

kątem będącym średni ą arytmetyczn ą kątów, pod jakimi s ą nachylone odcinki łączące punkt trajektorii odpowiadaj ący danej pozycji z punktami odpowiadaj ącymi pozycji poprzedniej i nast ępnej

θ0

θ

t

θ0

Wielomiany 3-go stopnia a ci ągłośćprzyspieszenia w pozycji po średniej

Załóżmy taki sam odst ęp czasowy t p pomi ędzy pozycj ą pocz ątkow ą i pośredni ą oraz po średni ą i końcow ą:

θ

t

θk

θ0

tp

θp

tp

Wielomian dla trajektorii θ0 – θp: Wielomian dla trajektorii θp – θk:

Wielomiany 3-go stopnia a ci ągłośćprzyspieszenia w pozycji po średniej – c.d.

Dla zapewnienia prostoty oblicze ń, każdy z wielomianów rozpatrujemy w dziedzinie <0 , tp>.Warunki ograniczaj ące:

Stąd współczynniki:

Wielomiany 3-go stopnia a ci ągłośćprzyspieszenia w pozycji po średniej – c.d.

Z równa ń 1,2 i 5:

Z równa ń 3 i 4:

Równanie 6:

Z równa ń 5 i 7:

Równanie 8:

Wielomiany 3-go stopnia a ci ągłośćprzyspieszenia w pozycji po średniej – c.d.

Z równa ń 10, 11 i 12:

Z równania 9:

Uwzględniaj ąc 11, 12 i 13:

Wielomiany 3-go stopnia a ci ągłośćprzyspieszenia w pozycji po średniej – c.d.

Jak wykazano wcze śniej:

więc:

Z równa ń 12 i 14:

Z równa ń 11 i 14:

Wielomiany 3-go stopnia a ci ągłośćprzyspieszenia w pozycji po średniej – c.d.

Na podstawie równa ń 13 i 14:

Podsumowuj ąc:

Wielomiany 3-go stopnia a ci ągłośćprzyspieszenia w pozycji po średniej – c.d.

Przy planowaniu trajektorii z wykorzystaniem wielom ianu o współczynnikach obliczonych według wyprowadzonych w zorów, prędkość połączenia ruchowego w poło żeniu po średnim b ędzie:

Przykład generowania trajektorii dla manipulatora dwuczłonowego

Proszę spróbowa ć opracowa ć to samodzielnie.

Generowanie trajektorii z wykorzystaniem funkcji liniowej ł ączonej z fragmentami parabolicznymi

Zastosowanie samej funkcji liniowej przy generowani u trajektorii nie zapewnia ci ągłości funkcji pr ędko ści:

θ

t

θ0

tk

θk

θ

tθ0

tk

θk

Dlatego dodajemy fragmenty paraboliczne. Pr ędkość w chwili t p oraz (tk–tp) musi by ć taka sama w cz ęści liniowej i parabolicznej:

tp tk–tp

θp

Zastosowanie funkcji liniowej ł ączonej z fragmentami parabolicznymi – c.d.

Na fragmentach parabolicznych zakładamy stałe przys pieszenie θp o warto ści ap:

Wówczas w chwili tp rozwijana jest pr ędkość:

Prędkość ta musi by ć równa pr ędkości na odcinku liniowym:

Zatem:

Chcemy obliczy ć tp. W powy ższej zale żności wyst ępuje warto ść θp, któr ą można obliczy ć ze wzoru:

Zastosowanie funkcji liniowej ł ączonej z fragmentami parabolicznymi – c.d.

Stąd:

Aby istniało rozwi ązanie, musi by ć ∆ ≥ 0 :

Dla:

Dla:

Zastosowanie funkcji liniowej ł ączonej z fragmentami parabolicznymi – c.d.

Zatem w obu przypadkach:

Możliwe rozwi ązania:

Czas trwania przyspieszenia tp nie mo że być większy od połowy tk:

Dalsze rozwa żania dla przypadku:

Zatem pierwiastek t p1 spełnia warunek tylko dla ∆ = 0

Zatem pierwiastek t p2 spełnia warunek zawsze, gdy ∆ ≥ 0

Zastosowanie funkcji liniowej ł ączonej z fragmentami parabolicznymi – c.d.

Stąd rozwi ązanie:

przy zało żeniu, że przyspieszenie musi mie ć warto ść co najmniej:

Przypadek skrajny (brak fragmentu liniowego):

(Rozważania dla przypadku θp < 0 prosz ę przeprowadzi ć samodzielnie)

Funkcja liniowa z fragmentami parabolicznymi: uwzgl ędnienie punktów po średnich

Załóżmy, że przyspieszenie na parabolicznym fragmencie trajek torii w pobli żu punktu k wynosi ak.

Prędkości na odcinkach liniowych pomi ędzy j i k oraz k i l wynosz ą:

θ

t

θk

t j

θj

θl l

tpktpj

tk

k

j

t l

Funkcja liniowa z fragmentami parabolicznymi: uwzgl ędnienie punktów po średnich – c.d.

Chcemy obliczy ć tpk. Korzystamy ze wzoru na przyspieszenie na fragmencie parabolicznym w pobli żu punktu k:

a poniewa ż:

więc:

Funkcja liniowa z fragmentami parabolicznymi: uwzgl ędnienie punktów po średnich – c.d.

Szczególna sytuacja odnosi si ę do poło żenia pierwszego i ostatniego (w poło żeniach tych pr ędko ść jest równa zeru):

Dla pierwszego fragmentu parabolicznego zakładamy p rzyspieszenie o warto ści bezwzgl ędnej a0:

θ

t

θ0

t1

θp

θ1

tp1

tp0

Funkcja liniowa z fragmentami parabolicznymi: uwzgl ędnienie punktów po średnich – c.d.

Prędkość w chwili tp0 wynosi:

Musi ona by ć równa pr ędkości na odcinku liniowym:

A poniewa ż:

Możliwe rozwi ązania:

Poniewa ż musi by ć spełniony warunek:

Funkcja liniowa z fragmentami parabolicznymi: uwzgl ędnienie punktów po średnich – c.d.

Dalsze rozwa żania dotycz ą przypadku θ1 > θ0 czyli θ0 > 0(przypadek θ1 < θ0 prosz ę rozpatrze ć samodzielnie)

Aby istniało rozwi ązanie, musi by ć ∆ ≥ 0:

niemo żliwe

Funkcja liniowa z fragmentami parabolicznymi: uwzgl ędnienie punktów po średnich – c.d.

więc dla wariantu 1:

Dla wariantu 2:

Zatem:

przy ograniczeniu:

Funkcja liniowa z fragmentami parabolicznymi: uwzgl ędnienie punktów po średnich – c.d.

Należy zauwa żyć, że ograniczenie to jest ostrzejsze, ni ż podane wcześniej ograniczenie wynikaj ące z warunku ∆ ≥ 0 :

Funkcja liniowa z fragmentami parabolicznymi: uwzgl ędnienie punktów po średnich – c.d.

Przy zastosowaniu omówionej metody, trajektoria nie przechodzi przez zadane punkty po średnie, a tylko w ich pobli żu.Aby uzyska ć efekt dokładnego przechodzenia przez punkty po średnie(czyli aby punkty po średnie były tzw. punktami przej ściowymi ), można wykorzysta ć dodatkowe tzw. punkty pseudopo średnie :

punkty pseudopo średnie

θ

t

θk

t j

θj

tk

k

j

Generowanie trajektorii z punktami po średnimi –potencjalne problemy

• Nieosi ągalne punkty po średnie• Duże prędkości k ątowe w parach obrotowych w pobli żu osobliwo ści• Osiąganie poło żenia pocz ątkowego i ko ńcowego z ró żną orientacj ą

kątową• …..

Opis trajektorii w j ęzyku programowania robota na przykładzie j ęzyka AL

• Move ARM to C with duration = 3 seconds• move ARM to C linearly with duration = 3 seconds• move ARM to C via B• move ARM to C via B, A, D• …..