👨🏾‍🎤 ☸️ 🕴🏿 Acelerômetros, magnetômetros e ângulos de orientação MEMS 🍦 🌍 🤐

Ao desenvolver dispositivos nos quais é necessário avaliar os ângulos de orientação, pode surgir a pergunta - qual sensor MEMS escolher. Os fabricantes de sensores fornecem muitos parâmetros dos quais precisamos obter informações úteis sobre a qualidade do sensor.

Para entender com que precisão de ângulo podemos confiar, você precisa fazer um certo esforço.

TL; DR : É descrito um pequeno script para Octave / MATLAB, que permite estimar erros no cálculo dos ângulos de orientação a partir das medições MEMS de acelerômetros e magnetômetros. Na entrada do script - parâmetros dos sensores das folhas de dados (e / ou erros de calibração). O artigo pode ser útil para quem começa a usar sensores inerciais em seus dispositivos. Um pequeno programa educacional sobre os sensores está incluído. Link para o github também.

Aceitamos imediatamente as seguintes condições:

Queremos avaliar os ângulos de orientação de um dispositivo estacionário.

Por quê?
, .
Para estimar ângulos, usaremos medições MEMS de acelerômetros e magnetômetros.

1. Breve programa educacional

Ângulos de orientação

Entenderemos os ângulos de orientação do objeto como os ângulos de Euler - rotação, inclinação, guinada, que conectam o sistema de coordenadas XYZ do próprio objeto e o sistema de coordenadas local leste-norte-topo local (ENU - East North Up). Os ângulos de rotação, inclinação e guinada indicam a rotação que os eixos XYZ precisam fazer para se mover no eixo ENU. Assim, ângulos zero significam que o eixo X do objeto está voltado para o leste, o eixo Y do objeto está voltado para o norte e o eixo Z está para cima.

A ordem de rotação dos eixos é do último ângulo: primeiro na guinada (em torno do eixo Z), depois no passo (em torno do eixo Y), depois no rolamento (em torno do eixo X).

Acelerômetro

Este é um sensor que mede a projeção de aceleração aparente no eixo de sensibilidade. Parece - porque mede a gravidade também, mesmo enquanto o acelerômetro está parado. É mais fácil imaginar um acelerômetro como um peso em uma mola; suas medições emitidas são proporcionais ao grau de tensão da mola. Se o acelerômetro estiver parado, a mola será esticada apenas pela gravidade. Se acelerar, haverá uma soma de forças: inércia do peso

(F = m \vec{a})

$\left (F=m\overrightarrow{a}\right )$ e gravidade

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

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

Aceitamos o seguinte modelo de medidas da tríade de acelerômetros ortogonais (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},$

Onde

a_{X Y Z}

$a_{XYZ}$ - aceleração medida no SSC (sistema de coordenadas próprias) XYZ,

m_{a}

$m_{a}$ - matriz de inclinação dos eixos e fatores de escala do acelerômetro,

A_{X Y Z}

$A_{XYZ}$ É o vetor da verdadeira aceleração no CCK XYZ,

b_{a}

$b_{a}$ É o vetor de deslocamento zero do acelerômetro,

n_{a}

$n_{a}$ - ruído de medição.

A matriz de inclinação dos eixos e fatores de escala é a seguinte:

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

onde os elementos estão localizados na diagonal principal (

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}$ ) São os fatores de escala e seus erros ao longo dos três eixos do acelerômetro, e os elementos restantes da matriz são as distorções dos eixos do acelerômetro.

Seleção de acelerômetro da folha de dados

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

Um sensor que mede a projeção da indução de campo magnético no eixo de sensibilidade. O magnetômetro é caracterizado por distorções do ferro duro e do ferro macio. A distorção do ferro duro é um efeito aditivo quando um componente constante é adicionado ao campo medido. O motivo pode ser, por exemplo, a ação de um ímã permanente ou um deslocamento de zero do sensor. A distorção de ferro macio é um efeito multiplicador que reflete uma mudança na direção e / ou atenuação do vetor de indução magnética. Esse efeito pode ser causado pela presença de um objeto metálico nas imediações do magnetômetro ou por distorções do próprio sensor - um erro no fator de escala ou uma distorção do seu eixo de sensibilidade.
Aceitamos o modelo de medição da tríade 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},$

Onde

m_{X Y Z}

$m_{XYZ}$ - medições do magnetômetro em CCK XYZ,

S_{m}

$S_{m}$ - matriz de inclinação diagonal dos eixos e fatores de escala (que descreve o efeito do ferro macio),

M_{X Y Z}

$M_{XYZ}$ É o vetor da verdadeira indução magnética no SSK,

b_{m}

$b_{m}$ - deslocamento dos zeros do magnetômetro (descreve o efeito do ferro duro),

n_{m}

$n_{m}$ - ruído de medição.
A matriz do eixo de inclinação e os fatores de escala do 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 localizados na 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}$ ) São os fatores de escala e seus erros ao longo dos três eixos do magnetômetro, os elementos restantes da matriz são as distorções dos eixos do magnetômetro. Todos os elementos da matriz também levam em consideração o efeito do ferro macio.

Escolha dos parâmetros do magnetômetro a partir de uma folha de dados

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 dos ângulos de orientação

Devido à presença de gravidade na Terra, os acelerômetros “detectam” a direção descendente. Suas medidas são usadas para calcular ângulos de rotação e inclinação. As fórmulas para cálculo podem ser encontradas aqui . O terceiro é o ângulo de guinada (e, neste caso, o azimute magnético), pode ser determinado devido à presença do campo magnético da Terra. O vetor de indução do campo magnético é medido por magnetômetros e suas medições estão envolvidas no cálculo do ângulo de guinada. Deve-se notar que, no cálculo do azimute magnético, são utilizadas medidas do magnetômetro, calculadas no plano. Aqui você pode encontrar a fórmula para calcular o azimute 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 ),$

Onde

a t a n 2

$atan2$ - função tangente de arco completo,

a_{X}

$a_{X}$ ,

a_{Y}

$a_{Y}$ ,

a_{Z}

$a_{Z}$ - medição do acelerômetro ao longo de três eixos no CCK,

m_{E}

$m_{E}$ ,

m_{N}

$m_{N}$ - medições do magnetômetro ao longo dos eixos X ', Y' (as medições dos magnetômetros são contadas no 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 dois sensores, que tomamos como exemplo - ADIS16488A e MPU-9250, os erros marginais na estimativa dos ângulos de orientação foram obtidos sob a influência combinada dos erros do acelerômetro e magnetômetro.

A tabela abaixo mostra os valores máximos de erros recebidos:

Ângulo	MPU-9250	ADIS16488A
Lista	$30^{\circ}$ $30^{\circ}$	$8^{\circ}$ $8^{\circ}$
Passo	$10^{\circ}$ $10^{\circ}$	$2^{\circ}$ $2^{\circ}$
Azimute magnético	$30^{\circ}$ $30^{\circ}$	$20^{\circ}$ $20^{\circ}$

O efeito combinado dos erros do acelerômetro e magnetômetro nos erros de estimativa dos ângulos de orientação:

É assim que os erros de estimativa de rotação são exibidos, dependendo dos valores de rotação e inclinação:
Erros de estimativa de inclinação, dependendo dos valores de rotação e inclinação:
Erros na estimativa do azimute magnético a partir dos ângulos de rotação e inclinação:
Erros de estimativa do azimute magnético a partir dos ângulos de rotação e do azimute magnético:
: