0 引言

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

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

1.1 坐標(biāo)軸選取

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

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

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

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

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

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

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

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

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

    采用GPS定位模塊與光流模塊相結(jié)合的方式來實(shí)現(xiàn)定位與懸停,，這可以滿足室內(nèi)外均可飛行的需要。慣性檢測(cè)單元采用MPU6050^[8]模塊,，用來實(shí)現(xiàn)對(duì)飛行器當(dāng)前的姿態(tài)角進(jìn)行檢測(cè),，從而控制飛行器的飛行姿態(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ì)

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

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

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

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

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

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

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

    設(shè)3路PID控制器的輸出分別為RollQut、PitchOut,、YawOut,，每個(gè)電機(jī)得到驅(qū)動(dòng)電路給出的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í)設(shè)置外環(huán)輸出為零，使內(nèi)環(huán)期望值為零,，調(diào)整PID參數(shù),，隨意沿X軸或Y軸轉(zhuǎn)動(dòng)飛行器至某一角度，當(dāng)其能自動(dòng)停止在該角度且不震蕩時(shí)為內(nèi)環(huán)最佳PID參數(shù),。然后調(diào)整外環(huán)PID參數(shù),，直至外環(huán)角度恢復(fù)有力且不震蕩時(shí)為外環(huán)最佳PID參數(shù)。至此串級(jí)PID控制器設(shè)計(jì)完畢,。

4 實(shí)驗(yàn)驗(yàn)證及分析

4.1 實(shí)驗(yàn)系統(tǒng)驗(yàn)證^[14]

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

4.2 實(shí)驗(yàn)結(jié)果分析

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

5 結(jié)論

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

參考文獻(xiàn)

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

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

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

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

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

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

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

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

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

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

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

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

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

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

作者信息:

田  睿1,，2,，孫迪飛1

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