drawquadrotor.m

四轴飞行器Matlab仿真分析,涉及四轴飞行器主要部件如Motor,camera,电池等

function drawquadrotor(rn, re, h, phi, theta, psi, C, target_y)
% re is the lateral position of the quadrotor on the rail
% phi is the roll angle of the quadrotor


P = defineParameters;
[Vert_quad, Face_quad, colors_quad]       = quadrotorVertFace(P);
%[Vert_rails, Face_rails, colors_rails]    = railVertFace(P);
[Vert_fov, Face_fov, colors_fov]          = fovVertFace(rn,re,h,phi,theta,psi,P,C);
%[Vert_target, Face_target, colors_target] = targetVertFace(P,C);
[X_target,Y_target,Z_target]              = targetXYZ(P,C);

% rotate rails by 90 degrees, and translate by
%Vert_rails = rotateVert(Vert_rails, pi/2);
%Vert_rails = translateVert(Vert_rails, [0; 0; P.l_rail/2]);

% rotate vertices by phi
Vert_quad = rotateVert(Vert_quad, -phi, theta, psi);

% translate vertices by re
Vert_quad = translateVert(Vert_quad, [rn; re; h]);

% translate target by 
%Vert_target = translateVert(Vert_target, [0; target_y; 0]);
[X_target,Y_target,Z_target] = translateXYZ(X_target,Y_target,Z_target,[0; target_y; 0]);


% collect all vertices and faces
V = [...
    Vert_quad;...
%    Vert_rails;...
    Vert_fov;...
%    Vert_target;...
    ];
F = [...
    Face_quad;...
%    size(Vert_quad,1) + Face_rails;...
%    size(Vert_quad,1) + size(Vert_rails,1) + Face_fov;...
    size(Vert_quad,1) + Face_fov;...
%    size(Vert_quad,1) + size(Vert_rails,1) + size(Vert_fov,1) + Face_target;...
    ]; 
patchcolors = [...
    colors_quad;...
%    colors_rails;...
    colors_fov;...
%    colors_target;...
    ];

% draw quadrotor
figure(1), clf
patch('Vertices', V, 'Faces', F,...
    'FaceVertexCData',patchcolors,'FaceColor','flat');
hold on
patch(X_target,Y_target,Z_target,'r')
view(144,30);
axis(1*[-1, 1, -1, 1, -1, 1])
set(gca,'visible','off')
hold on
rotate3d

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function P = defineParameters

% parameters
P.r = 0.1;   % radius of the rotor
P.l = 0.1;   % length of connecting rods
P.lw = 0.01; % width of rod
P.w = 0.1;   % width of the center pod
P.w_rail = 0.01; % width of the rail
P.l_rail = 5; % length of the rail
P.N = 10;     % number of points defining rotor

% define colors for faces
P.myred = [1, 0, 0];
P.mygreen = [0, 1, 0];
P.myblue = [0, 0, 1];
P.myyellow = [1,1,0];

%%%%%%%%%%%%%%%%%%%%%%%
function Vert=rotateVert(Vert,phi,theta,psi);
  % define rotation matrix
%  phi = phi+pi;
  R_roll = [...
          1, 0, 0;...
          0, cos(phi), -sin(phi);...
          0, sin(phi), cos(phi)];
  R_pitch = [...
          cos(theta), 0, sin(theta);...
          0, 1, 0;...
          -sin(theta), 0, cos(theta)];
  R_yaw = [...
          cos(psi), -sin(psi), 0;...
          sin(psi), cos(psi), 0;...
          0, 0, 1];
  R = R_roll*R_pitch*R_yaw;
%  R = R_yaw'*R_pitch'*R_roll';
  R = R_roll'*R_pitch'*R_yaw';

  % rotate vertices
  Vert = (R*Vert')';

% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% % rotate vertices by roll angle phi
% function Vert = rotateVert(Vert, phi, theta, psi)
% 
%   Rx = [...
%     1, 0, 0;...
%     0, cos(phi),  sin(phi);...
%     0, -sin(phi), cos(phi);...
%     ];
%   
%   Ry = [...
%     cos(theta), 0, -sin(theta);...
%     0, 1, 0;...
%     sin(theta), 0, cos(theta);...
%     ];
% 
%   Rz = [...
%     cos(psi), sin(psi), 0;...
%     -sin(psi), cos(psi) 0;...
%     0, 0, 1;...
%     ];
% 
%   Vert = Vert*Rx';
%   
 % end rotateVert

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% translate vertices by column vector T
function Vert = translateVert(Vert, T)

  Vert = Vert + repmat(T', size(Vert,1),1);

% end translateVert

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% translate vertices by column vector T
function [X,Y,Z] = translateXYZ(X,Y,Z,T)

  X = X + T(1)*ones(size(X));
  Y = Y + T(2)*ones(size(Y));
  Z = Z + T(3)*ones(size(Z));

% end translateXYZ


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [Vert_quad, Face_quad, colors_quad] = quadrotorVertFace(P)
% Vertices = [x,y,z] position of each vertex
% Faces = defines how the vertices are connnected for form faces.  Each set
% of our vertices defines one face.


%--------- vertices and faces for center pod ------------
% vertices of the center pod
Vert_center = [...
    P.w/2, P.w/2, P.w/2;...
    P.w/2, P.w/2, -P.w/2;...
    P.w/2, -P.w/2, -P.w/2;...
    P.w/2, -P.w/2, P.w/2;...
    -P.w/2, P.w/2, P.w/2;...
    -P.w/2, P.w/2, -P.w/2;...
    -P.w/2, -P.w/2, -P.w/2;...
    -P.w/2, -P.w/2, P.w/2;...
    ];
% define faces of center pod
Face_center = [...
        1, 2, 3, 4;... % front
        5, 6, 7, 8;... % back
        1, 4, 8, 5;... % top
        8, 4, 3, 7;... % right 
        1, 2, 6, 5;... % left
        2, 3, 7, 6;... % bottom
        ];
    
%--------- vertices and faces for connecting rods ------------    
% vertices for front rod
Vert_rod_front = [...
    P.w/2, P.lw/2, 0;...
    P.w/2, -P.lw/2, 0;...
    P.w/2, 0, P.lw/2;...
    P.w/2, 0, -P.lw/2;...
    P.l+P.w/2, P.lw/2, 0;...
    P.l+P.w/2, -P.lw/2, 0;...
    P.l+P.w/2, 0, P.lw/2;...
    P.l+P.w/2, 0, -P.lw/2;...
    ];
Face_rod_front = 8 + [...
        1, 2, 6, 5;... % x-y face
        3, 4, 8, 7;... % x-z face
    ];
% vertices for right rod
Vert_rod_right = Vert_rod_front*[0,1,0;1,0,0;0,0,1];
Face_rod_right = 16 + [...
        1, 2, 6, 5;... 
        3, 4, 8, 7;... 
    ];
% vertices for back rod
Vert_rod_back = Vert_rod_front*[-1,0,0;0,-1,0;0,0,1];
Face_rod_back = 24 + [...
        1, 2, 6, 5;... 
        3, 4, 8, 7;... 
    ];
% vertices for left rod
Vert_rod_left = Vert_rod_front*[0,-1,0;-1,0,0;0,0,1];
Face_rod_left = 32 + [...
        1, 2, 6, 5;... 
        3, 4, 8, 7;... 
    ];

%--------- vertices and faces for rotors ------------  
Vert_rotor = [];
for i=1:P.N,
    Vert_rotor = [Vert_rotor; P.r*cos(2*pi*i/P.N), P.r*sin(2*pi*i/P.N), 0];
end
for i=1:P.N,
    Vert_rotor = [Vert_rotor; P.r/10*cos(2*pi*i/P.N), P.r/10*sin(2*pi*i/P.N), 0];
end
Face_rotor = [];
for i=1:P.N-1,
    Face_rotor = [Face_rotor; i, i+1, P.N+i+1, P.N+i];
end
Face_rotor = [Face_rotor; P.N, 1, P.N+1, 2*P.N];

% front rotor
Vert_rotor_front = Vert_rotor + repmat([P.w/2+P.l+P.r, 0, 0],2*P.N,1);
Face_rotor_front = 40 + Face_rotor;
% right rotor
Vert_rotor_right = Vert_rotor + repmat([0, P.w/2+P.l+P.r, 0],2*P.N,1);
Face_rotor_right = 40 + 2*P.N + Face_rotor;
% back rotor
Vert_rotor_back = Vert_rotor + repmat([-(P.w/2+P.l+P.r), 0, 0],2*P.N,1);
Face_rotor_back = 40 + 2*P.N + 2*P.N + Face_rotor;
% left rotor
Vert_rotor_left = Vert_rotor + repmat([0, -(P.w/2+P.l+P.r), 0],2*P.N,1);
Face_rotor_left = 40 + 2*P.N + 2*P.N + 2*P.N + Face_rotor;

% collect all of the vertices for the quadrotor into one matrix
Vert_quad = [...
    Vert_center; Vert_rod_front; Vert_rod_right; Vert_rod_back;...
    Vert_rod_left; Vert_rotor_front; Vert_rotor_right;...
    Vert_rotor_back; Vert_rotor_left...
    ];
% collect all of the faces for the quadrotor into one matrix
Face_quad = [...
    Face_center; Face_rod_front; Face_rod_right; Face_rod_back;...
    Face_rod_left; Face_rotor_front; Face_rotor_right;...
    Face_rotor_back; Face_rotor_left...
    ];

myred = [1, 0, 0];
mygreen = [0, 1, 0];
myblue = [0, 0, 1];
myyellow = [1,1,0];

colors_quad = [...
    myblue;... % fuselage front
    myblue;... % back
    myyellow;... % top
    myblue;... % right
    myblue;... % left
    myblue;... % bottom
    mygreen;... % rod front
    mygreen;... %
    mygreen;... % rod right
    mygreen;... %
    mygreen;... % rod back
    mygreen;... %
    mygreen;... % rod left
    mygreen;... %
    ];
for i=1:P.N,
    colors_quad = [colors_quad; mygreen];   % front rotor
end
for i=1:P.N,
    colors_quad = [colors_quad; myblue];  % right rotor
end
for i=1:P.N,
    colors_quad = [colors_quad; myblue];  % left rotor
end
for i=1:P.N,
    colors_quad = [colors_quad; myblue];  % back rotor
end


% end quadrotorVertFace

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [Vert_rails, Face_rails, colors_rails] = railVertFace(P)
% Vertices = [x,y,z] position of each vertex
% Faces = defines how the vertices are connnected for form faces.  Each set
% of our vertices defines one face.

%-------vertices and faces for rails--------------
% vertices for right rail
Vert_rail_right = [...
    P.w/2+P.l, P.l_rail/2, P.w_rail;...
    P.w/2+P.l, P.l_rail/2, 0;...
    P.w/2+P.l, -P.l_rail/2, 0;...
    P.w/2+P.l, -P.l_rail/2, P.w_rail;...
    P.w/2+P.l+P.w_rail, P.l_rail/2, 0;...
    P.w/2+P.l+P.w_rail, -P.l_rail/2, 0;...
    ];
Face_rail_right = [...
        1, 2, 3, 4;... % x-y face
        5, 2, 3, 6;... % x-z face
    ];

% vertices for left rail
Vert_rail_left = Vert_rail_right*[-1,0,0;0,1,0;0,0,1];
Face_rail_left = 6 + [...
        1, 2, 3, 4;... % x-y face
        5, 2, 3, 6;... % x-z face
    ];

% collect vertices and faces
Vert_rails = [Vert_rail_right; Vert_rail_left];
Face_rails = [Face_rail_right; Face_rail_left];

colors_rails = [...
    P.myyellow;...% right rail
    P.myyellow;...%
    P.myyellow;...% left rail
    P.myyellow;...%
    ];

% end railVertFace

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [Vert_fov, Face_fov, colors_fov] = fovVertFace(pn,pe,h,phi,theta,psi,P,C)

%-------vertices and faces for camera field-of-view --------------
% vertices 
Vert_fov = [...
    pn, pe, h;...
    pn+h*tan(theta+C.cam_fov/2), pe+h*tan(phi+C.cam_fov/2), 0;...
    pn+h*tan(theta+C.cam_fov/2), pe+h*tan(phi-C.cam_fov/2), 0;...
    pn-h*tan(-theta+C.cam_fov/2), pe+h*tan(phi+C.cam_fov/2), 0;...
    pn-h*tan(-theta+C.cam_fov/2), pe+h*tan(phi-C.cam_fov/2), 0;...
    ];
Vert_fov = rotateVert(Vert_fov,0,0,psi);
Face_fov = [...
        1, 1, 2, 2;... % x-y face
        1, 1, 3, 3;... % x-y face
        1, 1, 4, 4;... % x-y face
        1, 1, 5, 5;... % x-y face
        2, 3, 5, 4;... % x-y face
    ];

colors_fov = [P.myblue; P.myblue; P.myblue; P.myblue; P.myyellow];


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [Vert_target, Face_target, colors_target] = targetVertFace(P,C)

Vert_target = [];
for i=1:P.N,
    Vert_target = [Vert_target; C.target(1)+C.target_size*cos(2*pi*i/P.N),...
        C.target(2)+C.target_size*sin(2*pi*i/P.N), C.target(3)];
end
for i=1:P.N,
    Vert_target = [Vert_target; C.target(1)+C.target_size/10*cos(2*pi*i/P.N),...
        C.target(2)+C.target_size/10*sin(2*pi*i/P.N), C.target(3)];
end

Face_target = [];
for i=1:P.N-1,
    Face_target = [Face_target; i, i+1, P.N+i+1, P.N+i];
end

Face_target = [Face_target; P.N, 1, P.N+1, 2*P.N];

colors_target = [];
for i=1:P.N,
    colors_target = [colors_target; P.myred]; % right rotor
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [X,Y,Z] = targetXYZ(P,C)
%   th=0:.1:2*pi;
%   X=C.target(1) + C.target_size*cos(th);
%   Y=C.target(2) + C.target_size*sin(th);
%   Z=C.target(3) + zeros(1,length(th));

  pts = C.target_size*[...
      2, 0, 0.1;...
      0, 2, 0.1;...
      0, 1, 0.1;...
      -2, 1, 0.1;...
      -2, -1, 0.1;...
      0, -1, 0.1;...
      0, -2, 0.1;...
      2, 0, 0.1;...
      ];
  X = pts(:,1);
  Y = pts(:,2);
  Z = pts(:,3);