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

， ，

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

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

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

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

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

磁干擾;多旋翼無人機(jī);飛行軌跡

0 引言

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

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

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

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

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

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

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

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

圖2 漸變異常

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

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

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

(1)

R=

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

(2)

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

(3)

(4)

(5)

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

3 基于航跡偏差的磁異常檢測(cè)

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

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

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

(6)

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

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

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

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

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

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

圖4 無干擾航向角差和航跡偏差報(bào)警時(shí)間圖

圖5 有干擾航向角差和航跡偏差報(bào)警時(shí)間圖

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

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

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

5 多旋翼無人機(jī)實(shí)際飛行測(cè)試

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

第一組實(shí)驗(yàn)為磁干擾迅速增加的實(shí)驗(yàn)，在磁傳感器Y軸上增加一個(gè)每秒遞增0.06高斯的干擾，多次飛行的報(bào)警時(shí)間如表1所示。

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

表1 快速增加磁干擾實(shí)際飛行報(bào)警時(shí)間表

表2 緩慢增加磁干擾實(shí)際飛行報(bào)警時(shí)間表

兩組實(shí)驗(yàn)各自架次1的報(bào)警時(shí)序圖和飛行軌跡圖如圖8至圖11所示。

圖8 磁干擾快速增加時(shí)航向角差和航跡偏差報(bào)警時(shí)間圖

圖9 磁干擾緩慢增加時(shí)航向角差和航跡偏差報(bào)警時(shí)間圖

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

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

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

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

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

6 結(jié)論

通過多次仿真飛行實(shí)驗(yàn)和多旋翼無人機(jī)實(shí)飛飛行實(shí)驗(yàn)，可得出以下結(jié)論：1)使用DGPS差分定位，在無磁干擾的情況下，多旋翼無人機(jī)飛行航跡與預(yù)設(shè)航線偏差較小，在設(shè)定閾值(1米)以內(nèi)，能滿足高精度作業(yè)要求；2)當(dāng)存在環(huán)境磁異常時(shí)，多旋翼無人機(jī)受其干擾，其飛行航跡會(huì)偏離預(yù)設(shè)航線，檢測(cè)航跡的偏離距離，通過與閾值比較，可以用來判斷是否存在環(huán)境磁干擾;3)磁航向角誤差檢測(cè)方法同樣可以用來檢測(cè)環(huán)境磁異常，與航跡偏差檢測(cè)方法各有優(yōu)缺點(diǎn)，綜合使用兩種方法，可有效(提高)環(huán)境磁場(chǎng)異常檢測(cè)的準(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é)報(bào)，2000:299.

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

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

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

[7] 符長青，曹 兵. 多旋翼無人機(jī)技術(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] 張勇剛，張?jiān)坪疲?寧．基于互補(bǔ)濾波器的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)用小型無人機(jī)姿態(tài)估計(jì)算法[J]．自動(dòng)化學(xué)報(bào)，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-)，男，重慶人，副教授，主要從事無人機(jī)導(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