原雨佳，王 偉，陳興邦

(哈爾濱工程大學(xué)自動(dòng)化學(xué)院，哈爾濱 150001)

0 引言

磁力計(jì)是一種測(cè)量磁場(chǎng)強(qiáng)度的儀器，通過(guò)對(duì)其三軸分量的計(jì)算能夠得到導(dǎo)航中重要的航向角信息，具有無(wú)累計(jì)誤差的優(yōu)點(diǎn)。然而磁力計(jì)有不可忽視的零位、靈敏度、非正交誤差，以及周圍環(huán)境影響造成的軟硬磁誤差，這些都會(huì)造成其輸出不準(zhǔn)確，從而影響解算航向角的精度。因此，在使用前需要對(duì)其進(jìn)行誤差分析，并采用有效的算法進(jìn)行補(bǔ)償校正，提高其實(shí)用性。

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

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

1 磁力計(jì)誤差模型

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

(1)

其中：

S=SscSnoSsi

b=bm+bhi

(2)

(3)

2 基于陀螺儀輔助的隨機(jī)漂移粒子群優(yōu)化算法

粒子群算法是一種仿效鳥(niǎo)類覓食過(guò)程的優(yōu)化算法，通過(guò)迭代不斷提高與目標(biāo)函數(shù)有關(guān)的種群的候選解，即通過(guò)位置與速度更新公式尋找適應(yīng)目標(biāo)函數(shù)的粒子，每個(gè)粒子的移動(dòng)不僅受自身經(jīng)驗(yàn)的影響，也受當(dāng)前種群中最優(yōu)位置的影響。與其他算法相比，粒子群算法需要設(shè)置和調(diào)整的參數(shù)較少，而且收斂速度快，近些年發(fā)展迅猛。

2.1 目標(biāo)函數(shù)

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

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

(4)

速度向量可以表示為：

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

(5)

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

(6)

其中：

(7)

從誤差模型上可以看出，各種誤差對(duì)各軸向產(chǎn)生的影響是不同的，在有陀螺儀輔助的情況下，通過(guò)陀螺儀解算姿態(tài)角信息，在已知地磁場(chǎng)的情況下得到理想載體系下的磁場(chǎng)矢量，此磁場(chǎng)矢量應(yīng)與磁力計(jì)補(bǔ)償后的磁場(chǎng)矢量相同。由此可以建立磁場(chǎng)矢量適應(yīng)函數(shù)，將其寫為下面的數(shù)學(xué)表達(dá)式

F2,i=argminf2(S,b)

(8)

2.2 隨機(jī)漂移粒子群優(yōu)化算法

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

該算法的具體流程為：

1)粒子群初始化

初始化粒子個(gè)數(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分別是每個(gè)粒子第j維對(duì)應(yīng)的位置最大值和最小值，對(duì)于磁力計(jì)校正，前9維對(duì)應(yīng)的位置限定范圍要小于后3維偏置對(duì)應(yīng)的位置范圍，這樣設(shè)置可以為濾波算法提供更實(shí)用的初值。

在初始化階段，粒子的個(gè)體最優(yōu)Pbesti(k=0)≡Pi(k=0)，通過(guò)式(8)計(jì)算每個(gè)粒子的適應(yīng)值，選擇最優(yōu)的位置為種群最優(yōu)值Gbest。

2)粒子群更新

與傳統(tǒng)PSO算法速度更新方式不同，RDPSO算法中粒子運(yùn)動(dòng)包括兩部分，即定向漂移運(yùn)動(dòng)和無(wú)規(guī)則隨機(jī)熱運(yùn)動(dòng)，數(shù)學(xué)表達(dá)式為：

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

(10)

式中，k是迭代次數(shù)，i(i=1,2,3,…,m)表示粒子群中第i個(gè)粒子，j(j=1,2,3,…,D)表示粒子的第j維，Vrij(k+1)表示無(wú)規(guī)則隨機(jī)熱運(yùn)動(dòng)，Vdij(k+1)表示定向漂移運(yùn)動(dòng)。

熱運(yùn)動(dòng)可以表示成：

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

(11)

其中：

(12)

(13)

(14)

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

粒子的定向漂移運(yùn)動(dòng)可以定義為：

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

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

(15)

式中，c1和c2分別是自我認(rèn)知和社會(huì)認(rèn)知,分別代表粒子自身位置最優(yōu)和種群位置最優(yōu)的影響；rand(0,1)是(0,1)之間的隨機(jī)數(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)

按照上式對(duì)粒子的速度和位置進(jìn)行更新，同時(shí)在更新過(guò)程中限制速度和位置在所定范圍內(nèi)。

3)搜索全局最優(yōu)

因?yàn)楸疚闹械膯?wèn)題是解決最小值問(wèn)題，因此種群的目標(biāo)就是找到最小的適應(yīng)值，對(duì)于每個(gè)粒子，如果新的適應(yīng)值小于Pbest對(duì)應(yīng)的適應(yīng)值，則更新當(dāng)前位置的Pbest，否則Pbest不變?？梢詫best寫為：

(17)

同樣的，如果新的Pbest對(duì)應(yīng)的適應(yīng)值小于此時(shí)Gbest對(duì)應(yīng)的適應(yīng)值，則更新Gbest，否則Gbest不變。可以將Gbest寫為：

(18)

其中，f是目標(biāo)函數(shù)，即式(8)。

4)迭代

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

由此可見(jiàn)，此算法就是通過(guò)迭代追蹤個(gè)體歷史最優(yōu)值和種群歷史最優(yōu)值，其流程圖如圖1所示。

3 仿真實(shí)驗(yàn)分析

本算法使用的環(huán)境是在哈爾濱市，依據(jù)IGRF12模型[17]計(jì)算當(dāng)?shù)氐牡卮艌?chǎng)矢量為Hi=[242.367;-46.225;-465.459]mG2。為了測(cè)試本算法的性能，分別做了以下幾個(gè)仿真模擬實(shí)驗(yàn)。

3.1 參數(shù)估計(jì)數(shù)量對(duì)比實(shí)驗(yàn)

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

表1 粒子群優(yōu)化算法參數(shù)設(shè)置表

表2 12個(gè)誤差參數(shù)估計(jì)結(jié)果

從表中可以看出，使用傳統(tǒng)算法無(wú)法準(zhǔn)確地估計(jì)出磁力計(jì)誤差模型中的12個(gè)參數(shù)，有些參數(shù)的估計(jì)結(jié)果與參考值相差較大。這是因?yàn)閭鹘y(tǒng)算法只選用約束磁場(chǎng)總量的目標(biāo)函數(shù)，沒(méi)有考慮磁場(chǎng)的三軸分量，造成當(dāng)誤差矩陣S陣為9個(gè)不同參數(shù)時(shí)，粒子群尋找最優(yōu)值受到較大干擾。而本文提出的RDPSO算法卻能得到較為精準(zhǔn)的參數(shù)估計(jì)結(jié)果。

3.2 旋轉(zhuǎn)度測(cè)試實(shí)驗(yàn)

為了簡(jiǎn)化校正中操作的復(fù)雜度，該實(shí)驗(yàn)通過(guò)模擬磁力計(jì)的幾種不同旋轉(zhuǎn)方式，驗(yàn)證算法在各種旋轉(zhuǎn)度下的性能。旋轉(zhuǎn)方式如下：

方式一：模擬磁力計(jì)在繞X、Y、Z軸各旋轉(zhuǎn)1周，即在空間中是3個(gè)完整的圓形；

方式二：模擬磁力計(jì)繞X、Y軸各旋轉(zhuǎn)1周，即在空間中是2個(gè)完整的圓形；

方式三：模擬磁力計(jì)繞X、Y軸各不完整旋轉(zhuǎn)1周，即在空間中是2個(gè)不完整的圓形；

方式四：模擬磁力計(jì)繞Z軸旋轉(zhuǎn)1周，即在空間中是1個(gè)完整的圓形。

針對(duì)四種不同旋轉(zhuǎn)方式獲取的采樣點(diǎn)，采用RDPSO算法進(jìn)行校正，得到的參數(shù)估計(jì)結(jié)果如表3所示。

表3 各種旋轉(zhuǎn)度測(cè)試結(jié)果

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

圖2～圖4所示為磁力計(jì)分別旋轉(zhuǎn)完整3周、完整2周及不完整2周時(shí)，磁力計(jì)校正前后的采樣點(diǎn)分布圖。理想情況下，采樣點(diǎn)應(yīng)該分布在以地磁場(chǎng)強(qiáng)度為半徑的球上，但由于各種誤差的干擾使其偏離。從圖中可以看出，校正前的采樣點(diǎn)都在球面外側(cè)，經(jīng)過(guò)本文算法校正使其重新分布在球面，說(shuō)明本算法能夠?qū)Υ帕τ?jì)誤差進(jìn)行有效補(bǔ)償。

3.3 動(dòng)態(tài)性能測(cè)試實(shí)驗(yàn)

為了測(cè)試本文所提算法在磁場(chǎng)動(dòng)態(tài)環(huán)境中的性能，做了以下的仿真實(shí)驗(yàn)。模擬所在磁場(chǎng)環(huán)境是變化的，即不再設(shè)定磁場(chǎng)強(qiáng)度為一個(gè)定值，仿真中將其設(shè)置為線性變化的，得到的實(shí)驗(yàn)結(jié)果如表4所示。

表4 動(dòng)態(tài)性能測(cè)試結(jié)果

Fg是當(dāng)粒子為最優(yōu)位置時(shí)，適應(yīng)函數(shù)的值，即通過(guò)式(8)得到的值，可以表征算法的精度。因此，可以看出，在磁場(chǎng)變化時(shí)，本文所提算法比傳統(tǒng)算法的精度高，說(shuō)明本文所提算法具有更高的動(dòng)態(tài)適應(yīng)能力。

3.4 陀螺儀精度影響測(cè)試實(shí)驗(yàn)

為了驗(yàn)證不同陀螺儀精度對(duì)算法精度的影響，本文針對(duì)幾種常見(jiàn)的陀螺儀精度做了仿真實(shí)驗(yàn)，得到的結(jié)果如表5所示。

表5 陀螺儀精度影響測(cè)試結(jié)果

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

4 實(shí)測(cè)實(shí)驗(yàn)驗(yàn)證

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

表6 ADIS16488參數(shù)

表7 不同算法參數(shù)估計(jì)結(jié)果

5 結(jié)論

1)解決了傳統(tǒng)粒子群優(yōu)化算法用于磁力計(jì)校正時(shí)需要簡(jiǎn)化誤差模型，同時(shí)粒子容易陷入局部最優(yōu)的問(wèn)題；

2)磁力計(jì)的誤差參數(shù)利用陀螺儀提供的角速率信息，建立補(bǔ)償前后的磁場(chǎng)矢量適應(yīng)函數(shù)，再迭代尋找粒子的全局最優(yōu)值求解；

3)仿真與實(shí)測(cè)實(shí)驗(yàn)表明，只要將磁力計(jì)繞其三軸中的任意兩軸旋轉(zhuǎn)不完整一周即可估計(jì)磁力計(jì)的12個(gè)誤差參數(shù)，相對(duì)于傳統(tǒng)算法操作簡(jiǎn)單、動(dòng)態(tài)性好；

4)僅針對(duì)陀螺儀零位漂移對(duì)算法精度產(chǎn)生的影響進(jìn)行了仿真實(shí)驗(yàn)，并沒(méi)有對(duì)陀螺儀各種誤差進(jìn)行詳細(xì)的分析；

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