소형 무인 비행체 4 - 과제 2

2020. 6. 4. 18:47

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

저작자표시

'로봇 > 제어' 카테고리의 다른 글

소형 무인 비행체 5 - 과제 2 (0)	2020.06.05
소형 무인 비행체 5 - 과제 - 트림 (0)	2020.06.05
소형 무인 비행체 3 - 과제 (0)	2020.06.04
시뮬링크에서 s 함수로 모델링하기 (0)	2020.06.04
소형 무인 비행체 2 - 과제 2 (0)	2020.06.02

내 블로그 - 관리자 홈 전환	`Q` `Q`
새 글 쓰기	`W` `W`

글 수정 (권한 있는 경우)	`E` `E`
댓글 영역으로 이동	`C` `C`

이 페이지의 URL 복사	`S` `S`
맨 위로 이동	`T` `T`
티스토리 홈 이동	`H` `H`
단축키 안내	`Shift` + `/` `⇧` + `/`

집밖은 위험해

소형 무인 비행체 4 - 과제 2

'로봇 > 제어' 카테고리의 다른 글

+ Recent posts

티스토리툴바

개인정보

단축키

내 블로그

블로그 게시글

모든 영역