本發(fā)明涉及機(jī)器人領(lǐng)域，具體涉及一種為第三方提供空間定位信息的自跟隨機(jī)器人裝置及方法。

背景技術(shù)：

隨著機(jī)器人相關(guān)技術(shù)不斷在生產(chǎn)生活領(lǐng)域的應(yīng)用，人們對機(jī)器人的交互功能需求越來越高，機(jī)器人跟隨技術(shù)是人機(jī)交互領(lǐng)域的重要組成部分，機(jī)器人跟隨技術(shù)在機(jī)器人家用、商用以及軍用多個領(lǐng)域都有重要的作用?，F(xiàn)有的發(fā)明專利cn105955251a涉及一種機(jī)器人的視覺跟隨控制方法及機(jī)器人，傳感器檢測單元檢測周圍環(huán)境獲取環(huán)境的深度數(shù)據(jù)圖，并將獲取的深度數(shù)據(jù)圖傳送給數(shù)據(jù)處理控制單元；數(shù)據(jù)處理控制單元接收到深度數(shù)據(jù)圖后對數(shù)據(jù)進(jìn)行處理，獲取真實世界坐標(biāo)系，對環(huán)境中的人和障礙物進(jìn)行識別，并對跟隨目標(biāo)進(jìn)行骨架和動作識別和跟蹤，獲取執(zhí)行命令；運動執(zhí)行單元根據(jù)獲取的跟隨目標(biāo)信息控制機(jī)器人對跟隨目標(biāo)進(jìn)行跟蹤，根據(jù)獲取的執(zhí)行命令對機(jī)器人進(jìn)行控制，執(zhí)行所述命令。該發(fā)明可以快速識別跟隨目標(biāo)，完成對多人環(huán)境中跟隨目標(biāo)的有效辨識，在跟隨過程中根據(jù)跟隨目標(biāo)的位置變化迅速做出反應(yīng)，適應(yīng)絕大多數(shù)的商用應(yīng)用場景。但是使用在虛擬現(xiàn)實的空間定位，要求精度高、延時低、穩(wěn)定性高。往往只能通過在室內(nèi)固定的位置架設(shè)攝像頭或激光發(fā)射器等，而且定位范圍有限、成本高。

本發(fā)明的內(nèi)容

本發(fā)明的目的是為解決上述不足，提供一種為第三方提供空間定位信息的自跟隨機(jī)器人裝置及方法。

本發(fā)明的目的是通過以下技術(shù)方案實現(xiàn)的：

一種為第三方提供空間定位信息的自跟隨機(jī)器人裝置，包括視覺單元、云臺、控制裝置和行走輪，視覺單元安裝在云臺上，云臺安裝在控制裝置的上部，4個行走輪安裝在控制裝置的兩側(cè)，控制裝置包括運動機(jī)構(gòu)、主板、運動距離測量機(jī)構(gòu)、通信單元、第三方系統(tǒng)和定制光學(xué)標(biāo)記，運動機(jī)構(gòu)、運動距離測量機(jī)構(gòu)和通信單元分別連接主板，第三方系統(tǒng)連接通信單元，定制光學(xué)標(biāo)記連接第三方系統(tǒng)。

運動機(jī)構(gòu)包括伺服電機(jī)和傳動機(jī)構(gòu)，運動機(jī)構(gòu)在預(yù)設(shè)的軌道上運行，所述軌道為滑軌、光學(xué)標(biāo)記軌道、或者通過slam記錄的無形路徑。

云臺能在前進(jìn)和轉(zhuǎn)彎的過程中，保證視覺模塊的穩(wěn)定，從而保證體驗者的虛擬視角的穩(wěn)定和平滑。

第三方系統(tǒng)為移動vr頭戴式顯示器、背負(fù)式主機(jī)和主機(jī)端頭戴式顯示器、需要被定位的游戲道具中的一種。

視覺單元采集與體驗者的相對位置信息，結(jié)合運動距離測量進(jìn)行計算，將體驗者相對于場地的絕對空間位置信息通過通信模塊發(fā)送給體驗者。

一種為第三方提供空間定位信息的方法，具體方法如下：

步驟201：基于雙目紅外視覺模塊的單光點標(biāo)識定位計算

在標(biāo)定好的雙目視覺系統(tǒng)的相機(jī)鏡片上貼上紅外濾光片，過濾除標(biāo)識點外的光點信息。通過雙目視覺系統(tǒng)拍攝的差異圖片，對匹配的光點標(biāo)識進(jìn)行三維空間的定位。其計算如下：

設(shè)兩個鏡頭中心點的中點作為雙目系統(tǒng)的坐標(biāo)原點，中心點距離為b；光軸方向為z軸正方向，左相機(jī)鏡頭中點到右相機(jī)鏡頭中點的方向為x軸正方向；相機(jī)設(shè)待測的光點標(biāo)識三維坐標(biāo)為(x,y,z)，設(shè)左右攝像機(jī)拍攝到的點在成像平面的坐標(biāo)分別為(xl,yl)、(xr,yr)，兩個攝像機(jī)的焦距為f。根據(jù)相似三角形的性質(zhì)可以知道：

求解的

步驟301：多點標(biāo)識的目標(biāo)旋轉(zhuǎn)計算

由步驟一可以測量出多個光點的三維坐標(biāo).通過測量人體上四個不共面的光學(xué)標(biāo)識的三維坐標(biāo)可以確定人體相對于相機(jī)的的旋轉(zhuǎn)信息。具體方法如下：

設(shè)被測量四個點a、b、c、d的齊次化坐標(biāo)為：p'd＝(x1,y1,z1,1)，p'c＝(x2,y2,z2,1)，p'b＝(x3,y3,z3,1)，p'a＝(x4,y4,z4,1)。構(gòu)建pd＝(0,0,0,1)，pc＝(|dc|,0,0,1)，pb＝(0,|db|,0,1)，pa＝(0,0,|da|,1)。設(shè)所求的旋轉(zhuǎn)信息有基于機(jī)器人坐標(biāo)系的旋轉(zhuǎn)矩陣r2，偏移矩陣t2：

p＝[r|t]3x4p'

根據(jù)的旋轉(zhuǎn)關(guān)系，等式右邊代入測量點坐標(biāo)，等式坐標(biāo)代入對應(yīng)的構(gòu)建點的坐標(biāo)，求解12條線性方程組即可求解旋基于機(jī)器人坐標(biāo)系的轉(zhuǎn)矩陣和偏移矩陣的12個參數(shù)[r|t]3x4。

步驟202：設(shè)計軌道

根據(jù)具體場地，設(shè)定有向的連續(xù)軌道，軌道的高度是恒定的，以機(jī)器人的起始位置為世界坐標(biāo)系的原點。機(jī)器人只能在設(shè)定的軌道上作沿著軌道向前或沿著軌道向后運動，其視覺平面始終垂直軌道的切線。

機(jī)器人在軌道上的實時運動中，由于軌道是預(yù)設(shè)的，根據(jù)機(jī)器人在軌道的三維坐標(biāo)在世界坐標(biāo)系下的三維坐標(biāo)為t1，和機(jī)器人和軌道切線的關(guān)系，可以知道其在世界坐標(biāo)系下旋轉(zhuǎn)矩陣r1

步驟302：移動跟隨

設(shè)定機(jī)器人需要保持的相對距離r(t)；機(jī)器人實時測量與人的相對距離c(t)；通過pid控制器，計算差值e(t)＝r(t)-c(t)和調(diào)整量根據(jù)pid控制器的調(diào)整量，在預(yù)設(shè)定的軌跡上選擇向前或者向后運動，在實時測量的距離c(t)和設(shè)定距離r(t)的差值穩(wěn)定在可接受范圍內(nèi)停止機(jī)器人的運動。

步驟401：坐標(biāo)系換算

根據(jù)步驟301得出的目標(biāo)在機(jī)器人坐標(biāo)系的旋轉(zhuǎn)矩陣r2，偏移矩陣t2和步驟202得出的機(jī)器人在世界坐標(biāo)下的旋轉(zhuǎn)矩陣r1，偏移矩陣t1，計算目標(biāo)在世界坐標(biāo)系的旋轉(zhuǎn)矩陣r和偏移矩陣t，即可確定目標(biāo)在大型場景中的位置和旋轉(zhuǎn)。

r＝r1r2

本發(fā)明具有如下有益的效果：

本發(fā)明使用了紅外激光發(fā)射器、紅外濾光片、紅外被動反光標(biāo)記組合的方式，可以有效排除環(huán)境的干擾；降低區(qū)分人和環(huán)境算法、生成人體骨骼的運算時間，提高了實時性，降低了定位過程中的攝像頭使用量和計算量，降低定位技術(shù)難度，可在vr移動系統(tǒng)中實現(xiàn)定位產(chǎn)品量化。

附圖說明

圖1為本發(fā)明的機(jī)器人整體結(jié)構(gòu)示意圖；

圖2為本發(fā)明的機(jī)器人功能框圖；

圖3為本發(fā)明的方法流程圖；

圖4為本發(fā)明的雙目單點定位原理圖；

圖5為本發(fā)明的多點標(biāo)識的目標(biāo)旋轉(zhuǎn)計算原理圖；

圖6為本發(fā)明的場地的設(shè)定和抽象圖；

圖7為本發(fā)明的相對距離pid控制圖；

圖8為本發(fā)明的應(yīng)用場景圖。

具體實施方式

下面結(jié)合附圖對本發(fā)明作進(jìn)一步的說明：

如圖1和圖2所示，一種為第三方提供空間定位信息的自跟隨機(jī)器人裝置，包括視覺單元1、云臺2、控制裝置9和行走輪10，視覺單元1安裝在云臺2上，云臺2安裝在控制裝置9的上部，4個行走輪10安裝在控制裝置9的兩側(cè)，控制裝置9包括運動機(jī)構(gòu)3、主板4、運動距離測量機(jī)構(gòu)5、通信單元6、第三方系統(tǒng)7和定制光學(xué)標(biāo)記8，運動機(jī)構(gòu)3、運動距離測量機(jī)構(gòu)5和通信單元6分別連接主板4，第三方系統(tǒng)7連接通信單元6，定制光學(xué)標(biāo)記8連接第三方系統(tǒng)7。

運動機(jī)構(gòu)3包括伺服電機(jī)和傳動機(jī)構(gòu)，運動機(jī)構(gòu)在預(yù)設(shè)的軌道上運行，所述軌道為滑軌、光學(xué)標(biāo)記軌道、或者通過slam記錄的無形路徑。

云臺2能在前進(jìn)和轉(zhuǎn)彎的過程中，保證視覺模塊的穩(wěn)定，從而保證體驗者的虛擬視角的穩(wěn)定和平滑。

第三方系統(tǒng)7為移動vr頭戴式顯示器、背負(fù)式主機(jī)和主機(jī)端頭戴式顯示器、需要被定位的游戲道具中的一種。

視覺單元采集與體驗者的相對位置信息，結(jié)合運動距離測量進(jìn)行計算，將體驗者相對于場地的絕對空間位置信息通過通信模塊發(fā)送給體驗者。

一種為第三方提供空間定位信息的方法，方法流程如圖3所示：

步驟201：基于雙目紅外視覺模塊的單光點標(biāo)識定位計算

在標(biāo)定好的雙目視覺系統(tǒng)的相機(jī)鏡片上貼上紅外濾光片，過濾除標(biāo)識點外的光點信息。通過雙目視覺系統(tǒng)拍攝的差異圖片，對匹配的光點標(biāo)識進(jìn)行三維空間的定位。其計算如下：

如圖4，設(shè)兩個鏡頭中心點的中點作為雙目系統(tǒng)的坐標(biāo)原點，中心點距離為b；光軸方向為z軸正方向，左相機(jī)鏡頭中點到右相機(jī)鏡頭中點的方向為x軸正方向；相機(jī)設(shè)待測的光點標(biāo)識三維坐標(biāo)為(x,y,z)，設(shè)左右攝像機(jī)拍攝到的點在成像平面的坐標(biāo)分別為(xl,yl)、(xr,yr)，兩個攝像機(jī)的焦距為f。根據(jù)相似三角形的性質(zhì)可以知道：

求解的

步驟301：多點標(biāo)識的目標(biāo)旋轉(zhuǎn)計算

由步驟一可以測量出多個光點的三維坐標(biāo).通過測量人體上四個不共面的光學(xué)標(biāo)識的三維坐標(biāo)可以確定人體相對于相機(jī)的的旋轉(zhuǎn)信息。具體方法如下：

如圖5，設(shè)被測量四個點a、b、c、d的齊次化坐標(biāo)為：p'd＝(x1,y1,z1,1)，p'c＝(x2,y2,z2,1)，p'b＝(x3,y3,z3,1)，p'a＝(x4,y4,z4,1)。構(gòu)建pd＝(0,0,0,1)，pc＝(|dc|,0,0,1)，pb＝(0,|db|,0,1)，pa＝(0,0,|da|,1)。設(shè)所求的旋轉(zhuǎn)信息有基于機(jī)器人坐標(biāo)系的旋轉(zhuǎn)矩陣r2，偏移矩陣t2：

p＝[r|t]3x4p'

根據(jù)的旋轉(zhuǎn)關(guān)系，等式右邊代入測量點坐標(biāo)，等式坐標(biāo)代入對應(yīng)的構(gòu)建點的坐標(biāo)，求解12條線性方程組即可求解旋基于機(jī)器人坐標(biāo)系的轉(zhuǎn)矩陣和偏移矩陣的12個參數(shù)[r|t]3x4。

步驟202：設(shè)計軌道

如圖6，根據(jù)具體場地，設(shè)定有向的連續(xù)軌道，軌道的高度是恒定的，以機(jī)器人的起始位置為世界坐標(biāo)系的原點。機(jī)器人只能在設(shè)定的軌道上作沿著軌道向前或沿著軌道向后運動，其視覺平面始終垂直軌道的切線。

機(jī)器人在軌道上的實時運動中，由于軌道是預(yù)設(shè)的，根據(jù)機(jī)器人在軌道的三維坐標(biāo)在世界坐標(biāo)系下的三維坐標(biāo)為t1，和機(jī)器人和軌道切線的關(guān)系，可以知道其在世界坐標(biāo)系下旋轉(zhuǎn)矩陣r1

步驟302：移動跟隨

如圖7的pid控制器，設(shè)定機(jī)器人需要保持的相對距離r(t)；機(jī)器人實時測量與人的相對距離c(t)；通過pid控制器，計算差值e(t)＝r(t)-c(t)和調(diào)整量根據(jù)pid控制器的調(diào)整量，在預(yù)設(shè)定的軌跡上選擇向前或者向后運動，在實時測量的距離c(t)和設(shè)定距離r(t)的差值穩(wěn)定在可接受范圍內(nèi)停止機(jī)器人的運動。

步驟401：坐標(biāo)系換算

根據(jù)步驟301得出的目標(biāo)在機(jī)器人坐標(biāo)系的旋轉(zhuǎn)矩陣r2，偏移矩陣t2和步驟202得出的機(jī)器人在世界坐標(biāo)下的旋轉(zhuǎn)矩陣r1，偏移矩陣t1，計算目標(biāo)在世界坐標(biāo)系的旋轉(zhuǎn)矩陣r和偏移矩陣t，即可確定目標(biāo)在大型場景中的位置和旋轉(zhuǎn)。

r＝r1r2

如圖8所示，體驗者在虛擬主題公園的場地中走動，背上背負(fù)式主機(jī)電腦，戴上虛擬現(xiàn)實頭戴式顯示器。跟隨機(jī)器人運行在體驗者的前面，并保持一定的距離。跟隨機(jī)器人通過wifi、藍(lán)牙等無線通信方式，為體驗者提供虛擬視角、數(shù)據(jù)手套、游戲道具的空間位置和姿態(tài)信息，并能保證視角平滑地移動。