一種四旋翼飛行器超聲波避障系統

胥加林，游安華，蔡璨

(南京理工大學 a. 機械工程學院; b. 工程訓練中心，江蘇 南京 210094)

0 引言

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

本文開發的四旋翼飛行器避障系統，首先，采集超聲波數據，并在分析該數據的基礎上，結合一階低通濾波、限幅濾波和中位值濾波對其進行處理，根據處理后的數據通過控制飛行器Y軸和Z軸來實現避障，且前、后、左、右4個超聲波模塊對飛行器的控制是線性疊加的，相互之間不會產生干擾，最后進行了飛行測試。

1 系統硬件設計

超聲波避障系統主要包括STM32微控制器、4個超聲波測距模塊等。圖1為系統總體結構圖。

圖1 系統總體結構圖

障礙物距離檢測方法有雷達測距法、超聲波測距法、紅外測距法等。其中，超聲波測距法具有縱向分辨率高、成本低、體積小等特點，適合飛行器自主飛行時對障礙物距離的檢測。

系統選擇基于渡越時間法的US-100超聲波測距模塊，該模塊可實現2～450 cm的非接觸測距，模塊自帶的溫度傳感器可對測距結果進行校正，工作電壓為2.4～5.5 V，模塊通過串口與主控芯片進行通信。

2 系統軟件設計

系統軟件設計主要包括超聲波數據的采集與傳輸、超聲波數據的濾波、避障飛行的控制策略。

2.1 超聲波數據的采集與傳輸

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

2.2 超聲波數據的濾波

超聲波信號具有以下特點：

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

2) 回波信號的幅值隨測量距離的增大呈指數規律衰減，且脈沖發射結束后，由于慣性換能器會繼續振動，影響回波信號的接收。

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

圖2 超聲波數據波形

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

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

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

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

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

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

(1)

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

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

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

(2)

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

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

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

(3)

式中，a為濾波系數；t為采樣時間。

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

圖3 濾波算法流程圖

圖2中的原始數據經過濾波算法處理后的數據波形如圖4所示。

圖4 處理后的超聲波數據

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

2.3 避障控制策略

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

圖5 避障控制流程圖

3 測試驗證

為驗證該系統的有效性，對該四旋翼飛行器進行了實際飛行測試。試驗條件為：飛行器以0.4 m/s的速度向墻壁飛去，測量飛行器與墻壁之間的最小距離，共進行了20次試驗，結果如表1所示。

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

表1 飛行試驗結果

4 結語

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

參考文獻:

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

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

[3] 李震，盧加納，洪添勝，等. 山地果園鋼絲繩牽引貨運機超聲波避障系統[J]. 農業機械學報， 2011，42(10): 116-120.

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

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