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

桑森，田國會，段朋，吳皓

山東大學控制科學與工程學院，濟南 250061

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

桑森，田國會，段朋，吳皓

山東大學控制科學與工程學院，濟南 250061

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

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

1 引言

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

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

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

2 智能空間下基于雙目立體視覺的目標跟蹤定位

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

圖1 雙目立體視覺框圖

2.1 雙目立體視覺的目標匹配

在智能空間下可能會存在多個移動目標，需要對兩個攝像機檢測到的移動目標區(qū)域進行匹配，或者因為視角的關系，移動目標只出現(xiàn)在一個攝像機的視野范圍內(nèi)，而另一個攝像機觀察不到，由此需要在兩個視野范圍內(nèi)搜尋同一個移動目標區(qū)域，在此不需要進行精確的點的匹配，只要確定兩個區(qū)域是同一個移動目標即可。

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

圖2 雙目立體視覺的極線幾何約束

對兩個攝像機的投影方程進行整理：

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

x是空間點的世界坐標，將式（3）、（4）消去x展開得：

將式（5）展開得到三個等式，消去z1、z2就可以得到u1和u2的關系式：

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

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

（1）使用標定好的兩個攝像機對同一場景進行拍攝，得到兩幅圖像I1和I2。

（2）對圖像I1(I2)進行目標檢測，根據(jù)要求選定參考目標，建立目標區(qū)域顏色模型，同時選定幾個特征點，建立其對應的極線約束方程。

（3）對圖像I2(I1)進行目標檢測，如果某個運動區(qū)域與極線約束方程相交，則將該區(qū)域作為候選目標區(qū)域，建立該區(qū)域的顏色模型。

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

2.2 目標三維重建

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

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

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

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

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

3 機器人自定位

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

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

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

4 基于激光的人腿輪廓聚類分析

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

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

圖4 基于近鄰點聚類得到的腿部輪廓

當機器人與人進行人機交互時，兩者之間距離太近（0～0.4 m），會使人產(chǎn)生不安和厭惡感，距離太遠（大于2 m），反而會丟失目標，因此使機器人與目標人之間的距離保持在1.3 m左右。圖5是激光檢測到人腿的坐標圖，以兩腿的中心點作為人的位置。

圖5 激光對人腿的檢測和位置計算

設人的中心點Xφ與機器人前進方向夾角為θ，激光與中心點的距離為d，左腿X1和右腿X2與激光Ylaser的夾角分別為α和β，且與激光的距離分別為Lα和Lβ，由圖中三角關系可得：X1laser=lαsinα；Y1laser=lαcosα；X2laser=lβsinβ；Y2laser=lβcosβ，所以目標人的中心點坐標：

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

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

圖6 傳感器信息處理與融合過程

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

首先預測目標人的坐標，計算預測系統(tǒng)狀態(tài)及協(xié)方差：

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

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

計算卡爾曼濾波增益矩陣：

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

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

6 實驗結(jié)果與分析

6.1 實驗環(huán)境設計

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

6.2 智能空間下的目標檢測和定位

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

圖7 存在干擾目標時的目標檢測

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

圖8 分布視覺的定位界面

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

圖9 目標人定位實驗圖

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

6.3 同時機器人定位與目標跟蹤

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

圖11 機器人跟蹤目標人的軌跡

表1 目標人在不同時間段的定位平均誤差cm

7 結(jié)束語

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

[1]譚民，王碩.機器人技術研究進展[J].自動化學報，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]李國棟，田國會，周鳳余.服務機器人智能空間中頂棚投影器建模與外參數(shù)標定研究[J].控制與決策，2014.

[8]曹焱.基于雙目立體視覺信息的三維重建方法研究[D].長春：吉林大學，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*的機器人路徑規(guī)劃算法[J].計算機工程與科學，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]楊宏，李亞安，李國輝.一種改進擴展卡爾曼濾波新方法[J].計算機工程與應用，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)中時間配準算法分析[J].雷達與對抗，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.

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

桑森（1987—），男，碩士研究生，研究領域為機器人，多傳感器信息融合；田國會（1969—），男，工學博士，博士后，教授、博士生導師，研究領域為：服務機器人，智能空間，多機器人系統(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