徐東甫，白　越, 宮　勛，吳子毅,續(xù)志軍

(1.中國科學(xué)院 長春光學(xué)精密機械與物理研究所，吉林 長春 130033;

2.中國科學(xué)院大學(xué)，北京 100039)

?

多旋翼無人機在變控制量下的三軸磁羅盤校正

徐東甫1,2，白越1*, 宮勛1，吳子毅1,2,續(xù)志軍1

(1.中國科學(xué)院 長春光學(xué)精密機械與物理研究所，吉林 長春 130033;

2.中國科學(xué)院大學(xué)，北京 100039)

為了在特殊環(huán)境下有效使用多旋翼無人機飛行器的磁羅盤，研究了大電流、控制量變化情況下磁羅盤的校正問題，提出了一種磁羅盤自適應(yīng)校準方法。推導(dǎo)了磁羅盤誤差的變化規(guī)律并建立了誤差模型，分析了硬磁誤差隨控制量變化的規(guī)律，進而提出了一種基于整體最小二乘法的空間直線擬合方法。通過空間直線擬合，將得到的控制量和硬磁補償?shù)膶?yīng)關(guān)系用于實時調(diào)整對空間飛行器的硬磁補償，最終解決了控制量變化對磁羅盤的影響。進行了實驗驗證，結(jié)果表明：提出的方法可將飛行器控制量變化帶來的磁羅盤硬磁誤差基本抵消；在實際飛行中，多旋翼無人飛行器的偏航誤差可由最大的15°減小到3°以內(nèi)。本文所給出的方法在控制量變化，大電流情況下使用時，可以很好地校正磁羅盤航向角誤差，提高導(dǎo)航精度。

多旋翼無人機；磁羅盤；變控制量；硬磁誤差；誤差補償

*Correspondingauthor,E-mail:baiy@ciomp.ac.cn

1　引　言

近年來，隨著無人飛行器技術(shù)的成熟[1-3],多旋翼飛行器因其結(jié)構(gòu)簡單、操作靈活、具有垂直起降和懸停能力等優(yōu)點成為了研究熱點[4-6]。對于多旋翼飛行器自主飛行，航向角精度和航向角穩(wěn)定性對其導(dǎo)航非常重要。電子磁羅盤具有體積小、成本低、無累計誤差、可自動尋北等特點,被廣泛應(yīng)用于飛行器、車輛的導(dǎo)航系統(tǒng)中，為其提供航向角。但其精度容易受到周圍電源、導(dǎo)線、電機等鐵磁物質(zhì)的影響, 因此其校準技術(shù)成為研究的熱點[7-9]。

文獻[10]提出了一種新的標(biāo)定方法，得到更好的校正效果；文獻[11]采用一種避免豎立載體來校正磁羅盤的方法，節(jié)省了校正時間；文獻[12]利用慣性測量傳感器和GPS輔助校正磁羅盤文；文獻[13]利用飛行器左右盤旋飛行時的采樣數(shù)據(jù)，實現(xiàn)對載體上鐵磁材料引起的羅差的自動補償，采用橢圓假設(shè)算法，利用飛機在多于5個不同方向平飛時的采樣數(shù)據(jù)來自動補償平飛時的羅差；文獻[14]利用多傳感器信息，將磁羅盤磁傳感器與GPS 或慣性儀表的輸出信息進行補償濾波, 以校準磁傳感器的測量值, 具有在線實時補償能力；文獻[15]通過對系數(shù)矩陣的奇異性進行分析, 采用最小二乘方法實現(xiàn)了對橢圓模型誤差系數(shù)的辨識，克服了不穩(wěn)定性, 對校準過程中的突變干擾有很強的抑制能力。

本文針對多旋翼無人飛行器布局緊湊，磁場環(huán)境惡劣、電磁干擾不確定等情況，深入分析磁羅盤的變化趨勢，提出了針對控制量變化的磁羅盤自適應(yīng)校準方法。可有效抵消多旋翼飛行器控制量變化對磁場的影響，對變化的硬磁誤差進行補償，實現(xiàn)不同控制量下的磁羅盤自適應(yīng)校正。并通過實驗，對所提出的校正方法與傳統(tǒng)的橢球擬合法在適應(yīng)范圍和精度兩方面進行比較, 結(jié)果證明了該方法的有效性。

2　磁羅盤誤差分析

2.1磁羅盤誤差分析

(1)

其中:η=[φ，θ，ψ]T，

圖1　參考坐標(biāo)系Fig.1　Reference coordinate system

進而得到航向角:

(2)

其中:ε為地磁偏角。

三軸磁羅盤的誤差分為自身誤差和羅差，磁羅盤自身存在的誤差包括制造誤差與安裝誤差，這種誤差比較固定，可以得到較好的解決。羅差是由磁阻傳感器周圍的鐵磁材料、電器設(shè)備等環(huán)境磁場與地球磁場混疊產(chǎn)生。因為羅差的存在，使三軸磁羅盤不能正確感應(yīng)地球磁場，從而產(chǎn)生航向角誤差。所以羅差是影響磁羅盤精度最主要的因素。

羅差分為硬磁誤差和軟磁誤差，其誤差模型可以由Poisson方程描述:

(3)

磁羅盤在小地域內(nèi)做任意姿態(tài)運動時，地磁場矢量的標(biāo)量值‖H‖為固定值，若不考慮外磁干擾, 則三軸磁傳感器的測量值H滿足：

HT·H=‖H‖2,

(4)

取D的2范數(shù)并整理為曲面的二次型形式:

(5)

可以認為磁羅盤三軸之間基本正交，不平行角度和感應(yīng)磁場軸間干擾都較小，所以誤差K矩陣嚴格對角占優(yōu)，能夠保證式中KKT嚴格對角占優(yōu)，為正定矩陣。因此式表示一個橢球面。在已知羅差系數(shù)且測得H1的情況下, 可以求出H。

H=K-1(H1-σ).

(6)

2.2變控制量時硬磁誤差變化

上述校正方法在多旋翼飛行器的控制量不變時是可行的。但如果多旋翼飛行器處在飛行狀態(tài)下，控制量變化就會產(chǎn)生較強的磁場干擾，影響航向角的精度，該情況在帶載能力較強的多旋翼飛行器上表現(xiàn)的尤為嚴重。主要原因是其執(zhí)行機構(gòu)工作電流大、磁場干擾嚴重，在大地坐標(biāo)系垂直方向上機動或者更換載荷等情況下，常導(dǎo)致工作電流10 A量級的變化，磁羅盤硬磁誤差不斷變化，從而導(dǎo)致航向角的偏差。

(7)

當(dāng)?shù)卮艃A角為σ，根據(jù)地磁傾角的定義有：

(8)

將式(8)代入式(7)得到羅差:

(9)

由控制量變化在機體坐標(biāo)系上產(chǎn)生的總磁場矢量Hm大小改變而方向基本不變的。設(shè)該磁場強度為Hm，其與XY平面夾角為α，X軸夾角為與Y軸夾角為β。所以電氣元件在機體坐標(biāo)系下的磁場分量為

(10)

將式(10)和(7)帶入式(9)：

(11)

在理想的情況下Δφ正比于Hm，又因為Hm隨著控制量變化而變化，即在姿態(tài)角不變的情況下，羅差(主要是硬磁誤差)隨控制量變化而變化。

3　磁羅盤自適應(yīng)補償算法

由于羅差不恒定，通常的橢球擬合的方法不能滿足變化控制量的使用要求。為補償隨著控制量變化帶來的硬磁誤差，采用整體最小二乘法進行離線擬合，并和傳統(tǒng)的橢圓擬合方法相結(jié)合，設(shè)計了磁羅盤自適應(yīng)校正算法，如圖2所示。

圖2　偏航角補償算法Fig.2　Compensation algorithm of yaw angle

對磁羅盤的補償分為地面校正和空中校正兩部分。在地面時，通過橢球擬合，對磁羅盤進行經(jīng)典硬磁校正和軟磁校正，完成電子磁羅盤的初步校正；同時通過整體最小二乘法進行離線的空間直線擬合，找到控制量與磁場變化量的對應(yīng)關(guān)系；在空中時，主控芯片得到飛行器的控制量，通過控制量與磁場變化量的關(guān)系，實時的調(diào)整磁場補償量，抵消因控制量變化帶來的磁場影響，得到準確的磁羅盤數(shù)據(jù)，最后與陀螺儀數(shù)據(jù)一起經(jīng)算法融合，得到最終的航向角。

地面校正為：

(11)

同時，針對硬磁誤差變化的現(xiàn)象和數(shù)據(jù)，離線擬合出控制量和磁場的關(guān)系，為空中的自適應(yīng)磁場校正做準備。在工程中，通常使用最小二乘法對大量數(shù)據(jù)進行擬合，但是在擬合控制量和磁場的關(guān)系時存在兩個重要問題：

1) 空間直線的一般表達式為：

(12)

為具有6個參數(shù)的連等式，并不是簡單的線性關(guān)系，所以最小二乘法并不能直接擬合[16]；

2)對磁場的測量值本身存在測量誤差，普通最小二乘法擬合效果不佳。

針對以上問題，首先將標(biāo)準型方程轉(zhuǎn)換為整體最小二乘法模型，并用TLS法對數(shù)據(jù)進行空間直線擬合[18]。

式(12)等價為：

(13)

式(13)化簡為：

V=B·ξ-L,

(14)

將式(5)代入平差準則：

對矩陣和參數(shù)向量中的各個元素求導(dǎo),得到迭代方程：

(15)

(16)

迭代的具體解算步驟可概括為：

1)獲取未知參數(shù)的初值ξ(0)；

4)重復(fù)步驟2)、3)，直到2次計算的參數(shù)值之差小于一定的閾值，退出迭代，輸出結(jié)果。

4　測量實驗與結(jié)果

根據(jù)現(xiàn)有的實驗條件，將首先對不同控制量下多旋翼飛行器進行空間磁場采樣實驗，并進行橢球擬合，以驗證不同控制量對羅差的影響；再通過空間直線擬合算法，離線擬合出控制量和硬磁誤差變化量的關(guān)系曲線，進而得到準確的偏航數(shù)據(jù)；最后進行羅差補償精度對比實驗和自主飛行軌跡對比實驗，來驗證研究內(nèi)容的實際應(yīng)用效果。

4.1飛行器磁場采樣

為驗證式(10)，保持多旋翼飛行器各個執(zhí)行機構(gòu)的控制量不變，通過轉(zhuǎn)臺分別在X-Y、Y-Z、X-Z平面旋轉(zhuǎn)多旋翼飛行器，對圓周上的各點磁場強度采樣記錄，進行橢球擬合。然后增加控制量，重復(fù)上述試驗過程，最終得到多旋翼飛行器在不同控制量時的磁場采樣擬合曲線，如圖3所示。

圖3　磁羅盤采樣擬合Fig.3　Sample fitting of Magnetic compass

圖4　空間最小二乘法擬合曲線Fig.4　Total least square fitting of space line

可見飛行器的執(zhí)行單元和導(dǎo)線等電氣元件，在機體坐標(biāo)系上產(chǎn)生了一個大小隨控制量增大而增大的磁場分量，且其變化方向在空間內(nèi)基本保持一條直線，這在本質(zhì)上是硬磁誤差的變化，因此實驗結(jié)果符合上文結(jié)論：隨著控制量的變化，飛行器的硬磁誤差也隨之變化。

為補償這部分變化的硬磁誤差，使用整體最小二乘法，在離線情況下，通過已有的飛行器磁場采樣數(shù)據(jù)，擬合各個橢球心的連線即得到了控制量和磁場變化量的關(guān)系，如圖4所示。

通過整體最小二乘法離線擬合出控制量與磁場變化量的關(guān)系，實時的調(diào)整磁場補償量，抵消因控制量變化量的磁場影響，得到準確的磁羅盤數(shù)據(jù)，最后與陀螺儀數(shù)據(jù)一起經(jīng)算法[17]融合，得到準確的航向角。

4.2羅差補償精度對比試驗和實際使用對比

將無人飛行器固定在無磁轉(zhuǎn)臺上，然后轉(zhuǎn)臺上以間隔旋轉(zhuǎn)一周，分別用傳統(tǒng)的橢球擬合法和本文的方法對磁羅盤進行羅差補償，通過對比飛行器處在靜止、起飛和加速3種狀態(tài)下的磁航向角剩余誤差Δψ。如圖5～7所示，飛機的控制量分別為0、200和300(對應(yīng)電流為0 A、32 A、53 A)，φ1為校正前的磁航向角剩余誤差，φ2為只進行傳統(tǒng)橢圓擬合校正后的磁航向角誤差，φ3為進行本文方法校正后的磁航向角剩余誤差。

圖5　磁航向剩余誤差(0控制量電流0 A時)Fig.5　Residual error of Magnetic heading (controlled quantity at 0)

圖6　磁航向剩余誤差(200控制量電流32 A時)Fig.6　Residual error of Magnetic heading (controlled quantity at 200)

圖7　磁航向剩余誤差(300控制量電流53 A時)Fig.7　Residual error of Magnetic heading (controlled quantity at 300)

圖5～7的結(jié)果統(tǒng)計如表1所示，在靜止(控制量為0)情況下，磁羅盤自適應(yīng)校正方法和傳統(tǒng)的橢圓擬合方法效果相似，當(dāng)控制量變化，飛行器在大電流狀態(tài)下工作時，普通的橢圓擬合羅差校正已不能較好地校正硬磁誤差的偏移，而磁羅盤校正方法可以較好的解決此問題；當(dāng)控制量加大時，兩者的校正效果差距進一步加大。

表1　實驗結(jié)果對比

為進一步驗證實驗結(jié)論，進行多旋翼無人機自主軌跡飛行對比試驗。使用本文方法校正磁羅盤和傳統(tǒng)校正方法進行自主飛行，圖8所示為使用傳統(tǒng)校正的飛行軌跡，圖9所示為使用本文自適應(yīng)校正的飛行軌跡。

圖8　傳統(tǒng)校正的飛行軌跡Fig.8　Flight path before correction

圖9　自適應(yīng)校正的飛行軌跡Fig.9　Flight path after correction

使用本文校正方法后，自主飛行的誤差明顯減少，可見本方法對控制量變化引起的磁航向角誤差有較好的校正效果。

5　結(jié)　論

多旋翼無人飛行器磁羅盤的誤差會隨控制量變化而變化，具體表現(xiàn)為硬磁誤差的偏移。當(dāng)飛行器工作在大電流，控制量變化的環(huán)境時，普通的羅差補償方法無法很好的完成校正工作。本文首先通過整體最小二乘法擬合得出了飛行器控制量變化和磁場的變化關(guān)系；然后結(jié)合經(jīng)典羅差補償，對飛行器進行地面和空中兩種校正。經(jīng)實驗證明，此方法確實有效：將飛行器控制量變化帶來的磁羅盤硬磁誤差基本抵消，飛行器在空中飛行時磁航向誤差減小到3°以內(nèi)，基本達到了使用要求。另外，對影響磁羅盤精度的多個干擾源一次校正，在野外的工作中減少了大量工作。

[1]李迪，陳向堅，續(xù)志軍．增益自適應(yīng)滑模控制器在微型飛行器飛行姿態(tài)控制中的應(yīng)用[J]．光學(xué) 精密工程，2013, 21(5): 1183-1191.

LI D, CHENG X J, XU ZH J. Gain adaptive sliding mode controller for flight attitude control of MAV[J].OptPrecisionEng., 2013, 21(5): 1183-1191.

[2]錢默抒, 姜斌, 許德智, 等. 無人機姿控系統(tǒng)魯棒動態(tài)面容錯控制設(shè)計[J]. 系統(tǒng)工程與電子技術(shù), 2014, 36(9): 1798-1803.

QIU M SH, JIANG B, XU D ZH,etal.. Robust dynamics surface fault tolerant control design for attitude control systems of UAV[J].SystemsEngineeringandElectronics,2014,36(9): 1798-1803.

[3]宮勛，白越，趙常均，等．Hex-Rotor無人飛行器及其飛行控制系統(tǒng)設(shè)計[J]．光學(xué) 精密工程，2012, 20(11): 2450-2458.

GONG X, BAI Y, ZHAO CH J,etal.. Hex-Rotor aircraft and its autonomous flight control system [J].Opt.PrecisionEng., 2012, 20(11): 2450-2458. (in Chinese)

[4]張欣, 白越, 趙常均, 等. 多旋翼姿態(tài)解算中的改進自適應(yīng)擴展Kalman算法[J].光學(xué) 精密工程, 2014, 22(12): 3384-3390.

ZHANG X, BAI Y, ZHAO CH J,etal.. Improved adaptive extended Kalman algorithm for attitude estimation of multi-rotor UAV [J].Opt.PrecisionEng., 2014, 22(12): 3384-3390. (in Chinese)

[5]SEBESTA K D, BOIZOT N. A real-time adaptive high-gain EKF, applied to a quadcopter inertial navigation system [J].IEEETransactionsonIndustrialElectronics, 2014, 61(1): 495-503.

[6]GRZONKA S, GRISETTI G, BURGARD W. A fully autonomous indoor quadrotor[J].Robotics,IEEETransactionson, 2012, 28(1): 90-100.

[7]FANG J, SUN H, CAO J,etal.. A novel calibration method of magnetic compass based on ellipsoid fitting[J].InstrumentationandMeasurement,IEEETransactionson, 2011, 60(6): 2053-2061.

[8]MARKOVIC R, KRAJNC A, MATKO D. Calibration of a solid-state magnetic compass using angular-rate information from low-cost sensors[J].Science,Measurement&Technology,IET, 2011, 5(2): 54-58.

[9]LI M, ROUF V T, THOMPSON M J,etal..Three-axis Lorentz-force magnetic sensor for electronic compass applications[J].MicroelectromechanicalSystems,Journalof,2012, 21(4): 1002-1010.

[10]FARAJIDAVAR A, BLOCK J M, GHOVANLOO M. A comprehensive method for magnetic sensor calibration: A precise system for 3-D tracking of the tongue movements[C].EngineeringinMedicineandBiologySociety(EMBC), 2012AnnualInternationalConferenceoftheIEEE.IEEE, 2012: 1153-1156.

[11]戴磊,齊俊桐,吳沖,等.旋翼飛行機器人磁羅盤誤差分析及校準[J].2012, 23(4): 418- 423.

DAI L,QI J T,WU CH,etal.. Magnetic compass error analysis and calibration for rotorcraft flying robot [J].Robot,2012,23(4):418-423.

[12]蔡體菁, 劉瑩, 宋軍, 等. 嵌入式 GPS/MIMU/磁羅盤組合導(dǎo)航系統(tǒng)[J]. 儀器儀表學(xué)報, 2011 (12): 2695-2699.

CAI T J, LIU Y,SONG J,etal.. Embedded integrated GPS/MIMU/compass navigation system[J].ChineseJournalofScientificInstrument,2010,31(12):2695-2699.

[13]劉詩斌. 無人機磁航向測量的自動羅差補償研究[J]. 航空學(xué)報, 2007, 28(2)：411-414.

LIU SH B, Study on automatic magnetic deviation compensation of mafntic heading measurement for UAV [J].ActaAeronauticaetAstronauticaSinica, 2007, 28(2): 411-414.

[14]MARKOVIC R, KRAJNC A, MATKO D. Calibration of a solid-state magnetic compass using angular-rate information from low-cost sensors[J].Science,Measurement&Technology,IET, 2011, 5(2): 54-58.

[15]劉仁浩, 王華. 數(shù)字磁羅盤的全姿態(tài)羅差補償[J]. 光學(xué) 精密工程, 2011, 19(8): 1867.

LIU R H, WANG H. All attitudemagnetic deviation compensationfor digital magnetic compass [J].Opt.PrecisionEng., 2011, 19(8): 1867.

[16]丁克良, 沈云中, 歐吉坤. 整體最小二乘法直線擬合[J]. 遼寧工程技術(shù)大學(xué)學(xué)報: 自然科學(xué)版, 2010 (1): 44-47.

DING K, SHENG Y, OU J K. Methods of line-fitting based on total least-squares [J].JournalofLiaoningTechnicalUniversity(NaturalScience), 2010, 29(1): 44-47.

[17]韓輔君, 徐靜, 宋世忠. 基于低成本多傳感器的自適應(yīng)組合濾波[J]. 光學(xué) 精密工程, 2011, 19(12): 3007-3015.

HAN F J, XU J, SONG SH ZH. Adaptive attitude estimation filtering with low-cost multi-sensors for MAHRS[J].Opt.PrecisionEng., 2011, 19(12): 3007-3015.

徐東甫(1987-)，男，吉林省吉林市人，博士研究生，2011年于吉林大學(xué)獲得學(xué)士學(xué)位，主要從事無人飛行器組合導(dǎo)航與控制方面的研究。E-mail:xu.dong.fu@163.com

導(dǎo)師簡介：

續(xù)志軍(1953-)，男，吉林長春人，研究員，博士生導(dǎo)師，主要從事電子技術(shù)、自動控制方面的研究。E-mail：xuzj538@ciomp.ac.cn

(版權(quán)所有未經(jīng)許可不得轉(zhuǎn)載)

Correction of 3-axis magnetic compass in multi-rotor UAV under variable control amounts

XU Dong-fu1,2, BAI Yue1*,GONG Xun1, WU Zi-yi1,2, XU Zhi-jun1

(1.ChangchunInstituteofOptics,FineMechanicsandPhysics,ChineseAcademyofSciences,Changchun130033,China； 2.UniversityofChineseAcademyofSciences,Beijing100039,China)

To use effectively the magnetic compass in a multi-rotor Unmanned Aerial Vehicle(UAV) in special environments, this paper researches the magnetic deviation correction method when it works at a large current and a variable control amount. Firstly, the variation patterns of magnetic deviation were derived and an error model was established. The relationship between hard magnetic error and the control amount was also analyzed. Then, a spatial linear fitting method was proposed based on Total Least Square (TLS). Finally, the relationship between hard magnetic error and control amount was adopted to implement the hard magnetic compensation of the UAV. In this way, the magnetic deviation was adaptively compensated and the compass was corrected in real time. Experimental results indicate that the hard magnetic deviation from the changed control amount can be counteracted by proposed method, and the yaw error of the UAV reduces from 15°to less than 3°during the flight. It concludes that the method proposed in this paper effectively corrects the yaw error of the magnetic compass when it works at the high current and large control amount and the navigation accuracy and capability to variable loads of the UAV is improved greatly.

multi-rotor Unmanned Aerial Vehicle(UAV); magnetic compass; variable control amount; hard magnetic error;error compensation

2015-11-12；

2016-01-11.

國家自然科學(xué)基金資助項目(No.11372309, No.61304017)；吉林省科技發(fā)展計劃重點項目(No.20150204074GX)；吉林省與中科院合作科技專項資金資助項目(No. 2014SYHZ0004)

1004-924X(2016)08-1940-08

V279; V241.61

A

10.3788/OPE.20162408.1940