尹鵬飛，蘇寧波，郭 達(dá)，付梓軒

(天津中德應(yīng)用技術(shù)大學(xué) 航空航天學(xué)院，天津 300352)

0 引言

多旋翼無(wú)人機(jī)作為空中機(jī)器人的一種研究與應(yīng)用平臺(tái)近年來(lái)持續(xù)受到關(guān)注，而旋翼無(wú)人機(jī)系統(tǒng)則被廣泛應(yīng)用于軍用、民用領(lǐng)域。定點(diǎn)懸停是無(wú)人機(jī)自主飛行控制與應(yīng)用研究的基礎(chǔ)，在執(zhí)行航線飛行與測(cè)繪等作業(yè)任務(wù)時(shí)，通常依托GPS、BD結(jié)合IMU 形成廣域定位系統(tǒng)進(jìn)行位置測(cè)算[1-2]。而對(duì)于固定或移動(dòng)目標(biāo)點(diǎn)跟蹤與著陸時(shí)，通常采用視覺(jué)傳感器進(jìn)行位置估計(jì)與定位[3-4]。相較于相對(duì)成熟的基于GPS定位系統(tǒng)的基于視覺(jué)方式的目標(biāo)點(diǎn)定位通常面臨多點(diǎn)光源照射不均、槳旋產(chǎn)生光影擾動(dòng)、獲取圖像隨本體振動(dòng)等問(wèn)題，常常帶來(lái)定位參考點(diǎn)匹配困擾[5-6]，造成無(wú)人機(jī)系統(tǒng)在期望定位點(diǎn)處沿3個(gè)軸向振蕩漂移。

針對(duì)無(wú)人機(jī)定點(diǎn)環(huán)節(jié)的位姿信息處理，研究者們提出了一系列的算法。張承岫等[7]以經(jīng)典互補(bǔ)濾波為基礎(chǔ)，提出了一種可自適應(yīng)補(bǔ)償系數(shù)的互補(bǔ)濾波算法，完成了相對(duì)準(zhǔn)確的姿態(tài)信息解算。陳孟元等[8]將傳統(tǒng)卡爾曼濾波和互補(bǔ)濾波算法進(jìn)行比較，提出了一種改進(jìn)型的互補(bǔ)濾波算法，結(jié)果顯示，數(shù)據(jù)平滑能力更強(qiáng)，收斂速度更快，有效地實(shí)現(xiàn)了飛行器姿態(tài)數(shù)據(jù)的融合，提高了飛行器的姿態(tài)估計(jì)精度。

本研究以球心半徑20cm作為定點(diǎn)懸停目標(biāo)點(diǎn)域，研究了基于互補(bǔ)濾波的高度信息融合和基于迭代法動(dòng)態(tài)閾值計(jì)算確定平面中心點(diǎn)[9-11]，對(duì)姿態(tài)與高度信息進(jìn)行了串級(jí)PID控制[12]，給出了系列實(shí)驗(yàn)結(jié)果。

1 硬件系統(tǒng)設(shè)計(jì)

基于視覺(jué)定位的多旋翼無(wú)人機(jī)系統(tǒng)采用了通行的模塊化設(shè)計(jì)架構(gòu)，綜合考量了機(jī)載機(jī)電系統(tǒng)的特征尺寸、處理速度、能源配給、有效載荷等因素，進(jìn)行了組件選型、整機(jī)裝調(diào)及飛行性能測(cè)試。定型系統(tǒng)主要包括四旋翼機(jī)體結(jié)構(gòu)、動(dòng)力系統(tǒng)、供電系統(tǒng)、視覺(jué)采集系統(tǒng)、高度采集系統(tǒng)、飛控系統(tǒng)和通信系統(tǒng)。

A.多旋翼機(jī)體結(jié)構(gòu)：根據(jù)所設(shè)計(jì)的無(wú)人機(jī)任務(wù)載荷，估算最大起飛重量約為2.3 kg，采用X型機(jī)體結(jié)構(gòu)，軸距根據(jù)動(dòng)力系統(tǒng)所選槳葉尺寸采用450機(jī)架。B.動(dòng)力系統(tǒng)：依據(jù)無(wú)刷電機(jī)的拉力及效率，確定采用22 161 250 KV的無(wú)刷電機(jī)與PEC-9045槳。根據(jù)電機(jī)需要的最大電流，確定使用30 A電子調(diào)速器。C.供電系統(tǒng)：采用雙源供電模式，動(dòng)力供電與其他電子設(shè)備分別由不同的電池供給，以保證控制信號(hào)的穩(wěn)定性。根據(jù)電機(jī)功率及任務(wù)續(xù)航要求，計(jì)算得出了動(dòng)力電池采用3S 2 600 mAh，視覺(jué)采集系統(tǒng)等其他電子設(shè)備電池采用3S 1 300 mAh。D.視覺(jué)采集系統(tǒng)：基于TSL1401線陣CCD的圖像采集模塊獲取地面標(biāo)識(shí)圖像信息，采用OpenMV3 CamM7 r 2嵌入式圖像處理模塊對(duì)定點(diǎn)標(biāo)志物進(jìn)行識(shí)別和信息處理，為多旋翼無(wú)人機(jī)提供精確的位置信息。E.高度采集系統(tǒng)：采用US-100超聲波測(cè)距模塊，可以對(duì)范圍2～450 cm的區(qū)域進(jìn)行有效探測(cè)，依據(jù)ms 5611氣壓計(jì)測(cè)量數(shù)據(jù)可有效分辨飛行所需任務(wù)高度。MPU 6050加速度計(jì)除提供姿態(tài)解算所需信息外，也提供軸向數(shù)據(jù)用于融合判定高度。F.飛控系統(tǒng)：采用STM32F407芯片作為主控芯片,通過(guò)UART串口讀取各傳感器模塊數(shù)據(jù)，同時(shí)通過(guò)位置導(dǎo)航解算與姿態(tài)解算模塊，輸出各通道所需的PWM信號(hào)至電調(diào)電機(jī)。G.通信系統(tǒng)：采用開(kāi)源飛控的無(wú)線數(shù)傳模塊NRF24L01實(shí)現(xiàn)PC端和機(jī)載飛控及視覺(jué)采集模塊的無(wú)線通信，便于調(diào)試和數(shù)據(jù)觀測(cè)。

2 軟件系統(tǒng)設(shè)計(jì)

多旋翼無(wú)人機(jī)定位系統(tǒng)及解算控制關(guān)系如圖2所示，主要包括4部分：高度信息濾波與融合、目標(biāo)位置信息檢測(cè)、位置與高度導(dǎo)航、無(wú)人機(jī)姿態(tài)與油門(mén)解算。首先由高度信息濾波與融合模塊分別將超聲波和氣壓計(jì)采集得到的高度數(shù)據(jù)與加速度計(jì)獲取數(shù)據(jù)進(jìn)行互補(bǔ)融合濾波，同時(shí)線性CCD采集像素信息，通過(guò)動(dòng)態(tài)閾值計(jì)算及灰度門(mén)限法提取出中心點(diǎn)，得到無(wú)人機(jī)與中心點(diǎn)之間的偏差，輸出至位置與高度導(dǎo)航模塊。將PID計(jì)算的期望姿態(tài)與高度信息輸入姿態(tài)及油門(mén)解算模塊，通過(guò)串級(jí)PID控制器，解算出4個(gè)電機(jī)PWM信號(hào)，輸出相應(yīng)的電機(jī)。

圖1 解算與控制關(guān)系框圖Fig.1 Diagram of calculation and control

2.1 基于互補(bǔ)濾波的高度信息融合

圖2 高度波形Fig.2 Height waveform

多旋翼無(wú)人機(jī)的飛行高度可通過(guò)加速度計(jì)測(cè)得的Z軸加速度進(jìn)行二次積分獲得，如圖2a所示。數(shù)據(jù)的高頻特性較好，能良好地跟隨飛行高度的變化，但隨著時(shí)間的積累會(huì)產(chǎn)生1個(gè)靜態(tài)積分誤差，且誤差隨著時(shí)間的積累逐步增大。通過(guò)氣壓計(jì)和超聲波可以測(cè)量獲取高度信息，從圖2b可以看出。由氣壓計(jì)所測(cè)量的高度數(shù)據(jù)會(huì)在真實(shí)的飛行高度附近浮動(dòng)，存在著1個(gè)高頻噪聲。從圖2c可以看出，超聲波所測(cè)量的高度數(shù)據(jù)，在0～2 m范圍內(nèi)相對(duì)準(zhǔn)確度更高，兩者的低頻特性整體良好，但在實(shí)際高度附近均存在高頻振動(dòng)。

將Z軸向加速度積分處理后與通過(guò)氣壓計(jì)和超聲波獲取的高度信息進(jìn)行互補(bǔ)濾波，通過(guò)低通濾波去除氣壓計(jì)與超聲波測(cè)量值的高頻振動(dòng)干擾，通過(guò)高通濾波去除加速度計(jì)積分帶來(lái)的累積誤差，以速度互補(bǔ)濾波為例，模型如圖3所示。

圖3 互補(bǔ)濾波模型Fig.3 Complementary filtering model

根據(jù)加速度計(jì)、氣壓計(jì)與超聲波的檢測(cè)數(shù)據(jù)不同，對(duì)速度與高度進(jìn)行了兩次互補(bǔ)濾波處理，同時(shí)鑒于氣壓計(jì)在低高度區(qū)間精度較低，超聲波在低高度區(qū)間時(shí)精度相對(duì)較高的特點(diǎn)，對(duì)兩組互補(bǔ)濾波后的數(shù)據(jù)進(jìn)行了加權(quán)融合，高度信息濾波與融合模塊的系統(tǒng)框圖如圖4所示。

圖4 數(shù)據(jù)融合關(guān)系框圖Fig.4 Data fusion relation diagram

2.2 基于全局閾值的迭代閾值計(jì)算

飛行過(guò)程中獲取的視覺(jué)信息因受設(shè)備振動(dòng)、光照變化、槳旋擾動(dòng)影響，導(dǎo)致對(duì)參考點(diǎn)模型的識(shí)別成功率通常較低。利用迭代算法獲取標(biāo)志點(diǎn)分割的自適應(yīng)閾值，步驟如下：

(1)無(wú)人機(jī)起飛前在起始位置獲取靜態(tài)圖像，圖像尺寸與位置固定，根據(jù)雙峰值均值算法，確定閾值初始值T(K)。

(2)依據(jù)閾值T(K)將圖像分為目標(biāo)和背景，灰度等級(jí)大于等于T(K)的像素點(diǎn)組成認(rèn)定為目標(biāo)，灰度等級(jí)小于T(K)的像素點(diǎn)組成認(rèn)定為背景。

(3)分別計(jì)算目標(biāo)和背景區(qū)域內(nèi)像素的灰度平均值u0(K)和uB(K)。

(4)計(jì)算更新閾值

(1)

(5)當(dāng)T(K+1)-T(K)<δ時(shí)，其中δ為精度，則結(jié)束搜索，將T(K)作為所求的閾值，否則更新閾值為T(mén)(K+1)并轉(zhuǎn)至步驟(2)繼續(xù)迭代處理。

2.3 確定中心點(diǎn)位置

根據(jù)動(dòng)態(tài)閾值計(jì)算模塊得出該循環(huán)周期最佳閾值T(K)，將灰度圖像中大于T(K)的像素點(diǎn)位置位置Pij坐標(biāo)求平均，即可確定目標(biāo)中心點(diǎn)位置，計(jì)算如下：

(2)

(3)

其中，imax，jmax分別為目標(biāo)像素點(diǎn)Pij的二維數(shù)組長(zhǎng)度；xPij，yPij為各目標(biāo)像素點(diǎn)的橫縱坐標(biāo)；X，Y為最終計(jì)算出中心點(diǎn)Pij的坐標(biāo)。

2.4 基于串級(jí)PID的姿態(tài)和高度解算設(shè)計(jì)

飛控的姿態(tài)與高度解算采用串級(jí)PID控制方式。以姿態(tài)控制為例，串級(jí)PID控制系統(tǒng)由兩個(gè)串連起來(lái)的PID調(diào)節(jié)器組成，分別為姿態(tài)角度控制環(huán)與角速度控制環(huán)，即響應(yīng)速度快的姿態(tài)角速度控制放在內(nèi)環(huán)，響應(yīng)速度慢的姿態(tài)角度控制放在外環(huán)。先給定俯仰角的參考值ΦP，然后經(jīng)外環(huán)PID控制算法計(jì)算后得到俯仰角速度的參考值ΦP，再經(jīng)內(nèi)環(huán)PID控制算法計(jì)算后可以得俯仰控制系統(tǒng)的輸入值，提供給電機(jī)轉(zhuǎn)速控制器。相比單級(jí)的PID控制，串級(jí)PID控制響應(yīng)更為明顯，系統(tǒng)工作效率相對(duì)較高，能夠更為穩(wěn)定地對(duì)姿態(tài)變化進(jìn)行控制。

3 實(shí)驗(yàn)驗(yàn)證與分析

實(shí)驗(yàn)場(chǎng)區(qū)設(shè)置在學(xué)校的實(shí)訓(xùn)中心飛行實(shí)驗(yàn)區(qū)，如圖5所示，地面布設(shè)KT板，并標(biāo)記了半徑10 cm的中心點(diǎn)域。通過(guò)3組實(shí)驗(yàn)對(duì)以下3項(xiàng)關(guān)鍵內(nèi)容進(jìn)行了驗(yàn)證：

(1)通過(guò)不同高度連續(xù)飛行實(shí)驗(yàn)，驗(yàn)證互補(bǔ)濾波算法對(duì)高度數(shù)據(jù)融合的效果，證明其有效性。

(2)通過(guò)強(qiáng)弱光條件對(duì)比實(shí)驗(yàn)，驗(yàn)證基于全局閾值的迭代閾值計(jì)算對(duì)環(huán)境的適用性。

(3)通過(guò)定點(diǎn)懸停實(shí)驗(yàn)，驗(yàn)證定位系統(tǒng)在水平軸向和垂直軸向?qū)C(jī)體控制的穩(wěn)定性。

3.1 不同高度連續(xù)飛行實(shí)驗(yàn)

在實(shí)驗(yàn)場(chǎng)區(qū)進(jìn)行了不同高度連續(xù)飛行實(shí)驗(yàn)，飛行高度主要集中在20～160 cm。從圖6可以看出，互補(bǔ)濾波所得到的飛行高度數(shù)據(jù)，具有加速度計(jì)、氣壓計(jì)與超聲波三者所得高度各自的優(yōu)點(diǎn)，平均誤差0.18 cm，能較為準(zhǔn)確地反映出無(wú)人機(jī)的實(shí)際高度信息，最大誤差僅0.56 cm，主要出現(xiàn)在加速度值急劇變化的附近，對(duì)目標(biāo)任務(wù)整體影響極為有限。

圖6 互補(bǔ)濾波融合處理后的高度波形Fig.6 Height waveform after complementary filtering and fusion

3.2 不同光強(qiáng)下閾值自適應(yīng)實(shí)驗(yàn)

為比較在不同光強(qiáng)下閾值對(duì)目標(biāo)中心點(diǎn)域(前景)與點(diǎn)域外場(chǎng)區(qū)(背景)的分割效果。在背景灰度均值100和30兩種情況下，對(duì)前景、背景灰度值及動(dòng)態(tài)閾值的變化進(jìn)行了實(shí)時(shí)記錄。由圖7a、b可以看出，兩種條件下動(dòng)態(tài)閾值全部處于目標(biāo)中心點(diǎn)域灰度值和點(diǎn)域外場(chǎng)區(qū)灰度值之間，整體區(qū)分效果良好，對(duì)兩者均能夠進(jìn)行有效分割，從而達(dá)到了對(duì)定位目標(biāo)點(diǎn)的辨識(shí)。在弱光條件下，動(dòng)態(tài)閾值有部分離散采樣數(shù)據(jù)向背景偏移，但在單次采樣對(duì)比時(shí)均未發(fā)生混雜，未對(duì)目標(biāo)辨識(shí)產(chǎn)生實(shí)質(zhì)影響。

3.3 定點(diǎn)懸停實(shí)驗(yàn)

在實(shí)驗(yàn)場(chǎng)區(qū)進(jìn)行了定點(diǎn)懸停實(shí)驗(yàn)，高度設(shè)定800 cm，從圖8a中可以看出，飛行器能夠穩(wěn)定控制在目標(biāo)高度線上下，平均誤差不超過(guò)2 cm，最大誤差5.4 cm。與此同時(shí)，從圖8b中可以看出，在水平軸向上，飛行器能夠被控制在目標(biāo)中心點(diǎn)半徑20 cm圓域內(nèi)，表現(xiàn)出了良好的定點(diǎn)懸停特性。

圖7 強(qiáng)弱光條件下動(dòng)態(tài)閾值變化曲線Fig.7 Dynamic threshold curve under strong and weak light condition

圖8 定點(diǎn)懸停實(shí)驗(yàn)水平與垂直軸向數(shù)據(jù)變化Fig.8 Horizontal and vertical axial data changes in fixed-point hovering experiment

4 結(jié)論

針對(duì)旋翼無(wú)人機(jī)定點(diǎn)懸停中存在的視覺(jué)模型匹配易受外界環(huán)境因素影響、高度噪聲復(fù)雜等問(wèn)題，采用基于迭代算法的自適應(yīng)閾值的圖像分割算法與互補(bǔ)濾波高度信息噪聲處理，設(shè)計(jì)了串級(jí)PID定點(diǎn)控制器，得到以下結(jié)論。

(1)對(duì)加速度計(jì)、氣壓計(jì)、超聲波等多源數(shù)據(jù)進(jìn)行了互補(bǔ)濾波與融合處理，實(shí)現(xiàn)低通濾波消除高頻振動(dòng)干擾，同時(shí)高通濾波對(duì)于去除累積誤差具有明顯效果。

(2)通過(guò)迭代算法計(jì)算動(dòng)態(tài)閾值，解決了不同光照條件下視覺(jué)模型匹配的問(wèn)題，能夠有效分割圖像邊界。

(3)基于串級(jí)PID實(shí)現(xiàn)了姿態(tài)與油門(mén)的解算，形成了多源信息融合的多旋翼無(wú)人機(jī)室內(nèi)定位系統(tǒng)，開(kāi)展了不同高度連續(xù)飛行、不同光強(qiáng)下閾值自適應(yīng)以及定點(diǎn)懸停系列實(shí)驗(yàn)，充分驗(yàn)證了系統(tǒng)的有效性。