W SPR nr 05 Interpolacja Trajektorii

background image

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

background image

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.

background image

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.

background image

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

background image

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.

background image

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)

background image

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

background image

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

background image

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

background image

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

''

background image

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

''

background image

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)

background image

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

background image

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

background image

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

background image

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

background image

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

background image

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



background image

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



background image

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)

background image

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)

background image

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

background image

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

background image

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

background image

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

background image

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

background image

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).

background image

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.

background image

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.

background image

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


Document Outline


Wyszukiwarka

Podobne podstrony:
W SPR nr 05 06 Jacobiany manipulatora
spr nr 2 IM lab! 05 14
PE Nr 05 98
PE Nr 05 97
spr nr 7-1, Studia, geodezja wyższa
Spr.nr 10, Studia, Chemia, chemia7
2005C16wyklad 13 nr 05
spr nr 2, do wydruku
spr nr 4 Efekt Halla, efekt halla, Politechnika Warszawska
Cwiczenie nr 05 Wybrane metody anlizy jakosciowej Reakcje anlityczne wybranych kationow
NaP 1993, nr 05 06
Biofizyka kontrolka do cw nr 05 Nieznany (2)
BDiA Projektowanie Semestr 6 Zajecia nr 05 Trasowanie rowow
PE Nr 05 95
ĆWICZENIE NR 05 - Oznaczanie granicy plastyczności gruntów spoistych wp, Mechanika Gruntów
Rozwiązanie zadania nr 05, egzamin na rzeczoznawcę majątkowego, pyt ministerstwo
Ćw nr 5, 05.., 03
PODKŁAD MAPOWY 1 400 PROJEKT NR 05 ZABUDOWA ISTN

więcej podobnych podstron