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

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

BSOSapegin_Kvatern.m

Object

Quat3pov

Quat3pov

multQ

KvatKU

Napcos

CtoP2T3F1

Vect2KSM

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

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

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

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

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

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

• Процедура Qvat3pov.m визначає кінематику повороту системи координат зв’язаної з об’єктом відносно опорної

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

• Процедура multQ.m дозволяє здійснювати добуток кватерніонів

• Процедура KvatPov.m описує поворот відносно одної осі

• Процедура Kvat2Ug.m переводить значення кватерніонів у кути орієнтації

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

• Процедура Vect2KSM.m формує кососиметричну матрицю

Розглянемо роботу складеної програмної моделі.

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

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

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

%Кватернионах

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.8*pi; Ep=0;

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

F0=0; Ft0=0; Fm=0.2; OmF=0.8*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=Qvat3pov(P,T,F);

tspan=[0,TK];

y=y0;

y2=y0;

y3=y0;

ttt(k)=t;

% Численное интегрирования при помощи метода Рунге-Кутта

while t<TK

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

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

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

k=k+1;

t=tout;

y=yout;

y2=y2out;

y3=y3out;

ttt(k)=t;

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

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

Ug1=Ug(1); %Пси

Ug2=Ug(2); %Тета

Ug3=Ug(3); %Фи

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

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

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

[P3,T3,F3]=Kvat2Ug(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,'dQ^s^p _1','FontName','MS Sans Serif','FontSize',13')

text (0.2, 0.89,'--- = ---(\Omega_p^p^s)','FontName','MS Sans Serif','FontSize',13)

text (0.2, 0.86,'dt ^2','FontName','MS Sans Serif','FontSize',13')

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_-Kvatern.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.

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

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

%Правые части уравнения движения в кватернионах

function z=KvatKU(t,y)

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

OM=[0,UgSk];

%Правые части КУЭ

z=multQ(y,OM)/2;

Орієнтація об’єкта визначається через повний кватерніон повороту використовуючи кути, отримані із Object. Для цього була створена процедура Qvat3pov. В ній значення кута та рядок одиничної матриці Е подаються до функції KvatPov, яка виконує представлення відповідного кута у вигляді кватерніону. Після цього отримані кватерніони перемножуються за допомогою процедури multQ. В результаті ми маємо повний кватерніон повороту системи S відносно Р.

Тексти відповідних процедур подано нижче.

%Начальные значения кватерниона

function KP=Qvat3pov(P,T,F)

E1=[0 1 0];

E2=[0 0 1];

E3=[1 0 0];

KP1=KvatPov(E1,P); %Первый поворот

KP2=KvatPov(E2,T); %Второй поворот

KP3=KvatPov(E3,F); %Третий поворот

KP12=multQ(KP1,KP2);

KP=multQ(KP12,KP3); %Результирующий кватернион поворота от СК S k P

%Поворот вокруг одного угла в кватернионах

function KP=KvatPov(E,D)

kp=cos(D/2);

kpv=sin(D/2).*E;

KP=[kp kpv];

%Произведение кватернионов

function Q=multQ(Q1,Q2)

q1=Q1(1);

q2=Q2(1);

qv1=[Q1(2) Q1(3) Q1(4)];

qv2=[Q2(2) Q2(3) Q2(4)];

qvx=Vect2KSM(qv1);

q=q1*q2-qv1*qv2';

qv=q1*qv2+q2*qv1+(qvx*(qv2'))';

Q=[q qv];

Задавши кінематику об'єкта через кватерніони, ми можемо приступити до формування процедури інтегрування рівняння (1.35). Для цього використовуються раніше створені функції чисельного інтегрування RK2.1, RK2.2, RK4.2. Проте в роботі вони посилаються на процедуру KvatKU, що формує праву частину рівняння (1.35).

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

%Правые части уравнения движения в кватернионах

function z=KvatKU(t,y)

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

OM=[0,UgSk];

%Правые части КУЭ

z=multQ(y,OM)/2;

Для подальшого аналізу похибок зручно мати значення дрейфу у кутах Ейлера-Крилова. Для цього необхідно сформувати матрицю напрямних косинусів із отриманого кватерніона та розрахувати значення кутів використовуючи процедуру CtoP2T3F. Для формування матриці напрямних косинусів були створені процедури NapCos та Vect2KSM.

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

function C=NapCos(KP)

E=[1 0 0; 0 1 0; 0 0 1];

q0=KP(1);

qv=[KP(2) KP(3) KP(4)];

qx=Vect2KSM(qv);

C=E+2*q0*qx+2*qx*qx;

function A=Vect2KSM(a)

A=[0 -a(3) a(2); a(3) 0 -a(1); -a(2) a(1) 0];