四旋翼无人机的几何跟踪控制matlab源码

众所周知，四旋翼UA V通常是欠驱动的非线性系统，控制它们是一个挑战，特别是在进行激进机动的情况下。我们在这个项目中的目标是研究非线性几何控制方法来控制四旋翼。几何控制理论是关于状态空间的几何如何影响控制问题的研究。在控制系统工程中，通常不会仔细考虑动态系统的基本几何特征。微分几何控制技术利用这些几何属性进行控制系统设计和分析。目的是在非线性流形上而不是局部图上表示系统动力学和控制输入。基于系统动力学的几何特性，此基于微分几何的方法用于建模和控制系统。同样，目的是设计一种控制器，该控制器具有全局稳定性，即系统可以从任何初始状态恢复。简要讨论了在光滑非线性几何配置空间上描述的四旋翼系统的配置，并根据微分几何原理进行了分析。这使我们避免了本地图表上否则会出现的任何类型的奇异之处。

% function[varargout] =  geometric_tracking_controller(varargin)
%%% INITIALZING WORKSPACE
close all; 

addpath('./Geometry-Toolbox/');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Using Geometry-Toolbox; thanks to Avinash Siravuru %%
% https://github.com/sir-avinash/geometry-toolbox    %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    
%%%Implemeting The two cases mentioned in the paper    
for iter = 1:2
    switch(iter)
    case 1
        clear;
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        %%% INITIALZING SYSTEM PARAMETERS %%%
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % moment of Inertia in Kg(m^2)
            quad_params.params.mass = 0.5 ;
        % moment of Inertia in Kg(m^2)
            quad_params.params.J = diag([0.557, 0.557, 1.05]*10e-2);
        % acceleration via gravity contant
            quad_params.params.g = 9.81 ;
        % interial fram axis
            quad_params.params.e1 = [1;0;0] ;
            quad_params.params.e2 = [0;1;0] ;
            quad_params.params.e3 = [0;0;1] ;
        % distance of center of mass from fram center in m
            quad_params.params.d = 0.315;
        % fixed constant in m
            quad_params.params.c = 8.004*10e-4;
        %defining parameters different for each trajectories
        quad_params.params.x_desired =  nan;
        quad_params.params.gen_traj = 1;        %change here
        quad_params.params.vel = nan;
        quad_params.params.acc = nan;
        quad_params.params.b1d = nan;
        quad_params.params.w_desired =  [0;0;0];
        quad_params.params.k1 = diag([5, 5 ,9]);
        quad_params.params.k2 = diag([5, 5, 10]);
        quad_params.params.kR = 200;
        quad_params.params.kOm = 1;
        
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        %%% INTIALIZING - INTIAL PARAMETERS x,v,R,w %%%
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Intial position
            x_quad_0 = [0;0;0];
        %     xQ0 = [1;3;2];
        % Intial velocity
            v_quad_0 = zeros(3,1);
        % Initial orientation
%             R0 = RPYtoRot_ZXY(0*pi/180, 0*pi/180, 0*pi/180);
        R0 = eye(3);
        % Intial angular velocity
            w0= zeros(3,1);
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Concatenating the entire initial condition into a single vector
            x0 = [x_quad_0; v_quad_0; reshape(R0,9,1); w0];
        
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        
        %%%%%%%% SIMULATION
        odeopts = odeset('RelTol', 1e-8, 'AbsTol', 1e-9) ;
        % odeopts = [] ;
        [t, x] = ode15s(@odefun_quadDynamics, [0 20], x0, odeopts, quad_params) ;

        % Computing Various Quantities
        disp('Evaluating...') ;
        index = round(linspace(1, length(t), round(1*length(t))));
        % ind = 0:length(t);
        for i = index
           [~,xd_,f_,M_] =  odefun_quadDynamics(t(i),x(i,:)',quad_params);
           xd(i,:) = xd_';
           pos_err_fx(i) = norm(x(i,1:3)-xd(i,1:3));
           vel_err_fx(i) = norm(x(i,4:6)-xd(i,4:6));
           f(i,1)= f_;
           M(i,:)= M_';
        end

        %%% Plotting graphs
        plott(t,x,xd,pos_err_fx,vel_err_fx);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%% CASE 2 %%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%        
    case 2
            clear;
        %%% INITIALZING SYSTEM PARAMETERS %%%
        % moment of Inertia in Kg(m^2)
            quad_params.params.mass = 0.5 ;
        % moment of Inertia in Kg(m^2)
            quad_params.params.J = diag([0.557, 0.557, 1.05]*10e-2);
        % acceleration via gravity contant
            quad_params.params.g = 9.81 ;
        % interial fram axis
            quad_params.params.e1 = [1;0;0] ;
            quad_params.params.e2 = [0;1;0] ;
            quad_params.params.e3 = [0;0;1] ;
        % distance of center of mass from fram center in m
            quad_params.params.d = 0.315;
        % fixed constant in m
            quad_params.params.c = 8.004*10e-4;
        % Desired position
            quad_params.params.x_desired =  [0;0;0];
            quad_params.params.w_desired =  [0;0;0];
        %defining parameters different for each trajectories
        quad_params.params.gen_traj = 0;
        quad_params.params.vel = 0;
        quad_params.params.acc = 0;
        quad_params.params.b1d = [1;0;0];
        quad_params.params.k1 = diag([5, 5 ,9]);
        quad_params.params.k2 = diag([5, 5, 10]);
        quad_params.params.kR = 50;
        quad_params.params.kOm = 2.5;
        
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        %%% INTIALIZING - INTIAL PARAMETERS x,v,R,w %%%
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Intial position
            x_quad_0 = [0;0;0];
        %     xQ0 = [1;3;2];
        % Intial velocity
            v_quad_0 = zeros(3,1);
        % Initial orientation
            R0 = [1 0 0;0 -0.9995 -0.0314; 0 0.0314 -0.9995];
        % Intial angular velocity
            w0= zeros(3,1);
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Concatenating the entire initial condition into a single vector
            x0 = [x_quad_0; v_quad_0; reshape(R0,9,1); w0];
        
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        
        %%%%%%%% SIMULATION
        odeopts = odeset('RelTol', 1e-8, 'AbsTol', 1e-9) ;
        % odeopts = [] ;
        [t, x] = ode15s(@odefun_quadDynamics, [0 20], x0, odeopts, quad_params) ;

        % Computing Various Quantities
        disp('Evaluating...') ;
        index = round(linspace(1, length(t), round(1*length(t))));
        % ind = 0:length(t);
        for i = index
           [~,xd_,f_,M_] =  odefun_quadDynamics(t(i),x(i,:)',quad_params);
           xd(i,:) = xd_';
           pos_err_fx(i) = norm(x(i,1:3)-xd(i,1:3));
           vel_err_fx(i) = norm(x(i,4:6)-xd(i,4:6));
           f(i,1)= f_;
           M(i,:)= M_';
        end

        %%% Plotting graphs
        plott(t,x,xd,pos_err_fx,vel_err_fx);
    otherwise
        fprintf('\n invalid case');
    end
end
            
    
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% %%% INTIALIZING - INTIAL PARAMETERS x,v,R,w %%%
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% % Intial position
%     x_quad_0 = [0;0;0];
% %     xQ0 = [1;3;2];
% % Intial velocity
%     v_quad_0 = zeros(3,1);
% % Initial orientation
% %     R0 = RPYtoRot_ZXY(0*pi/180, 10*pi/180, 20*pi/180);
%     R0 = RPYtoRot_ZXY(0*pi/180, 0*pi/180, 0*pi/180);
% % Intial angular velocity
%     w0= zeros(3,1);
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% % Concatenating the entire initial condition into a single vector
%     x0 = [x_quad_0; v_quad_0; reshape(R0,9,1); w0];
    
    
    
% %%%%%%%% SIMULATION
% odeopts = odeset('RelTol', 1e-8, 'AbsTol', 1e-9) ;
% % odeopts = [] ;
% [t, x] = ode15s(@odefun_quadDynamics, [0 20], x0, odeopts, quad_params) ;
% 
% % Computing Various Quantities
% disp('Evaluating...') ;
% index = round(linspace(1, length(t), round(1*length(t))));
% % ind = 0:length(t);
% for i = index
%    [~,xd_,f_,M_] =  odefun_quadDynamics(t(i),x(i,:)',quad_params);
%    xd(i,:) = xd_';
%    pos_err_fx(i) = norm(x(i,1:3)-xd(i,1:3));
%    vel_err_fx(i) = norm(x(i,4:6)-xd(i,4:6));
%    f(i,1)= f_;
%    M(i,:)= M_';
% end
% 
% %%% Plotting graphs
% plott(t,x,xd,pos_err_fx,vel_err_fx);

    
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%% Function definitions %%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%