倾斜补偿的电子罗盘(1)：地磁场，磁传感器，倾斜补偿

倾斜补偿的电子罗盘(1)：地磁场，磁传感器，倾斜补偿

地磁场和磁传感器

地磁场可以用于获取方位信息。以北半球为例，地磁场方向不是与地面水平，而是与水平方向有一定的倾角（指向地面），称为磁倾角（Inclination)。同时，地磁场的方向也与地理的北方不同，两者的夹角称为磁偏角（Declination)。地磁场的磁感应强度大约在30~70uT，与所在地有关。

例如，根据下面这个网站，上海的磁偏角约为-6°，磁倾角约为47°，磁感应强度49.1uT（水平方向33uT，垂直方向36uT）。

World Magnetic Model Calculator (bgs.ac.uk)

（在后面的介绍中忽略磁倾角，假设磁北就是地理北。一般可以通过当地经纬度查询到磁偏角并加以换算）

地磁传感器，一般有XY两轴或者XYZ三轴，大部分基于霍尔效应或是磁阻效应，用于手机、无人机等，实现电子罗盘功能。例如，两轴磁传感器可以通过测量XY两个方向上的磁感应强度，通过简单的计算获得设备的朝向。假设磁传感器的XY平面保持水平，X轴读数hx，Y轴读数hy，则设备的X轴方向与磁北的夹角为$\theta =atan2(\frac{h_y}{h_x})$，θ范围是(-180°,180°]。

hx	hy	θ
+	0	0
+	+	(0,90°)
0	+	90°
-	+	(90°,180°)
-	0	180°
-	-	(-180°,-90°)
0	-	-90°
+	-	(-90°,0)

2D和3D的旋转

某个位置地磁场的方向是相对恒定的，而磁传感器的姿态却是变化的，因此磁传感器每个方向的读数与姿态有关。如下图，对于MPU-6500，磁传感器的姿态可以使用绕着XYZ三个轴的旋转来表示，以逆时针旋转（从x轴的箭头看箭尾）为正。

例如，绕着Z轴旋转，Z轴读数不变，仅改变X轴和Y轴的读数。

原读数：
$$v0=[h_x,h_y{]}^T=[Hcos\theta ,Hsin\theta {]}^T$$
根据示意图，XY逆时针旋转$\delta$后的读数：
$$v^{\prime }=[h_x^{\prime },h_y^{\prime }{]}^T=[Hcos(\theta -\delta ),Hsin(\theta -\delta ){]}^T=[Hcos\theta cos\delta +Hsin\theta sin\delta ,Hsin\theta cos\delta -Hcos\theta sin\delta ]$$
这种旋转可以用矩阵表示：
$$v^{\prime }=\left[\begin{array}{cc}cos\delta & sin\delta \\ -sin\delta & cos\delta \end{array}\right]\left[\begin{array}{c}Hcos\theta \\ Hsin\theta \end{array}\right]=R_{z2d}(\delta )v_0$$

然后把Z轴补上，变成一个3x3的矩阵（因为Z轴读数不变，所以只有3,3的元素是1，其他都是0）：
$$R_z(\psi )=\left[\begin{array}{ccc}cos\psi & sin\psi & 0\\ -sin\psi & cos\psi & 0\\ 0& 0& 1\end{array}\right]$$
同理，围绕X轴和Y轴旋转，对应的矩阵为：
$$R_x(\varphi )=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\varphi & sin\varphi \\ 0& -sin\varphi & cos\varphi \end{array}\right],R_y(\theta )=\left[\begin{array}{ccc}cos\theta & 0& -sin\theta \\ 0& 1& 0\\ sin\theta & 0& cos\theta \end{array}\right]$$

注意$R_y(\theta )$的$sin\theta$符号与其他两个矩阵不同，这与坐标轴设置有关，这里对应于上图MPU6500。

对于三轴磁传感器，定义一个初始位置：XY平面平行于水平面，同时X轴与地磁场方向重合。此时读数为：$h_0=[HcosI,0,HsinI{]}^T$。I是磁倾角。

1. 如果只有Z轴的旋转，则读数变为：
   $$h_{rz}=R_z(\psi )h_0=h_{rz}=R_z(\psi )h_0=\left[\begin{array}{ccc}cos\psi & sin\psi & 0\\ -sin\psi & cos\psi & 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}HcosI\\ 0\\ HsinI\end{array}\right]=\left[\begin{array}{c}HcosIcos\psi \\ -HcosIsin\psi \\ HsinI\end{array}\right]$$
   此时可以直接用$\psi =atan(\frac{-h_y}{h_x})$获得方位角。

2. 如果XYZ轴都有旋转，则读数变为：
   $$h_{rxyz}=R_x(\varphi )R_y(\theta )R_z(\psi )h_0=R_x(\varphi )R_y(\theta )h_{rz}$$
   此时需要进行倾斜补偿，即基于读数$h_{rxyz}$，还原为仅有Z轴旋转的读数$h_{rz}$，然后同样可以获得方位角。

倾斜补偿

原理

进行倾斜补偿，需要对X轴和Y轴反向转动，按之前的记号，X轴转动$(-\varphi )$，Y轴转动$(-\theta )$：
$$\begin{gathered} h_{rz}=[h_{x0},h_{y0},h_{z0}]=[h_x,h_y,h_z]=R_y(-\theta )R_x(-\varphi )h_{rxyz} \\ =\left[\begin{array}{ccc}cos\theta & 0& sin\theta \\ 0& 1& 0\\ -sin\theta & 0& cos\theta \end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\varphi & -sin\varphi \\ 0& sin\varphi & cos\varphi \end{array}\right]h_{rxyz} \\ =\left[\begin{array}{ccc}cos\theta & sin\theta sin\varphi & sin\theta cos\varphi \\ 0& cos\varphi & -sin\varphi \\ -sin\theta & cos\theta sin\varphi & cos\theta cos\varphi \end{array}\right]\left[\begin{array}{c}{h}_x\\ h_y\\ h_z\end{array}\right] \end{gathered}$$
其中，$[h_x,h_y,h_z{]}^T$是实际读数。

因此，考虑倾斜补偿后，方位角变为：
$$\psi =atan\left(\frac{-h_{y0}}{h_{x0}}\right)=atan\left(\frac{h_{y}sin\varphi -h_{x}cos\varphi }{h_{x}cos\theta +h_{y}sin\theta sin\varphi +h_{z}sin\theta cos\varphi }\right)$$
所以，基于当前$\varphi$和$\theta$的信息，就可以进行倾斜补偿。

使用加速度传感器获得角度信息

根据三轴加速度传感器的读数可以获得所需的角度信息。注意在读数时，加速度传感器没有其他方向上的加速，只受到重力影响。

假设初始状态下，加速度传感器的XY轴平行于水平面，Z轴与重力方向相同，则初始状态的读数：$a_0=[0,0,g{]}^T$

可以看出，围绕Z轴旋转对读数没有影响，即：$R_z(\psi )a_0=a_0$

在分别围绕XY两轴旋转后，
$$a_{rxyz}=R_x(\varphi )R_y(\theta )R_z(\psi )a_0=R_x(\varphi )R_y(\theta )a_0$$

同样，在XY两轴反向旋转后，理论上，读数应和初始位置的读数相同：
$$R_y(-\theta )R_x(-\varphi )a_{rxyz}=\left[\begin{array}{ccc}cos\theta & sin\theta sin\varphi & sin\theta cos\varphi \\ 0& cos\varphi & -sin\varphi \\ -sin\theta & cos\theta sin\varphi & cos\theta cos\varphi \end{array}\right]\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]=\left[\begin{array}{c}0\\ 0\\ g\end{array}\right]$$

因此有：
$$\begin{gathered} a_{x}cos\theta +a_{y}sin\theta sin\varphi +a_{z}sin\theta cos\varphi =0 \\ {a}_{y}cos\varphi -a_{z}sin\varphi =0 \end{gathered}$$
从而计算两个角度：
$$\begin{gathered} \varphi =atan\left(\frac{a_y}{a_z}\right) \\ \theta =atan\left(\frac{-a_x}{a_{y}sin\varphi +a_{z}cos\varphi }\right) \end{gathered}$$

总结

• 地磁场和磁传感器
• 电子罗盘基本原理
• 通过加速度传感器获得角度信息，进行倾斜补偿

目前是假设这些传感器的测量值是完全准确的。但实际上会有各种测量误差， 需要进行校准，后面继续补充。

参考资料

• NXP的应用手册AN4246、AN4247、AN4248，解释得比较清楚 （https://www.nxp.com/docs/en/application-note/AN4248.pdf）

• 很多三轴（磁、加速度、陀螺仪）、六轴（一般是加速度+陀螺仪，少量加速度+磁）、九轴（磁+加速度+陀螺仪）的传感器，可以看下应用手册

例如，ISM303DAC集成了三轴加速度传感器和三轴磁传感器，便于实现电子罗盘。