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.
