Interpolacja trajektorii
Interpolacja trajektorii
Sterowanie i
Sterowanie i
Programowanie
Programowanie
Robotów
Robotów
Wykład nr 05
Wykład nr 05
dr inż. Tomasz Trawiński
Politechnika Śląska, Wydział
Politechnika Śląska, Wydział
Elektryczny
Elektryczny
KATEDRA MECHATRONIKI
KATEDRA MECHATRONIKI
Metody planowania trajektorii
Metody planowania trajektorii
• Proces projektowania bezkolizyjnej trajektorii przeprowadza się gdy dane są:
1. Opisy geometryczne:
• Przestrzeni roboczej,
• Manipulatora,
• Chwytaka – narzędzia,
• Przeszkód
2. Algorytm wykrywania kolizji,
3. Równania kinematyki.
Metody planowania trajektorii
Metody planowania trajektorii
Trajektoria łączy konfigurację początkową:
)
,
,
,
(
2
1
n
s
s
s
s
q
q
q
q
z konfiguracją końcową:
)
,
,
,
(
2
1
n
k
k
k
k
q
q
q
q
przy założeniu, że nie zostały naruszone
ograniczenia konstrukcyjne.
Interpolacja trajektorii
Interpolacja trajektorii
Ruch od punktu do punktu.
• Konieczne jest wygenerowanie gładkiej krzywej łączącej punkt początkowy i końcowy,
• Dla zadań bardziej skomplikowanych niezbędne jest bardzo często podanie dodatkowych punktów pośrednich a także dodatkowych ograniczeń na prędkość lub przyspieszenie,
• Przy zadanych punktach początkowych, końcowych oraz ewentualne pośrednich do znalezienia niezbędnych wartości zmiennych przegubowych konieczne jest rozwiązanie
kinematyki odwrotnej
Interpolacja trajektorii
Interpolacja trajektorii
Ruch od punktu do punktu – interpolacja wielomianem
3 stopnia.
• Jednym ze sposobów generowania gładkiej krzywej jest zastosowanie
wielomianowej funkcji zmiennej w czasie.
• Zakładamy, że w chwili „t
0
” zmienna przegubowa „i” spełnia warunki:
0
0
q
t
q
i
'
0
0
q
t
q
i
oraz w „t
k
” (czasie końcowym) osiąga wartości:
1
k
q
t
q
i
'
1
k
q
t
q
i
(1)
(2)
Warunki (1) i (2) są to tzw. więzy interpolacji.
Interpolacja wielomianem 3 stopnia
Interpolacja wielomianem 3 stopnia
Ruch od punktu do punktu.
Mając nałożone cztery więzy (równania (1) i (2)) potrzebny jest wielomian
z czterema niezależnymi współczynnikami „a
i
”, które muszą być tak
dobrane, aby spełniać te ograniczenia.
(3)
Rozważając trajektorię trzeciego stopnia wyrażoną równaniem:
3
3
2
2
1
0
t
a
t
a
t
a
a
q
d
i
Oraz pochodną po czasie z ww. wielomianu – czyli prędkość:
2
3
2
1
3
2
t
a
t
a
a
q
d
i
(4)
Interpolacja wielomianem 3 stopnia
Interpolacja wielomianem 3 stopnia
Ruch od punktu do punktu.
Połączenie równań (3) i (4) z równaniami więzów (1) i (2) daje
cztery równania z czterema niewiadomymi:
(5)
3
0
3
2
0
2
0
1
0
0
t
a
t
a
t
a
a
q
2
0
3
0
2
1
0
3
2
'
t
a
t
a
a
q
2
k
3
k
2
1
1
3
2
'
t
a
t
a
a
q
Interpolacja wielomianem 3 stopnia
Interpolacja wielomianem 3 stopnia
Ruch od punktu do punktu.
Równania (5) mogą zostać przedstawione w postaci macierzowej:
(6)
'
1
1
'
0
0
3
2
1
0
2
k
k
3
k
2
k
k
2
0
0
3
0
2
0
0
3
2
1
0
1
3
2
1
0
1
q
q
q
q
a
a
a
a
t
t
t
t
t
t
t
t
t
t
Interpolacja wielomianem 3 stopnia
Interpolacja wielomianem 3 stopnia
Przykład 1.
Rozwiążmy równanie (6) przy następujących więzach:
co sprowadza się do rozwiązania równania macierzowego:
0
0
3
2
1
0
1
1
1
1
0
0
1
0
0
0
0
1
1
0
3
2
1
0
q
q
a
a
a
a
'
1
1
'
0
0
3
2
1
0
2
k
k
3
k
2
k
k
2
0
0
3
0
2
0
0
3
2
1
0
1
3
2
1
0
1
q
q
q
q
a
a
a
a
t
t
t
t
t
t
t
t
t
t
0
,
0
,
1
,
0
'
1
'
0
k
0
q
q
t
t
Interpolacja wielomianem 3 stopnia
Interpolacja wielomianem 3 stopnia
Przykład 1.
Sprowadza się to do następujących obliczeń:
1
0
3
1
0
2
1
0
0
2
2
3
3
0
q
-
q
a
q
+
q
-
a
a
q
a
0
3
2
0
3
2
1
1
3
2
1
0
1
0
0
a
+
a
+
a
q
+a
+a
+a
a
a
q
a
W efekcie otrzymujemy wielomiany:
3
1
0
2
1
0
0
)
(
2
)
(
3
)
(
t
-q
q
t
+q
-q
q
t
q
i
2
1
0
1
0
'
)
(
6
)
(
6
)
(
t
-q
q
t
+q
-q
t
q
i
t
-q
q
+q
-q
t
q
i
)
(
12
)
(
6
)
(
1
0
1
0
''
Interpolacja wielomianem 3 stopnia
Interpolacja wielomianem 3 stopnia
Przykład 1.
Przykład 1.
q
0
= 10
q
1
=-20
3
1
0
2
1
0
0
)
(
2
)
(
3
)
(
t
-q
q
t
+q
-q
q
t
q
i
2
1
0
1
0
'
)
(
6
)
(
6
)
(
t
-q
q
t
+q
-q
t
q
i
t
-q
q
+q
-q
t
q
i
)
(
12
)
(
6
)
(
1
0
1
0
''
Interpolacja wielomianem 3 stopnia
Interpolacja wielomianem 3 stopnia
Jeżeli czas początkowy „t
0
” nie jest równy zero, możemy wyprowadzić wzór ogólny na interpolację wielomianami 3 stopnia,
przesuwając oś czasu w lewo o „t
0
”.
Jeśli mamy czas początkowy „t
0
” i końcowy „t
k
” oraz więzy:
q
d
(t
0
)=q
0
,
q
d
(t
k
)=q
1
,
q
’d
(t
0
)=q
’0
,
q
’d
(t
k
)=q
’1
3
0
3
2
0
2
0
1
0
)
(
)
(
)
(
)
(
t
t
a
t
t
a
t
t
a
a
t
q
d
3
0
k
0
k
'
1
'
0
1
0
3
2
0
k
0
k
'
1
'
0
0
1
2
'
0
1
0
0
)
(
)
)(
(
)
(
2
)
(
)
)(
2
(
)
(
3
t
t
t
t
q
q
q
q
a
t
t
t
t
q
q
q
q
a
q
a
q
a
Gdzie:
Uwaga:
warunki końcowe z
kroku
wcześniejszego są
warunkami
początkowymi dla
kroku następnego
(przemieszczenie i
prędkość).
(8)
Interpolacja wielomianem 3 stopnia
Interpolacja wielomianem 3 stopnia
% Deklaracja zmienny symbolicznych
syms a0 a1
a2
a3
...
% wspolczynniki wielomianu interpolujacego
q0
qk
...
% zmienne przegubowe poczatkowa i koncowa
dq0
dqk
% predkosci brzegowe
% zadany czas poczatkowy i koncowy
syms
t0
tk
% postac wielomianu - 3 stopnia
% qi=a0+a1*t+a2*t^2+a3*t^3
% postac predkosci
% dqi=a1+2*a2*t+3*a3*t^2
% Formulowanie rownania do wyznaczenia wspolczynnikow wielomianow
A=[1 t0 t0^2 t0^3;
...
0 1 2*t0 3*t0^2;
...
1 tk tk^2 tk^3;
...
0 1 2*tk 3*tk^2]
a=[a0;a1;a2;a3]
b=[q0; dq0; qk; dqk]
ai=inv(A)*b
f(u)
wielomian int.
t0
tk
q0
qk
dq0
dqk
ai
Subsystem
Scope2
Scope1
Scope
du/dt
Derivative1
du/dt
Derivative
-20
Constant4
-20
Constant3
10
Constant2
1
Constant1
0
Constant
Clock
Interpolacja wielomianem 3 stopnia
Interpolacja wielomianem 3 stopnia
Wielomian 3
stopnia
Obliczanie
współczynników
wielomianu 3
stopnia
Interpolacja wielomianem 3 stopnia
Interpolacja wielomianem 3 stopnia
1
ai
f(u)
a3
f(u)
a2
f(u)
a1
f(u)
a0
6
dqk
5
dq0
4
qk
3
q0
2
tk
1
t0
ai =
(qk*t0^2*(t0 - 3*tk))/(t0 - tk)^3 - (dqk*t0^2*tk)/(t0 - tk)^2 -
(dq0*t0*tk^2)/(t0 - tk)^2 + (q0*tk^2*(3*t0 - tk))/(t0 - tk)^3
dqk + (3*tk*(2*qk - 2*q0 + dq0*tk + dqk*tk))/(t0 - tk)^2 +
(2*tk*(dq0 + 2*dqk))/(t0 - tk) - (6*tk^2*(q0 - qk))/(t0 - tk)^3
-(tk*(3*qk - 3*q0 + t0*(dq0 - dqk)) + t0^2*(dq0 + 2*dqk) -
tk^2*(2*dq0 + dqk) - t0*(3*q0 - 3*qk))/(t0 - tk)^3
(dq0 + dqk)/(t0 - tk)^2 - (2*q0 - 2*qk)/(t0 - tk)^3
f(u)
wielomian int.
t0
tk
q0
qk
dq0
dqk
ai
Subsystem
Scope2
Scope1
Scope
du/dt
Derivative1
du/dt
Derivative
-20
Constant4
-20
Constant3
10
Constant2
1
Constant1
0
Constant
Clock
Interpolacja wielomianem 3 stopnia
Interpolacja wielomianem 3 stopnia
u(1)+u(2)*u(5)+u(3)*u(5)^2+u(4)*
u(5)^3
Interpolacja wielomianem 3 stopnia
Interpolacja wielomianem 3 stopnia
Postać współczynników wielomianu 3 stopnia przy założeniu że prędkość
początkowa i końcowa równają się zero:
ai =
(qk*t0^3 - 3*qk*t0^2*tk + 3*q0*t0*tk^2 - q0*tk^3)/(t0 - tk)^3
-(6*t0*tk*(q0 - qk))/(t0 - tk)^3
(3*(q0 - qk)*(t0 + tk))/(t0 - tk)^3
-(2*(q0 - qk))/(t0 - tk)^3
a
0
a
1
a
2
a
3
Interpolacja trajektorii
Interpolacja trajektorii
Ruch od punktu do punktu – interpolacja wielomianem 5
stopnia.
• Zakładamy, że w chwili „t
0
” zmienna przegubowa „i” spełnia warunki:
0
0
q
t
q
i
'
0
0
q
t
q
i
oraz w „t
k
” (czasie końcowym) osiąga wartości:
1
k
q
t
q
i
'
1
k
q
t
q
i
(9)
(10)
Warunki (9) i (10) są to tzw. więzy interpolacji.
''
0
0
q
t
q
i
''
1
k
q
t
q
i
Interpolacja wielomianem 5 stopnia
Interpolacja wielomianem 5 stopnia
• Jeśli wymaga się ciągłości trajektorii aż do drugiej pochodnej, to powinny być spełnione następujące warunki:
0
0
q
t
q
i
0
0
t
q
i
oraz w „t
k
” (czasie końcowym) osiąga wartości:
1
k
q
t
q
i
0
k
t
q
i
(11)
(12)
0
0
t
q
i
0
k
t
q
i
Interpolacja wielomianem 5 stopnia
Interpolacja wielomianem 5 stopnia
Powyższe sześć warunków może spełnić wielomian piątego stopnia
5
0
5
4
0
4
3
0
3
2
0
2
0
1
0
)
(
)
(
)
(
)
(
)
(
)
(
t
t
a
t
t
a
t
t
a
t
t
a
t
t
a
a
t
q
Który po uwzględnieniu równań więzów (11) i (12) przyjmuje postać:
0
1
5
4
3
0
)
6
15
10
(
q
q
t
t
t
q
t
q
(13)
(14)
Interpolacja wielomianem 5 stopnia
Interpolacja wielomianem 5 stopnia
Wielomian piątego stopnia
5
5
4
4
3
3
2
2
1
0
)
(
t
a
t
a
t
a
t
a
t
a
a
t
q
Pierwsza pochodna czasowa wielomianu piątego stopnia
4
5
3
4
2
3
2
1
5
4
3
2
)
(
t
a
t
a
t
a
t
a
a
t
q
Druga pochodna czasowa wielomianu piątego stopnia
3
5
2
4
3
2
20
12
6
2
)
(
t
a
t
a
t
a
a
t
q
(15)
(16)
(17)
Interpolacja wielomianem 5 stopnia
Interpolacja wielomianem 5 stopnia
Ruch od punktu do punktu.
(18)
5
0
5
4
0
4
3
0
3
2
0
2
0
1
0
0
t
a
t
a
t
a
t
a
t
a
a
q
4
0
5
3
0
4
2
0
3
0
2
1
0
5
4
3
2
'
t
a
t
a
t
a
t
a
a
q
Połączenie równań (15) - (17) z równaniami więzów (9) i (10) daje
sześć równań z sześcioma niewiadomymi:
3
0
5
2
0
4
0
3
2
0
20
12
6
2
''
t
a
t
a
t
a
a
q
5
5
4
4
3
3
2
2
1
0
1
k
k
k
k
k
t
a
t
a
t
a
t
a
t
a
a
q
4
5
3
4
2
3
2
1
1
5
4
3
2
'
k
k
k
k
t
a
t
a
t
a
t
a
a
q
3
5
2
4
3
2
1
20
12
6
2
''
k
k
k
t
a
t
a
t
a
a
q
Interpolacja wielomianem 5 stopnia
Interpolacja wielomianem 5 stopnia
Formułujemy równania macierzowe:
''
1
'
1
1
''
0
'
0
0
5
4
3
2
1
0
3
2
4
3
2
5
4
3
2
3
0
2
0
0
4
0
3
0
2
0
0
5
0
4
0
3
0
2
0
0
20
12
6
2
0
0
5
4
3
2
1
0
1
20
12
6
2
0
0
5
4
3
2
1
0
1
q
q
q
q
q
q
a
a
a
a
a
a
t
t
t
t
t
t
t
t
t
t
t
t
t
t
t
t
t
t
t
t
t
t
t
t
k
k
k
k
k
k
k
k
k
k
k
k
Interpolacja wielomianem 5 stopnia
Interpolacja wielomianem 5 stopnia
% Deklaracja zmienny symbolicznych
syms a0 a1
a2
a3
a4
a5
...
% wspolczynniki wielomianu interpolujacego
q0
qk
...
% zmienne przegubowe poczatkowa i koncowa
dq0
dqk
...
% predkosci brzegowe
ddq0
ddqk
% przyspieszenia brzegowe
% zadany czas poczatkowy i koncowy
syms
t0
tk
% postac wielomianu - 5 stopnia
% qi=a0+a1*t+a2*t^2+a3*t^3+a4*t^4+a5*t^5
% Formulowanie rownania do wyznaczenia wspolczynnikow wielomianow
A=[1 t0 t0^2 t0^3 t0^4 t0^5;
...
0 1 2*t0 3*t0^2 4*t0^3 5*t0^4;
...
0 0 2 6*t0 12*t0^2 20*t0^3;
...
1 tk tk^2 tk^3 tk^4 tk^5;
...
0 1 2*tk 3*tk^2 4*tk^3 5*tk^4;
...
0 0 2 6*tk 12*tk^2 20*tk^3]
a=[a0;a1;a2;a3;a4;a5]
b=[q0; dq0; ddq0; qk; dqk; ddqk]
%postaci poszukiwanych wspólczynników znajdziemy z rozwiazania rownania
aa=inv(A)*b
Interpolacja wielomianem 5 stopnia
Interpolacja wielomianem 5 stopnia
f(u)
wielomian int.
t0
tk
q0
qk
dq0
dqk
ddq0
ddqk
ai
Subsystem
Scope2
Scope1
Scope
du/dt
Derivative1
du/dt
Derivative
0
Constant5
0
Constant4
-20
Constant3
10
Constant2
1
Constant1
0
Constant
Clock
Model budujemy podobnie jak dla interpolacji wielomianem 3 stopnia
Interpolacja wielomianem 7 stopnia
Interpolacja wielomianem 7 stopnia
Jednak w rzeczywistych zastosowaniach często pożądane jest wprowadzenie
punktów pośrednich, które podzielą przedział ruchu na podprzedziały:
rozbieg (ruch przyspieszony) i wybieg (ruch opóźniony).
W takim przypadku jeśli trzeba uzyskać ciągłość do drugiego rzędu, to do
warunków trzeba dodać:
Wartości „q
r
” i „q
w
” wyznacza się jako rozwiązania zadania odwrotnego kinematyki.
Wszystkie wymienione warunki spełnia wielomian siódmego stopnia:
r
r
q
t
q
w
w
q
t
q
7
0
7
2
0
2
0
1
0
)
(
)
(
)
(
)
(
t
t
a
t
t
a
t
t
a
a
t
q
Interpolacja trajektorii
Interpolacja trajektorii
Wady interpolacji wielomianowej:
Wady interpolacji wielomianowej:
– Współczynniki wielomianu są zależne od współrzędnych punktów przejściowych,
– Stosowanie wielomianu siódmego stopnia lub wyższych stopni wiąże się z
niedogodnościami, np. często pojawiającymi się pulsacjami funkcji (źródło
niepotrzebnych ruchów).
Segmentacja liniowo-paraboliczna SLP
Segmentacja liniowo-paraboliczna SLP
Segmentacja liniowo-paraboliczna SLP.
Segmentacja liniowo-paraboliczna SLP.
• Jest stosowana gdy potrzebne jest utrzymanie stałej prędkości wzdłuż fragmentów trajektorii:
– w fazie początkowej prędkość narasta,
– w fazie środkowej prędkość jest stała,
– w fazie końcowej prędkość maleje.
Segmentacja liniowo-paraboliczna SLP
Segmentacja liniowo-paraboliczna SLP
Faza początkowa.
Faza początkowa.
• W czasie „t
0
” do „t
b
” – stosuje się do interpolacji wielomian 2
stopnia. Powoduje to liniowy wzrost prędkości.
Faza środkowa.
Faza środkowa.
• Od czasu „ t
b
” (czas łączenia) – do interpolacji stosuje się
wielomian 1 stopnia. Powoduje to że prędkość jest stała.
Faza końcowa.
Faza końcowa.
• Od czasu „t
f
- t
b
”– do interpolacji stosuje się wielomian 2
stopnia. Powoduje to że prędkość zmienia się liniowo.
Segmentacja liniowo-paraboliczna SLP
Segmentacja liniowo-paraboliczna SLP
k
b
k
k
k
b
k
b
k
k
b
t
t
t
t
t
a
t
at
t
a
q
t
t
t
t
Vt
Vt
q
q
t
t
t
a
q
t
q
2
2
2
0
2
2
2
0
0
2
0