王木華，蘇成悅，朱文杰，蔡則鵬，任開眾，陳元電，徐 勝

（廣東工業(yè)大學(xué) 物理與光電工程學(xué)院，廣東 廣州 510006）

隨著多旋翼無人機(jī)的廣泛應(yīng)用，機(jī)載小型慣性導(dǎo)航系統(tǒng)逐漸成為人們的研究熱點. 慣性導(dǎo)航系統(tǒng)主要由三軸陀螺儀、加速度計和磁力計構(gòu)成，是無人機(jī)飛控獲取自身姿態(tài)信息的主要來源和穩(wěn)定飛行的保障[1]，廣泛應(yīng)用于消費(fèi)級、行業(yè)等領(lǐng)域無人機(jī). 磁力計是測量磁場強(qiáng)度和方向，用于定位設(shè)備方位的模塊，也稱磁力儀或高斯計[2]. 磁力計在多旋翼無人機(jī)的慣性導(dǎo)航系統(tǒng)中提供絕對偏航信息，其精度決定了飛控姿態(tài)解算的精度，對無人機(jī)的飛行極其重要[3].地球磁場的強(qiáng)度約為50～60 μT，多旋翼無人機(jī)在飛行中其機(jī)載磁力計的測量精度會受到板載電路電磁特性、電機(jī)和空間環(huán)境磁場的干擾，導(dǎo)致飛控接收到錯誤的航向信息[4].

當(dāng)有外部磁場干擾時，磁力計測量數(shù)據(jù)偏差較大，導(dǎo)致測量精確度下降. 對于磁力計的校準(zhǔn)問題有一些學(xué)者進(jìn)行了研究. 有學(xué)者針對三軸磁力計校正模型系數(shù)之間存在的相關(guān)性，提出的基于遺傳算法三軸磁力計校正系數(shù)求解[5]，很好地解決了磁力計校正系數(shù)之間的相關(guān)性問題，但該方法模型復(fù)雜、計算量大，實際效果不明確;有學(xué)者提出的一種基于最小二乘的磁力計橢球擬合靜態(tài)校準(zhǔn)方法[6]，解決了一類靜態(tài)校準(zhǔn)問題. 有針對其他設(shè)備的磁力計的校準(zhǔn)問題，提出的 “8”字旋轉(zhuǎn)方式的動態(tài)校準(zhǔn)方法[7]，但是該方法對空中的機(jī)載磁力計的校準(zhǔn)難以實現(xiàn). 在已有的研究中，校準(zhǔn)主要集中在靜態(tài)校準(zhǔn)，板載電路電磁特性干擾可以通過該方法進(jìn)行修正校準(zhǔn)[8]，靜態(tài)校準(zhǔn)之后的磁力計數(shù)據(jù)會隨時間積累產(chǎn)生偏移[9]，同時電機(jī)和隨機(jī)出現(xiàn)的環(huán)境磁場也將對磁力計產(chǎn)生干擾.多旋翼無人機(jī)在空中飛行時的磁力計校準(zhǔn)問題存在嚴(yán)重缺陷，因此動態(tài)實時校準(zhǔn)的研究對多旋翼無人機(jī)安全穩(wěn)定飛行具有重要意義.

本文針對上述問題，在多旋翼無人機(jī)靜態(tài)校準(zhǔn)的基礎(chǔ)上提出一種自適應(yīng)動態(tài)校準(zhǔn)的方法. 該方法能有效解決多旋翼無人機(jī)磁力計的動態(tài)實時校準(zhǔn)問題，使多旋翼無人機(jī)在空中飛行時能根據(jù)磁場環(huán)境的變化，實時校準(zhǔn)磁力計，更新無人機(jī)航向信息.

1 小角度最小二乘迭代球心的動態(tài)校準(zhǔn)

多旋翼無人機(jī)在空中飛行時，沒有外部磁場干擾的理想情況下，機(jī)載的磁力計只能感應(yīng)到一個方向恒定指北的地球磁場[10]，將機(jī)載磁力計繞三維空間的各個方向旋轉(zhuǎn)360°，地球磁場將均勻分布在X、Y、Z軸上，磁力計的輸出可以擬合成一個球心在坐標(biāo)原點的標(biāo)準(zhǔn)圓球[11]. 由前文對載體周圍磁場分析可得磁力計的數(shù)學(xué)模型:

由此可知，系統(tǒng)誤差對磁力計輸出的影響在解析幾何上表現(xiàn)為使測量數(shù)據(jù)擬合結(jié)果由球體畸變成橢球，同時磁場干擾的影響，會使輸出的坐標(biāo)點發(fā)生偏移，擬合出的橢球中心點也對應(yīng)偏移[13]，偏移量大小代表了磁力計因周圍磁場干擾產(chǎn)生的誤差大小.

最小二乘擬合橢球的具體實現(xiàn)過程為:首先定義磁力計測量的值為，擬合后的橢球中心為，半徑為r，則可得磁力計誤差為

令

根據(jù)最小二乘，將誤差的平方和對所有未知量求偏導(dǎo)，并令偏導(dǎo)為0，即

求解可得擬合后橢球中心mr，即磁力計的零偏，將測量數(shù)據(jù)減去該零偏，即可完成磁力計的校準(zhǔn)[14].

根據(jù)多旋翼無人機(jī)的飛行原理，其在空中穩(wěn)定飛行時，機(jī)體會發(fā)生偏轉(zhuǎn). 如圖1所示，無人機(jī)改變航向自旋時，機(jī)體坐標(biāo)系相對于地球坐標(biāo)系繞Z軸旋轉(zhuǎn)ψ角；向左或右飛行，繞X軸旋轉(zhuǎn)θ角；向前或后飛行時，繞Y軸旋轉(zhuǎn)φ角. 區(qū)別于靜態(tài)校準(zhǔn)，無人機(jī)在空中穩(wěn)定飛行過程中不可能實現(xiàn)空間各角度360°旋轉(zhuǎn)，此動態(tài)校準(zhǔn)過程中機(jī)體坐標(biāo)系均繞X、Y、Z軸完成小角度的偏轉(zhuǎn). 利用無人機(jī)穩(wěn)定飛行過程中的小角度偏轉(zhuǎn)，磁力計實時生成磁場數(shù)據(jù)點.

圖1 小角度旋轉(zhuǎn)示意圖Fig.1 The diagram of small angle rotation

由于動態(tài)校準(zhǔn)的磁場環(huán)境更加復(fù)雜，此方法對磁力計數(shù)據(jù)點的選取比靜態(tài)校準(zhǔn)更加嚴(yán)格，每兩個數(shù)據(jù)點需大于10 μT，以此屏蔽掉合理數(shù)據(jù)點周圍的大量噪聲點[15]. 如圖2所示，將數(shù)據(jù)點映射到平面來分析，選取n個合理的數(shù)據(jù)點C1到Cn，利用最小二乘將其擬合為一段弧，求取圓心. 將此平面擬合過程演繹到無人機(jī)機(jī)體坐標(biāo)系中，則可得到擬合的部分球面，根據(jù)部分球面解算出球心，將下一時刻選取的n個磁力計合理數(shù)據(jù)點減去該球心坐標(biāo)，得到新的數(shù)據(jù)點，即完成一次校準(zhǔn). 利用新的數(shù)據(jù)點進(jìn)行最小二乘擬合，求得球心坐標(biāo)，即可進(jìn)行下一次校準(zhǔn).

此動態(tài)校準(zhǔn)方法采用迭代的思想，每完成一次誤差校準(zhǔn)，數(shù)據(jù)點均進(jìn)行一次更新，再取合理的數(shù)據(jù)點進(jìn)行第2次誤差校準(zhǔn)，從而實現(xiàn)磁力計的實時動態(tài)校準(zhǔn).

圖2 最小二乘擬合示意圖Fig.2 The diagram of least squares fitting

2 仿真驗證

本文利用matlab仿真進(jìn)行驗證. 分別設(shè)置半徑為20的球體在三維坐標(biāo)的零偏，并隨機(jī)生成15 000個數(shù)據(jù)點. 對這些數(shù)據(jù)進(jìn)行加噪，噪聲幅度為30%，利用該方法進(jìn)行動態(tài)校準(zhǔn)，如表1所示，隨機(jī)預(yù)設(shè)置了3種零偏值，在未利用該方法進(jìn)行動態(tài)校準(zhǔn)時，matlab結(jié)算出的零偏與預(yù)設(shè)置零偏差距較大，與之對應(yīng)采用該方法動態(tài)校準(zhǔn)后，結(jié)算零偏和預(yù)設(shè)置零偏基本對應(yīng)，且誤差較小，可信度高. 圖3為其中一組數(shù)據(jù)點追蹤零偏的情況，從圖中可看出數(shù)據(jù)點在3個軸向均能快速響應(yīng)，準(zhǔn)確追蹤到對應(yīng)零偏. 因此在仿真環(huán)境下，該方法能準(zhǔn)確有效解算出磁力計零偏，從而實現(xiàn)磁力計動態(tài)校準(zhǔn).

表1 三軸零偏及誤差Tab.1 The zero offset and error of three axis

圖3 三軸估計零偏Fig.3 The estimated zero offset of three axis

3 實驗驗證

本實驗采用的是以STM32F4系列芯片為主控的無人機(jī)飛控平臺，磁力計采用的是LSM303DTR，包含一個三軸磁力計和一個三軸加速計，采用數(shù)字接口. 磁力計的測量范圍從130 μT到810 μT共分7檔，用戶可以自由選擇. 并且在2000 μT以內(nèi)的磁場環(huán)境下都能夠保持一致的測量效果和相同的靈敏度[16]. 它的分辨率可以達(dá)到0.8 μT，并且內(nèi)部采用12位ADC，以保證對磁場強(qiáng)度的精確測量. 與采用霍爾效應(yīng)原理的磁力計相比，LSM303DTR的功耗低，精度高，線性好，并且不需要穩(wěn)定補(bǔ)償.

本文選取空曠實驗場地，周圍磁場環(huán)境較為純凈. 采用傳統(tǒng)的USB數(shù)據(jù)線將無人機(jī)飛控與電腦端地面站連接，地面站采集飛控收集的磁力計數(shù)據(jù)，采樣頻率為10 Hz，并將數(shù)據(jù)保存于電腦，為避免電腦磁場干擾，數(shù)據(jù)線長度為2 m以上[17]. 將無人機(jī)飛控在空間進(jìn)行小角度傾轉(zhuǎn)，磁力計的數(shù)據(jù)通過飛控傳輸?shù)降孛嬲?，收集?0 897個數(shù)據(jù)點，利用matlab將所有數(shù)據(jù)點連接起來，其在三維坐標(biāo)中的分布如圖4所示.

圖4 磁力計輸出數(shù)據(jù)分布Fig.4 The data distribution of magnetometer output

由上文分析知小角度最小二乘擬合迭代算法對合理數(shù)據(jù)點的選取要求嚴(yán)格，將合理數(shù)據(jù)點擬合可得橢球體，不滿足要求的噪點則雜亂無章地分布在橢球體表明附近，如圖5所示. 利用小角度最小二乘擬合迭代的動態(tài)校準(zhǔn)后可得出球心坐標(biāo)為(9.89，959.91， 940.26)，即為三軸零偏，符合理論分析及仿真結(jié)果. 如圖6所示，在多旋翼無人機(jī)機(jī)載設(shè)備及周圍磁場的影響下，實際測量磁場模值波動較大，主要表現(xiàn)在2 700個數(shù)據(jù)點之前，隨著數(shù)據(jù)點的增加，迭代的作用逐漸顯現(xiàn)，動態(tài)校準(zhǔn)后的模制能很好地跟隨靜態(tài)校準(zhǔn)的結(jié)果，并趨于穩(wěn)定，并且在3 000數(shù)據(jù)點附近. 本文將飛控移入另一個環(huán)境，此時動態(tài)校準(zhǔn)仍能很好抵抗噪聲干擾，模值保持較低值且未出現(xiàn)波動.因此多旋翼無人機(jī)在空中飛行無法進(jìn)行三維空間任意旋轉(zhuǎn)校準(zhǔn)的情況下，可利用本文提出的小角度最小二乘迭代法實現(xiàn)多旋翼無人機(jī)動態(tài)實時校準(zhǔn).

圖6 校準(zhǔn)前后磁場模值Fig.6 The variation of magnetic field modulus before and after calibration

4 結(jié)語

本文在多旋翼無人機(jī)靜態(tài)校準(zhǔn)的基礎(chǔ)上，提出了一種小角度最小二乘迭代的動態(tài)校準(zhǔn)方法. 根據(jù)仿真結(jié)果，該動態(tài)校準(zhǔn)方法在3個角度能較好地跟蹤預(yù)設(shè)置的零偏誤差，且誤差很小. 從實驗結(jié)果可得出該方法在多旋翼無人機(jī)的磁力計動態(tài)校準(zhǔn)過程中能達(dá)到靜態(tài)校準(zhǔn)的效果，動態(tài)校準(zhǔn)的模值最終趨于穩(wěn)定與靜態(tài)校準(zhǔn)模值相同. 本文提出的小角度最小二乘迭代法能快捷有效地解決多旋翼無人機(jī)的動態(tài)校準(zhǔn)問題，進(jìn)一步減少周圍磁場環(huán)境對磁力計測量數(shù)據(jù)的影響，從而提高多旋翼無人機(jī)磁力計的自適應(yīng)能力. 該方法校準(zhǔn)效果好，易于實現(xiàn)，能廣泛應(yīng)用于多旋翼無人機(jī)磁力計的動態(tài)校準(zhǔn).