基于超聲波傳感器的四旋翼飛行器室內(nèi)定位與控制

藍波，吳昊，郭清才，郭朋，劉興東，曾飛

（北京石油化工學院信息工程學院，北京102617）

近年來，四旋翼飛行器的應(yīng)用與研究廣泛受到關(guān)注，隨著嵌入式處理器、微傳感器技術(shù)和控制理論的發(fā)展和成熟，四旋翼飛行器逐步向高效、多功能化方向發(fā)展，廣泛應(yīng)用于軍事、民用、以及科學研究等多個領(lǐng)域[1]。與此同時，四旋翼飛行器在定位與控制方面也得到了很大發(fā)展，目前定位技術(shù)主要采用GPS、人工視覺、光流法和超聲波等實現(xiàn)。GPS在四旋翼導(dǎo)航與定位方面應(yīng)用已經(jīng)相對成熟，但其主要用于室外，在室內(nèi)應(yīng)用時存在信號采集不穩(wěn)定，定位不精確的情況[2-3]。人工視覺的攝像頭對室內(nèi)光線的要求較高，當光線偏暗或明暗變化較快時，存在采不到數(shù)據(jù)或誤采集情況；另一方面，攝像頭采集的數(shù)據(jù)量巨大，運算處理時間較長，實時性得不到可靠保障[4-5]。近年來，使用光流法進行飛行導(dǎo)航定位和避障也成為小型飛行器研究領(lǐng)域的一個熱點問題，但光流法算法復(fù)雜，對處理器運算速度要求較高，定位精度還不高［6-7］。超聲波技術(shù)具有不受室內(nèi)光線影響、數(shù)據(jù)處理簡單、測距精度高、成本低廉、方便應(yīng)用等優(yōu)點，在機器人測距與定位方面得到廣泛應(yīng)用。本文提出一種將超聲波傳感器應(yīng)用于四旋翼飛行器進行室內(nèi)定位與運動控制的方案[8-9]。

1 系統(tǒng)架構(gòu)

設(shè)計采用PIXHawk作為飛行控制芯片，6個AVR單片機ATmega 328P作為協(xié)處理控制芯片。安裝在四旋翼飛行器不同位置的超聲波傳感器負責接收來自各個方向障礙物的反射波，并傳送給AVR單片機，通過計算發(fā)射與反射之間的時間差得到飛行器與室內(nèi)障礙物之間的距離。負責高度測量的AVR單片機通過IIC的方式與飛行控制芯片通信，負責前、后、左、右4個水平方向距離測量的AVR單片機先通過IIC通訊方式將數(shù)據(jù)先發(fā)送到主AVR單片機，主AVR單片機再通過串口通訊的方式將數(shù)據(jù)發(fā)送到飛行控制芯片中。飛行控制芯片將五組數(shù)據(jù)整合處理后得到四旋翼飛行器在房間內(nèi)的方位，并輸出四路PWM波信號到電調(diào)，進而控制無刷電機，實現(xiàn)對四旋翼飛行器飛行姿態(tài)的控制。整個系統(tǒng)的架構(gòu)如圖1所示。

圖1 系統(tǒng)整體架構(gòu)

2 硬件系統(tǒng)實現(xiàn)

2.1 超聲波傳感器

方案選用US-100超聲波傳感器，如圖2所示。該傳感器探測距離為2～450 cm，精度為0.3 cm±1%，可以實現(xiàn)對距離的精確測量，確保室內(nèi)定位的精確度。同時，US-100自帶溫度補償功能，可以對測距結(jié)果進行校正，提高了測距精度。

圖2 US-100超聲波傳感器

US-100超聲波測距模塊測距分為兩種模式測距分為兩種模式，一種為電平觸發(fā)測距模式，另一種為串口觸發(fā)測距模式，本設(shè)計采用電平觸發(fā)模式。該模式只需要在Trig/TX管腳輸入一個10US以上的高電平，系統(tǒng)便可發(fā)出8個40 kHz的超聲波脈沖，然后檢測回波信號。當檢測到回波信號后，模塊還要進行溫度值的測量，然后根據(jù)當前溫度對測距結(jié)果進行校正，將校正后的結(jié)果通過Echo/RX管腳輸出。在此模式下，模塊將距離值轉(zhuǎn)化為340 μm/s時的時間值的2倍，通過Echo端輸出一高電平，可根據(jù)此高電平的持續(xù)時間來計算距離值。即距離值為（高電平時間*340 μm/s）/2[10-13]。電平觸發(fā)模式時序如圖3所示。

圖3 US-100電平觸發(fā)時序

2.2 飛行控制芯片

飛行控制芯片PIXHawk是一個開源的高端自動駕駛儀硬件，采用多余度供電系統(tǒng)，可實現(xiàn)不間斷供電，保障了四旋翼飛行器飛行時的穩(wěn)定性以及安全性。另一方面，PIXHawk5個串口可以實現(xiàn)與5個協(xié)處理控制芯片單獨通信，加快了數(shù)據(jù)傳輸速度。PIXHawk的MCU采用32位STM32F427，STM32F427擁有Cortex-M4核心同時帶有浮點運算器，處理速度快，對飛行的實時性控制提供了保障[14]。飛行控制芯片如圖4所示。

圖4 飛行控制芯片PIXHawk

2.3 協(xié)處理控制芯片

AVR ATmega 328P作為協(xié)處理控制芯片，用來處理來自不同方位超聲波傳感器接收到的數(shù)據(jù)，處理后通過串口以及IIC的通訊方式發(fā)送到飛行控制芯片上。采用協(xié)處理控制芯片進行預(yù)處理超聲波傳感器接收到的數(shù)據(jù)大大節(jié)省了主控制芯片處理數(shù)據(jù)的負擔，提高了四旋翼飛行控制的實時性[15-16]。

水平方向共有前后左右4個AVR單片機以及與其對應(yīng)的US-100超聲波傳感器，二者通過直插接口的方式相連接。水平方向AVR單片機對US-100超聲波傳感器反饋回的數(shù)據(jù)進行處理，然后以IIC方式發(fā)送至主AVR單片機中。水平方向AVR單片機和超聲波傳感器數(shù)據(jù)通信電路如圖5所示。

圖5 水平方向AVR數(shù)據(jù)通信電路

主AVR單片機由IIC方式接收到水平四個方向的數(shù)據(jù)后進行數(shù)據(jù)整合，然后經(jīng)過串口通訊的方式發(fā)送給飛行控制芯片，飛行控制芯片通過計算得出偏差，發(fā)出相對應(yīng)的四路PWM波到四個電調(diào)，經(jīng)信號放大后，電調(diào)產(chǎn)生相應(yīng)的電流驅(qū)動無刷電機，對四旋翼飛行器水平位置進行調(diào)整，進而實現(xiàn)飛行器避障飛行以及定點等穩(wěn)定的飛行狀態(tài)[17-18]。主AVR數(shù)據(jù)通信電路如圖6和圖7所示。

為減輕主AVR單片機處理數(shù)據(jù)的復(fù)雜程度，四旋翼飛行器的高度測量由高度AVR單片機單獨負責。負責高度測量的超聲波傳感器US-100返回數(shù)據(jù)后直接傳送給高度AVR單片機，由其對數(shù)據(jù)進行整合計算后通過IIC方式傳送到飛行控制芯片，根據(jù)偏差產(chǎn)生相應(yīng)PWM波對四旋翼飛行器的高度進行調(diào)整。高度AVR數(shù)據(jù)通信及IIC接口電路如圖8所示。

圖6 主AVR數(shù)據(jù)通信網(wǎng)絡(luò)

圖7 主AVR數(shù)據(jù)通信接口

3 軟件系統(tǒng)實現(xiàn)

協(xié)處理控制芯片程序主要實現(xiàn)對各個方向障礙物距離的測量并將數(shù)據(jù)發(fā)送至飛控芯片。AVR單片機程序流程圖如圖9所示。

飛控程序用于對飛行姿態(tài)的控制，同時利用協(xié)處理控制芯片提供的距離數(shù)據(jù)進行偏差調(diào)整，使四旋翼飛行器達到預(yù)定房間內(nèi)位置，實現(xiàn)飛行器室內(nèi)定位懸停與定距離飛行。飛行控制芯片程序流程圖如圖10所示。

圖8 高度AVR數(shù)據(jù)通信電路

圖9 協(xié)處理控制芯片程序流程圖

圖10 飛控程序流程圖

協(xié)處理控制芯片中寫入超聲波測距程序以及與飛行控制芯片串口通信的程序。因四旋翼飛行器對實時性要求較高，所以將串口通信波特率設(shè)置為115200以提高數(shù)據(jù)的傳送速率。飛行控制芯片中的程序主要負責飛行姿態(tài)的調(diào)整實現(xiàn)室內(nèi)定位，首先設(shè)置期望四旋翼飛行器飛到的高度：

#define Altitude_Expectation_cm 70

設(shè)置4個方向超聲波傳感器期望的懸停定位距離：

uint16_t Rng_Expectation_cm[SONARNUMBER]={200，200，100，100}；

設(shè)定變量用于記錄四旋翼在調(diào)整飛行姿態(tài)定位時超聲波小于特定值的次數(shù)，當記錄次數(shù)達到特定值時觸發(fā)中斷，四旋翼飛行器提前做出減速動作，避免由于慣性帶來的碰撞：

uint16_t Count_In100=0；//超聲波探測距離小于2 m的次數(shù)

uint16_t Count_In50=0；//超聲波探測距離小于1 m的次數(shù)

如果左側(cè)超聲波探測距離小于期望值距離或探測距離小于設(shè)定的安全距離，就向右飛，控制系數(shù)越大，四旋翼飛行器飛行速度越快：

if（Rng_Target_cm[S_LEFT]＜Rng_Expectation_cm[S_LEFT]&&（Rng_Target_cm[S_LEFT]＞ 0））

{RollControl+=（Rng_Expectation_cm[S_LEFT]-Rng_Target_cm[S_LEFT]）*5；}

如果右側(cè)超聲波探測距離小于期望值距離或探測距離小于設(shè)定的安全距離，就向左飛，控制系數(shù)越大，四旋翼飛行器飛行速度越快：

if （Rng_Target_cm[S_RIGHT] ＜Rng_Expecta?tion_cm[S_RIGHT]&&（Rng_Target_cm[S_RIGHT]＞0））

{RollControl-=（Rng_Expectation_cm[S_RIGHT]-Rng_Target_cm[S_RIGHT]）*5；}

同理，前后兩個方向也按照以上算法對四旋翼位置進行調(diào)整。

4 結(jié)論

在試驗過程中，四旋翼飛行器懸停定位誤差前后左右四個方向不超過2 cm，高度誤差不超過1 cm。試驗數(shù)據(jù)結(jié)果表明，基于超聲波傳感器的四旋翼飛行器室內(nèi)定位與控制方案，有效地解決了飛行器在室內(nèi)的懸停定位問題，可以用于室內(nèi)飛行避障。在室外環(huán)境時，可以通過與GPS等技術(shù)的結(jié)合，為四旋翼飛行器提供更加精確的懸停定位。

參考文獻：

[1]岳基隆，張慶杰，朱華勇.微小型四旋翼無人機研究進展及關(guān)鍵技術(shù)淺析[J].電光與控制，2010（10）：46-52.

[2]黃艷虎，陳家裕，陳倍昌，等.基于GPS的四旋翼飛行器定位系統(tǒng)的設(shè)計[J].電腦知識與技術(shù)，2016（8）：205-206.

[3]He R，Prentice S，Roy N.Planning in information space for a quadrotor helicopter in a GPS-denied environment[C]//Robotics and Automation，2008.ICRA 2008.IEEE International Conference on.IEEE，2008：1814-1820.

[4]陳小龍，唐強，車軍，等.基于人工視覺的四旋翼飛行器室內(nèi)定位與控制[J].兵工自動化，2012（5）：61-64.

[5]曹美會，鮮斌，張旭，等.基于視覺的四旋翼無人機自主定位與控制系統(tǒng)[J].信息與控制，2015（2）：190-196，202.

[6]宋宇，翁新武，郭昕剛.基于光流和慣性導(dǎo)航的小型無人機定位方法[J].傳感器與微系統(tǒng)，2015（1）：13-16.

[7]Zingg Simon，Scaramuzza Davide，Weiss Stephan，et al.MAV navigation through indoor corridors us?ing optical flow[C]//IEEE International Conference on Robotics and Automation，Anchorage，USA，2010：3361-3368.

[8]楊淑媛.旋翼無人機室內(nèi)自主避障飛行研究[J].山西電子技術(shù)，2016（6）：86-88.

[9]韓軍，常瑞麗.智能移動機器人超聲波測距定位系統(tǒng)的研究[J].機械設(shè)計與制造，2010（1）：164-166.

[10]程華彬，張穎超.基于超聲波傳感器的移動機器人定位研究[J].傳感器與微系統(tǒng)，2013（4）：58-60，64.

[11]方登建，戴宇進，趙紅超，康宇航.四旋翼無人飛行器的室內(nèi)定位、導(dǎo)航與建圖[J].海軍航空工程學院學報，2014（4）：345-350.

[12]Tsai C C.A localization system of a mobile robot by fusing dead-reckoning and ultrasonic measure?ments[J].IEEE Transactions on Instrumentation and Measurement，1998，47（5）：1399-1404.

[13]Ohya I，Kosaka A，Kak A.Vision-based naviga?tion by a mobile robot with obstacle avoidance us?ing single-camera vision and ultrasonic sensing[J].IEEE Transactions on Robotics and Automation，1998，14（6）：969-978.

[14]Meier L，Tanskanen P，F(xiàn)raundorfer F，et al.The PIXHAWK open-source computer vision frame?work for MAVs[J].The International Archives of the Photogrammetry，Remote Sensing and Spatial Information Sciences，2011，38（1）：C22.

[15]Wani S A，Chaudhari R P.Ethernet enabled digital I/O control in embedded systems[C]//Computing，Electronics and Electrical Technologies（ICCEET），2012 International Conference on.IEEE，2012：634-637.

[16]崔陸軍，尚會超.基于AVR單片機的無線測溫系統(tǒng)[J].儀表技術(shù)與傳感器，2012（9）：57-59.

[17]黃克亞.基于虛擬仿真和ISP下載的AVR單片機實驗?zāi)Ｊ窖芯縖J].實驗技術(shù)與管理，2013（8）：81-85.

[18]趙道新，李炳辰.基于PCF8591的I2C總線多路溫度測量系統(tǒng)[J].制造業(yè)自動化，2013（5）：36-37，55.