Algorytmy wysokiej dokładności śledzenia trajektorii
robota przemysłowego
05.12.2019
mgr inż. Wojciech Zwonarz
promotor: prof. dr hab. inż. Andrzej Turnau
Akademia Górniczo-Hutnicza im. Stanisława Staszica w Krakowie
AGH University of Science and Technology
Agenda
● Trajektorie w robotyce
● Interpolacje Beziera dla zmniejszenia ilości obliczeń
● Kinematyka i dynamika robotów
● Typowe algorytmy regulacji
● Sterowanie predykcyjne dla śledzenia trajektorii
● Funkcje kary za zbliżanie się do przeszkody i ograniczeń oraz za niedokładne śledzenie
● Podsumowanie
Trajektoria ruchu
W pracy opisano trzy elementarne trajektorie:
Helisa – linia śrubowa
Elipsa
Prosta
Interpolacja krzywymi Beziera
Interpolacja typu spline
krzywymi Beziera drugiego rzędu okazała się być
najdokładniejszym
sposobem odwzorowania trajektorii w prostej postaci wielomianowej.
X(t
0)
X(t
1) X(t
k)
X(t
2)
P
2P
1P
3Krzywe Beziera drugiego rzędu
Krzywe B-sklejane są to ,,sklejone'' wielomiany
niskiego stopnia, zwykle nie większego niż trzeci. Są one regularne w rzędzie zależnym od stopnia wielomianu.
Podstawowe wzory opisujące aproksymowaną drogę
efektora mają podaną postać.
p ( t ) = ∑
i=0 m−n−1
p
iN
in( t ) , t ∈ ⟨u
n,u
m−1⟩
N
i0 ( t ) = { 1 dla t 0 dla t ∈ ∉ [ [ u u
ii,u ,u
i+1i+ 1] ] }
N
in( t ) = t−u
iu
i+n−u
iN
in−1( t ) + u
i+n+ 1−t
u
i+n+ 1−u
i+1N
i+1n−1( t ) , dla n> 0 p (t)= [ P 1 P 2 P 3 ] [ −2 1 1 −2 1 2 0 0 0 ] [ t 1 t 2 ]
P
i= { v 2 X X ( ( t ( t t
i−1ii) ) –X –X ) − ( ( v t t
i −1i −1( t
i) ) ) –v dla v ( t
i) t
i( t +v
i−1( ) t =v
i −1( ) t t
ii −1) dla v ( t
i−1) ≠ v ( t
i) }
Kinematyka i dynamika robotów
Macierz transformacji
pomiędzy członami i oraz i–1.
Dynamika uzyskana przy pomocy równania Eulera- Lagrange'a. Na podstawie kinematyki uzyskanej przy pomocy notacji DH.
Wynikowe równanie jest nieliniowe względem
zmiennych przegubowych.
L=E k − E p d dt
∂ L
∂ ˙q − ∂ L
∂ q =τ
D(q) ¨q+C(q , ˙q) ˙q+g(q)+f (q , ˙q)=τ E K ( q , ˙q)= 1
2 ˙q T ∑ i=1 n [ m i J v
i( q) T J v
i( q)
+ J ω
i( q) T R i ( q) I i R i ( q) T J ω
i( q) ] ˙q
E P (q)= ∑ i=1 n g T r ci m i
A i i−1 = RotZ (Θ i ) TranZ (d i ) TranX (a i ) RotX (α i ) Dynamika:
Kinematyka:
Sterowniki robotów
Typy regulatorów
omawiane w pracy
Klasyczne
Adaptacyjne
Neuronowe
Predykcyjne
Regulatory neuronowe
Regulatory wzbogacone o sieci neuronowe zwykle składają się z członu PD oraz sieci neuronowej ze sprzężeniem w przód, która pozwala na
redukcję błędu od
elementów nieliniowych
dynamiki.
Regulatory predykcyjne
Regulator predykcyjny składa się z bloku predykcyjnego oraz bloku poszukiwania minimum funkcji kosztu sterowania.
¯y (t ) y (t)
˙y (t ) v(t) u(t)
¯ u(t)
Wyznaczone sterowanie Zadana trajektoria
Próbne sterowanie Wyjście obiektu
Wyjście modelu obiektu
Pochodna po czasie wyjścia obiektu C Regulator
O ¯ Model obiektu
Wybór najlepszego sterowania
Robot dwuczłonowy jako obiekt regulacji
predykcyjnej.
Funkcja kosztu
Koszt sterowania można opisać jako błąd na
zadanym horyzoncie czasowym predykcji.
J ( u 1 ,u 2 ,h ) = ∫
t= 0 h
|q ( t ) − v ( t ) | 2 dt
Predykcja numeryczna
Predykcja używa metod numerycznych.
Najprostsza predykcja:
q i+1 =q i + ˙q i ⋅ d sim
Poszukiwanie minimum
Metoda Powella jest
iteracyjną bezgradientową metodą poszukiwania
minimum funkcji wielowymiarowej.
Jako metody poszukiwania na prostej, użyto złotego podziału. Jest ona prosta w implementacji oraz
szybka w wykonaniu.
Sterowanie przy pomocy funkcji kary
Użycie funkcji kary pozwala na zwiększenie liczby źródeł błędu. Oznacza to, że podczas poszukiwania minimum możliwym jest jednoczesne podążanie za trajektorią oraz omijanie przeszkód.
f p ( t ) =f q ( x ( t ) ) +f R ( x ( t ) ) +f o ( x ( t ) )
f p f q
f R f o Funkcja kary
Kara od położenia
Kara od ograniczeń robota
Kara od przeszkód
Kary od położenia końcowego i ograniczeń robota
Odległość pomiędzy położeniem końcówki robota
w danym ruchu oraz docelowym położeniem w kroku.
f ¯ x =ϴ ( ¯x ( t n ) − x ( t n ) ) 2
Kara która jest nakładana na regulator za przekroczenie ograniczeń konstrukcyjnych robota. Przykład dla
maksymalnej dopuszczalnej zmiany położenia w jednym kroku.
f R =ξ ( x ( t n ) − ρ x v ( t n−1 ) ) 4
Kara od przeszkody
Kara za zbytnie zbliżenie się do przeszkody o zadanej wielkości.
Przykład jednowymiarowy.
f o 1 ( x ( t n ) ) =arctan ( ξ n ( x ( t n ) − ( x̅ o −r̅ o ) ) ) +
− arctan ( ξ n ( x ( t n ) − ( x̅ o +r̅ o ) ) )
Kara dla wielowymiarowej przeszkody
Przykład jednowymiarowy można z łatwością rozszerzyć do N wymiarów:
Dla M przeszkód w N wymiarowej przestrzeni funkcja kary przyjmuje postać:
f o N = ∏
i=1 N
f o i ( x ( t ) )
f o = ∑
i=1 M
f o N ( x ( t ) )
Kara dla wielu przeszkód
Funkcja kary w przestrzeni 2D
Dla przestrzeni dwu-
wymiarowej możliwym jest narysowanie wykresu funkcji kary.
Funkcja kary z jedną
przeszkodą w zasięgu ruchu
manipulatora.
Detekcja przeszkód
Detekcja ruchu
Detekcja koloru
Wykryta przeszkoda
Obliczanie pozycji 3D
Wyznaczenie położenia przy pomocy prostego przekształcenia geometrycznego
X 1 =F C
1
+ ⃗ a 1 − ( ⃗ a 1 · ⃗ a 2 ) ( ⃗ a 2 · ⃗f ) + ( ⃗ a 1 · ⃗f )( ⃗ a 2 · ⃗ a 2 ) ( ⃗ a 1 ·⃗ a 1 )( ⃗ a 2 · ⃗ a 2 ) − ( ⃗ a 1 · ⃗ a 2 ) ( ⃗ a 1 · ⃗ a 2 )
X 2 =F C
2
+⃗ a 2 − ( ⃗ a 1 · ⃗ a 2 )( ⃗ a 1 · ⃗f ) + ( ⃗ a 2 · ⃗f ) ( ⃗ a 1 · ⃗ a 1 ) ( ⃗ a 1 · ⃗ a 1 ) ( ⃗ a 2 · ⃗ a 2 ) − ( ⃗ a 1 · ⃗ a 2 ) ( ⃗ a 1 · ⃗ a 2 )
X est = X 1 +X 2
2
Pomiary rzeczywiste z dwóch kamer
Błąd podczas manewru omijania
Funkcja błędu lepiej obrazuje faktyczne zachowanie robota.
W pierwszej fazie robot zwalniał aby nie uderzyć w przeszkodę.
Gdy przeszkoda została
ominięta robot, znów
mógł przyspieszyć.
Manewr omijania
Wynik eksperymentu rzeczywistego
omijania przeszkody znajdującej się na torze ruchu.
Regulator omija przeszkodę bez interwencji
operatora.
Kara w trakcie trwania ruchu
Funkcja kary osiąga maksimum w chwili mijania przeszkody.
Po manewrze
ominięcia funkcja
kary maleje.
Podsumowanie
Oryginalnym wkładem autora jest:
●
implementacja złożonej procedury polegającej na przejściu od planowanej trajektorii, z zastosowaniem punktów kontrolnych, przez równania kinematyki odwrotnej do przestrzeni
przegubowej robota, a następnie konstrukcji trajektorii
interpolacyjnej przy użyciu krzywych Beziera, co pozwala na jej użycie w funkcji kary,
●
