|
|
羅盤(pán)和加計(jì)的校準(zhǔn)是日常開(kāi)發(fā)中最基礎(chǔ)的工作，特邀Echo老師對(duì)羅盤(pán)和加速度計(jì)校準(zhǔn)的工程方法進(jìn)行總結(jié)，為小伙伴你們解惑，是有此文。
作者信息
Echo，本名鄒佳池，從事嵌入式軟件開(kāi)發(fā)。
超詳細(xì)講解：羅盤(pán)和加速度計(jì)校正方法
(附C源代碼)
1.為什么要校正我們都知道，羅盤(pán)是測(cè)量周?chē)拇艌?chǎng)強(qiáng)度，若不存在外在磁場(chǎng)的干擾，只存在地磁的話(huà)，理論上羅盤(pán)旋轉(zhuǎn)測(cè)得的磁場(chǎng)是一個(gè)圓球。
可是現(xiàn)實(shí)空間中，除了地磁場(chǎng)外，還存在其他的磁場(chǎng)干擾，這里我索性將它分為兩大類(lèi)。
第一類(lèi)：地球空間中的磁場(chǎng)，這類(lèi)磁場(chǎng)有個(gè)特點(diǎn)，就是隨著羅盤(pán)坐標(biāo)系的轉(zhuǎn)動(dòng)，磁場(chǎng)方向不變，類(lèi)似地磁場(chǎng)。
第二類(lèi)：羅盤(pán)坐標(biāo)空間中的磁場(chǎng)，這類(lèi)磁場(chǎng)源一般是固定在飛機(jī)上的，所以隨著羅盤(pán)坐標(biāo)系的轉(zhuǎn)動(dòng)，磁場(chǎng)方向也跟著轉(zhuǎn)動(dòng)。但是對(duì)于羅盤(pán)坐標(biāo)系來(lái)說(shuō)，卻是一個(gè)恒定值。
對(duì)于第一類(lèi)的磁場(chǎng)，目前我了解到的還沒(méi)有什么好的方式可以進(jìn)行校正（如果哪位大神知道，還請(qǐng)告知）。而我后面要介紹的校正方法，即是濾除第二類(lèi)磁場(chǎng)的干擾，校正的思想即是基于最小二乘法的橢球擬合算法。
注：這個(gè)只能校正磁場(chǎng)強(qiáng)度固定不變的磁場(chǎng)，而對(duì)于電機(jī)這種變化的磁場(chǎng)，我沒(méi)有測(cè)驗(yàn)過(guò)，不知道電機(jī)產(chǎn)生磁場(chǎng)的強(qiáng)度大小跟電機(jī)轉(zhuǎn)速的關(guān)系怎樣，如果誰(shuí)有研究過(guò)的，還請(qǐng)告知，謝謝。
2.橢球擬合校正理論推導(dǎo)網(wǎng)絡(luò)上有許多關(guān)于橢球擬合校正的論文，我都沒(méi)有細(xì)看，因?yàn)槟切┕蕉紝?xiě)得晦澀難懂，沒(méi)有那個(gè)耐心，我這里盡量用最簡(jiǎn)潔的語(yǔ)言介紹校正方法的理論基礎(chǔ)。
首先建立數(shù)值模型，設(shè)測(cè)量值為：
,校正后的值為：，
平移參數(shù)為：
縮放參數(shù)為：
他們之間的關(guān)系如下所示：
我們校正后的目標(biāo)就是使得校正值近似分布在一個(gè)圓球上，而圓球的公式大家都知道：x2+y2+z2=R2，故我們將校正后的值帶入圓球公式，與理論的圓球半徑平方做差，構(gòu)建誤差u：
將校正值用測(cè)量值替換，變?yōu)椋?br />
可以看到，這分明就是個(gè)橢圓公式嘛~
記：
則我們的誤差u可以寫(xiě)為如下形式：
下面就是校正的核心思想了：假設(shè)我們有許多組數(shù)據(jù)，我們要求得一組參數(shù)，使得所有數(shù)據(jù)的誤差和最小，即∑u最小，但是由于u有正有負(fù)，所以符號(hào)相反的誤差有可能相互抵消。那么加絕對(duì)值呢？這個(gè)也不可取，因?yàn)閷?duì)絕對(duì)值函數(shù)求極小值十分復(fù)雜。那么我們自然就想到對(duì)u求平方和，即：
U=∑u^2
我們把u看成一個(gè)未知數(shù)，這個(gè)函數(shù)是一個(gè)二次函數(shù)，其有極小值點(diǎn)。而為了求得這個(gè)極小值點(diǎn)，我們對(duì)其做偏導(dǎo)即可：
記：
則我們可以將偏導(dǎo)寫(xiě)成如下形式：
B是已知的，P是我們待求的參數(shù)矩陣，故可以通過(guò)求齊次線(xiàn)性方程組，來(lái)求得P的各個(gè)參數(shù)的解。
齊次線(xiàn)性方程組求解的過(guò)程我這里就不詳細(xì)解釋了，我算法中使用的方法是經(jīng)典的高斯消元法，有興趣的可以仔細(xì)看看。
當(dāng)我們求得P的各個(gè)未知數(shù)a,b,c,d,e,f,g后，需要通過(guò)這幾個(gè)參數(shù)反求出我們的偏移量(ox,oy,oz)和縮放量(gx,gy,gz)。
在反求解之前，我們先回到上一個(gè)式子，BxP=0。其實(shí)滿(mǎn)足這個(gè)式子的解P有無(wú)數(shù)組，我們可以將式子改寫(xiě)成BxCP=0，C是一個(gè)任意常數(shù)，即
我們通過(guò)解線(xiàn)性方程組求得的只是這個(gè)解系中的一個(gè)基本解，所以我們首先要求出這個(gè)基本解的C。
其算式經(jīng)推導(dǎo)如下,帶入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)
最后，將這六個(gè)參數(shù)回調(diào)到之前的式子中去，即完成校正。
3.校正程序源碼(C語(yǔ)言)代碼太長(zhǎng)了 就不貼出來(lái)了，放在網(wǎng)盤(pán)種大家下載
羅盤(pán)與加計(jì)校準(zhǔn)方法 C源代碼
單片機(jī)源程序如下:
- #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;
-
- //構(gòu)建VxVt矩陣(Vt為V的轉(zhuǎn)置)，并進(jìn)行累加
- 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++){
- //將當(dāng)前列最大的一行移上來(lái)
- 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，無(wú)需處理
-
- 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;
- }
- //求行最簡(jiǎn)型矩陣
- int Matrix_RowSimplify(void)
- {
- int c = n;//返回值，表示(解的任意常量數(shù)+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))//平移，找出本行第一個(gè)非零；
- {
- row--;
- continue;
- }
- //
- c--;//少一個(gè)常量；
- //
- //化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;//已經(jīng)為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();
- //輸入任意個(gè)數(shù)磁場(chǎng)測(cè)量點(diǎn)坐標(biāo)，請(qǐng)盡量保證在橢球上分布均勻
- 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();
- //賦值任意解參數(shù)值
- ……………………
- …………限于本文篇幅 余下代碼請(qǐng)從51黑下載附件…………
所有資料51hei提供下載:
Calibration.rar
(2 KB, 下載次數(shù): 143)
2017-11-9 04:11 上傳
點(diǎn)擊文件名下載附件
|
評(píng)分
-
查看全部評(píng)分
|
[復(fù)制鏈接]
