0 引言

    四旋翼飛行器是近來各大科研機(jī)構(gòu)研究的熱門方向，由于其控制算法經(jīng)典而又復(fù)雜，現(xiàn)已成為各大科研院校作為學(xué)生實踐能力提升的一項重要實踐課。自2013年以來，在每屆全國大學(xué)生電子設(shè)計競賽中，四旋翼飛行器題也已成為參賽學(xué)生熱門首選題目^[1]。然而，由于四旋翼飛行器具有非線性、強(qiáng)耦合、欠驅(qū)動及多變量等特點(diǎn)，使得飛行器的設(shè)計及其控制都難以達(dá)到預(yù)定要求。

1 四旋翼飛行器物理數(shù)學(xué)模型

1.1 坐標(biāo)軸選取

    本文選取載體坐標(biāo)系來描述飛行器的運(yùn)動規(guī)律，如圖1所示，載體坐標(biāo)系其原點(diǎn)位于機(jī)體質(zhì)心，選取右前上坐標(biāo)系，即其X軸沿機(jī)體橫軸向右，Y軸沿機(jī)體縱軸向前，Z軸沿機(jī)體豎軸向上^[2]。根據(jù)載體坐標(biāo)系和導(dǎo)航坐標(biāo)系之間的相對關(guān)系來定義載體的航向角Yaw、俯仰角Pitch、橫滾角Roll 3個姿態(tài)角。

1.2 姿態(tài)解算方案

    當(dāng)前很多學(xué)者對四旋翼飛行器采用歐拉角法、方向余弦法進(jìn)行姿態(tài)解算，經(jīng)仔細(xì)分析可知這些姿態(tài)解算方案存在缺陷^[3-4]。歐拉角微分方程僅有3個未知數(shù)，但每個方程都包含三角函數(shù)運(yùn)算，當(dāng)θ=90°時方程將出現(xiàn)奇點(diǎn)，此時會導(dǎo)致飛行器姿態(tài)解算錯誤，編程人員只能在θ=90°時強(qiáng)制修正姿態(tài)角以避開解算錯誤，因此歐拉角法不能全姿態(tài)工作；方向余弦法雖然可以全姿態(tài)工作，但微分方程組高達(dá)9維，計算量大，對CPU計算速度要求高，而且不可避免地會產(chǎn)生非正交化誤差。本文將采用四元數(shù)法進(jìn)行飛行器全姿態(tài)解算^[5]，可以保證全姿態(tài)工作，不受限制，且微分方程只有四維，相比方向余弦法計算量小，四元數(shù)法得到的方向余弦矩陣的性能優(yōu)于方向余弦法。

    四元數(shù)由1個實數(shù)單位和3個虛數(shù)單位i，j，k構(gòu)成4個元的數(shù)，可表示為：

    則導(dǎo)航坐標(biāo)系n到載體坐標(biāo)系b的四元數(shù)變換為：

    至此，四旋翼的物理數(shù)學(xué)模型建立完畢，在程序設(shè)計中將通過軟件算法實現(xiàn)以上數(shù)據(jù)姿態(tài)解算。

2 系統(tǒng)硬件電路設(shè)計

2.1 系統(tǒng)電路設(shè)計方案

    控制系統(tǒng)電路結(jié)構(gòu)包括單片機(jī)、顯示模塊、光流模塊、慣性檢測、按鍵模塊以及電機(jī)、電調(diào)等部分。采用STM32F103單片機(jī)作為主控^[7]，其主頻達(dá)到72 MHz，保證了姿態(tài)數(shù)據(jù)的快速解算，提高了系統(tǒng)的響應(yīng)速度。

    采用GPS定位模塊與光流模塊相結(jié)合的方式來實現(xiàn)定位與懸停，這可以滿足室內(nèi)外均可飛行的需要。慣性檢測單元采用MPU6050^[8]模塊，用來實現(xiàn)對飛行器當(dāng)前的姿態(tài)角進(jìn)行檢測，從而控制飛行器的飛行姿態(tài)。電調(diào)采用大功率MOS管形成電子調(diào)速器，電機(jī)采用颶風(fēng)U2208無刷電機(jī)，保證系統(tǒng)的穩(wěn)定性及可靠性，系統(tǒng)電路結(jié)構(gòu)圖如圖2所示。

2.2 硬件電路原理圖設(shè)計

    根據(jù)圖2所示系統(tǒng)電路結(jié)構(gòu)圖，設(shè)計的電路原理圖如圖3所示^[9]。采用意法半導(dǎo)體公司微處理器STM32F103C8T6作為主控單元，慣性處理單元采用全球首例整合性6軸運(yùn)動處理組件MPU6050，整合了陀螺儀和加速度計傳感器為一體，免除了二者時間軸之差的問題^[8]；根據(jù)MPU6050的I²C接口擴(kuò)展磁場方案，擴(kuò)展了磁場傳感器HMC5883，實現(xiàn)了九軸運(yùn)動處理；設(shè)計了4路采用MOS管Q1實現(xiàn)的電子速度調(diào)節(jié)器，以實現(xiàn)對4路無刷電機(jī)速度的控制調(diào)節(jié)等。

3 系統(tǒng)軟件程序編寫

3.1 數(shù)據(jù)濾波程序設(shè)計

    飛行器在運(yùn)行時，電機(jī)高速轉(zhuǎn)動引起的機(jī)體高頻振動、環(huán)境電磁干擾以及溫漂等均會產(chǎn)生干擾噪聲信號，這些信號會被靈敏的慣性傳感器MPU6050采集到，給后期控制帶來干擾誤差，因此需要設(shè)計濾波器將其去除或降至最低。由于下一時刻的角度、角速度數(shù)據(jù)具有可預(yù)測性，因此本文將采用卡爾曼濾波利用系統(tǒng)線性狀態(tài)方程對系統(tǒng)觀測數(shù)據(jù)、狀態(tài)進(jìn)行實時最優(yōu)估計，以濾去這些干擾噪聲信號。設(shè)計的卡爾曼濾波器詳細(xì)參數(shù)如下^[10-11]：

    飛行器上電后，將卡爾曼濾波前后數(shù)據(jù)通過無線模塊傳遞給上位機(jī)進(jìn)行結(jié)果觀察，飛行器在各軸平衡位置附近來回運(yùn)動時，卡爾曼濾波很好地濾去了這些干擾信號，其中X軸濾波前后波形圖如圖4所示。從圖中可以看出，卡爾曼濾波濾去這些干擾噪聲，X軸角度數(shù)據(jù)輸出穩(wěn)定，其他軸數(shù)據(jù)與此類似。

3.2 串級PID控制器設(shè)計

    卡爾曼濾波后的角度數(shù)據(jù)、角速度數(shù)據(jù)最終作為控制系統(tǒng)實時姿態(tài)信息。由于四旋翼飛行器運(yùn)動過程復(fù)雜，單級PID調(diào)節(jié)無法保證控制系統(tǒng)穩(wěn)定性，參數(shù)過大時，姿態(tài)角由大角度偏差恢復(fù)到平衡位置時極易引起震蕩；參數(shù)過小時，姿態(tài)角由小角度偏差恢復(fù)到平衡位置時又顯得力度不夠。此問題主要是因為在單級PID調(diào)節(jié)中，角度恢復(fù)速度無法跟隨角度大小變化來控制導(dǎo)致。因此本文將考慮采用串級PID調(diào)節(jié)方案^[12-13]，將PID控制器分內(nèi)環(huán)與外環(huán)兩個層面來考慮。如圖5所示，內(nèi)環(huán)主要對角速度進(jìn)行PID調(diào)節(jié)，期望值為外環(huán)角度。這可保證系統(tǒng)從偏離期望角向平衡位置轉(zhuǎn)動的角速度與角度成正比，即：角度偏差較大時，轉(zhuǎn)動角速度大；角度偏差小時，轉(zhuǎn)動角速度也小。這一方面保證了恢復(fù)到平衡位置的速度，另一方面避免了在平衡位置震蕩問題。

    設(shè)3路PID控制器的輸出分別為RollQut、PitchOut、YawOut，每個電機(jī)得到驅(qū)動電路給出的PWM線性組合為：

    X軸Pitch左端電機(jī)：Throttle+roll_out+yaw_out；

    X軸Pitch右端電機(jī)：Throttle-roll_out+yaw_out；

    Y軸Roll前端電機(jī)：Throttle-roll_out-yaw_out；

    Y軸Roll后端電機(jī)：Throttle+pitch_out-yaw_out。

    將飛行器分別沿X軸、Y軸固定，以調(diào)整內(nèi)外環(huán)PID參數(shù)。首先調(diào)整內(nèi)環(huán)PID參數(shù)，此時設(shè)置外環(huán)輸出為零，使內(nèi)環(huán)期望值為零，調(diào)整PID參數(shù)，隨意沿X軸或Y軸轉(zhuǎn)動飛行器至某一角度，當(dāng)其能自動停止在該角度且不震蕩時為內(nèi)環(huán)最佳PID參數(shù)。然后調(diào)整外環(huán)PID參數(shù)，直至外環(huán)角度恢復(fù)有力且不震蕩時為外環(huán)最佳PID參數(shù)。至此串級PID控制器設(shè)計完畢。

4 實驗驗證及分析

4.1 實驗系統(tǒng)驗證^[14]

    根據(jù)以上設(shè)計，將陀螺儀、加速度計數(shù)據(jù)經(jīng)卡爾曼濾波器融合濾波后，將數(shù)據(jù)通過串口發(fā)送至PC上位機(jī)(波特率為9 600 b/s)，通過匿名科創(chuàng)提供的地面站軟件，對飛行器橫滾角解算、俯仰角解算均進(jìn)行了3D觀察校驗。當(dāng)手動旋轉(zhuǎn)X軸到某一角度時，上位機(jī)顯示俯仰角會跟隨偏轉(zhuǎn)而其他角度不變；當(dāng)手動旋轉(zhuǎn)Y軸到某一角度時，上位機(jī)顯示橫滾角會跟隨偏轉(zhuǎn)而其他角度不變，過程平穩(wěn)，響應(yīng)速度快，證明此設(shè)計符合要求。其中俯仰角解算結(jié)果如圖6所示，其他與此類似。

4.2 實驗結(jié)果分析

    通過室外飛行可以看出，飛行器能夠平穩(wěn)地在空中飛行。通過對橫滾角、俯仰角、偏航角3種姿態(tài)角控制，可以實現(xiàn)飛行器在空中的穩(wěn)定懸停、前進(jìn)、后退、偏航等姿態(tài)。

5 結(jié)論

    四旋翼飛行器是一種較為復(fù)雜的控制系統(tǒng)，本文通過對四旋翼飛行器進(jìn)行物理數(shù)學(xué)模型創(chuàng)建，分析了幾種姿態(tài)解算方案的不足，設(shè)計了四元數(shù)姿態(tài)解算方案，消除了運(yùn)算中的轉(zhuǎn)動不可交換性誤差，設(shè)計卡爾曼濾波對加速度計、陀螺儀數(shù)據(jù)進(jìn)行融合濾波，保證了數(shù)據(jù)的穩(wěn)定性及可靠性。同時，對四旋翼飛行器進(jìn)行了電路設(shè)計和程序編寫，實驗結(jié)果表明，該系統(tǒng)能夠在空中實現(xiàn)穩(wěn)定懸停并能按預(yù)定方向平穩(wěn)飛行。

參考文獻(xiàn)

[1] 王瑩.2017全國大學(xué)生電子設(shè)計競賽落幕，4.3萬名學(xué)生參賽[J].電子產(chǎn)品世界，2018(1)：85-86.

[2] 潘錦珊.氣體動力學(xué)基礎(chǔ)(3版)[M].西安：西北工業(yè)大學(xué)出版社，1995.

[3] 野瑩瑩，張艷珠，鄒心宇，等.歐拉角姿態(tài)解算的改進(jìn)[J].裝備制造技術(shù)，2018，279(3)：110-111，140.

[4] 王勇軍，李智，李翔.采用組合濾波算法的無人機(jī)航向測量系統(tǒng)研究[J].電子技術(shù)應(yīng)用，2018，44(2)：39-43.

[5] 葉锃鋒，馮恩信.基于四元數(shù)和卡爾曼濾波的兩輪車姿態(tài)穩(wěn)定方法[J].傳感技術(shù)學(xué)報，2012，25(4)：524-528.

[6] 張德先，聶桂根.基于分解四元數(shù)的自適應(yīng)姿態(tài)四元數(shù)卡爾曼濾波[J].控制理論與應(yīng)用，2018(3)：367-374.

[7] 孫威，殷興輝，王新君.基于STM32的頻譜測量系統(tǒng)的設(shè)計[J].電子技術(shù)應(yīng)用，2019，45(3)：102-105，109.

[8] 黃瑞敏，李建奇，張斌，等.四旋翼飛行器追蹤地面移動目標(biāo)控制策略研究[J].電子技術(shù)應(yīng)用，2018，44(5)：73-77，82.

[9] 華成英.模擬電子技術(shù)基本教程[M].北京：清華大學(xué)出版社，2006.

[10] 喬少杰，韓楠，朱新文，等.基于卡爾曼濾波的動態(tài)軌跡預(yù)測算法[J].電子學(xué)報，2018，46(2)：418-423.

[11] 梁龍凱，張麗英，何文超，等.概率神經(jīng)網(wǎng)絡(luò)多模型卡爾曼濾波定位導(dǎo)航算法[J].電子技術(shù)應(yīng)用，2018，44(6)：66-68，73.

[12] 林巍，王亞剛.串級控制系統(tǒng)閉環(huán)辨識及PID參數(shù)整定[J].控制工程，2018(1)：11-18.

[13] 張濤，張曉宇，王輝俊.基于PWM和PID的直流電動機(jī)控制系統(tǒng)設(shè)計與制作[J].實驗室研究與探索，2014，33(7)：134-138.

[14] 江卉.四旋翼無人機(jī)姿態(tài)角回路穩(wěn)定性控制仿真[J].計算機(jī)仿真，2018(1)：70-73.

作者信息:

田  睿1，2，孫迪飛1

(1.河南工業(yè)職業(yè)技術(shù)學(xué)院 基礎(chǔ)科學(xué)部，河南 南陽473000；2.鄭州大學(xué) 物理工程學(xué)院，河南 鄭州450001)