www.gusucode.com > Simulink——飞行器轨迹恢复源码程序 > Simulink——飞行器轨迹恢复源码程序/Aerial_recovery_demo/mav_guidance.m
function uout = mav_guidance(uu,P)
% process inputs
[cam,mav,drogue,t] = processInputs(uu,P);
% extended Kalman filter to estimate eta_az, eta_el
% [eta_az, eta_el] = ekf(mav, cam, t, P);
% [eta_az, eta_el] = ekf_delay(mav, cam, t, P);
eta_az = 0;
eta_el = 0;
% maintain initial airspeed
u_V = sat( P.mav.k_V*(P.mav.V - mav.V), P.mav.Vdotbar);
% use pronav algorithm to prosecute target
% [u_phi, u_q] = pronav(mav, cam, eta_az, eta_el, P);
% tail chase the drogue using direct pursuit and drogue states
[u_phi, u_q] = directPursuit(mav, drogue, t, P);
% point gimbal at preset values
az_d = P.mav.gim.az; % command preset values
el_d = P.mav.gim.el;
% gimbal servo commands
u_az = sat( P.mav.k_az*(az_d - mav.az), P.mav.gim.azdotbar);
u_el = sat( P.mav.k_el*(el_d - mav.el), P.mav.gim.eldotbar);
% output of guidance algorithm
uout = [u_V; u_phi; u_q; u_az; u_el; eta_az; eta_el];
%=============================================================================
% pronav
% proportional navigation algorithm for prosecuting target
% output desired roll angle and desired pitch rate
%=============================================================================
%
function [u_phi, u_q] = pronav(mav, cam, eta_az, eta_el, P)
% determine if target is in camera field-of-view
flag_out_of_fov = isTargetInFOV(cam);
% if target is not in camera field-of-view, do nothing
% otherwise use pronav
if flag_out_of_fov, % target not in camera field-of-view
% do nothing
u_phi = 0;
u_q = 0;
else
% target is in the camera field of view
% pronav using the camera data.
% Step 0: compute rotation matrices
R_g1_b = [... % body to gimbal-1 frame
cos(mav.az), -sin(mav.az), 0;...
sin(mav.az), cos(mav.az), 0;...
0, 0, 1];
R_g_g1 = [... % gimbal-1 frame to gimbal frame
cos(mav.el), 0, sin(mav.el);...
0, 1, 0;...
-sin(mav.el), 0, cos(mav.el)];
R_g_b = R_g1_b*R_g_g1; % gimbal to body frame
R_c_g = [... % camera to gimbal frame
0, 0, 1;...
1, 0, 0;...
0, 1, 0];
R_b_b2 = [... % body to body-2 frame
1, 0, 0;...
0, cos(mav.phi), -sin(mav.phi);...
0, sin(mav.phi), cos(mav.phi)];
R_b2_b1 = [... % body-2 to body-1 frame
cos(mav.gam), 0, sin(mav.gam);...
0, 1, 0;...
-sin(mav.gam), 0, cos(mav.gam);...
];
R_b1_i = [... % body-1 to inertial frame
cos(mav.chi), -sin(mav.chi), 0;...
sin(mav.chi), cos(mav.chi), 0;...
0, 0, 1;...
];
% Step 1: compute ellhat in camera frame
% ellhat is the normalize line-of-sight vector
F = sqrt(P.cam.f^2 + cam.eps_x^2 + cam.eps_y^2);
ellhat_c = [...
cos(eta_el)*cos(eta_az);...
-sin(eta_az);...
sin(eta_el)*cos(eta_az);...
];
% Step 3: compute ellhatdot in camera frame
% ellhatdot is the normalized line-of-sight rate
Z = 1/F^3*[...
cam.eps_y^2+P.cam.f^2, -cam.eps_x*cam.eps_y;...
-cam.eps_x*cam.eps_y, cam.eps_x^2+P.cam.f^2;...
-cam.eps_x*P.cam.f, -cam.eps_y*P.cam.f];
epsdot = [cam.eps_x_dot; cam.eps_y_dot];
Ldot_div_L = (cam.eps_x*cam.eps_x_dot + cam.eps_y*cam.eps_y_dot)/F...
- cam.eps_size_dot/cam.eps_size;
omega_g_b_b = R_g1_b*[0; mav.eldot; mav.azdot];
omega_b_i_b = [mav.p; mav.q; mav.r];
omegatilde = R_c_g'*R_g_b'*(omega_g_b_b + omega_b_i_b);
ellhatdot_c = Ldot_div_L * ellhat_c + Z*epsdot +...
1/F * cross( omegatilde, [cam.eps_x; cam.eps_y; P.cam.f] );
% ellhatdot_c = cross( ellhat_c, omegatilde ) + Ldot_div_L * ellhat_c
% Step 4. Compute Omega_perp in the camera frame
Omega_perp_c = cross(ellhat_c, ellhatdot_c);
% Step 5: transform Omega_perp to the body-2 frame
Omega_perp_b2 = R_b2_b1 * R_b_b2 * R_g_b * R_c_g * Omega_perp_c;
% Step 6: compute the desired body frame acceleration
% acceleration command for pronav
N = P.mav.navigation_ratio; % navigation ratio
V_I_b2 = [mav.V; 0; 0];
a_b2 = N*cross(Omega_perp_b2, V_I_b2);
% Step 7: output autopilot commands
% use modified polar conversion logic
% sigmoid
k = P.mav.sigmoid_gain;
sig = sign(a_b2(2))*(1-exp(-k*a_b2(2)))/(1+exp(-k*a_b2(2)));
% roll command
phi_command = sig*atan2(a_b2(2),abs(a_b2(3)));
% pitch rate command
gamdot_command = -sign(a_b2(3))*norm(a_b2);
% autopilot commands: roll angle and pitch rate
u_phi = sat( P.mav.k_roll*(phi_command - mav.phi), P.mav.phidotbar );
u_q = gamdot_command;
end
%=============================================================================
% directPursuit
% direct pursuit given knowledge of drogues position
%=============================================================================
function [u_phi, u_q] = directPursuit(mav, drogue, t, P)
% line-of-sight vector in inertial frame
ell = [...
drogue.n - mav.n;...
drogue.e - mav.e;...
drogue.d - mav.d;...
];
% compute heading and flight path angle of line-of-sight vector
chi_d = atan2(ell(2),ell(1));
gam_d = asin(-ell(3) / sqrt(ell(1:2)'*ell(1:2)) );
% roll angle command
% compute desired heading rate
psi_d_dot = P.mav.k_yaw * wrap(chi_d-mav.chi);
% compute desired roll angle
u_phi = atan(mav.V*psi_d_dot/P.g);
% pitch rate command
u_q = P.mav.k_pitch * ( gam_d - mav.gam );
%=============================================================================
% wrap
% wrap chi between [-pi, pi]
%=============================================================================
function chi = wrap(chi)
while chi > pi,
chi = chi-2*pi;
end
while chi < -pi,
chi = chi+2*pi;
end
%=============================================================================
% sat
% saturate the input u at the constraint c
%=============================================================================
%
function out = sat(u,c)
if u>c,
out=c;
elseif u<-c,
out=-c;
else
out=u;
end
%========================================================
% isTargetInFOV
% determine if the target is in the FOV of camera
%========================================================
function flag_out_of_fov = isTargetInFOV(cam)
if cam.eps_size < 1,
flag_out_of_fov = 1;
else
flag_out_of_fov = 0;
end
%========================================================
% processInputs
% interpret inputs create data structures
% differentiate angles and camera values
%========================================================
function [cam, mav, drogue, t] = processInputs(uu,P)
% get current time
t = uu(end);
% declare persistent data
persistent phidot
persistent phi_k1
persistent gamdot
persistent gam_k1
persistent chidot
persistent chi_k1
persistent azdot
persistent az_k1
persistent eldot
persistent el_k1
persistent eps_x_dot
persistent eps_x_k1
persistent eps_y_dot
persistent eps_y_k1
persistent eps_size_dot
persistent eps_size_k1
M = size(P.drogue.markers,1); % number of markers on the drogue
% initialize persistent data
if t<P.mav.T_guidance,
phidot = 0;
phi_k1 = 0;
gamdot = 0;
gam_k1 = 0;
chidot = 0;
chi_k1 = 0;
azdot = 0;
az_k1 = 0;
eldot = 0;
el_k1 = 0;
eps_x_dot = zeros(M,1);
eps_x_k1 = zeros(M,1);
eps_y_dot = zeros(M,1);
eps_y_k1 = zeros(M,1);
eps_size_dot = zeros(M,1);
eps_size_k1 = zeros(M,1);
end
% camera data
NN = 0;
cam.eps_x = uu(1+NN:M+NN); % x-location of target in camera (pixels)
NN = NN+M;
cam.eps_y = uu(1+NN:M+NN); % y-location of target in camera (pixels)
NN = NN+M;
cam.eps_size = uu(1+NN:M+NN); % size of target in camera (pixels)
cam.delay = P.mav.T_cam/P.mav.T_guidance; % camera delay
% set flag if new camera data has been received.
if (cam.eps_x~=eps_x_k1) | (cam.eps_y~=eps_y_k1) | (cam.eps_size~=eps_size_k1),
cam.new_data_flag = 1;
else
cam.new_data_flag = 0;
end
% MAV data
NN = NN+M;
mav.n = uu(1+NN); % N-E-D position of MAV
mav.e = uu(2+NN);
mav.d = uu(3+NN);
mav.chi = uu(4+NN); % course
mav.V = uu(5+NN); % airspeed
mav.phi = uu(6+NN); % roll
mav.gam = uu(7+NN); % flight path angle (pitch)
mav.az = uu(8+NN); % gimbal azimuth
mav.el = uu(9+NN); % gimbal elevation
% Drogue data
NN = NN + 9;
drogue.n = uu(1+NN);
drogue.e = uu(2+NN);
drogue.d = uu(3+NN);
drogue.ndot = uu(4+NN);
drogue.edot = uu(5+NN);
drogue.ddot = uu(6+NN);
drogue.phi = uu(7+NN);
drogue.theta = uu(8+NN);
% dirty derivatives of phi, gam, az, el, eps_x, eps_y, eps_size
alpha1 = (2*P.mav.tau-P.mav.T_guidance)/(2*P.mav.tau+P.mav.T_guidance);
alpha2 = 2/(2*P.mav.tau+P.mav.T_guidance);
phidot = alpha1 * phidot + alpha2 * (mav.phi - phi_k1);
gamdot = alpha1 * gamdot + alpha2 * (mav.gam - gam_k1);
chidot = alpha1 * chidot + alpha2 * (mav.chi - chi_k1);
azdot = alpha1 * azdot + alpha2 * (mav.az - az_k1);
eldot = alpha1 * eldot + alpha2 * (mav.el - el_k1);
% alpha1 = (2*P.mav.tau-P.mav.T_cam)/(2*P.mav.tau+P.mav.T_cam);
% alpha2 = 2/(2*P.mav.tau+P.mav.T_cam);
eps_x_dot = alpha1 * eps_x_dot + alpha2 * (cam.eps_x - eps_x_k1);
eps_y_dot = alpha1 * eps_y_dot + alpha2 * (cam.eps_y - eps_y_k1);
eps_size_dot = alpha1 * eps_size_dot + alpha2 * (cam.eps_size - eps_size_k1);
mav.phidot = phidot;
mav.gamdot = gamdot;
mav.chidot = chidot;
mav.azdot = azdot;
mav.eldot = eldot;
cam.eps_x_dot = eps_x_dot; % time derivative of x-location
cam.eps_y_dot = eps_y_dot; % time derivative of y-location
cam.eps_size_dot = eps_size_dot; % time derivative of size
% update persisent variables
phi_k1 = mav.phi;
gam_k1 = mav.gam;
chi_k1 = mav.chi;
az_k1 = mav.az;
el_k1 = mav.el;
eps_x_k1 = cam.eps_x;
eps_y_k1 = cam.eps_y;
eps_size_k1 = cam.eps_size;
% convert phidot, gammadot, chidot to p, q, r
mav.p = mav.phidot - sin(mav.gam)*mav.chidot;
mav.q = cos(mav.phi)*mav.gamdot + sin(mav.phi)*cos(mav.gam)*mav.chidot;
mav.r = -sin(mav.phi)*mav.gamdot + cos(mav.phi)*cos(mav.gam)*mav.chidot;