一種服務(wù)機(jī)器人家庭全息地圖構(gòu)建方法研究

摘 要:提出了一種滿足家庭服務(wù)機(jī)器人環(huán)境認(rèn)知和智能服務(wù)需要的融合環(huán)境和目標(biāo)信息的家庭全息地圖。設(shè)計(jì)了局部幾何—全局拓?fù)涞娜⒌貓D分層表示模型。分析了機(jī)器人坐標(biāo)系、局部環(huán)境坐標(biāo)系和目標(biāo)的相對(duì)關(guān)系，給出了機(jī)器人局部環(huán)境自定位算法和基于坐標(biāo)變換的服務(wù)機(jī)器人全息地圖構(gòu)建方法。家庭環(huán)境下機(jī)器人實(shí)物實(shí)驗(yàn)表明，基于局部幾何—全局拓?fù)浔硎镜娜⒌貓D，服務(wù)機(jī)器人路徑規(guī)劃和任務(wù)執(zhí)行效率得到有效提升。

關(guān)鍵詞:家庭服務(wù)機(jī)器人; 家庭全息地圖; 幾何—拓?fù)浞謱幽Ｐ? QR code 標(biāo)簽

中圖分類(lèi)號(hào):TP242.6文獻(xiàn)標(biāo)志碼:A

文章編號(hào):1001-3695(2010)03-0981-05

doi:10.3969/j.issn.10013695.2010.03.047

Research on constructing of household holographic map for service robot

WU Pei-liang， KONG Ling-fu， ZHAO Feng-da

(College of Information Science Engineering， Yanshan University， Qinhuangdao Hebei 066004， China)

Abstract:This paper proposed a novel household holographic map， which combined information of environment and objects， to meet the needs of intelligent cognition and operation for home service robot. It designed a locallymetric and globallytopological hierarchical model. Analyxed the relationships between robot coordinate， local environment coordinate and object， and then presented both the algorithm for service robot selflocalization within local environment and the methods to build the holographic map based on coordinate transformation. The robot experiments in real household environments show that according to the holographic map described with the hierarchical metrictopological model， the efficiencies of both path planning and task execution for robot are obvious improved.

Key words:home service robot; household holographic map; hierarchical metrictopological model; QR code label

0 引言

在移動(dòng)式服務(wù)機(jī)器人研究領(lǐng)域中，環(huán)境地圖是機(jī)器人工作環(huán)境的抽象化表示，是機(jī)器人感知并適應(yīng)環(huán)境，繼而完成路徑規(guī)劃和定位導(dǎo)航，并最終提供特定服務(wù)的依據(jù)。目前，對(duì)于環(huán)境地圖表示方法已有許多文獻(xiàn)從不同側(cè)面給予了討論[1，2]。從機(jī)器人工作環(huán)境上看可分為室內(nèi)結(jié)構(gòu)化環(huán)境地圖和室外非結(jié)構(gòu)化環(huán)境地圖;從環(huán)境的描述方法來(lái)看，有幾何地圖、拓?fù)涞貓D和混合地圖等;從描述的空間范圍上看，有二維平面地圖和三維立體地圖等。

作為服務(wù)機(jī)器人的重要分支，家庭服務(wù)機(jī)器人工作于家庭室內(nèi)結(jié)構(gòu)化環(huán)境中，對(duì)于這種結(jié)構(gòu)事先已知的確定性環(huán)境的地圖描述方法研究較為廣泛。文獻(xiàn)[3]給出了一種基于幾何特征的室內(nèi)服務(wù)機(jī)器人環(huán)境地圖表示方法。文獻(xiàn)[4]提出了一種適于機(jī)器人在大規(guī)模室內(nèi)環(huán)境下自主導(dǎo)航的新型拓?fù)涞貓D;文獻(xiàn)[5，6]綜合考慮幾何和拓?fù)鋬煞N表示方法的優(yōu)缺點(diǎn)，創(chuàng)建了幾何—拓?fù)浠旌檄h(huán)境地圖，在全局空間采取拓?fù)涿枋鲆员ＷC全局連續(xù)性，在具體的局部環(huán)境中則采用幾何表示以利于機(jī)器人精確定位。上述地圖模型僅僅約束了機(jī)器人的可活動(dòng)空間，而沒(méi)有融入操作目標(biāo)的必要信息，利用這種地圖，機(jī)器人能夠解決“我在哪里”和“我怎樣到達(dá)那里”的問(wèn)題，但由于目標(biāo)在地圖中沒(méi)有體現(xiàn)，導(dǎo)致機(jī)器人對(duì)環(huán)境中各目標(biāo)及其分布缺乏必要的認(rèn)知，難以自主完成“目標(biāo)在哪里”和“我要去目標(biāo)那里”的任務(wù)?？梢?jiàn)，基于目前地圖的機(jī)器人路徑規(guī)劃和定位導(dǎo)航尚難以滿足家庭服務(wù)機(jī)器人任務(wù)的需要。

此外，盡管機(jī)器人對(duì)操作目標(biāo)的識(shí)別已得到了深入研究，基于顏色[7]、局部興趣點(diǎn)[8]等視覺(jué)特征對(duì)某一具體目標(biāo)或某類(lèi)目標(biāo)的識(shí)別具有較高的成功率。然而，家庭環(huán)境作為人們?nèi)粘Ｉ畹闹饕獔?chǎng)所，其中目標(biāo)繁多、背景雜亂，并呈現(xiàn)出一定的動(dòng)態(tài)性，而且，目標(biāo)的深層次信息往往并不能簡(jiǎn)單地通過(guò)機(jī)器人傳感器數(shù)據(jù)獲得。為了提供高效、智能服務(wù)，家庭服務(wù)機(jī)器人必須能夠有效地、深層次地理解其所在的工作環(huán)境(包括整個(gè)家庭空間、操作目標(biāo)、服務(wù)對(duì)象等)，并實(shí)時(shí)感知其變化。

基于上述考慮，本文把融合家庭全局空間及其中目標(biāo)資源分布和動(dòng)態(tài)變化的環(huán)境稱為服務(wù)機(jī)器人的全息家庭環(huán)境，記錄全息家庭環(huán)境信息的地圖稱為服務(wù)機(jī)器人的家庭全息地圖。這里，全息體現(xiàn)了兩個(gè)層次的含義:a)全息地圖記錄了環(huán)境及其中目標(biāo)的全局空間分布信息;b)全息地圖記錄了環(huán)境及其中目標(biāo)的動(dòng)態(tài)變化信息。本文設(shè)計(jì)了局部幾何—全局拓?fù)涞娜⒌貓D分層表示模型，并給出了基于坐標(biāo)變換的服務(wù)機(jī)器人全息地圖構(gòu)建方法。

1 家庭全息地圖的表示

1.1 局部幾何—全局拓?fù)浞謱幽Ｐ?/p>

局部幾何—全局拓?fù)涞幕旌戏謱迎h(huán)境模型一方面利用全局拓?fù)浣Y(jié)構(gòu)保持了全局一致性，另一方面基于局部幾何特征提高了機(jī)器人在非拓?fù)涔?jié)點(diǎn)位置處的定位精度[5，6]。全局拓?fù)涫窃谶壿嫿嵌壬系沫h(huán)境描述，其節(jié)點(diǎn)往往并不是單純的某一個(gè)空間點(diǎn)，而是代表包含一定量局部環(huán)境信息的標(biāo)志該節(jié)點(diǎn)特性的局部幾何地圖。機(jī)器人從一個(gè)節(jié)點(diǎn)運(yùn)動(dòng)到另一個(gè)節(jié)點(diǎn)，即機(jī)器人從一個(gè)局部幾何地圖進(jìn)入另一個(gè)局部幾何地圖，在此情況下要求全局拓?fù)涞貓D能夠給出這種狀態(tài)變化的描述，這就是拓?fù)溥叺淖饔谩?/p>

在傳統(tǒng)的幾何—拓?fù)涞貓D中，幾何特征是指由機(jī)器人傳感器獲取的環(huán)境高級(jí)特征，如線段、拐角等，拓?fù)涔?jié)點(diǎn)指不同通道之間的交叉區(qū)域或只有一個(gè)出口的區(qū)域，拓?fù)溥吤枋鰞蓚€(gè)拓?fù)涔?jié)點(diǎn)之間的連通性，當(dāng)兩個(gè)節(jié)點(diǎn)間存在拓?fù)溥厱r(shí)表明這兩個(gè)節(jié)點(diǎn)之間存在通路。

1.2 全息地圖表示方法

本文借鑒傳統(tǒng)幾何—拓?fù)涞貓D的思想，并對(duì)其進(jìn)行改進(jìn)以適于描述融合家庭環(huán)境及其中目標(biāo)分布的家庭全息地圖。全息地圖采用局部幾何—全局拓?fù)涞膬蓪咏Y(jié)構(gòu):局部層面上，局部幾何地圖分別由局部環(huán)境地圖和目標(biāo)信息構(gòu)成;全局層面上，在傳統(tǒng)拓?fù)涞貓D基礎(chǔ)上引入目標(biāo)拓?fù)涔?jié)點(diǎn)及其拓?fù)溥叺母拍睿磳⑼負(fù)涔?jié)點(diǎn)分為環(huán)境節(jié)點(diǎn)和目標(biāo)節(jié)點(diǎn)兩種，其中環(huán)境節(jié)點(diǎn)對(duì)應(yīng)局部環(huán)境特征模型及其數(shù)據(jù)，目標(biāo)節(jié)點(diǎn)對(duì)應(yīng)目標(biāo)過(guò)完備特征集模型及其數(shù)據(jù)。由于拓?fù)涔?jié)點(diǎn)分別表征局部環(huán)境或目標(biāo)，分兩種情況定義拓?fù)溥?a)當(dāng)拓?fù)溥呥B接兩個(gè)環(huán)境節(jié)點(diǎn)時(shí)，該拓?fù)溥叡碚鲀蓚€(gè)局部環(huán)境地圖之間的可達(dá)路徑信息，以供機(jī)器人路徑規(guī)劃;b)當(dāng)拓?fù)溥呥B接目標(biāo)節(jié)點(diǎn)和環(huán)境節(jié)點(diǎn)時(shí)，該拓?fù)溥叡碚髂繕?biāo)節(jié)點(diǎn)相對(duì)于環(huán)境節(jié)點(diǎn)的空間位置信息，以供機(jī)械臂操作規(guī)劃。

該家庭全息地圖的公式表述如下:

M={V，E}(1)

V=Ve∪Vo={v1，v2，…，vm，…，vM}(2)

vm={H，Lm} vm∈Ve{F，Gm} vm∈Vo(3)

E=Ee∪Eo={e0，e1，…，en，…，eN}(4)

en={vivj，Sij，wij} vi∈Ve and vj∈Ve{vivj，Pij，kij} vi∈Vo and vj∈Ve(5)

其中:M表示家庭全息環(huán)境的全局拓?fù)涞貓D;V表示拓?fù)涔?jié)點(diǎn)集，由環(huán)境拓?fù)涔?jié)點(diǎn)集合Ve和目標(biāo)拓?fù)涔?jié)點(diǎn)集合Vo的并集構(gòu)成。對(duì)于V中的拓?fù)涔?jié)點(diǎn)vm，若vm為環(huán)境拓?fù)涔?jié)點(diǎn)，有局部幾何地圖模型H和該局部幾何地圖的特征數(shù)據(jù)Lm與之綁定;若vm為目標(biāo)拓?fù)涔?jié)點(diǎn)，則有目標(biāo)過(guò)完備特征集模型F和該目標(biāo)的過(guò)完備特征集數(shù)據(jù)Gm與之綁定。E表示拓?fù)溥吋?，由連接環(huán)境—環(huán)境拓?fù)涔?jié)點(diǎn)的拓?fù)溥吋螮e和連接目標(biāo)—環(huán)境拓?fù)涔?jié)點(diǎn)的拓?fù)溥吋螮o的并集構(gòu)成。對(duì)于E中的每一條邊en，當(dāng)en連接兩個(gè)環(huán)境拓?fù)涔?jié)點(diǎn)vi、vj時(shí)，有表示節(jié)點(diǎn)vi、vj之間通路的說(shuō)明序列Sij和表示兩節(jié)點(diǎn)所對(duì)應(yīng)環(huán)境坐標(biāo)系間位姿關(guān)系的參數(shù)wij與之對(duì)應(yīng);當(dāng)en連接目標(biāo)拓?fù)涔?jié)點(diǎn)vi和環(huán)境拓?fù)涔?jié)點(diǎn)vj時(shí)，有表示節(jié)點(diǎn)vi、vj之間相對(duì)關(guān)系的說(shuō)明序列Pij和表示目標(biāo)在環(huán)境坐標(biāo)系中空間位置的參數(shù)kij與之對(duì)應(yīng)。

直觀起見(jiàn)，圖1給出了一個(gè)簡(jiǎn)易的家庭全息地圖實(shí)例以描述該問(wèn)題的典型場(chǎng)景，選擇走廊路口、房門(mén)和水杯三種常見(jiàn)環(huán)境特征或機(jī)器人操作目標(biāo)構(gòu)建局部幾何地圖。從局部幾何地圖中提取拓?fù)涔?jié)點(diǎn)，將一系列拓?fù)涔?jié)點(diǎn)按照一定的拓?fù)浣Y(jié)構(gòu)組織起來(lái)構(gòu)建出全局拓?fù)涞貓D。

2 家庭全息地圖的構(gòu)建

2.1 機(jī)器人傳感器模型

本文所用家庭服務(wù)機(jī)器人主體為一臺(tái)Pioneer Ⅲ型移動(dòng)機(jī)器人，為其配備了一套由PTZ攝像機(jī)和手眼攝像機(jī)組成的雙目系統(tǒng)(圖2)，機(jī)械手系統(tǒng)安裝在機(jī)器人頂部以獲得較大的操作空間，采用Logitech S5500作為手眼攝像機(jī)固定安裝在機(jī)械手末端平臺(tái)，該攝像機(jī)直接通過(guò)USB 2.0接口連接到機(jī)器人主機(jī)上。PTZ攝像機(jī)采用Canon VCC4，通過(guò)圖像采集卡連接到機(jī)器人主機(jī)上。不失一般性，以機(jī)械手基座坐標(biāo)系作為機(jī)器人坐標(biāo)系CR，假設(shè)XOR=(xOR，yOR，zOR，1)T為坐標(biāo)系CR中目標(biāo)點(diǎn)Obj的空間齊次坐標(biāo)，mh(huán)=(uh(huán)，vh(huán)，1)T和mp=(up，vp，1)T分別為空間點(diǎn)Obj在手眼攝像機(jī)和PTZ攝像機(jī)像平面上的像點(diǎn)齊次坐標(biāo)，兩攝像機(jī)均采用小孔成像模型，其成像過(guò)程由下面方程表示:

λh(huán)mh(huán)=[Kh(huán) 0]Rh(huán) th(huán)0T 1XOR(6a)

λPmP=[KP 0]RP tP0T 1XOR(6b)

其中:0為3×1的零向量;Kh(huán)和kP分別為手眼攝像機(jī)和PTZ攝像機(jī)的內(nèi)參矩陣，可離線標(biāo)定。[Rh(huán) th(huán)]和[RP tP]分別為兩攝像機(jī)相對(duì)于機(jī)器人坐標(biāo)系的外參矩陣，其計(jì)算過(guò)程如下:

對(duì)于手眼攝像機(jī)，機(jī)械手基座坐標(biāo)系(即機(jī)器人坐標(biāo)系)到機(jī)械手末端平臺(tái)坐標(biāo)系的變換矩陣[R1 t1]可通過(guò)由機(jī)械手控制器直接讀出，機(jī)械手末端平臺(tái)坐標(biāo)系與手眼攝像機(jī)坐標(biāo)系之間的手眼關(guān)系矩陣[Ra ta]為一定值，故可通過(guò)離線標(biāo)定[9]，則手眼攝像機(jī)在機(jī)器人坐標(biāo)系下的外參數(shù)矩陣[Rh(huán) th(huán)]可由下式得到:

[Rh(huán) th(huán)]=[R1 t1][Ra ta](7)

對(duì)于PTZ攝像機(jī)，機(jī)械手基座坐標(biāo)系與PTZ云臺(tái)坐標(biāo)系的變換矩陣[R2 t2]為一定值，故可通過(guò)離線標(biāo)定。不妨假設(shè)PTZ攝像機(jī)滿足以下兩個(gè)條件:旋轉(zhuǎn)中心和光心重合，pan和tilt的旋轉(zhuǎn)軸正交?；诖?，PTZ云臺(tái)坐標(biāo)系與PTZ攝像機(jī)坐標(biāo)系的變換矩陣[Rb tb]為

Rb=cos θP0sin θP-sin θP sin θTcos θTcos θP sin θT-sin θPcosθP-sin θTcos θP cos θT(8a)

tb=[0 0 0]T(8b)

其中:θP和θT分別為PTZ攝像機(jī)繞云臺(tái)坐標(biāo)系的x軸擺動(dòng)(pan)和y軸俯仰(tilt)的角度，其值可由云臺(tái)控制器直接讀出。故PTZ攝像機(jī)在機(jī)器人坐標(biāo)系下的外參數(shù)矩陣可由下式得到:

[RP tP]=[R2 t2][Rb tb](9)

除雙目視覺(jué)外，機(jī)器人還配備激光測(cè)距儀SICK LMS200作為外部傳感器，里程儀和數(shù)字羅盤(pán)作為內(nèi)部傳感器，相關(guān)傳感器模型已在文獻(xiàn)[10]中給出，在此不作詳細(xì)介紹。通過(guò)分析傳感器獲取的周?chē)h(huán)境數(shù)據(jù)，機(jī)器人同時(shí)完成自身定位和家庭全息地圖創(chuàng)建。

2.2 局部地圖的構(gòu)建

2.2.1 局部環(huán)境地圖構(gòu)建及機(jī)器人定位

在全息地圖的幾何—拓?fù)浣Y(jié)構(gòu)中，局部層面上幾何地圖由局部環(huán)境地圖和目標(biāo)信息構(gòu)成。本文中采用環(huán)境中的特征點(diǎn)、線段、路口和拐角作為局部環(huán)境特征模型H表征局部環(huán)境地圖，即

H={point，line，cross，corner}(10)

其中:環(huán)境特征主要利用激光測(cè)距儀和機(jī)器人視覺(jué)提取，對(duì)于點(diǎn)、線段、通路和拐角等環(huán)境特征的提取已有眾多文獻(xiàn)介紹[6，10，11]，在此不再贅述。

按照模型H從獲取的當(dāng)前環(huán)境數(shù)據(jù)中提取出點(diǎn)、線段等高級(jí)特征之后，離線地建立其與當(dāng)前局部環(huán)境地圖L的坐標(biāo)系CL的映射。由于機(jī)器人運(yùn)動(dòng)在與地面平行的水平面上，簡(jiǎn)化起見(jiàn)，設(shè)定機(jī)器人坐標(biāo)系CR和局部環(huán)境地圖坐標(biāo)系CL的z軸均豎直向上，且兩坐標(biāo)系的xy坐標(biāo)平面重合，則機(jī)器人坐標(biāo)系CR相對(duì)于當(dāng)前局部環(huán)境地圖坐標(biāo)系CL僅存在沿xy坐標(biāo)平面內(nèi)的平移和繞z軸的旋轉(zhuǎn)。若CR相對(duì)于CL的位姿參數(shù)用[RRL tRL]表征，則有

RRL=cos θRL-sin θRL0sin θRLcos θRL0001(11a)

tRL=[xRL yRL 0]T(11b)

其中:xRL、yRL 分別為坐標(biāo)系CR的原點(diǎn)在坐標(biāo)系CL中的x、y坐標(biāo)、θRL為坐標(biāo)系CR相對(duì)于坐標(biāo)系CL繞z軸旋轉(zhuǎn)的角度。為表述方便，后面將用[RRL(θRL) tRL(xRL，yRL)]表示CR相對(duì)于CL的位姿參數(shù)[RRL tRL]。

在線階段，利用實(shí)時(shí)獲取的環(huán)境特征與離線建立的局部環(huán)境地圖坐標(biāo)系中的特征進(jìn)行匹配，進(jìn)而求解機(jī)器人坐標(biāo)系CR與所在局部環(huán)境坐標(biāo)系CL之間的位姿參數(shù)[RRL(θRL) tRL(xRL，yRL)]，此即為機(jī)器人自定位。這也是后續(xù)全局拓?fù)涞貓D構(gòu)建的關(guān)鍵。

假設(shè)特征點(diǎn)Obj在機(jī)器人坐標(biāo)系CR下的齊次坐標(biāo)為XOR=(xOR，yOR，zOR，1)T，在局部環(huán)境地圖坐標(biāo)系CL中對(duì)應(yīng)點(diǎn)坐標(biāo)為XOL=(xOL，yOL，zOL，1)T，不難推導(dǎo)出如下關(guān)系:

xOL=xRL+x2OR+y2OR cos(β+θRL)(12a)

yOL=yRL+x2OR+y2OR sin(β+θRL)(12b)

其中:β=arctan(yOR/xOR)。上式存在三個(gè)未知數(shù)，因此至少需要兩對(duì)特征點(diǎn)對(duì)，即可求得xRL、yRL和θRL，進(jìn)而根據(jù)式(11a)(11b)得到機(jī)器人坐標(biāo)系CR與局部環(huán)境坐標(biāo)系CL之間的位姿參數(shù)[RRL(θRL) tRL(xRL，yRL)]。實(shí)際中，選取多組特征點(diǎn)對(duì)以提高xRL、yRL和θRL的求解精度。

2.2.2 目標(biāo)過(guò)完備特征集構(gòu)建

目標(biāo)過(guò)完備特征集盡量保留目標(biāo)的各種信息，用于在后續(xù)機(jī)器人工作過(guò)程中跟蹤和定位變動(dòng)的目標(biāo)，進(jìn)而完成全息地圖中相應(yīng)目標(biāo)節(jié)點(diǎn)和拓?fù)溥叺母?。本文中以雙目視覺(jué)作為主要傳感器，選擇目標(biāo)的尺寸、紋理、主色集和興趣點(diǎn)作為目標(biāo)過(guò)完備特征集模型F表征目標(biāo)，即

F={size，texture，ODCICS，SIFT}(13)

其中:size為目標(biāo)的尺寸特征，其數(shù)據(jù)可通過(guò)服務(wù)機(jī)器人雙目主動(dòng)視覺(jué)獲取;texture、ODCS和SIFT分別為目標(biāo)的紋理、主色集和興趣點(diǎn)SIFT特征，其數(shù)據(jù)均通過(guò)圖像處理獲取[7，8]。依據(jù)過(guò)完備特征集F，目標(biāo)特征可在機(jī)器人建圖過(guò)程中在線提取，但基于實(shí)時(shí)性考慮，本文利用服務(wù)機(jī)器人離線提取目標(biāo)過(guò)完備特征集數(shù)據(jù)。

2.3 全局拓?fù)涞貓D的構(gòu)建

在提取出局部環(huán)境地圖和目標(biāo)信息之后，需要將其映射為全局拓?fù)鋵用嫔系南鄳?yīng)拓?fù)涔?jié)點(diǎn)。廣義Voronoi圖(generalized Voronoi graph，GVG)易于描述室內(nèi)結(jié)構(gòu)化環(huán)境[12]，其節(jié)點(diǎn)對(duì)應(yīng)于走廊的交叉口或者一個(gè)開(kāi)放的區(qū)域(如房間、電梯等)。本文采用GVG模型來(lái)描述全息地圖，不同之處在于其拓?fù)涔?jié)點(diǎn)由式(2)中的環(huán)境節(jié)點(diǎn)和目標(biāo)節(jié)點(diǎn)構(gòu)成，環(huán)境節(jié)點(diǎn)仍對(duì)應(yīng)交叉路口或開(kāi)放區(qū)域，目標(biāo)節(jié)點(diǎn)則從家庭常見(jiàn)目標(biāo)提取得到。

對(duì)于拓?fù)涞貓D中的某一環(huán)境或目標(biāo)拓?fù)涔?jié)點(diǎn)，按照一定準(zhǔn)則將該拓?fù)涔?jié)點(diǎn)與地圖中其他已建拓?fù)涔?jié)點(diǎn)連接起來(lái)，即構(gòu)建拓?fù)溥叄ōh(huán)境—環(huán)境拓?fù)溥叺臉?gòu)建和目標(biāo)—環(huán)境拓?fù)溥叺臉?gòu)建。

2.3.1 環(huán)境—環(huán)境拓?fù)溥叺臉?gòu)建

從運(yùn)動(dòng)空間上看，機(jī)器人運(yùn)動(dòng)空間可以近似看做是與地表平行的二維平面，通過(guò)坐標(biāo)變換得到式(5)中環(huán)境拓?fù)涔?jié)點(diǎn)間的拓?fù)溥叄约氨硎径S平面內(nèi)通路關(guān)系的說(shuō)明序列和記錄兩局部環(huán)境地圖坐標(biāo)系的相對(duì)位姿參數(shù)。

假設(shè)在ti時(shí)刻，機(jī)器人處于由環(huán)境拓?fù)涔?jié)點(diǎn)vm表征的局部環(huán)境地圖Lm中，此時(shí)機(jī)器人坐標(biāo)系CRi相對(duì)于Lm坐標(biāo)系CLM的位姿參數(shù)[RRiLM(θRiLM) tRiLM(xRiLM，yRiLM)]已通過(guò)2.2.1節(jié)計(jì)算得到。在tj(j>i)時(shí)刻，機(jī)器人運(yùn)動(dòng)到由環(huán)境拓?fù)涔?jié)點(diǎn)vn表征的局部環(huán)境地圖Ln中，且此時(shí)機(jī)器人坐標(biāo)系CRj相對(duì)于Ln坐標(biāo)系CLn的位姿參數(shù)[RRiLn(θRjLn) tRjLn(xRjLn， yRjLn)]也已通過(guò)2.1節(jié)計(jì)算得到。以坐標(biāo)系CLm為基準(zhǔn)坐標(biāo)系，θk為tk(i≤k≤j-1)時(shí)刻機(jī)器人相對(duì)于基準(zhǔn)坐標(biāo)系的航向角，sk為tk到tk+1時(shí)刻機(jī)器人行駛過(guò)的距離，其值由里程儀換算得到;Δθk為tk到tk+1時(shí)刻機(jī)器人航向角的變化量，其值可以從數(shù)字羅盤(pán)直接讀出，則機(jī)器人坐標(biāo)系CRj相對(duì)于局部地圖Lm坐標(biāo)系CLM的位姿[RRjLm(θRjLm) tRjLm(xRjLm，yRjLm)]可通過(guò)如下航跡推算公式得到:

xRjLm=xRiLm+j-1k=iskcos θk(14a)

yRjLm=yRiLm+j-1k=isksinθk(14b)

θRjLm=θRiLm+j-1k=iΔθk(14c)

與式(11)類(lèi)似，環(huán)境拓?fù)涔?jié)點(diǎn)vn到vm之間拓?fù)溥叺膮?shù)(即局部環(huán)境幾何地圖Ln到Lm的位姿關(guān)系)wnm可記為[RLnLm(θLnLm) tLnLm(xLnLm，yLnLm)]，則其參數(shù)可通過(guò)如下坐標(biāo)變換得到:

xLnLm=xRjLm-x2RjLn+y2RjLn cos(α+θLnLm)(15a)

yLnLm=yRjLm-x2RjLn+y2RjLn sin(α+θLnLm)(15b)

θLnLm=θRjLm-θRjLn(15c)

其中α=arctan(yRj/xRj)。

2.3.2 目標(biāo)—環(huán)境拓?fù)溥叺臉?gòu)建

利用平面掃描式激光測(cè)距儀難以定位分布在三維空間中的操作目標(biāo)，故空間目標(biāo)定位主要通過(guò)機(jī)器人雙目主動(dòng)視覺(jué)完成。當(dāng)機(jī)器人能夠從目標(biāo)所在局部環(huán)境到達(dá)并操作該目標(biāo)時(shí)，建立式(5)中的連接目標(biāo)節(jié)點(diǎn)與所在環(huán)境節(jié)點(diǎn)的拓?fù)溥叄约懊枋鰞蓚€(gè)節(jié)點(diǎn)間的空間位置關(guān)系的說(shuō)明序列和記錄目標(biāo)在局部環(huán)境坐標(biāo)系下的空間位置參數(shù)。

假設(shè)在ti時(shí)刻，機(jī)器人坐標(biāo)系CRi相對(duì)于其所在局部環(huán)境地圖Lm坐標(biāo)系CLm的位姿參數(shù)[RRiLm(θRiLm) tRiLm(xRiLm，yRiLm)]已由2.2.1節(jié)得到。在機(jī)器人坐標(biāo)系CRi下，機(jī)器人雙攝像機(jī)的外參數(shù)矩陣已由式(7)(9)得到，聯(lián)立式(6a)(6b)，解得目標(biāo)Obj在機(jī)器人坐標(biāo)系CRi下的空間齊次坐標(biāo)XORi=(xORi，yORi，zORi，1)T。實(shí)際中，通過(guò)控制雙攝像機(jī)獲取多視角下的目標(biāo)圖像點(diǎn)對(duì)，以提高目標(biāo)點(diǎn)在機(jī)器人坐標(biāo)系下坐標(biāo)XORi的求解精度。

ti時(shí)刻Obj所對(duì)應(yīng)目標(biāo)節(jié)點(diǎn)與環(huán)境節(jié)點(diǎn)vm間拓?fù)溥叺膮?shù)(即Obj在局部環(huán)境地圖Lm的空間位置)kOLm可由平移向量[xOLm yOLm zOLm]T表示。其參數(shù)通過(guò)如下坐標(biāo)變換得到:

xOLm=xRiLm+x2ORi+y2ORi cos(β+θRiLm)(16a)

yOLm=yRiLm+x2ORi+y2ORi sin(β+θRiLm)(16b)

zOLm=zORi(16c)

其中β=arctan(yORi/xORi)。

3 實(shí)驗(yàn)與分析

3.1 實(shí)驗(yàn)描述

以前面所述Pioneer Ⅲ作為實(shí)驗(yàn)平臺(tái)，選擇面積約為9.5 m×10.5 m的真實(shí)家庭環(huán)境作為實(shí)驗(yàn)場(chǎng)地。如圖3所示，家庭環(huán)境主要由起居室、餐廳、廚房、書(shū)房和兩個(gè)臥室構(gòu)成。其中放置沙發(fā)、空調(diào)、水杯等目標(biāo)。為常見(jiàn)家庭目標(biāo)設(shè)計(jì)了便于機(jī)器人視覺(jué)識(shí)別的QR code標(biāo)簽[13]，將目標(biāo)的惟一標(biāo)志、屬性及其操作信息錄入到標(biāo)簽中，機(jī)器人通過(guò)讀取QR code標(biāo)簽來(lái)獲取目標(biāo)的相關(guān)信息。圖4給出了家庭環(huán)境中的幾種常見(jiàn)目標(biāo)及其標(biāo)簽。

3.2 家庭環(huán)境地圖的構(gòu)建

采用激光測(cè)距儀和CCD攝像機(jī)作為主要的外部傳感器，完成基于掃描匹配的環(huán)境特征提取;CCD攝像機(jī)用于識(shí)別目標(biāo)的QR code標(biāo)簽，并提取目標(biāo)的過(guò)完備特征集數(shù)據(jù)。針對(duì)圖3布局的家庭環(huán)境，通過(guò)遙操作方式控制機(jī)器人構(gòu)建出三種形式的家庭環(huán)境地圖，即基于傳統(tǒng)幾何—拓?fù)浔硎镜牡貓D、基于單純幾何特征表示的簡(jiǎn)單全息地圖和基于幾何—拓?fù)浔硎镜娜⒌貓D。圖5為三種地圖的俯視圖。由圖可見(jiàn)，傳統(tǒng)幾何—拓?fù)涞貓D由23個(gè)環(huán)境拓?fù)涔?jié)點(diǎn)和22條拓?fù)溥厴?gòu)成，為機(jī)器人建立了家庭環(huán)境可活動(dòng)空間的概念。單純幾何特征的簡(jiǎn)單全息地圖中給出了35個(gè)粘貼QR code標(biāo)簽的目標(biāo)及其在環(huán)境地圖中的位置，實(shí)現(xiàn)了機(jī)器人對(duì)家庭環(huán)境中各操作目標(biāo)位置的感知。幾何—拓?fù)浣Y(jié)構(gòu)的全息地圖中，全局層面上，拓?fù)涔?jié)點(diǎn)由23個(gè)環(huán)境節(jié)點(diǎn)和35個(gè)目標(biāo)節(jié)點(diǎn)構(gòu)成，拓?fù)溥呌?2條環(huán)境—環(huán)境拓?fù)溥吅?4條目標(biāo)—環(huán)境拓?fù)溥厴?gòu)成。由于餐廳墻角處的椅子在機(jī)器人操作范圍之外，故沒(méi)有建立其與環(huán)境節(jié)點(diǎn)的拓?fù)溥叀?/p>

3.3 性能分析

為了驗(yàn)證全息地圖的實(shí)用性和構(gòu)建方法的有效性，分別從地圖創(chuàng)建用時(shí)、基于地圖的機(jī)器人路徑規(guī)劃用時(shí)和任務(wù)執(zhí)行效率等角度進(jìn)行分析和評(píng)價(jià)。

如表1所示，從機(jī)器人地圖創(chuàng)建的耗時(shí)來(lái)看，利用SLAM技術(shù)機(jī)器人可在線構(gòu)建地圖:基于單純幾何特征表示的簡(jiǎn)單全息地圖構(gòu)建耗時(shí)主要取決于機(jī)器人的移動(dòng)速度和目標(biāo)發(fā)現(xiàn)與定位的耗時(shí);在機(jī)器人運(yùn)動(dòng)速度相同的情況下，由于從環(huán)境中提取拓?fù)涔?jié)點(diǎn)并構(gòu)建GVG拓?fù)涞貓D的計(jì)算量較大，傳統(tǒng)幾何—拓?fù)涞貓D和基于幾何—拓?fù)浠旌夏Ｐ偷娜⒌貓D構(gòu)建需要較長(zhǎng)的耗時(shí);此外，由于需要發(fā)現(xiàn)和定位環(huán)境中的目標(biāo)，基于幾何—拓?fù)浠旌夏Ｐ偷娜⒌貓D耗時(shí)更多。

從基于地圖的機(jī)器人路徑規(guī)劃效率角度來(lái)看，基于傳統(tǒng)幾何—拓?fù)涞貓D表示，機(jī)器人能夠從當(dāng)前位置無(wú)碰撞地運(yùn)動(dòng)到人為指定的操作目標(biāo)位置，或是通過(guò)特定的搜索策略遍歷整個(gè)地圖直至運(yùn)動(dòng)到環(huán)境中的特定目標(biāo)位置，可見(jiàn)，基于該地圖的機(jī)器人路徑規(guī)劃是被動(dòng)的?；趲缀翁卣鞯暮?jiǎn)單全息地圖，機(jī)器人能夠感知各操作目標(biāo)的位置，繼而完成由當(dāng)前位置到目標(biāo)位置的主動(dòng)路徑規(guī)劃，但由于路徑規(guī)劃的計(jì)算量巨大(尤其是對(duì)于三維地圖)，基于該地圖的路徑規(guī)劃速度較慢，難以滿足機(jī)器人工作的實(shí)時(shí)性要求。幾何—拓?fù)浠旌媳硎镜娜⒌貓D通

過(guò)拓?fù)鋵用嫔系娜致窂揭?guī)劃，快速生成從當(dāng)前位置到目標(biāo)位置的最優(yōu)路徑，在機(jī)器人導(dǎo)航過(guò)程中，不斷通過(guò)局部幾何地圖輔助機(jī)器人位姿矯正，確保機(jī)器人在非節(jié)點(diǎn)位置處仍能成功定位。

從基于地圖的機(jī)器人任務(wù)執(zhí)行效率角度來(lái)看，表1中列出了分別基于三種形式的環(huán)境地圖，機(jī)器人從廚房房門(mén)處出發(fā)，發(fā)現(xiàn)并抓取陽(yáng)臺(tái)處水杯的任務(wù)T的執(zhí)行時(shí)間(基于傳統(tǒng)幾何—拓?fù)涞貓D的機(jī)器人采用深度優(yōu)先搜索策略)，可見(jiàn)，基于幾何特征表示和基于幾何—拓?fù)浔硎镜膬煞N全息地圖機(jī)器人執(zhí)行效率具有明顯優(yōu)勢(shì)。

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

本文針對(duì)家庭服務(wù)機(jī)器人工作環(huán)境的復(fù)雜性，提出了融合環(huán)境特征和目標(biāo)信息的家庭全息地圖，其優(yōu)勢(shì)在于:

a)全息地圖將環(huán)境和目標(biāo)信息融合起來(lái)，并通過(guò)QR code標(biāo)簽實(shí)現(xiàn)環(huán)境和目標(biāo)的通用和準(zhǔn)確識(shí)別，深化了家庭服務(wù)機(jī)器人對(duì)其工作環(huán)境和操作目標(biāo)的認(rèn)知，是提高機(jī)器人服務(wù)效率和智能程度的有效途徑。

b)采用局部幾何—全局拓?fù)涞姆謱踊旌辖Y(jié)構(gòu)表示家庭環(huán)境。實(shí)驗(yàn)表明該表示方法具有較高的機(jī)器人路徑規(guī)劃和任務(wù)執(zhí)行效率。

本文對(duì)全息地圖的表示和構(gòu)建進(jìn)行了初步探討。機(jī)器人自身感知范圍有限，通過(guò)機(jī)器人巡游的策略發(fā)現(xiàn)環(huán)境及目標(biāo)變更的方法，無(wú)疑會(huì)消耗機(jī)器人的能量和服務(wù)時(shí)間。智能空間技術(shù)將物理世界與信息空間融合起來(lái)，力求實(shí)現(xiàn)家庭環(huán)境的全程動(dòng)態(tài)監(jiān)控，并通過(guò)推理機(jī)制預(yù)測(cè)環(huán)境變化以滿足各種特定任務(wù)的需要[14]。在智能空間指導(dǎo)下，機(jī)器人能夠及時(shí)有效地發(fā)現(xiàn)環(huán)境和目標(biāo)的變動(dòng)。因此，與智能空間交互的家庭服務(wù)機(jī)器人全息地圖創(chuàng)建與更新研究，將是本文進(jìn)一步的研究方向。

參考文獻(xiàn):

[1]

THRUN S. Simultaneous localization and mapping[J]. Springer Tracts in Advanced Robotics and Cognitive Approaches to Spatial Mapping， 2008， 38:13- 41.

[2]LEONARD J J， DURRANTWHYTE H F， COX I J. Dynamic map building for an autonomous mobile robot[J]. International Journal of Robotics Research， 1992， 11(4): 286-298.

[3]RODRIGUEZ L D， MATIA F， GALAN R. Building geometric feature based maps for indoor service robots[J]. Robotics and Autonomous Systems， 2006， 54(7):546- 558.

[4]石朝俠， 洪炳镕， 周彤， 等. 大規(guī)模環(huán)境下的拓?fù)涞貓D創(chuàng)建與導(dǎo)航[J]. 機(jī)器人，2007，29(5):433-438.

[5]TOMATIS N， NOURBAKHSH I， SIEGWART R. Hybrid simultaneous localization and map building: a natural integration of topological and metric[J]. Robotics and Autonomous System， 2003， 44(1):3-14.

[6]王珂， 王偉， 莊嚴(yán)， 等. 基于幾何—拓?fù)鋸V域三維地圖和全向視覺(jué)的移動(dòng)機(jī)器人自定位[J]. 自動(dòng)化學(xué)報(bào)， 2008， 34(11):1369-1378.

[7]WU Pei-liang， KONG Ling-fu， LI Xian-shan， et al. A hybrid algorithm combined color feature and keypoints for object detection[C]//Proc of the 3rd IEEE Conference on Industrial Electronics and Applications. 2008: 1408-1412.

[8]LOWE D G. Distinctive image features from scaleinvariant keypoints[J]. International Journal of Computer Vision， 2004， 60(2): 91-110.

[9]楊廣林， 孔令富， 王潔. 一種新的機(jī)器人手眼關(guān)系標(biāo)定方法[J]. 機(jī)器人， 2006， 28(4): 400-405.

[10]莊嚴(yán)， 王偉， 王珂， 等. 移動(dòng)機(jī)器人基于激光測(cè)距和單目視覺(jué)的室內(nèi)同時(shí)定位和地圖構(gòu)建[J]. 自動(dòng)化學(xué)報(bào)， 2005， 31(6):925-933.

[11]LI Mao-hai， HONG Bing-rong， LUO Rong-hua. Mobile robot hierarchical simultaneous localization and mapping using monocular vision[J]. Journal of Shanghai Jiaotong University， 2007， 12(6):765-772.

[12]RANGANATHAN P， HAYET J B， DEVY M， et al. Topological navigation and qualitative localization for indoor environment using multisensory perception[J]. Robotics and Autonomous System， 2002， 41(2-3): 137-144.

[13]穴洪濤，田國(guó)會(huì)，李曉磊，等. QR code在多種類(lèi)物體識(shí)別與操作中的應(yīng)用[J].山東大學(xué)學(xué)報(bào)， 2007， 37(6):25-30.

[14]HA Y G， SOHN J C， CHO Y J， et al. A robotic service framework supporting automated integration of ubiquitous sensors and devices[J]. Information Sciences， 2007， 177(3):657-679.