2. Опис програмних засобів
Головною задачею дослідження було визначення похибок інтегрування диференційних рівнянь руху БСО при використанні різних чисельних методів.
Для цього розраховувалась різниця між точними та про інтегрованими значеннями кутів.
Для реалізації математичних моделей БІНС була складена низка програм і підпрограм у програмному середовищі Matlab.
2.1 Комплекс програм моделювання з застосуванням кінематичних рівнянь Ейлера
Програмна модель, що реалізує кінематичні рівняння Ейлера (1.4) має наступний представлена на рис. 2.1
BSOSapeginRK_3met.m
Object.m
RK22.m
RK32.m
RK42.m
RK42.m
KUE.m
Рис. 2.1. Програмна моделькінематичні рівняння Ейлера
Програмна модель складається з наступних компонентів:
Керувальна програма BSOSapeginRK_3met.m
Функція Object.m, що визначає поточні параметри орієнтації об’єкта
Функції RK22.m, RK32.m, RK42.m, що реалізують відповідні методи чисельного інтегрування
Функція KUE.m обчислює праві частини кінематичних рівнянь Ейлера
Пояснимо взаємозв`язок та роботу кожного з компонентів.
Керувальна програма BSOSapeginRK_3met.m задає початкові умови і параметри моделі, реалізує процес чисельного інтегрування кінематичних рівнянь Ейлера різними методами, розрахунок точних значень кутів, та виводить результат у вигляді графічних залежностей похибок інтегрування від часу.
Текст програми BSOSapeginRK_3met.m наведений нижче.
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=2*pi; Ep=0;
T0=0; Tt0=0; Tm=0.; OmT=2*pi; Et=0;
F0=0; Ft0=0; Fm=0.2; OmF=2*pi; Ef=pi/2;
%Параметры интегрирования
TK=10; k=1; t=0; h=0.02;
%Начальные условия
[y0,PrUg,UgSk]=Object(t);
tspan=[0,TK];
y=y0;
y2=y0;
y3=y0;
X(k)=y(1);
Ps_22(k)=y(1); Ps_32(k)=y2(1); Ps_42(k)=y3(1);
Te_22(k)=y(2); Te_32(k)=y2(2); Te_42(k)=y3(2);
Fi_22(k)=y(3); Fi_32(k)=y2(3); Fi_42(k)=y3(3);
ttt(k)=t;
%Численное интегрирования при помощи метода Рунге-Кутта
while t<TK
[tout,yout]=RK22('KUE',t,y,h);
[tout,y2out]=RK32('KUE',t,y2,h);
[tout,y3out]=RK42('KUE',t,y3,h);
k=k+1;
t=tout;
y=yout;
y2=y2out;
y3=y3out;
ttt(k)=t;
Ps_22(k)=y(1); Ps_32(k)=y2(1); Ps_42(k)=y3(1);
Te_22(k)=y(2); Te_32(k)=y2(2); Te_42(k)=y3(2);
Fi_22(k)=y(3); Fi_32(k)=y2(3); Fi_42(k)=y3(3);
end
%Расчет ошибок
Ug1=P0+Pt0*ttt+Pm*sin(OmP*ttt+Ep); %Пси
Ug2=T0+Tt0*ttt+Tm*sin(OmT*ttt+Et); %Тета
Ug3=F0+Ft0*ttt+Fm*sin(OmF*ttt+Ef); %Фи
%Вывод результатов на графики
figure
subplot (3,4,[1,2,3]), plot (ttt,Ps_22-Ug1, '-*',ttt,Te_22-Ug2,ttt,Fi_22-Ug3, '.'),...
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,Ps_32-Ug1, '-*',ttt,Te_32-Ug2,ttt,Fi_32-Ug3, '.'),...
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,Ps_42-Ug1, '-*',ttt,Te_42-Ug2,ttt,Fi_42-Ug3, '.'),...
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.95, '\psi\prime=(\omega_ycos\phi-\omega_zsin\phi)/cos\theta')
text (-0.2, 0.90, '\theta\prime=\omega_ysin\phi-\omega_zcos\phi')
text (-0.2, 0.85, '\phi\prime=\omega_x-\psi\prime*sin\theta')
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, 'BSOSapeginRK_ 3met.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.mобчислює кути повороту основи, їх похідні за часом і проекції кутових швидкостей на вісі зв’язаної системи координат в заданий момент часу. При чому вони задаються за гармонічним законом у вигляді:
Текст функції Object.m наведено нижче.
%Функция вычисляющая углы, производные от углов и проэкции угловых
%скоростей
function [Ug,PrUg,UgSk]=Object(t)
global P0 Pt0 Pm OmP Ep T0 Tt0 Tm OmT Et F0 Ft0 Fm OmF Ef
%Углы
Ug(1)= P0+Pt0*t+Pm*sin(OmP*t+Ep); %Пси
Ug(2)= T0+Tt0*t+Tm*sin(OmT*t+Et); %Тета
Ug(3)= F0+Ft0*t+Fm*sin(OmF*t+Ef); %Фи
%Производные от углов
PrUg(1)=Pt0+Pm*OmP*cos(OmP*t+Ep);
PrUg(2)=Tt0+Tm*OmT*cos(OmT*t+Et);
PrUg(3)=Ft0+Fm*OmF*cos(OmF*t+Ef);
%Проекции угловых скоростей
ct=cos(Ug(2));
sf=sin(Ug(3));
cf=cos(Ug(3));
UgSk(1)=PrUg(3)+PrUg(1)*sin(Ug(2));
UgSk(2)=PrUg(1)*ct*cf+PrUg(2)*sf;
UgSk(3)=-PrUg(1)*ct*sf+PrUg(2)*cf;
Відповідно до отриманого завдання, чисельне інтегрування проводилося з використанням трьох різних методів інтегрування: модифікований метод Ейлера, метод Хойне та метод Рунге-Кутта. Для цього у програмному середовищі Matlab було складено відповідні функції RK22, RK32 і RK42.
Тексти функцій наведені нижче
function [tout,yout]=RK22(zpfun,t,y,h)
k1=feval(zpfun,t,y);
k2=feval(zpfun,t+h,y+h*k1);
F=(k1+k2)/2;
tout=t+h;
yout=y+h*F;
function [tout,yout]=RK32(zpfun,t,y,h)
k1=feval(zpfun,t,y);
k2=feval(zpfun,t+h/3,y+h*k1/3);
k3=feval(zpfun,t+2*h/3,y+2*h*k2/3);
F=(k1+3*k3)/4;
tout=t+h;
yout=y+h*F;
function [tout,yout]=RK42(zpfun,t,y,h)
k1=feval(zpfun,t,y);
k2=feval(zpfun,t+h/2,y+h*k1/2);
k3=feval(zpfun,t+h/2,y+h*k2/2);
k4=feval(zpfun,t+h,y+h*k3);
F=(k1+2*k2+2*k3+k4)/6;
tout=t+h;
yout=y+h*F;
Ці функції, в свою чергу, використовують підпрограму KUE.m для обчислення правих частин кінематичних рівнянь Ейлера (1.4).
Текст процедури KUE подано нижче.
function z=KUE(t,y)
[Ug,PrUg,UgSk]=Object(t);
Omx=UgSk(1);
Omy=UgSk(2);
Omz=UgSk(3);
%Правые части КУЭ
cf=cos(y(3)); sf=sin(y(3));
z(1)=(Omy*cf-Omz*sf)/cos(y(2));
z(2)=Omy*sf+Omz*cf;
z(3)=Omx-z(1)*sin(y(2));