Quaternions et gyroscope

Introduction

Un gyroscope mesure l'accélération angulaire autour des trois axes \( X \), \( Y \) et \( Z \) du repère du capteur respectivement notés \( \omega_X \), \( \omega_Y \) and \( \omega_Z \).

En intégrant deux fois ces accélérations angulaires, il est théoriquement possible d'estimer l'orientation du capteur au fil du temps. Notez qu'en pratique, ça ne fonctionne pas, car les erreurs se cumulent, c'est ce que l'on appele la dérive.

Explication

Définissons le vecteur \( S_{\omega} \) qui contient les accélérations (ou vitesses) angulaires:

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

avec \( \omega_X \), \( \omega_Y \) et \( \omega_Z \) exprimés en \( rad.s^{-2} \) (ou \( rad.s^{-1} \)).

Considérons maintenant la dérivée du quaternion qui décrit le taux de changement de l'orientation:

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

Avec :

En intégrant la dérivée du quaternion, il devient possible d'estimer l'orientation au fil du temps :

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

\( \Delta_t \) est le pas d'échantillonage.

Notons que pour certaines applications, le quaternion doit être normalisé après intégration :

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

Exemple

Cet exemple simule pendant 50 secondes l'évolution d'un gyroscope. La rotation évolue pendant 25 secondes autour de l'axe X, puis pendant 50 secondes autour de l'axe Y. Pour éviter une double intégration, nous supposons ici que le gyroscope produit une vitesse angulaire (et non une accélération).

Simulation of position estimate

Ci-dessous se trouve le script Matlab de cet exemple :

close all;
clear all;
clc;

%% Initial parameters

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

% Step time
dt=0.5;

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

%% Main loop
for i=2:100

    %% Simulate gyroscope value 
    % pi/8 rad/s  along X axis for the 50 first steps
    % pi/16 rad/s along Y axis for the 50 last steps 
    if (i<50) 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*dt;

    % 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,:)')';
    PX(i,:)=(M*PX(1,:)')';
    PY(i,:)=(M*PY(1,:)')';
    PZ(i,:)=(M*PZ(1,:)')';

    % Display the new point
    plot3 (P(i,1),P(i,2),P(i,3),'k.');
    plot3 (PX(i,1),PX(i,2),PX(i,3),'r.');
    plot3 (PY(i,1),PY(i,2),PY(i,3),'g.');
    plot3 (PZ(i,1),PZ(i,2),PZ(i,3),'b.');
    line ( [ P(i,1) , PX(i,1) ] , [ P(i,2) , PX(i,2) ],  [ P(i,3) , PX(i,3) ] ,'Color','r');
    line ( [ P(i,1) , PY(i,1) ] , [ P(i,2) , PY(i,2) ],  [ P(i,3) , PY(i,3) ] ,'Color','g');
    line ( [ P(i,1) , PZ(i,1) ] , [ P(i,2) , PZ(i,2) ],  [ P(i,3) , PZ(i,3) ] ,'Color','b');
    hold on;
    axis square equal;
    grid on;
    drawnow;

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

Les fichiers de l'exemple précédent peuvent être téléchargés ici :

quaternion_gyroscope.zip

Credit: cet exemple utilise la bibliothèque quaternion pour Matlab écrite par S. Madgwick's.

Voir aussi


Dernière mise à jour : 22/01/2021