Дубинка (гиря) подброшенная в воздух. Решение дифференциальных уравнений в MATLAB / Habr

Пример решения системы дифференциальных уравнений (ДУ) в MATLAB адаптивным и не адаптивным методами.

В MATLAB встроено множество численных решателей с адаптивным шагом для решения жестких, нежестких и полностью неявных систем. С помощью Symbolic Math Toolbox можно сначала выводить системы ДУ, а затем тут же решать их численными методами.

Описание модели

Для примера решим систему ДУ, которая описывает систему из двух масс m1 и m2, которые жестко соединены невесомым стержнем длинной L.

Дубинка подбрасывается в воздух и двигается в вертикальной плоскости XY в поле тяжести. Угол θ показан на рисунке, координаты центра масс примем за (x,y).

Пусть параметры системы будут следующими:

Найдем систему ДУ относительно центра масс в терминах Лагранжевой механики

Запишем энергию вращения системы:

Энергию движения:

Потенциальную энергию:

J - момент инерции, M - масса дубинки, ω - угловая скорость, g - ускорение свободного падения, h - высота.

syms Erot Emov J omega V M U g h T x(t) y(t) theta(t) l % зададим символьные переменные
eqnErot = Erot == 1/2*J*omega^2; % энергия вращения
eqnEmov = Emov == 1/2*M*V^2; % энергия движения
eqnU = U == M*g*h; % потенциальная энергия
eqnV = V == sqrt((diff(x,t))^2+(diff(y,t))^2); % скорость

Из закона сохранения энергии найдем выражение L(t,q,q')

eqnT = T == rhs(eqnErot) + rhs(eqnEmov); 
eqnT = subs(eqnT,[omega V],[diff(theta,t) rhs(eqnV)]); % кинетическая энергия
eqnU = subs(eqnU,h,y); % потенциальная энергия
syms L
eqnL = L == T - U;
eqnL = subs(eqnL,[T U],[rhs(eqnT) rhs(eqnU)]) % найдем функцию Лагранжа

Найдем уравнения движения дубинки подставив функцию Лагранжа в выражение (1):

eqns = functionalDerivative(-rhs(eqnL),[x y theta]) == [0;0;0] % минимизируем функционал
vars = [x(t); y(t); theta(t)]; % переменные

Решим систему уравнений

Для этого уменьшаем порядок системы до первого, переписываем систему в виде M(t,x(t))*x(t)'==F(t,x(t)) составляем пользовательскую функцию и решаем ее с помощью ode45.

[newEqs,newVars] = reduceDifferentialOrder(eqns,vars); % найдем эквивалентную систему первого порядка
[Mass,f] = massMatrixForm(newEqs,newVars); % найдем массовую матрицу для системы вида M(t,x(t))*x(t)'==F(t,x(t)) 
mass = odeFunction(Mass, newVars,J,M); % конвертируем массовую матрицу в пользовательскую функцию
func = odeFunction(f,newVars,l,g,M); % запишем пользовательскую функцию для F(t,x(t))
% зададим параметры
init_cond % инициализация начальных условий и параметров
y0est = [x0; y0; theta0; vx0; vy0; omega]; % вектор начальных состояний
tend = 3; % конец промежутка времени
opts = odeset('Mass',@(t,q) mass(t,q,Ixx,Msum)); % настройки решателя
Sol = ode45(@(t,q) func(t,q,l,g,Msum),[0 tend],y0est,opts); % решаем систему численно
step = 0.1; % шаг по времени
t = 0:step:tend; % промежуток времени
X = deval(Sol,t,1); % решение для x(t)
Y = deval(Sol,t,2); % решение для y(t)
Th = deval(Sol,t,3);% решение для theta(t)

Результаты

В результате построим графики движения центра масс (ЦМ) дубинки в пространстве и изменение угла θ от времени.

Построим анимацию

Решение системы ДУ с помощью не адаптивного решателя

Иногда при решении систем ДУ решателями с адаптивным шагом возникают ошибки, связанные с достижением решателем слишком маленького шага интегрирования. В такие моменты следует поменять решатель, либо уменьшить точность интегрирования, а если это не помогает, остается только выбрать другой промежуток интегрирования, что приводит к кусочному решению. К сожалению, в пакете MATLAB нет функций численного решения ДУ с фиксированным шагом (есть только в Simulink), по типу метода Руге-Кутты.

Для решения ДУ с фиксированным шагом придется самому написать функцию, либо скачать уже готовый скрипт с форума MATLAB, например тут.

Продемонстрируем решение системы ДУ в MATLAB с помощью метода Дормана-Принса.

Составим и решим систему ДУ относительно координат массы 1 (x1,y1) и массы 2 (x2,y2).

Найдем систему ДУ в формулировках Лагранжевой механики:

Запишем выражение для кинетической энергии первой и второй массы:

и выражения для потенциальной энергии первой и второй массы:

Запишем выражения для скоростей первой и второй массы:

Ограничивающее выражение:

где l – длинна дубинки.

syms E1 E2 U1 U2 M V1 V2 g h1 h2 m1 m2
eqnE1 = E1 == 1/2*m1*V1^2; % кинетическая энергия массы 1
eqnE2 = E2 == 1/2*m2*V2^2; % кинетическая энергия массы 2
eqnU1 = U1 == m1*g*h1; % потенциальная энергия массы 1
eqnU2 = U2 == m2*g*h2; % потенциальная энергия массы 2
syms T U C x1(t) y1(t) x2(t) y2(t) u(t) l
eqnV1 = V1 == sqrt((diff(x1,t))^2+(diff(y1,t))^2); % скорость массы 1
eqnV2 = V2 == sqrt((diff(x2,t))^2+(diff(y2,t))^2); % скорость массы 2
eqnC = C == (m1+m2)*u*((x2-x1)^2+(y2-y1)^2-l^2); % выражение для функции Лагранжа
eqnT = T == rhs(eqnE1) + rhs(eqnE2);
eqnT = subs(eqnT,[V1 V2],[rhs(eqnV1) rhs(eqnV2)]); % итоговая кинетическая энергия системы
eqnU = U == rhs(eqnU1) + rhs(eqnU2);
eqnU = subs(eqnU,[h1 h2],[y1 y2]); % итоговая потенциальная энергия системы

Найдем выражение L(t,q,q')

syms L
eqnL = L == T - U + C;
eqnL = subs(eqnL,[T U C],[rhs(eqnT) rhs(eqnU) rhs(eqnC)])  % функция Лагранжа

Найдем уравнения движения дубинки подставив функцию Лагранжа в выражение (1):

eqns = functionalDerivative(-rhs(eqnL),[x1 y1 x2 y2]); % минимизируем функционал
eqn5 = (x2-x1)^2 + (y2-y1)^2; % выражение ограничивающее расстояние между массами
eqns = [eqns; eqn5] == [0; 0; 0; 0; l^2] % итоговая система
vars = [x1(t); y1(t); x2(t); y2(t); u(t)]; % переменные

Решим систему уравнений

Для решения ДУ очень важно найти правильную пользовательскую функцию, пригодную для выбранного решателя, а также начальные условия.

Чтобы составить пользовательскую функцию, произведем следующий алгоритм:

[eqnsR,varsR] = reduceDifferentialOrder(eqns,vars); % найдем эквивалентную систему первого порядка
[ODEs,constraints] = reduceDAEToODE(eqnsR,varsR); % конвертируем систему первого порядка в такую систему, чтобы Якобиан был обратным
[massM,f] = massMatrixForm(ODEs,varsR); % найдем массовую матрицу M и систему F, такие чтобы выполнялось условие: M(t,x(t))*x(t)'==F(t,x(t))
Lvars = length(varsR); % сосчитаем количество неизвестных 
F = massM\f; % итоговая функция вида x(t)'== M(t,x(t))/F(t,x(t))
Mode = odeFunction(massM, varsR, m1, m2); % пользовательская функция для массовой матрицы
Fode = odeFunction(F, varsR, g, m1, m2); % пользовательская функция системы

Подставим численные значения в пользовательские функции:

init_cond % загрузим начальные условия
ODEsNumeric = subs(ODEs); % подставим численные значения в систему первого порядка
constraintsNumeric = subs(constraints); % подставим численные значения в ограничения
Mode = @(t,Y) Mode(t,Y,m1,m2); % пользовательская функция для массовой матрицы с численными значениями
Fode = @(t,Y) Fode(t,Y,g,m1,m2); % пользовательская функция системы с численными значениями

Найдем начальные условия для системы первого порядка:

L_CM = (m1*0+m2*l)/Msum; % центр масс дубинки
x01 = 1;        y01 = 1;        % начальное положение массы 1
x02 = x01+l;    y02 = y01;      % начальное положение массы 2 
vx01 = vx0;   vy01 = -omega*L_CM+vy0; % начальная скорость массы 1
vx02 = vx0;   vy02 = omega*(l-L_CM)+vy0; % начальная скорость массы 2
y0est = [x01; y01; x02; y02; 0; vx01; vy01; vx02; vy02]; % вектор начальных значений
yp0est = zeros(Lvars,1); % предположение о начальных значениях производных 
t0 = 0;
opt1 = odeset('Mass', Mode); % настройки поиска начальных условий
[y0, yp0] = decic(ODEsNumeric, varsR, constraintsNumeric, t0,...
                y0est, [1,1,1,0,0,1,1,0,1], yp0est, opt1); % поиск начальных условий и соответствующих производных

Зададим промежуток времени и решим систему решателем с фиксированным шагом ode5 (метод Дормана-Принса)

t = t0:0.01:tend; % промежуток времени
Sol = ode5(Fode,t,y0); % решение
X1 = Sol(:,1); % координаты X массы 1
Y1 = Sol(:,2); % координаты Y массы 1
X2 = Sol(:,3); % координаты X массы 2
Y2 = Sol(:,4); % координаты Y массы 2
Th = unwrap(atan2((Y2-Y1),(X2-X1))); % угол вращения дубинки
X = X1+L_CM*cos(Th); % координата X ЦМ
Y = Y1+L_CM*sin(Th); % координата Y ЦМ

Результаты

Построим такие же графики движения центра масс (ЦМ) дубинки в пространстве и изменение угла θ от времени.

Построим анимацию

Как видно, решения первой системы ДУ адаптивным решателем ode45 и второй системы ДУ фиксированным ode5, довольно похожи. Значит мы скорее всего не ошиблись в выводе уравнений.

Для сравнения решим эту же систему решателем для жестких уравнений ode15s:

Fode = odeFunction(f, varsR, g, m1, m2); % пользовательская функция системы
opt2 = odeset(opt1,'InitialSlope',yp0); % настройки решателя
Sol = ode15s(Fode, [t0 tend], y0, opt1); % решение

Сравним результаты решения адаптивного ode15s и не адаптивного ode5 решателей, найдем абсолютное значение ошибки между решениями:

Как можно видеть на графиках, квадрат ошибки порядка 10^-5, что немного, но особенно выводов тут не сделать. Плюсы и минусы адаптивных и не адаптивных решателей известны.

Если ни один адаптивный решатель не справляется с решением задачи, всегда поможет старый добрый метод Рунге-Кутты, главное верно задать пользовательскую функцию, пригодную для решателя.

Список источников: Solve Equations of Motion for Baton Thrown into Air

Ссылка на все файлы