电子罗盘的算法以及倾斜补偿算法

电子罗盘的算法以及倾斜补偿算法

这两天在研究电子罗盘的算法，看了若干代码和文章后，终于弄明白了。就此文章总结一下。

基础扫盲

1·地磁场

 地球的磁场像一个条形磁铁。磁场方向是地磁南极指向地磁北极。在南北地磁点处的磁场和当地的水平面垂直，在赤道处的磁场和当地的水平面平行，所以在北半球地磁场方向向北倾斜指向地面。
 用来衡量磁感应强度的大小的单位是Tesla或者Gauss(1 T= 10000 G)。随着地理位置的不同，通常地磁场的强度是0.4-0.6Gauss。需要特别注意的是，地磁北极和地理的北极并不重合，通常相差11度左右的夹角。

 地磁场是一个矢量，对于一个固定的地点来说，这个矢量可以分解为两个与当地水平面平行的分量Hx和Hy与一个和当地水平面垂直的分量Hz。
如果保持电子罗盘（地磁传感器，本文章验证使用的是LSM303传感器，该传感器集成了加速度传感器和地磁传感器）和当地的水平面平行，则地磁传感器的三个轴就和这三个分量对应起来。
对于水平方向的两个分量来说，他们的矢量和总是指向地磁北的。罗盘中的航向角（Azimuth）就是当前方向和地磁北的夹角。在电子罗盘水平的情况下，只需要磁力计水平方向的两个轴（X轴和Y轴）的检测数据就可以用反正切函数arctan()求出(结果是弧度)，当罗盘水平旋转时，航向角在0~360度之间变化。

2.矢量的叉积和点乘以及数据归一化

2.1矢量的叉积

 两个向量的叉乘，又叫向量积、外积、叉积，叉乘的运算结果是一个向量而不是一个标量。并且两个向量的叉积与这两个向量组成的坐标平面垂直。

对于向量a和向量b：

a和b的叉乘公式为：

其中：

根据i、j、k间关系，有：

叉乘几何意义
在三维几何中，向量a和向量b的叉乘结果是一个向量，更为熟知的叫法是法向量，该向量垂直于a和b向量构成的平面。

在3D图像学中，叉乘的概念非常有用，可以通过两个向量的叉乘，生成第三个垂直于a，b的法向量，从而构建X、Y、Z坐标系。如下图所示：
在二维空间中，叉乘还有另外一个几何意义就是：aXb等于由向量a和向量b构成的平行四边形的面积。、

2.2矢量的点乘

 向量的点乘,也叫向量的内积、数量积，对两个向量执行点乘运算，就是对这两个向量对应位一一相乘之后求和的操作，点乘的结果是一个标量。

点乘公式
对于向量a和向量b：

a和b的点积公式为：

在进行点乘计算时，要求一维向量a和向量b的行列数相同。

点乘几何意义
点乘的几何意义是可以用来表征或计算两个向量之间的夹角，以及在b向量在a向量方向上的投影，有公式：

2.3归一化

 向量归一化法有两种形式，一种是把数变为（0，1）之间的小数，一种是把有量纲表达式变为无量纲表达式。主要是为了数据处理方便提出来的，把数据映射到0～1范围之内处理，更加便捷快速。数据归一化的方法主要有线性函数法、对数函数法、反余切函数。
 对加速度计和磁力计的值进行归一化可以去掉其量纲，使它们可以放在一起计算。

算法

 在前文提到在地磁传感器水平放置的情况下可以由x轴和y轴的值通过arctan()函数计算出来。但是在实际使用中，地磁传感器并不是水平放置的，这个倾斜的夹角就会影响航向角的精度。这时候就要运用几何学去做倾斜补偿了。

1.利用旋转矩阵推导倾斜补偿公式

 电子罗盘的倾斜姿态可以通过三轴加速度传感器检测出，测出三个轴上重力加速度的分量，在通过计算可以得出Pitchg(俯仰角)和Roll(横滚角)。

 设在罗盘的翻滚角为γ\gammaγ,俯仰角为ϕ\phiϕ,航向角为ψ\psiψ时,三轴地磁传感器的测量输出M(γ,ϕ,ψ)=[Mx  My  Mz]TM(\gamma,\phi,\psi)=[Mx \;My\;Mz ]^TM(γ,ϕ,ψ)=[MxMyMz]T;设罗盘水平放置时具有相同角的三轴地磁传感器的输出为M(0,0,ψ)=[MHz  MHy  MHz]TM(0,0,\psi)=[M_{Hz}\;M_{Hy}\;M_{Hz}]^TM(0,0,ψ)=[MHz​MHy​MHz​]T根据罗盘实际姿态和水平面的关系，可得
{M(γ,ϕ,ψ)=Rϕ Rγ M(0,0,ψ)M(0,0,ψ)=Rγ−1 Rϕ−1 M(γ,ϕ,ψ)(1) \begin{cases} M_{(\gamma,\phi,\psi)}=R_\phi\,R_\gamma\,M_{(0,0,\psi)} \\ M_{(0,0,\psi)}=R_\gamma^{-1}\,R_\phi^{-1}\,M_{(\gamma,\phi,\psi)} \end{cases} \text {(1)} {M(γ,ϕ,ψ)​=Rϕ​Rγ​M(0,0,ψ)​M(0,0,ψ)​=Rγ−1​Rϕ−1​M(γ,ϕ,ψ)​​(1)
 其中Rϕ,RγR_\phi,R_\gammaRϕ​,Rγ​分别为翻滚角γ\gammaγ和俯仰角ϕ\phiϕ的旋转矩阵：

Rγ=[1000cosγsinγ0−sinγcosγ](2) R_\gamma= \begin{bmatrix} 1 & 0&0\\ 0 & cos\gamma & sin\gamma\\ 0 & -sin\gamma & cos\gamma\\ \end{bmatrix} \text {(2)} Rγ​=⎣⎡​100​0cosγ−sinγ​0sinγcosγ​⎦⎤​(2)
Rϕ=[cosϕ0−sinϕ010sinϕ0cosγ](3) R_\phi= \begin{bmatrix}cos\phi & 0 & -sin\phi\\ 0 & 1 & 0\\ sin\phi & 0 & cos\gamma\\ \end{bmatrix} \text {(3)} Rϕ​=⎣⎡​cosϕ0sinϕ​010​−sinϕ0cosγ​⎦⎤​(3)
将式（2）式（3）带入式（1）得
{MHx=Mxcosϕ+MzsinϕMHy=Mxsinγsinϕ+Mycosγ−Mzsinγcosϕ(4) \begin{cases} M_{Hx}=M_xcos\phi+M_zsin\phi \\ M_{Hy}= M_xsin\gamma sin\phi+M_ycos\gamma-M_zsin\gamma cos\phi \end{cases} \text {(4)} {MHx​=Mx​cosϕ+Mz​sinϕMHy​=Mx​sinγsinϕ+My​cosγ−Mz​sinγcosϕ​(4)
将式（4）带入反正切函数即可得到补偿后的航向角

附上手推公式过程

代码实现

2.利用使用矢量计算方法补偿倾斜

 利用地理（重力和地磁）矢量的方向不变的和可以任意平移的特点计算航向角可以直接忽略倾斜影响。本节内容全是作者的理解，若有不正确的地方欢迎指正。这个算法是arduino的一个库里面的，此算法厉害的地方是它对传感器的安装方式不是严格的。

矢量计算航向算法描述：利用加速度传感器读数确定向下的重力向量和地磁传感器确定的地磁向量的叉积可以得到指向东的向量。东向量和重力量的叉积是北向量（右手法则）。向东和向北的向量构成了水平面平面的基础。机体（电子罗盘）向量被投影到水平平面（点乘），在利用反正切函数即可求出航向角。这段文字里除了机体向量外，其他向量都是地理向量方向不变。

代码实现

关于入参from,这个和传感器安装的位置有关，比如地磁传感器的x轴与地磁场的x轴相反则入参为{-1，0，1}。其他安装方式以此类推。
至此本文完结。