单片机论坛

 找回密码
 立即注册

QQ登录

只需一步,快速开始

搜索
查看: 680|回复: 4
打印 上一主题 下一主题
收起左侧

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

[复制链接]
跳转到指定楼层
楼主
XFZ 发表于 2017-11-7 10:08 | 只看该作者 回帖奖励 |倒序浏览 |阅读模式
罗盘和加计的校准是日常开发中最基础的工作,特邀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源代码


单片机源程序如下:
  1. #include "stdafx.h"
  2. #include "string.h"
  3. #include "math.h"

  4. #define MATRIX_SIZE 7
  5. #define u8 unsigned char

  6. double m_matrix[MATRIX_SIZE][MATRIX_SIZE+1];
  7. int m = MATRIX_SIZE;       
  8. int n = MATRIX_SIZE+1;
  9. double m_result[MATRIX_SIZE];       

  10. void DispMatrix(void);

  11. double Abs(double a)
  12. {
  13.         return a<0 ? -a : a;
  14. }

  15. u8 Equal(double a,double b)
  16. {
  17.         return Abs(a-b) < 1e-6;
  18. }

  19. void ResetMatrix(void)
  20. {
  21.         int row , column;
  22.        
  23.         for(row = 0 ; row<m ; row++){
  24.                 for(column = 0 ; column<n ; column++)
  25.                         m_matrix[row][column] = 0.0f;
  26.         }
  27. }
  28.        
  29. void CalcData_Input(double x , double y , double z)
  30. {
  31.         double V[MATRIX_SIZE];
  32.         int row , column;
  33.        
  34.         V[0] = x*x;
  35.     V[1] = y*y;
  36.     V[2] = z*z;
  37.     V[3] = x;
  38.     V[4] = y;
  39.     V[5] = z;
  40.     V[6] = 1.0;
  41.        
  42.         //构建VxVt矩阵(Vt为V的转置),并进行累加
  43.         for(row = 0 ; row<MATRIX_SIZE ; row++){
  44.                 for(column = 0 ; column<MATRIX_SIZE ; column++){
  45.                         m_matrix[row][column] += V[row]*V[column];
  46.                 }
  47.         }
  48. }

  49. void SwapRow(int row1 , int row2)
  50. {
  51.         int column;
  52.         double tmp;
  53.        
  54.         for(column = 0 ; column<n ; column++){
  55.                 tmp = m_matrix[row1][column];
  56.                 m_matrix[row1][column] = m_matrix[row2][column];
  57.                 m_matrix[row2][column] = tmp;
  58.         }
  59. }

  60. void MoveBiggestElement2Top(int s_row , int s_column)
  61. {
  62.         int row,column;
  63.        
  64.         for(row = s_row+1 ; row<m ; row++){
  65.                 if( Abs(m_matrix[s_row][s_column])<Abs(m_matrix[row][s_column])){
  66.                         SwapRow(s_row , row);
  67.                 }
  68.         }
  69. }

  70. //高斯消元法,求行阶梯型矩阵
  71. u8 Matrix_GaussElimination(void)
  72. {
  73.         int row,column,i,j;
  74.         double tmp;
  75.        
  76.         for(row = 0,column=0 ; row<m-1 && column<n-1 ; row++,column++){
  77.                 //将当前列最大的一行移上来
  78.                 MoveBiggestElement2Top(row , column);
  79.                
  80.                 //整列都为0
  81.                 if(Equal(m_matrix[row][column],0.0f)){
  82.                         printf("qiyi matrix:%d %d\r\n" , row , column);
  83.                         //DispMatrix();
  84.                         //return 0;
  85.                         row--;
  86.                         continue;
  87.                 }
  88.                
  89.                 //高斯消元
  90.                 for(i = row+1 ; i<m ; i++){       
  91.                         if(Equal(m_matrix[i][column],0.0f))
  92.                                 continue;        //为0,无需处理
  93.                        
  94.                         tmp = m_matrix[i][column]/m_matrix[row][column];
  95.                        
  96.                         for(j = column ; j<n ; j++){
  97.                                 m_matrix[i][j] -= m_matrix[row][j]*tmp;
  98.                         }
  99.                 }

  100.                 DispMatrix();
  101.                 printf("\r\n");
  102.         }

  103.         return 1;
  104. }

  105. //求行最简型矩阵
  106. int Matrix_RowSimplify(void)
  107. {
  108.     int c = n;//返回值,表示(解的任意常量数+1);
  109.     //
  110.     int row,column,k,s,t;
  111.     double tmp;
  112.     //
  113.     for(row=0,column=0;row<m && column<n;row++,column++)
  114.     {
  115.         if(Equal(m_matrix[row][column],0))//平移,找出本行第一个非零;
  116.         {
  117.             row--;
  118.             continue;
  119.         }
  120.         //
  121.         c--;//少一个常量;
  122.         //
  123.         //化a[i][j]为1;
  124.         tmp = 1 / m_matrix[row][column];
  125.         for(k=column;k<n;k++)//前面的"0"就不处理了;
  126.             m_matrix[row][k] *= tmp;
  127.         //
  128.         //化a[s][j]为0
  129.         for(s=0;s<row;s++)//下面的0也不用处理;
  130.         {
  131.             if(Equal(m_matrix[s][column],0))
  132.                 continue;//已经为0;
  133.             //
  134.             tmp = m_matrix[s][column] / m_matrix[row][column];
  135.             for(t=column;t<n;t++)
  136.                 m_matrix[s][t] -= m_matrix[row][t]*tmp;
  137.             //
  138.         }
  139.     }
  140.     //
  141.     return c;
  142. }

  143. void Matrix_Solve(double* C , double* sol)
  144. {
  145.         int row,column,i;
  146.         int any_sol[MATRIX_SIZE];

  147.         //找出任意解的位置
  148.         memset(any_sol , 0 , MATRIX_SIZE);
  149.         for(row=0,column=0 ; row<m && column<n-1 ; row++,column++){
  150.                 if(Equal(m_matrix[row][column] , 0.0f)){
  151.                         any_sol[column] = 1;        //记录任意解的位置
  152.                         row--;        //右移1列
  153.                 }
  154.         }

  155.         //求解
  156.         row = 0;
  157.         for(column = 0 ; column<n-1 ; column++){
  158.                 if(any_sol[column] == 1){        //任意解
  159.                         sol[column] = C[column];
  160.                 }else{
  161.                         sol[column] = m_matrix[row][n-1];
  162.                         //加上任意解
  163.                         for(i = column+1 ; i<n-1 ; i++){
  164.                                 if(any_sol[i]==1 && !Equal(m_matrix[row][i],0.0f)){
  165.                                         sol[column] -= m_matrix[row][i]*C[i];
  166.                                 }
  167.                         }       
  168.                         row++;
  169.                 }
  170.         }
  171. }

  172. void DispMatrix(void)
  173. {
  174.         int row,column;
  175.        
  176.         for(row = 0 ; row<m ; row++){
  177.                 for(column = 0 ; column<n ; column++){
  178.                         printf("%.3f        " , m_matrix[row][column]);
  179.                 }
  180.                 printf("\r\n");
  181.         }
  182. }

  183. void Calc_Process(double radius)
  184. {
  185.         double C[MATRIX_SIZE];
  186.         double Res[MATRIX_SIZE];
  187.         int i;
  188.         double k;

  189.         ResetMatrix();

  190.         //输入任意个数磁场测量点坐标,请尽量保证在椭球上分布均匀
  191.         CalcData_Input(7 , -7 , -2);
  192.         CalcData_Input(-1 , -7 , -2);
  193.         CalcData_Input(3 , 3 , -2);
  194.         CalcData_Input(3 , -17 , -2);
  195.         CalcData_Input(3 , -7 , 4);
  196.         CalcData_Input(3 , -7 , -8);

  197.         Matrix_GaussElimination();
  198.         Matrix_RowSimplify();

  199.     //赋值任意解参数值
  200. ……………………

  201. …………限于本文篇幅 余下代码请从51黑下载附件…………
复制代码

所有资料51hei提供下载:
Calibration.rar (2 KB, 下载次数: 7)




分享到:  QQ好友和群QQ好友和群 QQ空间QQ空间 腾讯微博腾讯微博 腾讯朋友腾讯朋友
收藏收藏 转播转播 分享分享 分享淘帖 顶 踩
回复

使用道具 举报

沙发
admin 发表于 2017-11-9 04:12 | 只看该作者
好资料,51黑有你更精彩!!!
回复

使用道具 举报

板凳
xinzaizai 发表于 2017-11-24 16:50 | 只看该作者

好资料,帮了大忙
回复

使用道具 举报

地板
my51hei-huashen 发表于 2018-2-12 11:16 | 只看该作者
非常感谢!
回复

使用道具 举报

5#
iyzyh 发表于 2018-2-22 15:22 | 只看该作者
谢谢分享!刚刚好需要!
回复

使用道具 举报

您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

QQ|手机版|小黑屋|单片机论坛

Powered by 单片机教程网

快速回复 返回顶部 返回列表