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:2switch(iter)case 1clear;%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 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 contantquad_params.params.g = 9.81 ;% interial fram axisquad_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 mquad_params.params.d = 0.315;% fixed constant in mquad_params.params.c = 8.004*10e-4;%defining parameters different for each trajectoriesquad_params.params.x_desired =  nan;quad_params.params.gen_traj = 1;        %change herequad_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 positionx_quad_0 = [0;0;0];%     xQ0 = [1;3;2];% Intial velocityv_quad_0 = zeros(3,1);% Initial orientation
%             R0 = RPYtoRot_ZXY(0*pi/180, 0*pi/180, 0*pi/180);R0 = eye(3);% Intial angular velocityw0= zeros(3,1);%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Concatenating the entire initial condition into a single vectorx0 = [x_quad_0; v_quad_0; reshape(R0,9,1); w0];%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% SIMULATIONodeopts = odeset('RelTol', 1e-8, 'AbsTol', 1e-9) ;% odeopts = [] ;[t, x] = ode15s(@odefun_quadDynamics, [0 20], x0, odeopts, quad_params) ;% Computing Various Quantitiesdisp('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 graphsplott(t,x,xd,pos_err_fx,vel_err_fx);%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%% CASE 2 %%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%        case 2clear;%%% 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 contantquad_params.params.g = 9.81 ;% interial fram axisquad_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 mquad_params.params.d = 0.315;% fixed constant in mquad_params.params.c = 8.004*10e-4;% Desired positionquad_params.params.x_desired = [0;0;0];quad_params.params.w_desired = [0;0;0];%defining parameters different for each trajectoriesquad_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 positionx_quad_0 = [0;0;0];%     xQ0 = [1;3;2];% Intial velocityv_quad_0 = zeros(3,1);% Initial orientationR0 = [1 0 0;0 -0.9995 -0.0314; 0 0.0314 -0.9995];% Intial angular velocityw0= zeros(3,1);%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Concatenating the entire initial condition into a single vectorx0 = [x_quad_0; v_quad_0; reshape(R0,9,1); w0];%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% SIMULATIONodeopts = odeset('RelTol', 1e-8, 'AbsTol', 1e-9) ;% odeopts = [] ;[t, x] = ode15s(@odefun_quadDynamics, [0 20], x0, odeopts, quad_params) ;% Computing Various Quantitiesdisp('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 graphsplott(t,x,xd,pos_err_fx,vel_err_fx);otherwisefprintf('\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.