🎾 🤹🏿 🖖🏽 MEMS acelerómetros, magnetómetros y ángulos de orientación 🎼 🔘 🎳

Cuando se desarrollan dispositivos en los que es necesario evaluar los ángulos de orientación, puede surgir la pregunta: qué sensor MEMS elegir. Los fabricantes de sensores proporcionan muchos parámetros a partir de los cuales necesitamos obtener información útil sobre la calidad del sensor.

Para comprender con qué tipo de precisión de ángulo podemos contar, debe hacer un cierto esfuerzo.

TL; DR : se describe un pequeño script para Octave / MATLAB, que permite estimar errores en el cálculo de ángulos de orientación a partir de mediciones MEMS de acelerómetros y magnetómetros. En la entrada del script: parámetros de los sensores de las hojas de datos (y / o errores de calibración). El artículo puede ser útil para aquellos que comienzan a usar sensores inerciales en sus dispositivos. Se incluye un pequeño programa educativo sobre los sensores. Enlace a github también.

Aceptamos de inmediato las siguientes condiciones:

Queremos evaluar los ángulos de orientación de un dispositivo estacionario.

¿Por qué?
, .
Para estimar ángulos, utilizaremos mediciones MEMS de acelerómetros y magnetómetros.

1. Breve programa educativo

Ángulos de orientación

Entenderemos los ángulos de orientación del objeto como los ángulos de Euler: rodar, inclinar, guiñar, que conectan el propio sistema de coordenadas XYZ del objeto y el sistema de coordenadas local este-norte-superior (ENU - East North Up). Los ángulos de balanceo, cabeceo y guiñada denotan la rotación que los ejes XYZ deben hacer para moverse en el eje ENU. En consecuencia, los ángulos cero significan que el eje X del objeto está orientado hacia el este, el eje Y del objeto está mirando hacia el norte y el eje Z está hacia arriba.

El orden de rotación de los ejes es desde el último ángulo: primero en guiñada (alrededor del eje Z), luego en cabeceo (alrededor del eje Y), luego en rollo (alrededor del eje X).

Acelerómetro

Este es un sensor que mide la proyección de la aceleración aparente en el eje de sensibilidad. Parece, porque también mide la gravedad, incluso mientras el acelerómetro está parado. Es más fácil imaginar un acelerómetro como un peso en un resorte; sus medidas emitidas son proporcionales al grado de tensión del resorte. Si el acelerómetro está en reposo, el resorte se estira solo por gravedad. Si se acelera, habrá una suma de fuerzas: la inercia del peso.

(F = m \vec{a})

$\left (F=m\overrightarrow{a}\right )$ y gravedad

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

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

Aceptamos el siguiente modelo de medidas de la tríada de acelerómetros ortogonales (mutuamente perpendiculares):

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},$

Dónde

a_{X Y Z}

$a_{XYZ}$ - aceleración medida en SSC (sistema de coordenadas propio) XYZ,

m_{a}

$m_{a}$ - la matriz oblicua de los ejes y factores de escala del acelerómetro,

A_{X Y Z}

$A_{XYZ}$ - verdadero vector de aceleración en SSK XYZ,

b_{a}

$b_{a}$ es el vector de desplazamiento cero del acelerómetro,

n_{a}

$n_{a}$ es el ruido de medición.

La matriz sesgada de los ejes y factores de escala es la siguiente:

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},$

donde los elementos se encuentran en la diagonal principal (

) son los factores de escala y sus errores a lo largo de los tres ejes del acelerómetro, y los elementos restantes de la matriz son las deformaciones de los ejes del acelerómetro.

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}$

Selección del acelerómetro de la hoja de datos

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;

Magnetómetro

Un sensor que mide la proyección de la inducción de campo magnético en el eje de sensibilidad. El magnetómetro se caracteriza por distorsiones de hierro duro y hierro suave. La distorsión de hierro duro es un efecto aditivo cuando se agrega un componente constante al campo medido. La razón puede ser, por ejemplo, la acción de un imán permanente o un desplazamiento del sensor a cero. La distorsión de hierro suave es un efecto multiplicador que refleja un cambio en la dirección y / o atenuación del vector de inducción magnética. Este efecto puede ser causado por la presencia de un objeto metálico en la vecindad inmediata del magnetómetro o por las propias distorsiones del sensor, un error en el factor de escala o una distorsión de su eje de sensibilidad.
Aceptamos el modelo de medición de la tríada de magnetómetros:

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},$

Dónde

m_{X Y Z}

$m_{XYZ}$ - mediciones de magnetómetro en CCK XYZ,

S_{m}

$S_{m}$ - la matriz diagonal del eje oblicuo y los factores de escala (que describe el efecto del hierro blando),

M_{X Y Z}

$M_{XYZ}$ es el vector de la inducción magnética verdadera en SSK,

b_{m}

$b_{m}$ es el desplazamiento de los ceros del magnetómetro (describe el efecto de hierro duro),

n_{m}

$n_{m}$ es el ruido de medición.
La matriz del eje oblicuo y los factores de escala del magnetómetro:

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},$

elementos ubicados en la diagonal principal (

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

$S_{m,1,1}, S_{m,2,2}, S_{m,3,3}$ ) Son los factores de escala y sus errores a lo largo de los tres ejes del magnetómetro, los elementos restantes de la matriz son las distorsiones de los ejes del magnetómetro. Todos los elementos de la matriz también tienen en cuenta el efecto del hierro suave.

Elección de los parámetros del magnetómetro de una hoja de datos

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;$

Cálculo de ángulos de orientación.

Debido a la presencia de gravedad en la Tierra, los acelerómetros "detectan" la dirección hacia abajo. Sus medidas se utilizan para calcular los ángulos de balanceo y cabeceo. Las fórmulas para el cálculo se pueden encontrar aquí . El tercero es el ángulo de guiñada (y en este caso, el acimut magnético), que se puede determinar debido a la presencia del campo magnético de la Tierra. El vector de inducción de campo magnético se mide por magnetómetros y sus mediciones están involucradas en el cálculo del ángulo de guiñada. Cabe señalar que al calcular el acimut magnético, se utilizan mediciones de magnetómetro, calculadas en el plano. Aquí puede encontrar la fórmula para calcular el acimut magnético.

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 ),$

Dónde

a t a n 2

$atan2$ - función de tangente de arco completo,

a_{X}

$a_{X}$ ,

a_{Y}

$a_{Y}$ ,

a_{Z}

$a_{Z}$ - medición del acelerómetro a lo largo de tres ejes en el CCK,

m_{E}

$m_{E}$ ,

m_{N}

$m_{N}$ - mediciones del magnetómetro a lo largo de los ejes X ', Y' (las mediciones de los magnetómetros se cuentan en el plano).

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. —

Para dos sensores, que tomamos como ejemplo: ADIS16488A y MPU-9250, los errores marginales en la estimación de los ángulos de orientación se obtuvieron bajo la influencia combinada de los errores del acelerómetro y el magnetómetro.

La siguiente tabla muestra los valores máximos de errores recibidos:

Ángulo	MPU-9250	ADIS16488A
Rodar	$30^{\circ}$ $30^{\circ}$	$8^{\circ}$ $8^{\circ}$
Tono	$10^{\circ}$ $10^{\circ}$	$2^{\circ}$ $2^{\circ}$
Acimut magnético	$30^{\circ}$ $30^{\circ}$	$20^{\circ}$ $20^{\circ}$

El efecto combinado de los errores del acelerómetro y el magnetómetro sobre los errores de estimación de ángulos de orientación:

Así es como se ven los errores de estimación de balanceo dependiendo de los valores de balanceo y cabeceo:
Errores de estimación de cabeceo según los valores de balanceo y cabeceo:
Errores en la estimación del acimut magnético desde ángulos de balanceo y cabeceo:
Errores de estimación del acimut magnético desde ángulos de balanceo y acimut magnético:
Errores en la estimación del acimut magnético desde ángulos de inclinación y acimut magnético: