728x90

오늘날 사용되는 다양한 제어 이론들

- 고전 제어

- 현대 제어

- 강인 제어

 

 

자동 제어

- 공학 및 과학에서의 핵심 분야 중 하나

- 우주선 시스템, 로봇 시스템, 제조 시스템 등 다양한 산업 전반에서 사용

 

 

 

제어 이론의 역사

- 18세기 : 제임스 와트의 증기기관을 이용한 속도 제어

- 1922 : minorsky는 자동화 제어기 분야 연구. 시스템에 대한 미분 방정식으로 배의 안정성 유지되도록 구함

- 1932 : 나이퀴스트가 정현파 입력에 대한 개루프 시스템의 정상상태 응답으로 폐루프에서 안정성을 구하는 과정 설계

- 1940

    1. 보드 선도를 이용한 주파수 응답을 활용하여 성능 요구를 만족하는 선형 폐루프 제어기를 설계할수 있게됨.

    2. 압력, 온도등 제어하기 위한 PID 제어기가 산업 전반에서 사용됨

    3. 지글러와 니콜이 PID 제어기 튜닝 방법을 소개.

    4. 에반스가 루트 로커스 방법을 제안

- 1960 

   1. 디지털 컴퓨터가 만들어지면서 복잡한 시스템에 대한 시간 영역에서의 해석이 가능해짐.

   2. 현대에 들어와 복잡해진 제어기와 정밀도가 요구되는 일들을 다루기위함.

- 1960 ~ 1980

    1. 결정론적이고, 확률적인 시스템에 최적으로 제어하고, 복잡한 시스템에 적응해나가는 방법들이 연구됨

- 1980 ~ 1990

    1. 강인 제어에 대한 연구가 이루어짐.

 

 

 

고전 제어

- 주파수 응답과 루트로커스 방법은 고전 제어 이론의 핵심이되어 여러 성능 요구사항들을 지킬수 있었음.

- 하지만 고전 제어 이론은 단일 입력, 단일 출력 시스템에서만 사용가능

- 다중 입력, 다중 출력 시스템에서는 사용 불가

 

 

현대 제어

- 시스템 미분방정식의 시간 영역에 대한 해석을 기반

- 현실에 존재하는 제어 시스템을 수학적 모델링 하여 활용

-실제 시스템과 모델 사이의 오차가 존재하며, 이 오차에 시스템 안정성이 영향을 받음

 

 

300x250
728x90

1. 제어 시스템 소개

2. 시스템 수학적 모델링

3. 상태 변수 모델

4. 궤환 제어 시스템 특성

5. 피드백 제어 시스템 성능

6. 선형 피드백 시스템의 안전성

7. 루트 로커스 방법

8. 주파수 응답 방법

9. 주파수 영역에서의 안정성

10. 피드백 제어기 설계

11. 상태 변수 피드백 시스템 설계

12. 강인한 제어 시스템

13. 디지털 제어 시스템

 

 

 

 

 

1. 제어시스템 소개

 1.1 소개

 1.2 자동화 제어에 대한 역사

 1.3 제어 시스템의 예시

 1.4 엔지니어링 설계

 1.5 제어 시스템 설계

 1.6 기계적 시스템

 

2. 시스템 수학적 모델

 2.1 소개

 2.2 물리적 시스템에대한 미분 방정식

 2.3 물리적 시스템을 선형 근사화 하기

 2.4 라플라스 변환

 2.5 선형 시스템 전달함수

 2.6 블록 다이어그램

 2.7 signal flow 그래프 모델

 2.8 디자인 예시

 2.9 제어 설계 프로그램을 이용한 시스템 시뮬레이션

 

 

3. 상태 변수 모델

 3.1 소개

 3.2 동적 시스템의 상태 변수

 3.3 상태 미분 방정식

 3.4 signal-flow 그래프와 블록 다이어그램 모델

 3.5 signal flow 그래프와 블록 다이어그램을 대신할 방법들

 3.6 상태 방정식을 전달 함수로 변환하기

 3.7 시응답과 상태 전이 행렬

 3.8 디자인 예시

 3.9 제어기 설계 소프트웨어를 사용한 상태 변수 모델 해석

 

4. 피드백 제어 시스템 특성

 4.1 소개

 4.2 에러 신호 해석

 4.3 파라미터 변동에 따른 제어 시스템의 민감도

 4.4 피드백 제어 시스템에서 요란  신호

 4.5 제어기 과도 응답

 4.6 정상 상태 응답

 4.7 피드백 비용

 

-- 생략---

 

 

 

300x250
728x90

 

 기후 변화, 방사능 유출같은 세계적인 문제들로 인해, 그리고 에너지 소모를 줄이기 위해 많은 공학자들이 세로운 제어기 설계 방법을 찾으려고 노력하고 있습니다.

 

 그러한 설계 방법을 그린 엔지니어링 green engineering이라고 부르며, 그린 엔지니어링의 목표는 오염을 줄이고, 사람에게 유해를 줄이도록 하는 제품을 만드는것이 목표라 할수 있겠습니다. 이러한 그린 엔지니어링의 원칙에 따라서 피드벡 제어 시스템의 유용함을 보여드리고자 합니다.

 

 지구 온난화 문제와 오염을 줄이기 위해서는 환경 모니터링 시스템을 늘리고, 품질도 높여야만하는데요. 한 가지의 예시로 외부 환경을 관측하는 무선으로 이동하면서 감지하는 플랫폼들이 있습니다. 보내지는 전력과 전압의 변화 등을 측정하는것도 그러한 예시라고 할수 있겠습니다.

 

 수많은 그린 엔지니어링 시스템들을 다루기 위해서는 신중하게 전류와 전압을 보아야만 하는데요. 전력 변환기는 전력 공급망 상에서 흘러가는 다양한 크기의 전류를 측정하고 모니터링을 할수 있어야 하며, 여기서 사용되는 센서들은  측정치가 제어 시스템이 적절한 동작을 하는데 필요한 정보들을 제공하므로 제어 시스템의 핵심 요소라고 할수 있습니다.

 

 그린 엔지니어링에서의 제어시스템의 역활은 자동화와 정밀화 수준이 높아만 가면서 세계적으로 큰 역활을 하게 될겁니다. 그래서 이후에는 풍력 발전기같은 그린 엔지니어링의 제어나 태양의 밝기가 변화하는 사이에서도 전력 생산량을 최대로 하는 태양광 발전기 피드벡 제어 시스템을 설계해보겠습니다.

 

 제어 시스템으로 개발할수 있는 다른 재밌는 것으로 IoT가 있습니다. 여기서 IoT란 Internet of Things의 약어로 많은 전자 장치들이 연결된것을 말합니다. IoT 네트워크 상에서 수백만의 장치들은 각자의 컴퓨터가 내장되어 인터넷에 연결 됩니다. 이러한 연결된 장비들을 제어해보는것은 제어 공학도에게는 신나는 일이라 할수 있을것 같습니다.

 

 그래서 제어 공학은 재미있고, 도전할만한 분야라고 생각합니다. 제어 시스템은 본래 여러 분야가 혼합된 학문으로, 엔지니어링 커리큘럼 상에서 핵심역활을 한다고 할수 있겠습니다. 이 분야에서는 수학적 배경 지식이 많이 필요하므로, 정리와 증명과 같이 이론적인 부분들도 다루긴 할겁니다. 하지만 최종 목표는 현실에서 동작가능한 제어기를 설계하는 것이므로, ad-hoc(임기 응변)적인 방법과 직관력으로 피드백 제어기를 설계해 보겠습니다.

 

 앞으로 배우는데 있어서 가장 중요한 것은 과거에 다루었던 문제와 이에 대답을 다시 찾아보는것이라 할수 있겠습니다. 그래서 학생들이 수많은 문제들과 해결방안 그리고, 수십년 전에 있어왔던 해결 방안에 대해서 배워보아야 합니다. 옛날의 학습 방법에서 학생은 이러한 문제를 고민하지 않고 단순히 과제들을 풀기만 했었습니다. 하지만 이 과정을 통해서 배우는 사람들이 이론적인 어려움을 줄이고 창의성과 즐거움을 찾길 희망하고 있습니다.

 

 이제 피드벡 제어 이론의 구조에 대해서 살펴보고, 다양한 흥미로운 주제들을 드리고자 합니다. 그래서 여러분들이 피드벡 제어 시스템 이론과 실습에 도움이 되었으면 좋겠습니다.

300x250
728x90

부분분수 전개 doc

https://kr.mathworks.com/help/matlab/ref/residue.html

 

역라플라스 doc

https://kr.mathworks.com/help/symbolic/ilaplace.html

 

1. 하나의 전달함수가 주어질때 부분분수 전개 함수 residue로 부분 분수들의 분자와 분모들을 얻습니다.

2. 각 부분분수들을 ilaplace 함수로 역변환 시킨후 변수 after에다가 다 담아줬습니다.

 

 

 

syms s

num = [1 5];
den = [1 6 11 6];

before = tf(num, den)
after = 0;

[r, p, k] = residue(num, den)
% r 분자, p, 분모항



for i = 1:length(r)
    F = r(i)/(s - p(i))
    after = after + ilaplace(F);
end

after

 

300x250
728x90

 

 이번 장에서는 연속 폐루프를 이용해서 횡방향과 종방향 오토파일럿을 설계하였습니다. 횡방향 오토파일럿은 내부 루프로 롤 자세 유지 루프를 외부 루프로 방위각 유지 루프로 이루어집니다.

 

 종방향 오토파일럿은 고도에 따라 더 복잡하게 구성되는데 피치 자세 유지 루프는 모든 영역에서 사용됩니다. 이륙 영역에서는 비행체는 풀 스로틀 상태가 되며 이륙 피치각을 고정하도록 제어하게 됩니다. 상승 영역에서는 비행체는 풀 스로틀로, 피치, 대기 속도 유지 오토 파일럿 루프로 대기속도를 제어합니다. 하강 영역에서는 상승 영역과 비슷한데 대신 비행체에는 최소 스로틀을 줍니다. 고도 유지 영역에서 고도는 피치를 이용한 고도 오토파일럿 루프로 제어되고, 대기속도는 스로틀을 이용한 대기속도 오토파일럿 루프로 제어됩니다.

 

횡방향 오토파일럿 설계 과정 요약

입력

 - 전달함수 계수 $a_{phi_1}$, $a_{phi_2}$, 명목 대기속도 $V_a$, 에일러론 제한 $\delta_{a}^{max}$

 

튜닝 파라미터

 - 롤 각 제한 $\phi^{max}$, 댐핑 계수 $\zeta_{\phi}$, $\zeta_{\chi}$

 

계산할 고유 주파수

 - 식 (6.8)로 내부 루프의 고유주파수 $\omega_{n_{\phi}}$와 $\omega_{n_{\chi}}$ = $\omega_{n_{\phi}}$ / $W_{\chi}$를 사용해서 외부 루프의 고유 주파수를 계산해야 합니다.

 

계산할 게인

 - 식 (6.7), (6.9), (6.12), (6.13)으로 게인 $k_{p_{\phi}}$, $k_{d_{\phi}}$, $k_{p_{\chi}}$, $k_{i_{\chi}}$을 계산합니다.

 

 

종방향 오토파일럿 설계 과정 요약

입력

 - 전달함수 계수 $a_{\theta_{1}}$, $a_{\theta_{2}}$, $a_{\theta_{3}}$, $a_{V_{1}}$, 명목 대기속도 $V_a$, 엘리베이터 제한 $\delta_{e}^{max}$

 

튜닝 파라미터

 - 피치 각 제한 $e_{\theta}^{max}$, 댐핑 계수 $\zeta_{\theta}$, $\zeta_{h}$, $\zeta_{V}$, $\zeta_{V2}$와 고유 주파수 $\omega_{n_{v}}$, 고도 루프와 피치 이용 대기속도 루프의 분할 주파수  $W_h$, $W_{V_2}$ 

 

계산 고유 주파수들

- 식 (6.21)로 피치 루프 $\omega_{n_{\theta}}$ 고유 주파수 계산. $\omega_{n_{h}}$ = $\omega_{n_{\theta}}$ / $W_h$를 사용하여 고도 루프, $\omega_{n_{V2}}$ = $\omega_{n_{\theta}}$ / $W_{V_2}$를 사용하여 피치 이용 대기속도 루프의 고유 주파수 계산. 

 

계산 게인

- 식 6.22로 게인 $k_{p_{\theta}}$와 $k_{d_{\theta}}$를 계산하고, 식 6.23으로 피치 루프의 DC 게인을 구합니다. 식 6.25와 6.24로 $k_{p_{h}}$, $k_{i_{h}}$를계산하며,식6.28과6.27로 $k_{p v_{2}}$, $k_{i v_{2}}$를 구하고, 식 6.30, 6.29로 $k_{p_v}$, $k_{i_v}$를 구합니다.

 

300x250
728x90

5.2 mavsim_trim.slx복붙하고 동작하도록 파일 수정해라

 

 

% compute trim conditions using 'mavsim_chap5_trim.slx'
% nominal airspeed P.Va0 specified above with aircraft parameters
gamma = 0*pi/180;  % desired flight path angle (radians)
R     = 10000000000;        % desired radius (m) - use (+) for right handed orbit, 
                            %                          (-) for left handed orbit
Va = 25;


% set initial conditions 
x0 = [];
% specify which states to hold equal to the initial conditions
ix = [];

% specify initial inputs 
u0 = [...
    0;... % 1 - delta_e
    0;... % 2 - delta_a
    0;... % 3 - delta_r
    1;... % 4 - delta_t
    ];
% specify which inputs to hold constant
iu = [];

% define constant outputs
y0 = [...
    Va;...       % 1 - Va
    0;...        % 2 - alpha
    0;...        % 3 - beta
    ];
% specify which outputs to hold constant
iy = [1,3];

% define constant derivatives
dx0 = 

if R~=Inf, dx0(9) = Va*cos(gamma)/R; end  % 9 - psidot
% specify which derivaties to hold constant in trim algorithm
idx = [3; 4; 5; 6; 7; 8; 9; 10; 11; 12];

% compute trim conditions
[x_trim,u_trim,y_trim,dx_trim] = trim('mavsim_trim',x0,u0,y0,ix,iu,iy,dx0,idx);

% check to make sure that the linearization worked (should be small)
norm(dx_trim(3:end)-dx0(3:end))

% P.u_trim = u_trim;
% P.x_trim = x_trim;

% set initial conditions to trim conditions
% initial conditions
MAV.pn0    = 0;           % initial North position
MAV.pe0    = 0;           % initial East position
MAV.pd0    = -200;        % initial Down position (negative altitude)
MAV.u0     = x_trim(4);   % initial velocity along body x-axis
MAV.v0     = x_trim(5);   % initial velocity along body y-axis
MAV.w0     = x_trim(6);   % initial velocity along body z-axis
MAV.phi0   = x_trim(7);   % initial roll angle
MAV.theta0 = x_trim(8);   % initial pitch angle
MAV.psi0   = x_trim(9);   % initial yaw angle
MAV.p0     = x_trim(10);  % initial body frame roll rate
MAV.q0     = x_trim(11);  % initial body frame pitch rate
MAV.r0     = x_trim(12);  % initial body frame yaw rate  


 

trim 시뮬링크 모델이랑 트림 계산  파일이 저렇게 주어졌다.

 

xdot은 다음과 같으므로

 

값들은 아래와 같이 대입하면 될거같다

 

 

구현 하긴했는데

 

compute_trim에서 자꾸 dx0이랑 시뮬링크 블록상 연속 상태 크기랑 틀렸다고 에러뜬다.

 

한참 삽질하다보니 mav_dynamics에서 오일러각대신 쿼터니언을 쓰다보니 상태가 13개가 쓰여서 그렇더라

 

다시 챕터 3의 mav_dynamics를 오일러각으로 바꿧다.

 

function [sys,x0,str,ts,simStateCompliance] = mav_dynamics(t,x,u,flag,MAV)

switch flag

  %%%%%%%%%%%%%%%%%%
  % Initialization %
  %%%%%%%%%%%%%%%%%%
  case 0
    [sys,x0,str,ts,simStateCompliance]=mdlInitializeSizes(MAV);

  %%%%%%%%%%%%%%%
  % Derivatives %
  %%%%%%%%%%%%%%%
  case 1
    sys=mdlDerivatives(t,x,u,MAV);

  %%%%%%%%%%
  % Update %
  %%%%%%%%%%
  case 2
    sys=mdlUpdate(t,x,u);

  %%%%%%%%%%%
  % Outputs %
  %%%%%%%%%%%
  case 3
    sys=mdlOutputs(t,x);

  %%%%%%%%%%%%%%%%%%%%%%%
  % GetTimeOfNextVarHit %
  %%%%%%%%%%%%%%%%%%%%%%%
  case 4
    sys=mdlGetTimeOfNextVarHit(t,x,u);

  %%%%%%%%%%%%%
  % Terminate %
  %%%%%%%%%%%%%
  case 9
    sys=mdlTerminate(t,x,u);

  %%%%%%%%%%%%%%%%%%%%
  % Unexpected flags %
  %%%%%%%%%%%%%%%%%%%%
  otherwise
    DAStudio.error('Simulink:blocks:unhandledFlag', num2str(flag));

end

% end sfuntmpl

%
%=============================================================================
% mdlInitializeSizes
% Return the sizes, initial conditions, and sample times for the S-function.
%=============================================================================
%
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  = 12;
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.phi0;...
    MAV.theta0;...
    MAV.psi0;...
    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

%
%=============================================================================
% mdlDerivatives
% Return the derivatives for the continuous states.
%=============================================================================
%
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);
    phi   = x(7);
    theta= x(8);
    psi   = x(9);
    p     = x(10);
    q     = x(11);
    r     = x(12);
    fx    = uu(1);
    fy    = uu(2);
    fz    = uu(3);
    ell   = uu(4);
    m     = uu(5);
    n     = uu(6);
    
    cphi = cos(phi);
    ctheta = cos(theta);
    cpsi = cos(psi);
    sphi = sin(phi);
    stheta = sin(theta);
    spsi = sin(psi);
    
    
    pndot = u*ctheta*cpsi -v*(sphi*stheta*cpsi - cphi*spsi) +w*(cphi*stheta*cpsi + sphi*spsi);
    
    pedot = u*ctheta*spsi + v*(sphi*stheta*spsi + cphi*cpsi) + w*(cphi*stheta*spsi - sphi*cpsi);
    
    pddot = -stheta*u + v*sphi*ctheta + w*cphi *ctheta;
    
    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;
       
    phidot = p + sphi*tan(theta)*q + r*cphi*tan(theta);
    
    thetadot = q*cphi -sphi*r;
    
    psidot = sphi/ctheta*q + cphi/ctheta*r;
    
    pdot = MAV.Gamma1*p*q-MAV.Gamma2*q*r+MAV.Gamma3*ell+MAV.Gamma4*n;
    qdot = MAV.Gamma5*p*r-MAV.Gamma6*(p^2-r^2) +1/MAV.Jy*m;
    rdot = MAV.Gamma7*p*q - MAV.Gamma1*q*r + MAV.Gamma4*ell + MAV.Gamma8*n;
        

sys = [pndot; pedot; pddot; udot; vdot; wdot; phidot; thetadot; psidot; pdot; qdot; rdot];

% end mdlDerivatives

%
%=============================================================================
% mdlUpdate
% Handle discrete state updates, sample time hits, and major time step
% requirements.
%=============================================================================
%
function sys=mdlUpdate(t,x,u)

sys = [];

% end mdlUpdate

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

% end mdlOutputs

%
%=============================================================================
% mdlGetTimeOfNextVarHit
% Return the time of the next hit for this block.  Note that the result is
% absolute time.  Note that this function is only used when you specify a
% variable discrete-time sample time [-2 0] in the sample time array in
% mdlInitializeSizes.
%=============================================================================
%
function sys=mdlGetTimeOfNextVarHit(t,x,u)

sampleTime = 1;    %  Example, set the next hit to be one second later.
sys = t + sampleTime;

% end mdlGetTimeOfNextVarHit

%
%=============================================================================
% mdlTerminate
% Perform any end of simulation tasks.
%=============================================================================
%
function sys=mdlTerminate(t,x,u)

sys = [];

% end mdlTerminate

 

다음 값이 주어질때

트림 값

 

 

 

아래의 트림 시뮬레이션 모델에서 

 

트림 계산 입력이 아래와 같이 주어질때 

gamma = 2*pi/180;  % desired flight path angle (radians)
R     = 5000;        % desired radius (m) - use (+) for right handed orbit, 
                            %                          (-) for left handed orbit
Va = 100;

 

 

시뮬레이션

 

 

 

 

 

300x250
728x90

시뮬링크에서 트림과 선형화하기

 

F.1 시뮬링크 trim 명령 사용하기

 

 시뮬링크는 트림 상태를 계산하는 루틴을 제공하는데, help trim 으로 사용방법을 확인할 수 있습니다. 5.3장에서 봤던것 처럼 $V_a^*$, $\gamma^*$, $R^*$가 주어지면, $x^*$, $u^*$를 찾는것이 목표가 됩니다. $\dot{x^*}$ = f($x^*$, $u^*$ 로 여기서 x, u는 식 (5.17)과 (5.18)로 정의 됩니다. $\dot{x^*}$는 식 (5.21)에서 주어지며 f(x, u)는 식 (5.1) - (5.12)로 정의됩니다.

 

 트림 명령의 형식은

 

[x, u, y, dx] = trim('sys', x0, u0, y0, ix, iu, uy, dx0, idx);

 

 

 여기서 x는 계산된 트림상태 $x^*$, u는 계산됨 트림 입력 $u^*$, y는 계산된 트림 출력 $y^*$, dx는 계산된 상태의 미분 $\dot{x^*}$, 시뮬링크 모델은 sys.mdl에 명시되는데 모델의 상태들은 서브시스템 내 상태로 되어있고, 입력과 출력은 시뮬링크에서 inports, outports로 정의됩니다.

 

 

 

 그림 F.1은 비행체 트림 계산하는 시뮬링크 모델로 입력은 4개의 inports로 서보 명령인 delta_e, delta_a, delta_r, delta_t 가 있습니다. 이 블록의 상태들은 시뮬링크 상태로 우리는 $\zeta$ = ($p_n$, $p_e$, $p_d$, u, u, w, $\phi$, $\theta$, $\psi$, p, q, r)$^T$가 됩니다. 출력은 세개의 outports이며 대기속도 $V_a$, 받음각 $\alpha$, $\beta$이며 이 아웃풋을 트림 명령에다가 전달하여 $V_a$ = $V_a^*$가 유지되도록 하겠습니다.

 

 

 

 러더도 사용가능하면 $\beta^*$=0으로 유지시키는 트림 명령을 주어 정상선회 하도록 할수 있씁니다. 러더가 없는 경우 0상태로 만들수 없습니다.

 

 

 트림 계산에서 문제는 시스템의 비선형 방정식을 풀어야 하는데, 많은 경우 시뮬링크 트림 명령에는 초기화 추정 값으로 상태 x0, 입력 u0, 출력 y0, 상태 미분 dx0이 필요합니다. 우리가 일부 상태나 입력, 출력 또는 미분 상태같은 시작 상태를 알아서 초기 상태를 쓸수 있으면, 사용가능한 값들을 인덱스 벡터 ix, iu, iy, idx로 전달할 수 있습니다.

 

 

 우리의 상태가 아래와 같다면

 

 

이렇게 작성할 수 있습니다.

 

 

비슷하게 초기 상태, 입력, 출력들을 다음처럼 정의할수 있습니다.

 

 

그림 F.1 트림과 선형 상태 공간 모델 계산을 위한 시뮬링크 다이어그램

 

F.2 트림 수치 계산 Numerical Computation of Trim

 시뮬링크 말고 다른환경에서 시뮬레이션이 개발된다면, 트림 루틴을 따로 작성해야합니다. 이번 장에서는 어떻게 하는지 간단히 설명하고 $V_a^*$, $\gamma^*$, $R^*$ 파라미터들로 상승 선회 트림 제어에 대해 살펴보고, 트림 찾기 알고리즘에서 입력으로 사용하겠습니다. 입력 파라미터 $V_a^*$, $\gamma^*$, $R^*$와  $\alpha$, $\beta$, $\phi$로 트림 상태와 입력 구하는 계산과정을 살펴보겠습니다. 주어진 $V_a^*$, $\gamma^*$, $R^*$값으로 트림값  $\alpha^*$, $\beta^*$, $\phi^*$을 찾아낸다면 이제 트림 상태와 입력같을 구할수 있게됩니다.

 

 첫 단계는 상태 변수들을 $V_a^*$, $\gamma^*$, $R^*$로, 입력을 $\alpha^*$, $\beta^*$, $\phi^*$로 나타내는데 $V_a^*$, $\gamma^*$, $R^*$는 사용자 지정 입력이 되어 알고리즘은 최적의 트림 상태를 구할수 있는 $\alpha^*$, $\beta^*$, $\phi^*$ 계산합니다. 이 값으로 트림 상태 $x^*$, $u^*$를 찾는데 사용됩니다.

 

동체 좌표계 속도 $u^*$, $v^*$, $w^*$

 식 (2.7)에서 동체 좌표계상 속도들은 $V_a^*$, $\alpha^*$, $\beta^*$ 를 이용해서 나타낼 수있습니다.

 

피치각 $\theta^*$

 비행경로각의 정의로 다음과 같이 정리할 수있습니다.

 

각 변화율 p, q, r

 각 변화율은 식 (3.2)로 오일러각으로 나타낼수 있습니다.

 

엘리베이터 $\delta_e$

 $p^*$, $q^*$, $r^*$, $\delta_e^*$에 대해서 식 (5.11)을 풀면

 

에일러론 $\delta_a$과 러더 $\delta_r$

 에일러론과 러더 명령은 식 (5.10), 식(5.12)을 풀어서 구할 수 있습니다.

 

 

F.2.1 트림 알고리즘

 모든 관심변수와 제어입력은 $V_a^*$, $\gamma^*$, $R^*$, $\alpha^*$, $\beta^*$, $\phi^*$입니다. 트림 알고리즘의 입력은 $V^*$, $\gamma^*$, $R^*$이며,  $\alpha^*$, $\beta^*$, $\phi^*$값을 찾기위해 다음의 최적화 알고리즘을 사용합니다.

 

 이는 다음장에서 설명할 경사 하강 알고리즘으로 수치적으로 구할 수있습니다. 트림 알고리즘은 알고리즘 13에서 정리하였습니다.

 

F.2.2 경사하강법 수학적 구현

 이번 장의 목표는 최적화 문제를 풀수있는 단순한 경사 하강 알고리즘을 살펴보겠습니다.

 

 

알고리즘 13 트림

1. 입력 : 희망 대기속도 $V_a^*$, 희망 비행경로각 $\gamma^*$, 희망 선회 반지름 $R_*$

2. 계산 : ($\alpha^*$, $\beta^*$, $\theta^*$) = arg min || $\dot{x^*}$- f($x^*$, $u^*$) $||^2$

3. 트림 상태를 계산합시다.

 

 

4. 트림 입력

 

 

  기본 아이디어는 초기 시작 값 $\xi^{(0)}$에서 음수 기울기 방향으로 따라가는데, 이를 정리하면 다음과 같습니다. 이때 k는 양의 상수 값으로 하강 비율을 나타냅니다.

 

 

식 (F.4)를 이산 추정으로 한다면, 다음과 같고 $k_d$는 이산 스탭 크기를 의미합니다. 

 

 

 트림 계산을 위해서, 편미분 $\frac{\vartheta J} {\vartheta \xi}$는 해석적으로 구하기는 힘들지만 수치적인 방법으로 효율적으로 구할수 있습니다.

 

 이는 수치적으로 추정해서 구하면 다음과 같습니다. 이때 $\epsilon$은 작은 상수값이 됩니다.

 

 

F.3 시뮬링크 linmod 명령을 사용하여 상태공간 모델을 만들기

 시뮬링크는 다이어그램을 이용해서 선형 상태공간을 만들어내는 내장 기능을 제공하고 있습니다. 사용 방법에대해서 매트랩 프롬프트 상에 help linmod로 얻을 수있습니다. linmod 명령의 포맷은 다음과 같습니다.

 

 여기서 X, U는 상태와 입력값으로 선형화 될 값들이며, [A, B, C, D]는 상태공간 모델이 됩니다. 12 상태, 4입력인 F.1 그림에서 linmod 명령을 사용하면 결과 상태공간 모델 방정식은 식 (5.43)의 모델과 식 (5.50)을 포함해서 나오게 됩니다.

 

알고리즘 14. J = || $\dot{x^*}$ - f($x^*$, $u^*$) $||^20$

1. 입력 : $\alpha^*$, $\beta^*$, $\phi^*$, $V_a^*$, $R^*$, $\gamma^*$

2. $\dot{x^*}$계산하기 :

3. 트림 상태 계산 : 

 

 

4. 트림 입력 계산 : 

 

 

5. f($x^*$, $u^*$) 계산  :

 

 

6. J 계산 :

 

 

식 (5.43)을 얻기 위해서 다음 과정을 거치면 됩니다.

 

 

 

 

 

 

 

300x250
728x90

 

 

1. 에일러론, 엘리베이터, 러더, 추진 + 바람에 의한 힘과 모멘트 생성

2. mav_dynamics에 입력 -> 비행체 상태 출력

3. drawAircraft에서 비행체 그리기

 

1. forces_moment.m 파일 중력, 항공역학,  추진 힘과 모멘트 구현하기

 

진행하다보니 aerosonde uav 파라미터를 이미 가지고 있더라

 

% initialize the mav viewer
addpath('../tools');  

% initial conditions
MAV.pn0    = 0;     % initial North position
MAV.pe0    = 0;     % initial East position
MAV.pd0    = -100;  % initial Down position (negative altitude)
MAV.u0     = 25;     % initial velocity along body x-axis
MAV.v0     = 0;     % initial velocity along body y-axis
MAV.w0     = 0;     % initial velocity along body z-axis
MAV.phi0   = 0;     % initial roll angle
MAV.theta0 = 0;     % initial pitch angle
MAV.psi0   = 0;     % initial yaw angle
e = Euler2Quaternion(MAV.phi0, MAV.theta0, MAV.psi0);
MAV.e0     = e(1);  % initial quaternion
MAV.e1     = e(2);
MAV.e2     = e(3);
MAV.e3     = e(4);
MAV.p0     = 0;     % initial body frame roll rate
MAV.q0     = 0;     % initial body frame pitch rate
MAV.r0     = 0;     % initial body frame yaw rate
   
%physical parameters of airframe
MAV.gravity = 9.81;
MAV.mass = 11.0;
MAV.Jx   = 0.824;
MAV.Jy   = 1.135;
MAV.Jz   = 1.759;
MAV.Jxz  = 0.120;
MAV.S_wing        = 0.55;
MAV.b             = 2.90;
MAV.c             = 0.19;
MAV.S_prop        = 0.2027;
MAV.rho           = 1.2682;
MAV.e             = 0.9;
MAV.AR            = MAV.b^2/MAV.S_wing;

% Gamma parameters from uavbook page 36
MAV.Gamma  = MAV.Jx*MAV.Jz-MAV.Jxz^2;
MAV.Gamma1 = (MAV.Jxz*(MAV.Jx-MAV.Jy+MAV.Jz))/MAV.Gamma;
MAV.Gamma2 = (MAV.Jz*(MAV.Jz-MAV.Jy)+MAV.Jxz*MAV.Jxz)/MAV.Gamma;
MAV.Gamma3 = MAV.Jz/MAV.Gamma;
MAV.Gamma4 = MAV.Jxz/MAV.Gamma;
MAV.Gamma5 = (MAV.Jz-MAV.Jx)/MAV.Jy;
MAV.Gamma6 = MAV.Jxz/MAV.Jy;
MAV.Gamma7 = (MAV.Jx*(MAV.Jx-MAV.Jy)+MAV.Jxz*MAV.Jxz)/MAV.Gamma;
MAV.Gamma8 = MAV.Jx/MAV.Gamma;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% aerodynamic coefficients
MAV.C_L_0         = 0.23;
MAV.C_D_0         = 0.043;
MAV.C_m_0         = 0.0135;
MAV.C_L_alpha     = 5.61;
MAV.C_D_alpha     = 0.030;
MAV.C_m_alpha     = -2.74;
MAV.C_L_q         = 7.95;
MAV.C_D_q         = 0.0;
MAV.C_m_q         = -38.21;
MAV.C_L_delta_e   = 0.13;
MAV.C_D_delta_e   = 0.0135;
MAV.C_m_delta_e   = -0.99;
MAV.M             = 50;
MAV.alpha0        = 0.47;
MAV.epsilon       = 0.16;
MAV.C_D_p         = 0.0;

MAV.C_Y_0         = 0.0;
MAV.C_ell_0       = 0.0;
MAV.C_n_0         = 0.0;
MAV.C_Y_beta      = -0.98;
MAV.C_ell_beta    = -0.13;
MAV.C_n_beta      = 0.073;
MAV.C_Y_p         = 0.0;
MAV.C_ell_p       = -0.51;
MAV.C_n_p         = -0.069;
MAV.C_Y_r         = 0.0;
MAV.C_ell_r       = 0.25;
MAV.C_n_r         = -0.095;
MAV.C_Y_delta_a   = 0.075;
MAV.C_ell_delta_a = 0.17;
MAV.C_n_delta_a   = -0.011;
MAV.C_Y_delta_r   = 0.19;
MAV.C_ell_delta_r = 0.0024;
MAV.C_n_delta_r   = -0.069;

% Parameters for propulsion thrust and torque models
MAV.D_prop = 0.508;     % prop diameter in m

% Motor parameters
MAV.K_V = 145;                    % from datasheet RPM/V
MAV.KQ = (1/MAV.K_V)*60/(2*pi);   % KQ in N-m/A, V-s/rad
MAV.R_motor = 0.042;              % ohms
MAV.i0 = 1.5;                     % no-load (zero-torque) current (A)

% Inputs
MAV.ncells = 12;
MAV.V_max = 3.7*MAV.ncells;       % max voltage for specified number of battery cells

% Coeffiecients from prop_data fit
MAV.C_Q2 = -0.01664;
MAV.C_Q1 = 0.004970;
MAV.C_Q0 = 0.005230;

MAV.C_T2 = -0.1079;
MAV.C_T1 = -0.06044;
MAV.C_T0 = 0.09357;



 

그래서 챕터 3의 모델 미분 함수를 위 파라미터를 바로사용하는걸로 고쳤다.

 

%
%=============================================================================
% mdlDerivatives
% Return the derivatives for the continuous states.
%=============================================================================
%
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);
    
    pdot = MAV.Gamma1*p*q-MAV.Gamma2*q*r+MAV.Gamma3*ell+MAV.Gamma4*n;
    qdot = MAV.Gamma5*p*r-MAV.Gamma6*(p^2-r^2) +1/MAV.Jy*m;
    rdot = MAV.Gamma7*p*q - MAV.Gamma1*q*r + MAV.Gamma4*ell + MAV.Gamma8*n;
        

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

% end mdlDerivatives

 

위 파라미터와 현재 상태값으로  중력, 항공역학적, 추진 힘과 모멘트를 정리한 아래의 식을 구하면 된다.

 

4.2 forces_moments.m 에서 외풍 블록을 수정하고, 대기속도, 받음각, 사이드 슬립각, 바람 백터를 정리하라.

 4.1하는데 우선 대기속도, 받음각 부터 필요하므로 4,2부터 정리하면

 

 

위 식을 계산하는데 $u_r$, $v_r$, $w_r$이 필요하다. 이값들은 

 

 

이며, 동체 좌표계산 바람 속도가 필요한데, steady wind와 gust wind의 합으로 구한다.

 

steady wind와 gust wind는 force moment.m 의 파라미터로 전달되므로 이를 이용하면 되겠다.

 

% forces_moments.m
%   Computes the forces and moments acting on the airframe. 
%
%   Output is
%       F     - forces
%       M     - moments
%       Va    - airspeed
%       alpha - angle of attack
%       beta  - sideslip angle
%       wind  - wind vector in the inertial frame
%

function out = forces_moments(x, delta, wind, P)

    % relabel the inputs
    pn      = x(1);
    pe      = x(2);
    pd      = x(3);
    u       = x(4);
    v       = x(5);
    w       = x(6);
    phi     = x(7);
    theta   = x(8);
    psi     = x(9);
    p       = x(10);
    q       = x(11);
    r       = x(12);
    delta_e = delta(1);
    delta_a = delta(2);
    delta_r = delta(3);
    delta_t = delta(4);
    w_ns    = wind(1); % steady wind - North
    w_es    = wind(2); % steady wind - East
    w_ds    = wind(3); % steady wind - Down
    u_wg    = wind(4); % gust along body x-axis
    v_wg    = wind(5); % gust along body y-axis    
    w_wg    = wind(6); % gust along body z-axis
    
    % compute wind data in NED
    w_n = 0;
    w_e = 0;
    w_d = 0;
    
    % compute air data
    Va = 0;
    alpha = 0;
    beta = 0;
    
    % compute external forces and torques on aircraft
    Force(1) =  0;
    Force(2) =  0;
    Force(3) =  0;
    
    Torque(1) = 0;
    Torque(2) = 0;   
    Torque(3) = 0;
   
    out = [Force'; Torque'; Va; alpha; beta; w_n; w_e; w_d];
end



 

우선 ned축에 대한 바람부터 구해보자.

 

gust_wind는 그대로 써도 되지만

 

steady winid는 기체 좌표계에서 동체 좌표계로 로테이션 시켜주어야 한다.

 

 

chap2 의 drawAircraft 함수에 이 로테이션을 구현한 부분이 있었으니 사용하자

%%%%%%%%%%%%%%%%%%%%%%%
function XYZ=rotate(XYZ,phi,theta,psi)
  % define rotation matrix
  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;
  % rotate vertices
  XYZ = R*XYZ;
end

 

 

위 함수를 force_moments에 추가하고

 

각 방향에 대한 바람들을 다음과 같이 구하였다.

   w_v = [w_ns; w_es; w_ds];
   w_b = rotate(w_v, phi, theta, psi);
  
    
    % compute wind data in NED
    w_n = w_b(1) + u_wg;
    w_e = w_b(2) + v_wg;
    w_d = w_b(3) + w_wg;
    

 

 

그다음은 대기속도, 받음각, 사이드슬립을 구해보자

 

 

 

여기서 필요한 $u_r$, $v_r$, $w_r$은 동체 좌표계 상에서의 대기속도로 다음과 같이 구한다.

 

 

방금 전에 $u_w$, $v_w$, $w_w$를 구하였으므로, 입력 받은 u, v, w에서 바로 빼주면 되겠다.

 

바로 대기속도, 받음각, 사이드 슬립으로 정리하면

 

    v_ab = [u-w_n; v-w_e; w-w_d];
    
    % compute air data
    Va = sqrt(v_ab(1)^2 + v_ab(2)^2 + v_ab(3)^2);
    alpha = atan(v_ab(3)/ v_ab(1));
    beta = asin(v_ab(2)/Va);

 

 

이렇게 4.2 문제를 끝내고 힘과 모멘트를 구해보면

 

 

위 식에서 중력 요소는 다음과 같으며

 

forces_moments.m 은 MAV 파라미터를 변수 p로 받으므로

 

    % gravity forces
    f_g_x = -p.mass * p.gravity * sin(theta);
    f_g_y = p.mass * p.gravity * cos(theta) * sin(phi);
    f_g_z = p.mass * p.gravity * cos(theta) * cos(phi);
    f_g = [f_g_x; f_g_y; f_g_z];
    

 

 

다음은 항공영학적 힘을 구하려면

 

 

여기서 $C_x$($\alpha$) 같은 값들은

 

다음과 같이 정의된다.

 

비행체 파라미터로 우선 위 값들을 우선 정리하면

 

    % stability coefficients
    c_x_alpha = -P.C_D_alpha * cos(alpha) + P.C_L_alpha * sin(alpha);
    c_x_q_alpha = -P.C_D_q * cos(alpha) + P.C_L_q * sin(alpha);
    c_x_delta_e_alpha = -P.C_D_delta_e * cos(alpha) + P.C_L_delta_e * sin(alpha);
    c_z_alpha = -P.C_D_alpha * sin(alpha) - P.C_L_delta_e*cos(alpha);
    c_z_q_alpha = -P.C_D_q * sin(alpha) - P.C_L_q * cos(alpha);
    c_z_delta_e_alpha = -P.C_D_delta_e * sin(alpha) - P.C_L_delta_e * cos(alpha);

 

다시 돌아와 다음 식을 구하면

 

코드로 구현하면

 

 

    %aerodynamics forces
    tmp = 1/2*P.rho*Va^2*P.S_wing;
    f_a_x = c_x_alpha + c_x_q_alpha*P.c/(2*Va)*q + c_x_delta_e_alpha * delta_e;
    f_a_y = P.C_Y_0 + C_Y_beta * beta + C_Y_p *P.b/(2*Va)*p + C_Y_r*P.b/(2*Va)*r+C_Y_delta_a *delta_a+C_Y_delta_r *delta_r;
    f_a_z = c_z_alpha + c_z_q_alpha * P.c/(2*Va)*q + c_z_delta_e_alpha * delta_e;
    f_a = tmp * [f_a_x; f_a_y; f_a_z];
    

 

마지막으로 구할 힘은 추진 힘으로 x축에 대해서만 작용한다.

 

 

코드 상으로 아래와 같이 구현하면 되나

 

    %propulsion forces
    f_x_p = 1/2 * P.rho *P.S_prop * P.C_prop*( (P.k_motor *delta_t)^2 - Va^2);
    

파라미터 파일에서는 C_prop와 k_motor가 없더라

 

그래서 aerosonde_parameter.m에 다음 코드를 추가했다.

 

MAV.k_motor = 80;
MAV.C_prop  = 1;

 

 

 

힘에 대해서 정리하자면 다음과 같이 코드를 구현하면 된다.

    % compute external forces and torques on aircraft
    Force(1) =  f_g(1) + f_a(1) + f_x_p;
    Force(2) =  f_g(2) + f_a(2);
    Force(3) =  f_g(3) + f_a(3);

 

 

다음으로 각 축방향으로 작용하는 모멘트를 구해보자

 

 

여기서 항공역학적 모멘트 부분만 정리하면 코드상으로 다음과 같다.

 

    %aerodynamics moments
    tmp = 1/2 *P.rho * Va^2 * P.S_wing;
    m_l = P.C_ell_0 + P.C_ell_beta *beta + P.C_ell_p * P.b/(2*Va)*p + P.C.ell_r*b/(2*Va)*r + P.C_ell_delta_a * delta_a + P.C_ell_delta_r * delta_r;
    m_l = tmp *P.b*m_l;
    m_m = P.C_m_0 + P.C_m_alpha * alpha + P.C_m_q * P.c /(2*Va) * q + P.C_m_delta_e * delta_e;
    m_m = tmp*P.c *m_m;
    m_n = P.C_n_0 + P.C_n_beta * beta + P.C_n_p * P.b/(2* Va) *p + P.C_n_r * P.b/(2* Va) * r + P.C_n_delta_a * delta_a + P.C_n_delta_r * delta_r;
    m_n = tmp * P.b *m_n;

 

 

추진력에 의한 x축 모멘트를 구하여야 하나 우리의 경우는 0이므로 추진에 의한 모멘트는 무시하자

 

 

 

 

 

forces_moments.m

전체 코드를 정리하면

 

% forces_moments.m
%   Computes the forces and moments acting on the airframe. 
%
%   Output is
%       F     - forces
%       M     - moments
%       Va    - airspeed
%       alpha - angle of attack
%       beta  - sideslip angle
%       wind  - wind vector in the inertial frame
%

function out = forces_moments(x, delta, wind, P)

    % relabel the inputs
    pn      = x(1);
    pe      = x(2);
    pd      = x(3);
    u       = x(4);
    v       = x(5);
    w       = x(6);
    phi     = x(7);
    theta   = x(8);
    psi     = x(9);
    p       = x(10);
    q       = x(11);
    r       = x(12);
    delta_e = delta(1);
    delta_a = delta(2);
    delta_r = delta(3);
    delta_t = delta(4);
    w_ns    = wind(1); % steady wind - North
    w_es    = wind(2); % steady wind - East
    w_ds    = wind(3); % steady wind - Down
    u_wg    = wind(4); % gust along body x-axis
    v_wg    = wind(5); % gust along body y-axis    
    w_wg    = wind(6); % gust along body z-axis
    
    
   w_v = [w_ns; w_es; w_ds];
   w_b = rotate(w_v, phi, theta, psi);
  
    
    % compute wind data in NED
    w_n = w_b(1) + u_wg;
    w_e = w_b(2) + v_wg;
    w_d = w_b(3) + w_wg;
    
    
    v_ab = [u-w_n; v-w_e; w-w_d];
    
    % compute air data
    Va = sqrt(v_ab(1)^2 + v_ab(2)^2 + v_ab(3)^2);
    alpha = atan(v_ab(3)/ v_ab(1));
    beta = asin(v_ab(2)/Va);
    

    % gravity forces
    f_g_x = -P.mass * P.gravity * sin(theta);
    f_g_y = P.mass * P.gravity * cos(theta) * sin(phi);
    f_g_z = P.mass * P.gravity * cos(theta) * cos(phi);
    f_g = [f_g_x; f_g_y; f_g_z];
    
    
    % stability coefficients
    c_x_alpha = -P.C_D_alpha * cos(alpha) + P.C_L_alpha * sin(alpha);
    c_x_q_alpha = -P.C_D_q * cos(alpha) + P.C_L_q * sin(alpha);
    c_x_delta_e_alpha = -P.C_D_delta_e * cos(alpha) + P.C_L_delta_e * sin(alpha);
    c_z_alpha = -P.C_D_alpha * sin(alpha) - P.C_L_delta_e*cos(alpha);
    c_z_q_alpha = -P.C_D_q * sin(alpha) - P.C_L_q * cos(alpha);
    c_z_delta_e_alpha = -P.C_D_delta_e * sin(alpha) - P.C_L_delta_e * cos(alpha);
    
    %aerodynamics forces
    tmp = 1/2*P.rho*Va^2*P.S_wing;
    f_a_x = c_x_alpha + c_x_q_alpha*P.c/(2*Va)*q + c_x_delta_e_alpha * delta_e;
    f_a_y = P.C_Y_0 + P.C_Y_beta * beta + P.C_Y_p *P.b/(2*Va)*p + P.C_Y_r*P.b/(2*Va)*r+P.C_Y_delta_a *delta_a+P.C_Y_delta_r *delta_r;
    f_a_z = c_z_alpha + c_z_q_alpha * P.c/(2*Va)*q + c_z_delta_e_alpha * delta_e;
    f_a = tmp * [f_a_x; f_a_y; f_a_z];
    
    %propulsion forces
    f_x_p = 1/2 * P.rho *P.S_prop * P.C_prop*( (P.k_motor *delta_t)^2 - Va^2);

    %aerodynamics moments
    tmp = 1/2 *P.rho * Va^2 * P.S_wing;
    m_l = P.C_ell_0 + P.C_ell_beta *beta + P.C_ell_p * P.b/(2*Va)*p + P.C_ell_r*P.b/(2*Va)*r + P.C_ell_delta_a * delta_a + P.C_ell_delta_r * delta_r;
    m_l = tmp *P.b*m_l;
    m_m = P.C_m_0 + P.C_m_alpha * alpha + P.C_m_q * P.c /(2*Va) * q + P.C_m_delta_e * delta_e;
    m_m = tmp*P.c *m_m;
    m_n = P.C_n_0 + P.C_n_beta * beta + P.C_n_p * P.b/(2* Va) *p + P.C_n_r * P.b/(2* Va) * r + P.C_n_delta_a * delta_a + P.C_n_delta_r * delta_r;
    m_n = tmp * P.b *m_n;

    % compute external forces and torques on aircraft
    Force(1) =  f_g(1) + f_a(1) + f_x_p;
    Force(2) =  f_g(2) + f_a(2);
    Force(3) =  f_g(3) + f_a(3);
    
    Torque(1) = m_l;
    Torque(2) = m_m;   
    Torque(3) = m_n;
    out = [Force'; Torque'; Va; alpha; beta; w_n; w_e; w_d];
end


%%%%%%%%%%%%%%%%%%%%%%%
function XYZ=rotate(XYZ,phi,theta,psi)
  % define rotation matrix
  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;
  % rotate vertices
  XYZ = R*XYZ;
end

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

300x250
728x90

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

 

 

 

 

300x250
728x90

 

 

 이번장은 매트랩/시뮬링크 환경에 익숙한 사람들을 대상으로 진행하겠습니다. 다세한 정보는 매트랩/시뮬링크 문서를 참조해주시기 바랍니다. 시뮬링크는 상미분 방정식 사이 연결을 푸는대 필수적인 도구입니다.

 

 시뮬링크 각각의 블록들이 다음의 구조대로 되어있다고 가정해봅시다. 여기서 $x_c$ $\in$ $\mathbb{R}$은 초기 조건이 $x_{c0}$인 연속 상태이고, $x_d$ $\in$ $\mathbb{R}^{n_d}$는 초기 조건이 $x_{d0}$인 이산 상태, u $\in$ $\mathbb{R}^m$는 블록 입력, y $\in$ $\mathbb{R}^p$는 블록의 출력이며, t는 시뮬레이션 경과 시간을 의미합니다.

 

 

 

 s 함수는 다양한 방법으로 정의 가능한 시뮬링크 도구로 여기서 2가지 방법인 레벨 1 m file s함수와 C파일 s 함수에 대해 간단하게 살펴보겠습니다. c 파일 s 함수는 c코드로 컴파일되서 m 파일 s함수 보다 빠르게 실해오딥니다.

 

 

D.1 예시 : 2차 미분 방정식

 이 장에서는 표준 2차 전달함수로 시스템을 구현하는 과정을 살펴보겠는데 여기서 레벨 1 m 파일 s 함수와 c 파일 s 함수를 사용하겠습니다.

 

 

우선 식 (D.4)를 상태 공간상에 표현하겠습니다. 제어 표준 형태 control canonical form로 나타내면

 

 

D.1.1   레벨 1 m 파일 s 함수

식 d.5, d.6을 시스템으로 구현하는 m 파일 s 함수 코드는 아래와 같습니다.

 

function [sys,x0,str,ts] = second_order_m(t,x,u,flag, zeta,wn)
    switch flag,
    case 0,
        [sys,x0,str,ts]=mdlInitializeSizes;
        % initialize block
        case 1,
        	sys=mdlDerivatives(t,x,u,zeta,wn);
            % define xdot = f(t,x,u)
		case 3,
        	sys=mdlOutputs(t,x,u,wn);
            % define xup = g(t,x,u)
		otherwise,
			sys = [];
		end
	%==========================================================%
	function [sys,x0,str,ts]=mdlInitializeSizes
		sizes = simsizes;
		sizes.NumContStates = 2;
        sizes.NumDiscStates = 0;
        sizes.NumOutputs = 1;
        sizes.NumInputs = 1;
        sizes.DirFeedthrough = 0;
        sizes.NumSampleTimes = 1;
        sys = simsizes(sizes);
        
		x0  = [0; 0];  %  define initial conditions
    	str = [];  % str is always an empty matrix
    	% initialize the array of sample times
    	ts  = [0 0];      % continuous sample time


	%==========================================================%
	function xdot=mdlDerivatives(t,x,u,zeta,wn)
 		xdot(1) = −2*zeta*wn*x(1) − wn^2*x(2) + u;
		xdot(2) = x(1);
    
	%==========================================================%
	function y=mdlOutputs(t,x,u,wn)
    	y = wn^2*x(2);

 

여기서 첫 줄은 m 파일 함수를 정의하는 부분이 됩니다. 이 함수의 입력은 항상 경과 시간 t, 상태 x(연속 상태와 이산 상태를 연결한), 입력 u, 와 플래그는 기본적으로 있어야 하며, 사용자 정의 입력 파라미터를 추가할 수도 있습니다. 이번 경우에는 $\zeta$와 $\omega_n$을 사용하겠씁니다.

 

 시뮬링크 엔진은 s 함수를 호출하고 t, x, u, flag를 파라미터로 전달시키는데 flag == 0인경우 시뮬링크 엔진은 sys 구조체를 반환합니다. 여기서 sys 구조체는 초기 상태 x0, 빈 문자열 str, 샘플 타임배열인 ts로 구서오딥니다.

 

  flag == 1인 경우 시뮬링크 엔진은 f(t, x, u) 함수를 반환하고, flag == 2일때는 g(t, x, u), flag==3 일때는 h(t, x, u)를 반환합니다.

 

 스위치 문은 flag를 이용해서 적절한 함수를 호출하게 되는데, 초기화 구문을 살펴보면 연속 상태의 수, 이산 상태의 수, 출력 수, 입력의 수를 정의 합니다. 그리고, 출력을 입력으로 피드백 시킬것인지의 여부를 지정할 수도 있습니다. 

 

 

 

 

 

 

 

 

300x250

+ Recent posts