基于磁傳感器的四旋翼飛行器自主導(dǎo)航設(shè)計(jì)

鐘麗娜，王君浩，王 融

(1.南京航空航天大學(xué) a.金城學(xué)院;b.自動(dòng)化學(xué)院，南京 211156;2.無(wú)錫漢和航空技術(shù)有限公司，江蘇無(wú)錫 214000)

四旋翼飛行器能垂直起降、自由懸停，具有很強(qiáng)的機(jī)動(dòng)能力，并且體積小、質(zhì)量輕、攜帶方便，因此特別適合在復(fù)雜環(huán)境下使用，尤其是它能輕易進(jìn)入人不易進(jìn)入的惡劣環(huán)境［1－2］。目前，四旋翼無(wú)人飛行器在空中實(shí)時(shí)監(jiān)控、地形勘探、地區(qū)性救災(zāi)、電影航拍取景等條件苛刻的飛行任務(wù)中發(fā)揮著越來(lái)越重要的作用［3］。國(guó)內(nèi)外關(guān)于四旋翼飛行器的研究正日益廣泛［4－5］。

對(duì)于復(fù)雜環(huán)境的偵查、監(jiān)視任務(wù)，需要四旋翼飛行器具有無(wú)人自主飛行功能。傳統(tǒng)四旋翼飛行器需要手控實(shí)現(xiàn)飛行、姿態(tài)控制［6］，無(wú)法適應(yīng)復(fù)雜環(huán)境下無(wú)人自主飛行的要求。本文設(shè)計(jì)了一種利用低成本磁傳感器的自主飛行方法，通過(guò)與預(yù)定航線(xiàn)偏航的情況判斷，進(jìn)一步調(diào)節(jié)飛控系統(tǒng)，實(shí)現(xiàn)航路規(guī)劃自主飛行功能。

1 四旋翼無(wú)人機(jī)飛行原理

四旋翼無(wú)人機(jī)有4個(gè)電機(jī)，呈十字或X形排列，電機(jī)驅(qū)動(dòng)4片螺旋槳旋轉(zhuǎn)從而產(chǎn)生升力;4個(gè)電機(jī)軸距幾何中心的距離相等(呈前進(jìn)軸對(duì)稱(chēng)亦可，即異形機(jī)架)，當(dāng)對(duì)角2個(gè)軸產(chǎn)生的升力相同時(shí)可保證力矩的平衡，4軸不會(huì)向任何一個(gè)方向傾轉(zhuǎn);4個(gè)電機(jī)“一對(duì)正轉(zhuǎn)一對(duì)反轉(zhuǎn)”的方式使得繞豎直軸方向旋轉(zhuǎn)的反扭矩平衡，保證了4軸航向的穩(wěn)定性［7］。若4個(gè)電機(jī)的轉(zhuǎn)速做相應(yīng)的變化即可實(shí)現(xiàn)4軸在橫向、縱向、豎直方向和偏航方向上的運(yùn)動(dòng)。當(dāng)4軸需要向前方運(yùn)動(dòng)時(shí)，位于后方的2臺(tái)電機(jī)轉(zhuǎn)速提高，位于前方的2臺(tái)電機(jī)轉(zhuǎn)速下降，此時(shí)后方電機(jī)產(chǎn)生的升力大于前方電機(jī)的升力，4個(gè)軸就會(huì)沿幾何中心向前傾轉(zhuǎn)，槳葉升力沿縱向的分力驅(qū)動(dòng)4個(gè)軸向前運(yùn)動(dòng);當(dāng)4個(gè)軸需要向后方運(yùn)動(dòng)時(shí)，后方電機(jī)產(chǎn)生的升力小于前方電機(jī)的升力，4個(gè)軸就會(huì)沿幾何中心向后傾轉(zhuǎn)，槳葉升力沿縱向的分力驅(qū)動(dòng)4個(gè)軸向后運(yùn)動(dòng);當(dāng)4個(gè)軸需要向左偏航運(yùn)動(dòng)時(shí)，右方電機(jī)產(chǎn)生的升力大于左方電機(jī)的升力，4個(gè)軸就會(huì)沿幾何中心向左傾轉(zhuǎn)，槳葉升力沿縱向的分力驅(qū)動(dòng)四軸向左運(yùn)動(dòng);當(dāng)4個(gè)軸需要向右偏航運(yùn)動(dòng)時(shí)，右方電機(jī)產(chǎn)生的升力小于左方電機(jī)的升力，4個(gè)軸就會(huì)沿幾何中心向右傾轉(zhuǎn)，槳葉升力沿縱向的分力驅(qū)動(dòng)4個(gè)軸向右運(yùn)動(dòng)。

圖1 四旋翼飛行器運(yùn)動(dòng)原理

當(dāng)4個(gè)軸要向左旋轉(zhuǎn)時(shí)，逆時(shí)針旋轉(zhuǎn)的電機(jī)轉(zhuǎn)速上升，順時(shí)針旋轉(zhuǎn)的電機(jī)轉(zhuǎn)速下降，使向左的反扭距大于向右的反扭矩，4個(gè)軸在反扭距的作用下向左旋轉(zhuǎn);當(dāng)4個(gè)軸要向右旋轉(zhuǎn)時(shí)，逆時(shí)針旋轉(zhuǎn)的電機(jī)轉(zhuǎn)速下降，順時(shí)針旋轉(zhuǎn)的電機(jī)轉(zhuǎn)速上升，使向右的反扭距大于向左的反扭矩，4個(gè)軸在反扭距的作用下向右旋轉(zhuǎn);當(dāng)4個(gè)軸需要垂直運(yùn)動(dòng)時(shí)，4個(gè)槳產(chǎn)生的升力超過(guò)或者低于4個(gè)軸本身重力的時(shí)候，便能實(shí)現(xiàn)豎直方向上的上升與下降運(yùn)動(dòng);當(dāng)槳的升力與4個(gè)軸本身的重力相等的時(shí)候即可實(shí)現(xiàn)懸停。

2 系統(tǒng)總體設(shè)計(jì)

控制系統(tǒng)主要由飛控系統(tǒng)模塊、導(dǎo)航控制模塊、電機(jī)驅(qū)動(dòng)模塊、無(wú)線(xiàn)通信模塊構(gòu)成。飛控系統(tǒng)模塊采用PID算法對(duì)系統(tǒng)進(jìn)行控制和調(diào)節(jié)，根據(jù)四元數(shù)算法對(duì)陀螺儀輸出解散姿態(tài)信息以便調(diào)整和保持正常飛行姿態(tài)［8］;采用卡爾曼濾波算法進(jìn)行傳感器數(shù)據(jù)融合以提高精度［9］。導(dǎo)航控制模塊采用GPS接收機(jī)、氣壓計(jì)來(lái)提供位置、高度、航向信息。電機(jī)驅(qū)動(dòng)模塊由4個(gè)電機(jī)及相應(yīng)電子調(diào)速器構(gòu)成。無(wú)線(xiàn)通信模塊選用433 MHz的無(wú)線(xiàn)數(shù)字電臺(tái)與地面站遙測(cè)系統(tǒng)進(jìn)行通信。

圖2 系統(tǒng)總體設(shè)計(jì)

結(jié)合成本及硬件需求，主要硬件配置如下:

1)主控芯片

主控芯片采用Arduino Mega2560，為USB接口核心電路板，具有54路數(shù)字輸入輸出，適合需要大量IO接口的設(shè)計(jì)。處理器核心是 AT-mega2560，同時(shí)具有54路數(shù)字輸入/輸出口(其中16路可作為 PWM輸出)、16路模擬輸入、4路UART接口、1個(gè)16 MHz晶體振蕩器、1個(gè) USB口、1個(gè)電源插座、1個(gè)ICSP header和1個(gè)復(fù)位按鈕。

2)陀螺儀及加速度計(jì)

陀螺儀及加速度計(jì)選用MPU－6000，即全球首例整合性6軸運(yùn)動(dòng)處理組件。該器件成本低，體積小，由于整合性好避免了組合陀螺儀與加速器時(shí)之軸間差的問(wèn)題。MPU－6000整合了3軸陀螺儀以及3軸加速度計(jì)，并可藉由第2個(gè)I2C端口連接其他冗余加速度計(jì)、磁傳感器或其他傳感器的數(shù)位運(yùn)動(dòng)處理(digital motion processor，DMP)硬件加速引擎，主要以I2C端口以單一數(shù)據(jù)流的形式向應(yīng)用端輸出完整的9軸融合演算技術(shù)。

3)磁傳感器

磁傳感器采用霍尼韋爾 HMC5883L。這是一種表面貼裝的高集成模塊，并帶有數(shù)字接口的弱磁傳感器芯片，應(yīng)用于低成本羅盤(pán)和磁場(chǎng)檢測(cè)領(lǐng)域。HMC5883L包含最先進(jìn)的高分辨率HMC118X系列的磁阻傳感器，并附帶具有霍尼韋爾專(zhuān)利的集成電路，包括放大器、自動(dòng)消磁驅(qū)動(dòng)器、偏差校準(zhǔn)、能使羅盤(pán)精度控制在1～2°的12位模數(shù)轉(zhuǎn)換器。HMC5883L磁傳感器采用無(wú)鉛表面封裝技術(shù)，帶有16引腳，尺寸為3 mm ×3 mm ×0.9 mm。傳感器具有的對(duì)正交軸的低靈敏度固相結(jié)構(gòu)能用于測(cè)量地球磁場(chǎng)的方向和大小，其測(cè)量范圍從毫高斯到8高斯。

4)GPS接收機(jī)

GPS接收機(jī)采用Ublox公司生產(chǎn)的LEA6S接收機(jī)。該接收機(jī)跟蹤靈敏度高至－162 dBm，數(shù)據(jù)輸出頻率可達(dá)4 Hz.

3 基于磁傳感器的自主導(dǎo)航算法設(shè)計(jì)

為了實(shí)現(xiàn)四旋翼無(wú)人機(jī)飛行器自主導(dǎo)航的功能，設(shè)計(jì)了基于磁傳感器航向判斷的自主導(dǎo)航算法。本文的四旋翼無(wú)人機(jī)飛行平臺(tái)利用GPS報(bào)文中NMEA協(xié)議輸出的位置信息，由主控芯片Mega2560直接讀取，并將坐標(biāo)以“緯度，經(jīng)度”的形式輸出，分別對(duì)應(yīng)直角坐標(biāo)系中的X軸與Y軸。作為自動(dòng)導(dǎo)航算法的核心，相鄰航點(diǎn)間的距離及航向角解算的正確性和精確度會(huì)直接影響自動(dòng)導(dǎo)航的效果。

磁傳感器朝向正北時(shí)輸出的角度為0，對(duì)應(yīng)直角坐標(biāo)系中的Y軸正方向，正東方向?qū)?yīng)X軸正方向，正北方向?qū)?yīng)Y軸負(fù)方向，正西方向?qū)?yīng)X軸負(fù)方向。設(shè)定正北方向順時(shí)針旋轉(zhuǎn)的角度為航向角。此時(shí)，航點(diǎn)1和航點(diǎn)2的坐標(biāo)就變成了(X1，Y1)和(X2，Y2)。由此可以計(jì)算出航點(diǎn)1和航點(diǎn)2的距離L:

根據(jù)位置判斷，以正北方向?yàn)閰⒄盏慕^對(duì)航向角β1分別取值如下:

1)當(dāng) X2＞X1，Y2＞Y1時(shí)，

2)當(dāng) X2＞X1，Y2＜Y1時(shí)，

3)當(dāng) X2＜X1，Y2＜Y1時(shí)，

4)當(dāng) X2＜X1，Y2＞Y1時(shí)，

5)當(dāng) X2=X1，Y2＞Y1時(shí)，β1=0°;

6)當(dāng) X2＞X1，Y2=Y1時(shí)，β1=90°;

7)當(dāng) X2=X1，Y2＜Y1時(shí)，β1=180°;

8)當(dāng) X2＜X1，Y2=Y1時(shí)，β1=270°。

故以當(dāng)前航向角β0為參照的相對(duì)航向角β2計(jì)算如下:

對(duì)無(wú)人機(jī)的轉(zhuǎn)向控制方法如下:

1)當(dāng)β2＜0時(shí)，無(wú)人機(jī)逆時(shí)針旋轉(zhuǎn)

2)當(dāng)β2＞0時(shí)，無(wú)人機(jī)順時(shí)針旋轉(zhuǎn)

3)當(dāng)β2=0時(shí)，無(wú)人機(jī)不旋轉(zhuǎn)。

4 實(shí)際飛行測(cè)試與分析

為驗(yàn)證自主導(dǎo)航飛行功能，首先制作了一架模型驗(yàn)證機(jī)，利用四旋翼無(wú)人機(jī)在線(xiàn)模擬器驗(yàn)證了所涉及的PID參數(shù)，進(jìn)行了自動(dòng)穩(wěn)定測(cè)試、定點(diǎn)測(cè)試、多次航點(diǎn)測(cè)試，最后在校園內(nèi)進(jìn)行了實(shí)際飛行測(cè)試。PID參數(shù)設(shè)置如表1所示。

表1 PID參數(shù)設(shè)置

地面站設(shè)備采用基于Marvlink協(xié)議的433 M無(wú)線(xiàn)數(shù)傳和筆記本電腦，地圖信息采用 Google Earth三維地理信息系統(tǒng)控件，地面站控制軟件采用Ardupilot Mega Planner。遙控設(shè)備采用基于Marvlink協(xié)議的433 M無(wú)線(xiàn)數(shù)傳和基于DSM2傳輸協(xié)議的6通道遙控器，僅在起飛解鎖和降落關(guān)油門(mén)時(shí)使用。

四旋翼無(wú)人機(jī)自動(dòng)導(dǎo)航飛行圖如圖3所示。設(shè)定自主導(dǎo)航飛行航跡為從校操場(chǎng)到校圖書(shū)館前草坪，航跡設(shè)定如圖3(a)所示，總計(jì)航程為0.73 km。實(shí)際起飛和降落圖分別如圖3(b)、(c)所示。飛行過(guò)程中，只有起飛和降落通過(guò)手控操作來(lái)解鎖和關(guān)油門(mén)，其余航段為自主導(dǎo)航飛行。

圖3 四旋翼無(wú)人機(jī)自動(dòng)導(dǎo)航飛行圖

經(jīng)過(guò)自動(dòng)穩(wěn)定測(cè)試確定了四旋翼飛行器的穩(wěn)定性，并進(jìn)行了定點(diǎn)測(cè)試。在首次定點(diǎn)測(cè)試中，出現(xiàn)了飛行器的自振蕩現(xiàn)象。經(jīng)分析，自振蕩產(chǎn)生的原因是P值和I值較高引起的，適當(dāng)降低P、I值后，振蕩幾乎消失。參數(shù)調(diào)整合適后，進(jìn)行了跨區(qū)域自主導(dǎo)航飛行測(cè)試。在飛行過(guò)程中，無(wú)人機(jī)速度始終較快，這是考慮到抗風(fēng)性能而設(shè)定了較高的前進(jìn)速度、設(shè)定的懸停點(diǎn)較少。對(duì)于此現(xiàn)象的解決方法是減小NAV_WP值，增加懸停點(diǎn)，延長(zhǎng)懸停等待時(shí)間，這樣人可以小跑追上無(wú)人機(jī)以觀測(cè)無(wú)人機(jī)的飛行效果。在飛行過(guò)程中，除起飛和降落外，整個(gè)飛行器處于完全自主飛行狀態(tài)。為驗(yàn)證自主飛行算法，還設(shè)定了起飛、盤(pán)旋等飛行航跡進(jìn)行測(cè)試。結(jié)果表明:飛行器完全按照預(yù)設(shè)航跡進(jìn)行盤(pán)旋、并最終飛抵目的地，驗(yàn)證了本文所設(shè)計(jì)算法的有效性。

5 結(jié)束語(yǔ)

設(shè)計(jì)了一種基于磁傳感器的四旋翼自主導(dǎo)航飛行算法。該算法利用磁傳感器提供的航向信息與預(yù)設(shè)航跡進(jìn)行判斷，然后根據(jù)航向角差對(duì)四旋翼無(wú)人機(jī)進(jìn)行調(diào)節(jié)。設(shè)計(jì)并制作了四旋翼模型驗(yàn)證機(jī)，并進(jìn)行了實(shí)際飛行測(cè)試。實(shí)飛結(jié)果表明:該算法簡(jiǎn)單易行，且可快速執(zhí)行，實(shí)現(xiàn)了中短航程下自主導(dǎo)航飛行的無(wú)人機(jī)功能.

［1］李光春，王璐，王兆龍，等.基于四元數(shù)的四旋翼無(wú)人飛行器軌跡跟蹤控制［J］.應(yīng)用科學(xué)學(xué)報(bào)，2012，30(4):415－422.

［2］谷永晟，楊建軍，朱宇虹.四旋翼無(wú)人飛行器導(dǎo)航控制系統(tǒng)研究［J］.遙測(cè)遙控，2012，3:14 －15.

［3］Hoffmann G M，Huang H，Waslander S L，et al.Precision flight control for a multi-vehicle quadrotor helicopter testbed［J］.Control engineering practice，2011，19(9):1023－1036.

［4］Nicol C，Macnab C J B，Ramirez-Serrano A.Robust adaptive control of a quadrotor helicopter［J］.Mechatronics，2011，21(6):927 －938.

［5］Raffo G V，Ortega M G，Rubio F R.An integral predictive/nonlinear H∞control structure for a quadrotor helicopter［J］.Automatica，2010，46(1):29 －39.

［6］李浩濤.基于四旋翼無(wú)人機(jī)傳感器數(shù)據(jù)采集與處理的研究［D］.天津:天津大學(xué)，2012.

［7］周權(quán)，黃向華，朱理化.四旋翼微型飛行平臺(tái)姿態(tài)穩(wěn)定控制試驗(yàn)研究［J］.傳感器與微系統(tǒng)，2009，28(5):72－79.

［8］岳基隆，張慶杰，朱華勇.微小型四旋翼無(wú)人機(jī)研究進(jìn)展及關(guān)鍵技術(shù)淺析［J］.電光與控制，2010，17(10):46－52.

［9］劉乾，孫志鋒.基于ARM的四旋翼無(wú)人飛行器控制系統(tǒng)［J］.機(jī)電工程，2011，28(10):1237 －1240.