image thumbnail

Aerial Recovery Concept Demo (Gauss's Principle)

by

 

28 Feb 2013 (Updated )

Demonstrate the concept of aerial recovery of miniature aerial vehicles using a towed-cable system.

mav_guidance(uu,P)
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;  

Contact us