原雨佳,王 偉,陳興邦
(哈爾濱工程大學(xué)自動化學(xué)院,哈爾濱 150001)
磁力計是一種測量磁場強度的儀器,通過對其三軸分量的計算能夠得到導(dǎo)航中重要的航向角信息,具有無累計誤差的優(yōu)點。然而磁力計有不可忽視的零位、靈敏度、非正交誤差,以及周圍環(huán)境影響造成的軟硬磁誤差,這些都會造成其輸出不準(zhǔn)確,從而影響解算航向角的精度。因此,在使用前需要對其進行誤差分析,并采用有效的算法進行補償校正,提高其實用性。
目前針對磁力計校正的算法有很多。橢球擬合法是一種常見的磁力計校正算法,它不需要其他輔助信息,依據(jù)將未校正的橢球分布的磁力計數(shù)據(jù)還原成球型的原理實現(xiàn)校正。雖然此種算法原理簡單、計算量小,但是它需要空間中各個方向的采樣數(shù)據(jù),操作復(fù)雜[1-3]。極大似然估計法同樣不需要輔助信息,但它對初值的選取有較高的要求,而且計算量大[4-6]。除了以上兩種無姿態(tài)的離線校正算法,工程中更為常用的是利用非線性卡爾曼濾波算法實現(xiàn)對誤差參數(shù)的在線校正,該算法具有良好的實時性,但是難以預(yù)估傳感器的濾波初值和噪聲分布[7-9]。為了解決傳統(tǒng)磁力計校正算法中采樣點多和初值條件嚴(yán)苛等問題,武元新提出了將粒子群優(yōu)化(Particle Swarm Optimization,PSO)算法用于磁力計校正中,此種算法使用更加精準(zhǔn)的非線性誤差模型,并且無需良好的初值[10]。之后他對此算法進一步優(yōu)化,提出了增強型粒子群優(yōu)化(Stretching Particle Swarm Optimization,SPSO)算法,避免了粒子過早陷入局部最優(yōu),但是運算時間長[11]。目前的PSO算法多數(shù)采用校正前后的磁場總量不變作為適應(yīng)函數(shù),只能用于磁力計簡化模型,需要假設(shè)傳感器至少有一個軸與載體系重合,從而將12個未知參數(shù)簡化成9個或6個參數(shù)進行求解[12-13]。
針對傳統(tǒng)PSO算法用于磁力計校正中估計參數(shù)個數(shù)少的問題,本文提出了一種基于微機電系統(tǒng)(Micro-Electro-Mechanical System,MEMS)陀螺儀輔助的粒子群優(yōu)化算法對磁力計進行校正,為了避免算法過早陷入局部最優(yōu),并且在動態(tài)環(huán)境中具有較好的適應(yīng)性,采用了隨機漂移粒子群優(yōu)化算法[14]。通過理論仿真和實測數(shù)據(jù)證明,該算法能夠有效地補償磁力計誤差,具有更好的工程實用性。
根據(jù)對磁力計的靈敏度、零位、非正交以及軟硬磁誤差的分析,建立如下的磁力計誤差模型[15]。
(1)
其中:
S=SscSnoSsi
b=bm+bhi
(2)
(3)
粒子群算法是一種仿效鳥類覓食過程的優(yōu)化算法,通過迭代不斷提高與目標(biāo)函數(shù)有關(guān)的種群的候選解,即通過位置與速度更新公式尋找適應(yīng)目標(biāo)函數(shù)的粒子,每個粒子的移動不僅受自身經(jīng)驗的影響,也受當(dāng)前種群中最優(yōu)位置的影響。與其他算法相比,粒子群算法需要設(shè)置和調(diào)整的參數(shù)較少,而且收斂速度快,近些年發(fā)展迅猛。
粒子群濾波中有2個重要的極值,一個是粒子個體最優(yōu)值,用Pbest表示,另一個是種群的全局最優(yōu)值,用Gbest表示。算法的目的就是尋找Gbest使目標(biāo)函數(shù)值最小。假設(shè)待解決問題的粒子維數(shù)為D,種群中粒子個數(shù)為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,所以每個粒子可以用如下的數(shù)學(xué)式表示:
(6)
其中:
(7)
從誤差模型上可以看出,各種誤差對各軸向產(chǎn)生的影響是不同的,在有陀螺儀輔助的情況下,通過陀螺儀解算姿態(tài)角信息,在已知地磁場的情況下得到理想載體系下的磁場矢量,此磁場矢量應(yīng)與磁力計補償后的磁場矢量相同。由此可以建立磁場矢量適應(yīng)函數(shù),將其寫為下面的數(shù)學(xué)表達式
F2,i=argminf2(S,b)
(8)
傳統(tǒng)PSO算法在一定的迭代后,粒子速度會慢慢減小,使得種群收斂到某個最優(yōu)值,一旦環(huán)境發(fā)生變化,失去多樣性的種群很難對此做出快速響應(yīng)。在實際磁力計使用中,周圍有不可避免的磁性物質(zhì),這些都會對磁力計輸出造成影響,因此,本文采用一種具有較強全局搜索能力的隨機漂移粒子群優(yōu)化(Rondom Drift PSO,RDPSO)算法。受金屬導(dǎo)體內(nèi)自由電子運動模型的影響,通過將最優(yōu)值的搜索過程類比于自由電子向具有最小勢能位置的運動,從而提高算法的搜索能力[16]。
該算法的具體流程為:
1)粒子群初始化
初始化粒子個數(shù)m,粒子維數(shù)D,權(quán)重的最大值ωmax和最小值ωmin,最大的迭代次數(shù)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維對應(yīng)的位置最大值和最小值,對于磁力計校正,前9維對應(yīng)的位置限定范圍要小于后3維偏置對應(yīng)的位置范圍,這樣設(shè)置可以為濾波算法提供更實用的初值。
在初始化階段,粒子的個體最優(yōu)Pbesti(k=0)≡Pi(k=0),通過式(8)計算每個粒子的適應(yīng)值,選擇最優(yōu)的位置為種群最優(yōu)值Gbest。
2)粒子群更新
與傳統(tǒng)PSO算法速度更新方式不同,RDPSO算法中粒子運動包括兩部分,即定向漂移運動和無規(guī)則隨機熱運動,數(shù)學(xué)表達式為:
Vij(k+1)=Vrij(k+1)+Vdij(k+1)
(10)
式中,k是迭代次數(shù),i(i=1,2,3,…,m)表示粒子群中第i個粒子,j(j=1,2,3,…,D)表示粒子的第j維,Vrij(k+1)表示無規(guī)則隨機熱運動,Vdij(k+1)表示定向漂移運動。
熱運動可以表示成:
Vrij(k+1)=α|Mj(k)-pij(k)|φij(k)
(11)
其中:
(12)
(13)
(14)
式中,α是熱系數(shù),表征算法的全局搜索能力,本文改變其為固定常數(shù)的情況,設(shè)置為隨迭代次數(shù)增加而減小的S型函數(shù),數(shù)學(xué)表達式為式(12)。Mj(k)是種群粒子個體歷史最優(yōu)位置的平均值,數(shù)學(xué)表達式為式(13)。φij(k)是正態(tài)隨機分布函數(shù),s和uij(k)是(0,1)之間的2個不同隨機數(shù),采用式(14)求解。
粒子的定向漂移運動可以定義為:
Vdij(k+1)= rand(0,1)c1(pbestij(k)-pij(k))+
rand(0,1)c2(gbestj(k)-pij(k))
(15)
式中,c1和c2分別是自我認(rèn)知和社會認(rèn)知,分別代表粒子自身位置最優(yōu)和種群位置最優(yōu)的影響;rand(0,1)是(0,1)之間的隨機數(shù)。
綜上,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)
按照上式對粒子的速度和位置進行更新,同時在更新過程中限制速度和位置在所定范圍內(nèi)。
3)搜索全局最優(yōu)
因為本文中的問題是解決最小值問題,因此種群的目標(biāo)就是找到最小的適應(yīng)值,對于每個粒子,如果新的適應(yīng)值小于Pbest對應(yīng)的適應(yīng)值,則更新當(dāng)前位置的Pbest,否則Pbest不變??梢詫best寫為:
(17)
同樣的,如果新的Pbest對應(yīng)的適應(yīng)值小于此時Gbest對應(yīng)的適應(yīng)值,則更新Gbest,否則Gbest不變。可以將Gbest寫為:
(18)
其中,f是目標(biāo)函數(shù),即式(8)。
4)迭代
迭代停止的方法有兩種:種群的適應(yīng)值達到設(shè)定的最小值;迭代次數(shù)達到最大。本文使用的是第二種方法,如果迭代超出kmax,則迭代停止,此時的Gbest就是要求解的參數(shù)矩陣。
由此可見,此算法就是通過迭代追蹤個體歷史最優(yōu)值和種群歷史最優(yōu)值,其流程圖如圖1所示。
本算法使用的環(huán)境是在哈爾濱市,依據(jù)IGRF12模型[17]計算當(dāng)?shù)氐牡卮艌鍪噶繛镠i=[242.367;-46.225;-465.459]mG2。為了測試本算法的性能,分別做了以下幾個仿真模擬實驗。
為了比較本文所提算法與傳統(tǒng)PSO算法的參數(shù)估計數(shù)量,模擬磁力計分別繞X、Y、Z軸旋轉(zhuǎn)一周產(chǎn)生的數(shù)據(jù),設(shè)置采樣周期為0.01s,從生成的3000個點選取均勻分布的30個點,陀螺儀漂移設(shè)為6.25(°)/h,具體的參數(shù)設(shè)置如表1所示。分別使用傳統(tǒng)PSO算法和本文所提算法進行校正,校正結(jié)果如表2所示。
表1 粒子群優(yōu)化算法參數(shù)設(shè)置表
表2 12個誤差參數(shù)估計結(jié)果
從表中可以看出,使用傳統(tǒng)算法無法準(zhǔn)確地估計出磁力計誤差模型中的12個參數(shù),有些參數(shù)的估計結(jié)果與參考值相差較大。這是因為傳統(tǒng)算法只選用約束磁場總量的目標(biāo)函數(shù),沒有考慮磁場的三軸分量,造成當(dāng)誤差矩陣S陣為9個不同參數(shù)時,粒子群尋找最優(yōu)值受到較大干擾。而本文提出的RDPSO算法卻能得到較為精準(zhǔn)的參數(shù)估計結(jié)果。
為了簡化校正中操作的復(fù)雜度,該實驗通過模擬磁力計的幾種不同旋轉(zhuǎn)方式,驗證算法在各種旋轉(zhuǎn)度下的性能。旋轉(zhuǎn)方式如下:
方式一:模擬磁力計在繞X、Y、Z軸各旋轉(zhuǎn)1周,即在空間中是3個完整的圓形;
方式二:模擬磁力計繞X、Y軸各旋轉(zhuǎn)1周,即在空間中是2個完整的圓形;
方式三:模擬磁力計繞X、Y軸各不完整旋轉(zhuǎn)1周,即在空間中是2個不完整的圓形;
方式四:模擬磁力計繞Z軸旋轉(zhuǎn)1周,即在空間中是1個完整的圓形。
針對四種不同旋轉(zhuǎn)方式獲取的采樣點,采用RDPSO算法進行校正,得到的參數(shù)估計結(jié)果如表3所示。
表3 各種旋轉(zhuǎn)度測試結(jié)果
從表3中可以看出,當(dāng)采樣點在空間分布為不完整2周時,本文所提算法就能夠估計出磁力計誤差參數(shù),與空間分布為完整3周時校正精度接近。但如果單獨繞一軸旋轉(zhuǎn),即采樣點空間分布為平面1周時,由于粒子的多樣性過低,無法估計出磁力計12個誤差參數(shù),尤其是對偏置矩陣無法估計。因此,本文所提算法只需要磁力計在空間中繞X、Y、Z中的任意2個軸不完整旋轉(zhuǎn)1周即可實現(xiàn)對誤差參數(shù)的估計,相對于傳統(tǒng)算法大大簡化了操作的復(fù)雜度。
圖2~圖4所示為磁力計分別旋轉(zhuǎn)完整3周、完整2周及不完整2周時,磁力計校正前后的采樣點分布圖。理想情況下,采樣點應(yīng)該分布在以地磁場強度為半徑的球上,但由于各種誤差的干擾使其偏離。從圖中可以看出,校正前的采樣點都在球面外側(cè),經(jīng)過本文算法校正使其重新分布在球面,說明本算法能夠?qū)Υ帕τ嬚`差進行有效補償。
為了測試本文所提算法在磁場動態(tài)環(huán)境中的性能,做了以下的仿真實驗。模擬所在磁場環(huán)境是變化的,即不再設(shè)定磁場強度為一個定值,仿真中將其設(shè)置為線性變化的,得到的實驗結(jié)果如表4所示。
表4 動態(tài)性能測試結(jié)果
Fg是當(dāng)粒子為最優(yōu)位置時,適應(yīng)函數(shù)的值,即通過式(8)得到的值,可以表征算法的精度。因此,可以看出,在磁場變化時,本文所提算法比傳統(tǒng)算法的精度高,說明本文所提算法具有更高的動態(tài)適應(yīng)能力。
為了驗證不同陀螺儀精度對算法精度的影響,本文針對幾種常見的陀螺儀精度做了仿真實驗,得到的結(jié)果如表5所示。
表5 陀螺儀精度影響測試結(jié)果
從表5可以看出,陀螺儀的精度會對本文算法造成影響,但是影響較小。
為了驗證該算法在實際中的有效性,選用以ADIS16488為核心的MEMS-IMU系統(tǒng)作為實驗設(shè)備,實物如圖5所示,它的參數(shù)如表6所示。用ADIS16488 MEMS-IMU系統(tǒng)采集數(shù)據(jù),同時為了降低陀螺儀誤差對算法精度造成的影響,取陀螺儀靜態(tài)數(shù)據(jù)的平均值作為零漂減去,再采用本文所提算法、PSO算法和擴展卡爾曼濾波(Extended Kalman Filter,EKF)算法對磁力計校正,得到的參數(shù)估計結(jié)果如表7所示,利用本文算法得到的磁力計校正前后數(shù)據(jù)的對比如圖6所示。從表中可以看出,本文所提算法精度最高,說明本算法在實際系統(tǒng)中能夠?qū)崿F(xiàn)磁力計的校正。
表6 ADIS16488參數(shù)
表7 不同算法參數(shù)估計結(jié)果
1)解決了傳統(tǒng)粒子群優(yōu)化算法用于磁力計校正時需要簡化誤差模型,同時粒子容易陷入局部最優(yōu)的問題;
2)磁力計的誤差參數(shù)利用陀螺儀提供的角速率信息,建立補償前后的磁場矢量適應(yīng)函數(shù),再迭代尋找粒子的全局最優(yōu)值求解;
3)仿真與實測實驗表明,只要將磁力計繞其三軸中的任意兩軸旋轉(zhuǎn)不完整一周即可估計磁力計的12個誤差參數(shù),相對于傳統(tǒng)算法操作簡單、動態(tài)性好;
4)僅針對陀螺儀零位漂移對算法精度產(chǎn)生的影響進行了仿真實驗,并沒有對陀螺儀各種誤差進行詳細(xì)的分析;
5)本文算法僅能進行離線估計,需進一步研究在線估計磁力計誤差參數(shù)及陀螺儀誤差,提高算法的工程實用性。