Help simultus

ROBOT_6DOF_INV_KIN (226)

ROBOT_6DOF_INV_KIN (226)

ID: 226

Zastosowanie:

PLC

SIM

Opis skrótowy:

Funkcja obliczająca kinematykę odwrotną robota 6 osiowego

wejścia (31 sztuk):

nr	typ	impuls	oznaczenie	uwagi
1	R	nie	T00	Element [1,1] macierzy rotacji układu współrzędnych
2	R	nie	T01	Element [1,2] macierzy rotacji układu współrzędnych
3	R	nie	T02	Element [1,3] macierzy rotacji układu współrzędnych
4	R	nie	T10	Element [2,1] macierzy rotacji układu współrzędnych
5	R	nie	T11	Element [2,2] macierzy rotacji układu współrzędnych
6	R	nie	T12	Element [2,3] macierzy rotacji układu współrzędnych
7	R	nie	T20	Element [3,1] macierzy rotacji układu współrzędnych
8	R	nie	T21	Element [3,2] macierzy rotacji układu współrzędnych
9	R	nie	T22	Element [3,3] macierzy rotacji układu współrzędnych
10	R	nie	T03	Pozycja w osi 'x0' [mm] punktu x6 względem układu współrzędnych podstawy robota
11	R	nie	T13	Pozycja w osi 'y0' [mm] punktu x6 względem układu współrzędnych podstawy robota
12	R	nie	T23	Pozycja w osi 'z0' [mm] punktu x6 względem układu współrzędnych podstawy robota
13	R	nie	Q0	Aktualna współrzędna złączowa przegubu nr 1
14	R	nie	Q1	Aktualna współrzędna złączowa przegubu nr 2
15	R	nie	Q2	Aktualna współrzędna złączowa przegubu nr 3
16	R	nie	Q3	Aktualna współrzędna złączowa przegubu nr 4
17	R	nie	Q4	Aktualna współrzędna złączowa przegubu nr 5
18	R	nie	Q5	Aktualna współrzędna złączowa przegubu nr 6
19	R	nie	Lim00	Dolny limit współrzędnej złączowej nr 1
20	R	nie	Lim01	Górny limit współrzędnej złączowej nr 1
21	R	nie	Lim10	Dolny limit współrzędnej złączowej nr 2
22	R	nie	Lim11	Górny limit współrzędnej złączowej nr 2
23	R	nie	Lim20	Dolny limit współrzędnej złączowej nr 3
24	R	nie	Lim21	Górny limit współrzędnej złączowej nr 3
25	R	nie	Lim30	Dolny limit współrzędnej złączowej nr 4
26	R	nie	Lim31	Górny limit współrzędnej złączowej nr 4
27	R	nie	Lim40	Dolny limit współrzędnej złączowej nr 5
28	R	nie	Lim41	Górny limit współrzędnej złączowej nr 5
29	R	nie	Lim50	Dolny limit współrzędnej złączowej nr 6
30	R	nie	Lim51	Górny limit współrzędnej złączowej nr 6

wyjścia (6 sztuk):

nr	typ	impuls	oznaczenie	uwagi
1	B	nie	error	Wystąpienie błędu
2	I	nie	err_ID	Kod błędu
3	R	nie	j0	Współrzędna złączowa przegubu nr 1
4	R	nie	j1	Współrzędna złączowa przegubu nr 2
5	R	nie	j2	Współrzędna złączowa przegubu nr 3
6	R	nie	j3	Współrzędna złączowa przegubu nr 4
7	R	nie	j4	Współrzędna złączowa przegubu nr 5
8	R	nie	j5	Współrzędna złączowa przegubu nr 6

nr	typ	oznaczenie	uwagi
1	R	d1	Długość [mm] odsunięcia d1; domyślnie - 1
2	R	a1	Długość [mm] odsunięcia a1; domyślnie - 1
3	R	a2	Długość [mm] odsunięcia a2; domyślnie - 1
4	R	a3	Długość [mm] odsunięcia a3; domyślnie - 1
5	R	d4	Długość [mm] odsunięcia d4; domyślnie - 1
6	R	d6	Długość [mm] odsunięcia d6; domyślnie - 1

opis działania:

Funkcja ROBOT_6DOF_INV_KIN oblicza kinematykę odwrotną robota 6 osiowego, czyli współrzędne przegubowe robota na podstawie żądanej pozycji (x, y, z) oraz orientacji (R11...R33)

względem układu współrzędnych związanych z podstawą. Przyjęty został prawoskrętny układ współrzędnych. Na wejścia Q0...Q5

przekazywana jest informacja o aktualnych współrzędnych przegubowych, aby w kolejnym kroku czasowym robot przyjmował konfigurację najbliższą aktualnemu

(istnieje kilka rozwiązań współrzędnych przegubowych dla jednej pozycji). Na wejściach Lim ustawiane są minimalne oraz maksymalne wartości dla współrzędnych przegubowych.

Są to najczęściej ograniczenia wynikające z konstrukcji robota. Wyjście error informuje o aktualnym stanie wykonywanych obliczeń. Tabela z kodami błędów oraz

algorytm rozwiązania odwrotnego kinematyki, znajdują się poniżej.

kod błędu	uwagi
0	brak błędu
1	pozycja uzyskana, orientacja niemożliwa
2	pozycja niemożliwa do uzyskania (poza przestrzenią roboczą)
3	pozycja osobliwa
-1	błąd obliczeń (niepoprawne dane wejściowe)

Algorytm rozwiązania zadania odwrotnego - roboty PROF

Używane rejestry R i znaczniki M: brak