众所周知,四旋翼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 %%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%