劉永福，黃芳艷，林鎮(zhèn)滔，胡嘉就，陳德朝

基于超聲波的無(wú)人機(jī)室內(nèi)定位技術(shù)研究＊

劉永福，黃芳艷，林鎮(zhèn)滔，胡嘉就，陳德朝

（廣東開(kāi)放大學(xué)，廣東 廣州 510091；廣東理工職業(yè)學(xué)院，廣東 廣州 510091）

以超聲波為載體，設(shè)計(jì)和制作一種高精度、低成本的超聲波室內(nèi)定位系統(tǒng)。相對(duì)于三角位置測(cè)定，此系統(tǒng)采用四角位置測(cè)定，在室內(nèi)的四個(gè)頂點(diǎn)安裝超聲波發(fā)射頭和紅外發(fā)射頭，提高了定位精度和系統(tǒng)健壯性。針對(duì)超聲波信號(hào)發(fā)射的同步模式存在信號(hào)與節(jié)點(diǎn)不匹配、定位不確定和測(cè)試盲區(qū)問(wèn)題，異步模式會(huì)犧牲系統(tǒng)的定位精度問(wèn)題，設(shè)計(jì)自適應(yīng)模式切換原則，根據(jù)實(shí)際情況在兩種模式之間切換。異步模式只用來(lái)粗略估算待測(cè)目標(biāo)節(jié)點(diǎn)的位置，估算結(jié)果用來(lái)輔助同步模式完成信號(hào)和節(jié)點(diǎn)的匹配工作，從而定位目標(biāo)點(diǎn)。在異步模式、盲區(qū)和嚴(yán)重干擾情況下，系統(tǒng)采用運(yùn)動(dòng)軌跡預(yù)測(cè)算法，確保系統(tǒng)的預(yù)測(cè)精度，提高了定位系統(tǒng)的魯棒性。

超聲波；紅外；室內(nèi)定位；無(wú)人機(jī)

常用的室內(nèi)定位技術(shù)有基于Wi-Fi的室內(nèi)定位、基于iBeacon的室內(nèi)定位、基于UWB的室內(nèi)定位、基于紅外的室內(nèi)定位、基于激光雷達(dá)的室內(nèi)定位、基于LED的室內(nèi)定位、基于Zigbee的室內(nèi)定位和基于超聲波的室內(nèi)定位等。不管采用哪種技術(shù)，都追求一種性?xún)r(jià)比較高的技術(shù)方案來(lái)解決實(shí)際需求。對(duì)于廠(chǎng)房設(shè)備的室內(nèi)定位（例如室內(nèi)無(wú)人機(jī)、機(jī)器人、小車(chē)導(dǎo)航等業(yè)務(wù)），要求定位精度較高、成本低且抗環(huán)境干擾（光、溫度、濕度）能力強(qiáng)等。本系統(tǒng)以超聲波為載體，分析超聲波在空氣中傳播的特性和前人的技術(shù)方案，設(shè)計(jì)和制作一種高精度、低成本的超聲波室內(nèi)定位系統(tǒng)，對(duì)室內(nèi)的無(wú)人機(jī)設(shè)備進(jìn)行定位。

1 測(cè)試環(huán)境

測(cè)試環(huán)境為10 m×10 m×3 m的長(zhǎng)方體，其部署如圖1所示。

圖1 設(shè)備部署圖

圖1中，為原點(diǎn)（0，0，0），為軸，為軸，為軸。、、、的長(zhǎng)度都為10 m，的長(zhǎng)度為3 m?！鸨硎景l(fā)射節(jié)點(diǎn)，、、、四個(gè)發(fā)射節(jié)點(diǎn)分別位于四個(gè)頂角上；●表示待測(cè)目標(biāo)點(diǎn)，其在面上的投影為點(diǎn)。

，，，為房子的4個(gè)頂角，安裝4個(gè)超聲波發(fā)射節(jié)點(diǎn)，每個(gè)節(jié)點(diǎn)都裝有超聲波發(fā)射頭和2 W的紅外發(fā)射頭。超聲波發(fā)射頭發(fā)射特定測(cè)試信號(hào)（頻率為40 kHz，8個(gè)方波）實(shí)現(xiàn)發(fā)射距離增程，經(jīng)過(guò)適當(dāng)調(diào)整發(fā)射角度和增益，4個(gè)發(fā)射頭能很好覆蓋整個(gè)房間。紅外發(fā)射頭同步發(fā)射紅外光，通過(guò)紅外感知，待測(cè)目標(biāo)獲得同步測(cè)試信號(hào)。

2 測(cè)試方案

4個(gè)節(jié)點(diǎn)和一個(gè)無(wú)線(xiàn)節(jié)點(diǎn)（無(wú)線(xiàn)節(jié)點(diǎn)為SI44663無(wú)線(xiàn)模塊）通過(guò)信號(hào)線(xiàn)接入中控器，通過(guò)中控器控制信號(hào)收發(fā)。待測(cè)目標(biāo)節(jié)點(diǎn)點(diǎn)由超聲波接收頭（靈敏度為－75 dBm）、紅外光探頭和無(wú)線(xiàn)節(jié)點(diǎn)（無(wú)線(xiàn)節(jié)點(diǎn)為SI4463無(wú)線(xiàn)模塊）組成。根據(jù)三角位置測(cè)定，只需要3個(gè)發(fā)射節(jié)點(diǎn)，便可以獲得點(diǎn)在軸、軸、軸的坐標(biāo)，實(shí)現(xiàn)目標(biāo)點(diǎn)的定位，節(jié)省了成本，但降低了系統(tǒng)的精度和健壯性。

本系統(tǒng)采用4個(gè)發(fā)射節(jié)點(diǎn)，分別部署于4個(gè)頂角。增加了系統(tǒng)冗余，即使其中一個(gè)發(fā)射節(jié)點(diǎn)損壞，系統(tǒng)依然可以正常工作；在系統(tǒng)運(yùn)行過(guò)程中，可以根據(jù)就近原則選取信號(hào)參與定位計(jì)算。

就近原則主要考慮兩種情況：①4個(gè)節(jié)點(diǎn)同時(shí)發(fā)射測(cè)試信號(hào)的同步模式，只采用最先到達(dá)點(diǎn)的前3列信號(hào)；②4個(gè)節(jié)點(diǎn)輪流發(fā)射測(cè)試信號(hào)的異步模式，則排除時(shí)間最長(zhǎng)的1列信號(hào)。

3 距離測(cè)試過(guò)程

4 同步發(fā)射模式

4個(gè)超聲波發(fā)射節(jié)點(diǎn)同時(shí)發(fā)射測(cè)試信號(hào)的模式稱(chēng)為同步模式，4個(gè)超聲波發(fā)射節(jié)點(diǎn)不同時(shí)發(fā)射測(cè)試信號(hào)的模式稱(chēng)為異步模式。在同步模式下，存在信號(hào)與節(jié)點(diǎn)不匹配、定位不確定問(wèn)題和測(cè)試盲區(qū)問(wèn)題。

4.1 信號(hào)不匹配、定位不確定問(wèn)題

由于長(zhǎng)方體具有對(duì)稱(chēng)性，在同步發(fā)射模式下，目標(biāo)節(jié)點(diǎn)獲得的4個(gè)時(shí)間信號(hào)無(wú)法和發(fā)射節(jié)點(diǎn)，，，四點(diǎn)匹配，因此無(wú)法進(jìn)行精確定位。

4.2 測(cè)試盲區(qū)問(wèn)題

同步模式下，發(fā)射節(jié)點(diǎn)的對(duì)稱(chēng)性除了引入定位的不確定性外，還引入了測(cè)試盲區(qū)。

為了讓4列信號(hào)同時(shí)發(fā)射在空中互不重疊，且待測(cè)目標(biāo)能被正確識(shí)別，待測(cè)目標(biāo)到各信號(hào)發(fā)射節(jié)點(diǎn)間距離差必須大于7.82 cm。當(dāng)節(jié)點(diǎn)間距差不足7.82 cm時(shí)，收到的將是一串疊加長(zhǎng)信號(hào)。因此，在對(duì)角線(xiàn)及中線(xiàn)構(gòu)成的面中會(huì)存在 7.82 cm的空間盲區(qū)。

4.3 信號(hào)不匹配問(wèn)題和盲區(qū)的解決方案

不同發(fā)射節(jié)點(diǎn)發(fā)射其特有的超聲波頻率，待測(cè)目標(biāo)節(jié)點(diǎn)具備接收多種頻率的能力。

缺點(diǎn)是增加了硬件設(shè)計(jì)復(fù)雜度和系統(tǒng)維護(hù)難度，成本大幅度增加，用戶(hù)難以接受。

整個(gè)系統(tǒng)采用每個(gè)節(jié)點(diǎn)輪流發(fā)射信號(hào)的異步模式。異步模式下，目標(biāo)節(jié)點(diǎn)能識(shí)別出每個(gè)時(shí)長(zhǎng)所對(duì)應(yīng)的發(fā)射節(jié)點(diǎn)，不會(huì)存在由于節(jié)點(diǎn)對(duì)稱(chēng)性而引入的定位不確定性和信號(hào)重疊的情況，也不存在所謂的測(cè)試盲區(qū)。缺點(diǎn)是犧牲系統(tǒng)的定位精度。

5 自適應(yīng)切換模式

為了保證系統(tǒng)精度同時(shí)又能降低成本，本系統(tǒng)設(shè)計(jì)了自適應(yīng)切換模式。假設(shè)異步模式完成一輪信號(hào)收發(fā)需要時(shí)間為（取112 ms），同步模式完成一輪信號(hào)收發(fā)需要時(shí)間為（取43 ms）。

在目標(biāo)節(jié)點(diǎn)上，記錄每列超聲波的持續(xù)時(shí)間和同步模式時(shí)間內(nèi)獲得的信號(hào)列數(shù)，根據(jù)測(cè)得信號(hào)列數(shù)，通過(guò)無(wú)線(xiàn)節(jié)點(diǎn)請(qǐng)求中控器切換到相應(yīng)的工作模式。

5.1 自適應(yīng)模式切換原則

周期內(nèi)，＜3，干擾嚴(yán)重或2列以上信號(hào)疊加，取消本次測(cè)量，采用軌跡預(yù)測(cè)算法，并請(qǐng)求中控器啟用異步模式。周期內(nèi)，3≤＜4，利用3列信號(hào)測(cè)定坐標(biāo)。周期內(nèi)，=4，根據(jù)就近原則，去掉耗時(shí)最長(zhǎng)的一列信號(hào)，然后測(cè)定坐標(biāo)。周期內(nèi)，＞5，干擾嚴(yán)重，取消本次測(cè)量，采用軌跡預(yù)測(cè)算法，并請(qǐng)求中控器啟用異步模式。

中控器接收到超過(guò)2/3的目標(biāo)節(jié)點(diǎn)請(qǐng)求異步模式時(shí)，則切換到異步模式。異步模式中，每個(gè)目標(biāo)節(jié)點(diǎn)根據(jù)就近原則去掉四列信號(hào)中最大的一列，剩余3列信號(hào)參與計(jì)算粗略位置，利用粗略位置完成同步模式的信號(hào)和發(fā)射點(diǎn)的匹配，消除對(duì)稱(chēng)性引入的不確定因素。異步模式結(jié)束后馬上轉(zhuǎn)為同步模式，連續(xù)20次同步模式后，轉(zhuǎn)入異步模式（確保每秒有一次異步模式）。

5.2 相對(duì)誤差

由于超聲波傳播速度較慢，超聲波和點(diǎn)的相對(duì)運(yùn)動(dòng)會(huì)引入不可忽略的誤差。

按照可能引入最大誤差的情況來(lái)設(shè)定條件：超聲波的速度取340 m/s，點(diǎn)運(yùn)動(dòng)速度為2 m/s。點(diǎn)選取2個(gè)典型特征點(diǎn)：在角附近移動(dòng)或者在長(zhǎng)方體中心位置附近運(yùn)動(dòng)，如圖2所示。

圖2 O點(diǎn)在M角和中心柱附近運(yùn)動(dòng)的平面投影

5.2.1點(diǎn)在角附近運(yùn)動(dòng)

根據(jù)自適應(yīng)模式切換原則，符合原則2和3的條件，所以用同步模式進(jìn)行測(cè)量。

假設(shè)點(diǎn)非?？拷c(diǎn)，滿(mǎn)足：

超聲波在線(xiàn)路上傳播所需時(shí)間：

AM=/340=0.042 5 s

點(diǎn)的相對(duì)位移為0.042 5 s×2 m/s=8.5 cm，因此引入的誤差約為8.5 cm。

如果受到模式切換原則第5條約束，系統(tǒng)處于異步模式，那么目標(biāo)節(jié)點(diǎn)需要接收完4列信號(hào)，才能開(kāi)始進(jìn)行坐標(biāo)運(yùn)算。因此，其引入的誤差如下。

和計(jì)算AM同理，計(jì)算BM，CM，DM的值，4列信號(hào)總耗時(shí)為：

AM+BM+CM+DM=0.042 5+0.03+0.03+0.009=0.111 5 s

由此得相對(duì)運(yùn)動(dòng)引起誤差約為：

0.111 5 s×2 m/s=22.3 cm

5.2.2點(diǎn)在中心軸附近運(yùn)動(dòng)

根據(jù)自適應(yīng)模式切換原則，符合原則1的條件，所以用異步模式進(jìn)行測(cè)量。

假設(shè)目標(biāo)節(jié)點(diǎn)處于長(zhǎng)方體的中心點(diǎn)處，即：

完整接收完4列信號(hào)所需時(shí)長(zhǎng)：7.23÷340×4=0.084 8 s，可認(rèn)為相對(duì)運(yùn)動(dòng)誤差約為0.084 8×2 m/s=16.96 cm。

如果受自適應(yīng)模式切換原則約束，系統(tǒng)處于同步模式。同步模式下，目標(biāo)節(jié)點(diǎn)因受到多路信號(hào)疊加干擾，根據(jù)自適應(yīng)切換原則，會(huì)放棄此次測(cè)量，并請(qǐng)求中控啟用異步模式和進(jìn)入運(yùn)動(dòng)軌跡預(yù)測(cè)模式。

根據(jù)上述情況，不管待測(cè)目標(biāo)在點(diǎn)附近還是在中心點(diǎn)附近運(yùn)動(dòng)，待測(cè)目標(biāo)節(jié)點(diǎn)因?yàn)橄鄬?duì)運(yùn)動(dòng)都會(huì)引入較大的誤差。異步模式只作為待測(cè)目標(biāo)節(jié)點(diǎn)的粗略位置估算，估算結(jié)果用來(lái)輔助同步模式完成信號(hào)和節(jié)點(diǎn)的匹配工作。

5.3 運(yùn)動(dòng)軌跡預(yù)測(cè)

根據(jù)自適應(yīng)切換原則，異步模式和處于盲區(qū)或干擾嚴(yán)重的情況下，都會(huì)啟動(dòng)運(yùn)動(dòng)軌跡預(yù)測(cè)模式，進(jìn)一步提高系統(tǒng)的精度。

5.3.1 運(yùn)動(dòng)軌跡

因此點(diǎn)運(yùn)動(dòng)軌跡的預(yù)測(cè)轉(zhuǎn)化為點(diǎn)在軸、軸、軸投影的軌跡預(yù)測(cè)。

5.3.2 軌跡預(yù)測(cè)算法

以軸為例，系統(tǒng)記錄前3次軸的坐標(biāo)（1，2，3）。在同步模式下，系統(tǒng)每次測(cè)試的周期為（為了方便計(jì)算，公式中把記為），通過(guò)算法預(yù)測(cè)下一時(shí)刻4的位置，如圖3所示。

圖3 x軸運(yùn)動(dòng)軌跡預(yù)測(cè)

根據(jù)=/，得2處和3處的速度1和2：

1=（2﹣1）/（1）

2=（3﹣2）/（2）

2和3之間的加速度為：

=（2﹣1）/（3）

計(jì)算后知初始速度為2，加速度為，時(shí)間為。

代入式（2）（3），化簡(jiǎn)得：

同理，通過(guò)式（5）能預(yù)測(cè)軸和軸的下一時(shí)刻的坐標(biāo)。至此，完成了運(yùn)動(dòng)目標(biāo)的軌跡預(yù)測(cè)。在異步模式、盲區(qū)和嚴(yán)重干擾情況下，確保系統(tǒng)的預(yù)測(cè)精度。

通過(guò)上述方法，可以比較準(zhǔn)確地確定無(wú)人機(jī)在室內(nèi)GPS無(wú)效情況下的位置，以及預(yù)測(cè)無(wú)人機(jī)在室內(nèi)飛行時(shí)下一個(gè)時(shí)刻的運(yùn)動(dòng)位置。

6 結(jié)束語(yǔ)

本文采用四角定位、同步模式異步模式自適應(yīng)切換、運(yùn)動(dòng)軌跡預(yù)測(cè)等技術(shù)，設(shè)計(jì)制作的超聲波無(wú)人機(jī)室內(nèi)定位系統(tǒng)，不但降低了硬件成本和硬件的復(fù)雜程度，也提高了定位的精度，提高了定位系統(tǒng)的魯棒性。

V279；V249.3

A

10.15913/j.cnki.kjycx.2020.01.015

2095－6835（2020）01－0051－03

廣東理工職業(yè)學(xué)院大學(xué)生科技創(chuàng)新項(xiàng)目（編號(hào)：Pdjh2019b0734）

〔編輯：嚴(yán)麗琴〕