Математичні основи

Математичні основи розв'язувача програми SimPhant базуються на математичних та фізичних формулюваннях, представлених у книзі А. А. Шабани «Обчислювальна динаміка» (A.A. Shabana “Computational Dynamics“), з особливим акцентом на описі систем твердих тіл за допомогою диференційно-алгебраїчних рівнянь (ДАР).

Вектор станів і кватерніони (State Vector and Quaternions)

Для кожного твердого тіла, що рухається, стан визначається єдиним одновимірним числовим масивом — вектором стану:

$$\mathbf{Y \ } = \ \left\lbrack \mathbf{q} \mathbf{, \ } \dot{\mathbf{q}} \right\rbrack^{T}$$

Вектор стану визначається двома векторами:

Положення та орієнтація:$\mathbf{q} = \left\lbrack x , \ y , \ z , \ q_{w} , \ q_{x} , \ q_{y} , \ q_{z} \right\rbrack^{T}$,

Лінійна та кутові швидкості:$\dot{\mathbf{q}} = \left\lbrack v_{x} , \ v_{y} , \ v_{z} , \ \omega_{x} , \omega_{y} , \ \omega_{z} \right\rbrack^{T}$.

Де:

$\mathbf{r} = [ x , y , z ]^{T}$ вектор просторового положення тіла,

$\mathbf{v} = \left\lbrack v_{x} , v_{y} , v_{z} \right\rbrack^{T}$ вектор лінійної швидкості тіла,

$\mathbf{\omega} = \left\lbrack \omega_{x} , \omega_{y} , \omega_{z} \right\rbrack^{T}$ вектор кутової швидкості тіла в локальній системі координат тіла,

Кватерніони $( q_{w} , \ q_{x} , \ q_{y} , \ q_{z} )$ визначають орієнтацію тіла: кутове положення головних осей тіла в глобальній системі координат.

Використання кватерніонів дозволяє уникнути ситуації «блокування карданового підвісу» (gimbal lock) у процесі інтегрування (що могло б призвести до миттєвого збою розв'язувача DAE), а також обумовлено тим, що клас Rotation бібліотеки SciPy глибоко оптимізований для роботи з кватерніонами.

Кватерніони перетворюються на матрицю повороту:

$$A = \begin{bmatrix}1 \ - \ 2 \left( q_{y}^{2} + \ q_{z}^{2} \right) & 2 \left( q_{x} q_{y} - \ q_{w} q_{z} \right) & 2 \left( q_{x} q_{z} + \ q_{w} q_{y} \right) \\ 2 \left( q_{x} q_{y} + \ q_{w} q_{z} \right) & 1 \ - \ 2 \left( q_{x}^{2} + \ q_{z}^{2} \right) & 2 \left( q_{y} q_{z} - \ q_{w} q_{x} \right) \\ 2 \left( q_{x} q_{z} - \ q_{w} q_{y} \right) & 2 \left( q_{y} q_{z} + \ q_{w} q_{x} \right) & 1 \ - \ 2 \left( q_{x}^{2} + \ q_{y}^{2} \right)\end{bmatrix}$$

Перетворення матриці повороту на кватерніон і зворотне перетворення кватерніону на матрицю повороту тіла виконується бібліотекою SciPy.

Матриця повороту перетворюється на кути Ейлера (кути Тейта-Брайана типу Z-Y-X) виключно для оновлення 3D-графіки PyVista на екрані (у вікні перегляду):

$$\begin{cases}\theta_{z} = \arctan \left( \frac{A_{21}}{A_{11}} \right) \\ \theta_{y} = \arcsin \left( - A_{31} \right) \\ \theta_{x} = \arctan \left( \frac{A_{32}}{A_{33}} \right)\end{cases}$$

Співвідношення між першою похідною за часом кватерніонів тіла в тривимірному просторі та вектором кутової швидкості тіла у пов'язаній системі координат:

$$\dot{\mathbf{q}} = \frac{1}{2} \mathbf{G}^{T} \mathbf{\omega}$$

де G — матриця, сформована на основі поточних значень кватерніона. Похідна кватерніона з локальної кутової швидкості обчислюється вирішувачем:

$$\begin{aligned} \dot q_x &= \tfrac12 (q_w \omega_x + q_y \omega_z - q_z \omega_y) \\ \dot q_y &= \tfrac12 (q_w \omega_y + q_z \omega_x - q_x \omega_z) \\ \dot q_z &= \tfrac12 (q_w \omega_z + q_x \omega_y - q_y \omega_x) \\ \dot q_w &= -\tfrac12 (q_x \omega_x + q_y \omega_y + q_z \omega_z) \end{aligned}$$

Центроїдальна система координат тіла (Centroidal Body Coordinate System)

В архітектурі моделювання розглядається центроїдальна система координат тіла. Це окремий випадок у задачах просторової динаміки, коли за опорну точку вибирається центр мас тіла. У цьому випадку матриця мас твердого тіла є діагональною, а вектор відцентрових сил тотожно дорівнює нулю. У разі використання центроїдальної системи координат (початок якої збігається з центром мас) рівняння поступального та обертального руху твердого тіла математично розділяються. Це дозволяє представити загальну матрицю мас як блочно-діагональну постійну матрицю розміру 6NХ6N, де N — кількість тіл у моделі. Для окремого тіла (Body_i) локальна матриця мас $\mathbf{M}_{\mathbf{i}}$ розміру 6Х6 повністю розділяється на блок мас 3Х3 і блок інерції 3Х3:

$$\mathbf{M}_{\mathbf{i}} = \begin{bmatrix}m_{i} & 0 & 0 & 0 & 0 & 0 \\ 0 & m_{i} & 0 & 0 & 0 & 0 \\ 0 & 0 & m_{i} & 0 & 0 & 0 \\ 0 & 0 & 0 & J_{x x} & 0 & 0 \\ 0 & 0 & 0 & 0 & J_{y y} & 0 \\ 0 & 0 & 0 & 0 & 0 & J_{z z}\end{bmatrix}$$

Де $m_{i}$ — маса, а — $\widehat{\mathbf{J}}$ головні моменти інерції тіла Body_i.

Глобальна матриця мас системи:

$$\mathbf{M} = \begin{bmatrix}M_{1} & \cdots & 0 \\ \vdots & \ddots & \vdots \\ 0 & \cdots & M_{n}\end{bmatrix}$$

Використання пов'язаної з центром мас системи координат тіла є одним з основних припущень під час виведення фундаментальних рівнянь Ньютона — Ейлера, які використовуються в розв'язувачі:

$$\begin{bmatrix}M & \Phi_{q}^{T} \\ \Phi_{q} & 0\end{bmatrix} \begin{bmatrix}\ddot{q} \\ \lambda\end{bmatrix} = \begin{bmatrix}Q \\ \gamma\end{bmatrix}$$

Де:

$\mathbf{M}$: матриця мас і моментів інерції (розмірністю: $n \ \times n$),

$\mathbf{\Phi}_{\mathbf{q}}$: Матриця Якобіана для кінематичних зв'язків (розмірністю: $m \ \times n$),

$\ddot{\mathbf{q}}$: невідомі прискорення системи (розмірністю: $n \ \times 1$),

$\mathbf{\lambda}$: невідомі реакції зв'язків (розмірністю: $m \ \times 1$),

$\mathbf{Q}$: відомі зовнішні сили та моменти,

$\mathbf{\gamma}$: відомі кінематичні прискорення (права частина рівнянь),

$n = 6 N$ — кількість ступенів свободи, а — $m$ кількість кінематичних зв'язків.

У динаміці систем твердих тіл процес моделювання зводиться до розв'язання системи рівнянь з розширеною матрицею мас для визначення прискорень $\ddot{q}$ і множників Лагранжа $\lambda$, що є реакціями в кінематичних парах (шарнірах).

Гібридне формулювання (Hybrid Formulation)

Відповідно до просторової формулювання Ньютона-Ейлера, запропонованої Шабаною, рівняння Ньютона-Ейлера з урахуванням кінематичних зв'язків записуються з використанням гібридного формулювання (також званої формулюванням зі змішаними координатами). При цьому приймаються два основні припущення.

1. Поступальний рух розглядається в ГЛОБАЛЬНІЙ системі координат:

Координати $\mathbf{r} = [ x , y , z ]^{T}$ і сили $\mathbf{F}$, що викликають поступальний рух, визначаються в глобальній системі координат. Це спрощує запис рівнянь зв'язків $\Phi$, оскільки кінематичні пари фізично з'єднують тіла саме в глобальному просторі.

2. Обертальний рух розглядається в ЛОКАЛЬНІЙ системі координат (пов'язаній з тілом):

Кутова швидкість $\overline{\mathbf{\omega}} = \ \left\lbrack \overline{\omega_{x}} , \ \overline{\omega_{y}} , \ \overline{\omega_{z}} \right\rbrack^{T}$і моменти сил$\ ( \mathbf{T )}$ визначаються в локальній системі координат, оскільки тензор інерції $\widehat{\mathbf{J}}$ залишається незмінним у локальній системі відліку, пов'язаній з тілом. Оскільки маса також є сталою, вся матриця мас (для системи без урахування зв'язків) залишається незмінною. Вона формується всього один раз під час запуску програми. Тоді ж обчислюється і зворотна до неї матриця, необхідна для застосування методу доповнення Шура.

Архітектура фізичного рушія та потоки даних (Physics Engine Architecture & Data Flow)

Інтегратори SciPy та векторизовані користувацькі алгоритми вимагають, щоб стан усієї системи було представлено у вигляді єдиного одномірного числового масиву — вектора стану:

$$\mathbf{Y \ } = \ \left\lbrack \mathbf{q} \mathbf{, \ } \dot{\mathbf{q}} \right\rbrack^{T}$$

Для N рухомих тіл вектор $\mathbf{q}$ (положення/кути) має розмір 7N, а $\dot{\mathbf{q}}$ (швидкості) — 6N.

Загальний розмір $\mathbf{Y}$ становить 13N.

Архітектура симуляції розділяє фізичне формулювання завдання та процес просування в часі.

• Обчислювач системи (System Evaluator): єдина основна функція, яка приймає поточний час (t) і поточний вектор стану ($\mathbf{Y}$), формує матриці, розв'язує систему лінійних рівнянь і повертає значення прискорень.
• Інтегратор: алгоритм (RK4, Euler, BDF тощо), який запитує прискорення у обчислювача системи та використовує їх для переведення вектора стану на наступний часовий крок ($t \ + \ \Delta t$).

Крок 1: Збір даних та ініціалізація (об'єктно-орієнтований рівень)

Робота рушія починається на рівні інтерфейсу користувача (UI), де користувач задає тривимірний світ.

1.1 Об'єкти інтерфейсу користувача

Дані 3D CAD і налаштування користувача зберігаються у високорівневих класах Python:

• RigidBody (Жорстке тіло): зберігає масу, центр мас (CoG), тензори інерції та початковий кінематичний стан (положення, швидкість, кватерніон, кутова швидкість).
• Joint (З'єднання): зберігає інформацію про топологічний зв'язок між двома тілами (body_i, body_j), тип з'єднання (наприклад, обертальне, поступальне) та локальні просторові вектори, що визначають точки прив'язки й осі ковзання/обертання.
• Force (Сила): зберігає задані користувачем зовнішні сили, моменти сил і вектори гравітації.

1.2 Ініціалізація розв'язувача (MBSolver.init)

Під час створення екземпляра основного розв'язувача динаміки багатоланкових систем (MBSolver) виконується попередня обробка вхідних даних:

• Фільтрація основи (Ground Filtration): виключається тіло, позначене як «земля» (Ground), і формується список, що містить лише рухомі тіла. Вирішувач повністю ігнорує ступені свободи «землі» (зменшуючи таким чином розмірність матриці) і розглядає її як нерухому опору з нескінченною масою (індекс: -1).
• Зіставлення тіл (Body Mapping): кожному рухомому тілу присвоюється унікальний цілочисельний індекс (body_index_map).
• Попередній розрахунок матриці мас ($\mathbf{M}$): Оскільки маса та локальні характеристики інерції твердих тіл залишаються незмінними, розв'язувач одноразово формує глобальну блок-діагональну матрицю мас ($\mathbf{M}$) розмірності 6N×6N.

1.3 Пакування статичних даних для Numba (prepare_static_numba_data)

Оскільки JIT-компілятор Numba (що працює на рівні C) не може безпосередньо зчитувати об'єкти Python, розв'язувач витягує незмінні параметри з'єднань (типи, індекси тіл, локальні точки прив'язки та локальні осі) і перетворює їх на безперервні одновимірні та двовимірні масиви NumPy. Ця операція виконується рівно один раз перед початком симуляції.

Крок 2: Хід виконання (рівень інтегрування)

Коли користувач натискає кнопку «Solve» (Розв'язати), інтерфейс зчитує обраний метод інтегрування (наприклад, RK4, BDF, Symplectic Euler), який бере на себе повне керування часовим циклом. Незалежно від того, чи використовується простий явний цикл (як у методі Ейлера), чи складний неявний розв'язувач із бібліотеки SciPy (як BDF), інтегратор розглядає фізичну систему як «чорний ящик» — систему звичайних диференціальних рівнянь (ЗДР) першого порядку:

$$\frac{d Y}{d t} = f ( t , Y )$$

Для визначення динаміки системи інтегратор багаторазово викликає основний математичний модуль — метод evaluate_derivatives(t, Y). Цей метод виконує роль сполучної ланки (або «контрольної точки»).

Крок 3: Побудова матриці KKT (всередині методу evaluate_derivatives)

Щоразу, коли інтегратору потрібно визначити прискорення, він передає поточний час (t) і вектор стану ($\mathbf{Y}$) у метод evaluate_derivatives(t, Y), який виконує таку послідовність дій:

3.1 Розпакування стану та розрахунок сил ($\mathbf{Q}$)

• Метод unpack_state(Y) перетворює плоский вектор $\mathbf{Y}$ назад в об'єкти твердих тіл (RigidBody), оновлюючи їхні глобальні матриці повороту ($\mathbf{A}$) та швидкості ($\dot{\mathbf{q}}$).

• Метод build_force_vector() формує вектор сил правої частини рівняння (для системи без урахування обмежень):

  $$\mathbf{Q =} \mathbf{Q}_{\mathbf{e}} \mathbf{+} \mathbf{Q}_{\mathbf{v}}$$

Де

$\mathbf{Q}_{\mathbf{e}} \mathbf{=} \mathbf{Q}_{\mathbf{e x t e r n a l}}$ : зовнішні сили, гравітація та сили пружності пружин.

$\mathbf{Q}_{\mathbf{v}} \mathbf{=} \mathbf{Q}_{\mathbf{v e l o c i t y}}$ : розрахункові гіроскопічні сили та сили Коріоліса, що виникають під час обертання тіл: $\overline{\omega} \times \left( I \overline{\omega} \right)$.

3.2 Формування матриці Якобі ($\mathbf{\Phi}_{\mathbf{q}}$ і $\mathbf{\gamma}^{\mathbf{*}}$)

Перебір усіх кінематичних пар (зчленувань) і розрахунок кінематики для побудови:

• $\mathbf{\Phi}_{\mathbf{q}}$ (Матриця Якобі): матриця просторових напрямків, за якими обмежені ступені свободи.
• $\mathbf{\gamma}^{\mathbf{*}}$ (Вектор прискорень правої частини): містить центростремні та коріолісові прискорення в зчленуваннях ($\gamma$), скориговані за допомогою членів стабілізації Баумгарте ($- 2 \alpha \dot{\Phi} - \beta^{2}$) для запобігання розходженню зчленувань.

Крок 4: Розв'язання системи диференційно-алгебраїчних рівнянь (метод доповнення Шура)

Після формування матриць і векторів $M , Q , \Phi_{q} , \gamma^{*}$ система набуває вигляду, що відповідає розширеній матриці умов Каруша-Куна-Таккера (KKT)

$$\begin{bmatrix}M & \Phi_{q}^{T} \\ \Phi_{q} & 0\end{bmatrix} \begin{bmatrix}\ddot{q} \\ \lambda\end{bmatrix} = \begin{bmatrix}Q \\ \gamma^{*}\end{bmatrix}$$

Розв'язання цієї системи щодо множників Лагранжа $\mathbf{\lambda}$ та прискорень $\ddot{\mathbf{q}}$ виконується з використанням високоефективного методу доповнення Шура.

Крок 5: Повернення до інтегратора

Швидкості тіл ($\dot{\mathbf{q}}$) і знову обчислені прискорення ($\ddot{\mathbf{q}}$) передаються в інтегратор (вибраний користувачем), який виконує крок за часом для даної системи. Потім цикл повторюється, починаючи з кроку 3, до завершення симуляції.

Метод доповнення Шура та рішення KKT (Schur Complement method)

Доповнення Шура використовується замість стандартного методу розв'язання матричних рівнянь (наприклад, методу виключення Гауса). Математичний висновок і структурні переваги методу доповнення Шура.

Математичне обґрунтування доповнення Шура

У динаміці систем твердих тіл фізична система описується системою диференційно-алгебраїчних рівнянь (ДАР). Під час запису в матричному вигляді вона набуває форми класичної розширеної матриці (система KKT — Каруша-Куна-Таккера):

$$\begin{bmatrix}M & \Phi_{q}^{T} \\ \Phi_{q} & 0\end{bmatrix} \begin{bmatrix}\ddot{q} \\ \lambda\end{bmatrix} = \begin{bmatrix}Q \\ \gamma^{*}\end{bmatrix}$$

Де:

$\mathbf{M}$: матриця мас і моментів інерції (розмірністю: $n \ \times n$),

$\mathbf{\Phi}_{\mathbf{q}}$: Матриця Якобіана для кінематичних зв'язків (розмірністю: $m \ \times n$),

$\ddot{\mathbf{q}}$: невідомі прискорення системи (розмірністю: $n \ \times 1$),

$\mathbf{\lambda}$: невідомі реакції зв'язків (розмірністю: $m \ \times 1$),

$\mathbf{Q}$: відомі зовнішні сили та моменти,

$\mathbf{\gamma}^{\mathbf{*}}$: Відома права частина, що являє собою кінематичні прискорення ($\gamma$) з урахуванням стабілізації Баумгарте:

$$\gamma^{*} = \gamma - 2 \alpha \dot{\Phi} - \beta^{2}$$

$n$ — кількість ступенів свободи, а $m$ — кількість обмежень.

Висновок

Якщо розписати матрицю у вигляді двох окремих рівнянь, отримаємо:

1. Рівняння динаміки: $M \ddot{q} + \Phi_{q}^{T} \lambda = Q$
2. Кінематичне рівняння: $\Phi_{q} \ddot{q} = \gamma^{*}$

Метод доповнення Шура вирішує цю задачу шляхом алгебраїчної підстановки. Спочатку виділимо прискорення ($\ddot{\mathbf{q}}$) у рівнянні (1), помноживши його на зворотну матрицю мас ($\mathbf{M}^{\mathbf{- 1}}$):

$$\ddot{q} = M^{- 1} \left( Q - \Phi_{q}^{T} \lambda \right)$$

Далі ми підставляємо цей вираз для $\ddot{\mathbf{q}}$ безпосередньо у рівняння 2:

$$\Phi_{q} \left( M^{- 1} \left( Q - \Phi_{q}^{T} \lambda \right) \right) = \gamma^{*}$$

Тепер розкриємо дужки для $\Phi_{q}$ і перетворимо рівняння так, щоб виділити невідомі сили реакції ($\lambda$) у лівій частині:

$$\left( \Phi_{q} M^{- 1} \Phi_{q}^{T} \right) \lambda = \Phi_{q} M^{- 1} Q - \gamma^{*}$$

Таким чином, ми звели систему до одного лінійного рівняння: $C \lambda = \text{RHS}$

Де:

• $C = \Phi_{q} M^{- 1} \Phi_{q}^{T}$ — матриця доповнення Шура (іноді називається провідною матрицею Якобі або матрицею Якобі, ортогоналізованою за масою).
• $\text{RHS} = \Phi_{q} M^{- 1} Q - \gamma^{*}$ — відомий вектор правої частини.

Вирішивши рівняння $C \lambda = \text{RHS}$ щодо множників Лагранжа ($\lambda$), ми підставляємо їх назад у рівняння динаміки і без труднощів обчислюємо потрібні прискорення ($\ddot{q}$).

Рішення з використанням регуляризації Тихонова (Tikhonov Regularization Solution)

Коли кінематичний ланцюг утворює замкнутий контур (наприклад, чотиризв'язковий механізм) або коли кілька шарнірів обмежують один і той самий ступінь свободи, матриця Якобі для зв'язків ($\Phi_{q}$) втрачає рядковий ранг. Без регуляризації матриця доповнення Шура ($C$) стає виродженою, її визначник перетворюється на нуль, і розв'язувач негайно аварійно завершує роботу з помилкою «Singular Matrix» (вироджена матриця).

Використання регуляризації Тихонова замість задання нескінченно жорстких зв'язків дозволяє внести незначну величину «податливості» (пружності) безпосередньо на діагональ матриці. Це миттєво усуває проблему виродженості матриці, не змінюючи її розмірності. Ми замінюємо нульовий блок у правому нижньому куті на $\mathbf{-} \mathbf{\epsilon} \mathbf{I}$, і система набуває такого вигляду:

$$\begin{bmatrix}\mathbf{M} & \mathbf{\Phi}_{\mathbf{q}}^{\mathbf{T}} \\ \mathbf{\Phi}_{\mathbf{q}} & \mathbf{- \epsilon I}\end{bmatrix} \begin{bmatrix}\ddot{\mathbf{q}} \\ \mathbf{\lambda}\end{bmatrix} \mathbf{=} \begin{bmatrix}\mathbf{Q} \\ \mathbf{\gamma}^{\mathbf{*}}\end{bmatrix}$$

Де:

$\epsilon$ — це значення (наприклад: -1E-7), що фізично представляє податливість зв'язку — величину, зворотну масі $kg^{-1}$ (або інерції $1/kgm^2$ для обертання),

$I = \begin{bmatrix}1 & \cdots & 0 \\ \vdots & 1 & \vdots \\ 0 & \cdots & 1\end{bmatrix}$ — одинична матриця.

Під час виконання блочного виключення для формування доповнення Шура ($C$), що використовується для знаходження множників Лагранжа ($\lambda$), рівняння перетворюється з виду

$$C = \Phi_{q} M^{- 1} \Phi_{q}^{T}$$

на

$$C_{r e g} = \Phi_{q} M^{- 1} \Phi_{q}^{T} + \epsilon I$$

Просто додавши $\epsilon$ до головної діагоналі матриці $C$, ми гарантуємо, що всі власні числа будуть точно більшими за нуль, а матриця стане безумовно позитивно визначеною, що виключає можливість аварійного завершення обчислень.

Використання доповнення Шура дає значні переваги порівняно зі стандартним методом виключення Гауса. Повна матриця KKT має розмірність $( n + m ) \times ( n + m )$, де $n$ — число ступенів свободи, а $m$ — число обмежень. Використовуючи доповнення Шура, ми зводимо розмір невідомої матриці з $( n + m ) \times ( n + m )$ до всього лише $( m \ \times m )$. Оскільки в локальних системах координат матриця $M$ є сталою, її зворотна матриця ($M^{- 1}$) обчислюється або факторизується заздалегідь (один раз). Розв'язання системи з матрицею малого розміру $m \ \times m$ на кожному кроці відбувається значно швидше, ніж розв'язання величезної матриці KKT методом виключення Гауса.

Кінематичні обмеження / зв'язки та матриці Якобі (Kinematic Constraints /Joints and Jacobian Matrices)

У цьому розділі описується математична основа кінематичних зв'язків.

Кінематична формулювання сферичного шарніра (Spherical Joint)

У динаміці багатоланкових систем сферичний шарнір (часто званий кульковим шарніром) обмежує три поступальні ступені свободи, але допускає всі три обертальні ступені свободи.

Визначення понять:

• $r_{i} , r_{j}$: Глобальні тривимірні вектори положення центру тяжіння (ЦТ) для тіл Body_i та Body_j.
• $A_{i} , A_{j}$: $3 \ \times 3$ Глобальні матриці повороту тіл Body_i та Body_j.
• $\overline{u_{i}} , \overline{u_{j}}$: Локальні тривимірні вектори, що з'єднують центр мас з точкою прив'язки шарніра на тілах Body_i та Body_j.
• $\overline{\omega_{i}} , \overline{\omega_{j}}$: Локальні тривимірні вектори кутової швидкості.
• $\widetilde{u}$: Кососиметрична матриця $3 \ \times 3$ вектора $\mathbf{u}$, яка математично реалізує операцію векторного добутку$\left( \widetilde{u} x = u \times x \right)$.

1. Рівняння кінематичного зв'язку за положенням ($\mathbf{\Phi}$)

Фізична сутність сферичного шарніра полягає в тому, що точка прив'язки на тілі Body_i і точка прив'язки на тілі Body_j повинні в будь-який момент часу займати одне й те саме положення в глобальній системі координат.

Глобальне положення точки прив'язки на тілі Body_i визначається як: $r_{i} + A_{i} \overline{u_{i}}$.

Таким чином, рівняння зв'язку дає 3 рядки (X, Y, Z) у нашій системі:

$$\Phi ( q , t ) = r_{i} + A_{i} \overline{u_{i}} - r_{j} - A_{j} \overline{u_{j}} = \begin{bmatrix}0 \\ 0 \\ 0\end{bmatrix}$$

2. Обмеження швидкості та Якобіан ($\mathbf{\Phi}_{\mathbf{q}}$)

У загальному випадку матриця обмежень Якобі визначається таким чином:

$$\Phi_{q} = \begin{bmatrix}\frac{\partial \Phi_{1}}{\partial q_{1}} & \frac{\partial \Phi_{1}}{\partial q_{2}} & \frac{\partial \Phi_{1}}{\partial q_{3}} & \cdots & \frac{\partial \Phi_{1}}{\partial q_{n}} \\ \frac{\partial \Phi_{2}}{\partial q_{1}} & \frac{\partial \Phi_{2}}{\partial q_{2}} & \frac{\partial \Phi_{2}}{\partial q_{3}} & \cdots & \frac{\partial \Phi_{2}}{\partial q_{n}} \\ \vdots & \vdots & \vdots & \ddots & \vdots \\ \frac{\partial \Phi_{m}}{\partial q_{1}} & \frac{\partial \Phi_{m}}{\partial q_{2}} & \frac{\partial \Phi_{m}}{\partial q_{3}} & \cdots & \frac{\partial \Phi_{m}}{\partial q_{n}}\end{bmatrix}$$

Де:

n — кількість координат системи,

m — число рівнянь зв'язків.

Для знаходження якобіана в рамках гібридної формулювання ми беремо першу похідну за часом від рівняння зв'язку за положенням ($\dot{\Phi} = 0$). Необхідно виділити швидкості, що визначають стан системи: глобальну лінійну швидкість ($v$) і локальну кутову швидкість ($\overline{\omega}$).

Швидкість точки прив'язки виражається таким чином:

$$\dot{\Phi} = v_{i} + A_{i} \left( \overline{\omega_{i}} \times \overline{u_{i}} \right) - v_{j} - A_{j} \left( \overline{\omega_{j}} \times \overline{u_{j}} \right) = 0$$

Використовуючи властивість кососиметричності векторного добутку ($\overline{\omega} \times \overline{u} = - \widetilde{u} \overline{\omega}$), перепишемо рівняння так, щоб виділити коефіцієнти якобіана, які увійдуть до розширеної матриці:

$$\dot{\Phi} = [ I ] v_{i} + \left\lbrack - A_{i} \widetilde{\overline{u}_{i}} \right\rbrack \overline{\omega_{i}} + [ - I ] v_{j} + \left\lbrack A_{j} \widetilde{\overline{u}_{j}} \right\rbrack \overline{\omega_{j}} = 0$$

На основі цього виділяємо $3 \ \times 12$ матрицю Якобі ($\mathbf{\Phi}_{\mathbf{q}}$) для сферичного з'єднання:

$$\Phi_{q} = \begin{bmatrix}I_{3 \times 3} & - A_{i} \widetilde{\overline{u}_{i}} & - I_{3 \times 3} & A_{j} \widetilde{\overline{u}_{j}}\end{bmatrix}$$

Кінематичне формулювання шарніра обертання (Revolute Joint)

Шарнір обертання (revolute joint) обмежує 5 ступенів свободи, залишаючи рівно один обертальний ступінь свободи (обертання навколо осі шарніра). Математично він формується шляхом об'єднання двох типів обмежень:

1. Сферичне обмеження (3 рядки): фіксує точку прив'язки тіла Body_i відносно точки прив'язки тіла Body_j.
2. Два обмеження ортогональності DP-1 на основі скалярного добутку (2 рядки): забезпечують сувору паралельність осі шарніра на тілі Body_i осі шарніра на тілі Body_j.

Визначення векторів:

Для запобігання вигину шарніра визначаються «вісь Z шарніра» (вектор$v$) і дві ортогональні «осі площини» (вектори $\mathbf{a}$ і$\mathbf{b}$).

• $\overline{u_{i}} , \overline{u_{j}}$: Локальні вектори точок прив'язки (від центру мас до шарніра).
• $\overline{v_{i}}$: Локальний вектор осі Z шарніра на тілі Body_i.
• $\overline{a_{j}} , \overline{b_{j}}$: Локальні вектори осей X та Y на тілі Body_j (перпендикулярні до осі шарніра).
• $v_{i} , a_{j} , b_{j}$: Глобальні версії цих векторів (наприклад,$v_{i} = A_{i} \overline{v_{i}}$).

1. Рівняння зв'язку за положенням ($\mathbf{\Phi}$)

Матриця зв'язків містить 5 рядків. Перші три рядки відповідають уже розглянутому нами сферичному зв'язку. Два останні рядки забезпечують умову, за якої вісь шарніра тіла Body_i залишається строго перпендикулярною осям X та Y тіла Body_j. Якщо вектор $v_{i}$ перпендикулярний до векторів $a_{j}$ і $b_{j}$, то математично він виявляється зафіксованим паралельно до вектора$v_{j}$.

$$\Phi ( q , t ) = \begin{bmatrix}r_{i} + A_{i} \overline{u_{i}} - r_{j} - A_{j} \overline{u_{j}} \\ v_{i} \cdot a_{j} \\ v_{i} \cdot b_{j}\end{bmatrix} = \begin{bmatrix}0 \\ 0 \\ 0 \\ 0 \\ 0\end{bmatrix}$$

2. Обмеження за швидкістю та матриця Якобі ($\mathbf{\Phi}_{\mathbf{q}}$)

Для знаходження матриці Якобі ми обчислюємо першу похідну за часом від рівнянь зв'язків. Перші 3 рядки (блоки сферичного шарніра) нам уже відомі. Залишається вивести вирази для рядків 4 і 5.

Похідна за часом від скалярного добутку двох обертальних векторів, заданих у глобальній системі координат, має вигляд:

$$\frac{d}{d t} \left( v_{i} \cdot a_{j} \right) = \dot{v_{i}} \cdot a_{j} + v_{i} \cdot \dot{a_{j}} = 0$$

Оскільки швидкості векторів у глобальній системі координат визначаються через векторний добуток ($\dot{v_{i}} = \omega_{i} \times v_{i}$), ми можемо перетворити змішані добутки так, щоб виділити потрібні швидкості стану ($v$ і $\overline{\omega}$). Це дозволяє отримати точні блоки матриці Якобі, що відповідають за обертання, для розширеної матриці:

Рядок 4 (Ортогональність до вектора $\mathbf{a}_{\mathbf{j}}$):

• Блок обертання для тіла Body_i: $A_{i}^{T} \left( v_{i} \times a_{j} \right)$
• Блок обертання для тіла Body_j: $A_{j}^{T} \left( a_{j} \times v_{i} \right)$

Рядок 5 (Ортогональність до вектора $\mathbf{b}_{\mathbf{j}}$):

• Блок обертання для тіла Body_i: $A_{i}^{T} \left( v_{i} \times b_{j} \right)$
• Блок обертання для тіла Body_j: $A_{j}^{T} \left( b_{j} \times v_{i} \right)$

Блоки матриці Якобі, що відповідають за поступальний рух для рядків 4 і 5, дорівнюють нулю [0, 0, 0], оскільки чисте обертання тіла не призводить до переміщення його центру мас.

Повністю сформований блок матриці Якобі розміру 5 × 12 для обертального шарніра:

$$\Phi_{q} = \begin{bmatrix}I_{3 \times 3} & - A_{i} \widetilde{\overline{u}_{i}} & - I_{3 \times 3} & A_{j} \widetilde{\overline{u}_{j}} \\ \lbrack \begin{matrix}0 & 0 & 0\end{matrix} \rbrack & A_{i}^{T} \left( v_{i} \times a_{j} \right) & \lbrack \begin{matrix}0 & 0 & 0\end{matrix} \rbrack & A_{j}^{T} \left( a_{j} \times v_{i} \right) \\ \lbrack \begin{matrix}0 & 0 & 0\end{matrix} \rbrack & A_{i}^{T} \left( v_{i} \times b_{j} \right) & \lbrack \begin{matrix}0 & 0 & 0\end{matrix} \rbrack & A_{j}^{T} \left( b_{j} \times v_{i} \right)\end{bmatrix}$$

Кінематичне формулювання поступального з'єднання (Prismatic Joint)

Поступальне з'єднання (prismatic joint) обмежує 5 ступенів свободи:

1. Три обмеження типу DP-1 на основі скалярного добутку (3 рядки): повністю фіксують взаємну орієнтацію двох тіл, виключаючи можливість їхнього повороту один відносно одного (крену, тангажу та рискання).
2. Два обмеження типу DP-2 на основі скалярного добутку (2 рядки): забезпечують суворе знаходження опорної точки повзуна всередині математичної «напрямної», заданої іншим тілом.

1. Рівняння зв'язку за положенням ($\mathbf{\Phi}$)

Визначення векторів:

Нехай Body_i — це «напрямна» (Track), а Body_j — «повзунок» (Slider).

• $P_{i} , P_{j}$: Глобальні координати локальних точок прив'язки (наприклад, $P_{i} = r_{i} + A_{i} \overline{u_{i}}$)
• $d$: Глобальний вектор відстані між точками прив'язки: $d = P_{j} - P_{i}$
• $v_{i}$: Глобальний вектор осі ковзання (вісь Z) на тілі Body_i.
• $a_{i} , b_{i}$: Глобальні вектори ортогональних осей площини (осі X та Y) на тілі Body_i.
• $a_{j} , b_{j}$: Глобальні вектори ортогональних осей площини (осі X та Y) на тілі Body_j.

Умови зв'язку:

Необхідно запобігти обертанню тіла Body_j відносно тіла Body_i. Це досягається шляхом забезпечення суворого перпендикулярного розташування (кут 90°) їх ортогональних осей, що перевіряється за допомогою скалярного добутку.

Далі необхідно запобігти зісковзуванню тіла Body_j з напрямної. Якщо повзунок рухається виключно вздовж осі $v_{i}$, то проекції вектора відстані d на осі $a_{i}$ (вісь X) і $b_{i}$ (вісь Y) напрямної мають дорівнювати нулю.

У результаті отримуємо систему з 5 рівнянь:

$$\Phi ( q , t ) = \begin{bmatrix}v_{i} \cdot a_{j} \\ v_{i} \cdot b_{j} \\ a_{i} \cdot b_{j} \\ d \cdot a_{i} \\ d \cdot b_{i}\end{bmatrix} \, = \begin{bmatrix}0 \\ 0 \\ 0 \\ 0 \\ 0\end{bmatrix}$$

Рядки 1–3 задають обмеження обертання. Рядки 4–5 задають обмеження поступального руху.

2. Обмеження швидкостей і Якобіан ($\mathbf{\Phi}_{\mathbf{q}}$)

Для формування блоків розширеної матриці ми беремо першу похідну ($\dot{\Phi} = 0$).

Рядки 1–3 (блокування обертання):

Це стандартні обмеження типу DP-1 (аналогічні обмеженням на вигин у шарнірі обертання Revolute). Оскільки чисте обертання не викликає переміщення центру мас, відповідні блоки матриці Якобі для поступального руху дорівнюють нулю: [0, 0, 0].

Для рядка 1 ($v_{i} \cdot a_{j} = 0$):

• Блок обертання тіла Body_: $A_{i}^{T} \left( v_{i} \times a_{j} \right)$
• Блок обертання тіла Body_j: $A_{j}^{T} \left( a_{j} \times v_{i} \right)$

Аналогічна схема застосовується для рядка 2 (з використанням векторів$v_{i} , b_{j}$) і для рядка 3 (з використанням векторів$a_{i} , b_{j}$).

Рядки 4 і 5 (блокування поступального руху):

Диференціювання рівняння $d \cdot a_{i} = 0$ вимагає застосування правила похідної добутку: $\dot{\Phi_{4}} = \dot{d} \cdot a_{i} + d \cdot \dot{a_{i}} = 0$.

Підставивши вираз $\dot{d} = v_{j} + \left( \omega_{j} \times s_{j} \right) - v_{i} - \left( \omega_{i} \times s_{i} \right)$ (де $s$ — вектор у глобальній системі координат від центру мас до точки прив'язки), можна виділити змінні стану швидкостей і визначити блоки матриці Якобі:

Для рядка 4 ($d \cdot a_{i} = 0$):

• Блок поступального руху тіла Body_i: $- a_{i}^{T}$
• Блок обертання тіла Body_i: $A_{i}^{T} \left( a_{i} \times \left( d + s_{i} \right) \right)$
• Блок поступального руху тіла Body_j: $a_{i}^{T}$
• Блок обертання тіла Body_j: $A_{j}^{T} \left( s_{j} \times a_{i} \right)$

Рядок 5 використовує точно таку саму структуру, замінюючи вектор $a_{i}$ на $b_{i}$.

Повністю зібрана матриця Якобі для поступального з'єднання ($\mathbf{\Phi}_{\mathbf{q}}$)

$$\Phi_{q} = \begin{bmatrix}\begin{bmatrix}0 & 0 & 0\end{bmatrix} & A_{i}^{T} \left( v_{i} \times a_{j} \right) & \begin{bmatrix}0 & 0 & 0\end{bmatrix} & A_{j}^{T} \left( a_{j} \times v_{i} \right) \\ \begin{bmatrix}0 & 0 & 0\end{bmatrix} & A_{i}^{T} \left( v_{i} \times b_{j} \right) & \begin{bmatrix}0 & 0 & 0\end{bmatrix} & A_{j}^{T} \left( b_{j} \times v_{i} \right) \\ \begin{bmatrix}0 & 0 & 0\end{bmatrix} & A_{i}^{T} \left( a_{i} \times b_{j} \right) & \begin{bmatrix}0 & 0 & 0\end{bmatrix} & A_{j}^{T} \left( b_{j} \times a_{i} \right) \\ - a_{i}^{T} & A_{i}^{T} \left( a_{i} \times \left( d + s_{i} \right) \right) & a_{i}^{T} & A_{j}^{T} \left( s_{j} \times a_{i} \right) \\ - b_{i}^{T} & A_{i}^{T} \left( b_{i} \times \left( d + s_{i} \right) \right) & b_{i}^{T} & A_{j}^{T} \left( s_{j} \times b_{i} \right)\end{bmatrix}$$

Кінематичне формулювання циліндричного шарніра (Cylindrical Joint)

Циліндричний шарнір обмежує рівно 4 ступені свободи. Він дозволяє тілу Body_j ковзати вздовж осі Z тіла Body_i та вільно обертатися навколо цієї ж осі Z.

Визначення векторів:

Нехай Body_i — це «напрямна/вал», а Body_j — «втулка».

• $P_{i} , P_{j}$: Глобальні координати локальних точок прив'язки.
• $d$: Глобальний вектор відстані між точками прив'язки: $d = P_{j} - P_{i}$.
• $v_{i}$: Глобальний вектор осі ковзання/обертання (вісь Z) на тілі Body_i.
• $a_{i} , b_{i}$: Глобальні вектори ортогональних осей площини (осі X та Y) на тілі Body_i.
• $a_{j} , b_{j}$: Глобальні вектори ортогональних осей площини (осі X та Y) на тілі Body_j.

1. Рівняння обмежень положення ($\mathbf{\Phi}$)

Для створення циліндричного шарніра використовуються 4 рівняння:

Два обмеження обертання (рядки 1 і 2): ми забезпечуємо сувору паралельність осі Z втулки ($v_{j}$) осі Z вала ($v_{i}$). Це досягається шляхом вимоги, щоб вісь $v_{i}$ була перпендикулярною (кут 90°) осям X та Y втулки ($a_{j} , b_{j}$). Ми навмисно не використовуємо третє обмеження обертання ($a_{i} \cdot b_{j} = 0$), що дозволяє втулці вільно обертатися.

Два обмеження поступального руху (рядки 3 і 4): Ми вимагаємо, щоб проекція вектора відстані $d$ на осях X і Y вала ($a_{i} , b_{i}$) дорівнювала нулю. Це гарантує, що втулка не зміститься вбік відносно вала.

Це дає нам 4 рядки:

$$\Phi ( q , t ) = \begin{bmatrix}v_{i} \cdot a_{j} \\ v_{i} \cdot b_{j} \\ d \cdot a_{i} \\ d \cdot b_{i}\end{bmatrix} \, = \begin{bmatrix}0 \\ 0 \\ 0 \\ 0\end{bmatrix}$$

2. Обмеження швидкостей і Якобіан ($\mathbf{\Phi}_{\mathbf{q}}$)

Для отримання блоків матриці Якобі, що входять до розширеної матриці, обчислюється перша похідна за часом ($\dot{\Phi} = 0$). Оскільки використовуються ті самі рівняння, що і для поступального шарніра, блоки матриці Якобі ідентичні.

Рядки 1 і 2 (обмеження обертання):

Чисте обертання не викликає переміщення центру мас , тому блоки матриці Якобі, що відповідають за поступальний рух, дорівнюють нулю: [0, 0, 0].

Для рядка 1 ($v_{i} \cdot a_{j} = 0$):

• Блок обертання для тіла Body_i: $A_{i}^{T} \left( v_{i} \times a_{j} \right)$
• Блок обертання для тіла Body_j: $A_{j}^{T} \left( a_{j} \times v_{i} \right)$

Рядок 2 має аналогічну структуру, але вектор $a_{j}$ замінено на $b_{j}$.

Рядки 3 і 4 (обмеження переміщення):

Диференціювання рівняння $d \cdot a_{i} = 0$ дозволяє виділити змінні стану швидкостей. Нехай $s_{i}$ і $s_{j}$ — глобальні вектори, що з'єднують центри мас з точками прив'язки.

Для рядка 3 ($d \cdot a_{i} = 0$):

• Блок поступального руху для тіла Body_i: $- a_{i}^{T}$
• Блок обертання для тіла Body_i: $A_{i}^{T} \left( a_{i} \times \left( d + s_{i} \right) \right)$
• Блок поступального руху для тіла Body_j: $a_{i}^{T}$
• Блок обертання для тіла Body_j: $A_{j}^{T} \left( s_{j} \times a_{i} \right)$

Рядок 4 має аналогічну структуру, але вектор $a_{i}$ замінено на $b_{i}$.

Повна матриця Якобі для циліндричного шарніра ($\mathbf{\Phi}_{\mathbf{q}}$):

$$\Phi_{q} = \begin{bmatrix}\begin{bmatrix}0 & 0 & 0\end{bmatrix} & A_{i}^{T} \left( v_{i} \times a_{j} \right) & \begin{bmatrix}0 & 0 & 0\end{bmatrix} & A_{j}^{T} \left( a_{j} \times v_{i} \right) \\ \begin{bmatrix}0 & 0 & 0\end{bmatrix} & A_{i}^{T} \left( v_{i} \times b_{j} \right) & \begin{bmatrix}0 & 0 & 0\end{bmatrix} & A_{j}^{T} \left( b_{j} \times v_{i} \right) \\ - a_{i}^{T} & A_{i}^{T} \left( a_{i} \times \left( d + s_{i} \right) \right) & a_{i}^{T} & A_{j}^{T} \left( s_{j} \times a_{i} \right) \\ - b_{i}^{T} & A_{i}^{T} \left( b_{i} \times \left( d + s_{i} \right) \right) & b_{i}^{T} & A_{j}^{T} \left( s_{j} \times b_{i} \right)\end{bmatrix}$$

Кінематичне формулювання плоского з'єднання (Planar Joint)

Плоске зчленування усуває рівно 3 ступені свободи:

1. Обмежує поступальний рух уздовж вектора нормалі (утримує шайбу на столі).
2. Обмежує обертання навколо осей тангажу та рискання (утримує шайбу в горизонтальному положенні на столі).

Плоске зчленування зберігає 3 ступені свободи: вільне ковзання по площині (вздовж осей X та Y) та вільне обертання навколо вектора нормалі.

Визначення векторів:

Нехай Body_i — «Площина» (наприклад, стіл), а Body_j — «Шайба».

• $P_{i} , P_{j}$: Глобальні координати локальних точок прив'язки.
• $d$: Глобальний вектор відстані між точками прив'язки: $d = P_{j} - P_{i}$
• $v_{i}$: Глобальний вектор нормалі до площини на тілі Body_i (вісь Z зчленування).
• $a_{j} , b_{j}$: Глобальні ортогональні осі площини (осі X та Y) на тілі Body_j.

1. Рівняння обмежень положення ($\mathbf{\Phi}$)

Необхідно запобігти відриву тіла Body_j від площини та його нахилу.

Два обмеження обертання (рядки 1 і 2): ми задаємо умову, за якої осі площини шайби ($a_{j} , b_{j}$) завжди перпендикулярні до вектора нормалі столу ($v_{i}$) (кут 90 градусів). Це гарантує, що дві поверхні залишаються математично паралельними, дозволяючи при цьому шайбі вільно обертатися.

Одне обмеження поступального руху (рядок 3): ми задаємо умову, за якої проекція вектора відстані d на вектор нормалі столу ($v_{i}$) дорівнює нулю. Це фіксує шайбу безпосередньо на поверхні.

У результаті отримуємо такі 3 рядки:

$$\Phi ( q , t ) = \begin{bmatrix}v_{i} \cdot a_{j} \\ v_{i} \cdot b_{j} \\ d \cdot v_{i}\end{bmatrix} \, = \begin{bmatrix}0 \\ 0 \\ 0\end{bmatrix}$$

2. Кінематичне обмеження за швидкістю та матриця Якобі ($\mathbf{\Phi}_{\mathbf{q}}$)

Ми обчислюємо першу похідну за часом ($\dot{\Phi} = 0$), щоб отримати блоки матриці Якобі для розширеної матриці.

Рядки 1 і 2 (обмеження обертання):

Це стандартні обмеження типу «скалярний добуток» (DP1 або Dot-1), які ми використовували для обертальних і поступальних кінематичних пар. Оскільки чисте обертання не викликає переміщення центру мас, блоки матриці Якобі, що відповідають за поступальний рух, дорівнюють нулю: [0, 0, 0].

Для рядка 1 ($v_{i} \cdot a_{j} = 0$):

• Блок обертання для тіла Body_i: $A_{i}^{T} \left( v_{i} \times a_{j} \right)$
• Блок обертання для тіла Body_j: $A_{j}^{T} \left( a_{j} \times v_{i} \right)$

Рядок 2 має точно таку ж структуру, але вектор $a_{j}$ замінено на $b_{j}$.

Рядок 3 (обмеження поступального руху):

Диференціюючи скалярний добуток, отримуємо $\dot{\Phi_{3}} = \dot{d} \cdot v_{i} + d \cdot \dot{v_{i}} = 0$.

Підставляючи вирази для кінематики швидкостей у глобальній системі координат

$\left( \dot{d} = v_{j} + \left( \omega_{j} \times s_{j} \right) - v_{i} - \left( \omega_{i} \times s_{i} \right) \right)$,

ми виділяємо змінні стану і отримуємо такі блоки матриці Якобі:

Для рядка 3 ($d \cdot v_{i} = 0$):

• Блок поступального руху для тіла Body_i: $- v_{i}^{T}$
• Блок обертання для тіла Body_i: $A_{i}^{T} \left( v_{i} \times \left( d + s_{i} \right) \right)$
• Блок поступального руху для тіла Body_j: $v_{i}^{T}$
• Блок обертання для тіла Body_j: $A_{j}^{T} \left( s_{j} \times v_{i} \right)$

Це повністю повторює математичний опис поступальної пари, з тією лише різницею, що замість двох векторів напрямних ($a_{i} , b_{i}$) використовується один нормальний вектор$v_{i}$.

Повна матриця Якобі для плоского з'єднання ($\mathbf{\Phi}_{\mathbf{q}}$):

$$\Phi_{q} = \begin{bmatrix}\begin{bmatrix}0 & 0 & 0\end{bmatrix} & A_{i}^{T} \left( v_{i} \times a_{j} \right) & \begin{bmatrix}0 & 0 & 0\end{bmatrix} & A_{j}^{T} \left( a_{j} \times v_{i} \right) \\ \begin{bmatrix}0 & 0 & 0\end{bmatrix} & A_{i}^{T} \left( v_{i} \times b_{j} \right) & \begin{bmatrix}0 & 0 & 0\end{bmatrix} & A_{j}^{T} \left( b_{j} \times v_{i} \right) \\ - v_{i}^{T} & A_{i}^{T} \left( v_{i} \times \left( d + s_{i} \right) \right) & v_{i}^{T} & A_{j}^{T} \left( s_{j} \times v_{i} \right)\end{bmatrix}$$

Кінематичне формулювання жорсткого з'єднання (Fixed Joint)

Жорстке з'єднання (Fixed Joint) є найповнішим обмеженням: воно усуває всі 6 ступенів свободи (6 DOF). Воно виключає будь-яке відносне поступальне переміщення та обертання. Два тіла жорстко скріплені одне з одним і діють як єдине тверде тіло.

Жорстке з'єднання — це, по суті, об'єднання обмежень на поступальний рух (як у сферичного шарніра) та обмежень на обертання (як у призматичного шарніра).

Визначення векторів:

Нехай тіло Body_i — «базове» (Base), а тіло Body_j — «приєднуване» (Attachment).

• $\overline{u_{i}} , \overline{u_{j}}$: Локальні координати точок прив'язки (від центру мас до шарніра).
• $v_{i} , a_{i} , b_{i}$: Глобальна ортогональна тріада (осі Z, X, Y) для тіла Body_i.
• $v_{j} , a_{j} , b_{j}$: Глобальна ортогональна тріада (осі Z, X, Y) для тіла Body_j.

1. Рівняння обмежень положення ($\mathbf{\Phi}$)

Необхідно виключити розбіжність точок прив'язки, а також запобігти взаємному повороту тіл (крен, тангаж і рискання).

1. Три обмеження на поступальний рух (рядки 1–3): Глобальна точка прив'язки тіла Body_i має точно збігатися з глобальною точкою прив'язки тіла Body_j (обмеження сферичного шарніра).
2. Три обмеження на обертання (рядки 4–6): ортогональні осі тіла Body_i повинні зберігати строго перпендикулярне положення відносно ортогональних осей тіла Body_j (обмеження обертання призматичного шарніра).

У результаті виходить рівно 6 рядків:

$$\Phi ( q , t ) = \begin{bmatrix}r_{i} + A_{i} \overline{u_{i}} - r_{j} - A_{j} \overline{u_{j}} \\ v_{i} \cdot a_{j} \\ v_{i} \cdot b_{j} \\ a_{i} \cdot b_{j}\end{bmatrix} = \begin{bmatrix}0 \\ 0 \\ 0 \\ 0 \\ 0 \\ 0\end{bmatrix}$$

2. Кінематичне обмеження за швидкістю та матриця Якобі ($\mathbf{\Phi}_{\mathbf{q}}$)

Ми беремо першу похідну за часом ($\dot{\Phi} = 0$), щоб отримати блоки матриці Якобі для розширеної матриці системи. Оскільки ми об'єднуємо два вже розглянуті нами типи зчленувань, нам достатньо просто скомпонувати (скласти) їхні матриці Якобі.

Рядки з 1-го по 3-й (обмеження на поступальний рух):

Це ті самі блоки, що і для сферичного шарніра.

Нехай $s_{i} = A_{i} \overline{u_{i}}$ і$s_{j} = A_{j} \overline{u_{j}}$, тоді

• Блок поступального руху для тіла Body_i: $I$ (3x3 одинична матриця)
• Блок обертального руху для тіла Body_i: $- A_{i} \widetilde{s_{i}}$ (кососиметрична матриця вектора $s_{i}$)
• Блок поступального руху для тіла Body_j: $- I$
• Блок обертального руху для тіла Body_j: $A_{j} \widetilde{s_{j}}$

Рядки з 4-го по 6-й (обмеження на обертальний рух):

Це ті самі блоки (типу DP1 «скалярний добуток — 1»), що і для поступальної кінематичної пари. Чисте обертання не викликає переміщення центру мас (CoG), тому блоки матриці Якобі, що відповідають за поступальний рух, дорівнюють нулю: [0, 0, 0].

Для рядка 4 ($v_{i} \cdot a_{j} = 0$):

• Блок обертального руху для тіла Body_i: $A_{i}^{T} \left( v_{i} \times a_{j} \right)$
• Блок обертального руху для тіла Body_j: $A_{j}^{T} \left( a_{j} \times v_{i} \right)$

У 5-му рядку використовуються вектори$v_{i} , b_{j}$, а в 6-му — $a_{i} , b_{j}$.

Повністю сформований блок 6 × 12 матриці Якобі для жорсткого з'єднання ($\mathbf{\Phi}_{\mathbf{q}}$):

$$\Phi_{q} = \begin{bmatrix}I_{3 \times 3} & - A_{i} \widetilde{s_{i}} & - I_{3 \times 3} & A_{j} \widetilde{s_{j}} \\ \lbrack \begin{matrix}0 & 0 & 0\end{matrix} \rbrack & A_{i}^{T} \left( v_{i} \times a_{j} \right) & \lbrack \begin{matrix}0 & 0 & 0\end{matrix} \rbrack & A_{j}^{T} \left( a_{j} \times v_{i} \right) \\ \lbrack \begin{matrix}0 & 0 & 0\end{matrix} \rbrack & A_{i}^{T} \left( v_{i} \times b_{j} \right) & \lbrack \begin{matrix}0 & 0 & 0\end{matrix} \rbrack & A_{j}^{T} \left( b_{j} \times v_{i} \right) \\ \lbrack \begin{matrix}0 & 0 & 0\end{matrix} \rbrack & A_{i}^{T} \left( a_{i} \times b_{j} \right) & \lbrack \begin{matrix}0 & 0 & 0\end{matrix} \rbrack & A_{j}^{T} \left( b_{j} \times a_{i} \right)\end{bmatrix}$$

Обмеження за прискоренням (гамма) для всіх типів з'єднань

Вектор прискорення в правій частині рівняння ($\gamma$ або $Q_{d}$) описує, як у матричному розв'язувачі визначаються центростремна сила та сила Коріоліса.

Математичні визначення та змінні

У глобальній системі координат визначено такі вектори:

• $\omega_{i} , \omega_{j}$: вектори глобальної кутової швидкості тіл Body_i та Body_j.
• $s_{i} , s_{j}$: вектори глобальної відстані від центру мас тіла до точки прив'язки зчленування (Anchor).
• $v_{i} , a_{i} , b_{i}$: глобальна ортогональна тріада (осі Z, X, Y) на тілі Body_i.
• $v_{j} , a_{j} , b_{j}$: Глобальна ортогональна тріада (осі Z, X, Y) на тілі Body_j.
• $d$: вектор глобальної відстані між точками прив'язки, де$d = \left( r_{j} + s_{j} \right) - \left( r_{i} + s_{i} \right)$.

Для зчленувань, що допускають ковзання (призматичних, циліндричних, плоских), також визначаються похідні вектора відстані $d$, пов'язані зі швидкістю та центроспрямованим прискоренням:

• Швидкість зміни відстані:

  $$\dot{d} = \left( v e l_{j} + \left( \omega_{j} \times s_{j} \right) \right) - \left( v e l_{i} + \left( \omega_{i} \times s_{i} \right) \right)$$

• Центростремна складова зміни відстані:

  $$\ddot{d_{c e n t}} = \left( \omega_{j} \times \left( \omega_{j} \times s_{j} \right) \right) - \left( \omega_{i} \times \left( \omega_{i} \times s_{i} \right) \right)$$

У наведених нижче рівняннях індекси x, y, z позначають скалярні декартові компоненти отриманого тривимірного вектора.

1) Сферичний шарнір (усуває 3 ступені свободи)

Обмежує поступальне переміщення виключно в точках кріплення.

• $$\gamma_{1} = - \left( \omega_{i} \times \left( \omega_{i} \times s_{i} \right) \right)_{x} + \left( \omega_{j} \times \left( \omega_{j} \times s_{j} \right) \right)_{x}$$
• $$\gamma_{2} = - \left( \omega_{i} \times \left( \omega_{i} \times s_{i} \right) \right)_{y} + \left( \omega_{j} \times \left( \omega_{j} \times s_{j} \right) \right)_{y}$$
• $$\gamma_{3} = - \left( \omega_{i} \times \left( \omega_{i} \times s_{i} \right) \right)_{z} + \left( \omega_{j} \times \left( \omega_{j} \times s_{j} \right) \right)_{z}$$

2) Обертальний шарнір (усуває 5 ступенів свободи)

Поєднує в собі сферичну фіксацію (рядки 1–3) з двома ортогональними фіксаторами вигину (рядки 4–5) відносно осі шарніра$v_{i}$.

• $$\gamma_{1} = - \left( \omega_{i} \times \left( \omega_{i} \times s_{i} \right) \right)_{x} + \left( \omega_{j} \times \left( \omega_{j} \times s_{j} \right) \right)_{x}$$
• $$\gamma_{2} = - \left( \omega_{i} \times \left( \omega_{i} \times s_{i} \right) \right)_{y} + \left( \omega_{j} \times \left( \omega_{j} \times s_{j} \right) \right)_{y}$$
• $$\gamma_{3} = - \left( \omega_{i} \times \left( \omega_{i} \times s_{i} \right) \right)_{z} + \left( \omega_{j} \times \left( \omega_{j} \times s_{j} \right) \right)_{z}$$
• $$\gamma_{4} = - \left\lbrack \left( \left( \omega_{i} \times \left( \omega_{i} \times v_{i} \right) \right) \cdot a_{j} \right) + 2 \left( \left( \omega_{i} \times v_{i} \right) \cdot \left( \omega_{j} \times a_{j} \right) \right) + \left( v_{i} \cdot \left( \omega_{j} \times \left( \omega_{j} \times a_{j} \right) \right) \right) \right\rbrack$$
• $$\gamma_{5} = - \left\lbrack \left( \left( \omega_{i} \times \left( \omega_{i} \times v_{i} \right) \right) \cdot b_{j} \right) + 2 \left( \left( \omega_{i} \times v_{i} \right) \cdot \left( \omega_{j} \times b_{j} \right) \right) + \left( v_{i} \cdot \left( \omega_{j} \times \left( \omega_{j} \times b_{j} \right) \right) \right) \right\rbrack$$

3) Поступальний зв'язок (усуває 5 ступенів свободи)

Поєднує в собі повне блокування обертання (рядки 1–3) і блокування переміщення у двох напрямках (рядки 4–5) уздовж осі напрямної $v_{i}$.

• $$\gamma_{1} = - \left\lbrack \left( \left( \omega_{i} \times \left( \omega_{i} \times v_{i} \right) \right) \cdot a_{j} \right) + 2 \left( \left( \omega_{i} \times v_{i} \right) \cdot \left( \omega_{j} \times a_{j} \right) \right) + \left( v_{i} \cdot \left( \omega_{j} \times \left( \omega_{j} \times a_{j} \right) \right) \right) \right\rbrack$$
• $$\gamma_{2} = - \left\lbrack \left( \left( \omega_{i} \times \left( \omega_{i} \times v_{i} \right) \right) \cdot b_{j} \right) + 2 \left( \left( \omega_{i} \times v_{i} \right) \cdot \left( \omega_{j} \times b_{j} \right) \right) + \left( v_{i} \cdot \left( \omega_{j} \times \left( \omega_{j} \times b_{j} \right) \right) \right) \right\rbrack$$
• $$\gamma_{3} = - \left\lbrack \left( \left( \omega_{i} \times \left( \omega_{i} \times a_{i} \right) \right) \cdot b_{j} \right) + 2 \left( \left( \omega_{i} \times a_{i} \right) \cdot \left( \omega_{j} \times b_{j} \right) \right) + \left( a_{i} \cdot \left( \omega_{j} \times \left( \omega_{j} \times b_{j} \right) \right) \right) \right\rbrack$$
• $$\gamma_{4} = - \left\lbrack \left( \ddot{d_{c e n t}} \cdot a_{i} \right) + 2 \left( \dot{d} \cdot \left( \omega_{i} \times a_{i} \right) \right) + \left( d \cdot \left( \omega_{i} \times \left( \omega_{i} \times a_{i} \right) \right) \right) \right\rbrack$$
• $$\gamma_{5} = - \left\lbrack \left( \ddot{d_{c e n t}} \cdot b_{i} \right) + 2 \left( \dot{d} \cdot \left( \omega_{i} \times b_{i} \right) \right) + \left( d \cdot \left( \omega_{i} \times \left( \omega_{i} \times b_{i} \right) \right) \right) \right\rbrack$$

4) Циліндричне з'єднання (усуває 4 ступені свободи)

Дозволяє виконувати ковзання та обертання вздовж осі $v_{i}$.

• $$\gamma_{1} = - \left\lbrack \left( \left( \omega_{i} \times \left( \omega_{i} \times v_{i} \right) \right) \cdot a_{j} \right) + 2 \left( \left( \omega_{i} \times v_{i} \right) \cdot \left( \omega_{j} \times a_{j} \right) \right) + \left( v_{i} \cdot \left( \omega_{j} \times \left( \omega_{j} \times a_{j} \right) \right) \right) \right\rbrack$$
• $$\gamma_{2} = - \left\lbrack \left( \left( \omega_{i} \times \left( \omega_{i} \times v_{i} \right) \right) \cdot b_{j} \right) + 2 \left( \left( \omega_{i} \times v_{i} \right) \cdot \left( \omega_{j} \times b_{j} \right) \right) + \left( v_{i} \cdot \left( \omega_{j} \times \left( \omega_{j} \times b_{j} \right) \right) \right) \right\rbrack$$
• $$\gamma_{3} = - \left\lbrack \left( \ddot{d_{c e n t}} \cdot a_{i} \right) + 2 \left( \dot{d} \cdot \left( \omega_{i} \times a_{i} \right) \right) + \left( d \cdot \left( \omega_{i} \times \left( \omega_{i} \times a_{i} \right) \right) \right) \right\rbrack$$
• $$\gamma_{4} = - \left\lbrack \left( \ddot{d_{c e n t}} \cdot b_{i} \right) + 2 \left( \dot{d} \cdot \left( \omega_{i} \times b_{i} \right) \right) + \left( d \cdot \left( \omega_{i} \times \left( \omega_{i} \times b_{i} \right) \right) \right) \right\rbrack$$

5) Плоске з'єднання (усуває 3 ступені свободи)

Обмежує переміщення та нахил відносно нормального вектора $v_{i}$.

• $$\gamma_{1} = - \left\lbrack \left( \left( \omega_{i} \times \left( \omega_{i} \times v_{i} \right) \right) \cdot a_{j} \right) + 2 \left( \left( \omega_{i} \times v_{i} \right) \cdot \left( \omega_{j} \times a_{j} \right) \right) + \left( v_{i} \cdot \left( \omega_{j} \times \left( \omega_{j} \times a_{j} \right) \right) \right) \right\rbrack$$
• $$\gamma_{2} = - \left\lbrack \left( \left( \omega_{i} \times \left( \omega_{i} \times v_{i} \right) \right) \cdot b_{j} \right) + 2 \left( \left( \omega_{i} \times v_{i} \right) \cdot \left( \omega_{j} \times b_{j} \right) \right) + \left( v_{i} \cdot \left( \omega_{j} \times \left( \omega_{j} \times b_{j} \right) \right) \right) \right\rbrack$$
• $$\gamma_{3} = - \left\lbrack \left( \ddot{d_{c e n t}} \cdot v_{i} \right) + 2 \left( \dot{d} \cdot \left( \omega_{i} \times v_{i} \right) \right) + \left( d \cdot \left( \omega_{i} \times \left( \omega_{i} \times v_{i} \right) \right) \right) \right\rbrack$$

6) Жорстке з'єднання (усуваються всі 6 ступенів свободи)

Повністю скріплює два тіла, об'єднуючи сферичну фіксацію та три фіксації обертання.

• $$\gamma_{1} = - \left( \omega_{i} \times \left( \omega_{i} \times s_{i} \right) \right)_{x} + \left( \omega_{j} \times \left( \omega_{j} \times s_{j} \right) \right)_{x}$$
• $$\gamma_{2} = - \left( \omega_{i} \times \left( \omega_{i} \times s_{i} \right) \right)_{y} + \left( \omega_{j} \times \left( \omega_{j} \times s_{j} \right) \right)_{y}$$
• $$\gamma_{3} = - \left( \omega_{i} \times \left( \omega_{i} \times s_{i} \right) \right)_{z} + \left( \omega_{j} \times \left( \omega_{j} \times s_{j} \right) \right)_{z}$$
• $$\gamma_{4} = - \left\lbrack \left( \left( \omega_{i} \times \left( \omega_{i} \times v_{i} \right) \right) \cdot a_{j} \right) + 2 \left( \left( \omega_{i} \times v_{i} \right) \cdot \left( \omega_{j} \times a_{j} \right) \right) + \left( v_{i} \cdot \left( \omega_{j} \times \left( \omega_{j} \times a_{j} \right) \right) \right) \right\rbrack$$
• $$\gamma_{5} = - \left\lbrack \left( \left( \omega_{i} \times \left( \omega_{i} \times v_{i} \right) \right) \cdot b_{j} \right) + 2 \left( \left( \omega_{i} \times v_{i} \right) \cdot \left( \omega_{j} \times b_{j} \right) \right) + \left( v_{i} \cdot \left( \omega_{j} \times \left( \omega_{j} \times b_{j} \right) \right) \right) \right\rbrack$$
• $$\gamma_{6} = - \left\lbrack \left( \left( \omega_{i} \times \left( \omega_{i} \times a_{i} \right) \right) \cdot b_{j} \right) + 2 \left( \left( \omega_{i} \times a_{i} \right) \cdot \left( \omega_{j} \times b_{j} \right) \right) + \left( a_{i} \cdot \left( \omega_{j} \times \left( \omega_{j} \times b_{j} \right) \right) \right) \right\rbrack$$

Стабілізація Баумгарте – коректор дрейфу (Baumgarte Stabilization)

Численне інтегрування призводить до виникнення незначних помилок округлення. В результаті з часом елементи зчленування (Joint) фізично розходяться. Необхідно застосувати стабілізацію Баумгарте, щоб математично повернути їх у вихідне положення, подібно до дії системи «пружина-демпфер».

У розширеній матриці для всіх зчленувань ми замінюємо стандартний параметр $\gamma$ на стабілізований$\gamma^{*}$:

$$\gamma^{*} = \gamma - 2 \alpha \dot{\Phi} - \beta^{2}$$

Де:

• $\Phi$ — поточна помилка положення (розраховується на кроці 1),
• $\dot{\Phi}$ — поточна помилка швидкості (розраховується на кроці 2).
• $\alpha$ і $\beta$ — налаштовувані константи (див. розділ «Стабілізація Баумгарте»).