基于MEMS陀螺儀輔助的粒子群優化磁力計校正

原雨佳，王 偉，陳興邦

(哈爾濱工程大學自動化學院，哈爾濱 150001)

0 引言

磁力計是一種測量磁場強度的儀器，通過對其三軸分量的計算能夠得到導航中重要的航向角信息，具有無累計誤差的優點。然而磁力計有不可忽視的零位、靈敏度、非正交誤差，以及周圍環境影響造成的軟硬磁誤差，這些都會造成其輸出不準確，從而影響解算航向角的精度。因此，在使用前需要對其進行誤差分析，并采用有效的算法進行補償校正，提高其實用性。

目前針對磁力計校正的算法有很多。橢球擬合法是一種常見的磁力計校正算法，它不需要其他輔助信息，依據將未校正的橢球分布的磁力計數據還原成球型的原理實現校正。雖然此種算法原理簡單、計算量小，但是它需要空間中各個方向的采樣數據，操作復雜[1-3]。極大似然估計法同樣不需要輔助信息，但它對初值的選取有較高的要求，而且計算量大[4-6]。除了以上兩種無姿態的離線校正算法，工程中更為常用的是利用非線性卡爾曼濾波算法實現對誤差參數的在線校正，該算法具有良好的實時性，但是難以預估傳感器的濾波初值和噪聲分布[7-9]。為了解決傳統磁力計校正算法中采樣點多和初值條件嚴苛等問題，武元新提出了將粒子群優化(Particle Swarm Optimization，PSO)算法用于磁力計校正中，此種算法使用更加精準的非線性誤差模型，并且無需良好的初值[10]。之后他對此算法進一步優化，提出了增強型粒子群優化(Stretching Particle Swarm Optimization，SPSO)算法，避免了粒子過早陷入局部最優，但是運算時間長[11]。目前的PSO算法多數采用校正前后的磁場總量不變作為適應函數，只能用于磁力計簡化模型，需要假設傳感器至少有一個軸與載體系重合，從而將12個未知參數簡化成9個或6個參數進行求解[12-13]。

針對傳統PSO算法用于磁力計校正中估計參數個數少的問題，本文提出了一種基于微機電系統(Micro-Electro-Mechanical System，MEMS)陀螺儀輔助的粒子群優化算法對磁力計進行校正，為了避免算法過早陷入局部最優，并且在動態環境中具有較好的適應性，采用了隨機漂移粒子群優化算法[14]。通過理論仿真和實測數據證明，該算法能夠有效地補償磁力計誤差，具有更好的工程實用性。

1 磁力計誤差模型

根據對磁力計的靈敏度、零位、非正交以及軟硬磁誤差的分析，建立如下的磁力計誤差模型[15]。

(1)

其中：

S=SscSnoSsi

b=bm+bhi

(2)

(3)

2 基于陀螺儀輔助的隨機漂移粒子群優化算法

粒子群算法是一種仿效鳥類覓食過程的優化算法，通過迭代不斷提高與目標函數有關的種群的候選解，即通過位置與速度更新公式尋找適應目標函數的粒子，每個粒子的移動不僅受自身經驗的影響，也受當前種群中最優位置的影響。與其他算法相比，粒子群算法需要設置和調整的參數較少，而且收斂速度快，近些年發展迅猛。

2.1 目標函數

粒子群濾波中有2個重要的極值，一個是粒子個體最優值，用Pbest表示，另一個是種群的全局最優值，用Gbest表示。算法的目的就是尋找Gbest使目標函數值最小。假設待解決問題的粒子維數為D，種群中粒子個數為m，在第k次迭代中，第i個粒子的位置向量可以表示為：

Pi(k)=(pi1(k),pi2(k),pi3(k),…,piD(k))T

(4)

速度向量可以表示為：

Vi(k)=(vi1(k),vi2(k),vi3(k),…,viD(k))T

(5)

從磁力計誤差模型中可以看出，校正算法的目的就是求解公式中的S和b，所以每個粒子可以用如下的數學式表示：

(6)

其中：

(7)

從誤差模型上可以看出，各種誤差對各軸向產生的影響是不同的，在有陀螺儀輔助的情況下，通過陀螺儀解算姿態角信息，在已知地磁場的情況下得到理想載體系下的磁場矢量，此磁場矢量應與磁力計補償后的磁場矢量相同。由此可以建立磁場矢量適應函數，將其寫為下面的數學表達式

F2,i=argminf2(S,b)

(8)

2.2 隨機漂移粒子群優化算法

傳統PSO算法在一定的迭代后，粒子速度會慢慢減小，使得種群收斂到某個最優值，一旦環境發生變化，失去多樣性的種群很難對此做出快速響應。在實際磁力計使用中，周圍有不可避免的磁性物質，這些都會對磁力計輸出造成影響，因此，本文采用一種具有較強全局搜索能力的隨機漂移粒子群優化(Rondom Drift PSO，RDPSO)算法。受金屬導體內自由電子運動模型的影響，通過將最優值的搜索過程類比于自由電子向具有最小勢能位置的運動，從而提高算法的搜索能力[16]。

該算法的具體流程為：

1)粒子群初始化

初始化粒子個數m，粒子維數D，權重的最大值ωmax和最小值ωmin，最大的迭代次數kmax，速度限制的最大值Vmax和最小值Vmin，位置限制的最大值Pmax和最小值Pmin。按照式(9)初始化粒子速度和位置。

Pi,j(k=0)=Pminj+rand(0,1)(Pmaxj-Pminj)

Vi,j(k=0)=Vmin+rand(0,1)(Vmax-Vmin)

(9)

式中，Pminj和Pmaxj分別是每個粒子第j維對應的位置最大值和最小值，對于磁力計校正，前9維對應的位置限定范圍要小于后3維偏置對應的位置范圍，這樣設置可以為濾波算法提供更實用的初值。

在初始化階段，粒子的個體最優Pbesti(k=0)≡Pi(k=0)，通過式(8)計算每個粒子的適應值，選擇最優的位置為種群最優值Gbest。

2)粒子群更新

與傳統PSO算法速度更新方式不同，RDPSO算法中粒子運動包括兩部分，即定向漂移運動和無規則隨機熱運動，數學表達式為：

Vij(k+1)=Vrij(k+1)+Vdij(k+1)

(10)

式中，k是迭代次數，i(i=1,2,3,…,m)表示粒子群中第i個粒子，j(j=1,2,3,…,D)表示粒子的第j維，Vrij(k+1)表示無規則隨機熱運動，Vdij(k+1)表示定向漂移運動。

熱運動可以表示成：

Vrij(k+1)=α|Mj(k)-pij(k)|φij(k)

(11)

其中：

(12)

(13)

(14)

式中，α是熱系數，表征算法的全局搜索能力，本文改變其為固定常數的情況，設置為隨迭代次數增加而減小的S型函數，數學表達式為式(12)。Mj(k)是種群粒子個體歷史最優位置的平均值，數學表達式為式(13)。φij(k)是正態隨機分布函數，s和uij(k)是(0,1)之間的2個不同隨機數，采用式(14)求解。

粒子的定向漂移運動可以定義為：

Vdij(k+1)= rand(0,1)c1(pbestij(k)-pij(k))+

rand(0,1)c2(gbestj(k)-pij(k))

(15)

式中，c1和c2分別是自我認知和社會認知,分別代表粒子自身位置最優和種群位置最優的影響；rand(0,1)是(0,1)之間的隨機數。

綜上，RDPSO算法的粒子速度和位置公式為：

vij(k)=α|Mj(k)-pij(k)|φij(k)+

rand(0,1)c1(pbestij(k)-pij(k))+

rand(0,1)c2(gbestj(k)-pij(k))

(16)

pij(k+1)=pij(k)+vij(k+1)

(17)

按照上式對粒子的速度和位置進行更新，同時在更新過程中限制速度和位置在所定范圍內。

3)搜索全局最優

因為本文中的問題是解決最小值問題，因此種群的目標就是找到最小的適應值，對于每個粒子，如果新的適應值小于Pbest對應的適應值，則更新當前位置的Pbest，否則Pbest不變。可以將Pbest寫為：

(17)

同樣的，如果新的Pbest對應的適應值小于此時Gbest對應的適應值，則更新Gbest，否則Gbest不變。可以將Gbest寫為：

(18)

其中，f是目標函數，即式(8)。

4)迭代

迭代停止的方法有兩種：種群的適應值達到設定的最小值；迭代次數達到最大。本文使用的是第二種方法，如果迭代超出kmax，則迭代停止，此時的Gbest就是要求解的參數矩陣。

由此可見，此算法就是通過迭代追蹤個體歷史最優值和種群歷史最優值，其流程圖如圖1所示。

3 仿真實驗分析

本算法使用的環境是在哈爾濱市，依據IGRF12模型[17]計算當地的地磁場矢量為Hi=[242.367;-46.225;-465.459]mG2。為了測試本算法的性能，分別做了以下幾個仿真模擬實驗。

3.1 參數估計數量對比實驗

為了比較本文所提算法與傳統PSO算法的參數估計數量，模擬磁力計分別繞X、Y、Z軸旋轉一周產生的數據，設置采樣周期為0.01s，從生成的3000個點選取均勻分布的30個點，陀螺儀漂移設為6.25(°)/h，具體的參數設置如表1所示。分別使用傳統PSO算法和本文所提算法進行校正，校正結果如表2所示。

表1 粒子群優化算法參數設置表

表2 12個誤差參數估計結果

從表中可以看出，使用傳統算法無法準確地估計出磁力計誤差模型中的12個參數，有些參數的估計結果與參考值相差較大。這是因為傳統算法只選用約束磁場總量的目標函數，沒有考慮磁場的三軸分量，造成當誤差矩陣S陣為9個不同參數時，粒子群尋找最優值受到較大干擾。而本文提出的RDPSO算法卻能得到較為精準的參數估計結果。

3.2 旋轉度測試實驗

為了簡化校正中操作的復雜度，該實驗通過模擬磁力計的幾種不同旋轉方式，驗證算法在各種旋轉度下的性能。旋轉方式如下：

方式一：模擬磁力計在繞X、Y、Z軸各旋轉1周，即在空間中是3個完整的圓形；

方式二：模擬磁力計繞X、Y軸各旋轉1周，即在空間中是2個完整的圓形；

方式三：模擬磁力計繞X、Y軸各不完整旋轉1周，即在空間中是2個不完整的圓形；

方式四：模擬磁力計繞Z軸旋轉1周，即在空間中是1個完整的圓形。

針對四種不同旋轉方式獲取的采樣點，采用RDPSO算法進行校正，得到的參數估計結果如表3所示。

表3 各種旋轉度測試結果

從表3中可以看出，當采樣點在空間分布為不完整2周時，本文所提算法就能夠估計出磁力計誤差參數，與空間分布為完整3周時校正精度接近。但如果單獨繞一軸旋轉，即采樣點空間分布為平面1周時，由于粒子的多樣性過低，無法估計出磁力計12個誤差參數，尤其是對偏置矩陣無法估計。因此，本文所提算法只需要磁力計在空間中繞X、Y、Z中的任意2個軸不完整旋轉1周即可實現對誤差參數的估計，相對于傳統算法大大簡化了操作的復雜度。

圖2～圖4所示為磁力計分別旋轉完整3周、完整2周及不完整2周時，磁力計校正前后的采樣點分布圖。理想情況下，采樣點應該分布在以地磁場強度為半徑的球上，但由于各種誤差的干擾使其偏離。從圖中可以看出，校正前的采樣點都在球面外側，經過本文算法校正使其重新分布在球面，說明本算法能夠對磁力計誤差進行有效補償。

3.3 動態性能測試實驗

為了測試本文所提算法在磁場動態環境中的性能，做了以下的仿真實驗。模擬所在磁場環境是變化的，即不再設定磁場強度為一個定值，仿真中將其設置為線性變化的，得到的實驗結果如表4所示。

表4 動態性能測試結果

Fg是當粒子為最優位置時，適應函數的值，即通過式(8)得到的值，可以表征算法的精度。因此，可以看出，在磁場變化時，本文所提算法比傳統算法的精度高，說明本文所提算法具有更高的動態適應能力。

3.4 陀螺儀精度影響測試實驗

為了驗證不同陀螺儀精度對算法精度的影響，本文針對幾種常見的陀螺儀精度做了仿真實驗，得到的結果如表5所示。

表5 陀螺儀精度影響測試結果

從表5可以看出，陀螺儀的精度會對本文算法造成影響，但是影響較小。

4 實測實驗驗證

為了驗證該算法在實際中的有效性，選用以ADIS16488為核心的MEMS-IMU系統作為實驗設備，實物如圖5所示，它的參數如表6所示。用ADIS16488 MEMS-IMU系統采集數據，同時為了降低陀螺儀誤差對算法精度造成的影響，取陀螺儀靜態數據的平均值作為零漂減去，再采用本文所提算法、PSO算法和擴展卡爾曼濾波(Extended Kalman Filter，EKF)算法對磁力計校正，得到的參數估計結果如表7所示，利用本文算法得到的磁力計校正前后數據的對比如圖6所示。從表中可以看出，本文所提算法精度最高，說明本算法在實際系統中能夠實現磁力計的校正。

表6 ADIS16488參數

表7 不同算法參數估計結果

5 結論

1)解決了傳統粒子群優化算法用于磁力計校正時需要簡化誤差模型，同時粒子容易陷入局部最優的問題；

2)磁力計的誤差參數利用陀螺儀提供的角速率信息，建立補償前后的磁場矢量適應函數，再迭代尋找粒子的全局最優值求解；

3)仿真與實測實驗表明，只要將磁力計繞其三軸中的任意兩軸旋轉不完整一周即可估計磁力計的12個誤差參數，相對于傳統算法操作簡單、動態性好；

4)僅針對陀螺儀零位漂移對算法精度產生的影響進行了仿真實驗，并沒有對陀螺儀各種誤差進行詳細的分析；

5)本文算法僅能進行離線估計，需進一步研究在線估計磁力計誤差參數及陀螺儀誤差，提高算法的工程實用性。