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 |