MATLAB模拟四旋翼无人机飞行，机翼可独立旋转

bash 复制代码
clear; clc; close all;

f = figure('Color','w','Position',[100 100 900 700]);
ax = axes('DataAspectRatio',[1 1 1],'Color',[0.98 0.98 1]);
hold on; grid on; box on;
xlabel('X'); ylabel('Y'); zlabel('Z');
title('无人机','FontSize',12,'FontWeight','bold');
view(60,25); lighting gouraud; material shiny;
axis([-15 15 -15 15 -5 20]);

% 轨迹
traj = plot3(0,0,0,'r-','LineWidth',2);
traj_x = []; traj_y = []; traj_z = [];

% ===================== 固定绘制无人机（本地坐标）=====================
% 机身（永远不会消失）
[x,y,z] = ellipsoid(0,0,0, 2,1.2,0.5);
body = surf(x,y,z,'FaceColor',[0.05 0.08 0.15],'EdgeColor','none');

% 机臂
arm_L = 3.5;
arm_R = 0.12;
motors = [arm_L,arm_L,0; -arm_L,arm_L,0; arm_L,-arm_L,0; -arm_L,-arm_L,0];
for i = 1:4
    cylinder3d([0,0,0], motors(i,:), arm_R, [0.1 0.1 0.13]);
end

% 电机
for i = 1:4
    [xm,ym,zm] = cylinder(0.25,50); zm = zm*0.2-0.1;
    xm = xm+motors(i,1); ym = ym+motors(i,2);
    surf(xm,ym,zm,'FaceColor',[0 0 0],'EdgeColor','none');
end

% 起落架
plot3([1.3,1.3],[1.3,1.3],[0,-1.2],'Color',[0.07 0.07 0.09],'LineWidth',4);
plot3([-1.3,-1.3],[1.3,1.3],[0,-1.2],'Color',[0.07 0.07 0.09],'LineWidth',4);
plot3([1.3,1.3],[-1.3,-1.3],[0,-1.2],'Color',[0.07 0.07 0.09],'LineWidth',4);
plot3([-1.3,-1.3],[-1.3,-1.3],[0,-1.2],'Color',[0.07 0.07 0.09],'LineWidth',4);

% 4叶旋翼
blades = [];
prop_R = 1.2;
blade_w = 0.1;
for m = 1:4
    ox = motors(m,1); oy = motors(m,2); oz = 0.15;
    for k = 0:3
        ang = k*pi/2;
        bx = [ox, ox+prop_R*cos(ang)-blade_w*sin(ang), ox+prop_R*cos(ang)+blade_w*sin(ang), ox];
        by = [oy, oy+prop_R*sin(ang)+blade_w*cos(ang), oy+prop_R*sin(ang)-blade_w*cos(ang), oy];
        bz = [oz, oz+0.02, oz+0.02, oz];
        p = patch(bx,by,bz,[0.93 0.93 0.96],'EdgeColor',[0.1 0.1 0.12]);
        blades = [blades, p];
    end
end

light('Position',[6,6,8]); light('Position',[-6,-6,6]);

% ===================== 核心：整体飞行（绝对不消失）=====================
angle = 0; t = 0;
while ishandle(f)
    % 飞行路径
    t = t + 0.08;
    cx = 12*sin(0.3*t);
    cy = 12*cos(0.4*t);
    cz = 10 + 4*sin(0.5*t);
    yaw = sin(0.2*t);

    % 记录轨迹
    traj_x = [traj_x, cx];
    traj_y = [traj_y, cy];
    traj_z = [traj_z, cz];
%     plot3(traj_x,traj_y,traj_z,'r-')
%     hold on
%     set(traj,'XData',traj_x,'YData',traj_y,'ZData',traj_z);

    % 旋翼旋转
    angle = angle + 6;
    for m = 1:4
        ox = motors(m,1); oy = motors(m,2); oz = 0.15;
        for k = 0:3
            a = angle + k*90;
            idx = (m-1)*4 + k + 1;
            bx = [ox, ox+prop_R*cosd(a)-blade_w*sind(a), ox+prop_R*cosd(a)+blade_w*sind(a), ox];
            by = [oy, oy+prop_R*sind(a)+blade_w*cosd(a), oy+prop_R*sind(a)-blade_w*cosd(a), oy];
            bz = [oz, oz+0.02, oz+0.02, oz];
            set(blades(idx),'XData',bx,'YData',by,'ZData',bz);
        end
    end

    % ==================== 关键修复：整体移动，不修改原始数据！永不消失！====================
    % 把【整个无人机】当作一个整体，直接整体平移+旋转
    hg = hgtransform;
    set(body,'Parent',hg);
    set(findobj('Type','surf'),'Parent',hg);
    set(findobj('Type','patch'),'Parent',hg);
    set(findobj('Type','line'),'Parent',hg);
    
    R = makehgtform('translate',[yaw yaw yaw]);
    T = makehgtform('translate',[cx cy cz]);
    set(hg,'Matrix',T*R);
xlim([-20 20])
ylim([-20 20])
zlim([-20 20])
    drawnow;
end

% 圆柱体函数
function cylinder3d(startPos,endPos,r,color)
    dir = endPos - startPos; L = norm(dir); dir = dir/L;
    [X,Y,Z] = cylinder(r,50); Z = Z*L;
    R = rotMatrix([0,0,1],dir);
    for i = 1:size(X,1)
        for j = 1:size(X,2)
            v = R * [X(i,j);Y(i,j);Z(i,j)];
            X(i,j)=v(1)+startPos(1); Y(i,j)=v(2)+startPos(2); Z(i,j)=v(3)+startPos(3);
        end
    end
    surf(X,Y,Z,'FaceColor',color,'EdgeColor','none');
end

function R = rotMatrix(v1,v2)
    v = cross(v1,v2); s = norm(v); c = dot(v1,v2);
    vx = [0,-v(3),v(2);v(3),0,-v(1);-v(2),v(1),0];
    R = eye(3) + vx + vx^2*(1-c)/(c+1);
end