🦉 ❣️ 🎾 MEMS加速度计，磁力计和定向角 🛌🏽 👩🏾‍✈️ 🥋

在开发必须评估方向角的设备时，可能会出现问题-选择哪种MEMS传感器。传感器制造商提供许多参数，我们需要从这些参数中获取有关传感器质量的有用信息。

要了解我们可以依靠哪种角度精度，您需要付出一定的努力。

TL; DR：描述了一个用于Octave / MATLAB的小脚本，该脚本允许根据加速度计和磁力计的MEMS测量值估算取向角时的误差。在脚本输入处-数据表中的传感器参数（和/或校准错误）。该文章对那些开始在设备中使用惯性传感器的人可能有用。包含有关传感器的小型教育程序。也链接到github。

我们立即接受以下条件：

我们要评估固定设备的定向角。

为什么？
根本无法使用公式来计算移动设备的方向；您需要使用棘手的算法。
为了估算角度，我们将使用加速度计和磁力计的MEMS测量。

1.简短的教育计划

定向角

我们将把物体的定向角理解为欧拉角-滚动角，俯仰角和偏航角，它们将物体自身的坐标系XYZ与本地的东西向北-顶坐标系统（ENU-东北向上）连接起来。横摆角，俯仰角和偏航角表示XYZ轴必须旋转才能在ENU轴上移动。因此，零角度表示对象的X轴面向东方，对象的Y轴朝北，Z轴朝上。

轴的旋转顺序从最后一个角度开始：首先在偏航（绕Z轴）上，然后在俯仰（绕Y轴）上，然后在横滚（绕X轴）上。

加速度计

这是一种传感器，用于测量视在加速度在灵敏度轴上的投影。似乎-因为它也测量重力，即使加速度计静止不动也是如此。最容易将加速度计想象为弹簧上的重量；其发布的测量值与弹簧的张力程度成正比。如果加速度计处于静止状态，则弹簧仅在重力作用下拉伸。如果它加速，则将产生合力：重量的惯性

(F = m \vec{a})

$\left (F=m\overrightarrow{a}\right )$ 和重力

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

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

我们接受以下正交（互相垂直）加速度计三重测量的模型：

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

哪里

在SSC（自身坐标系）XYZ中测得的加速度，

a_{X Y Z}

$a_{XYZ}$

-加速度计的轴和比例因子的偏斜矩阵，

m_{a}

$m_{a}$

-SSK XYZ中的真实加速度矢量，

A_{X Y Z}

$A_{XYZ}$

是加速度计的零位移矢量，

b_{a}

$b_{a}$

是测量噪声。轴和比例因子的偏斜矩阵如下：

n_{a}

$n_{a}$

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

元素位于主对角线上的位置（

）是比例因子和他们的错误沿加速度计的三个轴，和所述矩阵的其余元件是加速度计的轴的经纱。

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

从数据表中选择加速度计

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;

磁力计

一种传感器，用于测量磁场感应在灵敏度轴上的投影。磁力计的特征在于硬铁和软铁的变形。当将恒定分量添加到测量场中时，铁铁变形是一种加性效应。原因可能是例如永磁体的作用或传感器的自零偏移。软铁变形是一种乘数效应，反映了磁感应矢量的方向变化和/或衰减。这种影响可能是由于磁力仪附近存在金属物体或传感器自身的变形（比例因子误差或灵敏度轴变形）引起的。
我们接受三强磁力计的测量模型：

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}

$m_{XYZ}$ -CCK XYZ中的磁力计测量值，

S_{m}

$S_{m}$ 斜轴和比例因子的对角矩阵（描述软铁的影响），

M_{X Y Z}

$M_{XYZ}$ 是SSK中真实磁感应的矢量，

b_{m}

$b_{m}$ 是磁力计零点的位移（描述了硬铁作用），

n_{m}

$n_{m}$ 是测量噪声。
斜轴矩阵和磁力计的比例因子：

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

位于主对角线上的元素（

）是比例因子和其沿着所述磁力计的三个轴的误差，所述矩阵的其余元件的磁力计的轴的扭曲。所有基体元素还考虑了软铁的作用。

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

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

从数据表中选择磁力计参数

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

取向角的计算

由于地球上存在重力，所以加速度计“感知”向下的方向。它们的测量值用于计算侧倾角和俯仰角。计算公式可以在这里找到。第三个是偏航角（在这种情况下为磁方位角），可以通过存在地球磁场来确定。磁场感应矢量由磁力计测量，并且它们的测量都涉及偏航角的计算。应该注意的是，在计算磁方位角时，使用了在平面中计算出的磁力计测量值。在这里您可以找到计算磁方位角的公式。

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

哪里

a t a n 2

$atan2$ 全圆弧切线函数，

a_{X}

$a_{X}$ ，

a_{Y}

$a_{Y}$ ，

a_{Z}

$a_{Z}$ -沿着CCK三轴加速度计的测量结果，

m_{E}

$m_{E}$ ，

磁力计沿X'，Y'轴的测量值（磁力计的测量值在平面中计数）。

m_{N}

$m_{N}$

2.取向角估计中的误差

算法说明

我们形成随机的欧拉角阵列，包括滚动，俯仰和偏航。他们将为模型中对象的真实方向指定选项集。
为什么有很多角度？
, — .
从随机的横摇，俯仰，偏航角，形成从SSK XYZ到LSK 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}.$
仍然需要重新计算从SSK到“地平线”的磁力计测量值：
$m_{X Y Z^{'}} = C_{X Y Z}^{X Y Z^{'}} \cdot m_{X Y Z} .$
$m_{XYZ'}=C_{XYZ}^{XYZ'}\cdot m_{XYZ}.$
根据磁力计的“水平”测量，计算出磁方位角：
$y a w^{'} = a t a n 2 (\frac{m_{Y^{'}}}{m_{X^{'}}}) .$
$yaw'=atan2\left ( \frac{m_{Y'}}{m_{X'}} \right ).$
估计方位角时的误差计算为真实的侧倾角，俯仰角和偏航角之差，并根据传感器的测量值计算得出-侧倾'，俯仰角，偏航'。

3.结果-角度估计误差的计算

我们以两个传感器为例-ADIS16488A和MPU-9250，在加速度计和磁力计误差的综合影响下获得了估计方位角的边际误差。

下表显示了收到的错误的最大值：

角度	MPU-9250	ADIS16488A
滚	$30^{\circ}$ $30^{\circ}$	$8^{\circ}$ $8^{\circ}$
沥青	$10^{\circ}$ $10^{\circ}$	$2^{\circ}$ $2^{\circ}$
磁方位角	$30^{\circ}$ $30^{\circ}$	$20^{\circ}$ $20^{\circ}$