2.2 Комплекс програм моделювання з застосуванням кінематичних рівнянь Пуассона

Програмна модель, що реалізує кінематичні рівняння Пуассона (1.14) має наступний представлена на рис. 2.2

BSOSapeginRK_Poisson.m

Object.m

P2T3F1toC.m

RK22.m

RK32.m

RK42.m

RK42.m

Poisson.m

CtoP2T3F1.m

Рис. 2.2. Програмна модель кінематичні рівняння Пуассона

Програмна модель складається з наступних компонентів:

• Керувальна програма BSOSapeginRK_Poisson.m

• Функція Object.m, що визначає поточні параметри орієнтації об’єкта

• Функції RK22.m, RK32.m, RK42.m, що реалізують відповідні методи чисельного інтегрування

• Функція Poisson.m обчислює праві частини кінематичних рівнянь Пуассона

• Процедура P2T3F1toC.m складає матрицю направляючих косинусів С

• Процедура CtoP2T3F1.m розраховує значення кутів з елементів матриці С

Пояснимо взаємозв`язок та роботу кожного з компонентів.

Керувальна програма BSOSapeginRK_Poisson.m задає початкові умови і параметри моделі, реалізує процес чисельного інтегрування кінематичного рівнянняПуассона різними методами, розраховує точне значення кутів, розраховує похибки інтегрування та виводить результат у вигляді графічних залежностей від часу.

Текст керувальної програми BSOSapeginRK_Poisson подано нижче.

%Управляющая программа численного интегрирования уравнений движения

%Пуассона

clc, clear all

global P0 Pt0 Pm OmP Ep T0 Tt0 Tm OmT Et F0 Ft0 Fm OmF Ef

%Задание параметров углов

P0=0; Pt0=0; Pm=0.2; OmP=0.2*pi; Ep=0;

T0=0; Tt0=0; Tm=0; OmT=0; Et=0;

F0=0; Ft0=0; Fm=0.2; OmF=0.2*pi; Ef=pi/2;

%Параметры интегрирования

TK=100; k=1; t=0; h=0.01;

%Начальные значения углов

[Ug,PrUg,UgSk]=Object(0);

P=Ug(1); T=Ug(2); F=Ug(3);

y0=P2T3F1toC(P,T,F);

tspan=[0,TK];

y=y0;

y2=y0;

y3=y0;

ttt(k)=t;

%Численное интегрирования

while t<TK

[tout,yout]=RK22('Poisson',t,y,h);

[tout,y2out]=RK32('Poisson',t,y2,h);

[tout,y3out]=RK42('Poisson',t,y3,h);

k=k+1;

t=tout;

y=yout;

y2=y2out;

y3=y3out;

ttt(k)=t;

%Точные значения

Ug1=P0+Pt0*t+Pm*sin(OmP*t+Ep); %Пси

Ug2=T0+Tt0*t+Tm*sin(OmT*t+Et); %Тета

Ug3=F0+Ft0*t+Fm*sin(OmF*t+Ef); %Фи

%Значения после моделирования

[P,T,F]=CtoP2T3F1(y);

[P2,T2,F2]=CtoP2T3F1(y2);

[P3,T3,F3]=CtoP2T3F1(y3);

%Расчет ошибок

DP_22(k)=P-Ug1; DP_32(k)=P2-Ug1; DP_42(k)=P3-Ug1;

DT_22(k)=T-Ug2; DT_32(k)=T2-Ug2; DT_42(k)=T3-Ug2;

DF_22(k)=F-Ug3; DF_32(k)=F2-Ug3; DF_42(k)=F3-Ug3;

end

%Вывод результатов на графики

figure

subplot (3,4,[1,2,3]), plot (ttt,DP_22, '-*',ttt,DT_22,'o',ttt,DF_22, '.'),...

title('Модифікований метод Эйлера (2.2)','FontName','MS Sans Serif','FontSize',12'), xlabel('t'), xlabel('Час, с'), ylabel('Кут, рад'), grid,legend('\Delta\psi','\Delta\theta','\Delta\phi')

subplot (3,4,[5,6,7]), plot (ttt,DP_32, '-*',ttt,DT_32,'o',ttt,DF_32, '.'),...

title('Хойне (3.2)','FontName','MS Sans Serif','FontSize',12'), xlabel('t'), xlabel('Час, с'), ylabel('Кут, рад'), grid,legend('\Delta\psi','\Delta\theta','\Delta\phi')

subplot (3,4,[9,10,11]),plot(ttt,DP_42, '-*',ttt,DT_42,'o',ttt,DF_42, '.'),...

title('Рунге-Кутта (4.2)','FontName','MS Sans Serif','FontSize',12'), xlabel('t'), xlabel('Час, с'), ylabel('Кут, рад'), grid,legend('\Delta\psi','\Delta\theta','\Delta\phi')

subplot (3,4,[4,8,12]), axis off, text(-0.2, 1, 'Інтегрується рівняння Пуассона:','FontName','MS Sans Serif','FontSize',14')

text (0.2, 0.92, 'dC','FontName','MS Sans Serif','FontSize',12.5')

text (0.2, 0.91, '--- = C*(\omegax)','FontName','MS Sans Serif','FontSize',12.5)

text (0.2, 0.88,'dt','FontName','MS Sans Serif','FontSize',12.5')

subplot (3,4,[4,8,12]), axis off, text(-0.2, 0.8, 'Закони руху основи:','FontName','MS Sans Serif','FontSize',14')

text (-0.2, 0.75, '\psi=\psi_o+\psi_o\primet+\psi_msin(\omega_\psit+\epsilon_\psi)')

text (-0.2, 0.70, '\theta=\theta_o\theta_o\primet+\theta_msin(\omega_\thetat+\epsilon_\theta)')

text (-0.2, 0.65, '\phi=\phi_o+\phi\primet_o+\phi_msin(\omega_\phit+\epsilon_\phi)')

subplot (3,4,[4,8,12]), axis off, text(-0.2, 0.6, 'Початкові параметри:','FontName','MS Sans Serif','FontSize',14')

text (-0.2, 0.55, sprintf ('\\psi_o=%g', P0))

text (0.2, 0.55, sprintf ('\\psi\\prime_o=%g', Pt0))

text (0.6, 0.55, sprintf ('\\psi_m=%g', Pm))

text (-0.2, 0.50, sprintf ('\\omega_\\psi=%g', OmP))

text (0.2, 0.50, sprintf ('\\epsilon_\\psi=%g', Ep))

text (-0.2, 0.45, sprintf ('\\theta_o=%g',T0))

text (0.2, 0.45, sprintf ('\\theta\\prime_o=%g',Tt0))

text (0.6, 0.45, sprintf ('\\theta_m=%g', Tm))

text (-0.2, 0.40, sprintf ('\\omega_\\theta=%g', OmT))

text (0.2, 0.40, sprintf ('\\epsilon_\\theta=%g', Et))

text (-0.2, 0.35, sprintf ('\\phi_o=%g', F0))

text (0.2, 0.35, sprintf ('\\phi\\prime_o=%g',Ft0))

text (0.6, 0.35, sprintf ('\\phi_m=%g', Fm))

text (-0.2, 0.30, sprintf ('\\phi_\\theta=%g', OmF))

text (0.2, 0.30, sprintf ('\\phi_\\theta=%g', Ef))

text (-0.2, 0.25,'Крок інтегрування:','FontName','MS Sans Serif','FontSize',14')

text (-0.2, 0.20, sprintf ('h=%g', h))

text (-0.2, 0.15,'-------------------------------','FontName','MS Sans Serif','FontSize',14')

text (-0.2, 0.10, 'BSOSapegin_ Poison.m')

text (-0.2, 0.05, 'Сапегін О.М.')

text (-0.2, 0.00, 'Кафедра ПСОН, група ПГ-72')

tm=fix(clock); Tv=tm(4:5); h1=text(-0.2, -0.05, [sprintf(' %g:', Tv),' ',date]);

Функції Object та процедуриRK22, RK32, RK42 описані вище у пункті 2.1.

Однак у цьому випадку чисельне інтегрування проводиться з використанням рівняння Пуассона, що реалізоване у процедурі Poisson.

Її текст подано нижче.

%Уравнения движения Пуассона

function z=Poisson(t,y)

[Ug,PrUg,UgSk]=Object(t);

OM=[0,-UgSk(3),UgSk(2);UgSk(3),0,-UgSk(1);-UgSk(2),UgSk(1),0];

%Правые части Пуассона

z=y*OM;

Для реалізації рівняння Пуссона необхідно скласти із значень кутів, розрахованих у функції Object, матрицю направляючих косинусів С вигляду (1.7). Для цього використовується процедура P2T3F1toC.

Проте для подальшого аналізу похибок інтегрування необхідно зворотний розрахунок кутів орієнтації використовуючи матрицю С. У програмній моделі для цього використовується процедура CtoP2T3F1.

Тексти функцій P2T3F1toC та CtoP2T3F1 відповідно подані нижче.

function C=P2T3F1toC(P,T,F)

cp=cos(P); sp=sin(P);

ct=cos(T); st=sin(T);

cf=cos(F); sf=sin(F);

%Матрица направляющих косинусов

c11=ct*cp;

c12=sf*sp-st*cf*cp;

c13=cf*sp+st*sf*cp;

c21=st;

c22=ct*cf;

c23=-sf*ct;

c31=-sp*ct;

c32=sf*cp+st*cf*sp;

c33=cp*cf-st*sp*sf;

C=[c11 c12 c13; c21 c22 c23; c31 c32 c33];

%Нахождения углов из матрицы напр. косинусов

function [P,T,F]=CtoP2T3F1(C)

P=atan2(-C(3,1),C(1,1)); %Пси

T=asin(C(2,1)); %Тета

F=atan2(-C(2,3),C(2,2)); %Фи