0 引言

    四旋翼飛行器是近來各大科研機(jī)構(gòu)研究的熱門方向，由于其控制算法經(jīng)典而又復(fù)雜，現(xiàn)已成為各大科研院校作為學(xué)生實(shí)踐能力提升的一項(xiàng)重要實(shí)踐課。自2013年以來，在每屆全國(guó)大學(xué)生電子設(shè)計(jì)競(jìng)賽中，四旋翼飛行器題也已成為參賽學(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ò)展磁場(chǎng)方案，擴(kuò)展了磁場(chǎng)傳感器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全國(guó)大學(xué)生電子設(shè)計(jì)競(jìng)賽落幕，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)