Пример вывода уравнений движения системы тел методом Кейна

15 Aug 2024 in Механика / Cae / Matlab

Вывод уравнений пространственного движения системы двух тел, используя метод Кейна.

Метод Кейна

Метод разработан в 1961 проф. Т. Кейном (Стэндфорский университет) (Kane, T.R., Dynamics of nonholonomic systems, J. App. Mech., 28, 574, 1961). Метод основан на принципе Даламбера-Лагранжа и ориентирован на машинное формирование уравнений движения. При выводе уравнений движения используются вспомогательные векторные величины, которые здесь будут называться частными линейными \(\mathbf v_{ik}\) и угловыми скоростями \(\boldsymbol \omega_{ik}\). Эти величины являются векторными множителями, стоящими перед обобщенными скоростями в выражениях для линейных и угловых скоростей тел механической системы. Например, скорость материальной точки на плоскости, положение которой определяется двумя координатами x и y, в декартовой системе координат можно представить как: \[\mathbf v = \mathbf{e}_x \dot{x} + \mathbf{e}_y \dot{y}\]

В этом выражении частными линейными скоростями будут единичные векторы \(\mathbf{e}_x\) и \(\mathbf{e}_x\).

При вращении твердого тела вокруг неподвижной точки частными угловыми скоростями будет орты связанной с телом системы координат \(\mathbf{e}_x^c, \mathbf{e}_y^c, \mathbf{e}_z^c\), поскольку угловая скорость тела будет определяться выражением \[\boldsymbol{\omega} = \mathbf{e}_x^c \omega_x + \mathbf{e}_y^c \omega_y + \mathbf{e}_z^c \omega_z\]

Рассмотрим движение несвободной системы материальных точек. Движение каждой точки определяется внешней силой и силой реакции: \[m_k \mathbf a_k = \mathbf F_k + \mathbf R_k, \quad k=1,\ldots,n.\]

После умножения каждого уравнения на соответствующее виртуальное перемещение \(\delta \mathbf r_k\) и сложения всех уравнений, для идеальных связей получим общее уравнение динамики: \[\sum_{k=1}^N \left(\mathbf F_k - m_k \mathbf a_k \right)\cdot \delta \mathbf r_k + \sum_{k=1}^N \underbrace{\mathbf R_k \cdot \delta \mathbf r_k}_{0} = 0.\]

Подставляя вариации радиус-вектора \(\mathbf r_k(q_1,\ldots,q_n)\) в обобщённых координатах: \[\delta \mathbf r_k = \sum_{i=1}^n \frac{\partial \mathbf r_k}{\partial q_i} \delta q_i.\]

получим \[\sum_{k=1}^N \left(\mathbf F_k - m_k \mathbf a_k \right)\cdot \sum_{i=1}^n \frac{\partial \mathbf r_k}{\partial q_i} \delta q_i = 0.\]

После изменения порядка суммирования: \[\sum_{i=1}^n \sum_{k=1}^N \left(\mathbf F_k \cdot \frac{\partial \mathbf r_k}{\partial q_i} - m_k \mathbf a_k \cdot \frac{\partial \mathbf r_k}{\partial q_i} \right) \delta q_i = 0.\]

и с учётом независимости вариаций обобщённых координат: \[\sum_{k=1}^N \left(\mathbf F_k \cdot \frac{\partial \mathbf r_k}{\partial q_i} - m_k \mathbf a_k \cdot \frac{\partial \mathbf r_k}{\partial q_i} \right)= 0, \quad i=1,\ldots,n.\]

Частная производная вектора скорости точки по обобщенной скорости имеет вид: \[\frac{\partial \mathbf v_k}{\partial \dot q_i} = \frac{\partial {\mathbf r_k}}{\partial {q}_i} = \mathbf u_{ki}.\]

и называется частной скоростью: \[\mathbf u_{ki} = \frac{\partial \mathbf v_k}{\partial \dot q_i}\]

Подставляя \[\frac{\partial {\mathbf r_k}}{\partial {q}_j} = \frac{\partial \mathbf v_k}{\partial \dot q_j}\]

в уравнения движения \[\sum_{k=1}^N \left(\mathbf F_k \cdot \frac{\partial \mathbf r_k}{\partial q_i} - m_k \mathbf a_k \cdot \frac{\partial \mathbf r_k}{\partial q_i} \right)= 0, \quad i=1,\ldots,n.\]

получим \[\sum_{k=1}^N \left(\mathbf F_k \cdot \frac{\partial \mathbf v_k}{\partial \dot q_i} - m_k \mathbf a_k \cdot \frac{\partial \mathbf v_k}{\partial \dot q_i} \right)= 0, \quad i=1,\ldots,n.\]

или \[\sum_{k=1}^N \left(\mathbf F_k \cdot \mathbf u_{ki} - m_k \mathbf a_k \cdot \mathbf u_{ki} \right)= 0, \quad i=1,\ldots,n.\]

Обозначив обобщенные активные силы \(Q_i\) и силы инерции \(Q_i^*\): \[Q_i = \sum_{k=1}^{N} \mathbf F_k \cdot \mathbf u_{ki}, \quad Q_i^* = - \sum_{k=1}^{N} m_k \mathbf a_k \cdot \mathbf u_{ki}\]

уравнения \[\sum_{k=1}^N \left(\mathbf F_k \cdot \mathbf u_{ki} - m_k \mathbf a_k \cdot \mathbf u_{ki} \right)= 0, \quad i=1,\ldots,n.\]

принимают вид: \[Q_i + Q_i^* = 0.\]

Алгоритм

1. Выбираются обобщенные координаты \(q_1,q_2\ldots,q_n\).
2. Определяются скорости тел, которые могут быть как производные обобщенных координат, так и их линейными комбинациями (например, угловые скорости для твердого тела).
3. Определяются выражения для векторов скоростей точек приложения сил.
4. Определяются выражения для векторов скоростей центров масс тел.
5. Определяются выражения для векторов угловых скоростей тел.
6. Определяются векторы линейных ускорений тел системы и их угловые ускорения.
7. Определяются частные линейные и угловые скорости для точек приложения сил, центров масс тел.
8. Определяются силы и моменты (активные и инерционные).
9. Вычисляются скалярные произведения сил, моментов и частных скоростей.

Пример

Схема системы

Используя метод Кейна, построим модель движения механической системы с четырьмя степенями свободы, представленной на рисунке 1. Система состоит из двух стержней связанных шарнирами. Первый стержень при помощи сферического шарнира связан с неподвижным основанием, второй стержень соединяется с первым при помощи цилиндрического шарнира.

Движение механизма происходит под действием силы тяжести, действующей отрицательном направлении оси \(Oz_0\).

Рисунок 1 - Схема механизма

Положение шарниров задано координатными столбцами векторов \(\mathbf{c}_{11}^{(1)}\), \(\mathbf{c}_{12}^{(1)}\), \(\mathbf{c}_{22}^{(2)}\):

• вектор \(\mathbf{c}_{11}\) соединяет центр масс тела 1 и шарнирную точку (О) первого шарнира;
• вектор \(\mathbf{c}_{12}\) соединяет центр масс тела 1 и шарнирную точку (А) второго шарнира;
• вектор \(\mathbf{c}_{22}\) соединяет центр масс тела 2 и шарнирную точку (А) второго шарнира.

Кинематические соотношения

Положение системы определим столбцом обобщенных координат: \[\mathbf q = [\alpha_1,\alpha_2,\alpha_3,\alpha_4]^T\]

Первые три угла определяют ориентацию базиса \(C_1 x_1 y_1 z_1\), связанного с телом 1 относительно неподвижного базиса \(O x_0 y_0 z_0\). Для определения ориентации используется последовательность XY’Z’’ (углы Брайнта). Матрица преобразования координат из базиса \(C_1 x_1 y_1 z_1\) в базис \(O x_0 y_0 z_0\) определяется выражением: \[\mathbf{A}^{01} = \mathbf{A}_{x}(\alpha_1) \mathbf{A}_{y}(\alpha_2) \mathbf{A}_{z}(\alpha_3)\]

Угол \(\alpha_4\) определяет ориентацию тела 2 относительно тела 1. Вращение тела 2 относительно тела 1 происходит вокруг общей оси \(y\), поэтому матрица преобразования координат из базиса \(C_2 x_2 y_2 z_2\), связанного со вторым телом в базис \(C_1 x_1 y_1 z_1\), будет иметь вид: \[\mathbf{A}^{12} = \mathbf{A}_{y}(\alpha_4)\]

Сформируем столбец скоростей системы, но не из обобщенных скоростей, а из проекций угловых скоростей тела 1 на его связанные оси и производной угла поворота второго тела относительно первого. \[\mathbf{u}=\left[\omega_x^{\left(1\right)},\omega_y^{\left(1\right)},\omega_z^{\left(1\right)},{\dot{\alpha}}_4\right]^T\]

Эта особенность метода Кейна позволяет упростить вывод уравнений движения системы.

Скорость и ускорение центра масс тела 1

Вектор скорости центра масс первого тела определяется выражением: \[\vec{V}_1 = \vec{\omega}_1 \times (-\vec{c}_{11}).\]

Координатная форма этого выражения в неподвижной СК \(A_1 x_0 y_0 z_0\) будет иметь вид: \[\mathbf{V}_1^{(0)}=\mathbf{A}^{01}{\widetilde{\mathbf{\omega}}}_1^{(1)}\ \left(-\mathbf{c}_{11}^{\left(1\right)}\right)=\mathbf{A}^{01}{\widetilde{\mathbf{c}}}_{11}^{\left(1\right)}\mathbf{\omega}_1^{(1)}=\left[\begin{matrix}\mathbf{A}^{01}{\widetilde{\mathbf{c}}}_{11}^{\left(1\right)}&\mathbf{0}_3\\\end{matrix}\right]\mathbf{u}=\mathbf{Q}_{V1}^{(0)}\mathbf{u}\]

Оператор тильда используется для матричной записи векторного произведения и преобразует координатный столбец вектора в кососимметрическую матрицу: \[\tilde{\mathbf a} = \begin{bmatrix} 0 & -a_z & a_y \\ a_z & 0 & -a_x \\ -a_y & a_x & 0 \end{bmatrix}, \quad \mathbf a \times \mathbf b = \tilde{\mathbf a} \mathbf b\]

Матрица \(\mathbf{Q}_{V1}^{(0)}\) имеет размерность \(3\times n\), где \(n\) – число степеней свободы системы. Каждый столбец этой матрицы представляет собой частную скорость центра масс тела 1, например, первый вектор \(\mathbf{u}_{11}\) можно получить следующим образом: \[\mathbf{u}_{11} = \mathbf{Q}_{V1}^{(0)} \begin{bmatrix} 1 \\ 0 \\ 0 \\ 0 \end{bmatrix}.\]

Координатный столбец ускорения центра масс получим, продифференцировав выражение для скорости: \[\mathbf{a}_1^{(0)} = \mathbf{Q}_{V1}^{(0)} \dot{\mathbf{u}} + \dot{\mathbf{Q}}_{V1}^{(0)} \dot{\mathbf{u}}\]

Производная матрицы частных векторов: \[\dot{\mathbf{Q}}_{V1}^{(0)} = \begin{bmatrix} \mathbf{A}^{01} \tilde{\boldsymbol \omega}_1^((1) ) \tilde{\mathbf c}_{11}^{(1)} & \mathbf{0}_3 \end{bmatrix}\]

где \({0}_3\) – столбец нулей \(3 \times 1\).

Угловая и скорость ускорение тела 1

Координатный столбец вектора угловой скорости первого тела в проекциях на собственные оси будет определяться выражением \[\boldsymbol{\omega}_1^{(1)} = \begin{pmatrix} \mathbf{E}_{3} & \mathbf{0}_3 \end{pmatrix} \mathbf{u} = \mathbf{Q}_{w1}^{(1)} \mathbf{u}\]

Матрица \(\mathbf{Q}_{w1}^{(1)}\) как и матрица \(\mathbf{Q}_{V1}^{(0)}\) имеет размерность \(3\times n\), где \(n\). Каждый столбец матрицы представляет собой частную угловую скорость тела 1.

Угловое ускорение тела 1 \[\boldsymbol{\varepsilon}_1^{(1)} = \mathbf{Q}_{w1}^{(1)} \dot{\mathbf{u}} + \dot{\mathbf{Q}}_{w1}^{(1)} \mathbf{u}\]

Матрица \(\mathbf{Q}_{w1}^{(1)}\) не зависит от времени, поэтому \(\dot{\mathbf{Q}}_{V1}^{(0)} = 0\).

Скорость и ускорение центра масс тела 2

Вектор скорости центра масс второго тела представляет собой сумму скорости движения второго тела вместе с первым и скорости второго тела относительно первого: \[\vec{V}_2 = \vec{V}_{2}^e + \vec{V}_{2}^r = \vec{\omega}_1 \times (-\vec{c}_{11}+\vec{c}_{12}-\vec{c}_{22})\]

В координатной матричной форме это выражение имеет вид: \[\mathbf{V}_2^{\left(0\right)}=\mathbf{A}^{01}{\widetilde{\mathbf{\omega}}}_1^{\left(1\right)}\ \left(-\mathbf{c}_{11}^{\left(1\right)}+\mathbf{c}_{12}^{\left(1\right)}-\mathbf{A}^{12}\mathbf{c}_{22}^{\left(2\right)}\right)+\mathbf{A}^{01}\mathbf{A}^{12}\left(-{\widetilde{\mathbf{e}}}_y\mathbf{c}_{22}^{\left(2\right)}\right){\dot{\alpha}}_4\]

где \(\mathbf{e}_y = [0,1,0]\) – единичный вектор оси y – оси вращения второго тела относительно первого.

Изменим порядок векторного произведения для того, чтобы вынести координатный столбец угловой скорости первого тела в правую часть выражения: \[\mathbf{V}_2^{\left(0\right)} = -\mathbf{A}^{01}\left(-{\widetilde{\mathbf{c}}}_{11}^{\left(1\right)}+{\widetilde{\mathbf{c}}}_{12}^{\left(1\right)}-\widetilde{\mathbf{A}^{12}\mathbf{c}_{22}^{\left(2\right)}}\right)\mathbf{\omega}_1^{\left(1\right)}+\mathbf{A}^{01}\mathbf{A}^{12}{\widetilde{\mathbf{c}}}_{22}^{\left(2\right)}\mathbf{e}_y{\dot{\alpha}}_4\]

и введем матрицу частных скоростей тела 2 \(\mathbf{Q}_{V2}^{(0)}\): \[\mathbf{V}_2^{\left(0\right)} = \left[-\begin{matrix}\mathbf{A}^{01}\left(-{\widetilde{\mathbf{c}}}_{11}^{\left(1\right)}+{\widetilde{\mathbf{c}}}_{12}^{\left(1\right)}-\widetilde{\mathbf{A}^{12}\mathbf{c}_{22}^{\left(2\right)}}\right)&\mathbf{A}^{01}\mathbf{A}^{12}{\widetilde{\mathbf{c}}}_{22}^{\left(2\right)}\mathbf{e}_y\\\end{matrix}\right]\mathbf{u}=\mathbf{Q}_{V2}^{(0)}\mathbf{u}\]

Продифференцируем выражение для \(\mathbf{V}_2^{\left(0\right)}\) для определения ускорения центра масс тела 2: \[\mathbf{a}_1^{(0)}=\mathbf{Q}_{V1}^{(0)}\dot{\mathbf{u}}+{\dot{\mathbf{Q}}}_{V1}^{(0)}\mathbf{u}\]

где \({\dot{\mathbf{Q}}}_{V1}^{(0)}\) – производная матрицы \(\mathbf{Q}_{V2}^{(0)}\): \[{\dot{\mathbf{Q}}}_{V2}^{(0)}= \begin{bmatrix} -\mathbf{A}^{01}{\widetilde{\mathbf{\omega}}}_1^{\left(1\right)}\left(-{\widetilde{\mathbf{c}}}_{11}^{\left(1\right)}+{\widetilde{\mathbf{c}}}_{12}^{\left(1\right)}-\widetilde{\mathbf{A}^{12}\mathbf{c}_{22}^{\left(2\right)}}\right)+\mathbf{A}^{01}\widetilde{\left(\mathbf{A}^{12}{\widetilde{\mathbf{\Omega}}}_2^{\left(2\right)}\mathbf{c}_{22}^{\left(2\right)}\right)} & \mathbf{A}^{01}{\widetilde{\mathbf{\omega}}}_1^{\left(1\right)}\mathbf{A}^{12}{\widetilde{\mathbf{c}}}_{22}^{\left(2\right)}\mathbf{e}_y+\mathbf{A}^{01}\mathbf{A}^{12}{\widetilde{\mathbf{\Omega}}}_2^{\left(2\right)}{\widetilde{\mathbf{c}}}_{22}^{\left(2\right)}\mathbf{e}_y \end{bmatrix}\]

Угловая скорость и ускорение второго тела

Угловая скорость тела 2 определяется угловой скоростью тела 1 (\(\vec{\omega}_1\)) и угловой скоростью тела 2 относительно тела 1 (\(\vec{\Omega}_2\)): \[\vec{\omega}_2 = \vec{\omega}_1 + \vec{\Omega}_2.\]

В матричной координатной форме это выражение имеет вид (в системе координат тела 2): \[\boldsymbol{\omega}_2^{(2)}=\left[\left(\mathbf{A}^{12}\right)^T\mathbf{E}_\mathbf{3}\ \ \ \mathbf{e}_y\right]\dot{\mathbf{u}}=\mathbf{Q}_{w2}^{(2)}\mathbf{u}\]

Координатный столбец углового ускорения тела 2 определим дифференцированием выражения для \(\boldsymbol{\omega}_2^{(2)}\): \[\boldsymbol{\varepsilon}_2^{(2)}=\mathbf{Q}_{w2}^{(2)}\dot{\mathbf{u}}+{\dot{\mathbf{Q}}}_{w2}^{(2)}\mathbf{u}\]

где \[{\dot{\mathbf{Q}}}_{w2}^{(2)}=\left[\left(\mathbf{A}^{12}{\widetilde{\mathbf{\Omega}}}_2^{\left(2\right)}\right)^T\mathbf{E}_\mathbf{3}\ \ \ \mathbf{0}_3\right]\]

Силы

Движение механической системы происходит под действием силы тяжести, действующей отрицательном направлении оси \(Oz_0\). Координатные столбцы сил, действующих на тела 1 и 2 в неподвижном базисе: \[\mathbf{F}_1^{(0)} = [0,0,-m_1 g]^T, \quad \mathbf{F}_2^{(0)} = [0,0,-m_2 g]^T\]

Уравнения движения

Сила инерции первого тела определяется выражением: \[\boldsymbol{\Phi}_1 = - m_1 \mathbf{a}_1 = - m_1 [ \mathbf{Q}_{V1}^{(0)} \dot{\mathbf{u}} + \dot{\mathbf{Q}}_{V1}^{(0)} \mathbf{u} ]\]

Это выражение необходимо умножить скалярно на соответствующие частные скорости центра масс первого тела. Учитывая, что необходимые частные скорости это столбцы матрицы \(\mathbf{Q}_{V1}^{(0)}\), скалярное произведение можно записать в матричном виде следующим образом \[\mathbf{Q}_{v1} = \begin{bmatrix} \mathbf{u}_{11} \cdot \boldsymbol{\Phi}_1 \\ \mathbf{u}_{12} \cdot \boldsymbol{\Phi}_1 \\ \mathbf{u}_{13} \cdot \boldsymbol{\Phi}_1 \\ \mathbf{u}_{14} \cdot \boldsymbol{\Phi}_1 \\ \end{bmatrix} = (\mathbf{Q}_{V1}^{(0)})^T \left\{- m_1 \left( \mathbf{Q}_{V1}^{(0)} \dot{\mathbf{u}} + \dot{\mathbf{Q}}_{V1}^{(0)} \mathbf{u} \right) \right\}\]

Главный момент сил инерции тела 1 \[\mathbf M_{c1}^{\Phi} = - \mathbf{J}_1 \boldsymbol{\varepsilon}_1^{(1)} - \boldsymbol{\omega_1}^{(1)} \times \mathbf{J}_1 \boldsymbol{\omega}_1^{(1)}\]

скалярно умножается на частные угловые скорости тела 1: \[\mathbf{Q}_{w1} = \begin{bmatrix} \mathbf{w}_{11} \cdot \mathbf M_{c1}^{\Phi} \\ \mathbf{w}_{12} \cdot \mathbf M_{c1}^{\Phi} \\ \mathbf{w}_{13} \cdot \mathbf M_{c1}^{\Phi} \\ \mathbf{w}_{14} \cdot \mathbf M_{c1}^{\Phi} \\ \end{bmatrix} = (\mathbf{Q}_{w1}^{(1)})^T \left[- \mathbf{J}_1 (\mathbf{Q}_{w1}^{(1)} \dot{\mathbf{u}} + \dot{\mathbf{Q}}_{w1}^{(1)} \mathbf{u}) - \boldsymbol{\omega_1}^{(1)} \times \mathbf{J}_1 \boldsymbol{\omega}_1^{(1)} \right]\]

Аналогичные выражения определяются для тела 2 \[\mathbf{Q}_{v2} = \begin{bmatrix} \mathbf{u}_{21} \cdot \boldsymbol{\Phi}_2 \\ \mathbf{u}_{22} \cdot \boldsymbol{\Phi}_2 \\ \mathbf{u}_{23} \cdot \boldsymbol{\Phi}_2 \\ \mathbf{u}_{24} \cdot \boldsymbol{\Phi}_2 \\ \end{bmatrix} = (\mathbf{Q}_{V2}^{(0)})^T \left\{- m_2 \left( \mathbf{Q}_{V2}^{(0)} \dot{\mathbf{u}} + \dot{\mathbf{Q}}_{V2}^{(0)} \mathbf{u} \right) \right\}\] \[\mathbf{Q}_{w2} = \begin{bmatrix} \mathbf{w}_{21} \cdot \mathbf M_{c1}^{\Phi} \\ \mathbf{w}_{22} \cdot \mathbf M_{c1}^{\Phi} \\ \mathbf{w}_{23} \cdot \mathbf M_{c1}^{\Phi} \\ \mathbf{w}_{24} \cdot \mathbf M_{c1}^{\Phi} \\ \end{bmatrix} = (\mathbf{Q}_{w2}^{(2)})^T \left[- \mathbf{J}_2 (\mathbf{Q}_{w2}^{(2)} \dot{\mathbf{u}} + \dot{\mathbf{Q}}_{w2}^{(2)} \mathbf{u}) - \boldsymbol{\omega}_2^{(2)} \times \mathbf{J}_2 \boldsymbol{\omega}_2^{(2)} \right]\]

Далее определяются обобщенные силы. Поскольку силы приложены в центрах масс тел, векторы этих сил скалярно умножаются на соответствующие частные линейные скорости центров масс: \[\mathbf{F}_{v1} = (\mathbf{Q}_{V1}^{(0)})^T \mathbf{F}_1\] \[\mathbf{F}_{v2} = (\mathbf{Q}_{V2}^{(0)})^T \mathbf{F}_2\]

Сложив полученные обобщённые силы \[\mathbf Q_{v1} + \mathbf Q_{v2} + \mathbf Q_{w1} + \mathbf Q_{w2} +\mathbf Q_{v1} + \mathbf{F}_{v1} + \mathbf{F}_{v2} = 0,\]

и выделив слагаемые с производной координатного столбца скорости, получим систему дифференциальных уравнений в матричной форме относительно \(\dot{\mathbf{u}}\): \[\mathbf{Q}_{v1} + \mathbf{Q}_{v2} + \mathbf{Q}_{w1} + \mathbf{Q}_{w1} + \mathbf{F}_{v1} + \mathbf{F}_{v2} = 0\] \[\begin{aligned} & \sum_{i=1}^{2}{\left\{-m_i\left[\mathbf{Q}_{Vi}^{\left(0\right)}\right]^T\mathbf{Q}_{Vi}^{\left(0\right)}-\left[\mathbf{Q}_{wi}^{\left(i\right)}\right]^T\mathbf{J}_i^{\left(i\right)}\mathbf{Q}_{wi}^{\left(i\right)}\right\}\dot{\mathbf{u}}} = \\ & = \sum_{i=1}^{2}{m_i\left[\mathbf{Q}_{Vi}^{\left(0\right)}\right]^T{\dot{\mathbf{Q}}}_{Vi}^{\left(0\right)}\mathbf{u}} + \sum_{i=1}^{2}{\left[\mathbf{Q}_{wi}^{\left(i\right)}\right]^T\left[\mathbf{J}_i^{\left(i\right)}{\dot{\mathbf{Q}}}_{wi}^{\left(i\right)}\mathbf{u}+{\widetilde{\mathbf{\omega}}}_i^{\left(i\right)}\mathbf{J}_i^{\left(i\right)}\mathbf{\omega}_i^{\left(i\right)}\right]\ } - \sum_{i=1}^{2}{\left(\mathbf{Q}_{Vi}^{\left(0\right)}\right)^T\mathbf{F}_i} \end{aligned}\]

Эти уравнения интегрируются совместно кинематическими уравнениями, связывающими производные углов с угловыми скоростями: \[\dot{\alpha}_1 = \sec \alpha_2 (\omega_{1x}^{(1)} \cos \alpha_3 - \omega_{1y}^{(1)} \sin \alpha_3),\] \[\dot{\alpha}_2 = \omega_{1x}^{(1)} \sin \alpha_3 + \omega_{1y}^{(1)} \cos \alpha_3,\] \[\dot{\alpha}_3 = \tan \alpha_2 (\omega_{1y}^{(1)} \sin \alpha_3 - \omega_{1x}^{(1)} \cos \alpha_3) + \omega_{1z}^{(1)}.\]

MATLAB-код

Главный файл-скрипт

clear all; clc;

% Матрицы тензоров инерции тел в главных центральных осях
p.J1 = eye(3)*10;
p.J2 = eye(3)*10;
% Шарнирные векторы
p.c11 = [0;0;+1];
p.c12 = [0;0;-1];
p.c22 = [0;0;+1];
% Массы тел
p.m1 = 1;
p.m2 = 1;
% Ускорение свободного падения
p.g = 9.807;

% Начальные условия
q0 = [pi/4;pi/4;pi/4;pi/4;  0;0;0;0];

% Относительная погрешность
opt = odeset('RelTol',1e-7);
% Запуск процесса интегрирования
[t,q] = ode113(@(t,q) dqdt_kane(t,q,p), [0,10], q0,opt);

% Графики
close all;
figure;
plot(t,q(:,1:4)*180/pi);
legend('\alpha_1','\alpha_2','\alpha_3','\alpha_4');
xlabel('t, c'); 
ylabel('\alpha_i, градус');

figure;
plot(t,q(:,5:8)*180/pi);
legend('\omega_{1x}','\omega_{1y}','\omega_{1z}','d\alpha_4/dt');
xlabel('t, c'); 
ylabel('Угловые скорости, ...^o/c');

Функция правых частей

function dq = dqdt_kane(t,q,p)

dq = zeros(8,1);

% Матрицы элементарных поворотов
Ax = @(x) [1 0 0; 0 cos(x) -sin(x); 0 sin(x) cos(x)];
Ay = @(x) [cos(x) 0 sin(x); 0 1 0; -sin(x) 0 cos(x)];
Az = @(x) [cos(x) -sin(x) 0; sin(x) cos(x) 0; 0 0 1];
% Единичные векторы осей
ex = [1;0;0];
ey = [0;1;0];
ez = [0;0;1];
e0 = [0;0;0];

% Матрица-столбец скоростей
u  = q(5:8);

% Матрица преобразования координат из 1 в 0
A01 = Ax(q(1))*Ay(q(2))*Az(q(3));
% Матрица преобразования координат из 2 в 1
A12 = Ay(q(4));
% Матрица преобразования координат из 2 в 0
A02 = A01*A12;

% Угловая скорость тела 1
w1_1  = q(5:7);
% Относительная угловая скорость тела 2 относительно тела 1 в СК 2
Ow1_2 = ey*q(8);
% Абсолютная угловая скорость тела 2 в СК 2
w2_2  = A12'*w1_1+Ow1_2;

% Частные линейные скорости центра масс С1 в 0
Q1V_0  = [A01*tilde(p.c11) zeros(3,1)];
% Производная Q1V_0
dQ1V_0 = [A01*tilde(w1_1)*tilde(p.c11) zeros(3,1)];

% Частные линейные скорости центра масс С2 в 0
Q2V_0  = [-A01*(-tilde(p.c11)+tilde(p.c12)-tilde(A12*p.c22)) A01*A12*tilde(p.c22)*ey];
dQ2V_0 = [-A01*tilde(w1_1)*(tilde(-p.c11)+tilde(p.c12)-tilde(A12*p.c22))+A01*tilde(A12*tilde(Ow1_2)*p.c22),...
          A01*tilde(w1_1)*A12*tilde(p.c22)*ey+A01*A12*tilde(Ow1_2)*tilde(p.c22)*ey];

% Частные угловые скорости 1 в 0 не нужны 
Q1w_1  = [eye(3)   zeros(3,1)];
dQ1w_1 = [zeros(3) zeros(3,1)];

% Частные угловые скорости 2 в 0 не нужны 
Q2w_2  = [A12'*eye(3) ey];
dQ2w_2 = [(A12*tilde(Ow1_2))'*eye(3) zeros(3,1)];

% Силы тяжести
F1 = [0;0;-p.m1*p.g];
F2 = [0;0;-p.m2*p.g];

% Матрица масс
M = -p.m1*(Q1V_0')*Q1V_0-p.m2*(Q2V_0')*Q2V_0-(Q1w_1')*(p.J1*Q1w_1)-(Q2w_2')*p.J2*Q2w_2;

% Матрица правых частей
B = -(-p.m1*(Q1V_0')*dQ1V_0*u-(Q1w_1')*(p.J1*dQ1w_1*u+tilde(w1_1)*p.J1*w1_1)-...
       p.m2*(Q2V_0')*dQ2V_0*u-(Q2w_2')*(p.J2*dQ2w_2*u+tilde(w2_2)*p.J2*w2_2)+...
       (Q1V_0')*F1+(Q2V_0')*F2  );

% Решаем СЛУ относительно старших производных (угловых ускорений)
du = M\B;

% Кинематические уравнения
% Производные углов
dq(1) = sec(q(2))*(q(5)*cos(q(3))-q(6)*sin(q(3)));
dq(2) = q(5)*sin(q(3))+q(6)*cos(q(3));
dq(3) = tan(q(2))*(q(6)*sin(q(3))-q(5)*cos(q(3)))+q(7);
dq(4) = q(8);
% Угловые ускорения
dq(5:8) = du;

end