楊 剛，千 博，陳建安，鄭春紅，李威威

(西安電子科技大學(xué) 超高速電路設(shè)計與電磁兼容教育部重點實驗室，陜西 西安 710071)

四軸飛行器(quadcopter)是一種可以垂直起降的無人機，具有輕巧便攜、操控簡便、 機動性及環(huán)境適應(yīng)性強、制造和應(yīng)用成本低廉等特點，國內(nèi)外越來越多的院校、科研機構(gòu)和制造商紛紛投入其研究與生產(chǎn)的行列。以四軸飛行器為代表的多旋翼飛行器在航空攝影、大地測繪、資源勘查、大氣監(jiān)測、森林防火、災(zāi)難救援及公共安全、反恐、偵查等領(lǐng)域有著非常廣闊的應(yīng)用前景[1]。

由于近年四軸飛行器的興起，尤其是在校學(xué)生對其具有非常濃厚的興趣，以四軸飛行器的設(shè)計作為嵌入式實驗的課程不僅滿足了在校學(xué)生對興趣的追求，而且能夠起到學(xué)以致用的作用，符合電子信息工程專業(yè)的嵌入式實驗教學(xué)的要求。

1 四軸飛行器的結(jié)構(gòu)與飛行原理

四軸飛行器的機械結(jié)構(gòu)在1907年的 Gyroplane No.I載人飛行器原型機設(shè)計成功時就已經(jīng)確定，一直延續(xù)至今，不過現(xiàn)在設(shè)計的小型四軸無人飛行器的中心位置的駕駛艙由控制系統(tǒng)代替。

四軸飛行器的結(jié)構(gòu)主要有機架、無刷電機及電調(diào)、旋翼和飛行控制計算機等4部分組成。

1)機架是連接各個部分的機械部分，支撐飛行器的飛行和各個部分的位置安裝與固定。

2)無刷電機及電調(diào)為四軸飛行器提供動力。

3)螺旋槳是在無刷電機的帶動下旋轉(zhuǎn)，與空氣相互作用提供升力，使四軸飛行器離開地面，應(yīng)用在四軸飛行器中的螺旋槳旋轉(zhuǎn)方向有正反兩種。

4)飛行控制計算機是四軸飛行器的控制中心，主要包括控制器、電源、傳感器組及信號接收發(fā)射器等。四軸飛行器的控制模型按照其機械結(jié)構(gòu)的對稱性可以分為十字型和X型，十字型的優(yōu)點是控制簡單，缺點是穩(wěn)定性不好，而X型相反。本文設(shè)計采用的是X型模型，結(jié)構(gòu)圖如圖1所示。

M1和M3的螺旋槳方向相同，M2和M4的螺旋槳方向相同且與M1相反。

四軸飛行器的平穩(wěn)飛行是合理控制4個無刷電機轉(zhuǎn)速的結(jié)果，每一個機體的狀態(tài)都會使對應(yīng)的電機的轉(zhuǎn)速改變[2-3](如表1所示)。

圖1 四軸飛行器結(jié)構(gòu)圖

表1 X飛行模式電機運動狀態(tài)表

飛行狀態(tài)M1M2M3M4前(后)運動等量減速(加速)等量減速(加速)等量加速(減速)等量加速(減速)左(右)運動等量減速(加速)等量加速(減速)等量加速(減速)等量減速(加速)順(逆)轉(zhuǎn)向等量加速(減速)等量減速(加速)等量加速(減速)等量減速(加速)

2 實驗系統(tǒng)硬件方案

四軸飛行器的控制系統(tǒng)主要有加速度計、陀螺儀、遙控器、電源和控制器等部分組成。如圖2所示，是四軸飛行器實驗平臺的硬件設(shè)計框圖。

本系統(tǒng)的STM32F4負責數(shù)據(jù)的處理與控制，外圍電路包括從傳感模塊得到的角速度和加速度數(shù)據(jù)傳給STM32F4進行處理和計算后得到實時的姿態(tài)角信息，然后根據(jù)姿態(tài)角信息通過電調(diào)進行機體的狀態(tài)調(diào)節(jié)；從遙控器得到的控制信息被捕獲到后也通過電調(diào)對機體調(diào)節(jié)。LED指示燈可以對當前的狀態(tài)進行直觀表示，利于進行調(diào)試。

圖2 控制系統(tǒng)硬件設(shè)計框圖

2.1 微控制器STM32F4

STM32F4是Cortex-M4處理器[4]，是ARMv7-M體系結(jié)構(gòu)的延伸，在這種體系結(jié)構(gòu)中FPU被視作為協(xié)處理器，正常的過程管線與FPU共享同樣的指令擷取階段，但是指令的解碼和執(zhí)行階段是分開的，如圖3所示。它有單精度浮點單元，如果它的FPU啟用，能夠加快單精度浮點的運算，對于FPU的使用，芯片制造商已經(jīng)把有關(guān)的指令操作封裝為C的數(shù)學(xué)運算庫，這樣在用到處理函數(shù)時就可以直接調(diào)用，由于FPU僅僅支持單精度的計算，所以雙精度的運算只能通過軟件的方式實現(xiàn)，于是雙精度的運算可以使用規(guī)定好的C的數(shù)學(xué)運算函數(shù)庫來加快運算速度。

圖3 FPU管線原理

四軸飛行器的控制系統(tǒng)要求較高的實時控制效果，隨著控制算法的研究，涉及大量的數(shù)學(xué)計算，雖然算法的效果得到了改善但是為了能夠快速地在短時間內(nèi)處理完這些數(shù)據(jù)，這就對控制系統(tǒng)的微處理器性能的要求提高了，為了解決這種不足，通常要使用一個微處理器和一個DSP芯片，這雖然解決了上述問題，但是也帶來了在硬件電路設(shè)計上的復(fù)雜性和控制程序編寫的困難。為了徹底解決上述問題我們就可以利用STM32F4作為飛控的微處理器芯片，這樣可以在不改變程序結(jié)構(gòu)的情況下利用DSP的庫進行相關(guān)的浮點數(shù)操作，使計算的速度加快不少。

2.2 慣性傳感器模塊

MPU6050嵌入了3軸的MEMS陀螺儀、3軸的MEMS加速度計和一個數(shù)字運動處理器(digital motion processor，DMP)硬件加速引擎。為了方便數(shù)字化陀螺儀和加速度計的數(shù)據(jù)輸出，各有3個16 bit的ADCs。為了使該傳感器能夠適合快和慢的測量，具有不同的精度選擇，陀螺儀的精度范圍有±250°、±500°、±1 000°和±2 000°/sec。加速度計的精度范圍有±2、±4、±8 g和±16 g。

圖4 MPU6050模塊實物圖

本實驗采用MPU6050傳感器模塊，如圖4所示，該模塊是與控制板獨立的，只是把主要的引腳用插針引出來，便于連接和使用。該模塊上面只有MPU6050芯片和少量能使MPU6050芯片正常工作的電子元件，它的原理圖如圖5所示。

圖5 MPU6050模塊電路圖

3 實驗平臺軟件設(shè)計

本設(shè)計的主要軟件目標是在固定的時間周期內(nèi)進行掃描控制，包括遙控信息獲取、姿態(tài)角結(jié)算和控制輸出。每個部分的進行都是在中斷內(nèi)完成，根據(jù)任務(wù)的重要性進行中斷時間的設(shè)定，姿態(tài)角的結(jié)算是5 ms執(zhí)行一次，控制輸出是10 ms執(zhí)行一次，遙控信息的獲取是50 ms進行一次，如圖6所示。

3.1 系統(tǒng)初始化模塊

初始化模塊是軟件程序運行的開始，為系統(tǒng)各個部分的順利運行做好準備工作。初始化模塊包括軟件系統(tǒng)初始化和硬件系統(tǒng)初始化。軟件系統(tǒng)初始化包括定時器模式初始化、數(shù)據(jù)變量初始化、通信協(xié)議初始化、中斷向量表初始化和延遲函數(shù)初始化等。硬件系統(tǒng)初始化包括系統(tǒng)時鐘初始化、串口驅(qū)動初始化和I/O口使用初始化等。

圖6 軟件控制流程圖

3.2 姿態(tài)角結(jié)算

姿態(tài)角結(jié)算的準確性是能否穩(wěn)定飛行的關(guān)鍵。四軸飛行器是具有3個自由度的運動物體，術(shù)語分別是仰俯角、偏航角和旋轉(zhuǎn)角[5]，現(xiàn)在還沒有能夠直接測出運動角度的傳感器，都是間接地進行測量。本系統(tǒng)是利用角速度傳感器和加速度傳感器來分別獲得角速度和加速度，通過平滑濾波、數(shù)據(jù)融合及四元數(shù)[6]解算來獲得姿態(tài)角[7]，如圖7所示。

由于安裝傳感器水平位置的不確定性，需要進行偏移量的矯正，可以在傳感器安裝完畢后，系統(tǒng)在初始化狀態(tài)下進行若干次的傳感器數(shù)據(jù)讀取，取均值作為偏移量。從傳感器得到原始數(shù)據(jù)由于存在噪聲，如果直接利用會對后面的數(shù)據(jù)處理產(chǎn)生較大的干擾，我們在此使用平滑濾波進行對原始數(shù)據(jù)的處理，我們知道慣性傳感器自身的缺點，雖然對快速的運動能夠得到準確的角速度，但是對于慢速的運動具有漂移。而加速度計由于慣性的影響在短時間內(nèi)的精度不高，但是誤差卻不隨時間的增加而積累。因此我們可以利用兩者之間的優(yōu)缺點進行互補融合[8]。

圖7 姿態(tài)解算流程圖

本系統(tǒng)利用中斷進行姿態(tài)角的解算，每5 ms進行一次中斷處理，處理包括傳感器數(shù)據(jù)采集和姿態(tài)角融合算法[9]，流程圖如圖8所示。

本設(shè)計采用的MPU6050傳感器與控制器的通信方式是I2C總線協(xié)議。該協(xié)議有兩根信號線，分別是數(shù)據(jù)線和時鐘線，時鐘線是為數(shù)據(jù)的存儲位置做基準，數(shù)據(jù)線根據(jù)時鐘線的第幾個時鐘沿確定該位是第幾位數(shù)據(jù)，從而達到接收和發(fā)送數(shù)據(jù)的目的。

STM32系列控制器芯片對傳感器MPU6050的數(shù)據(jù)讀取包括加速度和角速度兩部分，控制器內(nèi)雖然已經(jīng)有了硬件I2C接口，但是為了方便程序的可移植性和便于調(diào)試，本系統(tǒng)使用普通I/O口的模擬I2C，即根據(jù)I2C的協(xié)議自己編寫軟件協(xié)議程序控制時序圖。

圖8 姿態(tài)角解算流程圖

本系統(tǒng)使用四元數(shù)進行姿態(tài)角的解算，由于我們使用傳感器獲得角速度和加速度，因此我們可以利用傳感器數(shù)據(jù)來更新四元數(shù)，其關(guān)系為：

(1)

從而得到四元數(shù)與姿態(tài)角的關(guān)系為：

(2)

φ=-arcsin2(q1q3-q0q2)

(3)

(4)

式中，θ為滾轉(zhuǎn)角(roll),φ為仰俯角(pitch),Ψ為航向角(yaw)。

該算法的中斷處理流程如圖9所示。為了防止從傳感器得到的數(shù)據(jù)具有突變性，進行簡單的數(shù)據(jù)濾波處理，本設(shè)計采用的是窗口為10的平滑濾波處理。經(jīng)過簡單的處理后，把加速度和角速度進行誤差補償融合，得到了較準確的合成后的角速度數(shù)據(jù)，然后利用已知的數(shù)學(xué)推導(dǎo)公式進行更新四元數(shù)系數(shù)，然后把實時的四元數(shù)系數(shù)進行歸一化處理后帶入到姿態(tài)角和四元數(shù)歸一化系數(shù)的關(guān)系公式內(nèi)，最后得到實時的姿態(tài)角信息。

圖9 姿態(tài)角處理程序流程圖

3.3 平穩(wěn)控制調(diào)節(jié)

由于四軸飛行器的控制系統(tǒng)是非線性的，建立數(shù)學(xué)模型比較困難，而PID控制器只需要根據(jù)實際的輸入數(shù)據(jù)和輸出數(shù)據(jù)就能進行控制，因此我們的控制器采用PID[10]。另外由于四軸飛行器的快速性，對控制的要求比較靈敏，為了能迅速地監(jiān)測和跟蹤控制，提高控制的穩(wěn)定性，我們采用串聯(lián)PID控制[11]，即對角速度PID和角度PID的雙閉環(huán)PID算法[12]，結(jié)構(gòu)圖如圖10所示，外環(huán)角度計算的PID輸出量作為內(nèi)環(huán)角速度PID的期望值，陀螺儀的測量值作為實際值，從而計算總的控制量。另外在實際中PID的微分項會較大，導(dǎo)致機體的震蕩，因此我們要加入一個一定截止頻率(如20 Hz)的低通濾波器[13]，流程圖如圖11所示。

本設(shè)計采用位置式數(shù)字PID控制，如下：

(5)

式中，e(k)是控制誤差，由期望值和實際測量值作減獲得，角度環(huán)的誤差為遙控器的操縱桿位置轉(zhuǎn)化的期望值與姿態(tài)角解算出的角度之差，角速度環(huán)的控制誤差為角度PID控制器的輸出與陀螺儀角速度的輸出之差。運用上面的算法分別對pitch、roll、yaw3個量進行計算，得到的優(yōu)化控制量PID_roll，PID_pitch，PID_yaw。對每一個量的3個參數(shù)P，I，D進行確定。

每個電機的控制量為：

M1_PWM=Throttle+PID_pitch-PID_roll-PID_yaw

M2_PWM=Throttle+PID_pitch+PID_roll+PID_yaw

M3_PWM=Throttle-PID_pitch+PID_roll-PID_yaw

M4_PWM=Throttle-PID_pitch-PID_roll+PID_yaw

圖10 PID控制結(jié)構(gòu)圖

圖11 PID控制流程圖

4 測試

四軸飛行器的PID參數(shù)的整定是比較重要的，參數(shù)是否合適決定了飛行效果的好壞，由于我們采用串聯(lián)PID，所以在整定時按照先內(nèi)后外的原則[14]進行。先把外環(huán)的PID設(shè)置為0，然后慢慢增加內(nèi)環(huán)的P，直到飛行器開始輕微地等幅震蕩，然后增加內(nèi)環(huán)D，直到震蕩消除，如果電機進行快速震動說明系數(shù)D太大；如果機體一直達不到水平位置并且有偏移，那就增加內(nèi)環(huán)I，直到消除偏移，另外I會導(dǎo)致系統(tǒng)的穩(wěn)定性破壞，所以系數(shù)I不易過大；內(nèi)環(huán)調(diào)好后飛行器基本能夠平穩(wěn)飛行，另外再進行外環(huán)的調(diào)整，實際上外環(huán)只進行比例P調(diào)節(jié)就已經(jīng)能夠使機體非常平穩(wěn)地飛行了，飛行效果圖如圖12所示。

圖12 實際飛行測試

5 結(jié)束語

本文主要是利用STM32F4芯片實現(xiàn)四軸飛行器的控制系統(tǒng)設(shè)計的教學(xué)實驗平臺，該平臺具有較高的新穎性和穩(wěn)定性，也具有拓展創(chuàng)新性。本

實驗不僅能使學(xué)生對嵌入式技術(shù)的應(yīng)用具有較好的理解，而且對學(xué)生的創(chuàng)新性和實踐能力具有較大的提高，本實驗平臺非常適合高校的嵌入式課程教學(xué)和相關(guān)的科學(xué)研究。

[1]SWEE K P,KUN L,KOK H Y.Systemtic design and implementation of a micro unmanned quadrotor system[J].Unmanned Systems,2014,2(2):1-21.

[2]李堯.四旋翼飛行器控制系統(tǒng)設(shè)計[D].大連：大連理工大學(xué)，2013.

[3]MIN B C,CHO C H,CHOI K M,et al.Development of a micro quad-rotor UAV for monitoring an Indoor environment[J].Advances in Robotics,2009(5744):262-271.

[4]JOSEPH Y.The definitive guide to ARM Cortex-M3 and Cortex-M4 processors[M].Waltham:Newnes,2014.

[5]ALTUG E.Vision based control of unmanned aerial vehicles with applications to autonomous four rotor helicopter quadrotor[D].Philadelphia:University of Pennsylvania,2003.

[6]鄧正隆.慣性技術(shù)[M].哈爾濱：哈爾濱工業(yè)大學(xué)出版社，2006.

[7]吳俊偉.無陀螺慣性組合導(dǎo)航系統(tǒng)濾波方法研究[D].哈爾濱：哈爾濱工程大學(xué)，2010.

[8]杜杉杉，譚田，吳昊，等.一種面向AHRS的改進互補濾波融合算法[J].國外電子測量技術(shù)，2015，34(3):13-18.

[9]劉輝邦，褚金奎，支煒，等.基于STM32的無人機姿態(tài)測量系統(tǒng)設(shè)計[J].傳感器與微系統(tǒng)，2013，32(8):108-110.

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

[11]陸偉男，蔡啟仲，李剛，等.基于四軸飛行器的雙閉環(huán)PID控制[J].科學(xué)技術(shù)與工程，2014，14(33):127-131.

[12]CHEN X,WANG L P.Step response identification of a quadcopter UAV using frequency-sampling filters[J].Ifac Papersoline,2015,48(28):122-127.

[13]RIBEIRO T T,CONCEICAO A G S,SA I,et al.Nonlinear model predictive formation control for quadcopters[J].Ifac Papersonline,2015,48(19):39-44.

[14]MAHMOOD A,KIM Y.Leader-following formation control of quadcopters with heading synchronization[J].Aerospace Science&Technology,2015(47):68-74.