看完这个，让你分分钟搞定罗盘与加计校准！

看完这个，让你分分钟搞定罗盘与加计校准！

 

罗盘和加计的校准是日常开发中最基础的工作，特邀Echo老师对罗盘和加速度计校准的工程方法进行总结，为小伙伴你们解惑，是有此文。

作者信息

Echo，本名邹佳池，从事嵌入式软件开发。

联系方式：QQ529380360

 

超详细讲解：罗盘和加速度计校正方法

(附C源代码)

1.为什么要校正

我们都知道，罗盘是测量周围的磁场强度，若不存在外在磁场的干扰，只存在地磁的话，理论上罗盘旋转测得的磁场是一个圆球。

可是现实空间中，除了地磁场外，还存在其他的磁场干扰，这里我索性将它分为两大类。

第一类：地球空间中的磁场，这类磁场有个特点，就是随着罗盘坐标系的转动，磁场方向不变，类似地磁场。

第二类：罗盘坐标空间中的磁场，这类磁场源一般是固定在飞机上的，所以随着罗盘坐标系的转动，磁场方向也跟着转动。但是对于罗盘坐标系来说，却是一个恒定值。

对于第一类的磁场，目前我了解到的还没有什么好的方式可以进行校正（如果哪位大神知道，还请告知）。而我后面要介绍的校正方法，即是滤除第二类磁场的干扰，校正的思想即是基于最小二乘法的椭球拟合算法。

注：这个只能校正磁场强度固定不变的磁场，而对于电机这种变化的磁场，我没有测验过，不知道电机产生磁场的强度大小跟电机转速的关系怎样，如果谁有研究过的，还请告知，谢谢。

2.椭球拟合校正理论推导

网络上有许多关于椭球拟合校正的论文，我都没有细看，因为那些公式都写得晦涩难懂，没有那个耐心，我这里尽量用最简洁的语言介绍校正方法的理论基础。

首先建立数值模型，设测量值为：

,校正后的值为：，

平移参数为：

缩放参数为：

他们之间的关系如下所示：

我们校正后的目标就是使得校正值近似分布在一个圆球上，而圆球的公式大家都知道：x2+y2+z2=R2，故我们将校正后的值带入圆球公式，与理论的圆球半径平方做差，构建误差u：

将校正值用测量值替换，变为：

可以看到，这分明就是个椭圆公式嘛~

记：

则我们的误差u可以写为如下形式：

下面就是校正的核心思想了：假设我们有许多组数据，我们要求得一组参数，使得所有数据的误差和最小，即∑u最小，但是由于u有正有负，所以符号相反的误差有可能相互抵消。那么加绝对值呢？这个也不可取，因为对绝对值函数求极小值十分复杂。那么我们自然就想到对u求平方和，即：

U=∑u^2

我们把u看成一个未知数，这个函数是一个二次函数，其有极小值点。而为了求得这个极小值点，我们对其做偏导即可：

记：

则我们可以将偏导写成如下形式：

B是已知的，P是我们待求的参数矩阵，故可以通过求齐次线性方程组，来求得P的各个参数的解。

齐次线性方程组求解的过程我这里就不详细解释了，我算法中使用的方法是经典的高斯消元法，有兴趣的可以仔细看看。

当我们求得P的各个未知数a,b,c,d,e,f,g后，需要通过这几个参数反求出我们的偏移量(ox,oy,oz)和缩放量(gx,gy,gz)。

在反求解之前，我们先回到上一个式子，BxP=0。其实满足这个式子的解P有无数组，我们可以将式子改写成BxCP=0，C是一个任意常数，即

我们通过解线性方程组求得的只是这个解系中的一个基本解，所以我们首先要求出这个基本解的C。

其算式经推导如下,带入a,b,c,d,e,f,g即可求解：

C=(d2/a + e2/b + f2/c - 4g)/4R2  (R为理论圆球半径)

ox=d/2a

oy=e/2b

oz=f/2c

gx=sqrt(a/C)

gy=sqrt(b/C)

gz=sqrt(c/C)

最后，将这六个参数回调到之前的式子中去，即完成校正。

3.校正程序源码(C语言)

代码太长了 就不贴出来了，放在网盘种大家下载

罗盘与加计校准方法 C源代码

http://pan.baidu.com/s/1dFiB6z3

根据这个算法原理，针对一款高性能陀螺仪编写的校正程序：

陀螺仪：

ADIS陀螺仪

算法显示：

磁场校正显示