IK Platform — Odwrotna kinematyka w praktyce

Cel modułu

Moduł 1 wyprowadzał rozwiązanie zamknięte punkt po punkcie. Tu celem jest operacyjne zapanowanie nad zbiorem rozwiązań: zobaczyć jednocześnie, jak kombinacje znaków gałęzi $(\mathrm{shoulder}, \mathrm{elbow}, \mathrm{wrist}) \in \{\pm\}^3$ manifestują się jako osiem różnych poz ramienia osiągających tę samą pozę efektora — i przekonać się, co się dzieje, gdy robot ślizga się po trajektorii między konfiguracjami, w których część gałęzi znika.

Przypomnienie: struktura mnogości rozwiązań

shoulder ∈ {right, left}: znak $\rho = \pm\sqrt{p_x^2 + p_y^2 - d_3^2}$ . Odpowiada wyborowi, czy bark „patrzy" w kierunku celu, czy odwraca się o $\approx 180°$ .
elbow ∈ {up, down}: znak $\pm\sqrt{L^2 - K^2}$ w prawie cosinusów. Łokieć zgięty „w górę" (zwyczajowo) lub „w dół".
wrist ∈ {noflip, flip}: znak $\sin q_5$ . Odpowiada obrotowi nadgarstka o $\pi$ .

Dla niektórych par (shoulder, elbow) dyskryminant łokcia $L^2 - K^2$ bywa ujemny — cel jest poza osiągalnością tej kombinacji, a w zbiorze znajduje się mniej niż 8 rozwiązań.

Wszystkie gałęzie jednocześnie

Sterownik po lewej generuje pozę docelową (zrzut z kontrolera kopiuje bieżące

T_0^{6}

robota sterowanego ręcznie). Poniżej 8 rozwiązań pokazanych jednocześnie — każde innym kolorem, zgodnie z legendą w selektorze gałęzi. Punkt czerwony = wspólny cel TCP.

Konfiguracja przegubów

θ₁2.8°

θ₂-77.7°

θ₃6.8°

θ₄8.5°

θ₅-19.3°

θ₆172.0°

Poza efektora (T₀⁶)

Pozycja [m]

x = 0.5000

y = 0.1500

z = 0.3000

Orientacja RPY [°]

R = 0.00

P = 90.00

Y = 0.00

Poza docelowa T*

Pozycja [m]

xyz

Orientacja RPY [°]

RPY

Aktywne gałęzie

elbow ↑ · wrist □

elbow ↑ · wrist ⟲

elbow ↓ · wrist □

elbow ↓ · wrist ⟲

sh R

sh L

gałąź	q₁	q₂	q₃	q₄	q₅	q₆	‖Δp‖	ΔR
shoulder R · elbow ↑ · wrist □	2.8°	-77.7°	6.8°	-171.5°	19.3°	-8.0°	6.2e-17	0.0e+0
shoulder R · elbow ↑ · wrist ⟲	2.8°	-77.7°	6.8°	8.5°	-19.3°	172.0°	6.2e-17	0.0e+0
shoulder R · elbow ↓ · wrist □	2.8°	16.4°	-181.4°	-2.9°	75.0°	-179.2°	6.2e-17	0.0e+0
shoulder R · elbow ↓ · wrist ⟲	2.8°	16.4°	-181.4°	177.1°	-75.0°	0.8°	6.2e-17	0.0e+0
shoulder L · elbow ↑ · wrist □	-149.4°	163.6°	6.8°	149.0°	81.7°	-175.0°	1.6e-16	0.0e+0
shoulder L · elbow ↑ · wrist ⟲	-149.4°	163.6°	6.8°	-31.0°	-81.7°	5.0°	1.6e-16	0.0e+0
shoulder L · elbow ↓ · wrist □	-149.4°	-102.3°	-181.4°	68.2°	33.2°	-64.4°	1.5e-16	0.0e+0
shoulder L · elbow ↓ · wrist ⟲	-149.4°	-102.3°	-181.4°	-111.8°	-33.2°	115.6°	1.5e-16	0.0e+0

Kolumny ‖Δp‖ i ΔR to residuum walidacyjne: norma różnicy pozycji [m] oraz kąt obrotu między

R_{FK}(q)

R^*

[rad]. Dla rozwiązania analitycznego wartości są rzędu

10^{-14}

— błąd precyzji zmiennoprzecinkowej, nie metody.

Ciągłość rozwiązań przy ruchu po trajektorii

Każda gałąź definiuje ciągłą funkcję

q_{\text{branch}}: \mathrm{dom} \to Q

, ale jej dziedzina — fragment przestrzeni kartezjańskiej, w którym gałąź zachowuje sens — jest ograniczona. Na granicy dziedziny

L^2 - K^2 \to 0

, dwa rozwiązania łokcia zlewają się w jedno, a prędkość przegubowa

\dot q

dąży do nieskończoności (singularność barku/łokcia). Na trajektorii widać to jako „zatrzymanie" niektórych gałęzi i pojawienie się innych.

t = 0.00

Trajektoria liniowa w przestrzeni kartezjańskiej od (0.45, 0.15, 0.30) do (0.25, -0.35, 0.55) z orientacją RPY = (0°, 180°, 0°) (narzędzie skierowane w dół). Pokazane są cztery gałęzie wrist-noflip; obserwuj, jak znikają/pojawiają się przy przekraczaniu granic osiągalności dla danej gałęzi.

Zasada selekcji rozwiązania w praktyce

Gdy solver zwraca zbiór rozwiązań, sterownik robota musi wybraćjedno. Klasyczna heurystyka — minimalizacja przemieszczenia przegubowego względem bieżącej konfiguracji:

q^* = \arg\min_{q \in \mathcal{S}(T^*)} \; \sum_{i=1}^{6} w_i \cdot \mathrm{wrap}_\pi(q_i - q_i^{\text{curr}})^2

gdzie $\mathcal{S}(T^*)$ to zbiór 8 (lub mniej) rozwiązań, a $w_i$ — wagi per przegub (często $w_i = 1$ , ale można penalizować duże obroty bazy).

Funkcja $\mathrm{wrap}_\pi$ to zawinięcie kąta do przedziału $(-\pi, \pi]$ . Formalnie:

\mathrm{wrap}_\pi(x) \;=\; x - 2\pi\,\left\lfloor \tfrac{x + \pi}{2\pi} \right\rfloor

Dlaczego jej używamy? Kąty $q$ i $q + 2\pi$ toten sam fizyczny obrót przegubu, ale bez zawinięcia ich różnica wynosiłaby $2\pi$ i przykłamała minimalizację. Np. zwykła różnica $(170^\circ) - (-170^\circ) = 340^\circ$ , ale rzeczywisty „najkrótszy ruch" między tymi położeniami to $20^\circ$ — i dopiero po zawinięciu do $(-\pi, \pi]$ dostajemy poprawną wartość.

Kryteria dodatkowe selekcji rozwiązania:

Ograniczenia przegubowe — odrzuć rozwiązania spoza dopuszczalnego zakresu.
Kolizje z otoczeniem (checker zewnętrzny).
Odległość od singularności — maksymalizuj $|\det J|$ lub miarę Yoshikawy $w = \sqrt{\det(J J^\top)}$ .
Płynność trajektorii — kara za zmianę gałęzi między kolejnymi punktami trajektorii.

Żadne z tych kryteriów nie jest częścią właściwego IK — to post-processing, który przenosimy na poziom planowania ruchu.

Synteza

Kluczowa obserwacja tego modułu: rozwiązanie zamknięte jest dokładne, ale wielokrotne. Sterownik, który ślepo bierze pierwsze rozwiązanie z listy — lub wybiera je przez porównanie z aktualnymi $q$ bez zawijania kątów do $(-\pi, \pi]$ — wygeneruje skokowe, nieciągłe trajektorie. W modułach 3–4 zobaczymy, jak solvery iteracyjne (Jakobianowe, optymalizacyjne) z zasady generują jedną, ciągłą gałąź — kosztem dokładności i odporności na singularności.