何甘寧，李立欣，高 昂，張會(huì)生

（西北工業(yè)大學(xué) 電子信息學(xué)院，陜西 西安 710129）

基于磁傳感器的串級(jí)PID自主控制四旋翼設(shè)計(jì)

何甘寧，李立欣，高 昂，張會(huì)生

（西北工業(yè)大學(xué) 電子信息學(xué)院，陜西 西安 710129）

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

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

四旋翼具有飛行穩(wěn)定、自由度多等特點(diǎn)，廣泛出現(xiàn)在軍事、農(nóng)業(yè)和民用等領(lǐng)域，具有很高的科研價(jià)值。同時(shí)作為一種無人機(jī)，自主飛行也是其系統(tǒng)技術(shù)的一項(xiàng)研究熱點(diǎn)[1]。

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

文中設(shè)計(jì)了基于磁傳感器的串級(jí)PID控制自主飛行系統(tǒng)。點(diǎn)到點(diǎn)的自主飛行分為兩個(gè)階段：自旋調(diào)整航向和直線飛行。根據(jù)GPS接收機(jī)提供的經(jīng)、緯度和磁傳感器提供的航向角，計(jì)算出目標(biāo)航向角和航線，作為外環(huán)PID控制量，計(jì)算出4個(gè)旋翼的轉(zhuǎn)速，再經(jīng)內(nèi)環(huán)PID控制使四旋翼做自旋運(yùn)動(dòng)，調(diào)整航向角為絕對(duì)航向角。再以航線為基準(zhǔn)，實(shí)際飛行中目標(biāo)航向角和偏航距為外環(huán)PID控制量，得出姿態(tài)參考值，經(jīng)內(nèi)環(huán)PID控制，實(shí)時(shí)調(diào)整飛行路線，從而以近似直線的路徑到達(dá)目標(biāo)點(diǎn)。此系統(tǒng)中，四旋翼只需單方向運(yùn)動(dòng)，加入串級(jí)PID使控制更加精確穩(wěn)定，并且飛行路線近似直線，使飛行變得更加高效。

1 系統(tǒng)結(jié)構(gòu)

本次四旋翼硬件平臺(tái)由以Atmega2560為核心的飛行控制系統(tǒng)、傳感器系統(tǒng)、電機(jī)控制系統(tǒng)等組成，其結(jié)構(gòu)如圖1所示。

圖1 硬件系統(tǒng)結(jié)構(gòu)圖

飛行控制系統(tǒng)是整個(gè)系統(tǒng)的核心，對(duì)傳感器獲取的數(shù)據(jù)進(jìn)行處理，解算得到4個(gè)電機(jī)的轉(zhuǎn)速，然后將此信息傳送給電調(diào)，控制電機(jī)的旋轉(zhuǎn)，從而實(shí)現(xiàn)對(duì)飛行姿態(tài)的控制。

2 自主飛行控制

2.1 數(shù)字PID控制器

模擬PID控制器的控制規(guī)律如下所示：

數(shù)字PID算法總體分為位置型控制算法和增量型控制算法[5]，本文采用的是位置型控制算法。為了實(shí)現(xiàn)數(shù)字PID算法，需將（1）式變成差分方程?？勺魅缦陆?/p>

從而離散化處理得

P參數(shù)的作用是減少穩(wěn)態(tài)誤差，讓飛機(jī)快速跟蹤輸入；D參數(shù)的作用是加快系統(tǒng)響應(yīng)，減小超調(diào)量，增加穩(wěn)定性，防止飛機(jī)左右晃動(dòng)；I參數(shù)的作用是可以消除靜態(tài)誤差，防止位置誤差的累積[8]。

2.2 自主航向控制

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

圖2 四旋翼從A到B飛行

而目標(biāo)航向角α的確定是四旋翼能自主且準(zhǔn)確地到達(dá)目標(biāo)B點(diǎn)的關(guān)鍵，為達(dá)到精確控制，采用雙閉環(huán)PID控制。外環(huán)PID控制器為位置導(dǎo)航級(jí)PID，根據(jù)外圍傳感器數(shù)據(jù)，計(jì)算得到四旋翼該如何飛行；內(nèi)環(huán)PID控制器為姿態(tài)控制級(jí)PID，以外環(huán)輸出和自身閉環(huán)回路的姿態(tài)反饋?zhàn)鳛檩斎肟刂齐姍C(jī)，使四旋翼按導(dǎo)航級(jí)計(jì)算得到的飛行計(jì)劃調(diào)整飛行。

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

2.3 直線飛行的航向修正

實(shí)際飛行中，要使四旋翼以近似直線路徑飛行，就必須根據(jù)偏航距和當(dāng)前航向角對(duì)飛行路線進(jìn)行修正。

如圖4所示，C點(diǎn)偏離預(yù)定航線，α′為當(dāng)前目標(biāo)航向角，d是C點(diǎn)的偏航距。將作為外環(huán)PID控制器的輸入量對(duì)航向進(jìn)行修正，其中α′+d×ξ為偏航修正因子，得到一航向參考值，再由內(nèi)環(huán)PID控制整機(jī)姿態(tài)，使四旋翼的位置慢慢向預(yù)定航線靠攏，從而實(shí)現(xiàn)四旋翼直線地飛往目標(biāo)點(diǎn)。

圖3 雙級(jí)閉環(huán)PID控制航向

圖4 航向修正示意圖

3 系統(tǒng)測(cè)試

四旋翼經(jīng)遙控器切換模式到自主飛行模式后，讀取事先存儲(chǔ)的目標(biāo)點(diǎn)的經(jīng)、緯度和高度信息，然后根據(jù)當(dāng)前自身與目標(biāo)的位置關(guān)系，規(guī)劃出路徑，從而能自主地飛行至目的地。

具體的測(cè)試結(jié)果如圖5所示，（a）中四旋翼處于懸停狀態(tài)，到圖（b）時(shí)航向做了一個(gè)角度的調(diào)整，使其朝向目標(biāo)點(diǎn)。 （b）（c）（d）圖是四旋翼的直線飛行過程，可以看出四旋翼飛行穩(wěn)定。

圖5 路徑規(guī)劃測(cè)試

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

圖6 實(shí)際與預(yù)定航線對(duì)比

圖7 偏航距的收斂

4 結(jié) 論

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

參考文獻(xiàn)：

[1]顏平，王麗丹，李夢(mèng)柯，等.基于STM32的四旋翼飛行器設(shè)計(jì)與實(shí)現(xiàn) [J].電子設(shè)計(jì)工程，2016（2）:187-189.

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

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

[4]鐘麗娜，王君浩，王融.基于磁傳感器的四旋翼飛行器自主導(dǎo)航設(shè)計(jì) [J].重慶理工大學(xué)學(xué)報(bào):自然科學(xué)版， 2013，27（12）:86-90.

[5]馮慶端，裴海龍.串級(jí)PID控制在無人機(jī)姿態(tài)控制的應(yīng)用[J].微計(jì)算機(jī)信息， 2009，25（22）:9-10.

[6]賈峰，孫曼.基于STM32的四旋翼飛行姿態(tài)串級(jí)控制[J].科學(xué)技術(shù)與工程， 2015，15（6）:220-225.

[7]王璞，鄧煒烽.四旋翼飛行器雙閉環(huán)串級(jí)PID姿態(tài)控制算法 [J].開封教育學(xué)院學(xué)報(bào)，2015，35（9）:91-93.

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

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

[10]張垚，鮮斌，殷強(qiáng)，等.基于ARM處理器的四旋翼無人機(jī)自主控制系統(tǒng)研究[J].中國(guó)科學(xué)技術(shù)大學(xué)學(xué)報(bào)， 2012，42（9）:753-760.

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

[12]夏偉，鄧靜.無人直升機(jī)自動(dòng)航線飛行控制律設(shè)計(jì)與仿真[J].直升機(jī)技術(shù)，2013（3）:21-25.

[13]楊立本，章衛(wèi)國(guó)，何墉，等.一種自主四旋翼飛行器控制系統(tǒng)結(jié)構(gòu)研究[J].飛行力學(xué)，2016，34（1）:59-62.

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

[15]羅誠(chéng).無人機(jī)路徑規(guī)劃算法研究[D].上海：復(fù)旦大學(xué)，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稿件編號(hào)：201607132

陜西省自然科學(xué)基礎(chǔ)研究計(jì)劃面上項(xiàng)目（2016JM6062）；中國(guó)航天科技集團(tuán)公司航天科技創(chuàng)新基金資助項(xiàng)目；西北工業(yè)大學(xué)研究生創(chuàng)意創(chuàng)新種子基金（Z2016106）

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