微小型無人機(jī)導(dǎo)航用三軸磁強(qiáng)計(jì)現(xiàn)場誤差校正方法

微小型無人機(jī)導(dǎo)航用三軸磁強(qiáng)計(jì)現(xiàn)場誤差校正方法

磁強(qiáng)計(jì)是測量磁場強(qiáng)度的傳感器，廣泛應(yīng)用于微小型無人機(jī)（aev）的方向跟蹤。在微小型無人機(jī)領(lǐng)域,一般使用捷聯(lián)于飛行器的三軸磁強(qiáng)計(jì)組合測量映射于飛行器本體坐標(biāo)系的磁場矢量來推算磁航向角。由于傳感器制造原因和外部磁場干擾,磁強(qiáng)計(jì)的輸出有誤差,需要進(jìn)行校正以提高測量精度。磁強(qiáng)計(jì)的測量受傳感器零位偏差、標(biāo)度因子誤差、非正交誤差、測量噪聲、外部磁場干擾誤差和非理想安裝誤差等多種誤差因素的影響。校正磁強(qiáng)計(jì)或磁羅盤的傳統(tǒng)方法是搖擺算法(SwingingAlgorithm),即將載體在水平面內(nèi)轉(zhuǎn)動(dòng)至一系列已知的方位,利用角度誤差獲得磁航向域的誤差模型,但搖擺算法適應(yīng)于平面羅盤,校正過程中需要載體保持水平,且需依賴外部航向參考,因此在應(yīng)用時(shí)有很多限制。一種工程中常用的三軸磁強(qiáng)計(jì)校正算法是極大極小值法,但其僅能估計(jì)磁強(qiáng)計(jì)的零位偏差和標(biāo)度因子誤差,誤差校正不充分。Gebre-Egziabher等提出了一種不需要外部參考的兩步估計(jì)算法,基于無誤差測量的三軸磁強(qiáng)計(jì)的輸出軌跡滿足橢球面約束的原理,在磁場域校正三軸磁強(qiáng)計(jì)的零位偏差、硬磁干擾誤差和標(biāo)度因子誤差,他們隨后又分別設(shè)計(jì)了基于迭代估計(jì)算法和批處理最小二乘估計(jì)算法的磁場自校正算法。Foster等將兩步估計(jì)算法推廣,增加了對(duì)非正交誤差的校正。Vasconcelos等指出磁強(qiáng)計(jì)的測量誤差估計(jì)問題可轉(zhuǎn)換為對(duì)橢球面旋轉(zhuǎn)、尺度變換和平移變換的估計(jì)問題,并設(shè)計(jì)了基于極大似然估計(jì)算法的磁場自校正算法。文獻(xiàn)中也給出了基于橢球約束的磁場自校正算法。但基于橢球約束的磁場自校正算法無法校正非對(duì)準(zhǔn)誤差,需要加以完備。飛行器任務(wù)載荷和飛行環(huán)境的改變往往會(huì)導(dǎo)致三軸磁強(qiáng)計(jì)外部磁場環(huán)境的改變,所以磁強(qiáng)計(jì)的現(xiàn)場校正也很必要。文獻(xiàn)中基于小型飛機(jī)和微小型無人機(jī)飛行時(shí)的姿態(tài)限制(平飛、左盤旋和右盤旋),給出了基于橢圓約束的磁強(qiáng)計(jì)校正算法,但受飛行器盤旋精度影響,實(shí)際應(yīng)用受到限制,且難以完全校正三軸磁強(qiáng)計(jì)。本文首先分析三軸磁強(qiáng)計(jì)的誤差來源,建立三軸磁強(qiáng)計(jì)誤差模型;然后推導(dǎo)基于兩步估計(jì)算法和圓約束非對(duì)準(zhǔn)誤差估計(jì)算法的三軸磁強(qiáng)計(jì)現(xiàn)場誤差校正方法;接著提出適用于微小型無人機(jī)的現(xiàn)場數(shù)據(jù)采樣策略;最后通過仿真和實(shí)驗(yàn)驗(yàn)證三軸磁強(qiáng)計(jì)現(xiàn)場誤差校正方法的有效性和實(shí)用性。1三軸磁強(qiáng)計(jì)誤差分析與建模1.1[hhxh對(duì)策確定磁場矢量三軸磁強(qiáng)計(jì)捷聯(lián)于姿態(tài)不斷變化的微小型無人機(jī)平臺(tái),磁航向的計(jì)算使用經(jīng)過姿態(tài)修正后的磁場測量值,即地磁場矢量在水平面的投影,磁場矢量轉(zhuǎn)換關(guān)系如圖1所示。圖1中:hh=[hhxhhyhhz]T為飛行器水平隨體坐標(biāo)系Ohxhyhzh下的磁場矢量;hb=[hbxhbyhbz]T為飛行器本體坐標(biāo)系Obxbybzb下的磁場矢量。磁航向計(jì)算公式為ψ=-arctanhhyhhx(1)hh=(Rθ)Τ(Rγ)Τhb(2)式中:ψ為磁航向角;γ和θ分別為滾轉(zhuǎn)角和俯仰角,Rγ和Rθ為對(duì)應(yīng)的旋轉(zhuǎn)矩陣。由式(1)和式(2)可見,若要獲得磁航向,需要獲得飛行器本體坐標(biāo)系下的磁場矢量。三軸磁強(qiáng)計(jì)校正的目的就是辨識(shí)干擾誤差參數(shù),由三軸磁場測量值確定準(zhǔn)確的飛行器本體坐標(biāo)系磁場矢量。1.2非準(zhǔn)誤差分析磁強(qiáng)計(jì)的測量誤差包括:傳感器零位偏差、標(biāo)度因子誤差、非正交誤差、測量噪聲、硬磁干擾誤差、軟磁干擾誤差以及由非理想安裝引起的傳感器坐標(biāo)系Osxsyszs和飛行器本體坐標(biāo)系之間的非對(duì)準(zhǔn)誤差,下面分別加以分析。受制造工藝限制,各個(gè)傳感器特性不一致,不同的三軸磁強(qiáng)計(jì)具有不同的零位偏差和不同的標(biāo)度因子。標(biāo)度因子誤差矩陣可表示為Csf=[1+Sx0001+Sy0001+Sz](3)式中:Sx、Sy和Sz為三軸磁強(qiáng)計(jì)的標(biāo)度因子誤差。受熱特性和電路測量因素的影響,磁強(qiáng)計(jì)輸出不可避免地具有測量噪聲,可建模為白噪聲。三軸磁強(qiáng)計(jì)的敏感軸不能完全正交,從而導(dǎo)致非正交測量誤差,誤差矩陣為Cno=[100sinρcosρ0sin?cosλsinλcos?cosλ](4)式中:ρ為實(shí)際ys軸和理想ys軸的夾角;?為實(shí)際zs軸和理想zs軸的夾角在xsOszs平面的投影;λ為實(shí)際zs軸和理想zs軸的夾角在ysOszs平面的投影。由于飛行器的結(jié)構(gòu)件或電子設(shè)備中包含鐵磁性材料,所以三軸磁強(qiáng)計(jì)的測量會(huì)受到地球磁場外的其他磁場干擾。如果干擾磁場為永久時(shí)不變磁場,則稱為硬磁干擾。硬磁干擾在磁強(qiáng)計(jì)的輸出中表現(xiàn)為固定零位偏移。如果干擾磁場是隨飛行器姿態(tài)變化的,則為軟磁干擾。軟磁干擾在三軸捷聯(lián)磁強(qiáng)計(jì)的輸出中表現(xiàn)為變化的零位偏差,軟磁干擾誤差矩陣為Csi=[1+αxxαxyαxzαyx1+αyyαyzαzxαzy1+αzz](5)式中:αij(i,j=x,y,z)為由j軸外部磁場誘導(dǎo)產(chǎn)生的磁場在i軸上的投影。軟磁干擾誤差可等效為標(biāo)度因子誤差、非正交誤差和非對(duì)準(zhǔn)誤差的綜合效應(yīng)。實(shí)際安裝情況下,由于三軸磁強(qiáng)計(jì)確立的傳感器坐標(biāo)系與飛行器本體坐標(biāo)系不能完全對(duì)準(zhǔn),因此相對(duì)于傳感器坐標(biāo)系的磁場強(qiáng)度測量值和相對(duì)于飛行器本體坐標(biāo)系的磁場強(qiáng)度測量值不同,此誤差即為非對(duì)準(zhǔn)誤差。非對(duì)準(zhǔn)誤差矩陣可表示為Cm=[cycz-cxsz+sxsyczsxsz+cxsyczcyszcxcz+sxsysz-sxcz+cxsysz-sysxcycxcy](6)式中:cx=cosηx,cy=cosηy,cz=cosηz,sx=sinηx,sy=sinηy,sz=sinηz,其中ηx、ηy和ηz分別為傳感器坐標(biāo)系相對(duì)于飛行器本體坐標(biāo)系繞xb軸、yb軸和zb軸的非對(duì)準(zhǔn)角。若非對(duì)準(zhǔn)角很小,則式(6)可以近似為Cm=[1-ηzηyηz1-ηx-ηyηx1](7)綜合上述各誤差分析,按照來源不同可將三軸磁強(qiáng)計(jì)的誤差分為3部分:傳感器本身誤差(零偏、標(biāo)度因子和測量噪聲),外部磁場干擾誤差和由非理想安裝引起的非對(duì)準(zhǔn)誤差。1.3等效非準(zhǔn)誤差測量綜合各誤差因素,三軸磁強(qiáng)計(jì)的測量模型可以表述為?hs=CsfCnoCsi(C-1mhb+bshi)+bs0+n(8)式中:上標(biāo)s表示傳感器坐標(biāo)系;?hs為傳感器測量的磁場矢量;bshi為硬磁干擾引起的零位誤差;bs0為三軸磁強(qiáng)計(jì)零位偏差;n為傳感器測量噪聲。令T=CsfCnoCsi,b=Tbshi+bs0=[bxbybz]T,則可將式(8)重寫為?hs=ΤC-1mhb+b+n(9)對(duì)T的逆矩陣進(jìn)行極分解,得T-1=PQ,其中P為正交陣,Q為對(duì)稱陣,則可將式(9)重寫為hcs=Cmthb=Q(?hs-b)-Qn(10)式中:hcs=[hcsxhcsyhcsz]T為傳感器坐標(biāo)系下經(jīng)過校正的磁場矢量;Cmt=PTC-1m為總的等效非對(duì)準(zhǔn)誤差矩陣,其中正交陣P為由軟磁干擾等引起的等效非對(duì)準(zhǔn)誤差。三軸磁強(qiáng)計(jì)校正的目的是利用磁場測量矢量獲取準(zhǔn)確的飛行器本體坐標(biāo)系磁場矢量,因此可將誤差模型式(8)轉(zhuǎn)化為三軸磁強(qiáng)計(jì)等效誤差模型式(10)求解,相應(yīng)地,三軸磁強(qiáng)計(jì)的校正問題由逐個(gè)求解各誤差系數(shù)轉(zhuǎn)換為求解等效非對(duì)準(zhǔn)誤差矩陣Cmt、等效轉(zhuǎn)換矩陣Q和等效零位誤差b,減少了估計(jì)參數(shù)數(shù)量,降低了校正復(fù)雜度。2三軸磁強(qiáng)計(jì)的現(xiàn)場誤差校正方法設(shè)計(jì)2.1標(biāo)量檢測算法在同一地理位置,地球磁場強(qiáng)度值是一個(gè)常量,即在飛行器本體坐標(biāo)系測得的磁場強(qiáng)度值和當(dāng)?shù)氐乩碜鴺?biāo)系下的磁場強(qiáng)度值相等,可表述為(Cmthb)Τ(Cmthb)=[Q(?hs-b)]Τ[Q(?hs-b)]=h2(11)式中:h為當(dāng)?shù)氐乩碜鴺?biāo)系下的磁場強(qiáng)度值,可由地球磁場模型或地磁圖獲取。由于磁航向?yàn)榇艌鰷y量值的比值,與磁場強(qiáng)度值無關(guān),所以本文取h為無量綱的相對(duì)值單位1,本文以下部分磁場值均為無量綱的相對(duì)值。由式(11)可以看出:理想磁場測量值的軌跡在一個(gè)球面上,而受到各種誤差影響的磁場測量值在一個(gè)橢球面上。通過擬合橢球面可獲取三軸磁強(qiáng)計(jì)的誤差校正參數(shù),此算法稱為標(biāo)量檢測算法(ScalarCheckingAlgorithm,SCA)或橢球面約束算法。使用該算法,可以推算出估計(jì)矩陣Q和向量b的兩步估計(jì)算法。將矩陣Q參數(shù)化,得Q=[q11q12q13q12q22q23q13q23q33](12)取常數(shù)M1～M10為中間變量:{Μ1=q211+q212+q313Μ2=q212+q222+q223Μ3=q313+q223+q233Μ4=2(q11q12+q12q22+q13q23)Μ5=2(q12q13+q22q23+q23q33)Μ6=2(q11q13+q12q23+q13q33)Μ7=-2Μ1bx-Μ4by-Μ6bzΜ8=-2Μ2by-Μ4bx-Μ5bzΜ9=-2Μ3bz-Μ5by-Μ6bzΜ10=Μ1(bx)2+Μ2(by)2+Μ3(bz)2+Μ4(bxby)+Μ5(bybz)+Μ6(bxbz)-h2于是,式(11)可以變換為如下線性方程:(?hsz)2=[(?hsx)2(?hsy)2?hsx?hsy?hsy?hsz?hsz?hsx?hsx?hsy?hsz1]Τ[-Μ1/Μ3-Μ2/Μ3-Μ4/Μ3-Μ5/Μ3-Μ6/Μ3-Μ7/Μ3-Μ8/Μ3-Μ9/Μ3-Μ10/Μ3](13)由于M1～M10為常數(shù),可使用多次磁場測量值將式(13)增廣為線性方程組形式,使用最小二乘法,可以估計(jì)參數(shù)M1～M10;之后利用中間變量表達(dá)式可求解Q和b。2.2非準(zhǔn)誤差系數(shù)的估計(jì)由式(11)可以看出:基于橢球面約束的磁場自校正算法不能估計(jì)等效非對(duì)準(zhǔn)誤差矩陣Cmt?；诖?本文提出了基于圓約束的非對(duì)準(zhǔn)誤差估計(jì)算法。當(dāng)飛行器載體繞天向軸水平旋轉(zhuǎn)一周后,所得的磁場測量軌跡校正后是一個(gè)在水平面上的圓,若存在非對(duì)準(zhǔn)誤差,則得到的磁場測量軌跡為一個(gè)不在水平面上的圓,如式(14)和式(15)所示。(hbx)2+(hby)2=1-(hbz)2(14)(-syhcsx+sxcxhcsy+cxcyhcsz)2=(hbz)2(15)通過估計(jì)圓的坐標(biāo)旋轉(zhuǎn)矩陣即可獲得非對(duì)準(zhǔn)誤差系數(shù)ηx和ηy,具體為,將式(15)進(jìn)行差分運(yùn)算,整理化簡,可得如下線性方程:-sy˙hcsx+sxcx˙hcsy+cxcy˙hcsz=0(16)將磁場測量值?hs轉(zhuǎn)換為hcs,用多個(gè)hcs值將式(16)增廣為線性方程組,使用最小二乘法估計(jì)即可得非對(duì)準(zhǔn)誤差系數(shù)ηx和ηy。通過運(yùn)算實(shí)際航向(可以使用全球定位系統(tǒng)(GPS)航向)和解算航向之間的差值,可得非對(duì)準(zhǔn)角ηz(包含磁偏角)為ηz=ψref-ψmea(17)式中:ψref為航向角參考值;ψmea為航向角測量值?，F(xiàn)場校正時(shí),飛行器不必嚴(yán)格保持水平旋轉(zhuǎn),可以使用加速度計(jì)獲得的傾角值對(duì)采樣數(shù)據(jù)進(jìn)行修正獲得水平旋轉(zhuǎn)采樣值。2.3數(shù)據(jù)集的參數(shù)估計(jì)和旋轉(zhuǎn)操作使用三軸磁強(qiáng)計(jì)現(xiàn)場誤差校正方法進(jìn)行三軸磁強(qiáng)計(jì)誤差系數(shù)估計(jì)時(shí),需要依據(jù)合適的采樣數(shù)據(jù),采樣原則是盡可能大范圍地覆蓋球面或橢球面。由于地磁矢量存在偏轉(zhuǎn)和傾斜,所以飛行器的轉(zhuǎn)動(dòng)或飛行軌跡需要進(jìn)行規(guī)劃,以便于采集有效合理的數(shù)據(jù)集進(jìn)行參數(shù)估計(jì)。同時(shí)要考慮可操作性,采用盡可能少的旋轉(zhuǎn)操作。本文考慮地磁矢量的偏轉(zhuǎn)和傾斜特性,提出了適合微小型無人機(jī)的現(xiàn)場數(shù)據(jù)采樣策略(見圖2):將安裝有三軸磁強(qiáng)計(jì)的微小型無人機(jī)分別繞東向、北向和天向軸旋轉(zhuǎn)一周。以北京地區(qū)為例,所得的理想磁場采樣軌跡如圖3所示,可見采樣點(diǎn)軌跡覆蓋了球面的主要特征。依據(jù)這種現(xiàn)場數(shù)據(jù)采樣策略,能夠在較少的旋轉(zhuǎn)操作下獲得較好的球面覆蓋度。本文方法應(yīng)用對(duì)象為微小型無人機(jī),其體積和質(zhì)量小,可以由現(xiàn)場人員手動(dòng)實(shí)現(xiàn)整周旋轉(zhuǎn)操作。算法不限定磁場采樣數(shù)據(jù)點(diǎn)位于理想采樣軌跡上,考慮人為操作中的傾斜和抖動(dòng),對(duì)整周旋轉(zhuǎn)操作的要求是:沿一個(gè)軸旋轉(zhuǎn)時(shí),保持其他兩軸的傾斜角范圍小于±30°。此方法現(xiàn)場操作中具備可行性。3模擬算法和實(shí)驗(yàn)首先使用仿真手段分析所提出的微小型無人機(jī)三軸磁強(qiáng)計(jì)現(xiàn)場誤差校正方法的性能,然后基于三軸磁強(qiáng)計(jì)的實(shí)測數(shù)據(jù)進(jìn)行分析,驗(yàn)證算法的實(shí)用性。3.1最大德國最大確定磁場誤差校正仿真參數(shù)設(shè)定如表1和表2所示,包含三軸磁強(qiáng)計(jì)所有誤差。因磁航向角的計(jì)算和地磁場強(qiáng)度絕對(duì)值無關(guān),故設(shè)定地磁場強(qiáng)度值為1.0000,磁強(qiáng)計(jì)測量噪聲強(qiáng)度值為0.0100;磁傾角和磁偏角取北京地區(qū)值,分別為55.0°和-6.0°。圖4為某次磁場軌跡仿真結(jié)果:上部黑色軌跡為使用現(xiàn)場校正策略得到的磁場測量軌跡,考慮人為操作的傾斜和抖動(dòng),沿各軸旋轉(zhuǎn)時(shí)傾斜角取標(biāo)準(zhǔn)差為30°的隨機(jī)值;下部灰色為校正后的磁場測量軌跡,依據(jù)現(xiàn)場數(shù)據(jù)采樣策略獲得的測量數(shù)據(jù)校正后可以很好地?cái)M合出球面。校正前磁場強(qiáng)度值的均值為2.5137,標(biāo)準(zhǔn)差為0.5740,而校正后磁場強(qiáng)度值的均值為0.9999,標(biāo)準(zhǔn)差為0.0108。矩陣Q和向量b的系數(shù)估計(jì)誤差小于4%。圖5為校正前后航向角誤差結(jié)果對(duì)比:校正前航向角誤差為100.6°(1σ),使用三軸磁強(qiáng)計(jì)現(xiàn)場誤差校正方法校正后的航向角誤差減為1.13°(1σ),小于1.2°,使用極大極小值法校正后的航向角誤差為14.77°(1σ)。對(duì)比可見,三軸磁強(qiáng)計(jì)現(xiàn)場誤差校正方法校正后的航向角誤差僅相當(dāng)于極大極小值法校正后的航向角誤差的8%,精度提升了92%,因此三軸磁強(qiáng)計(jì)現(xiàn)場誤差校正方法能夠更有效地校正磁場誤差。表3給出了使用基于圓約束的非對(duì)準(zhǔn)誤差估計(jì)算法得到的三軸磁強(qiáng)計(jì)的等效非對(duì)準(zhǔn)角的估計(jì)值,與表1對(duì)比可見:軟磁干擾極大增加了三軸磁強(qiáng)計(jì)的非對(duì)準(zhǔn)誤差,因此三軸磁強(qiáng)計(jì)安裝時(shí)要盡量避免軟磁干擾。3.2磁場強(qiáng)度波動(dòng)使用三軸磁強(qiáng)計(jì)的實(shí)測數(shù)據(jù)和兩軸轉(zhuǎn)臺(tái)實(shí)驗(yàn)裝置(見圖6),依據(jù)現(xiàn)場數(shù)據(jù)采樣策略獲取磁場測量值,對(duì)所提出的三軸磁強(qiáng)計(jì)現(xiàn)場誤差校正方法進(jìn)行了分析。圖7為校正前后磁場強(qiáng)度對(duì)比,校正前磁場強(qiáng)度波動(dòng)較大,校正后磁場強(qiáng)度波動(dòng)減小,且使用三軸磁強(qiáng)計(jì)現(xiàn)場誤差校正方法校正后的磁場強(qiáng)度值的波動(dòng)明顯小于使用極大極小值法后的。航向角誤差結(jié)果對(duì)比、航向角誤差統(tǒng)計(jì)值分別如圖8和表4所示。校正前航