盧敬元，曾祥燁，王靜宜，王 楊

（河北工業(yè)大學 電子信息工程學院，天津 300401）

四旋翼飛行器的雙回路PID控制器的設計與實現(xiàn)

盧敬元，曾祥燁，王靜宜，王 楊

（河北工業(yè)大學 電子信息工程學院，天津 300401）

四旋翼飛行器是當今市場的一個新興且關注度高的研究方向．PID算法當屬四旋翼飛行器的核心部分，優(yōu)秀的PID算法可以實現(xiàn)穩(wěn)定的懸停和快速的打舵響應．針對傳統(tǒng)的單回路PID控制器的不足設計了一種新的雙回路 PID控制器，結構上分為主回路和副回路，角速度作為主回路，角度作為副回路，副回路的輸出量作為主回路的期望值．并在角度姿態(tài)的獲取上創(chuàng)新的加入了濾波系統(tǒng)，從而使副回路獲得的角度曲線更加穩(wěn)定和平滑，降低了電機的工作溫度，提高了控制精度．控制器采用了STM 32F103作為控制核心．該款處理器有配套的DSP庫，可以實現(xiàn)FIR濾波器,濾波參數(shù)確定于MATLAB的FIR設計工具．經(jīng)過上機實驗說明了新的濾波雙回路PID系統(tǒng)不僅懸停十分穩(wěn)定，對于打舵響應十分迅速，而且具備一定的抗外力干擾能力，滿足設計需求．

雙回路PID算法;有限長單位沖激響應濾波器;MATLAB；四旋翼；STM 32

0 引言

四旋翼飛行器具有體積小、重量輕、姿態(tài)靈活、擴展性強等優(yōu)勢，能夠很好的進行偵查、航拍、運送輕型貨物等任務，是當前小型無人飛行器的熱門研究方向．本文的控制系統(tǒng)使用的處理器為STM 32F103ZET6，該款內核的特點就是高性能、低功耗，適合用于四旋翼這類對功耗要求敏感的系統(tǒng)．

近些年隨著各種自動控制技術的發(fā)展與成熟，出現(xiàn)了很多在概念上更加先進的算法，但是由于四旋翼的結構決定著其搭載的處理器的高復雜度的浮點、矩陣運算的能力較弱，所以當前的四旋翼控制算法還是以PID和各種改進型PID算法為主．目前大部分四旋翼的開源項目采用的都是原始的單回路PID控制[1]，本文設計的雙回路PID算法輔以濾波器在穩(wěn)定性和可移植性上都強于傳統(tǒng)的單環(huán)PID算法．

1 四旋翼結構模型和基本原理

1.1 機體物理模型

四旋翼由4個電機和螺旋槳構成，從結構上主要分為兩大類：“X”型和“十”型，主要構成部件為飛行控制器、遙控接收機、電機、電子調速器等等．本文主要介紹“X”型[2]，其結構圖如圖1所示，延順時針方向分別是1、2、3、4號電機，為克服機體自轉采用了不同轉向的旋槳，其中對角旋漿轉向相同，相鄰旋漿轉向相反，正前方為機頭方向．其中繞X軸旋轉的為俯仰角，繞Y軸旋轉的為橫滾角，繞 Z軸旋轉的為偏航角[3]．

圖1 四旋翼基本結構Fig.1 The structure of the four-rotor aircraft

4個電機分為2組，控制機體的3個基本姿態(tài)角：Roll，Pitch，Yaw．機體的所有姿態(tài)由這3種姿態(tài)角線性組合而成．姿態(tài)角、機體姿態(tài)和電機輸出對應如表1，因此在編寫飛行控制程序對機體進行姿態(tài)控制時，需要3組參數(shù)分別控制Roll、Pitch、Yaw這3個姿態(tài)角．表1中的1～4分別代表1～4號電機，“+”為電機的等量轉速提高；“ ”為電機的等量轉速降低．

表1 姿態(tài)電機輸出對應表Tab.1 The results of motor output

1.2 飛行控制系統(tǒng)模型

飛行控制系統(tǒng)模型[4]可以簡單表示為圖2，其中Moto1～Moto4表示1～4號電機的PWM控制信號[5]．在姿態(tài)融合時，主要依靠陀螺儀的角加速度來計算姿態(tài)角，由于陀螺儀長時間工作會產(chǎn)生漂移，故用加速度計輔以重力加速度對陀螺儀進行校正．但是在對偏航角（Yaw）計算時，由于Z軸垂直于水平面，加速度計無法校正，所以選擇使用地磁傳感器對Yaw進行專門校準．

圖2 飛控系統(tǒng)框圖Fig.2 Flight control system

1.3 飛行器的力學模型

飛行器在飛行過程中的總升力[6]來自于4個槳產(chǎn)生的升力之和，故升力Fi可以表示為

式中：i為第i個螺旋槳；Fi為升力，單位為牛頓（N）； 為當前空氣密度，單位為kg/m3；CT為阻力系數(shù)；A為螺旋槳旋轉之后的面積，單位為m2；為電機轉速，單位為rad/s；R為螺旋槳的半徑，單位為m．在本次試驗中采用的是4套相同規(guī)格的電機和槳，因此Ai、 、Ri近似相等，可簡化為用常量ki表示，故Fi可以表示為

由表1可知，在X形的四軸飛行器中，飛行姿態(tài)是由3個基本角決定的．將其在力學模型中加以分析，姿態(tài)角產(chǎn)生的實質就是由于2組槳的不同轉速而產(chǎn)生的力矩差．以橫滾角（Roll）為例，橫滾力矩T為

式中：l為電機到飛行器重心的距離，單位為m．

飛行姿態(tài)的控制是在油門轉速的基礎上對4個電機進行轉速的二次調整，所以每個電機的實際轉速可以表示為油門轉速0和調整轉速，姿態(tài)角的控制實際是在一個連續(xù)的時刻對不同的角加速度的控制，以橫滾角為例，使橫滾角的旋轉軸為X軸，繞X軸轉動的角加速度可以表示為

式中：ax表示繞X軸轉動的角加速度，單位為rad/s2；Ixx表示繞X軸的轉動慣量，單位為kg/m2．在離散控制系統(tǒng)中，某一特定時刻的四旋翼飛行器的姿態(tài)變化取決于a在時間T上的二重積分．

2 雙回路PID算法

飛行控制器的軟件方面核心是PID控制器．PID具有結構簡單、原理易懂、不需精確的系統(tǒng)模型等優(yōu)點．由于飛行器的結構多變且不規(guī)則，所以不能得到準確的機體數(shù)學模型，這也導致了對每一機體的結構和參數(shù)需要根據(jù)經(jīng)驗來選取，這時PID技術即是最優(yōu)選擇，PID控制器的機制就根據(jù)系統(tǒng)的期望值和測量值之間的誤差值通過比例、積分、微分的運算得到一個控制量來修正誤差．

在PID控制器中，使用最為廣泛和經(jīng)典的當屬單回路PID控制系統(tǒng)，其3個字母分別代表比例（proportion）、積分（integration）、微分（differentiation）．在四旋翼系統(tǒng)下期望值為遙控器輸出的期望角度，測量值為姿態(tài)傳感器輸出的角度數(shù)據(jù)，輸出值為控制電機轉速的PWM信號．

傳統(tǒng)的單回路PID在對于大型四旋翼飛行器的控制能力不足，例如在角度控制的PID系統(tǒng)在進行微小變化時并不敏感，而如果加大參數(shù)又會使在處理較大誤差時極易產(chǎn)生超調．此時角速度的變化最能反映瞬間的機體的姿態(tài)變化，在此設計一種針對于傳統(tǒng)單回路PID算法的改良算法：將2個單回路PID進行串聯(lián)，使用角速度來處理姿態(tài)變化，但是輸入量仍為角度的雙回路PID控制算法．

2.1算法基本原理

雙回路PID算法流程[7]如圖3所示，雙回路PID輸出量為電機的轉速信號，輸入量為實時的角度和角速度．雙回路PID主要分為主回路和副回路，主回路為角速度控制，副回路為角度控制．副回路主要負責劃定大概的調節(jié)范圍，然后由主回路來進行精確的角度實現(xiàn)．

圖3 雙回路PID算法流程圖Fig.3 Double-loop PID controller

2.2 參數(shù)調試

雙回路PID算法由2套PID參數(shù)構成，其調試方法和順序與普通PID有較大差異，參數(shù)意義和調試順序如下：

1）主回路P：修正誤差的主要參數(shù)，直接決定打舵相應．

2）主回路D：加快打舵相應，抑制P產(chǎn)生的回中震蕩．

3）主回路I：在一定范圍內抑制PD產(chǎn)生的超調效應．

在確定一個合適的P之后加入D，此時易產(chǎn)生震蕩，需要適當提高P以匹配D，合理的PD值中的P往往要大于初始P值．主回路參數(shù)調試時要去掉副回路，使期望歐拉角直接作為主回路的期望值，調試搖桿手感和打舵響應速度．調整到合適打舵響應之后重新接入副回路：

4）副回路P：期望角的放大倍數(shù)，使傳入主回路的期望值能與主回路的PID算法能較好匹配．

相對于成熟的單回路PID算法，雙回路PID算法在參數(shù)調試方面的復雜性主要存在于主、副回路的參數(shù)匹配，在兩回路綜合調試效果不佳時，不易確定問題回路，且在多姿態(tài)角協(xié)同調試時復雜度較高．雙回路PID目前還沒有比較成熟確定參數(shù)的數(shù)學模型，主要使用經(jīng)驗試湊法．

2.3 油門信號計算

本文采用位置式控制，即輸出量直接為最終的電機轉速，位置式的表達式[8]

其中：e(t)為在t時刻的期望值和測量值之差，稱之為誤差；kp為比例系數(shù)；ki為積分系數(shù)；kd為微分系數(shù)；u(t)為t時刻的輸出量．在實際使用中，可表示為時間段的累加，可表示為2次誤差值之差，將副回路輸出命名為ANGELout(t)，此時副回路的輸出可表示為

根據(jù)圖3可知，主回路的輸入量為eRate(t)=ANGELout(t)當前角速度，由此主回路的輸出可表示為

其中kpRate、kiRate、kdRate為主回路的比例、積分、微分系數(shù)．雙回路PID相應的也有兩套PID參數(shù)需要調整．

需注意的是由ANGELRateout(t)求出的是電機的調整轉速，電機輸出時還需加上基礎的油門轉速．最終進行油門信號輸出之前需要對油門信號進行限幅，防止過沖失控．電機的實際輸出可表示為式 (4)

式中POWER是基礎的油門信號，由遙控器撥桿直接給出．由式(3)可以分別求出俯仰角的調整值PITCHout、橫滾角的調整值ROLLout、偏航角的調整值YAWout．

2.4算法測試

實驗平臺[9]為對角電機孔距450mm的機架，姿態(tài)角和角速度的更新頻率約為200Hz，PID算法的控制頻率約為50 Hz，期望角恒為0，即讓機體處于懸停自穩(wěn)狀態(tài)，將機體的3個姿態(tài)角中的2個固定住，單獨調試一個姿態(tài)角，圖4所示．

將姿態(tài)角通過藍牙模塊上傳到上位機進行精確的角度分析，如圖5所示，上位機軟件下半部分為機體實時角度，上半部分為電機實時轉速，可見圖5在自穩(wěn)狀態(tài)下，機體的穩(wěn)定度較高，超調量很小，系統(tǒng)能很快進入自穩(wěn)狀態(tài)．但是由于雙回路的電機轉速的變化是取決于角速度變化，在高轉速時，電機的震動會使角速度產(chǎn)生大幅震蕩，導致電機轉速變化非常不線性，經(jīng)常會在轉速高頻率、大幅度改變的情況下工作，電機在短時間內大量發(fā)熱，這種高溫工作狀態(tài)下容易導致電機失磁損壞．解決方案是在姿態(tài)獲取時設計FIR濾波器消除震動干擾，平滑電機的控制曲線．

圖4 測試機架Fig.4 Structural testing

圖5 雙回路PID圖像Fig.5 The image of double-loop PID controller

3 FIR濾波器

FIR的設計主要有窗函數(shù)法、頻率采樣法、等波紋最佳逼近法等，本文采用窗函數(shù)設計法．在濾波器的實際實現(xiàn)中，由于是用一個有限長的序列h(n)代替理想濾波器中的h_d(n)，肯定會引起誤差，產(chǎn)生吉布斯（Gibbs）效應，也稱為截斷效應，使阻帶的衰減小，從而滿足不了技術上的要求．此時就要根據(jù)各種窗函數(shù)的特性和應用場景來選擇合適的窗函數(shù)來減小截斷效應，設計一個能滿足技術要求的FIR線性相位濾波器．

典型的窗函數(shù)有：矩形窗、三角形窗、漢寧窗、哈明窗、布萊克曼窗等．本次根據(jù)未濾波數(shù)據(jù)中的陀螺儀取值計算了機身最大晃動頻率、主要干擾來源，并在MATLAB根據(jù)各個窗的h(n)選擇了矩形窗．

3.1 基于STM 32F10x DSP庫的FIR實現(xiàn)

ST公司對于STM 32F10x有配套的DSP庫[10]實現(xiàn)豐富功能，本文采用的是其2.0版本，可以進行PID、FFT、FIR等多種操作．其中：FIR庫函數(shù)的函數(shù)體、輸入輸出參數(shù)如表2所示．

表2 fir_16by16_stm32函數(shù)說明Tab.2 The function description of fir_16by16_stm32

在使用該FIR函數(shù)時應注意以下幾點[11]：

1）為了提高芯片效率，根據(jù)直接性FIR結構特性，在定義輸入輸出數(shù)組長度時應滿足輸出數(shù)組的長度應為輸出數(shù)組的長度與濾波器系數(shù)的個數(shù)之差．

2）根據(jù)表2可知，此函數(shù)是由2個16位（short型）數(shù)卷積得到32位（int型）結果．在選取輸入函數(shù)是要注意16位數(shù)的范圍，即使輸入數(shù)組內的元素滿足．

3）由于STM 32系列只支持整型運算，而本文中無論是MATLAB得到的濾波器參數(shù)還是待濾波的陀螺儀角度都為浮點型，若強行運算會極大的降低運算效率增加時延，所以要在誤差可接受的前提下選取適當?shù)姆椒▽?組參數(shù)進行整型轉化．

3.2 濾波器設計

濾波器參數(shù)特點是中間部分的參數(shù)權重較高，當輸入陀螺儀角度發(fā)生較大變化，在經(jīng)過大概（階數(shù)/2）個周期過后輸出值才會有明顯的變化．本系統(tǒng)的運行頻率大概為50 Hz，故時延主要來自于每周期的參數(shù)傳遞而非卷積計算時間．

針對以上系統(tǒng)特性，選取濾波器時就更加傾向低階，在MATLAB中用FDA Tool對濾波器進行設計[12]，計算了機體擺動頻率后得到設計截止頻率為4 Hz，選取了10階矩形窗濾波器，其衰減特性比較符合使用需要．衰減特性如圖6所示．

圖6 矩形窗衰減特性Fig.6 The attenuation characteristics of rectangular window

3.3 實驗結果分析

FIR濾波器實驗與PID實驗采用相同的機架平臺，陀螺儀采用MPU6050，配合2.4 GHz的遙控裝置．數(shù)據(jù)波形采集于油門開度50%階段，將飛行器固定在平地上，此時飛行器位于離地臨界狀態(tài)．在當前油門開度下，由于飛行器被固定在水平地面上，所以現(xiàn)在的角度變化都為電機震動疊加產(chǎn)生的，將角度變化圖像放大，如圖7，可見角度數(shù)據(jù)以一個小且快的速率變化，此時的角速度也會高頻大幅度的震蕩，從而導致了電機轉速的震蕩．

圖7 未濾波角度Fig.7 Unfiltered angle

在系統(tǒng)中加入剛才設計好的FIR濾波器，相同的油門開度下將角度曲線放大，如圖8，可見之前因為電機震動產(chǎn)生的抖動已經(jīng)基本消除．此時的陀螺儀采樣延時實測可接受．

圖8 濾波后角度Fig.8 Filtered angle

4 雙回路PID和FIR濾波器結合調試

將調試好的FIR濾波器加入到測量歐拉角和測量0角速度之后，依舊用圖4的調試方法，在上位機得到圖9．

圖9 濾波后圖像Fig.9 Filtered image

將圖9與圖5對比，可見在雙回路PID算法中加入FIR濾波器效果十分顯著，在自穩(wěn)性影響不大的情況下大幅度的削減了電機震蕩，電機發(fā)熱也大大減小，由此證明FIR濾波器對于雙回路PID的優(yōu)化十分有效．

將機體初始橫滾角傾斜20°，期望角為0°，迅速推起油門至50%，得到姿態(tài)曲線如圖10，可見橫滾角立刻能回歸零點，而且?guī)缀鯖]有多余震蕩，超調量極小，穩(wěn)定且快速．在期望角設置為20°時對機體進行快速打舵，姿態(tài)曲線如11圖，可見機體能很好的執(zhí)行打舵指令，在快速連續(xù)變相打舵時也不會產(chǎn)生超調而失控．在機體懸停穩(wěn)定時人工引入外部干擾，姿態(tài)曲線如圖12，在外部產(chǎn)生干擾時機體能迅速反應而且迅速恢復懸停狀態(tài)，幾乎沒有超調，穩(wěn)定性較好．

圖10 自穩(wěn)姿態(tài)圖像Fig.10 The image of automatic stability

圖11 打舵姿態(tài)圖像Fig.11 The image of helm response

圖12 引入外部干擾圖像Fig.12 The image of introducing external interference

5 結束語

本文設計了基于STM 32F103平臺的四旋翼飛行控制器，使用雙回路PID算法，并在其中加入FIR濾波器，有效提高了四旋翼機體的自穩(wěn)性能和打舵響應速度，而且在FIR濾波器的優(yōu)化下顯著減小了電機負荷，有效的保護電機，同時也提高了平臺的可移植性，增強了系統(tǒng)的魯棒性．在經(jīng)過對兩回路PID參數(shù)整定仔細的后，本文設計的飛行控制系統(tǒng)可以很好的實現(xiàn)飛行器響應遙控信號進行飛行動作．

[1]Atheer L Salihl，Moghavvemi M，Haider AF Mohamed．Flight PID Controller Design for a UAVQuadrotor[C]//Scientific Research and Essays，2010．

[2]趙世榮，孫運強，石喜玲．基于信息融合四旋翼飛行器穩(wěn)定性改進設計 [J]．電子器件，2015，38（6）：1417-1420．

[3]劉峰，呂強，王國勝，等．四軸飛行器姿態(tài)控制系統(tǒng)設計 [J]．計算機測量與控制，2011，（3）：583-585．

[4]葉樹球，詹林．基于PID的四旋翼飛行器姿態(tài)控制系統(tǒng) [J]．計算機與現(xiàn)代化，2015（5）：117-120

[5]李秀英，劉彥博．基于PWM的四旋翼飛行器控制方法 [J]．吉林大學學報（信息科學版），2011，29（5）：464-472．

[6]劉浩蓬，龍長江，萬鵬，等．植保四軸飛行器的模糊PID控制 [J]．農(nóng)業(yè)工程學報，2015，31（1）：71-77

[7]李俊，李運堂．四旋翼飛行器的動力學建模及PID控制 [J]．遼寧工程技術大學學報（自然科學版），2012，31（1）：114-117．

[8]郭曉鴻，楊忠，楊成順，等．一種基于STM 32的四旋翼飛行器控制器 [J]．應用科技，2011，38（7）：35-40．

[9]韓志鳳，李榮冰，劉建業(yè)，等．小型四旋翼飛行器試驗平臺設計 [J]．測控技術，2013，32（10）：121-124．

[10]STMicroelectronics．UM 0585User manual[Z]．Swiss：STMicroelectronics，2010．

[11]龍吉．一種在STM 32應用FIR濾波器的設計與實現(xiàn) [J]．吉林化工學院，2015（10）：262-262．

[12]王春興，張彬．節(jié)點文獻基于Matlab的FIR數(shù)字濾波器設計 [J]．現(xiàn)代電子技術，2011，34（18）：83-85．

[責任編輯 代俊秋]

Design and implementation of double-loop PID controller used in four-rotor aircraft

LU Jingyuan，ZENG Xiangye，WANG Jingyi，WANG Yang

(School of Electronics and Information Engineering,Hebei University of Technology,Tianjin 300401,China)

Four-rotor aircraft is an emerging research and drawing more interests in the modern market.The core of the PID algorithm is undoubtedly the four-rotor aircraft,excellent PID algorithm can achieve stable hover and rapid response helm.Aiming at the shortcomings of traditional single-loop PID controller,we design anew double-loop PID controller. It is divided into main loop and secondary loop structure,the angular velocity as the main loop,the angle as the secondary loop,the output of the secondary loop as the main loop of expectations.A FIR filter is designed to reduce high-frequency interference and motor temperature and improve the control precision.The STM 32F103 is adopted to be the control core of the Four-rotor aircraft.Before we use the matched DSP library of the STM 32F103 to achieve FIR filter,we debug the related parameters of the FIR filter by means of MATLAB.Machine experiment illustrates the new double-loop PID filtering system not only makes the hover stable and the helm res ponse rapid,but also under control,to meet the design requirements．

double-loop PID;FIR;MATLAB;four-rotor aircraft;STM 32

TP13

A

1007-2373(2016)05-0016-07

10.14081/j.cnki.hgdxb.2016.05.003

2016-10-25

河北省自然科學基金（F2015202331）

盧敬元（1990-），男（漢族），碩士生．通訊作者：曾祥燁（1979-），男（漢族），講師，博士．