基于磁傳感器的四旋翼飛行器自主導航設計

鐘麗娜，王君浩，王 融

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

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

對于復雜環境的偵查、監視任務，需要四旋翼飛行器具有無人自主飛行功能。傳統四旋翼飛行器需要手控實現飛行、姿態控制［6］，無法適應復雜環境下無人自主飛行的要求。本文設計了一種利用低成本磁傳感器的自主飛行方法，通過與預定航線偏航的情況判斷，進一步調節飛控系統，實現航路規劃自主飛行功能。

1 四旋翼無人機飛行原理

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

圖1 四旋翼飛行器運動原理

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

2 系統總體設計

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

圖2 系統總體設計

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

1)主控芯片

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

2)陀螺儀及加速度計

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

3)磁傳感器

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

4)GPS接收機

GPS接收機采用Ublox公司生產的LEA6S接收機。該接收機跟蹤靈敏度高至－162 dBm，數據輸出頻率可達4 Hz.

3 基于磁傳感器的自主導航算法設計

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

磁傳感器朝向正北時輸出的角度為0，對應直角坐標系中的Y軸正方向，正東方向對應X軸正方向，正北方向對應Y軸負方向，正西方向對應X軸負方向。設定正北方向順時針旋轉的角度為航向角。此時，航點1和航點2的坐標就變成了(X1，Y1)和(X2，Y2)。由此可以計算出航點1和航點2的距離L:

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

1)當 X2＞X1，Y2＞Y1時，

2)當 X2＞X1，Y2＜Y1時，

3)當 X2＜X1，Y2＜Y1時，

4)當 X2＜X1，Y2＞Y1時，

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

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

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

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

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

對無人機的轉向控制方法如下:

1)當β2＜0時，無人機逆時針旋轉

2)當β2＞0時，無人機順時針旋轉

3)當β2=0時，無人機不旋轉。

4 實際飛行測試與分析

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

表1 PID參數設置

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

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

圖3 四旋翼無人機自動導航飛行圖

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

5 結束語

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

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

［2］谷永晟，楊建軍，朱宇虹.四旋翼無人飛行器導航控制系統研究［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］李浩濤.基于四旋翼無人機傳感器數據采集與處理的研究［D］.天津:天津大學，2012.

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

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

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