基于光流傳感器的四旋翼飛行器設(shè)計*

顏 瑞，何 亮，鄧小飛，向曉燕，陳善榮

(1.吉首大學(xué)信息科學(xué)與工程學(xué)院，湖南 吉首 416000；2.國網(wǎng)湖南省電力有限公司湘西經(jīng)濟(jì)技術(shù)研究所，湖南 吉首 416000)

目前，四旋翼飛行器自主導(dǎo)航系統(tǒng)是一個熱門的研究方向.四旋翼飛行器自主導(dǎo)航系統(tǒng)根據(jù)其使用環(huán)境的不同，主要分為室外自主導(dǎo)航系統(tǒng)和室內(nèi)自主導(dǎo)航系統(tǒng).由于在室外能夠接收衛(wèi)星信號，因此大都使用衛(wèi)星定位信號來設(shè)計室外自主導(dǎo)航系統(tǒng)，如王宏凱等[1]基于GPS/BDS開發(fā)出雙模定位導(dǎo)航系統(tǒng).而在室內(nèi)接收不到衛(wèi)星信號，無法利用GPS和BDS設(shè)計室內(nèi)自主導(dǎo)航系統(tǒng)，所以常通過光流算法來實現(xiàn)室內(nèi)的精準(zhǔn)自主導(dǎo)航系統(tǒng)的設(shè)計，如陳磊等[2]提出的基于稀疏光流的無人機(jī)自主導(dǎo)航方案.但是，一般的光流傳感器價格較高且對環(huán)境要求較嚴(yán)格，一旦環(huán)境發(fā)生變化就會導(dǎo)致輸出的數(shù)據(jù)產(chǎn)生誤差，從而降低控制性能.

針對這個問題，筆者擬提出一種直接對優(yōu)象光流傳感器輸出的位置數(shù)據(jù)進(jìn)行處理，以完成室內(nèi)自主導(dǎo)航系統(tǒng)的開發(fā)策略.該優(yōu)象光流傳感器技術(shù)成熟且價格便宜，適用于大部分的地面，對環(huán)境的要求較低.為了驗證策略的可行性，首先利用MPU9250姿態(tài)傳感器的數(shù)據(jù)并結(jié)合串級比例-積分-微分(Proportion Integration Differentiation，PID)控制算法實現(xiàn)自穩(wěn)飛行，然后在此基礎(chǔ)上，利用優(yōu)象光流傳感器輸出的位移、速度數(shù)據(jù)及超聲波的高度數(shù)據(jù)，并結(jié)合串級PID控制算法，來實現(xiàn)室內(nèi)的自主導(dǎo)航定點飛行.

1 四旋翼飛行器飛行控制原理

四旋翼小型飛行器有2種飛行結(jié)構(gòu)，一種是十字型，另一種是X型.為了更好地對四旋翼飛行器進(jìn)行編程控制，本研究采用的是X型，如圖1所示，圖中箭頭所指的方向為機(jī)頭方向.為了平衡槳葉轉(zhuǎn)動后對機(jī)身產(chǎn)生的反扭力，1和3號電機(jī)順時針旋轉(zhuǎn)，2和4號電機(jī)逆時針旋轉(zhuǎn).

圖1 飛行結(jié)構(gòu)示意Fig. 1 Schematic Diagram of the Flight Structure

下面從垂直起飛降落、前后飛行、左右飛行和偏航飛行等8個飛行動作，對四旋翼飛行器的4個電機(jī)轉(zhuǎn)速的改變進(jìn)行分析，以深入了解四旋翼飛行器飛行控制的原理.要控制四旋翼飛行器垂直起飛，就要求1～4號電機(jī)同時起轉(zhuǎn)，只有當(dāng)槳葉轉(zhuǎn)動產(chǎn)生的升力超過飛機(jī)自身重力時，飛機(jī)才開始起飛，這時4個電機(jī)轉(zhuǎn)速要一致.如果4個電機(jī)轉(zhuǎn)速同時減小且升力小于飛機(jī)自身重力，飛機(jī)就開始垂直降落.向前飛行時，要求1和4號電機(jī)轉(zhuǎn)速降低，同時提高2和3號電機(jī)的轉(zhuǎn)速；向后飛行時的要求則與之相反.向左飛行時，要求1和2號電機(jī)轉(zhuǎn)速降低，同時提高3和4號電機(jī)的轉(zhuǎn)速；向右飛行時的要求則與之相反.向左偏航飛行時，要求2和4號電機(jī)轉(zhuǎn)速提高，1和3號電機(jī)轉(zhuǎn)速降低；向右偏航飛行時的要求則與之相反.

2 硬件組成結(jié)構(gòu)

2.1 硬件平臺組成

如圖2所示，系統(tǒng)的硬件平臺主要由主控芯片、傳感器、接收機(jī)、無刷電機(jī)、無線通信模塊、電調(diào)、飛控電池和上位機(jī)組成.傳感器包括測量高度的超聲波傳感器、測量姿態(tài)的MPU9250和測量位移的光流傳感器；主控芯片采用Texas Instruments公司的TM4C123GH6PM，用來運行飛控程序；無線通信模塊采用DL-20，用于上位機(jī)與飛控之間的信息交流；電調(diào)采用好盈樂天XRotor 20A的無刷電調(diào)；無刷電機(jī)采用920 kV的2212無刷電機(jī)；飛控電池采用達(dá)普35C的航模鋰電池；接收機(jī)采用富斯的iA6B遙控接收機(jī)，用來接收遙控器的命令；上位機(jī)采用匿名上位機(jī)，用來實時監(jiān)控飛控運行時的各個參數(shù)，同時發(fā)送命令幀對四旋翼飛行器進(jìn)行控制，改變其運行模式.

圖2 硬件結(jié)構(gòu)Fig. 2 Structure Diagram of Hardware

2.2 主控芯片

采用TM4C123GH6PM芯片作為系統(tǒng)的主控核心，其內(nèi)核為ARM Cortex-M4F處理器，系統(tǒng)時鐘最高可達(dá)到80 MHz，內(nèi)有256 kB的FLASH、32 kB的SRAM和2 kB的EEPROM，還有搭載 TivaWareTM(適用于 C 系列)軟件的內(nèi)置ROM.該芯片不僅有豐富的外設(shè)資源，而且有大量的通信接口，因此完全能滿足飛行控制系統(tǒng)的要求.

2.3 傳感器

2.3.1 姿態(tài)傳感器 姿態(tài)傳感器采用MPU9250九軸傳感器，傳感器芯片內(nèi)置三軸的加速度計、陀螺儀和磁力計，可以通過I2C與其通信，獲取原始的姿態(tài)數(shù)據(jù).其中三軸陀螺儀可通過編程來改變其量程(±250，±500，±1 000，±2 000(°)/s)，然后輸出16位ADC三軸角速度數(shù)據(jù)；加速度計也可通過編程來改變其量程(±2，±4，±8，±16 g)，然后輸出16位ADC三軸加速度數(shù)據(jù).

2.3.2 超聲波傳感器 采用US-100超聲波模塊測量四旋翼飛行器的飛行高度.該超聲波模塊可以實現(xiàn) 2 cm～4.5 m 的非接觸測距功能，支持2.4～5.5 V 的寬電壓輸入范圍，靜態(tài)電流低于 2 mA；可以通過自帶溫度傳感器對測量的四旋翼飛行器高度數(shù)據(jù)進(jìn)行校正；同時具有I2C，UART等多種通信方式，內(nèi)帶看門狗，工作穩(wěn)定可靠.

2.3.3 光流傳感器 湖南優(yōu)象科技有限公司自主研發(fā)的優(yōu)象光流與同類型產(chǎn)品相比，具有價格便宜、體積小、易開發(fā)和不輸于其他光流的高性能等優(yōu)點，因此選擇它作為系統(tǒng)的位置傳感器.該光流模塊在室內(nèi)無法接收 GPS 信號的情況下，可以實時檢測四旋翼飛行器的水平移動距離，再通過 UART 串行總線接口輸出給飛行控制系統(tǒng)，同時通過算法控制飛機(jī)的水平移動距離，從而實現(xiàn)定點懸停功能.

2.4 無線通信模塊

采用DL-20無線串口透傳模塊作為系統(tǒng)的無線通信模塊.該模塊是基于UART接口的全雙工無線透明傳輸模塊，可以在2 400～2 450 MHz公用頻段工作；采用TI的cc2530芯片，符合IEEE 802.15.4協(xié)議；工作電壓為3.0～5.5 V，傳輸距離最遠(yuǎn)可達(dá)250 m，傳輸速率最高可達(dá)3 300 Bps；可以在點對點和廣播2種模式下工作.

3 軟件算法設(shè)計

3.1 主控算法設(shè)計

整個四旋翼控制系統(tǒng)采用雙環(huán)控制算法，包括位置控制環(huán)和姿態(tài)控制環(huán).姿態(tài)控制環(huán)內(nèi)部采用串級PID控制器，內(nèi)環(huán)為角速度PD控制器，外環(huán)為角度PD控制器；位置控制環(huán)也采用串級PID控制器，內(nèi)環(huán)為速度PID控制器，外環(huán)為位移PID控制器.主控制器的結(jié)構(gòu)如圖3所示.

圖3 主控制器的結(jié)構(gòu)Fig. 3 Structure of Main Controller

算法運行時，要在上位機(jī)上選擇是定點控制還是遙控器控制.若是定點控制，則先要從上位機(jī)中獲取期望位移，將期望位移送入位置環(huán)調(diào)控以得到期望角度，再將期望角度送入姿態(tài)控制環(huán)，最后將姿態(tài)控制環(huán)的輸出量計算為電機(jī)的控制量，從而實現(xiàn)四旋翼飛行器的自主飛行.若是遙控器控制，則只要將位置控制環(huán)輸出的期望角度換成遙控器的期望角度，即可使用遙控器控制四旋翼飛行器的飛行.

3.2 姿態(tài)解算算法設(shè)計

四旋翼飛行器的飛控系統(tǒng)，是通過對姿態(tài)解算算法得到的姿態(tài)角數(shù)據(jù)進(jìn)行PID控制，從而實現(xiàn)平穩(wěn)飛行的.如今常用的四旋翼飛行器的姿態(tài)表示方法有歐拉角、方向余弦矩陣和四元數(shù)[3].四元數(shù)表示法相比其他表示法，具有計算量小、算法簡單和操作方便的優(yōu)勢，而且避免了歐拉角的奇異性問題，因此本研究選用它來表示和更新姿態(tài).姿態(tài)解算算法的設(shè)計思路如圖4所示.

圖4 姿態(tài)解算算法的設(shè)計思路Fig. 4 Structure of Posture Solution Algorithm

角速度計在短時間內(nèi)測得的原始數(shù)據(jù)是比較準(zhǔn)確的，但是時間長了之后會受溫度和電壓的影響，此時再對角速度計的值進(jìn)行積分求角度，得到的角度值就有較大的誤差.而加速度計測得的值在短時間內(nèi)因噪聲而有較大的誤差，但是長時間測得的值是比較準(zhǔn)確的.所以，可以將加速度計和角速度計的值進(jìn)行綜合，從而獲得較準(zhǔn)確的姿態(tài)角信息.

姿態(tài)解算算法的流程為：首先將地理坐標(biāo)系下的重力向量轉(zhuǎn)換為機(jī)體坐標(biāo)系下的等效重力向量；然后將等效重力向量與單位加速度向量進(jìn)行叉乘運算以求得誤差，用這個誤差來校正加速度計輸出的數(shù)據(jù)；接著用校正后的角速度計的數(shù)據(jù)進(jìn)行四元數(shù)更新并歸一化；最后將經(jīng)歸一化的四元數(shù)轉(zhuǎn)換為姿態(tài)角.

設(shè)四元數(shù)為q=a+bi+cj+dk，其中a,b,c,d為實數(shù)，i，j，k為虛數(shù)單位.姿態(tài)角、方向余弦矩陣和四元數(shù)的轉(zhuǎn)換關(guān)系為

采用一階畢卡算法對四元數(shù)進(jìn)行更新，得到

3.3 定點控制算法設(shè)計

定點控制算法的流程(圖5)為：先采用串口通信的方式從光流傳感器中讀取原始數(shù)據(jù)，再通過原始數(shù)據(jù)解析出光流的位移和時間數(shù)據(jù)，最后利用位移和時間計算出速度數(shù)據(jù).這里采用角速度計的數(shù)據(jù)對計算得到的速度數(shù)據(jù)進(jìn)行旋度補償，從而使得飛機(jī)在只有旋轉(zhuǎn)沒有平移時的光流傳感器輸出為0.

圖5 定點控制算法的流程Fig. 5 Flow Chart of Fixed Point Algorithm

補償公式為

fx_speed_gyro_fix= ((fx_speed+LIMIT(((Gyro_deg.x)/57.3f),-1,1))*h)，

fy_speed_gyro_fix= ((fy_speed-LIMIT(((Gyro_deg.y)/57.3f),-1,1))*h).

其中：fx_speed_gyro_fix和fy_speed_gyro_fix為旋轉(zhuǎn)補償后的輸出；fx_speed和fy_speed為光流角速度；Gyro_deg.x和Gyro_deg.y為角速度計輸出的角速度經(jīng)過一階低通濾波后的值；h為當(dāng)前超聲波測得的高度；LIMIT ()為將(Gyro_deg.x)/57.3f和(Gyro_deg.y)/57.3f的數(shù)值限制在-1～1之間的限幅函數(shù).

單一的光流速度數(shù)據(jù)由于受到外界環(huán)境的影響而會產(chǎn)生誤差，因此將其與經(jīng)坐標(biāo)轉(zhuǎn)換后的加速度計的數(shù)據(jù)進(jìn)行融合，從而得到較準(zhǔn)確的光流速度數(shù)據(jù).

加速度計數(shù)據(jù)進(jìn)行坐標(biāo)轉(zhuǎn)換的公式為：

(1)

(2)

其中：xH，yH，zH為水平坐標(biāo)系下的加速度計的三軸數(shù)據(jù)；xB，yB，zB為載體坐標(biāo)系下的加速度計的三軸數(shù)據(jù).這里用姿態(tài)轉(zhuǎn)換矩陣來實現(xiàn)加速度計數(shù)據(jù)的坐標(biāo)轉(zhuǎn)換，當(dāng)偏航角φ=0時得到(1)式，然后由(1)式得到(2)式，(2)式可以將載體坐標(biāo)系下的加速度計的三軸數(shù)據(jù)轉(zhuǎn)換到水平坐標(biāo)系下.

接下來對光流的位移和速度數(shù)據(jù)進(jìn)行串級PID調(diào)控，以得到期望角度(圖6).即在得到期望位移后，將期望位移與光流反饋的位移相減得到的差值送入位移PID控制器，輸出期望速度；將期望速度與光流傳感器反饋的速度數(shù)據(jù)相減得到的差值送入速度PID控制器，輸出期望角度.

圖6 定點PID控制算法結(jié)構(gòu)Fig. 6 Structure of Fixed-Point PID Control Algorithm

3.4 姿態(tài)控制算法設(shè)計

姿態(tài)控制算法的結(jié)構(gòu)如圖7所示.算法的流程為：在獲得遙控器或位置環(huán)的期望角度數(shù)據(jù)后，將其與經(jīng)過姿態(tài)解算而得到的角度數(shù)據(jù)相減，再將差值送入角度PD控制器；將角度PD控制器輸出的期望角速度與角速度計測得的角速度數(shù)據(jù)相減，再將差值送入角速度PD控制器；角速度PD控制器的輸出量經(jīng)過油門計算作用到無刷電機(jī)，從而實現(xiàn)對四旋翼飛行姿態(tài)的調(diào)控.

圖7 姿態(tài)控制算法的結(jié)構(gòu)Fig. 7 Structure of the Attitude Control Algorithm

4 四旋翼飛行器的制作與測試

飛行器由1副450 mm軸距的機(jī)架、2對9443自鎖槳、4個2212無刷電機(jī)、4個好盈電調(diào)、1個富斯接收機(jī)和自制的PCB電路板搭配外設(shè)傳感器組成.測試時，先利用上位機(jī)發(fā)送命令幀，告知飛控此次為定點飛行模式，再發(fā)送開啟自主導(dǎo)航定點模式的命令幀，同時發(fā)送期望位移.四旋翼自主起飛，飛行到設(shè)定高度后立即執(zhí)行定點懸停任務(wù).借助Matlab軟件，將無線通信模塊傳送到匿名上位機(jī)上監(jiān)測的參數(shù)(橫滾角、俯仰角和飛行高度)數(shù)據(jù)繪制成曲線，如圖8所示.飛行測試時長為4 min，設(shè)定的期望位移為0，目標(biāo)高度為110 cm.從圖8可以看出：橫滾角和俯仰角的變動范圍均穩(wěn)定在-1°～1°之間，最大偏差小于2.0°；實際飛行時的高度與設(shè)定的目標(biāo)高度的最大誤差小于3 cm.

圖8 飛行參數(shù)曲線Fig. 8 Curve of Flight Parameters

5 結(jié)語

新型四旋翼飛行器采用Texas Instruments公司的TM4C123GH6PM芯片作為系統(tǒng)的控制核心，通過I2C總線通信的方式獲得MPU9250傳感器的九軸數(shù)據(jù)，通過串口通信的方式獲得超聲波和光流的數(shù)據(jù)，再結(jié)合位置控制器和姿態(tài)控制器，實現(xiàn)了室內(nèi)自主導(dǎo)航定點控制系統(tǒng)的設(shè)計.對新型四旋翼飛行器進(jìn)行飛行測試時發(fā)現(xiàn)，在室內(nèi)的普通地面，飛行器可以較準(zhǔn)確地懸停在目標(biāo)點，但是在灰白條紋地面，懸停效果卻不太理想.因此，筆者接下來將考慮利用對天光流來解決這個問題.