磁力計校正和數據處理

在飛控涉及到的幾個傳感器中磁力計因該是屬於最簡單的傳感器。在飛控上一般使用的磁力計傳感器芯片是HMC5883L、HMC5983、MPU9250、MAG3110等。這些芯片利用的物理原理都相同，只是在精度和數據接口上有些區別。下面簡單說說磁力計的原理。
我們的地球都是有磁場存在的，地理的南極是地磁的北極，在地球的每一個區域都是受地磁影響的，可以說地磁無處不在。而且地磁的方向都是一定的，除了在南北極地磁場是從上指到下（或者從下指到上）的，在其他區域基本都是由南指向北的（暫不考慮磁偏角）。這樣就給人類的遠行活動帶了極大的便利。由於地磁場的這個特性我們可以使用羅盤（指南針）很容易的在地球的任何角落辨別出南北，隨之另外兩個方向東西也能確定下來。磁力計的原理就是使用霍爾元件的原理測量磁場的傳感器。在實際的空間中只要不在赤道上空地磁場的方向有從南指向北的分量，還有從上指向下（或者從下指向上的分量）。霍爾元件只能測量一個方向的磁場，所以磁力計要在空間中測量磁場的話要使用三個方向的霍爾傳感器測量到三個正交的方向的磁場後合成一個合磁場，再使用這個磁場的方向和地磁場的方向做對比從而可以獲得傳感器自身的方向和姿態。這些是基於一個比較普遍的假設：在很小的區域可以認爲地磁場的方向和大小都是不變的。很小的區域相對於地球來說可能是一個或者幾個國家的大小。
下面使用stm32芯片讀取hmc5883的磁場數據然後使用matlab對數據進行處理。需要工程代碼的可以在留言中留下郵箱號向我要，我看到就會發給你。下面是工程主循環中的代碼：

while(1)
{
Delay(200);
X_Data = Single_Read(HMC5983_Write_Addr,0x03);
X_Data <<= 8;
X_Data += Single_Read(HMC5983_Write_Addr,0x04);
Z_Data = Single_Read(HMC5983_Write_Addr,0x05);
Z_Data <<= 8;
Z_Data += Single_Read(HMC5983_Write_Addr,0x06);
Y_Data = Single_Read(HMC5983_Write_Addr,0x07);
Y_Data <<= 8;
Y_Data += Single_Read(HMC5983_Write_Addr,0x08);
printf(“%d %d %d “,X_Data,Y_Data,Z_Data);
}

讀取三軸磁場的數據後按順序輸出數據。在PC端使用串口終端接收數據。在接收數據的過程中不斷變化傳感器的方向，由三個分量組成的空間向量其實就是地磁場相對於傳感器的位置向量。我們在各個位置旋轉傳感器，這樣由每個時刻的向量的端點組成的一個圖應該是一個以原點爲圓心的球。下面我們採集數據。

在MATLAB中數據全部放在數組A中，下面是MATLAB代碼：

與我們想象的不同，畫出來的圖形並不是一個球而是一個橢球，中心也不在原點。

再看看兩個軸X、Y之間的圖形（三維空間點的投影）。

很好！是一個圓，Y軸的數據分佈在0的上下，但是X軸的數據偏離了0點。由於傳感器在磁場的正反兩個方向測量到的數據應該是大小相同符號相反的數據，也就是它應該是一個以原點爲圓心的圓。現在這種情況說明數據是有零點漂移的，校正的方法很簡單，我們可以通過matlab的數組X可以看出X的最大值是676最小值爲-499，對最大值和最小值作和然後除以2就是X需要減去的零點補償值即：(676-499)/2 = 88.5。然後以後每次採集到的X軸數據減去一個零點漂移值88.5就能校正這個軸的數據。
MATLAB代碼：

Y = Y+25;
再次畫圖如下:

對Z軸的數據沒有在原點對齊所以也需要類似的處理

做了零點漂移校正後的數據如下：

matlab代碼：
Z = Z-149.5
此時我們在代碼中需要做的就是對每一次採集到的數據都減去相應的校正值。對單片機編程的C代碼應該改成如下：
X_Data = Single_Read(HMC5983_Write_Addr,0x03);
X_Data <<= 8;
X_Data += Single_Read(HMC5983_Write_Addr,0x04);
X_Data += 25;
Z_Data = Single_Read(HMC5983_Write_Addr,0x05);
Z_Data <<= 8;
Z_Data += Single_Read(HMC5983_Write_Addr,0x06);
Z_Data -= 149;
Y_Data = Single_Read(HMC5983_Write_Addr,0x07);
Y_Data <<= 8;
Y_Data += Single_Read(HMC5983_Write_Addr,0x08);
Y_Data += 25;

從上面的數據我們也看到，對於零點漂移校正的三軸數據，仍舊不是一箇中心在原點的球，而是一個橢球（半徑不同）。X軸上是587，Y軸上是571，Z軸上是638。下面就需要進行橢球校正，這個原理也很簡單。由於三個軸上的傳感器的測量敏感度不同導致了數據的幅值不同，因此我們需要做的就是以一個軸的數據爲標準對其餘兩個軸的數據進行加權。下面是具體操作：
以X軸爲標準
Y = Y*(Xmax-Xmin)/(Ymax-Ymin);
Z = Z*(Xmax-Xmin)/(Zmax-Zmin);

這樣就將圖形轉化成了一個半徑爲587.5的圓球了。

對應的單片機的C代碼改成：

X_Data = Single_Read(HMC5983_Write_Addr,0x03);
X_Data <<= 8;
X_Data += Single_Read(HMC5983_Write_Addr,0x04);
X_Data += 25;
Z_Data = Single_Read(HMC5983_Write_Addr,0x05);
Z_Data <<= 8;
Z_Data += Single_Read(HMC5983_Write_Addr,0x06);
Z_Data -= 149;
Z_Data = Z_Data*(587.5)/638.5;
Y_Data = Single_Read(HMC5983_Write_Addr,0x07);
Y_Data <<= 8;
Y_Data += Single_Read(HMC5983_Write_Addr,0x08);
Y_Data += 25;

下面如果傳感器是水平的，對應飛行器平衡的時候需要求解角度爲
Angle = arctan(X_Data/Y_Data);

                                            山東大學機器人研究中心