👩🏽‍✈️ 🔎 🤦🏿 Accéléromètres, magnétomètres et angles d'orientation MEMS 🏢 👎🏿 👩🏾‍🏭

Lors du développement de dispositifs dans lesquels il est nécessaire d'évaluer les angles d'orientation, la question peut se poser - quel capteur MEMS choisir. Les fabricants de capteurs fournissent de nombreux paramètres à partir desquels nous devons obtenir des informations utiles sur la qualité du capteur.

Pour comprendre sur quel type de précision d'angle nous pouvons compter, vous devez faire un certain effort.

TL; DR : Un petit script pour Octave / MATLAB est décrit, ce qui permet d'estimer les erreurs dans le calcul des angles d'orientation à partir des mesures MEMS des accéléromètres et magnétomètres. À l'entrée du script - paramètres des capteurs des fiches techniques (et / ou erreurs d'étalonnage). L'article peut être utile à ceux qui commencent à utiliser des capteurs inertiels dans leurs appareils. Un petit programme éducatif sur les capteurs est inclus. Lien vers github aussi.

Nous acceptons immédiatement les conditions suivantes:

Nous voulons évaluer les angles d'orientation d'un appareil stationnaire.

Pourquoi?
, .
Pour estimer les angles, nous utiliserons des mesures MEMS d'accéléromètres et de magnétomètres.

1. Bref programme éducatif

Angles d'orientation

Nous comprendrons les angles d'orientation de l'objet comme les angles d'Euler - roulis, tangage, lacet, qui relient le propre système de coordonnées XYZ de l'objet et le système de coordonnées local est-nord-haut (ENU - Est nord en haut). Les angles de roulis, de tangage et de lacet indiquent la rotation que les axes XYZ doivent effectuer pour se déplacer dans l'axe ENU. Par conséquent, des angles nuls signifient que l'axe X de l'objet fait face à l'est, l'axe Y de l'objet regarde vers le nord et l'axe Z est vers le haut.

L'ordre de rotation des axes est du dernier angle: d'abord en lacet (autour de l'axe Z), puis en tangage (autour de l'axe Y), puis en roulis (autour de l'axe X).

Accéléromètre

Il s'agit d'un capteur qui mesure la projection de l'accélération apparente sur l'axe de sensibilité. Il semble - parce qu'il mesure également la gravité, même lorsque l'accéléromètre est à l'arrêt. Il est plus facile d'imaginer un accéléromètre comme un poids sur un ressort; ses mesures émises sont proportionnelles au degré de tension du ressort. Si l'accéléromètre est au repos, le ressort n'est tendu que par gravité. S'il accélère, alors il y aura une somme de forces: inertie du poids

(F = m \vec{a})

$\left (F=m\overrightarrow{a}\right )$ et la gravité

(F_{g} = m \vec{g})

$\left (F_{g}=m\overrightarrow{g}\right )$

Nous acceptons le modèle de mesure suivant de la triade d'accéléromètres orthogonaux (mutuellement perpendiculaires):

a_{X Y Z} = m_{a} \cdot A_{X Y Z} + b_{a} + n_{a},

$a_{XYZ}= m_{a} \cdot A_{XYZ}+b_{a}+n_{a},$

Où

a_{X Y Z}

$a_{XYZ}$ - accélération mesurée en SSC (propre système de coordonnées) XYZ,

m_{a}

$m_{a}$ - matrice asymétrique des axes et des facteurs d'échelle de l'accéléromètre,

A_{X Y Z}

$A_{XYZ}$ Est le vecteur de l'accélération réelle dans CCK XYZ,

b_{a}

$b_{a}$ Est le vecteur de déplacement nul de l'accéléromètre,

n_{a}

$n_{a}$ - bruit de mesure.

La matrice d'inclinaison des axes et des facteurs d'échelle est la suivante:

m_{a} = [\begin{matrix} 1 + m_{a, 1, 1} & m_{a, 1, 2} & m_{a, 1, 3} \\ m_{a, 2, 1} & 1 + m_{a, 2, 2} & m_{a, 2, 3} \\ m_{a, 3, 1} & m_{a, 3, 2} & 1 + m_{a, 3, 3} \end{matrix}],

$m_{a}=\begin{bmatrix}1+m_{a,1,1} & m_{a,1,2} & m_{a,1,3}\\ m_{a,2,1}& 1+m_{a,2,2} & m_{a,2,3}\\ m_{a,3,1} & m_{a,3,2} & 1+m_{a,3,3} \end{bmatrix},$

où les éléments sont situés sur la diagonale principale (

1 + m_{a, 1, 1}, 1 + m_{a, 2, 2}, 1 + m_{a, 3, 3}

$1+m_{a,1,1}, 1+m_{a,2,2}, 1+m_{a,3,3}$ ) Les facteurs d'échelle et leurs erreurs le long des trois axes de l'accéléromètre, et les éléments restants de la matrice sont des distorsions des axes de l'accéléromètre.

Sélection de l'accéléromètre à partir de la fiche technique

MPU-9250

— Zero-G Initial Calibration Tolerance ( $\pm60mg$ $X, Y$ , $\pm80mg$ $Z$ ) — $g$ $10^{-3};$
— Initial Tolerance ( $\pm3\%$ ) — . , $10^{-2};$
— Cross Axis Sensitivity ( $\pm2\%$ ) — $10^{-2};$
— Noise Power Spectral Density $\left (300\frac{\mu g}{\sqrt{Hz}} \right )$ — $g$ $10^{-6};$
— Low Pass Filter Response $\left (5-260 Hz\right )$ — , . . ;

σ_{n o i s e} = G_{0} \cdot \sqrt{Π_{n o i s e}};

$\sigma _{noise}=G_{0}\cdot\sqrt{\Pi_{noise}};$

ADIS16488A:

— Bias Repeatability ( $\pm 16mg$ ) — $g$ $10^{-3};$
— (Sensitivity) Repeatability ( $\pm 0.5\%$ ) — ;
— Misalignment Axis to frame ( $\pm1^{\circ}$ ) — , (, );
— Noise Density $\left ( 0.063\frac{mg}{\sqrt{Hz}}rms \right )$ — $g;$
— $\left (3dB Bandwidth\right)$ — , MPU-9250;

Magnétomètre

Un capteur qui mesure la projection de l'induction du champ magnétique sur l'axe de sensibilité. Le magnétomètre est caractérisé par des distorsions de fer dur et de fer doux. La distorsion du fer dur est un effet additif lorsqu'un composant constant est ajouté au champ mesuré. La raison peut être, par exemple, l'action d'un aimant permanent ou un décalage auto-nul du capteur. La distorsion du fer doux est un effet multiplicateur reflétant un changement de direction et / ou d'atténuation du vecteur d'induction magnétique. Cet effet peut être causé par la présence d'un objet métallique à proximité immédiate du magnétomètre ou par les propres distorsions du capteur - une erreur dans le facteur d'échelle ou une distorsion de son axe de sensibilité.
Nous acceptons le modèle de mesure de la triade de magnétomètres:

m_{X Y Z} = S_{m} \cdot M_{X Y Z} + b_{m} + n_{m},

$m_{XYZ}=S_{m}\cdot M_{XYZ}+b_{m}+n_{m},$

Où

m_{X Y Z}

$m_{XYZ}$ - mesures magnétométriques en CCK XYZ,

S_{m}

$S_{m}$ - matrice asymétrique d'axes et de facteurs d'échelle (qui décrit l'effet du fer doux),

M_{X Y Z}

$M_{XYZ}$ Est le vecteur de la véritable induction magnétique en SSK,

b_{m}

$b_{m}$ - déplacement des zéros du magnétomètre (décrit l'effet fer dur),

n_{m}

$n_{m}$ - bruit de mesure.
La matrice de l'axe d'inclinaison et les facteurs d'échelle du magnétomètre:

S_{m} = [\begin{matrix} 1 + S_{m, 1, 1} & S_{m, 1, 2} & S_{m, 1, 3} \\ S_{m, 2, 1} & 1 + S_{m, 2, 2} & S_{m, 2, 3} \\ S_{m, 3, 1} & S_{m, 3, 2} & 1 + S_{m, 3, 3} \end{matrix}],

$S_{m}=\begin{bmatrix}1+S_{m,1,1} & S_{m,1,2} &S_{m,1,3}\\ S_{m,2,1}& 1+S_{m,2,2} & S_{m,2,3}\\ S_{m,3,1} & S_{m,3,2}&1+ S_{m,3,3} \end{bmatrix},$

éléments situés sur la diagonale principale (

S_{m, 1, 1}, S_{m, 2, 2}, S_{m, 3, 3}

$S_{m,1,1}, S_{m,2,2}, S_{m,3,3}$ ) Sont les facteurs d'échelle et leurs erreurs le long des trois axes du magnétomètre, les éléments restants de la matrice sont les distorsions des axes du magnétomètre. Tous les éléments de la matrice prennent également en compte l'effet du fer doux.

Choix des paramètres du magnétomètre à partir d'une fiche technique

MPU-9250

, , :

— $(1\mu T);$
— $(5\%);$
— , , — $(\pm2\%);$
— $(0.6\mu T);$

ADIS16488A

— Initial Bias Error $\left (\pm15 mgauss = 1.5 \mu T \right )$ — , $(0.5 \mu T)$ ;
— Initial Sensitivity Tolerance $\left (2\% \right );$
— Misalignment Axis to axis $\left (0.35^{\circ} \right )$ — , (, );
— Noise Density $\left (0.042\frac{mgauss}{\sqrt{Hz}} \right )$ — $\left (\frac{Tesla}{\sqrt{Hz}} \right );$
— $260 Hz;$

Calcul des angles d'orientation

En raison de la présence de la gravité sur Terre, les accéléromètres «détectent» la direction descendante. Leurs mesures sont utilisées pour calculer les angles de roulis et de tangage. Les formules de calcul peuvent être trouvées ici . Le troisième est l'angle de lacet (et dans ce cas, l'azimut magnétique), qui peut être déterminé en raison de la présence du champ magnétique terrestre. Le vecteur d'induction de champ magnétique est mesuré par des magnétomètres et leurs mesures sont impliquées dans le calcul de l'angle de lacet. Il convient de noter que dans le calcul de l'azimut magnétique, des mesures de magnétomètre sont utilisées, calculées dans le plan. Ici vous pouvez trouver la formule pour calculer l'azimut magnétique.

r o l l = a t a n (\frac{a_{Y}}{a_{Z}}),

$roll=atan\left ( \frac{a_{Y}}{a_{Z}} \right ),$

p i t c h = a t a n (\frac{- a_{X}}{\sqrt{a_{Y}^{2} + a_{Z}^{2}}}),

$pitch=atan\left ( \frac{-a_{X}}{\sqrt{a_{Y}^{2}+a_{Z}^{2}}} \right ),$

y a w = a t a n 2 (\frac{m_{E}}{m_{N}}),

$yaw=atan2\left ( \frac{m_{E}}{m_{N}} \right ),$

Où

a t a n 2

$atan2$ - fonction tangente arc complet,

a_{X}

$a_{X}$ ,

a_{Y}

$a_{Y}$ ,

a_{Z}

$a_{Z}$ - mesure de l'accéléromètre selon trois axes dans le CCK,

m_{E}

$m_{E}$ ,

m_{N}

$m_{N}$ - mesures du magnétomètre le long des axes X ', Y' (les mesures des magnétomètres sont comptées dans le plan).

2.

roll, pitch, yaw. .
?
, — .
roll, pitch, yaw XYZ ENU:

$C_{X Y Z}^{E N U} = | \begin{matrix} c y \cdot c p & - c r \cdot s y + s r \cdot c y \cdot s p & s r \cdot s y + c r \cdot c y \cdot s p \\ s y \cdot c p & c r \cdot c y + s r \cdot s y \cdot s p & - s r \cdot c y + c r \cdot s y \cdot s p \\ - s p & s r \cdot c p & c r \cdot c p \end{matrix} |,$
$C_{XYZ}^{ENU}=\begin{vmatrix} cy\cdot cp &-cr\cdot sy+sr\cdot cy\cdot sp &sr\cdot sy+cr\cdot cy\cdot sp \\ sy\cdot cp &cr\cdot cy+sr\cdot sy\cdot sp & -sr\cdot cy+cr\cdot sy\cdot sp \\ -sp &sr\cdot cp &cr\cdot cp \end{vmatrix},$
$cr=\cos (roll)$ , $sr=\sin(roll)$ , $cp=\cos(pitch)$ , $sp=\sin(pitch)$ , $cy=\cos(yaw)$ , $sy=\sin(yaw)$ .
:
$A_{X Y Z} = {(C_{X Y Z}^{E N U})}^{T} \cdot | \begin{matrix} 0 \\ 0 \\ - 1 \end{matrix} |,$
$A_{XYZ}=\left ( C_{XYZ}^{ENU} \right )^{T}\cdot \begin{vmatrix} 0\\ 0\\ -1\\ \end{vmatrix},$

$\begin{vmatrix} 0\\ 0\\ -1\\ \end{vmatrix}$ — , , g, ${(C_{XYZ}^{ENU} )}^{T}$ — ( ).
:
$a_{X Y Z} = (I + m_{a}) \cdot A_{X Y Z} + b_{a} + n_{a},$
$a_{XYZ}=\left ( I+m_{a} \right )\cdot A_{XYZ}+b_{a}+n_{a},$
() :
$r o l l^{'} = a t a n (\frac{a_{Y}}{a_{Z}}),$
$roll'=atan \left ( \frac{a_{Y}}{a_{Z}} \right ),$
$p i t c h^{'} = a t a n (\frac{- a_{X}}{\sqrt{a_{Y}^{2} + a_{Z}^{2}}}) .$
$pitch'=atan\left ( \frac{-a_{X}}{\sqrt{a_{Y}^{2}+a_{Z}^{2}}} \right ).$
«» , rpy2mat:
$C_{X Y Z}^{X Y Z^{'}} = r p y 2 m a t ({[\begin{matrix} r o l l^{'} \\ p i t c h^{'} \\ 0 \end{matrix}]}^{T}),$
$C_{XYZ}^{XYZ'}=rpy2mat\left ( \begin{bmatrix} roll'\\ pitch'\\ 0 \end{bmatrix}^{T} \right ),$
roll' pitch' — , , — .
ENU XYZ:
$M_{X Y Z} = {(C_{X Y Z}^{E N U})}^{T} \cdot M_{E N U} .$
$M_{XYZ}= {(C_{XYZ}^{ENU} )}^{T}\cdot M_{ENU}.$
:
$m_{X Y Z} = S_{m} \cdot M_{X Y Z} + b_{m} + n_{m} .$
$m_{XYZ}=S_{m}\cdot M_{XYZ}+b_{m}+n_{m}.$
«»:
$m_{X Y Z^{'}} = C_{X Y Z}^{X Y Z^{'}} \cdot m_{X Y Z} .$
$m_{XYZ'}=C_{XYZ}^{XYZ'}\cdot m_{XYZ}.$
«» c :
$y a w^{'} = a t a n 2 (\frac{m_{Y^{'}}}{m_{X^{'}}}) .$
$yaw'=atan2\left ( \frac{m_{Y'}}{m_{X'}} \right ).$
roll, pitch, yaw — roll', pitch', yaw'.

3. —

Pour deux capteurs, que nous avons pris comme exemple - ADIS16488A et MPU-9250, les erreurs marginales dans l'estimation des angles d'orientation ont été obtenues sous l'influence combinée des erreurs de l'accéléromètre et du magnétomètre.

Le tableau ci-dessous indique les valeurs maximales d'erreurs reçues:

Angle	MPU-9250	ADIS16488A
Rouleau	$30^{\circ}$ $30^{\circ}$	$8^{\circ}$ $8^{\circ}$
Pas	$10^{\circ}$ $10^{\circ}$	$2^{\circ}$ $2^{\circ}$
Azimut magnétique	$30^{\circ}$ $30^{\circ}$	$20^{\circ}$ $20^{\circ}$

L'effet combiné des erreurs de l'accéléromètre et du magnétomètre sur les erreurs d'estimation des angles d'orientation:

Voici à quoi ressemblent les erreurs d'estimation de roulis en fonction des valeurs de roulis et de tangage:
Erreurs d'estimation de tangage en fonction des valeurs de roulis et de tangage:
Erreurs d'estimation de l'azimut magnétique à partir des angles de roulis et de tangage:
Erreurs d'estimation de l'azimut magnétique à partir des angles de roulis et de l'azimut magnétique:
Erreurs d'estimation de l'azimut magnétique à partir des angles de tangage et de l'azimut magnétique: