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