소형 무인 비행체 3 - 과제

1. s 펑션 정리하기

2. 템플릿 완성하기

3. 운동 방정식과 애니메이션 블록 연결

 

 

1. s 펑션 정리하기

 3장 과제가 부록 F 정리하는건줄 알았는데 s펑션 내용인 부록 E더라.. 왠지 내용이 3장이랑 다른가 싶었는데 아무생각없이 정리하기 바빳다. 

 

 

2. 템플릿 완성하기

 

 

 

 

 

쿼터니언을 이용한 기구학과 동역학식

 

 

여기서 gamma들 구하는 방법은

 

감마 구하는데 필요한 관성모멘트 값은 아래의

우리가 사용하는 비행체 계수들 이용하여 구함

 

 

위 기구학, 동역학 식과 비행체 계수에 따라 모델 미분 함수 정리함.

function sys=mdlDerivatives(t,x,uu, MAV)
    pn    = x(1);
    pe    = x(2);
    pd    = x(3);
    u     = x(4);
    v     = x(5);
    w     = x(6);
    e0    = x(7);
    e1    = x(8);
    e2    = x(9);
    e3    = x(10);
    p     = x(11);
    q     = x(12);
    r     = x(13);
    fx    = uu(1);
    fy    = uu(2);
    fz    = uu(3);
    ell   = uu(4);
    m     = uu(5);
    n     = uu(6);
    
    pndot = e1^2+e0^2-e2^2-e3^2*u + 2*v*(e1*e2 - e3*e0) + 2*w*(e1*e3 + e2*e0);
    
    pedot = 2*u*(e1*e2+e3*e0)+v*(e2^2+e0^2-e1^2-e3^2) + w*2*(e2*e3-e1*e0);
    
    pddot = u*2*(e1*e3-e2*e0) + 2*v*(e2*e3 + e1*e0) + w*(e3^2+e0^2 - e1^2 - e2^2);
    
    udot = r*v - q*w + 1/MAV.mass*fx;
    
    vdot = p*w - r*u + 1/MAV.mass*fy;
    
    wdot = q*u - p*v + 1/MAV.mass*fz;
       
    e0dot = 1/2*(-p*e1 -q*e2 -r*e3);
    e1dot = 1/2*(p*e0 + r*e2 -q*e3);
    e2dot = 1/2*(e0*q+e1*-r+e3*p);
    e3dot = 1/2*(r*e0+q*e1-p*e2);
        
    jx = 0.8244;
    jy = 1.135;
    jz = 1.759;
    jxz = 0.1204;
    
    g=jx*jz-jxz^2;
    g1= (jxz*(jx-jy+jz))/g;
    g2=(jz*(jz-jy) + jxz^2)/g;
    g3=jz/g;
    g4= jxz/g;
    g5= (jz-jx)/jy;
    g6= jxz/jy;
    g7=((jx-jy)*jx+jxz^2)/g;
    g8=jx/g;
    
    pdot = g1*p*q-g2*q*r+g3*ell+g4*n;
    
    qdot = g5*p*r-g6*(p^2-r^2) +1/jy*m;
    
    rdot = g7*p*q - g1*q*r + g4*ell + g8*n;
        

sys = [pndot; pedot; pddot; udot; vdot; wdot; e0dot; e1dot; e2dot; e3dot; pdot; qdot; rdot];

mdlderivative만 했더니 비행체가 움직이질 않는다.

 

drawAircraft 함수에서 입력값을 확인해보니 전체가 0으로 뜨더라.

 

mav_dynamics의 출력을 설정안해서 생긴 문제였다.

 

아래의 초기화 함수를보면 출력 갯수를 12개로 설정했다.

function [sys,x0,str,ts,simStateCompliance]=mdlInitializeSizes(MAV)

%
% call simsizes for a sizes structure, fill it in and convert it to a
% sizes array.
%
% Note that in this example, the values are hard coded.  This is not a
% recommended practice as the characteristics of the block are typically
% defined by the S-function parameters.
%
sizes = simsizes;

sizes.NumContStates  = 13;
sizes.NumDiscStates  = 0;
sizes.NumOutputs     = 12;
sizes.NumInputs      = 6;
sizes.DirFeedthrough = 0;
sizes.NumSampleTimes = 1;   % at least one sample time is needed

sys = simsizes(sizes);

%
% initialize the initial conditions
%
x0  = [...
    MAV.pn0;...
    MAV.pe0;...
    MAV.pd0;...
    MAV.u0;...
    MAV.v0;...
    MAV.w0;...
    MAV.e0;...
    MAV.e1;...
    MAV.e2;...
    MAV.e3;...
    MAV.p0;...
    MAV.q0;...
    MAV.r0;...
    ];

%
% str is always an empty matrix
%
str = [];

%
% initialize the array of sample times
%
ts  = [0 0];

% Specify the block simStateCompliance. The allowed values are:
%    'UnknownSimState', < The default setting; warn and assume DefaultSimState
%    'DefaultSimState', < Same sim state as a built-in block
%    'HasNoSimState',   < No sim state
%    'DisallowSimState' < Error out when saving or restoring the model sim state
simStateCompliance = 'UnknownSimState';

% end mdlInitializeSizes

 

 

mdlOutputs으로 draw aircraft에서 사용할 변수 12개를 지정해서 출력해주면 된다.

%
%=============================================================================
% mdlOutputs
% Return the block outputs.
%=============================================================================
%
function sys=mdlOutputs(t,x)
    [phi, theta, psi] = Quaternion2Euler([x(7), x(8), x(9), x(10)]);
    y = [...
        x(1);
        x(2);
        x(3);
        x(4);
        x(5);
        x(6);
        phi;
        theta;
        psi;
        x(11);
        x(12);
        x(13);
        ];
sys = y;

% end mdlOutputs

 

s function 상에서 x는 쿼터니언을 사용하여 총 13개지만

 

draw Aircraft에서는 오일러각으로 나타내다보니 12개가 된다.

 

그래서 출력때 Quaternion2Euler 함수로 쿼터니언을 오일러 각으로 변환했다.

 

function [phi, theta, psi] = Quaternion2Euler(quaternion)
    % converts a quaternion attitude to an euler angle attitude
    e0 = quaternion(1);
    e1 = quaternion(2);
    e2 = quaternion(3);
    e3 = quaternion(4);
    phi = atan2( 2*(e0*e1 + e2*e3), (e0^2+e3^2 - e1^2 - e2^2));
    theta = asin(2*(e0*e2 - e1*e3) );
    psi = atan2(2*(e0*e3 + e1*e2), (e0*2 + e1^2 - e2^2 - e3^2));
end