羅盤和加計的校準是日常開發中最基礎的工作，特邀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, 下載次數: 143)
2017-11-9 04:11 上傳
點擊文件名下載附件
下載積分: 黑幣 -5
|
[復制鏈接]
QQ好友和群
QQ空間
騰訊微博
騰訊朋友
收藏
淘帖
頂
踩
