simulation d'un quaternion basé sur un contrôleur de backstepping optimal pour un drone dans MATLAB

Je suis en train d'essayer de mettre en œuvre cet article de recherche sur le libre accès Attitude Optimal Backstepping Controller basé sur Quaternion pour un UAV.

Je n'ai pas trouvé le même résultat dans cet article. Quelqu'un peut-il me dire où se trouve mon erreur dans ce code?

Cas n ° 1 . Soit la condition initiale de l'erreur du quaternion soit , ce qui correspond à un angle ; les matrices et sont calculées à l'aide de l'équation de Riccati et est choisi pour obtenir de bonnes performances. Pour ce cas, et sera stabilisé à 1. Les figures 1, 2 et 3 montrent les réponses d'attitude et les entrées de commande, ce qui donne une convergence en un temps bref. Ces résultats sont proches de la réflexion normale lors de l’analyse de l’emplacement initial de qui se situe dans le premier quadrant. $\mathrm{Q}_{e0}= (0.8224, 0.2226, 0.4397, 0.3604)^{T}$ $\phi(0) = 34.67^{\circ}$ $P_{1}$ $P_{2}$ $\gamma_{2}$ $q_{0}(0) >0$ $\phi$

function xdot = test(x)
R1=0.1*eye(3,3);
v=[0.0098,0.0098,0.0176];
J=diag(v);
R2=0.03*eye(3,3);
gamma= 150;
Q1=eye(3,3);
Q2=eye(3,3);
W_d=[0.56;0.61;0.42];
%--------------------------------------------------------------------------
R_T=[ x(1)^2+x(2)^2-x(3)^2-x(4)^2, 2*(x(2)*x(3)+x(1)*x(4)), 2*(x(2)*x(4)-x(1)*x(3))   
          2*(x(2)*x(3)-x(1)*x(4)), x(1)^2-x(2)^2+x(3)^2-x(4)^2, 2*(x(3)*x(4)+x(1)*x(2))   
          2*(x(2)*x(4)+x(1)*x(3)), 2*(x(3)*x(4)-x(1)*x(2)), x(1)^2-x(2)^2-x(3)^2+x(4)^2];
W_star=R_T*W_d;
q0=x(1);
qv=[x(2);x(3);x(4)];
W_aux=[x(5);x(6);x(7)];

B1=0.5*[0,x(4),-x(3);-x(4),0,x(2);x(3),-x(2),0]+q0*eye(3,3);
B2=inv(J);
A2=(-inv(J)*[0,x(7),-x(6);-x(7),0,x(5);x(6),-x(5),0]*J-inv(J)*[0,W_star(3),-W_star(2);-W_star(3),0,W_star(1);W_star(2),-W_star(1),0]*J);
G2=inv(J);
B22=inv(J);
z1=[x(8);x(9);x(10)];
z2=[x(11);x(12);x(13)];
p1=[x(14) x(15) x(16);x(17) x(18) x(19);x(20) x(21) x(22)];
p2=[x(23) x(24) x(25);x(26) x(27) x(28);x(29) x(30) x(31)];
W2=1/(2*gamma^2)*B22'*p2*z2;
Ksi2=-inv(R2)*B2'*p2*z2;

v1=-inv(R1)*inv(B1)*inv(R1)*p1*qv*sign(q0);
q0dot=-0.5*qv'*W_aux;
qvdot=B1*W_aux;
B1dot=0.5*[0,-qvdot(3),qvdot(2);qvdot(3),0,-qvdot(1);-qvdot(2),qvdot(1),0]+q0dot*eye(3,3);
p1dot=p1*B1*inv(R1)*B1'*p1-Q1;
v1dot=inv((B1^2*R1^2))*(B1*p1*qvdot + B1*p1dot*qv - p1*B1dot*qv)*sign(q0);
%v1dot=-inv(R1)*invB1dot*inv(R1)*p1*x1+(-inv(R1)*inv(B1)*inv(R1))*(p1dot*x1+p1*x1dot);
v2=inv(R2)*B2'*p2*z2+inv(B2)*v1dot*sign(q0)-inv(B2)*A2*v1*sign(q0)+inv(B2)*inv(p2)*B1'*p1*z1-inv(B2)*(1/2*gamma^2)*B22*(B22')*p2*z2;
W_auxdot=A2*W_aux+B2*v2+G2*W2;
z1dot=B1*z2-B1*inv(R1)*B1'*p1*z1;
z2dot=A2*z2+B2*Ksi2-inv(p2)*B1'*p1*z1;
p1dot=p1dot(:);
T2=1/(2*gamma^2)*G2*(G2')-B2*inv(R2)*B2';
p2dot=-A2'*p2-p2*A2-p2*T2*p2-Q2;
p2dot=p2dot(:);
xdot=[q0dot;qvdot;W_auxdot;z1dot;z2dot;p1dot;p2dot];
xdot=xdot(:);
end

exécuter la fonction

% test for function deri
clc;clear ALL;close all;
[t,x]=ode45(@(t,x)test(x),[0 1],[0.8224 0.2226 0.4397 0.3604 0 0 0 1 0.5 3 2 1 1.2 0.1 0.2 0.3 0.4 0.5 0.1 0.2 0.3 0.1 0.01 0.02 0.3 0.4 0.2 0.03 0.02 0.001 0.4])

control-engineering aerospace-engineering simulation matlab optimal-control chawi19
la source

Vous attendez-vous vraiment à ce que quelqu'un ici passe du temps à lire le journal et à travailler sur votre très long message? Essayez de contacter les auteurs du document - mais vous devrez leur donner plus d'informations que "mon code ne fonctionne pas!"

alephzero

@alephzero - OP vient de copier le papier par la poste. Comme le papier est déjà lié, je l’ai enlevé.

Chuck

simulation d'un quaternion basé sur un contrôleur de backstepping optimal pour un drone dans MATLAB

Réponses: