基于uwb的自主跟隨機(jī)器人定位方法及系統(tǒng)的制作方法
【專利摘要】本發(fā)明涉及一種基于UWB的自主跟隨機(jī)器人定位方法及系統(tǒng)，特定目標(biāo)為可自由移動(dòng)的人或者物；所述移動(dòng)機(jī)器人上固定至少三個(gè)位置成三角形態(tài)的UWB基站，所述自由移動(dòng)的人或物上放置UWB定位標(biāo)簽；所述的基站發(fā)送UWB信號(hào)；所述的標(biāo)簽接收從基站發(fā)來的UWB信號(hào)；標(biāo)簽與基站時(shí)間同步，利用TOA算法獲得基站與標(biāo)簽間的距離，然后通過幾何運(yùn)算獲得標(biāo)簽的坐標(biāo)，確定自由移動(dòng)的人或物相對(duì)于移動(dòng)機(jī)器人的位置和運(yùn)動(dòng)信息，實(shí)現(xiàn)移動(dòng)機(jī)器人對(duì)人或物的跟隨。本發(fā)明使用UWB無線定位技術(shù)實(shí)現(xiàn)自主跟隨機(jī)器人定位，采用窄帶脈沖進(jìn)行通信，不需要載波，傳輸功率高，功耗小，抗干擾能力和穿透能力強(qiáng)，提高了定位的精度和準(zhǔn)確性，從而實(shí)現(xiàn)特定目標(biāo)的跟隨。
【專利說明】
基于UWB的自主跟隨機(jī)器人定位方法及系統(tǒng)
技術(shù)領(lǐng)域
[0001] 本發(fā)明設(shè)及一種移動(dòng)機(jī)器人定位領(lǐng)域的自主跟隨機(jī)器人定位系統(tǒng)，具體地說，是 一種相對(duì)定位技術(shù)和機(jī)器人跟隨方法。
【背景技術(shù)】
[0002] 隨著工業(yè)和科學(xué)技術(shù)的發(fā)展，移動(dòng)機(jī)器人的研究和發(fā)展非常迅速，并逐漸進(jìn)入人 們的日常生活。目前服務(wù)機(jī)器人的一個(gè)典型技術(shù)是機(jī)器人跟蹤，它能識(shí)別特定目標(biāo)并跟隨 其運(yùn)動(dòng)，同時(shí)具有一定的人機(jī)交互功能。目前自主跟隨機(jī)器人的技術(shù)難點(diǎn)之一就是機(jī)器人 與目標(biāo)之間的相對(duì)定位。通過機(jī)器人與特定目標(biāo)的相對(duì)位置信息，規(guī)劃自身運(yùn)動(dòng)。
[0003] 現(xiàn)有的定位技術(shù)有GPS，超聲波定位技術(shù)，藍(lán)牙技術(shù)，紅外線技術(shù)，射頻識(shí)別技術(shù)， 無線局域網(wǎng)絡(luò)等。其中，GPS是目前應(yīng)用最廣泛的室外定位技術(shù)。然而GI^信號(hào)在經(jīng)過建筑的 遮擋后衰落較嚴(yán)重，多徑干擾嚴(yán)重，不適合在室內(nèi)定位;其他UWB的類似定位技術(shù)，例如超聲 波定位系統(tǒng)定位精度雖然較高，適用于室內(nèi)定位，但超聲波受多徑效應(yīng)和非視距傳播影響 很大，同時(shí)需要大量的底層硬件設(shè)施投資，成本太高。利用藍(lán)牙、RFID和紅外技術(shù)組成的網(wǎng) 絡(luò)進(jìn)行定位時(shí)，傳輸數(shù)據(jù)速率低，因此傳輸距離短，定位精度也比較低。

【發(fā)明內(nèi)容】

[0004] 本發(fā)明的目的是針對(duì)現(xiàn)有技術(shù)的不足，提供一種結(jié)構(gòu)簡(jiǎn)單，抗干擾能力和穿透能 力強(qiáng)，準(zhǔn)確性高的基于UWB的自主跟隨機(jī)器人定位方法及系統(tǒng)。
[0005] 為實(shí)現(xiàn)上述目的，本發(fā)明采取的技術(shù)方案是:一種基于UWB的自主跟隨機(jī)器人定位 系統(tǒng)，包括移動(dòng)機(jī)器人、特定目標(biāo)、UWB基站和標(biāo)簽，所述的特定目標(biāo)為可自由移動(dòng)的人或者 物;所述移動(dòng)機(jī)器人上固定至少=個(gè)位置成=角形態(tài)的UWB基站，所述自由移動(dòng)的人或物上 放置UWB定位標(biāo)簽;所述的基站發(fā)送UWB信號(hào);所述的標(biāo)簽接收從基站發(fā)來的UWB信號(hào);標(biāo)簽 與基站時(shí)間同步，利用TOA算法獲得基站與標(biāo)簽間的距離，然后通過幾何運(yùn)算獲得標(biāo)簽的坐 標(biāo)，確定自由移動(dòng)的人或物相對(duì)于移動(dòng)機(jī)器人的位置和運(yùn)動(dòng)信息，實(shí)現(xiàn)移動(dòng)機(jī)器人對(duì)自由 移動(dòng)的人或物的跟隨。
[0006] 所述的UWB基站為=個(gè)，=個(gè)UWB基站(3)在移動(dòng)機(jī)器人（1)身上按等邊=角形的= 個(gè)頂點(diǎn)位置擺放。
[0007] 所述的UWB標(biāo)簽穿戴于自由移動(dòng)的人的手腕或腰間、或掛戴于脖子上。
[000引一種基于UWB的自主跟隨機(jī)器人定位方法，包括UWB基站發(fā)射UWB信號(hào)，UWB定位標(biāo) 簽接收UWB信號(hào)并進(jìn)行定位解算;所述接收端解算方法具體步驟如下：
[0009] 步驟1，UWB基站定時(shí)發(fā)送UWB信號(hào)，UWB定位標(biāo)簽接收信號(hào)，且標(biāo)簽與基站時(shí)間同 步，利用TOA算法，根據(jù)UWB信號(hào)在UWB基站和UWB定位標(biāo)簽之間的傳播時(shí)間t，由d = c*t，C為 光速，得到S個(gè)UWB基站分別于UWB定位標(biāo)簽的距離AM，BM，CM為dl，d2，d3，根據(jù)已知的AB， BC，AC的距離W及AM，BM，CM的距離可W確定一個(gè)交點(diǎn)，運(yùn)個(gè)交點(diǎn)是分別WA，B，C為圓屯、，d 1， d2，d3為半徑的=個(gè)球體的交點(diǎn)，該交點(diǎn)就是標(biāo)簽所在的位置；
[0010] 定義S個(gè)UWB基站位置距離AB，BC，AC均為a;定義BC邊上的高所在延長(zhǎng)線為y軸，y 軸正向與機(jī)器人自身的方向一致;S個(gè)UWB基站構(gòu)成的等邊S角形的邊AB的中點(diǎn)為0(0,0， 0)，標(biāo)簽的坐標(biāo)為M(x，y，z)，其中Z為標(biāo)簽到基站所在水平面的高度;等邊=角形的中屯、為 H;
[0011] 步驟2,根據(jù)=個(gè)UWB基站到UWB定位標(biāo)簽的距離，通過解析幾何算出：標(biāo)簽M在等邊 S角形所在平面的投影M'點(diǎn)到等邊S角形中屯、的距離：
[0012]
[0013] 標(biāo)簽M在等邊S角形所在平面的投影r點(diǎn)與等邊S角形的中屯、H點(diǎn)的連線，即M'H 與y軸的夾巧郵方仿巧：
[0014]
[0015] 由上述步驟得到的化，0)即可確定特定目標(biāo)具體所在位置信息，W及根據(jù)下一個(gè) 時(shí)刻化，e)可W獲得特定目標(biāo)的運(yùn)動(dòng)信息，包括速度和方向角，所得位置誤差范圍在±10cm W內(nèi)，在誤差范圍內(nèi)移動(dòng)機(jī)器人根據(jù)位置信息確定自由移動(dòng)的人或物的位置;并根據(jù)每一 時(shí)刻移動(dòng)機(jī)器人獲得的自由移動(dòng)的人或物的位置和運(yùn)動(dòng)信息，對(duì)自由移動(dòng)的人或物進(jìn)行實(shí) 時(shí)跟隨，即移動(dòng)機(jī)器人可實(shí)現(xiàn)對(duì)自由移動(dòng)的人或物的跟隨任務(wù)。
[0016] 本發(fā)明的有益效果是:本發(fā)明采用UWB技術(shù)進(jìn)行自主跟隨機(jī)器人的定位，能在低功 率消耗的情況下實(shí)現(xiàn)高精度定位，增加了可靠信，并且能將通信與定位合二為一;使用=個(gè) 基站作為信號(hào)發(fā)射端，能有效的確定出特定目標(biāo)的位置，誤差小;=個(gè)基站相對(duì)位置固定， 只需要測(cè)出UWB信號(hào)從基站到標(biāo)簽的傳送時(shí)間，即可確定出目標(biāo)的相對(duì)位置，計(jì)算簡(jiǎn)單;通 過實(shí)時(shí)定位目標(biāo)的位置，可實(shí)現(xiàn)機(jī)器人對(duì)人的跟隨，操作方便;硬件實(shí)現(xiàn)簡(jiǎn)單，降低了成本， 提高了定位精度，有效的解決了多徑干擾的問題;可同時(shí)準(zhǔn)確實(shí)現(xiàn)室內(nèi)定位和室外定位。
【附圖說明】
[0017] 圖1是本發(fā)明的基于UWB的自主跟隨機(jī)器人定位系統(tǒng)示意圖；
[0018] 圖2是本發(fā)明的基于UWB的自主跟隨機(jī)器人定位系統(tǒng)及方法幾何原理圖。
[0019] 圖中：1.移動(dòng)機(jī)器人，2. S個(gè)UWB基站形成的等邊S角形，3. UWB基站，4.特定目標(biāo)， 5. UWB定位標(biāo)簽。
【具體實(shí)施方式】
[0020] 下面結(jié)合附圖本發(fā)明的進(jìn)一步說明。
[00別]請(qǐng)參照?qǐng)D1，本發(fā)明的一種基于UWB的自主跟隨機(jī)器人定位系統(tǒng)，UWB(Ultra Wideband)是一種無載波通信技術(shù)。該系統(tǒng)包括:移動(dòng)機(jī)器人1，UWB基站3,特定目標(biāo)4, UWB定 位標(biāo)簽5;所述的特定目標(biāo)4為可自由移動(dòng)的人或物;所述的UWB基站3共有=個(gè);所述的UWB 定位標(biāo)簽5穿戴于自由移動(dòng)的人或物身上，為可穿戴設(shè)備，比如穿戴于人的手腕，腰間或掛 戴于脖子上等等。所述移動(dòng)機(jī)器人1上固定至少=個(gè)UWB基站3，=個(gè)UWB基站3形成的等邊= 角形2，所述自由移動(dòng)的人或物上放置UWB定位標(biāo)簽5;所述的UWB基站3發(fā)送UWB信號(hào);所述的 UWB定位標(biāo)簽5接收從基站發(fā)來的UWB信號(hào);UWB定位標(biāo)簽5與UWB基站3時(shí)間同步，利用TOA算 法獲得基站與標(biāo)簽間的距離，然后通過幾何運(yùn)算獲得標(biāo)簽的坐標(biāo)，確定自由移動(dòng)的人或物 相對(duì)于移動(dòng)機(jī)器人I的位置和運(yùn)動(dòng)信息，實(shí)現(xiàn)移動(dòng)機(jī)器人I對(duì)自由移動(dòng)的人或物的跟隨。
[0022] 請(qǐng)參照?qǐng)D2,圖2是本發(fā)明的一種自主跟隨機(jī)器人的定位系統(tǒng)及方法幾何原理圖。 其S個(gè)UWB基站分別標(biāo)記為A，B，C，UWB定位標(biāo)簽標(biāo)記為M，UWB定位標(biāo)簽在UWB基站所在平面 的投影標(biāo)記為M'，S個(gè)UWB基站相互之間的距離AB，BC ,AC均標(biāo)記為a。根據(jù)UWB信號(hào)在UWB基 站和UWB定位標(biāo)簽之間的傳播時(shí)間和傳播速度可W得到=個(gè)UWB基站分別于UWB定位標(biāo)簽的 距離 41，81，〔1為(11，(12，(13。
[0023] 本發(fā)明的一種基于UWB的自主跟隨機(jī)器人定位方法，該方法包括基站發(fā)射UWB信 號(hào)，UWB定位標(biāo)簽接收UWB信號(hào)并進(jìn)行定位解算。
[0024] 所述接收端解算方法具體步驟如下：
[0025] 步驟1，UWB基站定時(shí)發(fā)送UWB信號(hào)，UWB定位標(biāo)簽接收信號(hào)，且標(biāo)簽與基站時(shí)間同 步，利用TOAUime Of arrival)算法，根據(jù)UWB信號(hào)在UWB基站和UWB定位標(biāo)簽之間的傳播時(shí) 間t，由d = c*t，k為光速），得到S個(gè)UWB基站分別于UWB定位標(biāo)簽的距離AM，BM，CM為dl，d2， d3，根據(jù)已知的AB，BC，AC的距離W及AM，BM，CM的距離可W確定一個(gè)交點(diǎn)，運(yùn)個(gè)交點(diǎn)是分別 WA，B，C為圓屯、，dl，d2，d3為半徑的=個(gè)球體的交點(diǎn)，該交點(diǎn)就是UWB定位標(biāo)簽所在的位置； [00%]定義S個(gè)UWB基站位置距離AB，BC，AC均為a;定義BC邊上的高所在延長(zhǎng)線為y軸，y 軸正向與機(jī)器人自身的方向一致;S個(gè)UWB基站構(gòu)成的等邊S角形的邊AB的中點(diǎn)為0(0,0， 0 )，UWB定位標(biāo)簽的坐標(biāo)為M( X，y，Z )，其中Z為UWB定位標(biāo)簽到UWB基站所在水平面的高度;等 邊S角形的中屯、為H;
[0027]步驟2，根據(jù)=個(gè)UWB基站到UWB定位標(biāo)簽的距離，通過解析幾何可W算出：
[002引標(biāo)絡(luò)M#望巧二佑化所#平而的巧影M'點(diǎn)幸Il望巧二佑化中如的陽離：
[0029]
[0030] 標(biāo)簽M在等邊S角形所在平面的投影r點(diǎn)與等邊S角形的中屯、H點(diǎn)的連線，即M'H 與y軸的賽值即節(jié)仿值.
[0031]
[0032] 由上述步驟得到的化，0)即可確定特定目標(biāo)具體所在位置信息，W及根據(jù)下一個(gè) 時(shí)刻化，e)可W獲得特定目標(biāo)的運(yùn)動(dòng)信息，包括速度和方向角，所得位置誤差范圍在±10cm W內(nèi)，位置和運(yùn)動(dòng)信息是基于機(jī)器人和特定目標(biāo)直接計(jì)算，不存在累積誤差。由于計(jì)算過程 中將自由移動(dòng)的人或物作為一個(gè)質(zhì)點(diǎn)，而實(shí)際中自由移動(dòng)的人具有一定的體積，故在誤差 范圍內(nèi)移動(dòng)機(jī)器人能根據(jù)位置信息確定人或物的位置。根據(jù)每一時(shí)刻移動(dòng)機(jī)器人獲得的自 由移動(dòng)的人或物的位置和運(yùn)動(dòng)信息，對(duì)自由移動(dòng)的人或物進(jìn)行實(shí)時(shí)跟隨，即移動(dòng)機(jī)器人可 實(shí)現(xiàn)對(duì)自由移動(dòng)的人或物的跟隨任務(wù)。
【主權(quán)項(xiàng)】
1. 一種基于UWB的自主跟隨機(jī)器人定位系統(tǒng)，包括移動(dòng)機(jī)器人（1)、特定目標(biāo)、UWB基站 (3)和UWB定位標(biāo)簽(5 )，所述的特定目標(biāo)(4)為自由移動(dòng)的人或者物，其特征在于:所述移動(dòng) 機(jī)器人（1)上固定至少三個(gè)位置成三角形態(tài)的UWB基站（3)，所述自由移動(dòng)的人或物上放置 UWB定位標(biāo)簽(5);所述的UWB基站（3)發(fā)送UWB信號(hào)；所述的UWB定位標(biāo)簽(5)接收從UWB基站 (3)發(fā)來的UWB信號(hào);UWB定位標(biāo)簽(5)與UWB基站⑶時(shí)間同步，利用TOA算法獲得基站與標(biāo)簽 間的距離，然后通過幾何運(yùn)算獲得標(biāo)簽的坐標(biāo)，確定自由移動(dòng)的人或物相對(duì)于移動(dòng)機(jī)器(1) 人的位置和運(yùn)動(dòng)信息，實(shí)現(xiàn)移動(dòng)機(jī)器人(1)對(duì)自由移動(dòng)的人或物的跟隨。2. 根據(jù)權(quán)利要求1所述的基于UWB的自主跟隨機(jī)器人定位系統(tǒng)，其特征在于:所述的UWB 基站（3)為三個(gè)，三個(gè)UWB基站（3)在移動(dòng)機(jī)器人（1)身上按等邊三角形的三個(gè)頂點(diǎn)位置擺 放。3. 根據(jù)權(quán)利要求1所述的基于UWB的自主跟隨機(jī)器人定位系統(tǒng)，其特征在于:所述的UWB 標(biāo)簽穿（5)戴于自由移動(dòng)的人的手腕或腰間、或掛戴于脖子上。4. 一種基于UWB的自主跟隨機(jī)器人定位方法，包括UWB基站發(fā)射UWB信號(hào)，UWB定位標(biāo)簽 接收UWB信號(hào)并進(jìn)行定位解算;所述接收端解算方法具體步驟如下： 步驟1，UWB基站定時(shí)發(fā)送UWB信號(hào)，UWB定位標(biāo)簽接收信號(hào)，且標(biāo)簽與基站時(shí)間同步，利 用TOA算法，根據(jù)UWB信號(hào)在基站和標(biāo)簽之間的傳播時(shí)間t，由d = c*t，c為光速，得到三個(gè)UWB 基站分別于UWB定位標(biāo)簽的距離AM，BM，CM為d 1，d2，d3，根據(jù)已知的AB，BC，AC的距離以及AM， BM，CM的距離可以確定一個(gè)交點(diǎn)，這個(gè)交點(diǎn)是分別以A，B，C為圓心，dl，d2，d3為半徑的三個(gè) 球體的交點(diǎn)，該交點(diǎn)就是UWB定位標(biāo)簽所在的位置； 定義三個(gè)UWB基站位置距離AB，BC，AC均為a;定義BC邊上的高所在延長(zhǎng)線為y軸，y軸正 向與機(jī)器人自身的方向一致;三個(gè)UWB基站構(gòu)成的等邊三角形的邊AB的中點(diǎn)為0(0，0，0 )，標(biāo) 簽的坐標(biāo)為M(x，y，z)，其中z為標(biāo)簽到基站所在水平面的高度;等邊三角形的中心為H; 步驟2，根據(jù)三個(gè)UWB基站到UWB定位標(biāo)簽的距離，通過解析幾何算出：標(biāo)簽Μ在等邊三角 形所在平面的投影Μ'點(diǎn)到等邊三角形中心的距離：標(biāo)簽Μ在等邊三角形所在平面的投影Μ'點(diǎn)與等邊三角形的中心Η點(diǎn)的連線，即Μ'Η與y軸 的夾角即方位角：由上述步驟得到的（L，Θ)即可確定特定目標(biāo)具體所在位置信息，以及根據(jù)下一個(gè)時(shí)刻 (L，0)可以獲得特定目標(biāo)的運(yùn)動(dòng)信息，包括速度和方向角，所得位置誤差范圍在± l〇cm以 內(nèi)，在誤差范圍內(nèi)移動(dòng)機(jī)器人根據(jù)位置信息確定自由移動(dòng)的人或物的位置;并根據(jù)每一時(shí) 刻移動(dòng)機(jī)器人獲得的自由移動(dòng)的人或物的位置和運(yùn)動(dòng)信息，對(duì)自由移動(dòng)的人或物進(jìn)行實(shí)時(shí) 跟隨，即移動(dòng)機(jī)器人可實(shí)現(xiàn)對(duì)自由移動(dòng)的人或物的跟隨任務(wù)。
【文檔編號(hào)】H04W4/00GK105828431SQ201610259595
【公開日】2016年8月3日
【申請(qǐng)日】2016年4月25日
【發(fā)明人】方晨晨, 邵翔, 王永雄, 饒林
【申請(qǐng)人】上海理工大學(xué)