Un gyroscope mesure une vitesse angulaire autour des trois axes , et du repère du capteur que nous nomerons respectivement , et . En intégrant cette vitesse angulaire, il est théoriquement possible d’estimer l’orientation au cours du temps. Définissons le vecteur contenant les vitesses angulaires :

Où , et sont exprimés en rad/s.

Considérons maintenant le quaternion dérivé qui décrit le changement d’orientation lié à la mesure du gyroscope :

Uù :

• est la dérivée à l’instant exprimée dans l’espace des quaternions.
  
• est l’orientation estimée à l’instant .
  

En intégrant cette dérivée, il devient possible d’estimer l’orientation au cours du temps:

Pour certaines applications, le quaternion doit être normalisé après l’intégration:

Exemple

close all;
clear all;
clc;


%% Initial parameters

% Initial quaternion
Q(1,:)=[1 0 0 0];

% Step time
dt=0.1;

% Initial coordinates of the point
P(1,:)=[1,2,3];



%% Main loop
for i=2:500
    
    %% Simulate gyroscope value 
    % pi/8 rad/s for the 250 first steps
    % pi/16 rad/s for the 250 last steps
    if (i<250) Sw=[0 pi/8 0 0]; else Sw=[0 0 pi/16 0]; end;

    %% Update orientation
    % Compute the quaternion derivative
    Qdot=quaternProd(0.5*Q(i-1,:),Sw);
    % Update the estimated position
    Q(i,:)=Q(i-1,:)+Qdot*0.1;
    % Normalize quaternion
    Q(i,:)=Q(i,:)/norm(Q(i,:));
    
    %% Update point coordinates
    % Compute the associated transformation marix
    M=quatern2rotMat(Q(i,:));
    % Calculate the coordinate of the new point
    P(i,:)=(M*P(1,:)')';
end


%% Display point trajectory
plot3 (P(:,1),P(:,2),P(:,3),'r.');
axis square equal;
grid on;
xlabel ('x');
ylabel ('y');
zlabel ('z');

Téléchargement

Quaternions and gyroscope example 1.54 KB

Quaternion_Gyroscope.zip