鐘麗娜，王君浩，王 融

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

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

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

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

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

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

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

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

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

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

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

1)主控芯片

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

2)陀螺儀及加速度計

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

3)磁傳感器

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

4)GPS接收機(jī)

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

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

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

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

根據(jù)位置判斷，以正北方向為參照的絕對航向角β1分別取值如下:

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

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

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

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

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

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

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

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

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

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

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

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

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

4 實際飛行測試與分析

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

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

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

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

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

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

5 結(jié)束語

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

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

［2］谷永晟，楊建軍，朱宇虹.四旋翼無人飛行器導(dǎo)航控制系統(tǒng)研究［J］.遙測遙控，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］李浩濤.基于四旋翼無人機(jī)傳感器數(shù)據(jù)采集與處理的研究［D］.天津:天津大學(xué)，2012.

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

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

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