描述三維空間物體的運動通常是在指定的座標系下進行，在不同的座標系下物體運動的軌跡和姿態不盡相同。求解物體在不同座標系下的位置和姿態的關鍵在於求解不同座標系之間的變換矩陣。

        以從地心座標系到體座標系為例，將地心座標系先繞z軸轉過俯仰角φ，再繞新座標系的y軸轉過偏航角ψ，最後繞新座標系的x軸轉過滾轉角γ，就得到了物體的體座標系。因此從地心座標系到物體體座標系的變換矩陣為

        其中，

        座標變換矩陣可簡單理解為，若慣性空間有一點，在地心座標系下的座標為,則在體座標系下看，該點的座標為。

        另外，從體座標系到地心座標系的座標變換矩陣滿足

        因此，若某一時間段物體的俯仰角、偏航角和滾轉角已知，則其上各點在地心座標系下的座標可以通過座標變換計算出來。利用MATLAB的繪圖功能，可將此姿態運動過程顯示出來。

        以某飛行器為例，實現其俯仰運動、偏航運動、滾轉運動以及組合運動顯示的MATLAB程式碼和效果圖如下。

俯仰運動

偏航運動

滾轉運動

組合運動

        運動顯示主程式程式碼如下

clear
clc
close all
%%
figure('Color','w','Position',[1,41,1550,740])
hold on; box on; axis equal off
xlabel('x'); ylabel('y'); zlabel('z');
axis([-10000,10000,-12000,12000,-12000,12000])
%%
vertex = shapeLoader([cd '\ShenZhou.txt']);
vertex(:,1) = vertex(:,1) + 5000;
vertex = (M1(pi/2)*vertex')';

%%
hFill = cell(1,length(vertex)/3);
for n = 1:length(vertex)/3
    hFill{1,n} = fill3(vertex(3*n-2:3*n,1),vertex(3*n-2:3*n,2),vertex(3*n-2:3*n,3),'r',...
       'EdgeAlpha',0);
end
set(gca,'View',[0,-60])
camlight

%%
%俯仰
%{
aviobj = VideoWriter('pitch.avi');
phi = [linspace(0,pi/3,10),linspace(pi/3,-pi/3,20)];
psi = linspace(0,0,30);
gamma = linspace(0,0,30);
%}
%偏航
%{
aviobj = VideoWriter('yaw.avi');
phi = linspace(0,0,30);
psi = [linspace(0,pi/3,10),linspace(pi/3,-pi/3,20)];
gamma = linspace(0,0,30);
%}
%滾轉
%{
aviobj = VideoWriter('roll.avi');
phi = linspace(0,0,30);
psi = linspace(0,0,30);
gamma = [linspace(0,pi/3,10),linspace(pi/3,-pi/3,20)];
%}
%組合
%
aviobj = VideoWriter('rotate.avi');
phi = [linspace(0,pi/3,10),linspace(pi/3,-pi/3,20)];
psi = [linspace(0,pi/3,10),linspace(pi/3,-pi/3,20)];
gamma = [linspace(0,pi/3,10),linspace(pi/3,-pi/3,20)];
%}
%%

set(aviobj,'FrameRate',5,'Quality',100);
open(aviobj);
for n = 2:length(phi)
    B_G = M1(gamma(n))*M2(psi(n))*M3(phi(n));
    for m = 1:length(vertex)/3
        newVertex = B_G\[vertex(3*m-2,:)',vertex(3*m-1,:)',vertex(3*m,:)'];
        set(hFill{1,m},'XData',newVertex(1,1:3)','YData',newVertex(2,1:3)','ZData',newVertex(3,1:3)')         
    end
    frame = getframe(gca);
    writeVideo(aviobj,frame);
    drawnow
end
close(aviobj);
 

        其中用到了三個座標系變換函式和三維模型讀取函式，分別如下

function [M] = M1(theta)
M = [1 0 0;0 cos(theta) sin(theta);0 -sin(theta) cos(theta)];
end

function [M] = M2(theta)
M = [cos(theta) 0 -sin(theta); 0 1 0;sin(theta) 0 cos(theta)];
end

function [M] = M3(theta)
M = [cos(theta) sin(theta) 0;-sin(theta) cos(theta) 0;0 0 1];
end

function [vertex,facetN] = shapeLoader(filename)

fid = fopen(filename,'r');
strline = cell(1,1);
counter = 1;
while ~feof(fid)
    strline{counter,1} = fgetl(fid);
    counter = counter + 1;
end
fclose(fid);
counterf = 1;
counterv = 1;
facetN = zeros(1,1);
vertex = zeros(1,1);
for n = 1:length(strline)
    if length(strline{n}) == 59
        if strcmp(strline{n}(3:14),'facet normal')
            facetN(counterf,1) = str2double(strline{n}(16:29));
            facetN(counterf,2)  = str2double(strline{n}(31:44));
            facetN(counterf,3)  = str2double(strline{n}(46:59));
            counterf = counterf + 1;
        elseif strcmp(strline{n}(7:12),'vertex')
            vertex(counterv,1) = str2double(strline{n}(16:29));
            vertex(counterv,2) = str2double(strline{n}(31:44));
            vertex(counterv,3) = str2double(strline{n}(46:59));
            counterv = counterv + 1;
        end
    end
end

end
 

        用到的模型資料的雲盤連結為：https://pan.baidu.com/s/1wY9sDERkqJmcJ-9eV1jm4w 密碼: 1nt1

        全部源程式的雲盤連結為：https://pan.baidu.com/s/1SK6_MDWQYcrf-nv6kKT_xQ 密碼: 6gdi

        歡迎大家和我共同探討學習相關的知識技能^_^