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

1 简介

It is a known fact that quadrotor UAVs are in general under-actuated and nonlinear system and itis a challenge to control them, especially in case of aggressive maneuvers. Our goal in this projectis to study the nonlinear geometric control approach to control a quadrotor. Geometric controltheory is the study on how geometry of the state space inflfluences control problems. In controlsystems engineering, the underlying geometric features of a dynamic system are often not consideredcarefully. Difffferential geometric control techniques utilize these geometric properties for controlsystem design and analysis. The objective is to express both the system dynamics and controlinputs on nonlinear manifolds instead of local charts. Based on the geometric properties of thesystem dynamic, this difffferential geometric based approach is used to model and control the system.Also, the intent is to design a controller that gives us global stability properties i.e. the systemcan recover from any initial state. The confifiguration of the quadrotor system described on smoothnonlinear geometric confifiguration spaces has been brieflfly discussed, and analyzed with the principlesof difffferential geometry. This allows us to avoid any kind of singularities that would otherwise ariseon local charts. Further, it is possible to construct an almost-globally defifined nonlinear geometriccontroller by defifining the error function on the same spaces utilizing the geometric properties.Finally, simulations demonstrate the stability and abilities of the nonlinear geometric controller.

2 部分代码

% 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 %%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

3 仿真结果

4 参考文献

[1] Avinash Siravuru. Geometric-toolbox for matlab. https://github.com/sir-avinash/Geometry-Toolbox/tree/453528fefea00ed7c9349fbd514b555fc187c04c, 2013.