基于串級PID四旋翼飛行器控制系統(tǒng)設(shè)計(jì)與實(shí)現(xiàn)*

肖秋霞，池慶，黃昌文，陳冠華

（廈門理工學(xué)院電氣工程與自動(dòng)化學(xué)院，福建 廈門 361024）

當(dāng)前我國智能制造產(chǎn)業(yè)正處于新一輪技術(shù)革命的10年周期起點(diǎn)上，而無人飛行器特別是四旋翼飛行器站在了這場變革的前沿，越來越受到人們的關(guān)注。進(jìn)入新世紀(jì)以來，我國無人飛行器產(chǎn)業(yè)得到了空前發(fā)展，不同于傳統(tǒng)的固定翼飛行器，四旋翼飛行器不僅體積小、而且質(zhì)量輕，更重要的是有著對起飛場地要求不高、零部件容易更換等一系列突出優(yōu)勢。

基于傳統(tǒng)PID控制理念本文設(shè)計(jì)了飛行器閉環(huán)調(diào)速控制系統(tǒng)，通過內(nèi)外環(huán)串級PID控制電機(jī)轉(zhuǎn)速從而改變相應(yīng)旋翼產(chǎn)生的升力并最終達(dá)到飛行器飛行姿態(tài)控制的目的。首先，在MATLAB中搭建閉環(huán)調(diào)速系統(tǒng)仿真模型并得到相應(yīng)變量輸出曲線，在理論層面驗(yàn)證本文所提方案的可行性。其次，在對飛行器硬件平臺(tái)進(jìn)行整體設(shè)計(jì)的同時(shí)對其組成部件進(jìn)行選型與組裝，同時(shí)進(jìn)行調(diào)速系統(tǒng)原理圖繪制工作，為系統(tǒng)軟件實(shí)現(xiàn)做好準(zhǔn)備。最后，編寫閉環(huán)調(diào)速系統(tǒng)軟件相應(yīng)算法以及進(jìn)行飛行器試飛調(diào)試，在實(shí)際調(diào)試中驗(yàn)證整體方案可行。本文的研究為四旋翼飛行器在自動(dòng)飛行領(lǐng)域的擴(kuò)展提供了基礎(chǔ)。

1 飛行器的調(diào)速系統(tǒng)控制原理

飛行器飛行時(shí)以直流無刷電機(jī)作為驅(qū)動(dòng)源，所需全部驅(qū)動(dòng)力由螺旋槳提供。飛行控制板通過采集外設(shè)傳感器模塊反饋來的數(shù)據(jù)并通過無刷電機(jī)驅(qū)動(dòng)器將其輸入到四個(gè)無刷電機(jī)中，因4個(gè)電機(jī)固定在各旋翼軸的頂點(diǎn)處，各旋翼的傾斜角、螺距角均相同因此要實(shí)現(xiàn)飛行器姿態(tài)控制只能通過改變電機(jī)轉(zhuǎn)速使得旋翼升力改變來獲得。在無人飛行器領(lǐng)域中飛行器的運(yùn)動(dòng)狀態(tài)通過機(jī)體坐標(biāo)系(o-xyz)中沿著坐標(biāo)軸方向的平動(dòng)和繞著坐標(biāo)軸的轉(zhuǎn)動(dòng)來描述。其中平動(dòng)運(yùn)動(dòng)有前后運(yùn)動(dòng)、左右運(yùn)動(dòng)和升降運(yùn)動(dòng)；轉(zhuǎn)動(dòng)運(yùn)動(dòng)有橫滾運(yùn)動(dòng)、俯仰運(yùn)動(dòng)和偏航運(yùn)動(dòng)，各自形成的姿態(tài)角分別稱為橫滾角、俯仰角和偏航角。

在目前的工業(yè)領(lǐng)域里PID控制是最常用的控制算法之一。圖1為控制器原理框圖，其中e(t)為給定的期望值；d(t)是控制輸入偏差，作為PID控制器的輸入；u(t)為PID控制器的輸出；y(t)是控制對象的實(shí)際控制輸出量。PID控制的原理是通過偏差計(jì)算得到給定期望與實(shí)際輸出之間的差值，這個(gè)差值輸入到控制器中經(jīng)變換得到一個(gè)輸出值作用于對控制對象，同時(shí)這個(gè)輸出值又返回控制器輸入端進(jìn)行再一次的偏差調(diào)節(jié)，最終使得整個(gè)控制系統(tǒng)的輸出值達(dá)到或保持在期望值附近，從而達(dá)到穩(wěn)定控制的效果。

圖1 PID控制器原理框圖

2 數(shù)學(xué)建模仿真平臺(tái)搭建

2.1 數(shù)學(xué)建模

四旋翼飛行器有4個(gè)輸入、6個(gè)自由度的輸出，對其進(jìn)行數(shù)學(xué)建模之前必須首先確立2種坐標(biāo)系：地理坐標(biāo)系（O-XYZ）和機(jī)體坐標(biāo)系（o-xyz）。(OXYZ)的作用是確立重心坐標(biāo)（x,y,z）在地理坐標(biāo)系中的坐標(biāo)值從而用來體現(xiàn)飛行器在地理空間中的具體位置；(o-xyz)的作用則是用來確定飛行器飛行時(shí)兩坐標(biāo)系之間形成姿態(tài)角從而用來描述相應(yīng)的飛行狀態(tài)。建模時(shí)用6個(gè)相互獨(dú)立的坐標(biāo)變量(x，y，z，α，β，γ)來描述飛行器的運(yùn)動(dòng)。其中飛行器機(jī)體在地理空間中的具體位置通過平動(dòng)坐標(biāo)變量ξ=(x，y，z)來體現(xiàn)，轉(zhuǎn)動(dòng)坐標(biāo)變量η=(α，β，γ)則用來描述飛行器具有的飛行姿態(tài)。確定飛行器在地理空間中的具體位置時(shí)通過歐拉角法計(jì)算兩坐標(biāo)系間的旋轉(zhuǎn)變換矩陣的方式來獲得。

仿真模型如圖2所示：

圖2 無刷電機(jī)SIMULINK 仿真模型

圖3 空間坐標(biāo)系各坐標(biāo)軸姿態(tài)角變化速率曲線

2.2 PID閉環(huán)調(diào)速系統(tǒng)外環(huán)控制

外環(huán)控制的輸入量為通過偏差計(jì)算得到給定期望與飛行器模型的實(shí)際輸出兩者之差，外環(huán)控制的輸出為水平面上橫滾、俯仰角位移和飛行器總升力U1。

2.3 PID閉環(huán)調(diào)速系統(tǒng)內(nèi)環(huán)控制

內(nèi)環(huán)控制輸入量為外環(huán)控制輸出量即飛行器各旋翼產(chǎn)生的總升力和機(jī)體坐標(biāo)系(o-xyz)各坐標(biāo)軸上的角速度ang_xs，ang_ys，ang_zs，各軸的角速度分量經(jīng)分配后作用到各無刷電機(jī)中實(shí)現(xiàn)預(yù)期的控制效果。利用上述串級PID的內(nèi)外環(huán)控制算法在simulink中完成四旋翼飛行器控制系統(tǒng)搭建，經(jīng)過仿真，得到圖3所示仿真結(jié)果。仿真結(jié)果表明本文所設(shè)計(jì)的閉環(huán)調(diào)速系統(tǒng)能保證四旋翼飛行器的飛行穩(wěn)定，并且在外界有一定程度的干擾時(shí)仍能保持自穩(wěn)，達(dá)到了姿態(tài)控制的目的，各參數(shù)具體數(shù)值表1所示。

表1 各PID控制器參數(shù)具體數(shù)值

3 飛行器的調(diào)速系統(tǒng)硬件結(jié)構(gòu)平臺(tái)

本文所設(shè)計(jì)的閉環(huán)調(diào)速系統(tǒng)能保證四旋翼飛行器的飛行穩(wěn)定，并且在外界有一定程度的干擾時(shí)仍能保持自穩(wěn)，達(dá)到了姿態(tài)控制的目的。四旋翼閉環(huán)調(diào)速系統(tǒng)硬件平臺(tái)搭建，它是檢驗(yàn)建模仿真結(jié)果可行性的驗(yàn)證環(huán)節(jié)，同時(shí)也為軟件平臺(tái)搭建提供實(shí)物基礎(chǔ)。飛行器硬件平臺(tái)整體框架如圖4所示,工作時(shí)MCU對遙控器發(fā)來的PWM控制信號進(jìn)行接收與處理，同時(shí)通過C接口采集各外設(shè)傳感器模塊的數(shù)據(jù)。

圖4 飛行器硬件平臺(tái)框架

在硬件平臺(tái)包括下列各個(gè)器件選型：

⑴ 機(jī)架結(jié)構(gòu)：四旋翼飛行器旋翼軸選用尼龍加碳纖維復(fù)合材料，強(qiáng)度高耐摔適合多次試飛調(diào)試；無刷電機(jī)選用新西達(dá)2212/1400KV系列，其中第一個(gè)數(shù)字代表電機(jī)轉(zhuǎn)子直徑與高度，即線圈外徑22 mm，長度12 mm，第二個(gè)數(shù)字代表當(dāng)外界供給電壓值為1 V時(shí)電機(jī)的空轉(zhuǎn)轉(zhuǎn)速為1400 r/min；螺旋槳選用8045全碳纖維系列，突出優(yōu)點(diǎn)是質(zhì)量輕且韌度大，保證四旋翼飛行器飛行穩(wěn)定。

⑵ 微處理器(MCU)：微處理器理念于上世紀(jì)70年代提出，是一種集成度高的整合多種功能的單芯片微型計(jì)算機(jī)。本文MCU最終STM32F407VGT6高性能信號控制器核心板系列。

⑶ 外設(shè)傳感器模塊：由于飛行器機(jī)體的載重及放置空間有限，因此所選擇的傳感器模塊體積與質(zhì)量不能過于龐大，故將范圍確定在微電子機(jī)械系統(tǒng)(Micro Electro Mechanical System，MEMS)慣性傳感器范圍內(nèi)。表2為最終選擇的外設(shè)傳感器模塊。

STM32 最小系統(tǒng)板由電源、時(shí)鐘、復(fù)位、JTAG下載、啟動(dòng)5 部分電路組成，是指讓STM32 芯片能正常工作所需的最基本組成電路。

電源電路進(jìn)行降壓轉(zhuǎn)化，將高電平5 V信號轉(zhuǎn)化成低電平3.3 V信號用來給最小系統(tǒng)板核心芯片供電，同時(shí)在高電平端并入一個(gè)二極管為的是防止出現(xiàn)電源正負(fù)極反接而燒壞最小系統(tǒng)板的情況；時(shí)鐘電路由外接的晶振及電容組成，作為最小系統(tǒng)板各組成電路正常工作的驅(qū)動(dòng)力通過晶振產(chǎn)生時(shí)鐘周期使最小系統(tǒng)板執(zhí)行操作；復(fù)位電路連接最小板核心芯片上的NRST管腳，由電容、電阻、按鍵帽組成。上電時(shí)給核心芯片發(fā)送信號讓其開始執(zhí)行程序直至斷電，同時(shí)電阻給電容充電此時(shí)NRST之間無高電平電壓信號，斷電復(fù)位時(shí)通過按鍵帽按壓重新對核心芯片NRST管腳供電；JTAG下載電路用于調(diào)試及燒錄仿真程序；啟動(dòng)電路BOOT0/BOOT1模式，具體模式的選擇是通過跳線帽在3.3V和接地端間的引接來完成。

4 飛行器的調(diào)速系統(tǒng)軟件設(shè)計(jì)

四旋翼飛行器閉環(huán)調(diào)速系統(tǒng)軟件部分由主程序和中斷程序2部分組成，下圖5為閉環(huán)系統(tǒng)軟件整體框架。四旋翼飛行器系統(tǒng)對實(shí)時(shí)控制要求很高，因此必須設(shè)定好系統(tǒng)響應(yīng)時(shí)間，故采用5 ms更新一次的中斷處理作為整個(gè)系統(tǒng)的時(shí)間基準(zhǔn)，所有的控制算法都在中斷時(shí)間里分時(shí)段完成，主函數(shù)的作用則是進(jìn)行中斷優(yōu)先組別設(shè)置、外設(shè)傳感器模塊初始化以及接收遙控器傳來的姿態(tài)控制指令。

圖5 程序控制流程圖

表2 外設(shè)傳感模塊產(chǎn)品參數(shù)

系統(tǒng)工作時(shí)主程序首先進(jìn)行初始化環(huán)節(jié)，而后通過期望值輸入進(jìn)入PID控制器調(diào)節(jié)得到實(shí)際值輸出，微處理器MCU對實(shí)際值進(jìn)行PWM 調(diào)制最終作用于電機(jī)進(jìn)行調(diào)速。同時(shí)輸出值經(jīng)PID閉環(huán)控制反饋回輸入端及中斷處理程序中，實(shí)現(xiàn)電機(jī)閉環(huán)調(diào)速功能；定時(shí)中斷處理實(shí)時(shí)更新各外設(shè)傳感器模塊測量數(shù)據(jù)以及對各傳感器測量數(shù)據(jù)進(jìn)行融合并輸入到PID控制器中。

4.1 初始化環(huán)節(jié)

閉環(huán)調(diào)速系統(tǒng)初始化環(huán)節(jié)包含了2部分，分別是微處理器(MCU)內(nèi)部模塊的初始化以及外設(shè)傳感器模塊的初始化。其中串口初始化、C接口和GPIO接口初始化、定時(shí)器初始化、PWM輸入捕捉、輸出初始化共同組成MCU內(nèi)部模塊初始化；外設(shè)傳感器模塊的初始化主要是三維角度傳感器MPU6050初始化和電子羅盤AK8975初始化。串口初始化是上位機(jī)通信與SPL06模塊讀取數(shù)據(jù)所需，上位機(jī)通信的實(shí)時(shí)性要求高故波特率采用115200 bps，SPL06波特率設(shè)置較低，為9600 bps；GPIO用來控制指示燈；PWM輸入捕獲用來捕捉遙控器初始化信號，PWM輸出用來驅(qū)動(dòng)電調(diào)給無刷電機(jī)供電；I2C端口主要是用來給外設(shè)傳感器讀取數(shù)據(jù)。

4.2 系統(tǒng)總中斷程序

閉環(huán)調(diào)速系統(tǒng)中總中斷程序的作用主要是收集各外設(shè)傳感器模塊測得的數(shù)據(jù)而后進(jìn)行數(shù)據(jù)融合并最終作用于無刷電機(jī)進(jìn)行調(diào)速。四旋翼飛行器起飛時(shí)系統(tǒng)初始化環(huán)節(jié)完成后系統(tǒng)總中斷程序也開始工作，且根據(jù)需要設(shè)置系統(tǒng)中斷固定掃描周期5 ms。在收集各外設(shè)傳感器模塊所測數(shù)據(jù)前必須進(jìn)行鎖定狀態(tài)下的校準(zhǔn)工作，主要是對三維角度傳感器MPU6050進(jìn)行校準(zhǔn)。校準(zhǔn)完成后進(jìn)行數(shù)據(jù)融合并最終傳送到PID控制器對無刷電機(jī)進(jìn)行調(diào)速。

4.3 閉環(huán)調(diào)速系統(tǒng)中斷環(huán)節(jié)

四旋翼飛行器閉環(huán)調(diào)速系統(tǒng)里需用到多個(gè)中斷，因此需要根據(jù)任務(wù)的先后和重要性設(shè)置中斷優(yōu)先級。共4個(gè)中斷源：TIM IN用于執(zhí)行單片機(jī)的輸入捕獲，以便獲取遙控器輸入指令；USART2用于執(zhí)行數(shù)據(jù)接收串口通信功能；TIM OUT用于執(zhí)行單片機(jī)的輸出比較，以便輸出4個(gè)PWM值控制無刷電機(jī)；TIM用于執(zhí)行單片機(jī)5 ms更新中斷，實(shí)現(xiàn)計(jì)數(shù)功能，優(yōu)先級由高至低。

4.4 PID閉環(huán)控制算法

PID閉環(huán)控制系統(tǒng)中通過期望值輸入到PID控制器進(jìn)行偏差計(jì)算和控制器各組成環(huán)節(jié)運(yùn)算后輸入到無刷電機(jī)實(shí)現(xiàn)調(diào)速的功能。同時(shí)輸出值又反饋回控制器輸入端為下一次期望值輸入進(jìn)行偏差計(jì)算時(shí)做準(zhǔn)備。在控制算法中對輸入變量設(shè)置限幅標(biāo)志以防止變量積分時(shí)出現(xiàn)過飽和的現(xiàn)象。每次控制時(shí)在PID控制器各組成單元中均會(huì)進(jìn)行本次計(jì)算偏差與上次保留偏差之間的大小比較以確定最新一次的各環(huán)節(jié)輸出值。這樣層層往復(fù)使得控制更為精準(zhǔn)與穩(wěn)定。

4.5 PWM調(diào)制控制電機(jī)轉(zhuǎn)速程序

PWM(Pulse Width Modulation)稱為脈沖寬度調(diào)制，在四旋翼飛行器中PWM輸出波形的占空比越大，則施加在無刷電機(jī)兩端等效的電壓越高，從而對應(yīng)的電機(jī)轉(zhuǎn)速就越大。STM32F407VGT6系列核心板微處理器模塊(MCU)接收來自遙控器和串級PID控制器的信號通過配置GPIO接口而輸出PWM信號，PWM信號經(jīng)無刷電機(jī)驅(qū)動(dòng)器中的MOS管放大電路放大電壓后驅(qū)動(dòng)無刷電機(jī)工作。

5 實(shí)物測試結(jié)果

PWM波形輸出通過RIGOL DS1102E示波器測量。為能在示波器上能有直觀顯示變化情況，本文設(shè)計(jì)的四旋翼飛行器PWM波形輸出占空比范圍為40%～75%，測量PWM波形輸出時(shí)引出STM32單片機(jī)上輸入到對應(yīng)無刷電機(jī)驅(qū)動(dòng)器的信號線及接地引線，分別與示波器的正負(fù)引線相連，測得的PWM波形如圖6中所。

圖6 PWM輸出波形顯示(占空比40%)

飛行器飛行時(shí)通過施加油門輸入增大PWM波形占空比從而改變無刷電機(jī)兩端等效電壓最終實(shí)現(xiàn)電機(jī)轉(zhuǎn)速的改變。圖7兩幅圖是實(shí)際測試過程中增加油門輸入時(shí)對應(yīng)的PWM波形變化情況。

6. 總結(jié)

本文針對四旋翼飛行器無刷電機(jī)調(diào)速，設(shè)計(jì)了一種比現(xiàn)有控制策略更為簡便的控制算法為其在自動(dòng)飛行方面的擴(kuò)展提供基礎(chǔ)，通過內(nèi)外環(huán)串級控制的方式解決了飛行器在變量耦合方面的問題，可大范圍用于無刷電機(jī)調(diào)速。確立四旋翼飛行器閉環(huán)調(diào)速系統(tǒng)整體框架，設(shè)計(jì)系統(tǒng)初始化初始化環(huán)節(jié)、中斷環(huán)節(jié)、PID閉環(huán)調(diào)速系統(tǒng)算法，并進(jìn)行最終的試飛調(diào)試，驗(yàn)證了本文所提的方案。

圖7 實(shí)際測試加以油門輸入的PWM波形變化情況