Archives pour la catégorie Recherche

Gyroscope et quaternions

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

 S_{\omega} = \left[ \begin{matrix} 0 && \omega_x && \omega_y && \omega_z \end{matrix} \right]

\omega_X, \omega_Y et \omega_Z 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 :

 \frac{dQ_k}{dt} = \frac{1}{2}.\hat{Q}_{k-1} \otimes S_{\omega}

Uù :


  • \frac{dQ_k}{dt} est la dérivée à l’instant k exprimée dans l’espace des quaternions.
  • \hat{Q}_{k} est l’orientation estimée à l’instant k.

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

 \hat{Q}_k = \hat{Q}_{k-1}+ \Delta_t.\frac{dQ_k}{dt}

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

 Q_k^n=\frac{\hat{Q}_k}{\| \hat{Q}_k \| }

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');

trajectory

Téléchargement