， ，

(1.國防科技大學(xué) 機電工程與自動化學(xué)院，長沙 410073；2.北京韋加無人機科技股份有限公司，長沙 410073)

基于DGPS航跡偏差的多旋翼無人機磁干擾檢測技術(shù)研究

何磊1，羅兵2，吳文啟1

(1.國防科技大學(xué)機電工程與自動化學(xué)院，長沙410073；2.北京韋加無人機科技股份有限公司，長沙410073)

為了解決多旋翼無人機在飛行作業(yè)過程中受到環(huán)境磁干擾導(dǎo)致作業(yè)異常的問題，在使用DGPS進行差分定位的基礎(chǔ)上，提出了一種基于航跡偏差的多旋翼無人機磁干擾檢測技術(shù);其基本原理是，當(dāng)多旋翼無人機受到磁場干擾時，其飛行航跡會偏離預(yù)設(shè)航線，檢測其航跡的偏離距離，通過與閾值比較，可以用來判斷是否存在環(huán)境磁干擾;實驗結(jié)果表明該方法可以有效檢測環(huán)境磁場異常，在某些情況下比傳統(tǒng)的磁航向角誤差閾值檢測方法可靠性更高，虛警率更小;綜合使用航跡偏差檢測方法和磁航向角誤差檢測方法，可有效(提高)環(huán)境磁場異常檢測的準(zhǔn)確度，降低虛警率。

磁干擾;多旋翼無人機;飛行軌跡

0 引言

多旋翼無人機主要使用磁傳感器配合微硅陀螺來測量航向。目前，對機體自身造成的磁傳感器誤差的補償方法已經(jīng)比較成熟[1-2]。然而，多旋翼無人機一般在低空區(qū)域飛行，受到環(huán)境的磁干擾，比上述機體自身的磁干擾復(fù)雜，例如礦區(qū)、大型鐵磁建筑和高壓線等。

此類干擾一般是低頻且無規(guī)則的，對于這類干擾，暫時還沒有有效的解決方法。而此類磁干擾會造成多旋翼飛機偏離正常航線飛行、無法定點懸停、位置控制失控等等現(xiàn)象，使得多旋翼無人機無法正常作業(yè)。所以，檢測出環(huán)境中的磁干擾，對于提高多旋翼無人機的作業(yè)可靠性有重要的意義。

文獻[3]中提出了基于統(tǒng)計特性，通過模糊-補償濾波的方法，消除磁羅盤測量值中高頻干擾與低頻干擾的影響，但是沒有進行實驗驗證。文獻[4]提出了在差分磁羅盤的基礎(chǔ)上，使用磁模量變化閾值判定和磁航向角差閾值判定加模糊c-均值FCM分類算法的方法，以提高動態(tài)低頻干擾的識別率。

單一使用閾值判斷法，檢測成功率并不穩(wěn)定，這是因為一方面環(huán)境磁干擾的因素復(fù)雜，另一方面檢測效果也與飛機自身的狀態(tài)有關(guān)。本文所提出的方法中，暫時不采用磁模量閾值判定法。因為，使用大疆450飛機搭配pixhawk飛控進行的測試結(jié)果表明，磁模量閾值判定法所用到的磁模量測量結(jié)果不穩(wěn)定。具體表現(xiàn)為在電機啟動后，在環(huán)境中不存在磁干擾的情況下，飛機原地旋轉(zhuǎn)，磁傳感器測量的磁模量也會發(fā)生周期變化，從而導(dǎo)致虛警，所以使用磁模量閾值判定法的檢測可靠性較低，故本文暫不采用磁模量閾值判定法。

本文針對多旋翼無人機磁傳感器受到外界磁干擾的情況，在磁航向角誤差閾值判斷方法的基礎(chǔ)上，增加了對多旋翼無人機航跡偏差的檢測，綜合兩種方法，以提高多旋翼無人機磁異常的檢測幾率并降低虛警率。多旋翼無人機航跡偏差[5]，即多旋翼無人機飛行航跡和預(yù)期航線發(fā)生偏差。這個偏差值，結(jié)合磁傳感器的測量結(jié)果，可以用來判斷多旋翼飛機是否受到磁干擾。

1 單獨使用磁航向差閾值判定法的局限性

姿態(tài)估計模塊采用的是互補濾波器算法，陀螺所測量的姿態(tài)信息時刻在跟蹤磁航向數(shù)據(jù)，因此在正常情況下，磁航向角和當(dāng)前姿態(tài)估計所得到的航向角偏差值小于設(shè)定閾值。在磁航向出現(xiàn)突變等異常情況下，上述偏差值會大于閾值。磁異常檢測方法就是基于閾值的檢測原理，根據(jù)實際經(jīng)驗，閾值設(shè)定為5σ，當(dāng)偏差值大于設(shè)定閾值時系統(tǒng)報警。

針對外界磁干擾劇烈增加和緩慢增加的兩種情況(即模擬飛機快速進入干擾區(qū)域和慢速進入干擾區(qū)域的飛行行為)，使用磁航向角差閾值判斷法，在開源飛控軟件pixhawk和仿真工具gazebo中進行仿真，結(jié)果如圖1和圖2所示[6]。

圖2 漸變異常

由圖可見，當(dāng)出現(xiàn)階躍型磁干擾時，航向角差閾值判定法能夠及時準(zhǔn)確地報警；當(dāng)出現(xiàn)漸變磁干擾時，飛機自身航向會跟隨磁傳感器的測量結(jié)果，航向角差閾值判定法報警會出現(xiàn)延遲及漏警的情況。

2 多旋翼無人機磁異常情況下的動力學(xué)模型分析

多旋翼無人機的動力學(xué)模型[7-9]如式(1)：

(1)

R=

假設(shè)多旋翼無人機以小傾角飛行，即φ≈θ≈0，則sinφ≈φ，sinθ≈θ，cosφ≈cosθ≈1，可得公式(2):

(2)

假設(shè)無人機水平高度不變，即高度通道有g(shù)=f/m，由公式(1)和公式(2)可以推出其水平通道的簡化模型，如公式(3)[10-11]：

(3)

(4)

(5)

即水平通道內(nèi)，實際產(chǎn)生的加速度與期望加速度有一個Δψ的夾角。

3 基于航跡偏差的磁異常檢測

本文提出了一種基于航跡偏差的多旋翼無人機磁異常檢測方法。造成多旋翼無人機航跡偏差的原因有很多種，例如GPS定位誤差、大風(fēng)天氣等。而本文研究的多旋翼無人機正常作業(yè)需要精確按照預(yù)設(shè)航線飛行，所以采用了一些技術(shù)手段來確保航跡飛行精度，例如GPS模塊采用了載波相位差分DGPS(RTK)，限制了作業(yè)風(fēng)速不能大于4級等。因此使用航跡偏差來檢測環(huán)境磁干擾的方法具有可行性。依據(jù)上文的對多旋翼無人機動力學(xué)模型推導(dǎo)可知，環(huán)境磁干擾會造成多旋翼無人機的實際輸出加速度與期望加速度在方向產(chǎn)生偏差，如公式(5)所示，在此加速度的作用下，飛行航跡也將與預(yù)設(shè)航線發(fā)生偏差，如圖3所示。

圖3 飛行軌跡推導(dǎo)

圖3中，A、B點為預(yù)設(shè)航線起點和終點，虛線為預(yù)設(shè)航線，實線為實際飛行航跡。d就是多旋翼無人機到預(yù)設(shè)航跡的最近距離，即航跡偏差值。計算方法如公式(6)所示：

(6)

航跡偏差的報警閾值，需要根據(jù)無人機機體特征和實際應(yīng)用環(huán)境來設(shè)置，本文研究的多旋翼無人機，當(dāng)實際航跡偏離預(yù)設(shè)航線達到1米以上時，作業(yè)任務(wù)即失效，所以將航跡偏差的報警閾值設(shè)置為1米。

在上述條件下，基于航跡偏差的磁異常檢測就是指當(dāng)多旋翼無人機在沿預(yù)定航跡飛行時，排除GPS定位誤差和大風(fēng)天氣等其他影響因素后，如果偏離預(yù)設(shè)航跡超過設(shè)定閾值，即認為當(dāng)前作業(yè)環(huán)境中存在低頻磁干擾，磁傳感器測量異常，無人機切換至不使用磁傳感器的飛行模式，從而避免繼續(xù)飛行而發(fā)生事故。

4 多旋翼無人機磁異常情況下航跡偏差仿真

首先，對磁干擾環(huán)境下的多旋翼無人機飛行航跡偏差進行仿真驗證。本文選用當(dāng)前主流開源飛控pixhawk作為多旋翼無人機仿真控制源代碼，使用開源機器人仿真軟件gazebo作為動力學(xué)仿真工具，在gazebo環(huán)境中人為加入磁干擾后，仿真多旋翼無人機的飛行情況。

在gazebo環(huán)境中，分別進行兩次仿真飛行，一次不引入磁干擾；一次引入的磁干擾是水平面內(nèi)的一個先緩慢增強而后逐漸減弱的水平面內(nèi)磁干擾。

在上述條件下，進行任務(wù)飛行模式的飛行仿真。其結(jié)果如圖3～6所示。

圖4 無干擾航向角差和航跡偏差報警時間圖

圖5 有干擾航向角差和航跡偏差報警時間圖

圖6 無干擾任務(wù)模式飛行航跡偏差圖

圖7 有干擾任務(wù)模式飛行航跡偏差圖

圖3和圖5中，直線為預(yù)設(shè)航跡，另外一條即是實際飛行航跡，其增粗部分為偏差大于報警閾值(1米)的航跡。圖4和圖6中實心點為航向角誤差報警點，空心點為航跡偏差報警點。仿真無干擾情況下，飛行器沿航跡飛行，有偏差，但不超過報警閾值，無報警；但磁傳感器在飛行過程中，出現(xiàn)3次虛警。仿真有干擾的情況下，從圖3中的航跡可以看到，磁干擾緩慢增加時，無人機的航跡仍然會產(chǎn)生發(fā)散的偏差，但圖4中的航向角誤差閾值報警卻延后于航跡誤差報警，即航向角誤差閾值檢測并不能及時準(zhǔn)確的發(fā)現(xiàn)初期緩慢增加的磁干擾，一直到航跡發(fā)散后對機體姿態(tài)造成明顯干擾后，才能發(fā)出報警信息。仿真結(jié)果符合上文的分析結(jié)論。

5 多旋翼無人機實際飛行測試

實際飛行實驗使用大疆450機架，X型布局，任務(wù)模式(自動按照預(yù)設(shè)航跡)飛行。由于自然環(huán)境中顯著磁干擾區(qū)域比較難找，所以仍然使用人為增加磁干擾的方法。實驗分為兩組，分別做磁干擾迅速增加和磁干擾緩慢增加的實驗，然后對比實驗結(jié)果。實驗地點的地球磁場總強度為0.6高斯左右。

第一組實驗為磁干擾迅速增加的實驗，在磁傳感器Y軸上增加一個每秒遞增0.06高斯的干擾，多次飛行的報警時間如表1所示。

第二組實驗為磁干擾緩慢增加的實驗，在磁傳感器Y軸上增加一個每秒遞增0.012 5高斯的干擾。多次飛行結(jié)果如表2所示。

表1 快速增加磁干擾實際飛行報警時間表

表2 緩慢增加磁干擾實際飛行報警時間表

兩組實驗各自架次1的報警時序圖和飛行軌跡圖如圖8至圖11所示。

圖8 磁干擾快速增加時航向角差和航跡偏差報警時間圖

圖9 磁干擾緩慢增加時航向角差和航跡偏差報警時間圖

圖10 磁干擾快速增加時航跡偏差和飛行軌跡圖

圖11 磁干擾緩慢增加時航跡偏差和飛行軌跡圖

從表1的數(shù)據(jù)可以看出，當(dāng)外界磁干擾劇烈增加時，磁航向差閾值判斷法可及時有效的檢測出磁異常，航跡偏差判斷法也可準(zhǔn)確檢測出磁異常，但報警時間要比磁航向差閾值判斷法滯后。滯后時間與磁干擾的大小和變化速度有關(guān)，也與姿態(tài)估計中磁傳感器對姿態(tài)的修正權(quán)重有關(guān)，在本次飛行實驗的環(huán)境中，大概滯后10秒左右。從圖8中可以看出，當(dāng)磁干擾急劇增加時，磁航向角發(fā)生了劇烈變化，但姿態(tài)航向角并不會劇烈變化，所以磁航向差閾值判斷法可以準(zhǔn)確報警。

從表2的數(shù)據(jù)可以看出，當(dāng)外界磁干擾緩慢增加時，航跡偏差判斷法仍然可以檢測出磁異常，報警延時在7秒左右。但磁航向差閾值判斷法，基本不能檢測出磁異常，其報警都出現(xiàn)在30秒之后，即報警出現(xiàn)在磁干擾導(dǎo)致姿態(tài)失控之后，此時磁干擾已經(jīng)導(dǎo)致較為嚴重的飛行異常，報警意義不明顯。圖10中磁航向差閾值判斷法的第一個報警點處，磁航向和姿態(tài)航向角出現(xiàn)了一個階躍變化，也說明了此時無人機姿態(tài)已經(jīng)失控。

兩組實驗數(shù)據(jù)可以說明，航跡偏差判斷法在各種情況下都可以有效檢測出磁異常。結(jié)合磁航向差閾值判斷法，可以提高磁異常檢測的可靠性和靈敏度。

6 結(jié)論

通過多次仿真飛行實驗和多旋翼無人機實飛飛行實驗，可得出以下結(jié)論：1)使用DGPS差分定位，在無磁干擾的情況下，多旋翼無人機飛行航跡與預(yù)設(shè)航線偏差較小，在設(shè)定閾值(1米)以內(nèi)，能滿足高精度作業(yè)要求；2)當(dāng)存在環(huán)境磁異常時，多旋翼無人機受其干擾，其飛行航跡會偏離預(yù)設(shè)航線，檢測航跡的偏離距離，通過與閾值比較，可以用來判斷是否存在環(huán)境磁干擾;3)磁航向角誤差檢測方法同樣可以用來檢測環(huán)境磁異常，與航跡偏差檢測方法各有優(yōu)缺點，綜合使用兩種方法，可有效(提高)環(huán)境磁場異常檢測的準(zhǔn)確度，降低虛警率。

[1] Mark Pedley， Michael Stanley. Magnetic Calibration Algorithms. Freescale Semiconductor， Document Number:AN5019.

[2] Vishwatheja S， Venkataratnam P， Siva Yellampalli. Algorithm development for soft and hard iron calibration of magneticcompass[J]. International Research Journal of Engineering and Technology (IRJET)， 2016，8.

[3] 富 立，范耀祖. 智能航跡推算系統(tǒng)的研究[J]. 航空學(xué)報，2000:299.

[4] 郝振海，楊新勇，黃圣國. 基于差分磁羅盤的動態(tài)干擾識別技術(shù)[J]. 西南交通大學(xué)學(xué)報，2010:445-450.

[5] 齊 驥，王宇鵬，鐘 志. 無人機多階段航跡預(yù)測協(xié)同任務(wù)規(guī)劃[J].計算機測量與控制，2016,24(6)：189-194.

[6] 孫肖南，孫 陶，辛春英，等. 航磁姿態(tài)數(shù)據(jù)收錄系統(tǒng)設(shè)計[J].計算機測量與控制，2017,25(5)：192-198.

[7] 符長青，曹 兵. 多旋翼無人機技術(shù)基礎(chǔ)[M]. 北京：清華大學(xué)出版社，2017.

[8] Wu J， Zhou Z B， Chen J J， et al．Fast complementary filter for attitude estimation using low cost MARG sensors[J]．IEEE Sensors Journal， 2016，16(18)：6697-7007.

[9] 張勇剛，張云浩，李 寧．基于互補濾波器的MEMS/GPS/地磁組合導(dǎo)航系統(tǒng)[J]．系統(tǒng)工程與電子技術(shù)，2014，36(11)：2272-2279．

[10]Goldenberg F．Geomagnetic navigation beyond magnetic compass[P]．IEEE PLAN，SanDiego，Califomia，2006：684-694.

[11]Wang W， Ma H， Xia M，et al.Attitude and altitude controllerdesign for quad-rotor type MAVs[J].Mathematical Problems inEngineering，2013.

[12] Mellinger D， Shomin M， Michael N， et al. Cooperative grasping and transport using multiple quadrotors[M]. Distributed autonomous robotic systems. Springer Berlin Heidelberg， 2013: 545-558.

[13] 彭孝東，張鐵民，李繼宇，等．基于傳感器校正與融合的農(nóng)用小型無人機姿態(tài)估計算法[J]．自動化學(xué)報，2015，41(4)：854-860．

ResearchonMagneticInterferenceDetectionofMulti-rotorUAVBasedonDGPSFlightPathDeviation

He Lei1， Luo Bing2， Wu Wenqi1

(1.College of Mechatronics Engineering and Automation， National University of Defense Technology，Changsha 410073， China； 2.Beijing Viga UAV technology corp.， LTD.， Changsha 410073， China)

In order to solve the problem that the multi - rotor unmanned aerial vehicle (UAV) is caused by environmental magnetic interference in the course of flight operation， a magnetic interference detection technology of multi - rotor unmanned aerial vehicle based on flight path deviation and differential positioning using DGPS is proposed. The basic principle is that when the multi-rotor UAV by the magnetic field interference， the flight path will deviate from the preset path， to detect the deviation of its track distance， compared with the threshold can be used to determine whether there is environmental interference. The experimental results show that this method can effectively detect the anomaly of environmental magnetic field， and in some cases， it is more reliable and less false than the traditional magnetic heading angle error detection method. The combination of flight path deviation detection method and magnetic heading angle error detection method can effectively improve the accuracy of environmental magnetic field anomaly detection and reduce false alarm rate.

magnetic interference;UAV;flight path

2017-07-17；

2017-08-09。

何 磊(1981-)，男，湖南長沙人，碩士研究生，主要從事導(dǎo)航技術(shù)與嵌入式系統(tǒng)方向的研究。

羅 兵(1971-)，男，重慶人，副教授，主要從事無人機導(dǎo)航與控制技術(shù)方向的研究。

吳文啟(1967-)，男，廣西南寧人，教授，博士生導(dǎo)師，主要從事高精度導(dǎo)航技術(shù)方向的研究。

1671-4598(2017)10-0039-04

10.16526/j.cnki.11-4762/tp.2017.10.011

V279

A