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 :
- \( \frac{dQ_k}{dt} \) est la dérivée à l'instant \( k \) exprimée dans l'espace des quaternions.
- \( \hat{Q}_{k} \) est l'estimation de l'orientation à l'instant \( k \).
- \( \otimes \) est le produit de quaternions.
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).
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 :
Credit: cet exemple utilise la bibliothèque quaternion pour Matlab écrite par S. Madgwick's.
Voir aussi
