桑森，田國(guó)會(huì)，段朋，吳皓

山東大學(xué)控制科學(xué)與工程學(xué)院，濟(jì)南 250061

智能空間下基于異質(zhì)傳感信息融合的目標(biāo)跟蹤

桑森，田國(guó)會(huì)，段朋，吳皓

山東大學(xué)控制科學(xué)與工程學(xué)院，濟(jì)南 250061

針對(duì)大范圍未知環(huán)境下的機(jī)器人目標(biāo)跟蹤問(wèn)題，在智能空間下分布式智能網(wǎng)絡(luò)設(shè)備與機(jī)器人本體二維激光的基礎(chǔ)上，提出了一種基于異質(zhì)信息融合結(jié)構(gòu)以實(shí)現(xiàn)機(jī)器人對(duì)目標(biāo)實(shí)時(shí)檢測(cè)和跟蹤的方法。系統(tǒng)通過(guò)顏色信息進(jìn)行移動(dòng)目標(biāo)匹配，根據(jù)三角測(cè)量原理基于最小二乘法對(duì)目標(biāo)進(jìn)行三維重建；檢測(cè)出運(yùn)動(dòng)目標(biāo)后，通過(guò)激光傳感器掃描目標(biāo)人腿進(jìn)行近鄰點(diǎn)聚類(lèi)獲得目標(biāo)的準(zhǔn)確深度信息；利用一種優(yōu)化的迭代擴(kuò)展卡爾曼濾波算法對(duì)異質(zhì)傳感器進(jìn)行信息融合，以實(shí)現(xiàn)基于智能空間的機(jī)器人定位與目標(biāo)跟蹤。實(shí)驗(yàn)結(jié)果驗(yàn)證了方法的有效性。

目標(biāo)跟蹤；分布式智能網(wǎng)絡(luò)設(shè)備；擴(kuò)展卡爾曼濾波；異質(zhì)傳感器信息融合

1 引言

隨著計(jì)算機(jī)、通訊技術(shù)的發(fā)展，具有人機(jī)交互能力的服務(wù)機(jī)器人已經(jīng)逐漸走入家庭、辦公室等場(chǎng)所，在助老助殘等領(lǐng)域發(fā)揮著重要的作用；而為實(shí)現(xiàn)機(jī)器人與人的積極友好互動(dòng)[1]，服務(wù)機(jī)器人的運(yùn)動(dòng)目標(biāo)檢測(cè)與跟蹤成為機(jī)器人的一項(xiàng)基本和重要功能。在過(guò)去的二十年中，很多學(xué)者對(duì)運(yùn)動(dòng)目標(biāo)的檢測(cè)與跟蹤進(jìn)行了大量深入的研究，提出了很多行之有效的方法；如基于膚色識(shí)別、基于運(yùn)動(dòng)模型和機(jī)器學(xué)習(xí)[2-3]等方法，而這些方法大多數(shù)使用單一的視覺(jué)傳感器，在進(jìn)行目標(biāo)跟蹤時(shí)缺少準(zhǔn)確的距離信息；為獲得機(jī)器人與目標(biāo)人的距離信息，Kazuyuki和Shintaro等人[4]用激光傳感器進(jìn)行人體輪廓檢測(cè)以得到高精度的距離測(cè)量，但此方法識(shí)別目標(biāo)的能力很差，當(dāng)有多移動(dòng)物體或障礙的情況下，容易造成跟蹤失誤；為此Wen Dai和A.Scheidig等人[5]用協(xié)方差交叉算法融合多個(gè)具有互補(bǔ)特性的傳感器信息，如機(jī)器人本體攜帶的視覺(jué)傳感器，激光傳感器，超聲波傳感器等，以增強(qiáng)對(duì)人體檢測(cè)和跟蹤的準(zhǔn)確性，彌補(bǔ)傳感器彼此的薄弱點(diǎn)，但人的垂直運(yùn)動(dòng)在研究中并沒(méi)有得到顯示，而且當(dāng)跟蹤丟失時(shí)，難以搜尋大范圍未知環(huán)境下目標(biāo)人的當(dāng)前位置，導(dǎo)致實(shí)時(shí)性降低。

受機(jī)器人感知環(huán)境信息的約束，傳統(tǒng)意義下，機(jī)器人本體傳感器的感知范圍相對(duì)狹窄，所能獲得的服務(wù)對(duì)象的信息十分有限，難以高質(zhì)量地完成對(duì)目標(biāo)的檢測(cè)與跟蹤；近年來(lái)，智能空間技術(shù)的應(yīng)用使服務(wù)機(jī)器人的研究進(jìn)入一個(gè)新的水平并取得了很多研究成果[6-7]。在智能空間技術(shù)的支持下，借助于機(jī)器人與智能空間的信息交互，為機(jī)器人服務(wù)的智能性和服務(wù)質(zhì)量的提高提供了解決途徑。

為解決上述問(wèn)題，以山東大學(xué)服務(wù)機(jī)器人實(shí)驗(yàn)室為背景，設(shè)計(jì)了基于四臺(tái)頂棚攝像機(jī)的全局視覺(jué)系統(tǒng)，建立了面向服務(wù)機(jī)器人的大小為7.35 m×15.3 m智能空間；通過(guò)多攝像機(jī)目標(biāo)交接，實(shí)現(xiàn)各視角跟蹤任務(wù)的分配，盡量得到目標(biāo)清晰的無(wú)遮擋圖像以確定全局環(huán)境下目標(biāo)人的位置，彌補(bǔ)了機(jī)器人感知范圍相對(duì)狹窄的缺陷，并與機(jī)器人進(jìn)行通信，使機(jī)器人準(zhǔn)確地搜尋到目標(biāo)人所在的位置，然后通過(guò)機(jī)器人本體激光傳感器掃描目標(biāo)人腿進(jìn)行近鄰點(diǎn)聚類(lèi)以獲得準(zhǔn)確的深度信息，最后將兩種傳感器的測(cè)量信息在智能空間服務(wù)器上用優(yōu)化的迭代擴(kuò)展卡爾曼濾波算法進(jìn)行異質(zhì)傳感器的信息融合，以得到目標(biāo)人準(zhǔn)確位姿估計(jì)，實(shí)現(xiàn)了更準(zhǔn)確的人機(jī)同步定位和目標(biāo)跟蹤。實(shí)驗(yàn)利用自行研制的家庭服務(wù)機(jī)器人驗(yàn)證了所提方法的有效性。

2 智能空間下基于雙目立體視覺(jué)的目標(biāo)跟蹤定位

當(dāng)對(duì)一個(gè)大的場(chǎng)景進(jìn)行監(jiān)控時(shí)，必須用到多個(gè)攝像機(jī)同時(shí)進(jìn)行監(jiān)控，并選擇最合適的兩臺(tái)攝像機(jī)對(duì)目標(biāo)進(jìn)行跟蹤定位，為盡可能得到清晰無(wú)遮擋圖形，先判斷是否被遮擋，選擇目標(biāo)未被遮擋的兩臺(tái)攝像機(jī)；若滿(mǎn)足條件的攝像機(jī)不止兩個(gè)，選擇離目標(biāo)近的兩臺(tái)攝像機(jī)組成雙目立體視覺(jué)，從兩個(gè)不同的位置和角度同時(shí)獲取同一個(gè)目標(biāo)的兩幅圖像，然后通過(guò)計(jì)算目標(biāo)在兩幅圖像中的視差，利用三角測(cè)量原理來(lái)得到目標(biāo)的三維信息[8]?；跀z像機(jī)的擺放位置不同，雙目立體視覺(jué)分為平視和任意放置的立體模型，如果要使用平視的雙目立體視覺(jué)，則要經(jīng)過(guò)準(zhǔn)確的校準(zhǔn)，在目前的實(shí)驗(yàn)室環(huán)境下難以達(dá)到此要求，因此使用任意放置的立體模型。圖1是通過(guò)三維重建方法來(lái)獲取目標(biāo)三維坐標(biāo)信息的流程圖。

圖1 雙目立體視覺(jué)框圖

2.1 雙目立體視覺(jué)的目標(biāo)匹配

在智能空間下可能會(huì)存在多個(gè)移動(dòng)目標(biāo)，需要對(duì)兩個(gè)攝像機(jī)檢測(cè)到的移動(dòng)目標(biāo)區(qū)域進(jìn)行匹配，或者因?yàn)橐暯堑年P(guān)系，移動(dòng)目標(biāo)只出現(xiàn)在一個(gè)攝像機(jī)的視野范圍內(nèi)，而另一個(gè)攝像機(jī)觀察不到，由此需要在兩個(gè)視野范圍內(nèi)搜尋同一個(gè)移動(dòng)目標(biāo)區(qū)域，在此不需要進(jìn)行精確的點(diǎn)的匹配，只要確定兩個(gè)區(qū)域是同一個(gè)移動(dòng)目標(biāo)即可。

為了能夠增加匹配結(jié)果的準(zhǔn)確性以及加快匹配算法的速度，減少噪聲匹配的干擾，本研究在雙目立體視覺(jué)中加入極線(xiàn)約束關(guān)系，如圖2所示I1和I2為左右兩個(gè)成像平面，OL和OR分別為左右兩個(gè)攝像機(jī)的光心，射線(xiàn)OLPn上各空間點(diǎn)在I1面上的投影都為PL，而在右圖像上的投影點(diǎn)組成一條直線(xiàn)d2，由此可以確定I1成像面上的PL點(diǎn)在I2成像面上的對(duì)應(yīng)點(diǎn)肯定在d2上，d2就被稱(chēng)為PL在右圖像上的極線(xiàn)，與之對(duì)應(yīng)的d1被稱(chēng)為Pr在左圖像上的極線(xiàn)，因此對(duì)于任意一空間點(diǎn)P，如果已知其在一個(gè)成像平面上的投影點(diǎn)，就可以在它的極線(xiàn)上搜索它所對(duì)應(yīng)的點(diǎn)。

圖2 雙目立體視覺(jué)的極線(xiàn)幾何約束

對(duì)兩個(gè)攝像機(jī)的投影方程進(jìn)行整理：

其中，X表示世界坐標(biāo)系下的齊次坐標(biāo)值，u1和u2表示圖像坐標(biāo)系下的齊次坐標(biāo)值。將式（1）和（2）展開(kāi)：

x是空間點(diǎn)的世界坐標(biāo)，將式（3）、（4）消去x展開(kāi)得：

將式（5）展開(kāi)得到三個(gè)等式，消去z1、z2就可以得到u1和u2的關(guān)系式：

其中[M]x是反對(duì)稱(chēng)矩陣，根據(jù)式（6），如果已知u1，就可得到關(guān)于u2的線(xiàn)性方程，反知u2，可得u1線(xiàn)性方程。

綜上所述，基于顏色相似性度量[9]的匹配方法如下：

（1）使用標(biāo)定好的兩個(gè)攝像機(jī)對(duì)同一場(chǎng)景進(jìn)行拍攝，得到兩幅圖像I1和I2。

（2）對(duì)圖像I1(I2)進(jìn)行目標(biāo)檢測(cè)，根據(jù)要求選定參考目標(biāo)，建立目標(biāo)區(qū)域顏色模型，同時(shí)選定幾個(gè)特征點(diǎn)，建立其對(duì)應(yīng)的極線(xiàn)約束方程。

（3）對(duì)圖像I2(I1)進(jìn)行目標(biāo)檢測(cè)，如果某個(gè)運(yùn)動(dòng)區(qū)域與極線(xiàn)約束方程相交，則將該區(qū)域作為候選目標(biāo)區(qū)域，建立該區(qū)域的顏色模型。

（4）將參考目標(biāo)的顏色模型與候選目標(biāo)的顏色模型進(jìn)行相似性度量，如果相似性大于一定閾值則認(rèn)為是同一目標(biāo)，否則為不同目標(biāo)。

2.2 目標(biāo)三維重建

左右攝像機(jī)目標(biāo)匹配完成之后，利用最小二乘法就可進(jìn)行目標(biāo)空間位置的計(jì)算，假設(shè)空間一點(diǎn)p(x，y，z)在兩個(gè)成像平面上的圖像坐標(biāo)分別為p1(ul,vl)和p2(ur,vr)，兩個(gè)攝像機(jī)的投影矩陣分別為Ml和Mr，根據(jù)成像模型可得：

式中，Ml和Mr表示投影矩陣，將公式（1）、（2）分別消去Zl、Zr，得：

并將其化成矩陣的形式可得到：

利用最小二乘法，解得p(x，y，z)的值為：

即P(x,y,z)為目標(biāo)所求的三維坐標(biāo)信息。

3 機(jī)器人自定位

機(jī)器人在對(duì)目標(biāo)人搜尋之前，首先要獲得本身在世界坐標(biāo)系下的位置，機(jī)器人的定位精度對(duì)目標(biāo)人的尋找有很大的影響，而機(jī)器人本體的里程計(jì)定位精度很低，難以滿(mǎn)足機(jī)器人定位要求，因此，采用智能空間下StarGazer紅外定位系統(tǒng)并融合里程計(jì)來(lái)實(shí)現(xiàn)機(jī)器人的實(shí)時(shí)精確定位。圖3為StarGazer系統(tǒng)紅外發(fā)射/接收器與被動(dòng)式紅外標(biāo)簽。

圖3 StarGazer紅外定位系統(tǒng)

本文將4×4矩陣式標(biāo)簽布置在智能空間的天花板上，將發(fā)射/接收器安裝在機(jī)器人上方的平臺(tái)上，當(dāng)發(fā)射/接收器檢測(cè)到標(biāo)簽時(shí)就通過(guò)串口將其相對(duì)于標(biāo)簽的位姿（包括X、Y軸分量和相對(duì)角度信息）傳遞給機(jī)器人，由于標(biāo)簽的ID號(hào)唯一，且標(biāo)簽在世界坐標(biāo)系中的位置己知，通過(guò)坐標(biāo)變換機(jī)器人就可獲取自身在世界坐標(biāo)系中的位姿，實(shí)現(xiàn)自定位，然后機(jī)器人通過(guò)A*算法[10]進(jìn)行路徑規(guī)劃找到目標(biāo)人當(dāng)前的位置；StarGazer系統(tǒng)的定位精度很高，誤差在2 cm以?xún)?nèi)，但存在數(shù)據(jù)跳變，導(dǎo)致定位不穩(wěn)定，將里程計(jì)與StarGazer的數(shù)據(jù)進(jìn)行融合可解決數(shù)據(jù)跳變的問(wèn)題，提高機(jī)器人定位的穩(wěn)定性。

4 基于激光的人腿輪廓聚類(lèi)分析

首先智能空間服務(wù)器將分布式視覺(jué)得到的目標(biāo)的世界坐標(biāo)轉(zhuǎn)化到機(jī)器人本身地圖的坐標(biāo)系上，然后經(jīng)無(wú)線(xiàn)網(wǎng)絡(luò)與機(jī)器人本體通信，把目標(biāo)坐標(biāo)發(fā)送給機(jī)器人，機(jī)器人依坐標(biāo)信息可搜尋當(dāng)前目標(biāo)人所在的位置；然后機(jī)器人采用激光傳感器掃描人腿[11]通過(guò)近鄰點(diǎn)聚類(lèi)分析來(lái)確定目標(biāo)人。

通過(guò)統(tǒng)計(jì)實(shí)驗(yàn)提取腿部的基本特征，如人腿模型近似一對(duì)圓弧形狀，人腿的直徑約為90～150 mm，人在站立時(shí)兩腿之間的距離一般為10～40 mm左右，人在正常行走時(shí)左腿和右腿前后之間的距離在0～400 mm之間變動(dòng)，因此通過(guò)腿部的這些屬性可以很容易地從干擾物中辨識(shí)出人的左右腿，例如室內(nèi)桌椅腿的寬度大概為10 mm左右，垃圾桶輪廓與人腿輪廓相似，都為圓弧形，但垃圾桶圓弧長(zhǎng)遠(yuǎn)遠(yuǎn)大于人腿。檢測(cè)到的人被視為目標(biāo)進(jìn)行跟蹤，圖4是基于近鄰點(diǎn)聚類(lèi)通過(guò)激光掃描得到腿部邊緣信息，紅線(xiàn)表示通過(guò)點(diǎn)的聚類(lèi)得到的腿部輪廓，白色區(qū)域?yàn)槿送人诘奈恢茫虚g空心原點(diǎn)為兩腿的中心點(diǎn)。

圖4 基于近鄰點(diǎn)聚類(lèi)得到的腿部輪廓

當(dāng)機(jī)器人與人進(jìn)行人機(jī)交互時(shí)，兩者之間距離太近（0～0.4 m），會(huì)使人產(chǎn)生不安和厭惡感，距離太遠(yuǎn)（大于2 m），反而會(huì)丟失目標(biāo)，因此使機(jī)器人與目標(biāo)人之間的距離保持在1.3 m左右。圖5是激光檢測(cè)到人腿的坐標(biāo)圖，以?xún)赏鹊闹行狞c(diǎn)作為人的位置。

圖5 激光對(duì)人腿的檢測(cè)和位置計(jì)算

設(shè)人的中心點(diǎn)Xφ與機(jī)器人前進(jìn)方向夾角為θ，激光與中心點(diǎn)的距離為d，左腿X1和右腿X2與激光Ylaser的夾角分別為α和β，且與激光的距離分別為L(zhǎng)α和Lβ，由圖中三角關(guān)系可得：X1laser=lαsinα；Y1laser=lαcosα；X2laser=lβsinβ；Y2laser=lβcosβ，所以目標(biāo)人的中心點(diǎn)坐標(biāo)：

5 基于優(yōu)化迭代擴(kuò)展卡爾曼濾波算法的異質(zhì)傳感器信息融合

為保證機(jī)器人對(duì)目標(biāo)人實(shí)時(shí)的準(zhǔn)確定位與跟蹤，采用優(yōu)化的迭代擴(kuò)展卡爾曼濾波[12-13]將激光傳感器與分布式視覺(jué)得到的目標(biāo)人的位姿信息進(jìn)行融合；激光可以直接探測(cè)到機(jī)器人與目標(biāo)人的距離信息，所以對(duì)激光信息的預(yù)處理只需要進(jìn)行相應(yīng)的坐標(biāo)轉(zhuǎn)換；而視覺(jué)系統(tǒng)僅能提供圖像的灰度或彩色信息，這種信息不是距離信息，因而不能夠直接和激光的探測(cè)信息進(jìn)行融合，所以，在視覺(jué)信息的預(yù)處理中還需要通過(guò)特征匹配和三維重建的方法來(lái)獲取立體視覺(jué)中的深度信息，以更好地實(shí)現(xiàn)對(duì)異質(zhì)傳感器信息[14]的融合。圖6為傳感器信息處理與融合過(guò)程。

圖6 傳感器信息處理與融合過(guò)程

本文研究采用的系統(tǒng)運(yùn)動(dòng)及觀測(cè)模型是非線(xiàn)性的，為了減少線(xiàn)性化誤差，提高非線(xiàn)性濾波性能，將迭代濾波理論引入到擴(kuò)展卡爾曼濾波方法中，重復(fù)利用觀測(cè)信息，得到更高的估計(jì)精度。并利用Levenberg-M arquardt方法優(yōu)化迭代[15]過(guò)程，增加迭代算法的穩(wěn)定性，針對(duì)本文研究的非線(xiàn)性傳感器系統(tǒng)，建立離散時(shí)間系統(tǒng)模型f和非線(xiàn)性觀測(cè)模型h：

首先預(yù)測(cè)目標(biāo)人的坐標(biāo)，計(jì)算預(yù)測(cè)系統(tǒng)狀態(tài)及協(xié)方差：

計(jì)算預(yù)測(cè)系統(tǒng)狀態(tài)、觀測(cè)預(yù)測(cè)及協(xié)方差：

其中修正的協(xié)方差陣：

計(jì)算卡爾曼濾波增益矩陣：

更新協(xié)方差矩陣、系統(tǒng)狀態(tài)：

其中，I為單位矩陣，Levenberg-Marquardt參數(shù)ui設(shè)定為0.1，使用參數(shù)ui對(duì)預(yù)測(cè)協(xié)方差陣進(jìn)行修正，保證算法具有全局收斂性，準(zhǔn)確地估計(jì)目標(biāo)人在每一時(shí)刻的狀態(tài)。

6 實(shí)驗(yàn)結(jié)果與分析

6.1 實(shí)驗(yàn)環(huán)境設(shè)計(jì)

本文實(shí)驗(yàn)所用的分布式視覺(jué)由四臺(tái)TOSHIBA GiantDragon CSGV 90CC3組成，最高分辨率可達(dá)640× 480像素，最大傳輸速率為90 f/s；機(jī)器人以山東大學(xué)服務(wù)機(jī)器人實(shí)驗(yàn)室自主研發(fā)的病房巡視機(jī)器人作為研究平臺(tái)，上位機(jī)操作系統(tǒng)為W indow s XP系統(tǒng)，軟件開(kāi)發(fā)平臺(tái)應(yīng)用Microsoft Visual Studio 2008，本體攜帶幾種傳感器設(shè)備，如StarGazer、以及激光傳感器等。實(shí)驗(yàn)用的激光傳感器為HOKUYO公司產(chǎn)品UTM-30LX 2D型激光掃描測(cè)距儀，安裝在機(jī)器人距離地面30 cm處，角度分辨率為0.25°，具有高精度、高分辨率、寬視場(chǎng)等特點(diǎn)。

6.2 智能空間下的目標(biāo)檢測(cè)和定位

在智能空間下可能會(huì)存在多個(gè)移動(dòng)目標(biāo)，需要對(duì)兩個(gè)攝像機(jī)檢測(cè)到的移動(dòng)目標(biāo)區(qū)域進(jìn)行匹配，如果視野內(nèi)兩個(gè)候選目標(biāo)都與選定目標(biāo)的顏色相似，通過(guò)顏色信息就不能進(jìn)行區(qū)分，這時(shí)引入極線(xiàn)約束進(jìn)行判斷，如圖7所示在左圖像中選定目標(biāo)的頭頂點(diǎn)，在右圖像中做該點(diǎn)的極線(xiàn)約束方程，則該方程一定通過(guò)目標(biāo)點(diǎn)在右圖像中的投影點(diǎn)。提取右圖像中所有輪廓的最高點(diǎn)，離該極線(xiàn)方程最近的點(diǎn)所對(duì)應(yīng)的輪廓區(qū)域，就是所要確定的匹配區(qū)域。

圖7 存在干擾目標(biāo)時(shí)的目標(biāo)檢測(cè)

考慮到人在房間內(nèi)運(yùn)動(dòng)時(shí)，身體容易受到桌子、椅子等物體的遮擋，但頭部一般不會(huì)被遮擋，因此，在計(jì)算目標(biāo)的位置時(shí)，選擇頭頂作為特征點(diǎn)；目標(biāo)在空間中的運(yùn)動(dòng)是連續(xù)的，頭頂?shù)奈恢貌粫?huì)發(fā)生突然大距離的變化，如果兩個(gè)輪廓相交，此時(shí)目標(biāo)的頭頂位置可能會(huì)發(fā)生很大的變化，對(duì)此也進(jìn)行判斷，同時(shí)用跟蹤框的中心點(diǎn)代替頭頂點(diǎn)進(jìn)行計(jì)算。如圖8所示上方為左右攝像機(jī)拍攝的圖像，此時(shí)用藍(lán)色的跟蹤框來(lái)跟蹤目標(biāo)。左下方顯示室內(nèi)的實(shí)驗(yàn)環(huán)境地圖，在地圖上用紅色線(xiàn)來(lái)描述目標(biāo)的移動(dòng)軌跡，頭頂位置用紅色圓圈表示，右下方為獲得人的定位坐標(biāo)，此刻定位的坐標(biāo)為（537 cm，156 cm）。

圖8 分布視覺(jué)的定位界面

其次，為了證明優(yōu)化迭代的擴(kuò)展卡爾曼濾波融合算法在目標(biāo)人定位中的準(zhǔn)確性，設(shè)計(jì)了一組實(shí)驗(yàn)，以坐標(biāo)點(diǎn)（50 cm，460 cm）為起始點(diǎn)，（450 cm，460 cm）為終止點(diǎn)畫(huà)一條直線(xiàn)，作為目標(biāo)人的行走軌跡；行人的速度范圍控制在0.3 m/s到1.0 m/s，首先用分布式視覺(jué)單獨(dú)對(duì)目標(biāo)人進(jìn)行全局定位，獲得目標(biāo)人在行走過(guò)程中的全局世界坐標(biāo)。然后用全局視覺(jué)與激光結(jié)合的方法進(jìn)行目標(biāo)人的定位與跟蹤，用EKF估計(jì)得到目標(biāo)人的全局世界坐標(biāo)。圖9為設(shè)計(jì)實(shí)驗(yàn)圖。

圖9 目標(biāo)人定位實(shí)驗(yàn)圖

實(shí)驗(yàn)數(shù)據(jù)如圖10所示，采用分布式視覺(jué)進(jìn)行機(jī)器人的全局定位雖然精度不低，但曲線(xiàn)振蕩幅度比較大，誤差在20 cm以?xún)?nèi)。而采用優(yōu)化迭代的擴(kuò)展卡爾曼濾波融合算法將兩種傳感器信息融合的定位方式，全局定位曲線(xiàn)更接近期望的路徑，而且保持了較好的連續(xù)性，變化幅度也較小，誤差基本保持在10 cm以?xún)?nèi)。

6.3 同時(shí)機(jī)器人定位與目標(biāo)跟蹤

為了證明在智能空間下本文所提出方法的可行性，實(shí)驗(yàn)中人沿著所設(shè)定的4.5 m×8 m的方框（黃線(xiàn)）路徑行走，起點(diǎn)即為終點(diǎn)，實(shí)驗(yàn)之前首先將視覺(jué)與激光的觀測(cè)數(shù)據(jù)時(shí)間經(jīng)時(shí)間配準(zhǔn)[16]，考慮到網(wǎng)絡(luò)傳輸時(shí)間和數(shù)據(jù)處理時(shí)間的延遲，采樣時(shí)間為160 ms，為了顯示融合算法的精確性，在智能空間服務(wù)器地圖中將視覺(jué)得到的人行走路徑用紅線(xiàn)畫(huà)出，而經(jīng)優(yōu)化迭代的EKF數(shù)據(jù)融合得到的人行走軌跡用黑線(xiàn)畫(huà)出，機(jī)器人跟蹤人得到的軌跡用綠線(xiàn)標(biāo)明。由圖11可以看出融合之后人的軌跡更接近所設(shè)定的軌跡，機(jī)器人相對(duì)于行人的運(yùn)行速度較慢，跟隨在行人后面，軌跡顯示該方法能精確地實(shí)現(xiàn)對(duì)目標(biāo)人的跟蹤，并計(jì)算目標(biāo)人在不同時(shí)間段采用不同方式定位的平均誤差，結(jié)果如表1顯示，采用EKF融合算法得到的定位精度明顯優(yōu)于單一傳感器的定位精度。

圖11 機(jī)器人跟蹤目標(biāo)人的軌跡

表1 目標(biāo)人在不同時(shí)間段的定位平均誤差cm

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

在智能空間下提出了用家庭陪護(hù)機(jī)器人去實(shí)時(shí)和準(zhǔn)確地跟蹤目標(biāo)人的方法，并且用實(shí)驗(yàn)證實(shí)了該方法的可行性。全局環(huán)境下借助智能空間去識(shí)別和跟蹤機(jī)器人視野之外的目標(biāo)人，擴(kuò)大了機(jī)器人跟蹤目標(biāo)的范圍，同時(shí)機(jī)器人本體激光能準(zhǔn)確地計(jì)算機(jī)器人與目標(biāo)的距離，用優(yōu)化迭代的EKF融合異質(zhì)傳感器的信息以彌補(bǔ)每個(gè)傳感器的測(cè)量弱點(diǎn)，得到目標(biāo)人的準(zhǔn)確位姿和速度信息，提高機(jī)器人跟蹤目標(biāo)的可靠性。在接下來(lái)的工作中，將著重研究機(jī)器人與人怎么保持一個(gè)靈活的距離，如機(jī)器人的任務(wù)是搬運(yùn)行李，應(yīng)保持與人一定距離。如果需要機(jī)器人和人類(lèi)之間的密切互動(dòng)，機(jī)器人應(yīng)接近人類(lèi)，讓人類(lèi)可以觸摸到機(jī)器人，以更好地實(shí)現(xiàn)人性化導(dǎo)航。

[1]譚民，王碩.機(jī)器人技術(shù)研究進(jìn)展[J].自動(dòng)化學(xué)報(bào)，2013，39（7）：963-972.

[2]Ercan A O，Gamal A E，Guibas L J.Object tracking in the presence of occlusions using multiple cameras：a sensor network approach[J].ACM Transactions on Sensor Networks（TOSN），2013，9（2）.

[3]Vadakkepat，Prahlad.Multimodal approach to human-face detection and tracking[C]//IEEE Transactions on Industrial Electronics，2008：1385-1393.

[4]Morioka，Kazuyuki.Human tracking based on integration of laser range scanners and cameras in intelligent space[C]// 2011 8th Asian on Control Conference（ASCC）.[S.l.]：IEEE，2011.

[5]Dai Wen，Cuhadar A，Liu P X.Robot tracking using vision and laser sensors[C]//IEEE International Conference on Automation Science and Engineering.[S.l.]：IEEE，2008.

[6]Rampinelli，Mariana.Use of computer vision for localization of a robotic wheelchair in an intelligent space[C]// Biosignals and Biorobotics Conference（BRC）.[S.l.]：IEEE，2013.

[7]李國(guó)棟，田國(guó)會(huì)，周鳳余.服務(wù)機(jī)器人智能空間中頂棚投影器建模與外參數(shù)標(biāo)定研究[J].控制與決策，2014.

[8]曹焱.基于雙目立體視覺(jué)信息的三維重建方法研究[D].長(zhǎng)春：吉林大學(xué)，2013.

[9]Andreas J，Koch R.Direct model-based tracking of 3d object deformations in depth and color video[J].International Journal of Computer Vision，2013，102（1/3）：239-255.

[10]張紫輝，熊岳山.未知環(huán)境下基于A*的機(jī)器人路徑規(guī)劃算法[J].計(jì)算機(jī)工程與科學(xué)，2013，34（11）：141-147.

[11]Hoyeon K，Chung Woojin，Yoo Yoonkyu.Detection and tracking of human legs for a mobile service robot[C]// 2010 IEEE/ASME International Conference on Advanced Intelligent Mechatronics（AIM）.[S.l.]：IEEE，2010.

[12]Mohammad A，Park Sang-Young.Simultaneous spacecraft attitude and orbit estimation using magnetic field vector measurements[J].Aerospace Science and Technology，2011，15（8）：653-669.

[13]楊宏，李亞安，李國(guó)輝.一種改進(jìn)擴(kuò)展卡爾曼濾波新方法[J].計(jì)算機(jī)工程與應(yīng)用，2010，46（19）：18-20.

[14]Bruno M，Delahoche L.An evidential approach for detection of abnormal behaviour in the presence of unreliable sensors[J].Information Fusion，2012，13（2）：146-160.

[15]Fang Huazhen，Wang Yebin.State of charge estimation for lithium-ion batteries：An adaptive approach[J].Control Engineering Practice，2014，25：45-54.

[16]彭焱，徐敏毓，金宏斌.多傳感器數(shù)據(jù)融合系統(tǒng)中時(shí)間配準(zhǔn)算法分析[J].雷達(dá)與對(duì)抗，2005，2（2）：16-19.

SANG Sen,TIAN Guohui,DUAN Peng,WU Hao

School of Control Science and Engineering,Shandong University,Jinan 250061,China

For a wide range of detection and tracking of target for a mobile service robot problem under the unknown environment,this article puts forward a method based on the heterogeneous information fusion structure to realize the target detection and tracking in real-time of robots,whose basis is distributed intelligent network devices and the robot’s 2D laser range finder in the intelligence space.The system matches the moving target by the information on color,and carries out the 3D reconstruction of the target depending on the triangulation technique and least square method.The moment it detects the moving target,in order to get the exact distance information,it w ill scan the target human’s leg by the laser range finder to cluster the nearest neighbor.Meanwhile,this article raises a better Iterative Extend Kalman Filter for the heterogeneous sensor information fusion to realize the simultaneous robot localization and target tracking based on the intelligence space.The experimental results verify the effectiveness of the proposed method.

target tracking;distributed intelligent network devices;extend Kalman filter;heterogeneous sensor information fusion

A

TP242.3

10.3778/j.issn.1002-8331.1402-0261

SANG Sen,TIAN Guohui,DUAN Peng,et al.Target track ing based on heterogeneous sensor in formation fusion in intelligent space.Computer Engineering and Applications,2014,50（16）：48-53.

國(guó)家高技術(shù)研究發(fā)展計(jì)劃（863）（No.2009AA 04Z220）；山東省自然科學(xué)基金資助課題（No.ZR2011FM 011）。

桑森（1987—），男，碩士研究生，研究領(lǐng)域?yàn)闄C(jī)器人，多傳感器信息融合；田國(guó)會(huì)（1969—），男，工學(xué)博士，博士后，教授、博士生導(dǎo)師，研究領(lǐng)域?yàn)椋悍?wù)機(jī)器人，智能空間，多機(jī)器人系統(tǒng)的協(xié)調(diào)與協(xié)作。E-mail：g.h.tian@sdu.edu.cn

2014-02-24

2014-03-21

1002-8331（2014）16-0048-06