基于磁傳感器的串級PID自主控制四旋翼設計

何甘寧，李立欣，高 昂，張會生

（西北工業大學 電子信息學院，陜西 西安 710129）

基于磁傳感器的串級PID自主控制四旋翼設計

何甘寧，李立欣，高 昂，張會生

（西北工業大學 電子信息學院，陜西 西安 710129）

為實現四旋翼精確高效的自主飛行，提出一種基于磁傳感器的串級PID控制自主飛行系統。該系統硬件平臺采用GPS接收機提供經、緯度，磁傳感器提供航向角；軟件上采用串級PID控制，外環為位置導航級，內環為姿態控制級。首先根據四旋翼起始點位置與預置目標點的關系得到目標航向角和航線，同飛往目標點過程中的偏航角和偏航距作為外環PID控制輸入量，得到姿態的參考值，然后經內環PID控制使系統能跟隨參考姿態，實時修正航線，使四旋翼以近似直線的軌跡飛往目標點。該系統具有控制精確，計算量小，飛行路徑簡短，飛行穩定的特點。最終以自行搭建的四旋翼為平臺，驗證了該自主飛行控制方法的有效性。

磁傳感器；串級PID；四旋翼；自主飛行；GPS

四旋翼具有飛行穩定、自由度多等特點，廣泛出現在軍事、農業和民用等領域，具有很高的科研價值。同時作為一種無人機，自主飛行也是其系統技術的一項研究熱點[1]。

當今，世界上對四旋翼的研究主要是針對四旋翼飛行器的自主飛行，基于慣性導航、視覺導航等[5]。文獻[3]基于GPS做了四旋翼兩方向同時運動的自主飛行方案，其主要優點是無需通過自旋調整航向，但兩個方向速度的計算量較大，控制較為復雜。文獻[4]同樣是基于磁傳感器控制四旋翼自主飛行，但并未涉及串級PID控制，同時摘要雖提及航線修正，但文中沒有具體說明。

文中設計了基于磁傳感器的串級PID控制自主飛行系統。點到點的自主飛行分為兩個階段：自旋調整航向和直線飛行。根據GPS接收機提供的經、緯度和磁傳感器提供的航向角，計算出目標航向角和航線，作為外環PID控制量，計算出4個旋翼的轉速，再經內環PID控制使四旋翼做自旋運動，調整航向角為絕對航向角。再以航線為基準，實際飛行中目標航向角和偏航距為外環PID控制量，得出姿態參考值，經內環PID控制，實時調整飛行路線，從而以近似直線的路徑到達目標點。此系統中，四旋翼只需單方向運動，加入串級PID使控制更加精確穩定，并且飛行路線近似直線，使飛行變得更加高效。

1 系統結構

本次四旋翼硬件平臺由以Atmega2560為核心的飛行控制系統、傳感器系統、電機控制系統等組成，其結構如圖1所示。

圖1 硬件系統結構圖

飛行控制系統是整個系統的核心，對傳感器獲取的數據進行處理，解算得到4個電機的轉速，然后將此信息傳送給電調，控制電機的旋轉，從而實現對飛行姿態的控制。

2 自主飛行控制

2.1 數字PID控制器

模擬PID控制器的控制規律如下所示：

數字PID算法總體分為位置型控制算法和增量型控制算法[5]，本文采用的是位置型控制算法。為了實現數字PID算法，需將（1）式變成差分方程。可作如下近似

從而離散化處理得

P參數的作用是減少穩態誤差，讓飛機快速跟蹤輸入；D參數的作用是加快系統響應，減小超調量，增加穩定性，防止飛機左右晃動；I參數的作用是可以消除靜態誤差，防止位置誤差的累積[8]。

2.2 自主航向控制

如圖2所示，四旋翼從A點到B點自主飛行時，欲將機頭朝向目標點B，則需根據A點朝向和A、B兩點的位置關系，得到自身需要旋轉的角度α，即目標航向角。圖中X軸正方向指向地理的正東，Y軸正方向指向地理的正北，點A在坐標軸原點。

圖2 四旋翼從A到B飛行

而目標航向角α的確定是四旋翼能自主且準確地到達目標B點的關鍵，為達到精確控制，采用雙閉環PID控制。外環PID控制器為位置導航級PID，根據外圍傳感器數據，計算得到四旋翼該如何飛行；內環PID控制器為姿態控制級PID，以外環輸出和自身閉環回路的姿態反饋作為輸入控制電機，使四旋翼按導航級計算得到的飛行計劃調整飛行。

四旋翼當前A點的經、緯度及其機頭方向θ可以由GPS模塊（內置磁傳感器）獲得，目標B點的經、緯度事先寫入四旋翼的DataFlash中。然后由A、B兩點的經緯度的差值，得到B點相對A點的坐標，求出絕對航向角β及目標航向角α。然后通過串級PID算法求得對每個電機的調整量，傳遞給四個電機，從而改變電機轉速使四旋翼自旋一個α角度，實現對四旋翼航向角的雙級閉環回路精確控制，具體框圖如圖3所示。

2.3 直線飛行的航向修正

實際飛行中，要使四旋翼以近似直線路徑飛行，就必須根據偏航距和當前航向角對飛行路線進行修正。

如圖4所示，C點偏離預定航線，α′為當前目標航向角，d是C點的偏航距。將作為外環PID控制器的輸入量對航向進行修正，其中α′+d×ξ為偏航修正因子，得到一航向參考值，再由內環PID控制整機姿態，使四旋翼的位置慢慢向預定航線靠攏，從而實現四旋翼直線地飛往目標點。

圖3 雙級閉環PID控制航向

圖4 航向修正示意圖

3 系統測試

四旋翼經遙控器切換模式到自主飛行模式后，讀取事先存儲的目標點的經、緯度和高度信息，然后根據當前自身與目標的位置關系，規劃出路徑，從而能自主地飛行至目的地。

具體的測試結果如圖5所示，（a）中四旋翼處于懸停狀態，到圖（b）時航向做了一個角度的調整，使其朝向目標點。 （b）（c）（d）圖是四旋翼的直線飛行過程，可以看出四旋翼飛行穩定。

圖5 路徑規劃測試

圖6是由實際飛行獲取的位置數據所描繪的航線圖，自主飛行過程中，實際航線與預定航線幾乎重合。此外，從圖7可以看出，四旋翼的偏航距從水平飛行最開始的35 cm慢慢縮小在-10 cm和10 cm之間，可見利用磁傳感器和串級PID可很好地控制四旋翼直線飛行時的航向。但當快到達一個目標點時，會有較大的偏差，可能原因是臨近目標點時，利用GPS的位置關系求取目標航向角，角度偏差變大，從而導致航線誤差變大；當懸停轉向時，四旋翼因外部環境因素使得其位置一直處于變動狀態，使得在目標點處有許多不規則的軌跡數據。

圖6 實際與預定航線對比

圖7 偏航距的收斂

4 結 論

文中首先介紹了系統的硬件平臺——四旋翼的系統結構，其具體的組成部分及功能。然后設計了一種基于磁傳感器，并利用串級PID的雙閉環控制自主飛行方法，最后以自行搭建的四旋翼為平臺對自主飛行方法做了驗證，結果表明四旋翼有很好地自主飛行能力，而且飛行穩定，實際飛行路線近似直線，縮短了飛行路程，提高了飛行效率。

參考文獻：

[1]顏平，王麗丹，李夢柯，等.基于STM32的四旋翼飛行器設計與實現 [J].電子設計工程，2016（2）:187-189.

[2]黃軍友.基于PID控制的X模式四旋翼飛行器研究[J].電子設計工程， 2014，22（2）:127-129.

[3]馬浩.基于Backstepping的多旋翼無人機自主飛行控制系統研究[D].南京：南京信息工程大學，2014.

[4]鐘麗娜，王君浩，王融.基于磁傳感器的四旋翼飛行器自主導航設計 [J].重慶理工大學學報:自然科學版， 2013，27（12）:86-90.

[5]馮慶端，裴海龍.串級PID控制在無人機姿態控制的應用[J].微計算機信息， 2009，25（22）:9-10.

[6]賈峰，孫曼.基于STM32的四旋翼飛行姿態串級控制[J].科學技術與工程， 2015，15（6）:220-225.

[7]王璞，鄧煒烽.四旋翼飛行器雙閉環串級PID姿態控制算法 [J].開封教育學院學報，2015，35（9）:91-93.

[8]吳友謙，裴海龍.基于Dubins曲線的無人直升機軌跡規劃[J].計算機工程與設計，2011，32（4）:1426-1429.

[9]杜浩.基于GPS/INS的多旋翼MAVs自主飛行系統研制[D].南京：南京信息工程大學，2012.

[10]張垚，鮮斌，殷強，等.基于ARM處理器的四旋翼無人機自主控制系統研究[J].中國科學技術大學學報， 2012，42（9）:753-760.

[11]葉文偉，劉琨，李中健.基于ARM內核單片機的四旋翼直升機飛行控制系統設計[J].電子設計工程， 2013，21（13）:79-82.

[12]夏偉，鄧靜.無人直升機自動航線飛行控制律設計與仿真[J].直升機技術，2013（3）:21-25.

[13]楊立本，章衛國，何墉，等.一種自主四旋翼飛行器控制系統結構研究[J].飛行力學，2016，34（1）:59-62.

[14]甄紅濤，齊曉慧，夏明旗，等.四旋翼無人直升機飛行控制技術綜述 [J].飛行力學，2012，30（4）:295-299.

[15]羅誠.無人機路徑規劃算法研究[D].上海：復旦大學，2010.

Design of cascade PID autonomous control four-rotor based on magnetic sensor

HE Gan-ning，LI Li-xin，GAO Ang，ZHANG Hui-sheng
（School of Electronics and Information，Northwestern Polytechnical University，Xi’an 710129，China）

In order to realize the autonomous flight of the four-rotor accurately and efficiently，a cascade PID control autonomous flight system based on magnetic sensor is proposed.The hardware platform of the system uses GPS receiver to provide the latitude and longitude，and the magnetic sensor provides the heading angle.In the software， the cascade PID control is adopted， the outer loop is the position navigation stage， and the inner loop is the attitude control stage.First， according to the relationship between the starting point position of the four rotor and the preset target，the target heading angle and route are obtained，with the Yaw angle and yaw pitch in the process flying to the target point as outer loop PID control input.Get the reference value of attitude，and then through the inner loop PID control to make the system can follow the reference attitude， real-time correct the route， so that the four-rotor canfly to the target point in the approximate straight-line trajectory.The system has the characteristics of accurate control， small amount of calculation， short flight path and stable flight.Finally， to verify the effectiveness of the autonomous flight control method，a four-rotor is built.

magnetic sensor；cascade PID； four-rotor； autonomous flight；GPS

V216.8

A

1674－6236（2017）12-0068-04

2016-07-18稿件編號：201607132

陜西省自然科學基礎研究計劃面上項目（2016JM6062）；中國航天科技集團公司航天科技創新基金資助項目；西北工業大學研究生創意創新種子基金（Z2016106）

何甘寧（1992—），男，浙江金華人，碩士研究生。研究方向：通信與信息系統。