ErikAntsupovControlTheory 25 янв в 16:02

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

Простой

6 мин

4.2K

Туториал

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

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

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

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

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

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

$\begin{array}{l} x_0 =1\;-\;\mathrm{начальное}\mathrm{положение}\mathrm{по}\;x\;\left(m\right)\\ y_0 =1\;-\;\mathrm{начальное}\mathrm{положение}\mathrm{по}\;y\;\left(m\right)\\ \theta_0 =0\;-\;\mathrm{начальное}\mathrm{положение}\mathrm{по}\mathrm{углу}\;\left(\mathrm{rad}\right)\\ V_{\mathrm{x0}} =5\;-\;\mathrm{начальная}\;\mathrm{скорость}\;\mathrm{по}\;x\;\left(m/s\right)\\ V_{\mathrm{y0}} =15\;-\;\mathrm{начальная}\;\mathrm{скорость}\;\mathrm{по}\;y\;\left(m/s\right)\\ \omega_0 =4\;-\;\mathrm{начальная}\;\mathrm{угловая}\;\mathrm{скорость}\;\left(\mathrm{rad}/s\right)\\ t_{\mathrm{end}} =3-\mathrm{конец}\;\mathrm{промежутка}\;\mathrm{времени}\;\left(s\right) \end{array}$

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

$\begin{array}{l} \frac{\mathrm{d}}{\mathrm{d}t}\frac{\mathrm{d}}{\mathrm{d}q^{\prime } }L\left(t,q,q^{\prime } \right)-\frac{\mathrm{d}}{\mathrm{d}q}L\left(t,q,q^{\prime } \right)=0\;\;\;\left(1\right)\\ \mathrm{где}\;q=\;\left(x,y\right)\ldotp \end{array}$

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

$\mathrm{E_{rot}}=\frac{J\,\omega ^2}{2}$

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

$\mathrm{E_{mov}}=\frac{M\,V^2}{2}$

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

$U=M\,g\,h$ $где: q' = V=\sqrt{{\left(\frac{\partial }{\partial t} x\left(t\right)\right)}^2+{\left(\frac{\partial }{\partial t} y\left(t\right)\right)}^2} - скорость\,дубинки,$

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)]) % найдем функцию Лагранжа

$L=\frac{M\,{\left({{\left(\frac{\partial }{\partial t}\;x\left(t\right)\right)}}^2 +{{\left(\frac{\partial }{\partial t}\;y\left(t\right)\right)}}^2 \right)}}{2}+\frac{J\,{{\left(\frac{\partial }{\partial t}\;\theta \left(t\right)\right)}}^2 }{2}-M\,g\,y\left(t\right)$

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

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

$\left(\begin{array}{c} M\,\frac{\partial^2 }{\partial t^2 }\;x\left(t\right)=0\\ M\,\frac{\partial^2 }{\partial t^2 }\;y\left(t\right)+M\,g=0\\ J\,\frac{\partial^2 }{\partial t^2 }\;\theta \left(t\right)=0 \end{array}\right)$

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

Для этого уменьшаем порядок системы до первого, переписываем систему в виде 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).

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

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

$E_{1}=\frac{{V_{1}}^2\,m_{1}}{2}, E_{2}=\frac{{V_{2}}^2\,m_{2}}{2};$

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

$U_{1}=g\,h_{1}\,m_{1}, U_{2}=g\,h_{2}\,m_{2}.$

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

$q_1' = V_1=\sqrt{{\left(\frac{\partial }{\partial t} x_1\left(t\right)\right)}^2+{\left(\frac{\partial }{\partial t} y_1\left(t\right)\right)}^2} q_2' = V_2=\sqrt{{\left(\frac{\partial }{\partial t} x_2\left(t\right)\right)}^2+{\left(\frac{\partial }{\partial t} y_2\left(t\right)\right)}^2}.$

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

$C=u\left(t\right)\,\left(m_{1}+m_{2}\right)\,\left({\left(x_{1}\left(t\right)-x_{2}\left(t\right)\right)}^2-l^2+{\left(y_{1}\left(t\right)-y_{2}\left(t\right)\right)}^2\right);$

где 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)])  % функция Лагранжа

$L=\frac{m_1 \,{\left({{\left(\frac{\partial }{\partial t}\;x_1 \left(t\right)\right)}}^2 +{{\left(\frac{\partial }{\partial t}\;y_1 \left(t\right)\right)}}^2 \right)}}{2}+\frac{m_2 \,{\left({{\left(\frac{\partial }{\partial t}\;x_2 \left(t\right)\right)}}^2 +{{\left(\frac{\partial }{\partial t}\;y_2 \left(t\right)\right)}}^2 \right)}}{2}-g\,m_1 \,y_1 \left(t\right)-g\,m_2 \,y_2 \left(t\right)+u\left(t\right)\,{\left(m_1 +m_2 \right)}\,{\left({{\left(x_1 \left(t\right)-x_2 \left(t\right)\right)}}^2 -l^2 +{{\left(y_1 \left(t\right)-y_2 \left(t\right)\right)}}^2 \right)}$

Найдем уравнения движения дубинки подставив функцию Лагранжа в выражение (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)]; % переменные

$\begin{array}{l} \left(\begin{array}{c} m_1 \,\frac{\partial^2 }{\partial t^2 }\;x_1 \left(t\right)-\sigma_8 +\sigma_7 -\sigma_6 +\sigma_5 =0\\ m_1 \,\frac{\partial^2 }{\partial t^2 }\;y_1 \left(t\right)+g\,m_1 -\sigma_4 +\sigma_3 -\sigma_2 +\sigma_1 =0\\ m_2 \,\frac{\partial^2 }{\partial t^2 }\;x_2 \left(t\right)+\sigma_8 -\sigma_7 +\sigma_6 -\sigma_5 =0\\ m_2 \,\frac{\partial^2 }{\partial t^2 }\;y_2 \left(t\right)+g\,m_2 +\sigma_4 -\sigma_3 +\sigma_2 -\sigma_1 =0\\ {{\left(x_1 \left(t\right)-x_2 \left(t\right)\right)}}^2 +{{\left(y_1 \left(t\right)-y_2 \left(t\right)\right)}}^2 =l^2 \end{array}\right)\\ \mathrm{}\\ \textrm{where}\\ \mathrm{}\\ \;\;\sigma_1 =2\,m_2 \,u\left(t\right)\,y_2 \left(t\right)\\ \mathrm{}\\ \;\;\sigma_2 =2\,m_2 \,u\left(t\right)\,y_1 \left(t\right)\\ \mathrm{}\\ \;\;\sigma_3 =2\,m_1 \,u\left(t\right)\,y_2 \left(t\right)\\ \mathrm{}\\ \;\;\sigma_4 =2\,m_1 \,u\left(t\right)\,y_1 \left(t\right)\\ \mathrm{}\\ \;\;\sigma_5 =2\,m_2 \,u\left(t\right)\,x_2 \left(t\right)\\ \mathrm{}\\ \;\;\sigma_6 =2\,m_2 \,u\left(t\right)\,x_1 \left(t\right)\\ \mathrm{}\\ \;\;\sigma_7 =2\,m_1 \,u\left(t\right)\,x_2 \left(t\right)\\ \mathrm{}\\ \;\;\sigma_8 =2\,m_1 \,u\left(t\right)\,x_1 \left(t\right) \end{array}$

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

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

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

[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

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

Теги:

Хабы:

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

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

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

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

Результаты

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

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

Результаты

Публикации

Истории

Ближайшие события