Cette page a été traduite par traduction automatique. Cliquez ici pour voir la dernière version en anglais.

ecompass

Orientation à partir des lectures du magnétomètre et de l'accéléromètre

Syntaxe

orientation = ecompass(accelerometerReading,magnetometerReading)

orientation = ecompass(accelerometerReading,magnetometerReading,orientationFormat)

orientation = ecompass(accelerometerReading,magnetometerReading,orientationFormat,'ReferenceFrame',RF)

Description

orientation = ecompass(accelerometerReading,magnetometerReading) renvoie un quaternion qui peut faire pivoter les quantités d'un cadre parent (NED) vers un cadre enfant (capteur).

orientation = ecompass(accelerometerReading,magnetometerReading,orientationFormat) spécifie le format d'orientation comme quaternion ou matrice de rotation.

orientation = ecompass(accelerometerReading,magnetometerReading,orientationFormat,'ReferenceFrame',RF) permet également de spécifier le référentiel RF de la sortie orientation . Spécifiez RF comme 'NED' (Nord-Est-Bas) ou 'ENU' (Est-Nord-Haut). La valeur par défaut est 'NED'.

Exemples

Déterminer la déclinaison de Boston

Utilisez l'intensité du champ magnétique connue et l'accélération appropriée d'un appareil pointé vers le nord géographique de Boston pour déterminer la déclinaison magnétique de Boston.

Définissez l’accélération et l’intensité du champ magnétique connues à Boston.

magneticFieldStrength = [19.535 -5.109 47.930];
properAcceleration = [0 0 9.8];

Transmettez l'intensité du champ magnétique et l'accélération à la fonction ecompass . La fonction ecompass renvoie un opérateur de rotation de quaternion. Convertissez le quaternion en angles d'Euler en degrés.

q = ecompass(properAcceleration,magneticFieldStrength);
e = eulerd(q,'ZYX','frame');

L'angle, e, représente l'angle entre le nord géographique et le nord magnétique à Boston. Par convention, la déclinaison magnétique est négative lorsque le nord magnétique est à l’ouest du nord géographique. Annulez l’angle pour déterminer la déclinaison magnétique.

magneticDeclinationOfBoston = -e(1)

magneticDeclinationOfBoston = -14.6563

Matrice de rotation de retour

La fonction ecompass fusionne les données du magnétomètre et de l'accéléromètre pour renvoyer un quaternion qui, lorsqu'il est utilisé dans un opérateur de rotation de quaternion, peut faire pivoter les quantités d'une image parent (NED) vers une image enfant. La fonction ecompass peut également renvoyer des matrices de rotation qui effectuent des rotations équivalentes à celles de l'opérateur quaternion.

Définissez une rotation qui peut prendre un cadre parent pointant vers le nord magnétique vers un cadre enfant pointant vers le nord géographique. Définissez la rotation comme un quaternion et une matrice de rotation. Ensuite, convertissez le quaternion et la matrice de rotation en angles d'Euler en degrés à des fins de comparaison.

Définissez l'intensité du champ magnétique en microteslas à Boston, MA, lorsqu'il est pointé vers le nord géographique.

m = [19.535 -5.109 47.930];
a = [0 0 9.8];

Déterminez le quaternion et la matrice de rotation capables de faire pivoter un cadre du nord magnétique au nord géographique. Affichez les résultats pour comparaison.

q = ecompass(a,m);
quaterionEulerAngles = eulerd(q,'ZYX','frame')

quaterionEulerAngles = 1×3

   14.6563         0         0

r = ecompass(a,m,'rotmat');
theta = -asin(r(1,3));
psi = atan2(r(2,3)/cos(theta),r(3,3)/cos(theta));
rho = atan2(r(1,2)/cos(theta),r(1,1)/cos(theta));
rotmatEulerAngles = rad2deg([rho,theta,psi])

rotmatEulerAngles = 1×3

   14.6563         0         0

Déterminer le vecteur de gravité

Utilisez ecompass pour déterminer le vecteur de gravité en fonction des données d'une IMU en rotation.

Chargez les données de l'unité de mesure inertielle (IMU).

load 'rpy_9axis.mat' sensorData Fs

Déterminez l’orientation du corps du capteur par rapport au cadre NED local au fil du temps.

orientation = ecompass(sensorData.Acceleration,sensorData.MagneticField);

Pour estimer le vecteur gravité, faites d'abord pivoter les lectures de l'accéléromètre du cadre du corps du capteur vers le cadre NED à l'aide du vecteur quaternion orientation .

gravityVectors = rotatepoint(orientation,sensorData.Acceleration);

Déterminez le vecteur de gravité comme une moyenne des vecteurs de gravité récupérés au fil du temps.

gravityVectorEstimate = mean(gravityVectors,1)

gravityVectorEstimate = 1×3

    0.0000   -0.0000   10.2102

Plateforme de filature sur chenilles

Fusionnez les données modélisées d'un accéléromètre et d'un gyroscope pour suivre une plate-forme en rotation à l'aide de données à la fois idéalisées et réalistes.

Générer une trajectoire de vérité terrain

Décrivez l’orientation de la plateforme par rapport à la vérité terrain au fil du temps. Utilisez l' kinematicTrajectory System object™ pour créer une trajectoire pour une plate-forme qui n'a pas de translation et tourne autour de son axe z .

duration = 12;
fs = 100;
numSamples = fs*duration;

accelerationBody = zeros(numSamples,3);

angularVelocityBody = zeros(numSamples,3);
zAxisAngularVelocity = [linspace(0,4*pi,4*fs),4*pi*ones(1,4*fs),linspace(4*pi,0,4*fs)]';
angularVelocityBody(:,3) = zAxisAngularVelocity;

trajectory = kinematicTrajectory('SampleRate',fs);

[~,orientationNED,~,accelerationNED,angularVelocityNED] = trajectory(accelerationBody,angularVelocityBody);

Modèle recevant des données IMU

Utilisez un System object imuSensor pour imiter les données reçues d'une IMU qui contient un magnétomètre idéal et un accéléromètre idéal.

IMU = imuSensor('accel-mag','SampleRate',fs);
[accelerometerData,magnetometerData] = IMU(accelerationNED, ...
                                           angularVelocityNED, ...
                                           orientationNED);

Fusionner les données IMU pour estimer l'orientation

Transmettez les données de l'accéléromètre et du magnétomètre à la fonction ecompass pour estimer l'orientation dans le temps. Convertissez l'orientation en angles d'Euler en degrés et tracez le résultat.

orientation = ecompass(accelerometerData,magnetometerData);
orientationEuler = eulerd(orientation,'ZYX','frame');

timeVector = (0:numSamples-1).'/fs;

figure(1)
plot(timeVector,orientationEuler)
legend('z-axis','y-axis','x-axis')
xlabel('Time (s)')
ylabel('Rotation (degrees)')
title('Orientation from Ideal IMU')

Répétez l'expérience avec un modèle de capteur IMU réaliste

Modifiez les paramètres de l' System object IMU pour obtenir des données réalistes du capteur IMU. Réinitialisez le IMU , puis appelez-le avec la même accélération, vitesse angulaire et orientation de vérité terrain. Utilisez ecompass pour fusionner les données IMU et tracer les résultats.

IMU.Accelerometer = accelparams( ...
    'MeasurementRange',20, ...
    'Resolution',0.0006, ...
    'ConstantBias',0.5, ...
    'AxesMisalignment',2, ...
    'NoiseDensity',0.004, ...
    'BiasInstability',0.5);
IMU.Magnetometer = magparams( ...
    'MeasurementRange',200, ...
    'Resolution',0.01);
reset(IMU)

[accelerometerData,magnetometerData] = IMU(accelerationNED,angularVelocityNED,orientationNED);

orientation = ecompass(accelerometerData,magnetometerData);
orientationEuler = eulerd(orientation,'ZYX','frame');

figure(2)
plot(timeVector,orientationEuler)
legend('z-axis','y-axis','x-axis')
xlabel('Time (s)')
ylabel('Rotation (degrees)')
title('Orientation from Realistic IMU')

Arguments d'entrée

accelerometerReading — Lectures de l'accéléromètre dans le système de coordonnées du corps du capteur (m/s²)

N-matrice par 3

Lectures de l'accéléromètre dans le système de coordonnées du corps du capteur en m/s², spécifié comme une matrice N-by-3. Les colonnes de la matrice correspondent aux axes x-, y- et z - du corps du capteur. Les lignes de la matrice, N, correspondent à des échantillons individuels. Les lectures de l'accéléromètre sont normalisées avant utilisation dans la fonction.

Types de données : single | double

magnetometerReading — Lectures du magnétomètre dans le système de coordonnées du corps du capteur (µT)

N-matrice par 3

Lectures du magnétomètre dans le système de coordonnées du corps du capteur en µT, spécifiées sous la forme d'une matrice N-by-3. Les colonnes de la matrice correspondent aux axes x-, y- et z - du corps du capteur. Les lignes de la matrice, N, correspondent à des échantillons individuels. Les lectures du magnétomètre sont normalisées avant utilisation dans la fonction.

Types de données : single | double

orientationFormat — Format utilisé pour décrire l'orientation

'quaternion' (par défaut) | 'rotmat'

Format utilisé pour décrire l'orientation, spécifié comme 'quaternion' ou 'rotmat'.

Types de données : char | string

Arguments de sortie

orientation — Orientation qui fait pivoter les quantités du système de coordonnées global au système de coordonnées du corps du capteur

N vecteur -par 1 de quaternions (par défaut) | Tableau 3 par 3 par N

Orientation qui peut faire pivoter des quantités d'un système de coordonnées global vers un système de coordonnées corporelles, renvoyé sous forme de vecteur de quaternions ou de tableau. La taille et le type du orientation dépendent du format utilisé pour décrire l'orientation :

• 'quaternion' –– N-par-1 vecteur de quaternions avec le même type de données sous-jacentes que l'entrée

• 'rotmat' –– 3-by-3-by- N tableau du même type de données que l'entrée

Types de données : quaternion | single | double

Algorithmes

La fonction ecompass renvoie un quaternion ou une matrice de rotation qui peut faire pivoter les quantités d'une image parent (NED par exemple) vers une image enfant (capteur). Pour les deux formats d'orientation de sortie, l'opérateur de rotation est déterminé en calculant la matrice de rotation.

La matrice de rotation est d'abord calculée avec un intermédiaire :

$$R=\left[\begin{array}{ccc}& & \\ (a\times m)\times a& a\times m& a\\ & & \end{array}\right]$$

puis normalisé par colonne. a et m sont respectivement l'entrée accelerometerReading et l'entrée magnetometerReading .

Pour comprendre le calcul de la matrice de rotation, considérons un point arbitraire sur la Terre et son repère NED local correspondant. Supposons un cadre de corps de capteur, [x, y, z], avec la même origine.

Rappelons que l'orientation d'un corps de capteur est définie comme l'opérateur de rotation (matrice de rotation ou quaternion) nécessaire pour faire pivoter une quantité d'un cadre parent (NED) vers un cadre enfant (corps du capteur) :

$$\left[\begin{array}{ccc}& & \\ & R& \\ & & \end{array}\right]\left[\begin{array}{c}\\ p_{\text{parent}}\\ \end{array}\right]=\left[\begin{array}{c}\\ p_{\text{child}}\\ \end{array}\right]$$

où

• R est une matrice de rotation 3x3, qui peut être interprétée comme l'orientation du cadre enfant.

• p parent est un vecteur 3 par 1 dans le cadre parent.

• p child est un vecteur 3 par 1 dans le cadre enfant.

Pour un corps de capteur stable, un accéléromètre renvoie l'accélération due à la gravité. Si le corps du capteur est parfaitement aligné avec le système de coordonnées NED, toute accélération due à la gravité se fait le long de l'axe z et l'accéléromètre indique [0 0 1]. Considérons la matrice de rotation requise pour faire pivoter une quantité du système de coordonnées NED vers une quantité indiquée par l'accéléromètre.

$$\left[\begin{array}{ccc}{r}_{11}& r_{21}& r_{31}\\ r_{12}& r_{22}& r_{32}\\ r_{13}& r_{23}& r_{33}\end{array}\right]\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]=\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]$$

La troisième colonne de la matrice de rotation correspond à la lecture de l'accéléromètre :

$$\left[\begin{array}{c}{r}_{31}\\ r_{32}\\ r_{33}\end{array}\right]=\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]$$

Une lecture du magnétomètre pointe vers le nord magnétique et se trouve dans le plan N- D . Encore une fois, considérons un cadre de corps de capteur aligné avec le système de coordonnées NED.

Par définition, l'axe E est perpendiculaire au plan N- D , donc D ⨯ N = E, dans une certaine mise à l'échelle d'amplitude. Si le cadre du corps du capteur est aligné avec NED, le vecteur d'accélération de l'accéléromètre et le vecteur de champ magnétique du magnétomètre se trouvent dans le plan N- D . Par conséquent a ⨯ m = y, encore une fois avec une certaine mise à l'échelle d'amplitude.

Considérez la matrice de rotation requise pour faire pivoter NED vers le cadre enfant, [x y z].

$$\left[\begin{array}{ccc}{r}_{11}& r_{21}& r_{31}\\ r_{12}& r_{22}& r_{32}\\ r_{13}& r_{23}& r_{33}\end{array}\right]\left[\begin{array}{c}0\\ 1\\ 0\end{array}\right]=\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]\times \left[\begin{array}{c}{m}_1\\ m_2\\ m_3\end{array}\right]$$

La deuxième colonne de la matrice de rotation correspond au produit vectoriel de la lecture de l'accéléromètre et de la lecture du magnétomètre :

$$\left[\begin{array}{c}{r}_{21}\\ r_{22}\\ r_{23}\end{array}\right]=\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]\times \left[\begin{array}{c}{m}_1\\ m_2\\ m_3\end{array}\right]$$

Par définition d'une matrice de rotation, la colonne 1 est le produit croisé des colonnes 2 et 3 :

$$\begin{array}{l}\left[\begin{array}{c}{r}_{11}\\ r_{12}\\ r_{13}\end{array}\right]=\left[\begin{array}{c}{r}_{21}\\ r_{22}\\ r_{23}\end{array}\right]\times \left[\begin{array}{c}{r}_{31}\\ r_{32}\\ r_{33}\end{array}\right]\\ =\left(a\times m\right)\times a\end{array}$$

Enfin, la matrice de rotation est normalisée par colonne :

$$R_{ij}=\frac{R_{ij}}{\sqrt{{\displaystyle \sum _{i=1}^3{R}_{ij}^2}}},\forall j$$

Remarque

L'algorithme ecompass utilise le nord magnétique, et non le nord vrai, pour le système de coordonnées NED.

Références

[1] Open Source Sensor Fusion. https://github.com/memsindustrygroup/Open-Source-Sensor-Fusion/tree/master/docs

Capacités étendues

Génération de code C/C++
Générez du code C et C++ avec MATLAB® Coder™.

Historique des versions

Introduit dans R2018b

Voir aussi

ahrsfilter | imufilter