罗盘和加计的校准是日常开发中最基础的工作，特邀Echo老师对罗盘和加速度计校准的工程方法进行总结，为小伙伴你们解惑，是有此文。
作者信息
Echo，本名邹佳池，从事嵌入式软件开发。
超详细讲解：罗盘和加速度计校正方法
(附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源代码
单片机源程序如下:
- #include "stdafx.h"
- #include "string.h"
- #include "math.h"
- #define MATRIX_SIZE 7
- #define u8 unsigned char
- double m_matrix[MATRIX_SIZE][MATRIX_SIZE+1];
- int m = MATRIX_SIZE;
- int n = MATRIX_SIZE+1;
- double m_result[MATRIX_SIZE];
- void DispMatrix(void);
- double Abs(double a)
- {
- return a<0 ? -a : a;
- }
- u8 Equal(double a,double b)
- {
- return Abs(a-b) < 1e-6;
- }
- void ResetMatrix(void)
- {
- int row , column;
-
- for(row = 0 ; row<m ; row++){
- for(column = 0 ; column<n ; column++)
- m_matrix[row][column] = 0.0f;
- }
- }
-
- void CalcData_Input(double x , double y , double z)
- {
- double V[MATRIX_SIZE];
- int row , column;
-
- V[0] = x*x;
- V[1] = y*y;
- V[2] = z*z;
- V[3] = x;
- V[4] = y;
- V[5] = z;
- V[6] = 1.0;
-
- //构建VxVt矩阵(Vt为V的转置)，并进行累加
- for(row = 0 ; row<MATRIX_SIZE ; row++){
- for(column = 0 ; column<MATRIX_SIZE ; column++){
- m_matrix[row][column] += V[row]*V[column];
- }
- }
- }
- void SwapRow(int row1 , int row2)
- {
- int column;
- double tmp;
-
- for(column = 0 ; column<n ; column++){
- tmp = m_matrix[row1][column];
- m_matrix[row1][column] = m_matrix[row2][column];
- m_matrix[row2][column] = tmp;
- }
- }
- void MoveBiggestElement2Top(int s_row , int s_column)
- {
- int row,column;
-
- for(row = s_row+1 ; row<m ; row++){
- if( Abs(m_matrix[s_row][s_column])<Abs(m_matrix[row][s_column])){
- SwapRow(s_row , row);
- }
- }
- }
- //高斯消元法，求行阶梯型矩阵
- u8 Matrix_GaussElimination(void)
- {
- int row,column,i,j;
- double tmp;
-
- for(row = 0,column=0 ; row<m-1 && column<n-1 ; row++,column++){
- //将当前列最大的一行移上来
- MoveBiggestElement2Top(row , column);
-
- //整列都为0
- if(Equal(m_matrix[row][column],0.0f)){
- printf("qiyi matrix:%d %d\r\n" , row , column);
- //DispMatrix();
- //return 0;
- row--;
- continue;
- }
-
- //高斯消元
- for(i = row+1 ; i<m ; i++){
- if(Equal(m_matrix[i][column],0.0f))
- continue; //为0，无需处理
-
- tmp = m_matrix[i][column]/m_matrix[row][column];
-
- for(j = column ; j<n ; j++){
- m_matrix[i][j] -= m_matrix[row][j]*tmp;
- }
- }
- DispMatrix();
- printf("\r\n");
- }
- return 1;
- }
- //求行最简型矩阵
- int Matrix_RowSimplify(void)
- {
- int c = n;//返回值，表示(解的任意常量数+1)；
- //
- int row,column,k,s,t;
- double tmp;
- //
- for(row=0,column=0;row<m && column<n;row++,column++)
- {
- if(Equal(m_matrix[row][column],0))//平移，找出本行第一个非零；
- {
- row--;
- continue;
- }
- //
- c--;//少一个常量；
- //
- //化a[i][j]为1；
- tmp = 1 / m_matrix[row][column];
- for(k=column;k<n;k++)//前面的"0"就不处理了；
- m_matrix[row][k] *= tmp;
- //
- //化a[s][j]为0
- for(s=0;s<row;s++)//下面的0也不用处理；
- {
- if(Equal(m_matrix[s][column],0))
- continue;//已经为0；
- //
- tmp = m_matrix[s][column] / m_matrix[row][column];
- for(t=column;t<n;t++)
- m_matrix[s][t] -= m_matrix[row][t]*tmp;
- //
- }
- }
- //
- return c;
- }
- void Matrix_Solve(double* C , double* sol)
- {
- int row,column,i;
- int any_sol[MATRIX_SIZE];
- //找出任意解的位置
- memset(any_sol , 0 , MATRIX_SIZE);
- for(row=0,column=0 ; row<m && column<n-1 ; row++,column++){
- if(Equal(m_matrix[row][column] , 0.0f)){
- any_sol[column] = 1; //记录任意解的位置
- row--; //右移1列
- }
- }
- //求解
- row = 0;
- for(column = 0 ; column<n-1 ; column++){
- if(any_sol[column] == 1){ //任意解
- sol[column] = C[column];
- }else{
- sol[column] = m_matrix[row][n-1];
- //加上任意解
- for(i = column+1 ; i<n-1 ; i++){
- if(any_sol[i]==1 && !Equal(m_matrix[row][i],0.0f)){
- sol[column] -= m_matrix[row][i]*C[i];
- }
- }
- row++;
- }
- }
- }
- void DispMatrix(void)
- {
- int row,column;
-
- for(row = 0 ; row<m ; row++){
- for(column = 0 ; column<n ; column++){
- printf("%.3f " , m_matrix[row][column]);
- }
- printf("\r\n");
- }
- }
- void Calc_Process(double radius)
- {
- double C[MATRIX_SIZE];
- double Res[MATRIX_SIZE];
- int i;
- double k;
- ResetMatrix();
- //输入任意个数磁场测量点坐标，请尽量保证在椭球上分布均匀
- CalcData_Input(7 , -7 , -2);
- CalcData_Input(-1 , -7 , -2);
- CalcData_Input(3 , 3 , -2);
- CalcData_Input(3 , -17 , -2);
- CalcData_Input(3 , -7 , 4);
- CalcData_Input(3 , -7 , -8);
- Matrix_GaussElimination();
- Matrix_RowSimplify();
- //赋值任意解参数值
- ……………………
- …………限于本文篇幅 余下代码请从51黑下载附件…………
所有资料51hei提供下载:
Calibration.rar
(2 KB, 下载次数: 137)
|
[复制链接]
