[MATLAB学习笔记]基于MATLAB的坐标系变换及飞行器姿态运动显示

        描述三维空间物体的运动通常是在指定的坐标系下进行，在不同的坐标系下物体运动的轨迹和姿态不尽相同。求解物体在不同坐标系下的位置和姿态的关键在于求解不同坐标系之间的变换矩阵。

        以从地心坐标系到体坐标系为例，将地心坐标系先绕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

        欢迎大家和我共同探讨学习相关的知识技能^_^