基于最小二乘的多旋翼無人機磁力計動態(tài)校準

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

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

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

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

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

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

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

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

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

令

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

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

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

圖1 小角度旋轉示意圖Fig.1 The diagram of small angle rotation

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

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

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

2 仿真驗證

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

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

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

3 實驗驗證

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

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

圖4 磁力計輸出數據分布Fig.4 The data distribution of magnetometer output

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

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

4 結語

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