Интегрирование уравнений движения со связями
Рассмотрим процедуру формирования и интегрирования уравнений движения механической системы совместно с уравнениями связей на примере модели двойного физического маятника.
Механическая система состоит из двух однородных стержней, связанных цилиндрическими шарнирами. Известны масса \(m_1\) и длина первого стержня \(l_1\), а также масса \(m_2\) и длина второго стержня \(l_2\). Движение системы происходит в вертикальной плоскости под действием силы тяжести, действующей на каждый стержень.
Уравнения движения этой системы можно записать, используя формализм Лагранжа (уравнения Лагранжа второго рода). В результате могут быть получены два уравнения движения для двух обобщенных координат, например, для углов \(\varphi_1\) и \(\varphi_2\), показанных на рисунке, которые однозначно определяют положение рассматриваемой системы.
Уравнения движения можно записать, используя избыточный набор координат, количество которых превышает число степеней свободы. В этом случае уравнений будет больше, но они будут иметь более простую структуру. Для записи таких уравнений заменим действие связей в двух шарнирах силами реакции и запишем уравнения движения каждого стержня как свободного твердого тела, движущегося в плоскости.
Положение стержня \(i\) будет определяется тремя параметрами, определяющими положение центра масс стержня и угол наклона к оси x: \(x_i\), \(y_i\), \(\varphi_i\). Для каждого стержня необходимо записать 3 уравнения движения под действием активных сил и сил реакций отброшенных связей. В каждой шарнирной точке будет действовать сила реакции, неизвестная по направлению и модулю, которую удобнее разложить по осям неподвижной системы координат. Уравнения движения будут иметь вид: \[\left\{ \begin{aligned} m_1 \ddot x_1 & =F_{1x}-R_{1x}+R_{2x}, \\ m_1 \ddot y_1 & =F_{1y}-R_{1y}+R_{2y}, \\ J_{1z} \ddot \varphi_1 & =M_{1z}-\frac{l_1}{2} R_{1x} \sin \varphi_1 + \frac{l_1}{2} R_{1y} \cos \varphi_1 - \frac{l_2}{2} R_{2x} \sin \varphi_1 + \frac{l_2}{2} R_{2y} \cos \varphi_1, \\ m_2 \ddot x_2 & =F_{2x}-R_{2x}, \\ m_2 \ddot y_2 & =F_{2y}-R_{2y}, \\ J_{2z} \ddot \varphi_2 & =M_{2z}-R_{2x} \frac{l_2}{2} \sin \varphi_2 + R_{2y} \frac{l_2}{2} \cos \varphi_2 \\ \end{aligned}\right.\]
где \(J_{1z}\), \(J_{2z}\) – моменты инерции первого и второго стержня относительно поперечной оси, проходящей через центр масс, \(F_{1x}\), \(F_{1y}\), – проекции главного вектора внешних сил, действующих на стержень 1. В рассматриваемом примере \(F_{1x} = 0\), \(F_{1y} = -m_1 g\), Аналогично для второго тела \(F_{2x} = 0\), \(F_{2y} = -m_2 g\). Внешние моменты на тела также не действуют \(M_{1z} = 0\), \(M_{2z} = 0\).
В систему уравнений свободного движения стержней входят неизвестные проекции силы реакции связей \(R_{1x}\), \(R_{1y}\), \(R_{2x}\), \(R_{2y}\), величина которых должна быть такой, чтобы точка \(A_1\) первого стержня совпадала с началом координат, а точка \(A_2\) второго стержня в процессе движения совпадала с точкой \(A_1\) первого стержня. Необходимо дополнить систему уравнений движения уравнениями связей.
Первые два уравнения связей говорят о том, что точка \(А_1\), принадлежащая первому стержню, должна совпадать в процессе движения с точкой \(A_0\) неподвижной системы координат, которая имеет нулевые координаты: \[\left\{ \begin{aligned} \boxed{x_{A_1} = x_{A_0}} = x_1 - \frac{l_1}{2} \cos \varphi_1 & = 0 \\ \boxed{y_{A_1} = y_{A_0}} = y_1 - \frac{l_1}{2} \sin \varphi_1 & = 0 \\ \end{aligned}\right.\]
Третье и четвертое уравнение связей требует совпадения координат точек \(B_1\) и \(B_2\), принадлежащих первому и второму стержню соответственно: \[\left\{ \begin{aligned} \boxed{x_{B_1} = x_{B_2}} = x_1 + \frac{l_1}{2} \cos \varphi_1 & = x_2 - \frac{l_2}{2} \cos \varphi_2 & \\ \boxed{y_{B_1} = y_{B_2}} = y_1 + \frac{l_1}{2} \sin \varphi_1 & = y_2 - \frac{l_2}{2} \sin \varphi_2 & \\ \end{aligned}\right.\]
Система 6 дифференциальных уравнений движения и 4 уравнений связей образуют систему дифференциально-алгебраических уравнений. Для решения этой системы дважды продифференцируем уравнения связей. В результате первого дифференцирования четырёх уравнений связей получим \[\left\{ \begin{aligned} \dot{x}_1 + \dot{\varphi}_1 \frac{l_1}{2} \sin \varphi_1 & = 0 \\ \dot{y}_1 - \dot{\varphi}_1 \frac{l_1}{2} \cos \varphi_1 & = 0 \\ \dot{x}_1 - \dot{\varphi}_1 \frac{l_1}{2} \sin \varphi_1 & = \dot{x}_2 + \dot{\varphi}_2 \frac{l_2}{2} \sin \varphi_2 & \\ \dot{y}_1 + \dot{\varphi}_1 \frac{l_1}{2} \cos \varphi_1 & = \dot{y}_2 - \dot{\varphi}_2 \frac{l_2}{2} \cos \varphi_2 & \\ \end{aligned}\right.\]
Первые два уравнения требуют совпадения уже не координат, а скоростей точек \(A_1\) и \(A_0\). Также последние два уравнения требуют совпадения скоростей точек \(B_1\) и \(B_2\). Продифференцируем уравнения связи еще раз и получим уравнения связей, которые требуют совпадения ускорений точек \(A_1\), \(A_0\) и \(B_1\), \(B_2\): \[\left\{ \begin{aligned} \ddot{x}_1 + \ddot{\varphi}_1 \frac{l_1}{2} \sin \varphi_1 + \dot{\varphi}_1^2 \frac{l_1}{2} \cos \varphi_1 & = 0 \\ \ddot{y}_1 - \ddot{\varphi}_1 \frac{l_1}{2} \cos \varphi_1 + \dot{\varphi}_1^2 \frac{l_1}{2} \sin \varphi_1 & = 0 \\ \ddot{x}_1 - \ddot{\varphi}_1 \frac{l_1}{2} \sin \varphi_1 - \dot{\varphi}_1^2 \frac{l_1}{2} \cos \varphi_1 & = \ddot{x}_2 + \ddot{\varphi}_2 \frac{l_2}{2} \sin \varphi_2 + \dot{\varphi}_2^2 \frac{l_2}{2} \cos \varphi_2 & \\ \ddot{y}_1 + \ddot{\varphi}_1 \frac{l_1}{2} \cos \varphi_1 - \dot{\varphi}_1^2 \frac{l_1}{2} \sin \varphi_1 & = \ddot{y}_2 - \ddot{\varphi}_2 \frac{l_2}{2} \cos \varphi_2 + \dot{\varphi}_2^2 \frac{l_2}{2} \sin \varphi_2 & \\ \end{aligned}\right.\]
Систему дифференциально-алгебраических уравнений с дважды продифференцированными уравнениями связей можно представить как систему линейный уравнений относительно ускорений и сил реакции. Запишем эту систему уравнений в матричной форме \[\begin{bmatrix} m_1 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & -1 & 0 \\ 0 & m_1 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & -1 \\ 0 & 0 & J_1 & 0 & 0 & 0 & \frac{l_1}{2} \sin \varphi_1 & -\frac{l_1}{2} \cos \varphi_1 & \frac{l_1}{2} \sin \varphi_1 & - \frac{l_1}{2} \cos \varphi_1 \\ 0 & 0 & 0 & m_2 & 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & m_2 & 0 & 0 & 0 & 0 & 1 \\ 0 & 0 & 0 & 0 & 0 & J_2 & 0 & 0 & \frac{l_2}{2} \sin \varphi_2 & - \frac{l_2}{2} \cos \varphi_2 \\ 1 & 0 & \frac{l_1}{2} \sin \varphi_1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & -\frac{l_1}{2} \cos \varphi_1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 1 & 0 & - \frac{l_1}{2} \sin \varphi_1 & -1 & 0 & -\frac{l_2}{2} \sin \varphi_2 & 0 & 0 & 0 & 0 \\ 0 & 1 & \frac{l_1}{2} \cos \varphi_1 & 0 & -1 & \frac{l_2}{2} \cos \varphi_2 & 0 & 0 & 0 & 0 \\ \end{bmatrix} \begin{bmatrix} \ddot{x}_1 \\ \ddot{y}_1 \\ \ddot{\varphi}_1 \\ \ddot{x}_2 \\ \ddot{y}_2 \\ \ddot{\varphi}_2 \\ R_{1x} \\ R_{1y} \\ R_{2x} \\ R_{2y} \end{bmatrix} = \begin{bmatrix} 0 \\ -m_1 g \\ 0 \\ 0 \\ -m_2 g \\ 0 \\ - \dot{\varphi}_1^2 \frac{l_1}{2} \cos \varphi_1 \\ - \dot{\varphi}_1^2 \frac{l_1}{2} \sin \varphi_1 \\ \dot{\varphi}_1^2 \frac{l_1}{2} \cos \varphi_1 + \dot{\varphi}_2^2 \frac{l_2}{2} \cos \varphi_2 \\ \dot{\varphi}_1^2 \frac{l_1}{2} \sin \varphi_1 + \dot{\varphi}_2^2 \frac{l_2}{2} \sin \varphi_2 \end{bmatrix}\]
Для записи уравнений движения системы в форме Коши необходимо решить эту систему, исключив из первых шести уравнений реакции связей. Аналитическое решение этой системы приведет к громоздким выражениям, поэтому целесообразно эту систему решать численным методом на каждом шаге численного интегрирования.
Начальные условия движения системы должны удовлетворять как исходным уравнениям связей, так и уравнениям связей после первого и второго дифференцирования. Учитывая, что интегрируется система уравнений движения с дважды продифференцированными уравнениями связей, в результате неизбежных погрешностей численного интегрирования исходные уравнения связей могут нарушаться.
Ниже приведен пример программы для моделирования движения двойного физического маятника в системе MATLAB.
Код программы на языке MATLAB
Главный файл-скрипт
% Основной исполняемый файл-скрипт
clear all; clc;
% масса стержня 1, его длина и момент инерции относительно оси Oz
p.m1 = 1;
p.L1 = 1;
p.J1 = (p.m1*p.L1^2)/12;
% масса стержня 2, его длина и момент инерции относительно оси Oz
p.m2 = 1;
p.L2 = 1;
p.J2 = (p.m2*p.L2^2)/12;
% Ускорение свободного падения
p.g = 9.807;
% Начальные условия движения
x10 = 0.5*p.L1; % начальное положение ЦМ стержня 1, координата x
y10 = 0; % начальное положение ЦМ стержня 1, координата y
f10 = 0; % начальное положение стержня 1, угол поворота
vx10 = 0; % начальное положение ЦМ стержня 1, скорость вдоль x
vy10 = 0; % начальное положение ЦМ стержня 1, скорость вдоль y
w10 = 0; % начальное положение стержня 1, угловая скорость
x20 = p.L1 + 0.5*p.L2; % начальное положение ЦМ стержня 2, координата x
y20 = 0; % начальное положение ЦМ стержня 2, координата y
f20 = 0; % начальное положение стержня 2, угол поворота
vx20 = 0; % начальное положение ЦМ стержня 2, скорость вдоль x
vy20 = 0; % начальное положение ЦМ стержня 2, скорость вдоль y
w20 = 0; % начальное положение стержня 2, угловая скорость
% Формируем столбец начальных условий
q0 = [x10;y10;f10;vx10;vy10;w10;x20;y20;f20;vx20;vy20;w20];
[t,q] = ode113(@(t,q) dqdt(t,q,p), 0:0.02:5, q0);
%% Графики изменения углов поворота
plot(t,q(:,3)*180/pi,'LineWidth',3)
hold on;
plot(t,q(:,9)*180/pi,'LineWidth',3)
hold off;
legend('\phi_1','\phi_2');
ylabel('Угол поворота, ...\circ');
%% Графики изменения угловых скоростей
plot(t,q(:,6)*180/pi,'LineWidth',3)
hold on;
plot(t,q(:,12)*180/pi,'LineWidth',3)
hold off;
legend('\omega_1','\omega_2');
xlabel('t, c');
ylabel('Угловая скорость, ...\circ/c');
%% Анимация движения системы
hold on;
axis([-2.0 , 2.0, -2.0, +0.5]);
for i=1:size(t)
cla;
xA = q(i,1)-p.L1*0.5*cos(q(i,3));
yA = q(i,2)-p.L1*0.5*sin(q(i,3));
xB1 = q(i,1)+p.L1*0.5*cos(q(i,3));
yB1 = q(i,2)+p.L1*0.5*sin(q(i,3));
xB2 = q(i,7)-p.L2*0.5*cos(q(i,9));
yB2 = q(i,8)-p.L2*0.5*sin(q(i,9));
xC2 = q(i,7)+p.L2*0.5*cos(q(i,9));
yC2 = q(i,8)+p.L2*0.5*sin(q(i,9));
line([xA xB1], [yA yB1] ,'LineWidth',6,'Color','Red');
line([xB2 xC2], [yB2 yC2],'LineWidth',6,'Color','Blue');
grid on;
getframe;
end
hold off;
Файл функция правых частей
function [dq, R] = dqdt(t,q,p)
% В этой функции вычисляются правые части дифференциальных уравнений
% движения.
%
% Входные переменные:
% t - текущее время
% q - столбец обобщенных координат и скоростей системы
% в том-же порядке, что и в матрице исходных данных
% в файле pendulum.m
% p - структура с параметрами системы
%
% Выходные:
% dq - производная столбца q
% R - столбец сил реакций
% Для наглядности обозначим элементы матрицы q собственными именами
% Координаты и скорости первого стержня
x1 = q(1); vx1 = q(4);
y1 = q(2); vy1 = q(5);
f1 = q(3); wz1 = q(6);
% Координаты и скорости второго стержня
x2 = q(7); vx2 = q(10);
y2 = q(8); vy2 = q(11);
f2 = q(9); wz2 = q(12);
% Формируем матрицу коэффициентов системы
A = [p.m1 0 0 0 0 0 1 0 -1 0 ;
0 p.m2 0 0 0 0 0 1 0 -1 ;
0 0 p.J1 0 0 0 p.L1*0.5*sin(f1) -p.L1*0.5*cos(f1) p.L1*0.5*sin(f1) -+p.L1*0.5*cos(f1);
0 0 0 p.m2 0 0 0 0 1 0;
0 0 0 0 p.m2 0 0 0 0 1;
0 0 0 0 0 p.J2 0 0 p.L2*0.5*sin(f2) -p.L2*0.5*cos(f2);
1 0 p.L1*0.5*sin(f1) 0 0 0 0 0 0 0;
0 1 -p.L1*0.5*cos(f1) 0 0 0 0 0 0 0;
1 0 -p.L1*0.5*sin(f1) -1 0 -p.L2*0.5*sin(f2) 0 0 0 0;
0 1 +p.L1*0.5*cos(f1) 0 -1 +p.L2*0.5*cos(f2) 0 0 0 0];
% Сила тяжести, действующая на первый и второй стержень (вдоль оси y)
F1y = -p.m1*p.g;
F2y = -p.m2*p.g;
% Столбец правых частей
B = [0; F1y; 0;
0; F2y; 0;
-p.L1*0.5*cos(f1)*wz1^2;
-p.L1*0.5*sin(f1)*wz1^2;
+p.L1*0.5*cos(f1)*wz1^2+p.L2*0.5*cos(f2)*wz2^2;
p.L1*0.5*sin(f1)*wz1^2+p.L2*0.5*sin(f2)*wz2^2];
% Решаем систему линейных уравнений и получаем ускорения и реакции связей
x = A\B;
% Реакции связей
R = x(7:10);
% Столбец результата функции (dq) представляет собой производную столбца q.
% Так первые три элемента столбца q есть координаты и угол поворота первого
% стержня, поэтому первые три элемента dq должны содержать проекции
% линейной скорости первого стержня и его угловой скорости вокруг оси z.
% Там где у столбца q расположены скорости, у столбца dq должны быть
% помещены ускорения, вычисленные на предыдущем этапе x=B/A.
% Можно написать проще, используя специальный синтаксис оператора (),
% возвращающего сразу несколько элементов матрицы:
dq = [q(4:6);x(1:3);q(10:12);x(4:6)];
