Autonomous Vehicle Simulation - Advanced Traffic Management

Postęp jest prosty, a ruch w drugą stronę – trudny

Robot ramę można opisać jako łańcuch sztywnych połączeń połączonych z obrotowymi przegubami. Dla każdego kąta przegubu obliczanie położenia końcówki chwytnej (chwytaka) jest prostą kolekcją rotacji i translacji — kinematyka do przodu, która wymaga jednej macierzy na przegub, zawsze dokładnie rozwiązywalna, zawsze daje dokładnie jeden wynik. Kinematyka wsteczna zadaje odwrotne pytanie: mając określoną pozycję chwytaka, jakie kąty przegubu go tam doprowadzą? To jest trudny kierunek. Dla każdego mechanizmu o więcej niż dwóch połączeniach w płaszczyźnie, zazwyczaj nie ma formuły zamkniętej, mogą istnieć wiele prawidłowych rozwiązań (łokcie skierowane do góry lub do dołu), a niektóre pozycje docelowe są niemożliwe, ponieważ znajdują się poza zasięgiem ramienia.

Kinematyka do przodu: łańcuch transformacji

Każda z przegrzanych wkładających się obrot o wartość kąta oraz translację wzdłuż długości połączenia do następnego przegrzania. Łączenie tych transformacji od podstawy do chwytaka daje jego położenie w funkcji każdego kąta przegrzanej wkładarki – klasyfikowany formalizm dla tego jest parametry Den Martina-Hartnera, cztery liczby na przegrzanie (długość połączenia, skręcenie połączenia, przesunięcie połączenia, kąt przegrzanej wkładki), które ustalają jednoznaczne i składające się z siebie konwencje dla łańcucha transformacji:

T_effector = T_1(theta_1) . T_2(theta_2) . ... . T_n(theta_n)
// each T_i is a 4x4 homogeneous rotation+translation matrix
// built from that joint's Denavit-Hartenberg parameters

Cykliczna Koordynata Descent

Zamiast rozwiązywać tę łańcuchowo analitycznie, Cykliczna Koordynata Descent (CKD) robi to iteracyjnie, krok po kroku, dla każdego połączenia, co czyni ją metodą wybraną do zastosowania w ramionach z kilkoma łączami lub z ograniczeniami położenia, które uniemożliwiają uzyskanie zamkniętych rozwiązań analitycznych. Rozpoczynając od połączenia najbliższego chwiatki i pracując wstecz w kierunku podstawy, każdy cykl wykonuje następujące czynności:

dla j = ostatniego do pierwszego: v1 = (pozycja końcówki chwiatki - pozycja połączenia[j]), znormalizowana v2 = (pozycja celu - pozycja połączenia[j]), znormalizowana kąt = kąt_między(v1, v2) obróć połączenie j o `kąt` wokół jego osi, w kierunku v2 zablokuj połączenie j w granicach położenia mechanicznego przerób kinematykę z przodu dla każdego połączenia po j powtarzaj cały cykl do momentu, aż końcówka chwiatki znajdzie się w odległości odpowiadającej tolerancji od celu, lub zostanie osiągnięty maksymalny limit iteracji Każda pojedyncza rotacja połączenia jest obliczeniem oparte na zamkniętym wzorze – obróć to jedno połączenie o dokładnie taki kąt, który najlepiej ukierunkowuje obecną końcówkę chwiatki w kierunku celu, przy założeniu, że wszystko inne jest ustalone – więc pełny cykl dla n połączeń kosztuje tylko O(n) pracy, a kilka takich cykli (rzadko więcej niż 10-15) zwykle konwerguje do dokładności poniżej milimetra. To znacznie tańsze niż alternatywna metoda oparta na Jacobianie – Jacobianie, która buduje i pseudo-odwrotność macierzy o wymiarach n x 3 (lub n x 6, z uwzględnieniem orientacji) pochodnych cząstkowych w każdej iteracji – choć jest bardziej odporna na błędy numeryczne w pewnych konfiguracjach, to znacznie droższa pod względem kosztu pojedynczego kroku.

for joint j = last .. first:
  v1 = (end_effector_pos - joint[j].pos), normalized
  v2 = (target_pos        - joint[j].pos), normalized
  angle = angle_between(v1, v2)
  rotate joint j by `angle` around its axis, toward v2
  clamp joint j to its mechanical limits
  recompute forward kinematics for every joint after j
repeat the whole sweep until the effector is within tolerance of target,
or a max iteration count is hit

Połączne granice, rezerwę i niemożliwe do osiągnięcia cele

Rzeczywiste stawy nie obracają się swobodnie – bark lub łokieć posiada twarde mechaniczne ograniczenia – dlatego każde iteracja CCD przywraca proponowany kąt do dopuszczalnego zakresu stawu przed kontynuacją ruchu. To właśnie tutaj widać elegancję CCD: ponieważ traktuje każdy staw niezależnie podczas jednego przebiegu, ograniczanie zakresu jednego stawu nie generuje dodatkowych kosztów, podczas gdy solver Jakobiego musi uwzględniać ograniczenia jako warunki w systemie liniowym, co jest znacznie bardziej skomplikowane.

Ramię z większą liczbą stawów niż 3 (tylko pozycja) lub 6 (pozycja + orientacja) stopni swobody potrzebnych do określenia celu, jest kinetycznie redundantne – istnieje nieskończenie wiele konfiguracji stawów, które osiągają tę samą pozycję chwytu, co dokładnie wyjaśnia, dlaczego człowiek może swobodnie poruszać łokciem w dużym łuku, a ręka pozostaje nieruchoma na stole. CCD nie wybiera spośród tych rozwiązań z żadną preferencją (jak solver Jakobiego z celem drugorzędnym w przestrzeni zerowej, który np. może preferować konfiguracje unikające przeszkód); po prostu konwerguje do dowolnego lokalnego rozwiązania znalezionego przez jego żądny, staw po stawie przebieg, nasiewany aktualną pozycją ramienia.

Gdy cel znajduje się poza zasięgiem ramienia (dalej niż suma długości wszystkich połączeń), nie istnieje żadna konfiguracja, a CCD zamiast tego konwerguje do najbliższego punktu osiągalnego wzdłuż linii prowadzącej do celu – ramię rozciąga się w całości i wskazuje na cel w jak najbardziej możliwy sposób, co jest prawidłowym i oczekiwanym zachowaniem dla nieosiągalnej prośby.

Od IK do sekwencji z podnoszenia i opuszczania

Zadanie automatycznego podnoszenia i opuszczania składa się z łańcucha rozwiązań IK: przemieszczenie powyżej obiektu, zejście na wysokość chwytnącą, zamknięcie klepki, wyciągnięcie, przemieszczenie powyżej celu, zejście, zwolnienie, wyciągnięcie. Każde punkty docelowe to niezależne rozwiązanie IK; unikanie przeszkód jest warstwowo dodawane poprzez wstawianie pośrednich punktów docelowych, które omijają znane przeszkody zamiast modyfikować samego rozwiązania IK – właśnie dlatego ten podział – punkty docelowe plus szybkie rozwiązanie IK dla każdego punktu docelowego – stanowi standardową strukturę kontrolerów przemysłowych do podnoszenia i opuszczania na hali, a nie tylko w tej symulacji.

Często zadawane pytania

Wypróbuj na żywo

Wszystko powyżej działa bezpośrednio w Twojej przeglądarce — otwórz Robot Arm Pick & Place i zmieniaj parametry podczas działania. Nic nie jest instalowane ani przesyłane na serwer, cały model działa w jednej karcie.