SECRETARÍA DE TRABAJO Resolución 1138/2015

clear; clc;

Mixing Control

%****************************************************************** % ROTOR INDEX HUMMINGBIRD (NO ROS)

% Rotor[1] : front % Rotor[2] : right % Rotor[3] : rear % Rotor[4] : left

disp('ROTOR INDEX HUMMINGBIRD (NO ROS):'); disp('Rotor[1] : front');

disp('Rotor[2] : right'); disp('Rotor[3] : rear'); disp('Rotor[4] : left'); disp(' ');

disp('MIXING CONTROL:');

syms deltaPhi deltaThe deltaPsi deltaZ % MIXING CONTROL MATRIX

% deltaOmegaFront = deltaThe - deltaPsi + deltaZ % deltaOmegaRight = -deltaPhi + deltaPsi + deltaZ % deltaOmegaRear = -deltaThe - deltaPsi + deltaZ % deltaOmegaLeft = deltaPhi + deltaPsi + deltaZ

% MixingM = [ deltaPHI, deltaTheta , deltaPsi, deltaZ] MixingM = [ 0 1 -1 -1;-1 0 1 -1;0 -1 -1 -1;1 0 1 -1];

deltaOmega = MixingM * [deltaPhi;deltaThe;deltaPsi;deltaZ]

Motor

%****************************************************************** % Debido que la dinamica del motor es mayor que la dinamica del quadrotor % se aproxima por una ganancia estatica

syms Kmotor

disp('W ROTOR IN RAD/S:');

OmegaFront = deltaOmega(1)*Kmotor OmegaRight = deltaOmega(2)*Kmotor OmegaRear = deltaOmega(3)*Kmotor OmegaLeft = deltaOmega(4)*Kmotor

disp('Pulsar cualquier tecla para continuar'); disp(' ');

pause();

ESTUDIO DEL CONTROL DE ACTITUD DE UN QUADROTOR HUMMINGBIRD DE ASCTEC UTILIZANDO ROS (ROBOTIC OPERATING SYSTEM) Departament d’Enginyeria de Sistemes, Automàtica i Informàtica Industrial

Quadrotor + ROT MATRIX

%****************************************************************** % QUADROTOR SYMBOLIC

%****************************************************************** % QUADROTOR STATES

syms P Q R Phi Theta Psi U V W X Y Z; % QUADROTOR FORCES AND MOMENTS syms Fbx Fby Fbz Mbx Mby Mbz; % QUADROTOR PARAMETERS syms Jxx Jyy Jzz m l h g Jr;

%****************************************************************** % ROT MATRIX DECLARATION

%****************************************************************** sPhi = sin(Phi); sThe = sin(Theta); sPsi = sin(Psi);

cPhi = cos(Phi); cThe = cos(Theta); cPsi = cos(Psi); tThe = tan(Theta);

% ROT Phi,X

disp('MATRIX ROT Phi,X: ');

Rx = [1 0 0; 0 cPhi -sPhi; 0 sPhi cPhi] % ROT Theta,Y

disp('MATRIX ROT Theta,Y: ');

Ry = [cThe 0 sThe; 0 1 0; -sThe 0 cThe] % ROT Psi,Z

disp('MATRIX ROT Psi,Z: ');

Rz = [cPsi -sPsi 0; sPsi cPsi 0; 0 0 1]

% ROT BODY FRAME TO EARTH FRAME Reb = R(Z,Psi)R(Y,The)R(X,Phi) % [ec. 4-10 pág. PDF 51 P.SANCHEZ]

disp('BODY FRAME TO EARTH FRAME Reb = Rz*Ry*Rx ') Reb = Rz*Ry*Rx

% RELATION BETWEEN ANGULAR VEL [ec. 4-15 pág.47 PDF P.SANCHEZ] disp('RELATION BETWEEN ANGULAR VEL: ')

H_Phi = [1 sPhi*tThe cPhi*tThe; 0 cPhi -sPhi; 0 sPhi/cThe cPhi/cThe]

%****************************************************************** % STATE EQUATIONS

%****************************************************************** disp('Pulsar cualquier tecla para continuar');

disp(' '); pause();

disp('NON-LINEAL EQUATIONS:');

% ANGULAR VEL IN BODY FRAME OMEGA_b = [P;Q;R];

% VEL LINEAL IN BODY FRAME V_b = [U;V;W];

% FORCES AND MOMENTS Fb = [Fbx;Fby;Fbz]; Mb = [Mbx;Mby;Mbz]; % QUADROTOR INERTIA Jb = diag([Jxx;Jyy;Jzz]);

ESTUDIO DEL CONTROL DE ACTITUD DE UN QUADROTOR HUMMINGBIRD DE ASCTEC UTILIZANDO ROS (ROBOTIC OPERATING SYSTEM) Departament d’Enginyeria de Sistemes, Automàtica i Informàtica Industrial

% ANGULAR ACC IN BODY FRAME (dP,dQ,dR)

% dw_b = 1/J * [M_b - (w_b X J*w_b)] [ec. 4 pág. 23 PDF A. CABEZA ] disp('ANGULAR ACC IN BODY FRAME (dP,dQ,dR):')

disp('dw_b = 1/J * [M_b - (w_b X J*w_b)] [ec. 4 pág. 23 PDF A. CABEZA ]'); dOMEGA_b = simplify( Jbinv*(Mb- cross(OMEGA_b,Jb*OMEGA_b)) );

% ANGLE VAR (dPhi,dTheta,dPsi) disp('ANGLE VAR (dPhi,dTheta,dPsi):') disp('Phidot = H_Phi*[P;Q;R]'); Phidot = simplify( H_Phi*OMEGA_b );

% LINEAL ACC IN BODY FRAME (dU,dV,dW)

% dV_b = 1/m * F_b - (w_b X V_b) [ ec.3 pág.23 PDF A. CABEZA ] disp('LINEAL ACC IN BODY FRAME (dU,dV,dW):');

disp('dV_b = 1/m * F_b - (w_b X V_b) [ ec.3 pág.23 PDF A. CABEZA ]'); dV_b = simplify( ((1/m)*Fb)-cross(OMEGA_b, V_b) );

% LINEAL VEL IN EARTH FRAME (dX,dY,dZ) % dPos_e = Reb*V_b

disp('LINEAL VEL IN EARTH FRAME (dX,dY,dZ):'); disp('dPos_e = Reb*V_b');

dPos_e = simplify( Reb*V_b );

disp('EQUATIONS f=[dP dQ dR dPhi dThe dPsi dU dV dW dX dY dZ]: ')

dP = dOMEGA_b(1); %dx1 dQ = dOMEGA_b(2); %dx2 dR = dOMEGA_b(3); %dx3 dPhi = Phidot(1); %dx4 dThe = Phidot(2); %dx5 dPsi = Phidot(3); %dx6 dU = dV_b(1); %dx7 dV = dV_b(2); %dx8 dW = dV_b(3); %dx9 dX = dPos_e(1); %dx10 dY = dPos_e(2); %dx11 dZ = dPos_e(3); %dx12

f = [dP dQ dR dPhi dThe dPsi dU dV dW dX dY dZ].'

disp('Pulsar cualquier tecla para continuar'); disp(' ');

pause();

% ROTOR INDEX HUMMINGBIRD (NO ROS) % Rotor[1] : front

% Rotor[2] : right % Rotor[3] : rear % Rotor[4] : left

syms OmegaR Tfront Trear Tleft Tright Qmfront Qmrear Qmleft Qmright

%SUBSTITUIR Mbx,Mby,Mbz,Fbx,Fby y Fbz

% No se tienen en cuenta para la linealización sobre (HOVER): % · Fuerzas de Hub y Drag

ESTUDIO DEL CONTROL DE ACTITUD DE UN QUADROTOR HUMMINGBIRD DE ASCTEC UTILIZANDO ROS (ROBOTIC OPERATING SYSTEM) Departament d’Enginyeria de Sistemes, Automàtica i Informàtica Industrial

disp('Moments:');

disp('Replace Mbx = Jr*OmegaR*Q+l*(Tleft-Tright) in dP (HOVER):') dP = simplify( subs( dP,{Mbx},[Jr*Q*OmegaR+l*(Tleft-Tright)] )) disp('Replace Mby = -Jr*OmegaR*P+l*(Tfront-Trear) in dQ (HOVER):') dQ = simplify( subs( dQ,{Mby},[-Jr*P*OmegaR+l*(Tfront-Trear)] ))

disp('Replace Mbz = -Qmfront + Qmright - Qmrear + Qmleft in dR (HOVER):') dR = simplify( subs( dR,{Mbz},[-Qmfront+Qmright-Qmrear+Qmleft] ))

disp(' ');

disp('Forces:');

disp('Gravity Force in Body Frame:') Fgb = Reb.'*[0;0;m*g]

disp('Thrust Force in Body Frame:')

Tb = sum([Tfront;Tright;Trear;Tleft])*[0;0;-1]

disp('Replace Fbx = Fgbx + Tbx in dU (HOVER):') dU = simplify( subs( dU,{Fbx},[Fgb(1)+Tb(1)] )) disp('Replace Fby = Fgby + Tby in dV (HOVER):') dV = simplify( subs( dV,{Fby},[Fgb(2)+Tb(2)] )) disp('Replace Fbz = Fgbz + Tbz in dW (HOVER):') dW = simplify( subs( dW,{Fbz},[Fgb(3)+Tb(3)] ))

f2 = [dP dQ dR dPhi dThe dPsi dU dV dW dX dY dZ].'

disp('Pulsar cualquier tecla para continuar'); disp(' ');

pause();

Substituir fuerzas de Thrust y Momento de Drag

disp('SUBSTITUIR FUERZAS DE THRUST Y MOMENTO DE DRAG POR VEL. ANG. ROTOR:'); syms Kthrust Kcounter

disp(' · Replace Tfront = Kthrust*(OmegaFront^2) in f2 (HOVER)') disp(' · Replace Tright = Kthrust*(OmegaRight^2) in f2 (HOVER)') disp(' · Replace Trear = Kthrust*(OmegaRear^2) in f2 (HOVER)') disp(' · Replace Tleft = Kthrust*(OmegaLeft^2) in f2 (HOVER)') disp(' · Replace Qmfront = Kcounter*(OmegaFront^2) in f2 (HOVER)') disp(' · Replace Qmright = Kcounter*(OmegaRight^2) in f2 (HOVER)') disp(' · Replace Qmrear = Kcounter*(OmegaRear^2) in f2 (HOVER)') disp(' · Replace Qmleft = Kcounter*(OmegaLeft^2) in f2 (HOVER)')

disp(' · Replace OmegaR = (OmegaRight + OmegaLeft - OmegaFront - OmegaRear) in f2 (HOVER)') f_non_lineal = simplify(... subs( f2,... {Tfront,Tright,Trear,Tleft,Qmfront,Qmright,Qmrear,Qmleft,OmegaR},... [ Kthrust*(OmegaFront^2),Kthrust*(OmegaRight^2),... Kthrust*(OmegaRear^2),Kthrust*(OmegaLeft^2),... Kcounter*(OmegaFront^2),Kcounter*(OmegaRight^2),... Kcounter*(OmegaRear^2),Kcounter*(OmegaLeft^2),...

ESTUDIO DEL CONTROL DE ACTITUD DE UN QUADROTOR HUMMINGBIRD DE ASCTEC UTILIZANDO ROS (ROBOTIC OPERATING SYSTEM) Departament d’Enginyeria de Sistemes, Automàtica i Informàtica Industrial

disp(' ');

disp('Evaluamos en f_non_lineal en el punto de HOVER:') disp('Conocemos que [P,Q,R,Phi,Theta,Psi,U,V,W] = 0') disp('Y [X,Y,Z] pueden tener cualquier valor')

% Si evaluamos en el Punto de HOVER: % [P,Q,R,Phi,Theta,Psi,U,V,W] = 0 % [X,Y,Z] pueden tener cualquier valor

f_nl_hover_eval = simplify( subs( f_non_lineal,...

{P Q R Phi Theta Psi U V W},sym(zeros(1,9)) ) )

disp('Reducimos f para facilitar encontrar las Ueq del sistema:'); index =1; for i=1:size(f_nl_hover_eval,1) if(f_nl_hover_eval(i) ~= 0) f_nl_hover_eval_red(index)=f_nl_hover_eval(i); index = index+1; end end f_nl_hover_eval_red = f_nl_hover_eval_red.'

disp('Pulsar cualquier tecla para continuar'); disp(' ');

pause();

disp('Calculamos los puntos de equilibrio del sistema restantes:'); disp(' '); [Sol_f_nl] = solve(f_nl_hover_eval_red == 0,... [deltaPhi,deltaThe,deltaPsi,deltaZ]); Ueq_Matrix = [simplify(Sol_f_nl.deltaPhi),... simplify(Sol_f_nl.deltaThe),... simplify(Sol_f_nl.deltaPsi),... simplify(Sol_f_nl.deltaZ)]; for i = 1:size(Ueq_Matrix,1)

disp('Punto equilibrio '+string(i)+':') disp(Ueq_Matrix(i,:));

end

UeqPointer = input('Escoga el vector Ueq de equilibrio para la evaluación: ');

if (UeqPointer == 0) || (UeqPointer>size(Ueq_Matrix,1)) disp('Punto de equilibrio escogido no existente')

end

Calculo Matrices Lineales Mediante Jacobianas

disp(' ');

disp('Calcular matrices lineales A y B del espacio de estados (SYMBOLIC):'); disp(' ');

ESTUDIO DEL CONTROL DE ACTITUD DE UN QUADROTOR HUMMINGBIRD DE ASCTEC UTILIZANDO ROS (ROBOTIC OPERATING SYSTEM) Departament d’Enginyeria de Sistemes, Automàtica i Informàtica Industrial

StateVector = [P,Q,R,Phi,Theta,Psi,U,V,W,X,Y,Z] InputVector = [deltaPhi,deltaThe,deltaPsi,deltaZ]

%

AL.A_Symbolic = simplify( jacobian(f_non_lineal,StateVector) ); BL.B_Symbolic = simplify( jacobian(f_non_lineal,InputVector) ); %

AL.AL_hover_Sym = AL.A_Symbolic; BL.BL_hover_Sym = BL.B_Symbolic;

eqPoint_sym = [StateVector(1:9) InputVector]

eqPoint_value = [zeros(1,9) Ueq_Matrix(UeqPointer,:)]

for i = 1:length(eqPoint_sym) AL.AL_hover_Sym = simplify (... subs(AL.AL_hover_Sym,{eqPoint_sym(1,i)},[eqPoint_value(1,i)])... ); end for i = 1:length(eqPoint_sym) BL.BL_hover_Sym = simplify (... subs(BL.BL_hover_Sym,{eqPoint_sym(1,i)},[eqPoint_value(1,i)])... ); end

disp('Matriz AL y BL evaluada en el Punto de equilibrio:'); eqPoint_value disp(' '); disp('AL:'); AL.AL_hover_Sym disp('BL:'); BL.BL_hover_Sym load aeroParam.mat load quadrotorModel.mat load Aero_hover_parameters.mat load Motor_Lineal_Data.mat

Compute Kmotor, Kthrust and Kcounter Gains

% Thrust in Hover:

% Tjh = CThj*rho*A*(Omegaj*Radius)^2*Radius = Kthrust*(Omegaj)^2

Kthrust_value = CTh(1)*aeroParam.rho*...

quadrotorModel.Surface*(quadrotorModel.Radius)^2 % Counter - Torque in Hover:

% Qmjh = CQhj*rho*A*(Omegaj*Radius)^2*Radius = Kcounter*(Omegaj)^2 Kcounter_value = CQh(1)*aeroParam.rho*...

quadrotorModel.Surface*(quadrotorModel.Radius)^3 % STATIC GAIN ROTOR 1st ORDER TRANSFER FUNC

ESTUDIO DEL CONTROL DE ACTITUD DE UN QUADROTOR HUMMINGBIRD DE ASCTEC UTILIZANDO ROS (ROBOTIC OPERATING SYSTEM) Departament d’Enginyeria de Sistemes, Automàtica i Informàtica Industrial

Algebraic Equilibrium Point

%[P,Q,R,Phi,The,Psi,U,V,W,deltaPhi,deltaThe,deltaPhi,deltaZ] eqPoint_value_alg = eval(... simplify(... subs(eqPoint_value,{g,m,Kmotor,Kthrust,Kcounter},... [quadrotorModel.g,quadrotorModel.mass,Kmotor_value,Kthrust_value,... Kcounter_value])... )... )

Compute Algebraic AL and BL Matrix

AL.AL_hover_Alg = AL.AL_hover_Sym; BL.BL_hover_Alg = BL.BL_hover_Sym; ParamVector_sym =[Kmotor,Kthrust,Kcounter,g,m,l,Jxx,Jyy,Jzz,Jr] ParamVector_value=[... Kmotor_value,Kthrust_value,Kcounter_value,... quadrotorModel.g,quadrotorModel.mass,quadrotorModel.Length,... quadrotorModel.Jb(1,1),quadrotorModel.Jb(2,2),quadrotorModel.Jb(3,3),... aeroParam.Jr] % Algebraic AL matrix for i = 1:length(ParamVector_sym) AL.AL_hover_Alg = simplify (... subs(AL.AL_hover_Alg,{ParamVector_sym(1,i)},[ParamVector_value(1,i)])... ); end % Algebraic BL matrix for i = 1:length(ParamVector_sym) BL.BL_hover_Alg = simplify (... subs(BL.BL_hover_Alg,{ParamVector_sym(1,i)},[ParamVector_value(1,i)])... ); end

Compute Numerical Value

AL.AL_hover_eval = eval(AL.AL_hover_Alg); BL.BL_hover_eval = eval(BL.BL_hover_Alg);

Crear Lineal Model (State Space)

% SUPONEMOS QUE PODEMOS LEER CUALQUIER ESTADO C = eye(size(AL.AL_hover_eval));

% NO DISTURBANCE % D [nestados x en entradas]

D = zeros(size(AL.AL_hover_eval,1),size(BL.BL_hover_eval,2));

Lineal_sys = ss(AL.AL_hover_eval,BL.BL_hover_eval,C,D); Lineal_sys.StateName ={'P';'Q';'R';'Phi';'The';'Psi';...

ESTUDIO DEL CONTROL DE ACTITUD DE UN QUADROTOR HUMMINGBIRD DE ASCTEC UTILIZANDO ROS (ROBOTIC OPERATING SYSTEM) Departament d’Enginyeria de Sistemes, Automàtica i Informàtica Industrial

Lineal_sys

save('Lineal_system.mat','Lineal_sys')

Motor Omega Ic %eqPoint=[P,Q,R,Phi,The,Psi,U,V,W,Vx,Vy,Vz,deltaPhi,deltaThe,deltaPhi,deltaZ] deltaOmega = simplify(... subs(deltaOmega,{deltaPhi,deltaThe,deltaPsi,deltaZ},... [eqPoint_value_alg(end-3),eqPoint_value_alg(end-2),... eqPoint_value_alg(end-1),eqPoint_value_alg(end)])... ); deltaOmega = eval(deltaOmega) OmegaFrontHover = Kmotor_value*deltaOmega(1) OmegaRightHover = Kmotor_value*deltaOmega(2) OmegaRearHover = Kmotor_value*deltaOmega(3) OmegaLeftHover = Kmotor_value*deltaOmega(4)

ESTUDIO DEL CONTROL DE ACTITUD DE UN QUADROTOR HUMMINGBIRD DE ASCTEC UTILIZANDO ROS (ROBOTIC OPERATING SYSTEM) Departament d’Enginyeria de Sistemes, Automàtica i Informàtica Industrial