；由于每個(gè) 信標(biāo)模塊都具有一定的覆蓋范圍，通過合理安排每個(gè)可能達(dá)到的坐標(biāo)處的信標(biāo)模塊位置， 使機(jī)器人在允許的全部運(yùn)動(dòng)范圍內(nèi)都能夠有信標(biāo)模塊的交叉覆蓋。其中，兩個(gè)信標(biāo)模塊可 確定機(jī)器人的二維坐標(biāo)，三個(gè)信標(biāo)模塊可確定機(jī)器人的三維坐標(biāo)，可根據(jù)電子地圖的類型 (平面電子地圖和三維電子地圖)來確定信標(biāo)模塊的數(shù)量。被呼喚的信標(biāo)按照測試時(shí)序安排 向移動(dòng)定位模塊發(fā)射無線同步信號(hào)，以使得移動(dòng)定位模塊在不同的時(shí)間段接收到不同的信 標(biāo)模塊發(fā)射來的超聲波信號(hào)。另外，當(dāng)存在多個(gè)機(jī)器人時(shí)，如果出現(xiàn)請(qǐng)求信標(biāo)模塊沖突的問 題，由電子定位平臺(tái)查詢正在工作的機(jī)器人所呼喚的信標(biāo)模塊是否被其它機(jī)器人占用，若 已占用，則通知該機(jī)器人需間隔某一時(shí)間后再動(dòng)作，間隔時(shí)間到后，繼續(xù)完成后面的定位測 試。間隔時(shí)間通常設(shè)為以毫秒為單位。還有，當(dāng)機(jī)器人偏離預(yù)設(shè)的運(yùn)動(dòng)軌跡時(shí)，仍可根據(jù)設(shè) 于機(jī)器人附近的其它運(yùn)動(dòng)軌跡中的信標(biāo)模塊實(shí)現(xiàn)自動(dòng)定位，從而避免機(jī)器人偏離運(yùn)動(dòng)軌跡 就迷路，不能正常工作的問題； (2) 再由信標(biāo)模塊按時(shí)序分別向主測模塊發(fā)射已編碼的超聲波信號(hào)，同時(shí)，收到同步信 號(hào)的移動(dòng)定位模塊啟動(dòng)數(shù)據(jù)采集時(shí)間窗口 T，實(shí)時(shí)采集數(shù)據(jù)并依次存儲(chǔ)，根據(jù)接收到的超聲 波信號(hào)的時(shí)序，分別標(biāo)記接收到的時(shí)間為T1, TV。。。,'，其中，i與所呼喚信標(biāo)模塊的數(shù)量相 同；然后處理計(jì)算每個(gè)時(shí)間段接收到的超聲波信號(hào)數(shù)據(jù)段，識(shí)別其編碼，若接收到多組相同 編碼的超聲波信號(hào)，只取最先到達(dá)的一組超聲波編碼信號(hào)，剔除其余雜波，再經(jīng)計(jì)算找到每 個(gè)時(shí)間段最先到達(dá)的此組超聲波編碼信號(hào)的起頭時(shí)間T/,T2',。。。,T/ ; 具體的，啟動(dòng)數(shù)據(jù)采集時(shí)間窗口 T后，開始連續(xù)采集數(shù)據(jù)，開啟數(shù)據(jù)采集窗口的那一刻 標(biāo)記為采集時(shí)間零點(diǎn)，在時(shí)間窗口內(nèi)采集的數(shù)據(jù)全部存入RAM (存儲(chǔ)時(shí)按照時(shí)序依次存儲(chǔ))， 并根據(jù)接收到的超聲波信號(hào)的時(shí)序，分別標(biāo)記接收到的時(shí)間為T1, T2,。。。, T1;在時(shí)間窗口關(guān)閉 后，移動(dòng)定位模塊將立即處理RAM中的數(shù)據(jù)。其中，根據(jù)接收到的超聲波信號(hào)的時(shí)序，分別 標(biāo)記接收到的時(shí)間為T1, T2,。。。, T1可以理解為：假設(shè)T=O. 15s，根據(jù)接收到的超聲波信號(hào)的時(shí) 序，接收到第一個(gè)信標(biāo)模塊發(fā)射的一組超聲波信號(hào)的時(shí)間為〇. 〇5s，接收到第二個(gè)信標(biāo)模塊 發(fā)射的一組超聲波信號(hào)的時(shí)間為0. ls，則0~0. 05s的時(shí)間段標(biāo)記為T1,0. 05s~0.1 s的時(shí)間 段標(biāo)記為T2;每個(gè)時(shí)間段只獲取最先到達(dá)的一組超聲波編碼彳目號(hào)，其中，獲取最先到達(dá)的一 組超聲波編碼信號(hào)是指最先到達(dá)某個(gè)超聲波感應(yīng)元或某幾個(gè)超聲波感應(yīng)元上的超聲波信 號(hào)，且一定是直線到達(dá)；其余雜波包括多徑效應(yīng)、非視距傳播和超聲波反射產(chǎn)生的信號(hào)，剔 除這些雜波可解決環(huán)境聲波的干擾問題； (3) 計(jì)算得到超聲波接收陣列與每個(gè)信標(biāo)模塊之間的距離Z1Sz1= IV Xc，其中，c為 超聲波信號(hào)在常溫下的傳播速度，并需要對(duì)超聲波信號(hào)的傳播速度c進(jìn)行修正； (4) 計(jì)算得出超聲波接收陣列理論中心與每個(gè)信標(biāo)模塊的距離L1以及信標(biāo)模塊與超聲 波接收陣列理論中心的右旋角γ1;其中，超聲波接收陣列理論中心是指超聲波接收陣列的 幾何中心點(diǎn)；右旋角是指該信標(biāo)模塊與超聲波接收陣列理論中心之間的距離直線沿超聲波 接收陣列理論中心的中軸線向右旋轉(zhuǎn)所呈的夾角； (5) 由移動(dòng)定位模塊甄選出兩路或三路信標(biāo)模塊直線到達(dá)超聲波接收陣列的超聲波信 號(hào)，并結(jié)合內(nèi)置的電子地圖進(jìn)行計(jì)算處理，即分別收發(fā)根據(jù)步驟（4)計(jì)算得到的超聲波接收 陣列理論中心與這兩路或三路信標(biāo)模塊之間的距離，并結(jié)合這兩路或三路信標(biāo)模塊的固定 坐標(biāo)計(jì)算得出機(jī)器人的所在位置的二維坐標(biāo)數(shù)據(jù)或三維坐標(biāo)數(shù)據(jù)； (6) 將所得的二維坐標(biāo)數(shù)據(jù)或三維坐標(biāo)數(shù)據(jù)實(shí)時(shí)無線傳送給電子定位平臺(tái)，生成機(jī)器 人的導(dǎo)航信息并顯示。
[0039] 為方便描述，本實(shí)施例以機(jī)器人在室內(nèi)移動(dòng)為例進(jìn)行具體說明。如圖1所示：移動(dòng) 定位模塊2固定安裝在機(jī)器人1的頂部，它搭載的超聲波接收陣列21安裝在機(jī)器人1的頂 部中心位置，超聲波接收陣列21包括若干個(gè)圓周分布的超聲波感應(yīng)元，從而接收來自各個(gè) 擁有固定坐標(biāo)的信標(biāo)模塊3所發(fā)送的超聲波信號(hào)。本實(shí)施例還包括一套電子定位平臺(tái)4。
[0040] 在四面墻上部或房間頂部安裝不少于3臺(tái)的信標(biāo)模塊3,每個(gè)信標(biāo)模塊3發(fā)射的超 聲波信號(hào)都將覆蓋自己的一片區(qū)域，當(dāng)有3路及以上的信號(hào)時(shí)，則共同覆蓋著一片區(qū)域，且 該覆蓋區(qū)域形成立體交叉式，機(jī)器人1在室內(nèi)移動(dòng)時(shí)，都可在形成立體交叉的超聲波覆蓋 區(qū)域內(nèi)行走(需要根據(jù)空間大小不同，合理安置信標(biāo)模塊)。當(dāng)機(jī)器人1需要定位信號(hào)時(shí)，按 照上述定位方法使被呼喚的信標(biāo)模塊3和機(jī)器人1配合同步測試。
[0041] 發(fā)射無線同步信號(hào)后的信標(biāo)模塊3,依次按時(shí)序向移動(dòng)定位模塊發(fā)射帶有各自編 碼的超聲波信號(hào)，同時(shí)收到同步信號(hào)的機(jī)器人1啟動(dòng)數(shù)據(jù)采集，機(jī)器人1透過所搭載的超聲 波陣列21將接收到3路以上的信號(hào)。如圖2所示：根據(jù)可靠性需要，甄選出3路用于計(jì)算 的超聲波信號(hào)數(shù)據(jù)（只采用直線到達(dá)的超聲波數(shù)據(jù)，剔除由于多徑效應(yīng)和非視距傳播造成 的干擾信號(hào))，該3路信號(hào)分別由來自于三維坐標(biāo)為（0, 0, 0)、（X，0, 0)和（0, Y，0)的信標(biāo)模 塊發(fā)射出，然后使用TDOA算法，可計(jì)算出機(jī)器人所在的三維坐標(biāo)（X，y，z):
其中，LpLjP L 3為超聲波接收陣列理論中心與三個(gè)信標(biāo)模塊的距離。為便于理解L i、 LjP L3的計(jì)算過程，本實(shí)施例還以三維坐標(biāo)為（0, 0, 0)的信標(biāo)模塊發(fā)射的超聲波信號(hào)為例 對(duì)計(jì)算L1的過程進(jìn)行具體說明。參照?qǐng)D3和圖4所示：超聲波接收陣列包括16個(gè)超聲波 感應(yīng)元r〇-rl5，當(dāng)（0, 0, 0)的信標(biāo)模塊的超聲波發(fā)射源發(fā)射超聲波時(shí)，被超聲波接收陣列 中的r〇-r4這五個(gè)超聲波感應(yīng)元接收，其中Zti-Z4S r〇-r4分別與超聲波發(fā)射源的距離，0 為超聲波接收陣列理論中心，R為圓周分布的16個(gè)超聲波感應(yīng)元與超聲波接收陣列理論中 心的距離，L1S超聲波發(fā)射源與超聲波接收陣列理論中心的距離，角度b和長度s為超聲波 接收陣列的固定尺寸。
[0042] 其中，在Zq-Z4中，z 3最小，z 2次小，r3的右旋角為α，角度爽.為直線Z2與長度s之 間的夾角，超聲波接收陣列理論中心到超聲波發(fā)射源的距離1^與最近的超聲波感應(yīng)元^的 角度偏移為β，此時(shí)超聲波接收陣列理論中心與超聲波發(fā)射源的角度為Y1，計(jì)算得出：
根據(jù)上述公式可求得L1的值，同理可求得L 2和L 3。其中，2。-24即超聲波感應(yīng)元與超聲 波發(fā)射源的距離可由下述測距方法計(jì)算得出（以三維坐標(biāo)為（X，0, 0)的信標(biāo)模塊為例進(jìn)行 說明）： 該測距方法包括以下步驟： (1) 由移動(dòng)定位模塊發(fā)起建立無線聯(lián)絡(luò)信號(hào)請(qǐng)求，呼喚三維坐標(biāo)為（〇,〇,〇)的信標(biāo)模 塊，被呼喚到的信標(biāo)模塊發(fā)送無線信號(hào)回應(yīng)，主測模塊收到回應(yīng)后完成無線握手； (2) 移動(dòng)定位模塊再次發(fā)射無線信號(hào)啟動(dòng)測距命令，信標(biāo)模塊收到命令后，先發(fā)射無線 起始同步信號(hào)，再發(fā)射已編址的超聲波信號(hào)； (3) 主測模塊在收到信標(biāo)模塊發(fā)射的無線起始同步信號(hào)時(shí)開啟數(shù)據(jù)采集時(shí)間窗口 T，實(shí) 時(shí)采集數(shù)據(jù)并存儲(chǔ)，處理計(jì)算接收到的超聲波信號(hào)數(shù)據(jù)段，識(shí)別其編碼，若接收到多組相同 編碼的超聲波信號(hào)，只取最先到達(dá)的一組超聲波編碼信號(hào)，剔除其余雜波，再經(jīng)計(jì)算找到最 先到達(dá)的此組超聲波編碼信號(hào)的起頭時(shí)間Τ' ； (4) 計(jì)算得到超聲波感應(yīng)元r3與該信標(biāo)模塊的超聲波發(fā)射源之間的距離ζ 3為ζ 3= Τ' Xc，其中，c為超聲波信號(hào)在常溫下的傳播速度（以下簡稱聲速)。
[0043]同理可計(jì)算出其它信標(biāo)模塊與超聲波感應(yīng)元的距離。
[0044]由于在外部一定條件下其聲速是一定的，當(dāng)外部條件發(fā)生改變，我們可以根據(jù)外 部條件的改變量來修正聲速。使用經(jīng)修正后的聲速來進(jìn)行計(jì)算，就可以在外在條件改變后 也能計(jì)算出精確的距離。
[0045] 由于各信標(biāo)模塊的坐標(biāo)是固定的，則它們之間的距離(絕對(duì)距離）也是固定的。在 超聲波有效覆蓋范圍內(nèi)的任意一個(gè)地方，先在固定的默認(rèn)聲速條件下，可計(jì)算出超聲波接 收陣列與兩個(gè)信標(biāo)模塊的相對(duì)距離和角度；再計(jì)算出兩個(gè)固定坐標(biāo)的信標(biāo)模塊之間的相對(duì) 距離(默認(rèn)聲速條件下)，再通過這兩個(gè)固定坐標(biāo)的信標(biāo)模塊之間的相對(duì)距離與它們之間的 絕對(duì)距離進(jìn)行比較，就可以計(jì)算出精確的當(dāng)前聲速。
[0046] 為便于理解，本實(shí)施例取三維坐標(biāo)為（X，0, 0)的信標(biāo)模塊A與（0, 0, 0)的信標(biāo)模塊 B為例進(jìn)行具體說明，參照?qǐng)D5所示： (I1為信標(biāo)模塊A與信標(biāo)模塊B之間的絕對(duì)距離（以mm為單位)，默認(rèn)聲速c i，當(dāng)前實(shí)際 聲速為c2, LpL2、右旋角yJP γ 2已在上述公式中計(jì)算得出，從而得到兩個(gè)固定坐標(biāo)的信 標(biāo)模塊之間的夾角P = T2-T1，兩個(gè)固定坐標(biāo)的信標(biāo)模塊之間的相對(duì)距離(12為：
通過計(jì)算兩個(gè)信標(biāo)模塊之間的絕對(duì)距離與相對(duì)距離之間的誤差百分率作為補(bǔ)償，進(jìn)而 計(jì)算