胥加林，游安華，蔡璨

(南京理工大學(xué) a. 機(jī)械工程學(xué)院; b. 工程訓(xùn)練中心，江蘇 南京 210094)

0 引言

四旋翼飛行器機(jī)械結(jié)構(gòu)簡單、操控性能優(yōu)越，具有良好的軍事偵察、搜索救援性能[1-2]。且隨著微電子技術(shù)、傳感器技術(shù)的進(jìn)步，四旋翼飛行器得到迅速發(fā)展[3]。其中飛行器飛行過程中避障是一個(gè)重要問題。國內(nèi)外許多學(xué)者對(duì)其進(jìn)行了研究，文獻(xiàn)[4]基于SRF04超聲波、STC單片機(jī)等模塊，開發(fā)了山地果園貨運(yùn)機(jī)避障系統(tǒng)，提高了軌道貨運(yùn)系統(tǒng)的安全性與穩(wěn)定性。文獻(xiàn)[5]提出一種基于金字塔和光流融合的飛行器避障算法，該算法準(zhǔn)確率高，但通過軟件實(shí)現(xiàn)該算法有待進(jìn)一步的研究。

本文開發(fā)的四旋翼飛行器避障系統(tǒng)，首先，采集超聲波數(shù)據(jù)，并在分析該數(shù)據(jù)的基礎(chǔ)上，結(jié)合一階低通濾波、限幅濾波和中位值濾波對(duì)其進(jìn)行處理，根據(jù)處理后的數(shù)據(jù)通過控制飛行器Y軸和Z軸來實(shí)現(xiàn)避障，且前、后、左、右4個(gè)超聲波模塊對(duì)飛行器的控制是線性疊加的，相互之間不會(huì)產(chǎn)生干擾，最后進(jìn)行了飛行測(cè)試。

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

超聲波避障系統(tǒng)主要包括STM32微控制器、4個(gè)超聲波測(cè)距模塊等。圖1為系統(tǒng)總體結(jié)構(gòu)圖。

圖1 系統(tǒng)總體結(jié)構(gòu)圖

障礙物距離檢測(cè)方法有雷達(dá)測(cè)距法、超聲波測(cè)距法、紅外測(cè)距法等。其中，超聲波測(cè)距法具有縱向分辨率高、成本低、體積小等特點(diǎn)，適合飛行器自主飛行時(shí)對(duì)障礙物距離的檢測(cè)。

系統(tǒng)選擇基于渡越時(shí)間法的US-100超聲波測(cè)距模塊，該模塊可實(shí)現(xiàn)2～450 cm的非接觸測(cè)距，模塊自帶的溫度傳感器可對(duì)測(cè)距結(jié)果進(jìn)行校正，工作電壓為2.4～5.5 V，模塊通過串口與主控芯片進(jìn)行通信。

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

系統(tǒng)軟件設(shè)計(jì)主要包括超聲波數(shù)據(jù)的采集與傳輸、超聲波數(shù)據(jù)的濾波、避障飛行的控制策略。

2.1 超聲波數(shù)據(jù)的采集與傳輸

模塊上電前，插上模式選擇跳線帽，使其工作于串口觸發(fā)模式，并設(shè)置STM32微控制器串口的波特率為9 600、1位起始位、1位停止位、8位數(shù)據(jù)位、無奇偶校驗(yàn)、無流控制。然后在TX管腳輸入0X55，觸發(fā)模塊發(fā)出40 kHz的脈沖，當(dāng)檢測(cè)到回波信號(hào)后，對(duì)其進(jìn)行溫度補(bǔ)償，最后通過RX管腳輸出結(jié)果到微控制器，用于后續(xù)的避障控制。

2.2 超聲波數(shù)據(jù)的濾波

超聲波信號(hào)具有以下特點(diǎn)：

1) 常溫下，超聲波的傳播速度為340 m/s，但溫度每升高1 ℃，速度約增加0.6 m/s；

2) 回波信號(hào)的幅值隨測(cè)量距離的增大呈指數(shù)規(guī)律衰減，且脈沖發(fā)射結(jié)束后，由于慣性換能器會(huì)繼續(xù)振動(dòng)，影響回波信號(hào)的接收。

由于以上因素，需對(duì)超聲波數(shù)據(jù)進(jìn)行濾波處理，圖2為四旋翼飛行器飛行過程中，超聲波原始數(shù)據(jù)，其中的小圖為3～5 s時(shí)間段數(shù)據(jù)的放大圖。

圖2 超聲波數(shù)據(jù)波形

圖2顯示了飛行器先遠(yuǎn)離障礙物，后靠近障礙物時(shí)，超聲波數(shù)據(jù)的變化波形。分析數(shù)據(jù)得：當(dāng)障礙物距離在100～200 cm之間時(shí)，超聲波數(shù)據(jù)小幅波動(dòng)，平均波動(dòng)范圍為70 cm；當(dāng)障礙物距離>200 cm時(shí)，超聲波數(shù)據(jù)出現(xiàn)大幅度的波動(dòng)，最大距離達(dá)1 100 cm，最小距離為50 cm，超出超聲波的量程，此時(shí)的數(shù)據(jù)不可用。分析其中的小圖得：障礙物距離<100 cm時(shí)，超聲波數(shù)據(jù)幾乎沒有波動(dòng)。

超聲波數(shù)據(jù)的頻率為75 Hz，飛行器的飛行速度為0.4 m/s，所以超聲波數(shù)據(jù)的變化是緩慢的，且由圖2可知，兩次數(shù)據(jù)之差≤5 cm。結(jié)合飛行器的飛行特點(diǎn)和超聲波數(shù)據(jù)的特點(diǎn)，設(shè)計(jì)的濾波算法如下：

1) 障礙物距離<50 cm時(shí)，數(shù)據(jù)波動(dòng)不大，采用計(jì)算量小的一階低通濾波法，快速更新障礙物距離。

2) 障礙物距離在50～200 cm之間時(shí)，結(jié)合限幅濾波、中位值濾波對(duì)此時(shí)的數(shù)據(jù)進(jìn)行濾波。舍棄本次數(shù)據(jù)與上次數(shù)據(jù)之差>10 cm數(shù)據(jù)，舍棄本次數(shù)據(jù)>600 cm的數(shù)據(jù)，然后對(duì)余下的數(shù)據(jù)進(jìn)行排序，取中間的一個(gè)數(shù)據(jù)作為此次測(cè)得的距離。

3) 障礙物距離>200 cm時(shí)，接近傳感器的量程，數(shù)據(jù)無規(guī)律可循，直接舍棄，并將此時(shí)的距離設(shè)為600 cm，在避障距離之外。

一階低通濾波算法模型為：

(1)

式中，ωc為介質(zhì)角頻率，rad/s；f為截止頻率，Hz。

用軟件模擬硬件一階低通濾波算法，將其用差分方程表示為：

Yn=a·Xn+(1-a)·Yn-1

(2)

式中，Xn為本次采集值；Yn-1為上次的濾波輸出值；a為濾波系數(shù)，其值通常遠(yuǎn)<1；Yn為本次濾波輸出值。

由上式可以看出，本次濾波的輸出值主要取決于上次濾波的輸出值，本次采樣值對(duì)濾波輸出有些修正作用。

濾波算法的截止頻率可由式(3)計(jì)算。

(3)

式中，a為濾波系數(shù)；t為采樣時(shí)間。

此濾波算法最終要通過軟件實(shí)現(xiàn)，為計(jì)算方便，a取一整數(shù)，1-a用256-a來代替，計(jì)算結(jié)果取最低字節(jié)即可，并在KeilARM軟件中用C語言實(shí)現(xiàn)該濾波算法，圖3為超聲波數(shù)據(jù)濾波算法的流程圖。

圖3 濾波算法流程圖

圖2中的原始數(shù)據(jù)經(jīng)過濾波算法處理后的數(shù)據(jù)波形如圖4所示。

圖4 處理后的超聲波數(shù)據(jù)

由圖4可知，5.5～9 s時(shí)，障礙物距離>200 cm，在避障控制范圍之外，便將其設(shè)為600 cm，不會(huì)使飛行器誤操作；分析其中的小圖可知，當(dāng)障礙物距離<200 cm時(shí)，經(jīng)過處理的數(shù)據(jù)，更加平滑，滿足避障控制的要求。

2.3 避障控制策略

通過試驗(yàn)測(cè)試可得，開始避障距離為160 cm時(shí)，飛行器在距離障礙物約50 cm處可以完全停住?；诎踩目紤]，設(shè)置開始避障距離為180 cm。當(dāng)障礙物距離<20 cm時(shí)，油門通道值設(shè)為最小值1 000；當(dāng)20 cm≤障礙物距離<180 cm時(shí)，控制飛行器左轉(zhuǎn)并后退，執(zhí)行避障控制，且隨著距離的減小控制量線性增加。圖5為避障控制流程圖。

圖5 避障控制流程圖

3 測(cè)試驗(yàn)證

為驗(yàn)證該系統(tǒng)的有效性，對(duì)該四旋翼飛行器進(jìn)行了實(shí)際飛行測(cè)試。試驗(yàn)條件為：飛行器以0.4 m/s的速度向墻壁飛去，測(cè)量飛行器與墻壁之間的最小距離，共進(jìn)行了20次試驗(yàn)，結(jié)果如表1所示。

由表1可知，完成避障控制時(shí)，飛行器與障礙物之間的距離基本在50～60 cm之間，飛行器距離障礙物的最小距離為43 cm，滿足四旋翼飛行器的安全需要。

表1 飛行試驗(yàn)結(jié)果

4 結(jié)語

本文基于US-100超聲波模塊和STM32微控制器開發(fā)了一種四旋翼飛行器避障系統(tǒng)。根據(jù)超聲波數(shù)據(jù)的特點(diǎn)，對(duì)其進(jìn)行濾波，濾波后的數(shù)據(jù)滿足避障控制的要求；設(shè)計(jì)了避障控制策略，進(jìn)行了飛行試驗(yàn)。試驗(yàn)結(jié)果顯示：當(dāng)飛行器以0.4 m/s的速度向墻壁飛去，開始避障距離為180 cm時(shí)，避障控制完成后，飛行器與障礙物之間的距離基本在50～60 cm之間，且最小距離為43 cm，滿足四旋翼飛行器飛行時(shí)的安全需要。

參考文獻(xiàn):

[1] 于雅莉，孫楓，王元昔. 基于多傳感器的四旋翼飛行器硬件電路設(shè)計(jì)[J]. 傳感器與微系統(tǒng)，2011,30(8): 113-115.

[2] 宋召青，劉曉，趙賀偉. 基于反演的四旋翼無人飛行器姿態(tài)自適應(yīng)控制[J]. 機(jī)械制造與自動(dòng)化，2014,43(3):201-203.

[3] 李震，盧加納，洪添勝，等. 山地果園鋼絲繩牽引貨運(yùn)機(jī)超聲波避障系統(tǒng)[J]. 農(nóng)業(yè)機(jī)械學(xué)報(bào)， 2011，42(10): 116-120.

[4] 趙海，陳星池，王加亮，等. 基于四軸飛行器的單目視覺避障算法[J]. 光學(xué)精密工程， 2014，22(8): 2232-2241.

[5] 孫賢安，陳南，王金湘. 基于51單片機(jī)的小車避障電路實(shí)現(xiàn)[J]. 機(jī)械制造與自動(dòng)化，2005,34(5):76-78.