Moduł 09 · analiza

Dynamika odwrotna (Newton-Euler)

Od trajektorii (q, q̇, q̈) do momentów napędowych τ — robot ES5, rekurencja w przód i w tył (forward/backward sweep), tensor bezwładności.

O czym jest ten moduł

Do tej pory skupialiśmy się na kinematyce — na pytaniu „przy jakich kątach przegubów $q$ końcówka znajdzie się w zadanym miejscu". Jeśli mamy już trajektorię $q(t)$ oraz jej pochodne $\dot q(t)$ i $\ddot q(t)$ , to natychmiast zaczyna się drugie pytanie: jakie momenty napędowe $\tau_i$ muszą wytworzyć silniki, żeby ten ruch faktycznie zaszedł?

To zadanie odwrotne dynamiki: znając ruch, znajdź siły. Jest ono podstawą trzech praktyk inżynierskich:

Sterowanie typu computed-torque — silnik dostaje feedforward $\tau$ wyliczony z modelu plus poprawkę PID od pomiarów. Bez tego pre-feedforwardu robot „spóźnia się" w dynamicznych ruchach.
Symulacja — silnik fizyki gry albo emulator robota w SI sterownika potrzebuje równań ruchu.
Optymalizacja — żeby minimalizować energię cyklu transportowego (główny temat dysertacji [Gruszka 2024]), musimy umieć liczyć $\tau$ , a stąd prąd i moc.

W całym module pracujemy na nowym robocie — ES5 (EasyRobots, polski producent) — bo to robot z dysertacji wzorcowej dla tego materiału. ES5 ma osie q₂q₃q₄ równoległe, więc spełnia formę B warunku Piepera (dla porównania: Puma 560 spełnia formę A — wrist intersect). Algorytm dynamiki jest jednak niezależny od formy Piepera — działa dla dowolnego robota DH.

Mapa algorytmu Newton-Euler

Pełen pipeline w jednym widoku. Numery kroków odsyłają do paneli niżej.

Newton-Euler vs Lagrange

Dwa równoważne sformułowania dynamiki manipulatora:

Newton-Euler (rekurencyjny) — propaguje stan kinematyczny od bazy do efektora (rekurencja w przód, ang. forward sweep), potem siły od efektora do bazy (rekurencja w tył, ang. backward sweep). Postać jawna, łatwa w implementacji, koszt $O(n)$ dla n przegubów. Używamy w tym module.
Lagrange II rodzaju — daje zwartą postać macierzową $M(q)\ddot q + C(q,\dot q)\dot q + g(q) = \tau$ , użyteczną do dowodów stabilności (np. control Lapunova). Numerycznie nie szybszy, kod bardziej skomplikowany.

[Gruszka, dysertacja 2024, str. 44]: „W pracy zdecydowano się na metodę Newtona–Eulera ze względu na prostszą postać równań w postaci jawnej dla wieloczłonowego łańcucha kinematycznego."

Animacja poniżej pokazuje co dokładnie znaczy „rekurencja w przód + w tył" — zanim wejdziemy w wzory, naciśnij ▶ play i zobacz przepływ informacji wzdłuż 6 ogniw:

Animacja: jak przebiega forward + backward sweep

1 / 14

Forward · inicjalizacja

Baza: ω₀ = 0, ε₀ = 0, a₀ = -g·ẑ (sztuczka Craig'a — grawitacja zaszyta w przyspieszeniu bazy)

Co warto zauważyć: w forward sweep przesuwamy się od bazy do końcówki i wyliczamy tylko kinematykę (ω, ε, a, a_C) — bez ani jednej siły. Dopiero gdy mamy a_C dla wszystkich ogniw, wyliczamy lokalnie F_C i N_C (Newton+Euler). Backward sweep przebiega od końcówki do bazy i przekształca te F_C, N_C w momenty napędowe τ_i, uwzględniając siły propagowane od ogniwa wyższego. To rozdzielenie jest kluczem do O(n) kosztu obliczeniowego.

Dlaczego dwa przebiegi (forward + backward)?

Kluczowa obserwacja Newton-Eulera, dzięki której cały algorytm jest $O(n)$ zamiast $O(n^3)$ :

Siły bezwładności ogniwa zależą tylko od jego własnej kinematyki ( $\mathbf{F}_{Ci} = m_i\mathbf{a}_{Ci}$ , $\mathbf{N}_{Ci} = I\boldsymbol\varepsilon + \boldsymbol\omega\times I\boldsymbol\omega$ ).
Kinematyka ogniwa nie zależy od żadnych sił — wynika tylko z $(q, \dot q, \ddot q)$ i propagacji geometrycznej od bazy.

Te dwa fakty pozwalają rozdzielić problem na dwa niezależne przebiegi:

Gdyby siły i kinematyka były wzajemnie sprzężone, musielibyśmy rozwiązywać układ $6n$ równań jednocześnie — koszt $O(n^3)$ . Tu rozdzielenie daje dwa kolejne, sekwencyjne przebiegi po n ogniwach, czyli $O(n)$ . To samo sformułowanie Lagrange'a (dla porównania) produkuje gęstą macierz $M(q)\in\mathbb R^{n\times n}$ i wymaga inwersji — szybsze i prostsze w pisaniu jest NE.

Inverse vs forward dynamics — co właściwie liczymy

Dynamika robota dzieli się na dwa zadania:

Inverse dynamics — znając ruch (q, q̇, q̈), znajdź momenty napędowe τ. To, czym się zajmuje ten cały moduł. Algorytm Newton-Euler rozwiązuje to w $O(n)$ . Aplikacje: computed-torque control, symulacja energii (M10), planowanie trajektorii.
Forward dynamics — znając momenty τ, znajdź przyspieszenia q̈ (a stąd całkowaniem ruch). Wymaga rozwiązania $\ddot q = M(q)^{-1}\bigl(\tau - C(q,\dot q)\dot q - g(q)\bigr)$ . Algorytmy: Composite Rigid Body Algorithm (CRBA) lub Articulated Body Algorithm (ABA, też O(n)). Aplikacje: silniki fizyki, symulatory robotów (MuJoCo, Gazebo).

W kroku 7 (computed-torque demo) obu używamy: inverse-dynamics jako feedforward dla kontrolera, forward-dynamics dla symulatora odpowiedzi robota.

← Powiązany moduł 1 (Puma 560): tam pokazujemy kinematykę odwrotną Pumy (forma A warunku Piepera). Tu ES5 (forma B) — geometria inna, ale algorytm dynamiki Newton-Euler jest identyczny dla obu robotów.

Laboratorium

Po lewej: model 3D robota ES5 z konwencją Z-up. Po prawej: trzy panele suwaków — konfiguracja $q$ , prędkości $\dot q$ , przyspieszenia $\ddot q$ . Pod robotem: wyliczone momenty napędowe $\tau_i$ w każdym przegubie, z rozkładem na część grawitacyjną i dynamiczną. Wartości aktualizowane na żywo.

● środki masy● elipsoidy bezwładności

Konfiguracja q [°]

θ₁0.0°

θ₂0.0°

θ₃0.0°

θ₄0.0°

θ₅0.0°

θ₆0.0°

Prędkości q̇ [rad/s]

θ₁0.00 rad/s

θ₂0.00 rad/s

θ₃0.00 rad/s

θ₄0.00 rad/s

θ₅0.00 rad/s

θ₆0.00 rad/s

Przyspieszenia q̈ [rad/s²]

θ₁0.00 rad/s²

θ₂0.00 rad/s²

θ₃0.00 rad/s²

θ₄0.00 rad/s²

θ₅0.00 rad/s²

θ₆0.00 rad/s²

…wyznaczanie momentów napędowych…

…obliczanie energii…

…obliczanie rozkładu momentów…

Krok 0

Newton-Euler dla pojedynczego ciała — fundament całego algorytmu

Zanim zmierzymy się z łańcuchem 6 ogniw, sprowadźmy problem do absolutnego minimum: jedno sztywne ciało, które zna swoją kinematykę (prędkość kątową $\boldsymbol\omega$ , przyspieszenie kątowe $\boldsymbol\varepsilon$ , przyspieszenie liniowe środka masy $\mathbf{a}_C$ ) i pyta: jaką siłę i jaki moment musi na nie działać, żeby ten ruch był możliwy?

Pojedyncze sztywne ciało (cylinder) z zadanym ruchem ( $\boldsymbol\omega, \boldsymbol\varepsilon, \mathbf{a}_C$ ) i wynikającymi z niego siłą bezwładności $\mathbf{F}_C$ oraz momentem bezwładności $\mathbf{N}_C$ w środku masy. Cała dynamika Newtona-Eulera dla jednego ciała to dwa równania widoczne wyżej.

Odpowiedź to dwa równania — Newtona dla translacji (siła = masa × przyspieszenie) i Eulera dla rotacji (moment = tensor bezwładności × przyspieszenie kątowe, plus człon żyroskopowy):

\boxed{\;\mathbf{F}_C = m\,\mathbf{a}_C\;}

\boxed{\;\mathbf{N}_C = I_C\,\boldsymbol\varepsilon + \boldsymbol\omega\times(I_C\,\boldsymbol\omega)\;}

To jest cała dynamika sztywnego ciała. Jeśli umiesz policzyć $\mathbf{F}_C$ i $\mathbf{N}_C$ dla jednego ciała, wiesz dynamikę. Cały dalszy algorytm Newton-Euler to tylko dwie operacje techniczne:

Skąd wziąć $\boldsymbol\omega, \boldsymbol\varepsilon, \mathbf{a}_C$ dla każdego ogniwa? → propagujemy je z bazy do końcówki (rekurencja w przód, kroki 2–3).
Co siła $\mathbf{F}_C$ i moment $\mathbf{N}_C$ danego ogniwa znaczą dla sąsiednich przegubów? → bilans sił od końcówki do bazy (rekurencja w tył, krok 5).

Złota zasada modułu

Każdy kolejny krok można sprowadzić do pytania: „gdzie tutaj wchodzi $\mathbf{F}_C = m\,\mathbf{a}_C$ a gdzie $\mathbf{N}_C = I\boldsymbol\varepsilon + \boldsymbol\omega\times I\boldsymbol\omega$ ?". Jeśli to widzisz — czytasz Newton-Eulera. Jeśli nie widzisz — gubi cię notacja, nie fizyka. Wróć tu i przeczytaj jeszcze raz.

Legenda notacji — czytaj zanim wejdziesz w równania

Newton-Euler operuje wektorami wyrażonymi w różnych układach współrzędnych (jeden na ogniwo). Konwencja Craig'a zapisuje to za pomocą lewego górnego indeksu:

Symbol	Znaczenie
${}^i\mathbf{v}$	wektor $\mathbf{v}$ wyrażony w układzie ogniwa i (jego współrzędne to liczby w bazie { $\hat x_i, \hat y_i, \hat z_i$ })
${}^i\mathbf{p}_{i+1}$	pozycja początku układu (i+1), wyrażona w układzie (i) — czyli „gdzie patrząc z ogniwa i znajduje się następny przegub"
${}^i\mathbf{p}_{Ci}$	pozycja środka masy własnego ogniwa i, w jego własnym układzie (stała geometryczna)
${}^{i+1}R_i$	macierz rotacji z układu (i) do (i+1) — bierze wektor wyrażony w (i), zwraca jego współrzędne w (i+1)
$\boldsymbol\omega,\ \boldsymbol\varepsilon$	prędkość i przyspieszenie kątowe ogniwa
$\mathbf{v},\ \mathbf{a}$	prędkość i przyspieszenie liniowe początku układu ogniwa
$\mathbf{v}_{Ci},\ \mathbf{a}_{Ci}$	prędkość i przyspieszenie środka masy ogniwa (inne niż początku!)
$\mathbf{F}_{Ci},\ \mathbf{N}_{Ci}$	siła i moment bezwładności w środku masy (z równań Newtona i Eulera)
$\mathbf{f}_i,\ \mathbf{n}_i$	siła i moment reakcji w przegubie i (to, co ogniwo (i-1) działa na ogniwo i)
$\tau_i$	skalarny moment napędowy — to, co musi wytworzyć silnik (rzut $\mathbf{n}_i$ na oś przegubu $\hat z_i$ )

Złota zasada: zanim pomnożysz dwa wektory, sprawdź czy są wyrażone w tym samym układzie. Jeśli nie — najpierw rotacja ${}^iR_j$ , dopiero potem działanie.

Wizualna mapa symboli — rysunek 6.2 z dysertacji

Wszystkie wektory wprowadzone w legendzie powyżej ( $\boldsymbol\omega, \boldsymbol\varepsilon, \mathbf{v}, \mathbf{a}, \mathbf{v}_{Ci}, \mathbf{a}_{Ci}, \mathbf{p}_{Ci}, \mathbf{p}_i$ ) mają swoje miejsce geometryczne na pojedynczym ogniwie manipulatora. Poniższy schemat służy jako słownik wizualny — wracaj do niego za każdym razem gdy w kolejnych krokach pojawi się wektor, którego umiejscowienia nie pamiętasz:

Schemat członu kinematycznego robota przegubowego — wektory ω, ε, v, a w przegubach i, i+1 oraz w środku masy — **Rys. 6.2.** Schemat członu kinematycznego robota przegubowego — wektory prędkości ω, v i przyspieszeń **ε, a** w przegubach i, *i+1* oraz w środku masy **v_Ci, a_Ci**. Wektor *p_Ci* wskazuje środek masy ogniwa, *p_i* łączy początki układów i i *i+1*. Lokalne osie współrzędnych (x, y, z) przypisane są wg konwencji DH (Craig).Źródło: [Gruszka, dysertacja 2024], rys. 6.2.

Krok 1

Założenia i parametry inercji

Algorytm Newton-Euler dla manipulatora jako sztywnego łańcucha ciał wymaga znajomości parametrów inercji każdego ogniwa:

Masa $m_i$ [kg]
Środek masy ${}^i\mathbf{p}_{Ci}$ [m] — w lokalnym układzie ogniwa (z konwencji DH).
Tensor bezwładności $I_{Ci}$ [kg·m²] — macierz 3×3 wokół środka masy, w lokalnym układzie ogniwa.

Tensor bezwładności jest symetryczny (3 momenty główne na diagonali + 3 momenty dewiacji poza nią), eq. (6.13) dysertacji:

I_C = \begin{bmatrix} I_{xx} & -I_{xy} & -I_{xz} \\ -I_{xy} & I_{yy} & -I_{yz} \\ -I_{xz} & -I_{yz} & I_{zz} \end{bmatrix}

Przyjęte założenia upraszczające (str. 51 dysertacji):

Sztywne ciała— każde ogniwo traktujemy jako idealnie sztywne. Pomijamy deformacje sprężyste prętów oraz histerezę przekładni harmonicznych. W praktyce dla cobotów typu Franka/UR błąd z tego tytułu jest < 1° na końcówce przy maksymalnym obciążeniu.
Niezmienność czasowa parametrów — masy i tensory bezwładności są stałe. Pomijamy rozszerzalność termiczną, zużycie, zmiany położenia środka masy przy chwytaniu obciążenia (chwyt obciążenia należałoby dodać jako zmodyfikowane parametry ostatniego ogniwa).
Brak tarcia w przegubach — tarcie Coulomba i lepkościowe dodajemy później osobno (jeśli potrzeba). Przy niskich prędkościach odpowiada za ~5–15% rzeczywistego momentu — w sterowniku zwykle traktuje się je jako zaburzenie kompensowane członem całkującym PID.
Silnik jako idealny generator momentu — pomijamy dynamikę elektromagnetyczną (stała czasowa indukcyjności), bezwładność wirnika redukowaną do przegubu oraz mody własne strukturalne. Tym wszystkim zajmuje się moduł 10 (silnik DC).

Skąd wartości liczbowe? W dysertacji (str. 55, Tab. 6.2) parametry inercji ES5 wyznaczono z modelu 3D w oprogramowaniu CAD. W tej aplikacji używamy oszacowania cylindrycznego — każde ogniwo aproksymujemy jednorodnym cylindrem o znanej masie i przybliżonych wymiarach. Algorytm działa identycznie; różnica jest w wartościach liczbowych (rzędu 10–30%).

▸ Pokaż tabelę parametrów inercji cylindrycznych ES5 (te, których faktycznie używamy)

Każde ogniwo zaaproksymowane jako jednorodny cylinder o długości L i masie m, z tensorem $I_C = \mathrm{diag}\bigl(\tfrac{1}{12}m(3r^2 + L^2),\,\tfrac{1}{12}m(3r^2 + L^2),\,\tfrac{1}{2}m r^2\bigr)$ . Wartości w lokalnym układzie ogniwa (osie wg konwencji DH).

ogniwo	m [kg]	L [m]	I_xx	I_yy	I_zz
1 (podstawa)	3.7	0.15	0.011	0.011	0.018
2 (bark)	8.4	0.43	0.137	0.137	0.013
3 (łokieć)	2.3	0.40	0.034	0.034	0.003
4 (przedramię)	1.2	0.11	0.002	0.002	0.001
5 (nadgarstek)	1.2	0.10	0.001	0.001	0.001
6 (kołnierz)	0.3	0.08	0.0003	0.0003	0.0002

Te liczby siedzą w src/lib/robots/es5.ts jako ES5_INERTIA— wszystkie momenty napędowe i panele energii czytają je stamtąd.

Krok 2

Rekurencja w przód · prędkości (forward sweep, eq. 6.6, 6.8)

Rekurencja w przód propaguje stan kinematyczny od bazy (gdzie $\boldsymbol\omega_0=\boldsymbol{0}$ , $\mathbf{v}_0=\boldsymbol{0}$ ) do końcówki, ogniwo po ogniwie. Wszystkie wektory geometryczne, którymi tu operujemy ( $\boldsymbol\omega, \boldsymbol\varepsilon, \mathbf{v}, \mathbf{a}, \mathbf{p}_i$ ), są pokazane na rysunku 6.2 powyżej — wróć do niego w razie wątpliwości.

Prędkość kątowa ogniwa (i+1) jest sumą: (a) prędkości ogniwa (i) obróconej do nowego układu, oraz (b) prędkości własnego przegubu wzdłuż jego osi z:

{}^{i+1}\boldsymbol{\omega}_{i+1} = {}^{i+1}R_i\,{}^i\boldsymbol{\omega}_i + \dot\theta_{i+1}\,\hat{\mathbf{z}}_{i+1}

Kluczowa intuicja: każdy przegub dodaje swój wkład tylko wzdłuż swojej osi z (z konwencji DH). Pozostałe składowe ω dziedziczy od poprzedniego ogniwa.

Prędkość liniowa początku układu (i+1) wynika z podstawowego wzoru kinematyki sztywnego ciała: ogniwo (i) ma prędkość $\mathbf{v}_i$ oraz obraca się z $\boldsymbol\omega_i$ , więc punkt odległy o $\mathbf{p}_{i+1}$ przesuwa się z prędkością $\mathbf{v}_i + \boldsymbol\omega_i\times\mathbf{p}_{i+1}$ :

{}^{i+1}\mathbf{v}_{i+1} = {}^{i+1}R_i\,\bigl({}^i\mathbf{v}_i + {}^i\boldsymbol{\omega}_i\times {}^i\mathbf{p}_{i+1}\bigr)

Bez członu translacyjnego od własnego przegubu — bo dla przegubu obrotowego sam obrót nie przesuwa początku ogniwa.

pułapkaNajczęstszy bug w implementacjach NE — kierunek transformacji R

Macierz ${}^{i+1}R_i$ w eq. (6.6) to rotacja przeprowadzająca wektor z układu (i) do (i+1). Tymczasem typowa funkcja DH (np. linkTransform / modifiedDHTransform) zwraca $T^{i-1}_i$ — macierz, której kolumny to baza układu (i) wyrażona w (i-1). Dla rotacyjnej części:

{}^{i+1}R_i = ({}^{i}R_{i+1})^{\!\top}

W kodzie używamy mat3TmulVec3(R, v) (mnożenie przez transpozycję) zamiast mat3mulVec3(R, v). Drobna różnica notacyjna, ale błąd kosztuje godziny debugowania — bo dla pozy home (q=0) wszystkie macierze są tożsamością i bug się nie ujawnia. Pojawia się dopiero przy ruchu, jako „dziwne" momenty napędowe które „prawie" mają sens.

Test diagnostyczny: ustaw $q = (\pi/2, 0, \dots, 0)$ i $\dot q = \ddot q = \mathbf{0}$ . Z poprawnym R^T momenty grawitacyjne $\tau_2, \tau_3$ powinny skalować się proporcjonalnie do długości ramienia. Z błędnym R — dostaniesz wartości pomieszane między osiami.

Krok 3

Rekurencja w przód · przyspieszenia (forward sweep, eq. 6.7, 6.9)

Przyspieszenie kątowe wymaga uwagi na człon Coriolisa: kombinację prędkości dziedziczonej i prędkości własnego przegubu, która generuje dodatkowe przyspieszenie kątowe nawet przy zerowym $\ddot\theta$ :

{}^{i+1}\boldsymbol{\varepsilon}_{i+1} = {}^{i+1}R_i\,{}^i\boldsymbol{\varepsilon}_i + ({}^{i+1}R_i\,{}^i\boldsymbol{\omega}_i)\times\dot\theta_{i+1}\hat{\mathbf{z}}_{i+1} + \ddot\theta_{i+1}\hat{\mathbf{z}}_{i+1}

Demo Coriolisa — obracająca się platforma z masą poruszającą się radialnie

ω platformy:1.5 rad/sv_r masy:0.60 m/s

widok:

Co zauważyć: w widoku „z tarczy" masa idzie po linii prostej (radialnie), ale działa na nią siła Coriolisa — żółta strzałka prostopadła do prędkości. Wzór: $\mathbf{F}_\text{Cor} = -2m\,\boldsymbol\omega\times\mathbf{v}_r$ . W widoku inercjalnym (przełącz przyciskiem wyżej) ta sama trajektoria to spirala — żadnej dodatkowej siły nie potrzeba, ruch jest „prosty" w sensie newtonowskim, tylko widziany z obracającego się układu wymaga „dodatku". Dokładnie taki sam dodatek pojawia się w eq. (6.7) NE jako $(R\,\boldsymbol\omega_i)\times\dot\theta_{i+1}\hat z$ — i właśnie dlatego ogniwo poniżej obracającej się bazy może mieć niezerowe $\boldsymbol\varepsilon$ nawet gdy $\ddot\theta = 0$ .

Przyspieszenie liniowe początku ogniwa (i+1) ma trzy wkłady: tangencjalny (od $\boldsymbol\varepsilon_i$ ), dośrodkowy (od $\boldsymbol\omega_i\times\boldsymbol\omega_i$ ), oraz dziedziczone $\mathbf{a}_i$ :

{}^{i+1}\mathbf{a}_{i+1} = {}^{i+1}R_i\,\bigl({}^i\boldsymbol{\varepsilon}_i\times {}^i\mathbf{p}_{i+1} + {}^i\boldsymbol{\omega}_i\times({}^i\boldsymbol{\omega}_i\times {}^i\mathbf{p}_{i+1}) + {}^i\mathbf{a}_i\bigr)

Inicjalizacja ${}^0\mathbf{a}_0 = -g\,\hat{\mathbf{z}}_{\text{world}}$ (sztuczka Craig'a) sprawia, że wszystkie pochodne $\mathbf{a}_i$ automatycznie zawierają grawitację — w rekurencji w przód nie trzeba o niej pamiętać oddzielnie.

Środek masy ogniwa dziedziczy prędkość i przyspieszenie analogicznie. Prędkość C_i to prędkość początku układu plus ω razy wektor do C_i (eq. 6.11):

{}^i\mathbf{v}_{Ci} = {}^i\mathbf{v}_i + {}^i\boldsymbol{\omega}_i\times {}^i\mathbf{p}_{Ci}

Przyspieszenie C_i (eq. 6.12) — analogicznie do przyspieszenia początku:

{}^i\mathbf{a}_{Ci} = {}^i\boldsymbol{\varepsilon}_i\times {}^i\mathbf{p}_{Ci} + {}^i\boldsymbol{\omega}_i\times({}^i\boldsymbol{\omega}_i\times {}^i\mathbf{p}_{Ci}) + {}^i\mathbf{a}_i

Dla typowej trajektorii pick-and-place na ES5 (q̇ ~1 rad/s, q̈ ~5 rad/s²), człon dośrodkowy ω×(ω×p) jest rzędu 0.5–2 m/s² — porównywalny z grawitacją (9.81 m/s²) i nie do pominięcia.

Notka kodowa — ε vs alpha

W tekście używamy greckiego $\boldsymbol\varepsilon$ (klasyczne oznaczenie przyspieszenia kątowego), w kodzie TypeScript jako alpha — ponieważ identyfikatory Unicode w nazwach zmiennych pogarszają czytelność i utrudniają autouzupełnianie w edytorach. To ta sama wielkość, dwie różne nazwy.

Krok 4

Tensor bezwładności i siły bezwładności (eq. 6.13–6.15)

Tu wprowadzamy symbole, które będą fundamentem dla rekurencji w tył: $\mathbf{F}_{Ci}$ (siła bezwładności w środku masy) i $\mathbf{N}_{Ci}$ (moment bezwładności). Rysunek 6.3 poniżej pokazuje je w kontekście wszystkich pozostałych sił działających na pojedyncze ogniwo — w tym sił reakcji w przegubach ( $\mathbf{f}_i, \mathbf{n}_i$ ), które wyliczymy w kroku 5:

Schemat sił i momentów działających na i-ty człon: siła bezwładności w środku masy F_Ci, moment bezwładności M_Ci, siła grawitacji F_gi, siły i momenty reakcji w przegubach F_i, M_i, F_{i+1}, M_{i+1} — **Rys. 6.3.** Sposób przyporządkowania sił i momentów działających na i-ty człon robota. **F_Ci, M_Ci** — siła i moment bezwładności w środku masy (wprowadzamy w tym kroku). **F_gi** — siła grawitacji. **F_i, M_i** — siła i moment reakcji w przegubie i (od ogniwa *i-1*). **F_{i+1}, M_{i+1}** — analogiczne wielkości od ogniwa *i+1*. Bilans tych sił daje rekurencję w tył (krok 5) — znając obciążenia zewnętrzne (zerowe za końcówką), wyliczamy siłę i moment w każdym przegubie idąc od końca do bazy.Źródło: [Gruszka, dysertacja 2024], rys. 6.3.

Mając przyspieszenie środka masy ${}^i\mathbf{a}_{Ci}$ , siła bezwładności (d'Alemberta) to po prostu II zasada Newtona:

{}^i\mathbf{F}_{Ci} = m_i\,{}^i\mathbf{a}_{Ci}

Ponieważ $\mathbf{a}_{Ci}$ dziedziczy grawitację z forward sweep, $\mathbf{F}_C$ w naszej konwencji zawiera zarówno siłę bezwładności (z dynamiki), jak i pozorną siłę grawitacji.

Moment bezwładności ogniwa składa się z dwóch części:

{}^i\mathbf{N}_{Ci} = I_{Ci}\,{}^i\boldsymbol{\varepsilon}_i + {}^i\boldsymbol{\omega}_i\times(I_{Ci}\,{}^i\boldsymbol{\omega}_i)

Pierwszy człon to po prostu „F=ma w postaci rotacyjnej" — $I\,\boldsymbol\varepsilon$ : moment potrzebny by nadać ciału przyspieszenie kątowe $\boldsymbol\varepsilon$ . Drugi człon $\boldsymbol\omega\times(I\,\boldsymbol\omega)$ to moment żyroskopowy — pojawia się gdy ciało już się obraca z prędkością $\boldsymbol\omega$ wokół osi, która nie pokrywa się z osią główną bezwładności tensora $I_C$ .

Intuicja — koło rowerowe trzymane w rękach

Klasyczna demonstracja na zajęciach z mechaniki: trzymasz oś szybko rozkręconego koła rowerowego za dwa wystające końce. Próbujesz je pochylić w prawo (chcesz nadać $\boldsymbol\varepsilon$ poziomy, w kierunku jazdy). Spodziewałbyś się, że poczujesz opór wzdłuż osi pochylania — tymczasem koło „skręca w górę" lub „w dół": twoje ręce czują moment prostopadły do kierunku, w którym próbujesz je pochylić.

To dokładnie człon $\boldsymbol\omega\times(I\,\boldsymbol\omega)$ . Wektor pędu kątowego $\mathbf{L} = I\,\boldsymbol\omega$ leży wzdłuż osi koła. Gdy nakładasz $\boldsymbol\omega_\text{pochylenia}$ poziomy, iloczyn wektorowy daje moment w trzeciej osi — i to właśnie ten moment czują twoje ręce. Zjawisko nazywa się precesją żyroskopową.

W manipulatorze pojawia się analogicznie: szybko obracające się ogniwo (np. nadgarstek wirujący z dużą $\boldsymbol\omega$ ) gdy reszta ramienia próbuje obrócić jego oś, „odpowiada" momentem prostopadłym, który musi zostać zrównoważony przez napęd w przegubie sąsiednim. Stąd dodatkowy człon w równaniu — fizycznie istnieje, a jego pominięcie dałoby błędne $\tau_i$ przy dużych prędkościach.

Specjalny przypadek: gdy $\boldsymbol\omega$ jest wektorem własnym $I_C$ (czyli leży wzdłuż jednej z osi głównych), wtedy $I\boldsymbol\omega = \lambda\boldsymbol\omega$ i $\boldsymbol\omega\times\lambda\boldsymbol\omega = 0$ — człon żyroskopowy znika. Innymi słowy: jeśli kierunek pędu kątowego zgadza się z kierunkiem prędkości kątowej, nie ma precesji.

Krok 5

Rekurencja w tył · siły reakcji w przegubach (backward sweep, Craig, 6.49)

Po wyliczeniu sił bezwładności w środkach mas wszystkich ogniw, lecimy w drugą stronę — od końcówki (gdzie nie ma obciążenia zewnętrznego) do bazy. Wracamy do rysunku 6.3 z kroku 4 — tym razem ze szczegółowym bilansem czterech składowych w przegubie.

Każde ogniwo balansuje: (a) siły reakcji od ogniwa wyższego, (b) własne siły bezwładności, (c) siły grawitacji (już zaszyte w $\mathbf{F}_C$ ):

{}^i\mathbf{f}_i = {}^iR_{i+1}\,{}^{i+1}\mathbf{f}_{i+1} + {}^i\mathbf{F}_{Ci}

Inicjalizacja: ${}^{n+1}\mathbf{f}_{n+1} = \boldsymbol{0}$ — brak obciążenia zewnętrznego za końcówką (jeśli robot trzyma jakiś przedmiot, dodajemy tu jego ciężar).

Moment w przegubie i to suma kilku składowych:

{}^i\mathbf{n}_i = {}^i\mathbf{N}_{Ci} + {}^iR_{i+1}\,{}^{i+1}\mathbf{n}_{i+1} + {}^i\mathbf{p}_{Ci}\times {}^i\mathbf{F}_{Ci} + {}^i\mathbf{p}_{i+1}\times({}^iR_{i+1}\,{}^{i+1}\mathbf{f}_{i+1})

Cztery człony: własny moment bezwładności, propagowany moment od ogniwa wyższego, moment od własnej siły bezwładności na ramieniu od początku układu do środka masy, moment od siły reakcji ogniwa wyższego na ramieniu od początku układu do następnego przegubu.

Moment napędowy w przegubie i — czyli to, czego silnik musi wytworzyć — to składowa $\mathbf{n}_i$ wzdłuż osi przegubu (czyli oś z układu i):

\boxed{\;\tau_i = {}^i\mathbf{n}_i \cdot \hat{\mathbf{z}}_i = (\,{}^i\mathbf{n}_i)_z\;}

Pozostałe składowe momentu są równoważone przez konstrukcję mechaniczną przegubu (łożyska). Tylko $\tau_i$ wymaga aktywnej kompensacji przez silnik.

Krok 6

Eksperyment: trajektoria pick-and-place

Dla zadanej trajektorii $(q(t), \dot q(t), \ddot q(t))$ wyliczamy $\tau(t)$ dla każdego z 6 napędów ES5 — w każdym kroku czasowym uruchamiamy pełny algorytm Newton-Euler. Wykres pokazuje 6 niezależnych linii (po jednej na napęd), oś pozioma to znormalizowany czas trajektorii τ ∈ [0, 1], oś pionowa to moment w Nm. Wybierz scenariusz w rozwijanej liście, dostosuj masę obciążenia w chwytaku i obserwuj jak zmienia się obciążenie napędów.

scenariusz:masa chwytaka [kg]:0.0

Zauważ: τ₂ (zielony) zwykle dominuje — to przegub trzymający masę całego ramienia plus przedramienia plus narzędzia. Przy ruchu tylko q₂ wszystkie pozostałe momenty są małe (utrzymują geometrię nadgarstka). Masa chwytaka liniowo skaluje moment grawitacyjny.

Co warto zauważyć: τ₂ (drugi przegub, dźwigający ramię w polu grawitacji) zwykle dominuje — zarówno w statyce, jak i dynamice. Dodanie 1 kg w chwytaku liniowo skaluje moment grawitacyjny we wszystkich napędach (efekt mnożnika ramienia: ciężar 1 kg na końcu robota o zasięgu 0.8 m daje +8 Nm).

Krok 7

Computed-torque control — domykamy obietnicę ze wstępu

Na początku modułu napisałem: „silnik dostaje feedforward τ wyliczony z modelu plus poprawkę PID". Czas pokazać że to działa. Symuluję 2R-planarny manipulator (forward dynamics rozwiązana analitycznie z równania Lagrange'a) wykonujący tę samą trajektorię dwoma kontrolerami: czystym PID i PID z dodanym feedforward'em $\tau_{ff} = \mathrm{NE}(q_d, \dot q_d, \ddot q_d)$ .

Po co to jest?

Aplikacja kanoniczna NE — computed-torque control. Wysyłamy do silnika dwa momenty: (1) feedforward wyliczony z modelu dynamiki NE jako $\tau_{ff} = \mathrm{NE}(q_d, \dot q_d, \ddot q_d)$ , plus (2) PD-poprawkę od pomiarów $\tau_{fb} = K_p (q_d - q) + K_d (\dot q_d - \dot q)$ . Idea: jeśli model dynamiki jest dokładny, $\tau_{ff}$ wystarczy do śledzenia idealnej trajektorii — a PD jest tylko żeby skompensować drobne błędy modelu. Porównaj poniżej: PID-only musi mieć duże Kp by „dogonić" trajektorię, PID+feedforward śledzi z minimalnym błędem nawet przy małych wzmocnieniach.

2R-planarny: PID-only vs PID + NE-feedforward

t = 0.000 s

Kp:50Kd:10.0t:

PID-only

PID + NE-feedforward (computed-torque)

Błąd śledzenia $|e(t)| = \sqrt{(q_d - q)^2}$ [rad]

PID-only

max |e| przez całą trajektorię:

489.6 mrad ≈ 28.05°

PID + NE-feedforward

max |e| przez całą trajektorię:

2.7 mrad ≈ 0.15°

Eksperymenty: przy domyślnych Kp=50, Kd=10 błąd PID-only jest ~10× większy niż PID+feedforward. Zwiększ Kp do 300 — PID-only zbliży się do feedforward, ale w realnym systemie tak duże Kp wzmocniłoby szum pomiarowy i ryzyko niestabilności. Feedforward NE osiąga to samo bez wysokich wzmocnień — to jest powód, dla którego ten moduł istnieje.

▸ Pokaż wzory kontrolerów

PID-only (faktycznie tu PD, bez I dla prostoty):

\tau = K_p(q_d - q) + K_d(\dot q_d - \dot q)

Computed-torque (PD + model-based feedforward):

\tau = \underbrace{M(q_d)\ddot q_d + C(q_d,\dot q_d)\dot q_d + g(q_d)}_{\tau_{ff} = \mathrm{NE}(q_d,\dot q_d,\ddot q_d)} + K_p(q_d - q) + K_d(\dot q_d - \dot q)

Gdy model dokładnie odzwierciedla rzeczywistą dynamikę i stan początkowy jest na trajektorii, sama $\tau_{ff}$ wystarczy — część PD pracuje tylko z zerem. W praktyce model ma niedoskonałości (tarcie, sprężystość), więc PD koryguje błędy.

Wniosek dydaktyczny: NE nie jest tylko teoretycznym algorytmem — to generator feedforward'u dla każdego sterownika trajektorii. Bez modelu dynamiki trzeba kompensować błąd agresywnymi wzmocnieniami PID (co wzmacnia szum pomiarowy i zbliża układ do granicy stabilności). Z modelem regulator pracuje przy znacznie mniejszych błędach śledzenia i można go bezpiecznie stroić blisko optimum stabilności.

Spróbuj sam — trzy eksperymenty w playgroundzie

Każdy eksperyment poniżej to konkretne polecenie do wykonania w sekcji „Laboratorium" wyżej. Rozwiń, ustaw odpowiednie wartości i porównaj swoje obserwacje z opisem.

▸spróbuj samEksperyment 1 — jak rośnie wkład dynamiki względem statyki

Ustaw w playgroundzie: Wróć do playgroundu. Ustaw q w pozycji „ramię w bok poziomo" (q₂ ≈ 90°, reszta 0). Suwakiem skala q̇ pod wykresem słupkowym przesuwaj od 0× do 5×.

Co powinieneś zaobserwować: Przy 0× widzisz samą statykę — czerwone słupki τ_grawit dominują, τ₂ największy (ramię w polu grawitacji). Powyżej 2× niebieskie słupki (τ_dynam) rosną kwadratowo i przy 3–4× przewyższają statykę dla τ₁ i τ₃. τ₂ pozostaje dominowany przez grawitację, bo dla typowej konfiguracji ramienia siła grawitacji jest stała, a wkład dynamiki zależy od q̇².

Dlaczego to ważne:w przemysłowych sterownikach często stosuje się tzw. gravity compensation — feedforward kompensujący tylko grawitację. Działa dobrze dla ruchów wolnych (q̇ < 1 rad/s), zawodzi dla agresywnych (q̇ > 3 rad/s) gdzie potrzebny jest pełen model Newton-Eulera, jak w kroku 7 (computed-torque).

▸spróbuj samEksperyment 2 — efekt żyroskopowy bez przegubowych przyspieszeń

Ustaw w playgroundzie: W playgroundzie ustaw q = home, wszystkie q̈ na 0. Podnieś tylko q̇₁ do 5 rad/s. Obserwuj τ₃, τ₄, τ₅ w TorqueDisplay. Zwiększaj stopniowo q̇₂ od 0 do 3 rad/s.

Co powinieneś zaobserwować: Mimo że żaden przegub nie ma przyspieszenia, momenty napędowe τ₃, τ₄, τ₅ rosną nieproporcjonalnie do q̇₂. To człon żyroskopowy ω×(I·ω) z eq. (6.15) — górne ogniwa „czują" obrót podstawy (q̇₁) skomponowany z obrotem własnym (q̇₂) jako moment prostopadły do obu osi.

Dlaczego to ważne: przy projektowaniu nadgarstków cobotów (np. końcówek z szybkimi obrotami narzędzia) efekty żyroskopowe są dominującym źródłem nieliniowości. Pomijanie ich w sterowniku daje błąd śledzenia rosnący kwadratowo z q̇.

▸spróbuj samEksperyment 3 — wyłącz grawitację (matematycznie)

Ustaw w playgroundzie: Nie da się wyłączyć grawitacji w UI, ale można symulować jej brak: ustaw q tak, że robot leży poziomo na boku (q₂ = 0, q₃ = 0 — ramię „płasko"). Ustaw q̇₁ = 1 rad/s, q̈₁ = 0. Czytaj τ_dynam (niebieska kolumna w TorqueDisplay).

Co powinieneś zaobserwować: τ_dynam pokazuje czysty wkład bezwładności+Coriolisa, odseparowany od grawitacji. Powinien być rzędu kilku Nm — duża część to człony odśrodkowe ω×(ω×p), proporcjonalne do \dot q_1^2. Sprawdź: zwiększ q̇₁ do 2 rad/s i zobacz że τ_dynam wzrasta ~4× (kwadrat od skalarnego skalowania prędkości).

Dlaczego to ważne: w robotach kosmicznych (np. ramię ISS) brak grawitacji oznacza że cała dynamika to człony bezwładnościowe — projektowanie sterowników wymaga innego strojenia niż dla robotów naziemnych.

Od τ(t) do doboru napędu — 4 metryki konstrukcyjne

Cała powyższa maszyneria Newton-Eulera produkuje jedną kluczową dla inżyniera rzecz: przebieg czasowy momentów napędowych $\tau_i(t)$ dla zadanej trajektorii. To jest dokładnie specyfikacja wymagań, którą wpisuje się do zapytania ofertowego u producenta silników (Maxon, Kollmorgen, Allied Motion) i przekładni (Harmonic Drive, Nabtesco). Z jednego przebiegu $\tau(t)$ wyciągamy cztery niezależne liczby — i każda z nich ogranicza inną własność docelowego napędu.

Cztery metryki konstrukcyjne — co każda ogranicza

τ_peak

\tau^{\max}_i = \max_t |\tau_i(t)|

Moment szczytowy — chwilowy maksymalny moment, który silnik musi wytworzyć (typowo trwa milisekundy: podczas startu, hamowania, zderzenia z constraintem). Ogranicza go krzywa T-N silnika (saturacja magnetyczna) oraz wytrzymałość mechaniczna przekładni (peak torque rating).

τ_rms

\tau^{\text{rms}}_i = \sqrt{\tfrac{1}{T}\!\int_0^T\!\tau_i^2(t)\,dt}

Moment skuteczny (RMS) — średnia kwadratowa po całym cyklu. Dlaczego kwadratowa? Bo straty cieplne silnika to $P_{\text{cieplna}} = I^2 R$ , a prąd jest proporcjonalny do momentu ( $\tau = K_t I$ ). RMS dyktuje moment ciągły z karty katalogowej i temperaturę uzwojeń — to ona decyduje czy silnik przeżyje cykl pracy bez przegrzania.

q̇_peak

\dot q^{\max}_i = \max_t |\dot q_i(t)|

Prędkość kątowa maksymalna przegubu. Po przeskalowaniu przez przełożenie przekładni daje wymaganą prędkość obrotową silnika $\omega_{\text{silnika}} = \dot q \cdot n$ . Ogranicza ją no-load speed silnika i max input speed przekładni harmonicznej (typowo 4000–6000 rpm).

P_peak

P^{\max}_i = \max_t |\tau_i(t)\,\dot q_i(t)|

Moc szczytowa wymagana w danym momencie. Dyktuje wybór drivera (sterownika silnika) i zasilacza (PSU). Często pojawia się w innym momencie cyklu niż τ_peak — bo tam gdzie τ jest max, q̇ często jest mała (i odwrotnie). Dla zasilania bateryjnego dodatkowo liczy się energia cyklu $E = \int_0^T |P(t)|\,dt$ .

Poniższy interaktywny panel czyta tę samą trajektorię którą widzisz na wykresie τ(t) wyżej, ale agreguje ją do tych czterech liczb per napęd. Zmieniaj scenariusz i payload i obserwuj, jak rosną wymagania konstrukcyjne. Wiersz z żółtym tłem wskazuje napęd krytyczny (najwyższe wymagane τ_peak) — to on dyktuje wybór całej rodziny silników.

…obliczanie metryk doboru napędów…

Następny krok: mając te cztery metryki, przechodzimy do modułu 11 (Dobór napędów), w którym budujemy pełny pipeline: krzywa T-N silnika z naniesionym punktem pracy, sprawdzenie bezwładności zredukowanej (wpływ na pasmo regulatora), cross-reference do realnych modeli z katalogów Maxon / Kollmorgen / Harmonic Drive, iteracja projektowa „za duży silnik → za drogo, za mały → niezawodność". To naturalne zwieńczenie wiedzy z modułów M9+M10 — tu liczyliśmy momenty, w M10 energię, w M11 składamy to w decyzję zakupową.

Wzorzec ręczny — 2R planarny manipulator

Zanim zaufamy pełnemu algorytmowi 6-DOF dla ES5, przeliczmy całość „na piechotę" dla najmniejszego nietrywialnego przypadku: dwa ogniwa, obroty w jednej płaszczyźnie, oś z prostopadła do tej płaszczyzny. To klasyczny model z każdego podręcznika robotyki (Spong, Murray–Li–Sastry, Siciliano) i daje się rozpisać ręcznie w ~15 wzorach.

2R planarny manipulator w polu grawitacji. Oś z układu globalnego jest prostopadła do płaszczyzny rysunku (z czytelnika ku obserwatorowi). Oba przeguby obrotowe — q₁ wokół osi z bazy, q₂ wokół osi z na końcu ogniwa 1. Środki mas C₁, C₂ w połowie długości każdego pręta.

Parametry modelu

Ogniwo i ∈ {1, 2}: jednorodny pręt o masie $m_i$ i długości $L_i$ .
Środek masy $\mathbf{p}_{Ci} = \tfrac{L_i}{2}\,\hat{x}_i$ (połowa długości w lokalnym układzie ogniwa).
Tensor bezwładności wokół osi z przez środek masy: $I_C = \tfrac{1}{12}\,m_i\,L_i^2$ (pozostałe składowe nie wpłyną na ruch w płaszczyźnie xy).
Konfiguracja: $q_1, q_2$ — kąty obrotu obu przegubów. Prędkości i przyspieszenia: $\dot q_1, \dot q_2, \ddot q_1, \ddot q_2$ .
Grawitacja: $\mathbf{g} = -g\,\hat{y}$ z $g = 9{,}81\,\mathrm{m/s^2}$ (kierunek -y).

Uproszczenia notacyjne: wszystkie wektory wyrażam w bazie globalnej (jedna płaszczyzna xy → 2 składowe wystarczą; trzecia, z, będzie zerowa dla translacji i pełna dla rotacji). Pomijam lewe górne indeksy — w 2D z jednym wspólnym układem nie są potrzebne.

Oznaczenia skrótowe: $c_i \equiv \cos q_i$ , $s_i \equiv \sin q_i$ , $c_{12} \equiv \cos(q_1+q_2)$ , $s_{12} \equiv \sin(q_1+q_2)$ .

Rekurencja w przód · kinematyka

Krok po kroku propaguję od bazy do końcówki. Każde równanie poniżej jest specjalizacją $(6.6)–(6.9)$ do 2D, w której wszystkie ω i ε są kolinearne z osią z, więc iloczyny wektorowe upraszczają się do skalarnego mnożenia z obrotem 90°.

Ogniwo 1 (od bazy)

Z bazy: $\boldsymbol\omega_0 = \mathbf{0}, \boldsymbol\varepsilon_0 = \mathbf{0}, \mathbf{a}_0 = \mathbf{0}$ . Tylko obrót q₁ wokół osi z:

\boldsymbol\omega_1 = \dot q_1\,\hat z, \qquad \boldsymbol\varepsilon_1 = \ddot q_1\,\hat z

Przyspieszenie środka masy C₁ (w odległości L₁/2 od osi q₁ wzdłuż ogniwa, czyli wektor $\mathbf{r}_{C1} = \tfrac{L_1}{2}(c_1, s_1, 0)$ ):

\mathbf{a}_{C1} = \boldsymbol\varepsilon_1\times\mathbf{r}_{C1} + \boldsymbol\omega_1\times(\boldsymbol\omega_1\times\mathbf{r}_{C1})

Po rozpisaniu (ω₁ wzdłuż ẑ, mnożenie wektorowe obraca w płaszczyźnie xy o 90°):

\mathbf{a}_{C1} = \tfrac{L_1}{2}\bigl(-\ddot q_1\,s_1 - \dot q_1^2\,c_1,\; \ddot q_1\,c_1 - \dot q_1^2\,s_1,\; 0\bigr)

Pierwszy człon (z $\ddot q_1$ ) — przyspieszenie tangencjalne. Drugi (z $\dot q_1^2$ ) — przyspieszenie dośrodkowe, zawsze skierowane do osi obrotu. Klasyczna dekompozycja ruchu po okręgu.

Ogniwo 2 (po propagacji przez przegub q₂)

Pozycja przegubu q₂ względem bazy: $\mathbf{p}_2 = L_1(c_1, s_1, 0)$ . Najpierw przyspieszenie tego pinu (z wzoru identycznego do $\mathbf{a}_{C1}$ tylko z pełnym L₁ zamiast L₁/2):

\mathbf{a}_2 = L_1\bigl(-\ddot q_1\,s_1 - \dot q_1^2\,c_1,\; \ddot q_1\,c_1 - \dot q_1^2\,s_1,\; 0\bigr)

Prędkość i przyspieszenie kątowe ogniwa 2 — wszystkie wzdłuż ẑ więc po prostu się dodają (iloczyn wektorowy $\boldsymbol\omega_1\times\dot q_2\hat z = 0$ ):

\boldsymbol\omega_2 = (\dot q_1 + \dot q_2)\,\hat z, \qquad \boldsymbol\varepsilon_2 = (\ddot q_1 + \ddot q_2)\,\hat z

Wektor od pinu q₂ do C₂: $\mathbf{r}_{C2,\text{loc}} = \tfrac{L_2}{2}(c_{12}, s_{12}, 0)$ . Przyspieszenie C₂ = przyspieszenie pinu q₂ + tangencjalne + dośrodkowe:

\mathbf{a}_{C2} = \mathbf{a}_2 + \boldsymbol\varepsilon_2\times\mathbf{r}_{C2,\text{loc}} + \boldsymbol\omega_2\times(\boldsymbol\omega_2\times\mathbf{r}_{C2,\text{loc}})

\mathbf{a}_{C2} = \mathbf{a}_2 + \tfrac{L_2}{2}\bigl(-(\ddot q_1+\ddot q_2)s_{12} - (\dot q_1+\dot q_2)^2 c_{12},\;\; (\ddot q_1+\ddot q_2)c_{12} - (\dot q_1+\dot q_2)^2 s_{12},\; 0\bigr)

Siły i momenty bezwładności (Newton + Euler)

Z równania Newtona (dla każdego ogniwa osobno):

\mathbf{F}_{C1} = m_1\,\mathbf{a}_{C1}, \qquad \mathbf{F}_{C2} = m_2\,\mathbf{a}_{C2}

Z równania Eulera — wszystkie ω i ε są wzdłuż ẑ, więc $\boldsymbol\omega\times(I\boldsymbol\omega) = 0$ (równoległe wektory). Zostaje tylko człon $I\boldsymbol\varepsilon$ :

\mathbf{N}_{C1} = I_{C1}\,\ddot q_1\,\hat z = \tfrac{1}{12}m_1 L_1^2\,\ddot q_1\,\hat z

\mathbf{N}_{C2} = I_{C2}\,(\ddot q_1 + \ddot q_2)\,\hat z = \tfrac{1}{12}m_2 L_2^2\,(\ddot q_1 + \ddot q_2)\,\hat z

To dokładnie tu znikł „moment żyroskopowy" z kroku 4 ogólnego algorytmu — dla obrotu wokół osi własnej (głównej) tensora bezwładności człon $\boldsymbol\omega\times I\boldsymbol\omega$ = 0. Dlatego przykład 2R nie pokazuje żyroskopu (potrzeba do tego 3D z osiami nieortogonalnymi).

Rekurencja w tył · siły reakcji i momenty napędowe

Zaczynamy od końcówki (gdzie nie ma obciążenia) i bilansujemy siły ogniwo po ogniwie. Grawitację uwzględniamy jawnie jako siłę $\mathbf{F}_{gi} = -m_i\,g\,\hat y$ w środku masy każdego ogniwa. W głównym algorytmie jest ona zaszyta w $\mathbf{a}_C$ przez sztuczkę Craig'a — tutaj rozdzielamy dla czytelności.

Ogniwo 2 (od końcówki)

Inicjalizacja za końcówką: $\mathbf{f}_3 = \mathbf{0},\ \mathbf{n}_3 = \mathbf{0}$ .

Bilans sił:

\mathbf{f}_2 = \mathbf{f}_3 + \mathbf{F}_{C2} - \mathbf{F}_{g2} = m_2\mathbf{a}_{C2} + m_2 g\,\hat y

Bilans momentów wokół pinu q₂ (czyli początku ogniwa 2): moment bezwładności + moment od $\mathbf{F}_{C2}$ na ramieniu $\mathbf{r}_{C2,\text{loc}}$ + moment od grawitacji:

\mathbf{n}_2 = \mathbf{N}_{C2} + \mathbf{r}_{C2,\text{loc}}\times(m_2\mathbf{a}_{C2}) + \mathbf{r}_{C2,\text{loc}}\times(m_2 g\,\hat y)

Moment napędowy w przegubie q₂ to składowa z wektora momentu (oś przegubu = ẑ):

\boxed{\;\tau_2 = (\mathbf{n}_2)_z\;}

Ogniwo 1

Bilans sił (dodajemy $\mathbf{f}_2$ propagowane od ogniwa 2):

\mathbf{f}_1 = \mathbf{f}_2 + \mathbf{F}_{C1} - \mathbf{F}_{g1} = \mathbf{f}_2 + m_1\mathbf{a}_{C1} + m_1 g\,\hat y

Bilans momentów wokół pinu q₁ — cztery składowe (zgodne z Craig 6.50):

\mathbf{n}_1 = \mathbf{N}_{C1} + \mathbf{n}_2 + \mathbf{r}_{C1}\times m_1\mathbf{a}_{C1} + \mathbf{p}_2\times\mathbf{f}_2 + \mathbf{r}_{C1}\times m_1 g\,\hat y

Człon $\mathbf{p}_2\times\mathbf{f}_2$ to moment, jakim ogniwo 2 „ciągnie" za pin q₂ — propaguje się na ramieniu długości $L_1$ .

\boxed{\;\tau_1 = (\mathbf{n}_1)_z\;}

Sanity check — statyka z gradientu energii potencjalnej

Dla $\dot q_1 = \dot q_2 = 0$ i $\ddot q_1 = \ddot q_2 = 0$ wszystkie $\mathbf{a}_C$ redukują się do zera, więc $\mathbf{F}_{Ci} = 0$ i $\mathbf{N}_{Ci} = 0$ . Zostają tylko grawitacyjne wkłady w $\mathbf{n}_i$ . Można je wyliczyć drugim, niezależnym sposobem — z gradientu energii potencjalnej:

V(q) = m_1 g\,h_{C1}(q) + m_2 g\,h_{C2}(q)

gdzie wysokości środków mas (z y w górę):

h_{C1} = \tfrac{L_1}{2}\sin q_1, \qquad h_{C2} = L_1\sin q_1 + \tfrac{L_2}{2}\sin(q_1+q_2)

Gradient:

\tau_1^{\text{grav}} = \frac{\partial V}{\partial q_1} = \bigl(\tfrac{m_1}{2} + m_2\bigr)g L_1\cos q_1 + \tfrac{m_2}{2}g L_2\cos(q_1+q_2)

\tau_2^{\text{grav}} = \frac{\partial V}{\partial q_2} = \tfrac{m_2}{2}g L_2\cos(q_1+q_2)

Dwa niezależne wyprowadzenia — wynik musi się zgadzać

Powyższe $\tau_i^{\text{grav}}$ z gradientu energii muszą być identyczne z $(\mathbf{n}_i)_z$ z Newton-Eulera podstawionym $\dot q = \ddot q = \mathbf{0}$ . Jeśli się zgadzają — masz pewność, że algorytm działa. Jeśli nie zgadzają się — szukaj błędu w mnożeniu macierzą rotacji albo w znaku iloczynu wektorowego. To najlepszy test jednostkowy dla implementacji NE.

Dla pełnej dynamiki (q̇, q̈ ≠ 0) podobny test można zrobić przez bilans mocy: $\sum_i \tau_i \dot q_i = \dot T + \dot V$ gdzie T to energia kinetyczna. Implementujemy go w panelu „Sanity check" pod playgroundem (planowane w kolejnej iteracji modułu).

Wzorzec liczbowy — pełny rachunek dla jednej trajektorii (6-DOF ES5)

Mając już intuicję z 2R-planarnego (powyżej), spójrzmy na pełny 6-DOF na konkretnym scenariuszu: q = (0°, 45°, 45°, 0°, 90°, 0°), obrót pierwszego przegubu z prędkością q̇₁ = 0.5 rad/s, zerowe przyspieszenia. Wszystkie wartości pośrednie wyliczone analitycznie (algorytm src/lib/dynamics/newton-euler.ts) i tu wypisane — można je przepisać do własnego notebooka i sprawdzić implementację linijka po linijce.

Scenariusz testowy

q [°]:(0, 45, 45, 0, 90, 0)q̇ [rad/s]:(0.50, 0.00, 0.00, 0.00, 0.00, 0.00)q̈ [rad/s²]:(0.00, 0.00, 0.00, 0.00, 0.00, 0.00)Inicjalizacja:⁰a₀ = (0, 0, +9.81) m/s² (konwencja Craig: -g·ẑ z g>0 dla z_world wzwyż)

Rekurencja w przód · ogniwo 1

ω₁(0.0000, 0.0000, 0.5000)rad/s

a₁(0.0000, 0.0000, 9.8100)m/s²

aᶜ₁(0.0000, 0.0020, 9.8100)m/s²

Fᶜ₁(0.0000, 0.0079, 38.5631)N

ω₁ = (0, 0, q̇₁) bo pierwszy przegub obraca się wokół osi z bazy. a₁ ≈ (0, 0, +g) — grawitacja w lokalnym układzie 1 (oś z₁ pokrywa się z osią z bazy).

Rekurencja w przód · ogniwo 2

ω₂(0.3536, 0.3536, 0.0000)rad/s

a₂(6.9367, 6.9367, 0.0000)m/s²

aᶜ₂(6.9108, 6.9626, -0.0310)m/s²

Fᶜ₂(72.1630, 72.7034, -0.3237)N

ω₂ ma niezerowe x i y bo q̇₁ z układu 1 zostaje obrócone przez R^2_1 (α₁ = π/2). Stąd efekty Coriolisa w propagacji a₂.

Rekurencja w przód · ogniwo 3

ω₃(0.5000, 0.0000, 0.0000)rad/s

a₃(9.8100, 0.0751, 0.0000)m/s²

aᶜ₃(9.8100, 0.0751, -0.0045)m/s²

Fᶜ₃(27.9193, 0.2138, -0.0128)N

Po Rz(q₃ = 45°) bez α₃ skręcenia, kierunek propagacji się zmienia zgodnie z kątem stawu. Fᶜ₃ zdominowane przez grawitację (oś x).

Rekurencja w tył · momenty napędowe (rzut N_i na oś z_i)

τ_1

0.0000

τ_2

31.2022

τ_3

-1.3255

τ_4

-1.4497

τ_5

0.0010

τ_6

0.0000

τ₁ ≈ 0 — pierwszy przegub porusza się wokół osi pionowej, więc grawitacja go nie obciąża, a brak q̈₁ oznacza brak siły bezwładności. τ₂ = 31.2 Nm dominuje — to przegub utrzymujący ramię w pozie q₂ = 45° przeciw grawitacji, plus dodatkowa składowa od ω₁ (efekt odśrodkowy przy ramieniu wykonującym łuk wokół osi 1). τ₃, τ₄ niewielkie — kostka i przedramię w stosunkowo neutralnej pozie.

Wszystkie liczby są spójne z algorytmem z src/lib/dynamics/newton-euler.ts. Wartości są w jednostkach SI (rad, rad/s, rad/s², m, m/s, m/s², N, Nm, kg, kg·m²). Dla q̈ ≠ 0 widoczne byłyby też niezerowe wartości $\varepsilon$ (przyspieszenia kątowe) we wszystkich ogniwach.

Ściąga formuł

Wszystkie kluczowe równania algorytmu Newton-Euler dla manipulatora w jednym miejscu — przydatne jako kompaktowa referencja przy implementacji albo powtórce przed egzaminem.

Inicjalizacja (baza)

{}^0\boldsymbol{\omega}_0 = \mathbf{0}, \quad {}^0\boldsymbol{\varepsilon}_0 = \mathbf{0}, \quad {}^0\mathbf{v}_0 = \mathbf{0}, \quad {}^0\mathbf{a}_0 = -g\,\hat{\mathbf{z}}_{\text{world}}

Sztuczka Craig'a: a₀ to "fictitious upward base acceleration" — symuluje grawitację działającą "w dół" na ogniwa, dzięki czemu w rekurencji w przód aᶜᵢ propaguje grawitację automatycznie.

Rekurencja w przód (forward sweep) · prędkość kątowa (eq. 6.6)

{}^{i+1}\boldsymbol{\omega}_{i+1} = {}^{i+1}R_i\,{}^i\boldsymbol{\omega}_i + \dot\theta_{i+1}\,\hat{\mathbf{z}}_{i+1}

Rekurencja w przód · przyspieszenie kątowe (eq. 6.7)

{}^{i+1}\boldsymbol{\varepsilon}_{i+1} = {}^{i+1}R_i\,{}^i\boldsymbol{\varepsilon}_i + ({}^{i+1}R_i\,{}^i\boldsymbol{\omega}_i)\times\dot\theta_{i+1}\hat{\mathbf{z}}_{i+1} + \ddot\theta_{i+1}\hat{\mathbf{z}}_{i+1}

Drugi człon to efekt Coriolisa — pojawia się gdy ogniwo dziedziczy ω od poprzedniego i jednocześnie ma własną prędkość przegubu.

Rekurencja w przód · prędkość liniowa początku ogniwa (eq. 6.8)

{}^{i+1}\mathbf{v}_{i+1} = {}^{i+1}R_i\,\bigl({}^i\mathbf{v}_i + {}^i\boldsymbol{\omega}_i\times {}^i\mathbf{p}_{i+1}\bigr)

Rekurencja w przód · przyspieszenie liniowe początku ogniwa (eq. 6.9)

{}^{i+1}\mathbf{a}_{i+1} = {}^{i+1}R_i\,\bigl({}^i\boldsymbol{\varepsilon}_i\times {}^i\mathbf{p}_{i+1} + {}^i\boldsymbol{\omega}_i\times({}^i\boldsymbol{\omega}_i\times {}^i\mathbf{p}_{i+1}) + {}^i\mathbf{a}_i\bigr)

Pierwszy człon — tangencjalny (od ε), drugi — odśrodkowy (analog v²/r), trzeci — przyspieszenie poprzedniego ogniwa (zawiera grawitację).

Środek masy · prędkość i przyspieszenie (eq. 6.11–6.12)

{}^i\mathbf{v}_{Ci} = {}^i\mathbf{v}_i + {}^i\boldsymbol{\omega}_i\times {}^i\mathbf{p}_{Ci}

{}^i\mathbf{a}_{Ci} = {}^i\boldsymbol{\varepsilon}_i\times {}^i\mathbf{p}_{Ci} + {}^i\boldsymbol{\omega}_i\times({}^i\boldsymbol{\omega}_i\times {}^i\mathbf{p}_{Ci}) + {}^i\mathbf{a}_i

Tensor bezwładności i siły bezwładności (eq. 6.13–6.15)

I_C = \begin{bmatrix} I_{xx} & -I_{xy} & -I_{xz} \\ -I_{xy} & I_{yy} & -I_{yz} \\ -I_{xz} & -I_{yz} & I_{zz} \end{bmatrix}

{}^i\mathbf{F}_{Ci} = m_i\,{}^i\mathbf{a}_{Ci}, \qquad {}^i\mathbf{N}_{Ci} = I_{Ci}\,{}^i\boldsymbol{\varepsilon}_i + {}^i\boldsymbol{\omega}_i\times(I_{Ci}\,{}^i\boldsymbol{\omega}_i)

F_C — siła d'Alemberta (z grawitacją). Drugi człon w N_C to moment żyroskopowy — odpowiedzialny za precesję szybko obracających się ogniw.

Rekurencja w tył (backward sweep) · siła reakcji w przegubie (Craig, 6.49)

{}^i\mathbf{f}_i = {}^iR_{i+1}\,{}^{i+1}\mathbf{f}_{i+1} + {}^i\mathbf{F}_{Ci}

Rekurencja w tył · moment siły w przegubie (Craig, 6.50)

{}^i\mathbf{n}_i = {}^i\mathbf{N}_{Ci} + {}^iR_{i+1}\,{}^{i+1}\mathbf{n}_{i+1} + {}^i\mathbf{p}_{Ci}\times {}^i\mathbf{F}_{Ci} + {}^i\mathbf{p}_{i+1}\times({}^iR_{i+1}\,{}^{i+1}\mathbf{f}_{i+1})

Moment napędowy w osi przegubu

\tau_i = {}^i\mathbf{n}_i \cdot \hat{\mathbf{z}}_i = (\,{}^i\mathbf{n}_i)_z

Składowa wzdłuż osi obrotu przegubu i (czyli z układu i). Pozostałe składowe momentu są zniwelowane przez konstrukcję mechaniczną przegubu.

Konwencja indeksów (Craig, modified DH)

${}^i\mathbf{p}_{i+1}$ — pozycja przegubu (i+1) w układzie (i).
${}^i\mathbf{p}_{Ci}$ — środek masy ogniwa (i) w układzie (i).
${}^{i+1}R_i$ — macierz rotacji z układu (i) do (i+1) (z linkTransform DH).
Górny indeks = w którym układzie wektor jest wyrażony.

Pułapki implementacyjne

Kierunek transformacji R — modifiedDHTransform zwraca macierz, której kolumny to baza (i) wyrażona w (i-1). Aby przejść z (i-1) do (i) w rekurencji w przód, trzeba użyć R^T, nie R.
Konwencja a₀ — Craig: a₀ = -g·ẑ (z grawitacją w rekurencji w przód, bez osobnego F_g). Niektóre teksty używają a₀ = 0 + osobny F_g — równoważne, ale nie wolno mieszać konwencji w jednej implementacji.
Tensor I_C w lokalnym układzie — I_C zdefiniowany w układzie ogniwa (i), nie w bazie. Twierdzenie Steinera (parallel-axis) potrzebne tylko wtedy gdy podaje się I względem początku ogniwa zamiast środka masy.
τ_i to składowa N_i wzdłuż z_i — nie norma, nie moduł. Dla przegubu obrotowego oś z_i to oś przegubu (konwencja DH).

Co dalej

Mając wektor momentów napędowych $\tau_i$ otwiera się kilka naturalnych dalszych dróg, w zależności od tego, do czego dynamika jest nam potrzebna:

Energia napędów — czy silniki są w stanie wytworzyć te momenty? Ile prądu potrzeba? Jaka jest moc chwilowa? Ile energii zużywa robot w cyklu transportowym? Tym zajmuje się moduł 10 — model elektromechaniczny silnika DC, przekładnia harmoniczna ze sprawnością zależną od obciążenia, i ostateczna metryka: zużycie energii dla zadanej trajektorii (kluczowa dla optymalizacji w rozdz. 7–8 dysertacji [Gruszka 2024]).
Dynamika kontaktu — gdy robot trzyma obiekt albo wchodzi w interakcję z otoczeniem, w backward sweep zamiast $\mathbf{f}_{n+1} = \mathbf{0}$ wpisujemy zmierzone (np. z czujnika 6-DOF F/T) lub żądane (impedance/admittance control) obciążenie zewnętrzne. Algorytm pozostaje ten sam — zmienia się tylko warunek początkowy ostatniej iteracji.
Identyfikacja parametrów — w praktyce wartości $m_i$ , $\mathbf{p}_{Ci}$ , $I_{Ci}$ z CAD są przybliżone (10–30% błędu). Można je poprawiaćz pomiarów — regresja liniowa po zadanej trajektorii pozwala wyestymować parametry z dokładnością < 3%. To temat rozdz. 9 dysertacji.
Sterowanie wielocielne (whole-body) — dla humanoidów i robotów mobilnych ten sam algorytm uogólnia się na drzewa (tree-structured) i z więzami floating-base. Implementacje: Pinocchio, RBDL, OCS2. Aplikacje: chód dwunożny, mobile manipulation.