List

Вариант реализации алгоритма метода отдельных тел на языке матричной алгебры MATLAB на примере модели тройного физического маятника, изображенного на рисунке. На каждое тело системы действует единичная сила веса, направленная “вниз” вдоль оси y неподвижной системы координат. Конфигурация системы задается тремя углами – обобщенными координатами: q_1, q_2, q_3.

Файл-скрипт start.m запускает процесс интегрирования и отображает результаты расчета: конфигурацию системы, графики изменения кинетической, потенциальной и полной энергии

% Интегрирование
% результат интегрирования:
% Y(:,1) столбец углов поворота 1 тела
% Y(:,2) столбец углов поворота 2 тела
% Y(:,3) столбец углов поворота 3 тела
% Y(:,4) столбец угл. скорости 1 тела
% Y(:,5) столбец угл. скорости 2 тела
% Y(:,6) столбец угл. скорости 3 тела
[t,Y] = ode113(’MethodOTODE’);
%% рисование конфигурации системы
subplot(1,2,1);
M = moviein(size(Y,1));
axis([-6 6 -6 6]);
hold on;
for i=1:size(Y,1)
p0=[0;0;0];
pA=[cos(Y(i,1));sin(Y(i,1));0]*2;
pB=pA+[cos(Y(i,2));sin(Y(i,2));0]*2;
pC=pB+[cos(Y(i,3));sin(Y(i,3));0]*2;
rod1=[p0’;pA’];
rod2=[pA’;pB’];
rod3=[pB’;pC’];
% line(x,y,z,opttions...)
line(rod1(:,1),rod1(:,2),rod1(:,3),’LineWidth’,4,’Color’,’Red’);
line(rod2(:,1),rod2(:,2),rod2(:,3),’LineWidth’,4,’Color’,’Green’);
line(rod3(:,1),rod3(:,2),rod3(:,3),’LineWidth’,4,’Color’,’Blue’);
M(:,i) = getframe;
cla;
end
% Вычисление скоростей тел
v12=Y(:,4).^2;
v22=4*Y(:,4).^2+4*cos(Y(:,1)-Y(:,2)).*Y(:,4).*Y(:,5)+Y(:,5).^2;
v32=(2*cos(Y(:,1)).*Y(:,4)+2*cos(Y(:,2)).*Y(:,5)+ ...
cos(Y(:,3)).*Y(:,6)).^2+(2*sin(Y(:,1)).*Y(:,4)+ ...
2*sin(Y(:,2)).*Y(:,5)+sin(Y(:,3)).*Y(:,6)).^2;
% кинетическая и потенциальная энергия
Ek=(v12+v22+v32)*0.5+0.5*(Y(:,4).*Y(:,4)+ ...
Y(:,5).*Y(:,5)+Y(:,6).*Y(:,6));
Ep=(sin(Y(:,1))+2*sin(Y(:,1))+sin(Y(:,2))+ ...
2*sin(Y(:,1))+2*sin(Y(:,2))+sin(Y(:,3)));
% Построение графика кинетической, потенциальной
% и полной энергии системы
subplot(1,2,2);
plot(t,[Ek,Ep,Ek+Ep]);

Файл-функция MethodOTODE.m формирует правую часть дифференциальных уравнений движения.

function [out1,out2,out3] = MethodOTODE(t,y,flag)
global n;
global q;
global dq;
global Mkast;
global Qkast;
if nargin < 3 | isempty(flag)
q{1}=y(1); % угол поворота тела 1
q{2}=y(2); % угол поворота тела 2
q{3}=y(3); % угол поворота тела 3
dq{1}=y(4);% угл. скорость тела 1
dq{2}=y(5);% угл. скорость тела 2
dq{3}=y(6);% угл. скорость тела 3
% предварительное вычисление
% матриц Mk* Qk*
for k=n:-1:1
Mkast{k}=GetMk(k);
Qkast{k}=GetQk(k);
end
e1=Getd2qk(1);
e2=Getd2qk(2);
e3=Getd2qk(3);
% заполняем массив производных y'
out1 = [dq{1};dq{2};dq{3};e1;e2;e3];
else
switch(flag)
% начальные условия
case 'init'
% объявляем матрицы, начальные условия, ...
init;
% время интегрирования от 0 до 5 с
out1 = [0 5];
% начальные условия
out2 = [q{1};q{2};q{3};dq{1};dq{2};dq{3}];
otherwise
error(['Unknown request ''' flag '''.']);
end
end

init.m

% Инициализация системы
% три тела
global n; n=3;
% массивы для хранения вычисленных значений Mk*, Qk*
global Qkast;Qkast=cell(n,1);
global Mkast;Mkast=cell(n,1);
% матрицы масс
global m; m=cell(n,1);
% [m 0 0; 0 m 0; 0 0 Jz]
m{1}=[1 0 0;0 1 0;0 0 1];
m{2}=[1 0 0;0 1 0;0 0 1];
m{3}=[1 0 0;0 1 0;0 0 1];
% массив обобщенных координат
global q; q=cell(n,1);
% начальные условия - углы
q{1}=0;q{2}=0;q{3}=0;
% массив производных обобщенных координат
global dq; dq=cell(n,1);
% начальные условия - угловые скорости
dq{1}=0;dq{2}=0;dq{3}=0;
% массив шарнирных векторов
global c; c=cell(n,n);
c{1,1}=[-1;0;0];c{1,2}=[1;0;0];
c{2,2}=[-1;0;0];c{2,3}=[1;0;0];
c{3,3}=[-1;0;0];
% массив сил-моментов
% [Fx;Fy,Mz]
global Q; Q=cell(n,1);
Q{1}=[0;-1;0];
Q{2}=[0;-1;0];
Q{3}=[0;-1;0];

Файл-функция GetMk.m вычисляет матрицы M*

function res=GetMk(k)
global n; % количество тел
global m; % матрицы масс тел
global Mkast;
if (k==n)
res=m{k};
else
Sn=GetSn(k+1);
Cn=GetCn(k+1);
Un=GetUn(k+1);
Mk=Mkast{k+1};
res=m{k}+Cn’*Mk*Cn-Cn’*Mk*Sn*inv(Un)*Sn’*Mk*Cn;
end

Файл-функция GetQk.m вычисляет матрицы Q∗

function res=GetQk(k)
global n;
global Q;
global Mkast;
if (k==n)
res=Q{k};
else
Sn=GetSn(k+1);
Cn=GetCn(k+1);
Un=GetUn(k+1);
Mk=Mkast{k+1};
Wprim=GetWprim(k+1);
Qn=GetQk(k+1);
res=Q{k}-Cn’*(Mk*(Sn*inv(Un)*Sn’*(Qn-Mk*Wprim)+Wprim)-Qn);
end

Файл-функция Getd2qk.m вычисляет матрицы значения qk''
function res=Getd2qk(k)

global Qkast;
global Mkast;
w=GetWk(k-1);
Uk=GetUn(k);
Qk=Qkast{k};
Mk=Mkast{k};
Ck=GetCn(k);
Sk=GetSn(k);
res=inv(Uk)*Sk’*(Qk-Mk*(Ck*w+GetWprim(k)));

Файл-функция GetCn.m вычисляет матрицу Cn , входящую в выражение
(3.174).

function res=GetCn(n)
global q;global c;
if (n==1)
res=zeros(3,3);
else
qn=q{n-1};
cn=c{n-1,n};
res=zeros(3,3);
res(1,1)=1;
res(2,2)=1;
res(1,3)=-sin(qn)*cn(1)-cos(qn)*cn(2);
res(2,3)=+cos(qn)*cn(1)-sin(qn)*cn(2);
end

Файл-функция GetSn.m вычисляет матрицу Sn
function res=GetSn(n)
global q;global c;
qn=q{n};
cn=c{n,n};
res=zeros(3,1);
res(1)=+cos(qn)*cn(2)+sin(qn)*cn(1);
res(2)=+sin(qn)*cn(2)-cos(qn)*cn(1);
res(3)=1;

Файл-функция GetUn.m вычисляет матрицу Un

function res=GetUn(n)
global Mkast;
Sn=GetSn(n);
Mk=Mkast{n};
res=Sn’*Mk*Sn;

Файл-функция GetSn.m вычисляет матрицу wn , входящую в выражение

function res=GetWprim(n)
global q;global dq;global c;
if (n==1)
q1=0;
dq1=0;
c1=[0;0;0];
else
q1=q{n-1};
dq1=dq{n-1};
c1=c{n-1,n};
end
q2=q{n};
dq2=dq{n};
c2=c{n,n};
cq1=cos(q1);cq2=cos(q2);
sq1=sin(q1);sq2=sin(q2);
res=zeros(3,1);
dq12=dq1*dq1;
dq22=dq2*dq2;
res(1)=dq12*(-cq1*c1(1)+sq1*c1(2))+dq22*(+cq2*c2(1)-sq2*c2(2));
res(2)=dq12*(-sq1*c1(1)-cq1*c1(2))+dq22*(+sq2*c2(1)+cq2*c2(2));
res(3)=0;

  Posts

July 14th, 2017

7-я Европейская конференция по космическому мусору

Презентация о проблеме космического мусора с обзором некоторых работ 7-ой Европейской конференции по космическому мусору в Германии. Презентация была представлена […]

June 30th, 2016

XII Summer Space School

In June 30 I took part in XII Summer Space School as a lecturer. The event was held at the Samara University. […]

March 5th, 2016

2nd IAA Latin American CubeSat Workshop

January 31st, 2016

Схема йо-йо: уменьшение угловой скорости КА при помощи двух грузов на тросе

В основе этого способа закон сохранения кинетического момента системы, который складывается из кинетического момента КА (произведение момента инерции на угловую […]

September 22nd, 2015

CubeSat SamSat-218 is ready for launch

July 6th, 2015

Построение в системе Mathematica модели движения двух тел, связанных пружиной

Построим уравнения движения механической системы с двумя степенями свободы, состоящей из двух тел, соединенных пружиной, при этом одно из тел […]

July 6th, 2015

Анимация в Mathematica

Рассмотрим простой пример построения в Математике анимации углового движения твердого тела, используя результаты расчётов, сохраненные в csv файле. Файл data.csv […]

July 6th, 2015

Алгоритм метода отдельных тел в MATLAB

Вариант реализации алгоритма метода отдельных тел на языке матричной алгебры MATLAB на примере модели тройного физического маятника, изображенного на рисунке. […]

April 10th, 2015

Simulation of the latching mechanism of a solar panel

Using the Mathematica function HeavisideTheta to simulate the latching mechanism of a solar panel. Link to the Mathematica file: Latch.nb