,, ,

(浙江工業(yè)大學(xué),信息工程學(xué)院,浙江 杭州 310023)

多機器人協(xié)同定位是利用系統(tǒng)中某些機器人的高精度定位,精確的導(dǎo)航能力,和引入相對觀測量(相對距離與相對方位),通過彼此之間的信息交互，使裝備低精度定位設(shè)備的機器人提高定位精度。當某機器人由于自身傳感器故障導(dǎo)致自身定位失效時,協(xié)同定位可以在一定程度上恢復(fù)其定位能力。在多機器人協(xié)同工作的實際場景中,考慮經(jīng)濟成本,不可能為每臺機器人都配備高精度的定位傳感器。通過給部分機器人安裝高精度傳感器,使用協(xié)同定位方法可以提高機器人平臺整體的定位精度。因此研究多機器人協(xié)同定位具有重要的理論價值與實際意義[1-2]。目前,對于多機器人協(xié)同定位技術(shù)研究常用的方法:1) 同構(gòu)分布式協(xié)同定位,這些方法使用的都是同構(gòu)系統(tǒng),要求是相同的傳感器,每個機器人各自處理自己所觀測的信息,這樣可以充分地利用各個機器人的觀測信息。提高整個平臺的精度。Roumeliotis等[3]提出了一種分布式的擴展卡爾曼濾波(EKF)形式[4-5],Hage等[6]提出的容錯協(xié)同定位也是使用擴展卡爾曼的形式,這些文獻都使用這種方法。但是這種方法需要實時更新每一個機器人的觀測信息,計算量增大,計算復(fù)雜度高。2) 異構(gòu)集中式協(xié)同定位,將所有機器人的信息同一集中處理,可以快速地處理信息,去除冗余的信息。如Wanasinghe等[7]提出的異構(gòu)機器人系統(tǒng)的分布式協(xié)同定位使用的是異構(gòu)集中式定位方法。這種方法雖然計算快,實現(xiàn)簡單，但是一旦系統(tǒng)崩潰,很難恢復(fù),魯棒性較差。所以針對這種問題,蔣榮欣等[8-9]使用多傳感器融合方式進行協(xié)同定位。利用多傳感器融合技術(shù)彌補由于單個傳感器問題導(dǎo)致的系統(tǒng)崩潰問題。傳統(tǒng)的融合方法如聯(lián)邦濾波的方式[10]信息分配系數(shù)是固定值,實際中各子系統(tǒng)的定位精度每時每刻都不同,導(dǎo)致整個系統(tǒng)的精度下降。分布式矩陣加權(quán)的方式[11]要計算各個子系統(tǒng)之間的互協(xié)方差,計算復(fù)雜,實時性較差。

針對上面研究存在的問題,提出了一種激光與RGB-D相機融合的多傳感器異構(gòu)定位系統(tǒng),克服單傳感器精度不高的缺點。首先建立移動機器人運動模型,再根據(jù)移動機器人配置的傳感器,結(jié)合不同傳感器的測量原理分別建立了RGB-D相機觀測模型與激光傳感器觀測模型,得到系統(tǒng)狀態(tài)觀測方程。由于移動機器人運動模型是高度非線性模型,而傳統(tǒng)的EKF線性化處理導(dǎo)致非線性誤差較大,采用無跡卡爾曼濾波器獲得局部估計值。為了處理多傳感器數(shù)據(jù),使用協(xié)方差交叉融合方法(CI)[12-15],CI融合不需要計算各子系統(tǒng)的互協(xié)方差,計算量小,魯棒性強,實時性高,克服了計算復(fù)雜的互協(xié)方差的缺點。最后搭建了實驗平臺并在該平臺上驗證了所提出算法的可行性與優(yōu)越性。

1 問題描述與建模

1.1 系統(tǒng)描述

所考慮的異構(gòu)機器人協(xié)作定位系統(tǒng)如圖1所示,其中的機器人i配備了高精度里程計,以及外部傳感器激光雷達和RGB-D相機。機器人j只配備低精度里程計。當機器人i與機器人j同時運動時,為了使低精度定位能力的機器人j提高定位精度,機器人i通過RGB-D相機、激光雷達獲取機器人j的位置信息,協(xié)同機器人j獲取更高精度的定位信息。針對該問題,提出了激光與視覺融合的異構(gòu)多機器人協(xié)作定位方法。

當前采樣時刻,機器人j利用自身的里程計信息與機器人運動模型估計出自身的位置信息。機器人i先利用RGB-D相機獲取機器人j與機器人i的相對距離、相對方位角,通過RGB-D觀測模型與機器人j的運動模型組成RGB-D局部估計器,得出第1組關(guān)于機器人j的位姿估計與估計方差,如圖1中灰色三角與虛線橢圓所示,再通過激光雷達獲取機器人j與機器人i的相對距離、相對方位角。通過激光觀察模型與機器人j的運動模型組成激光局部估計器,得出第2組關(guān)于機器人j的位姿估計與估計方差,如圖1中灰色三角與實線橢圓所示。由于噪聲的影響,兩組估計都不準確。為了獲得更高精度的位姿估計,將兩組位姿估計值進行二次融合,如圖1虛線三角所示,從而得到精度更高的機器人j的位姿估計。為了實現(xiàn)異構(gòu)多機器人協(xié)作定位方法需要建立機器人的運動模型,并且針對不同傳感器特性建立各自的觀測模型。

圖1 異構(gòu)多機器人協(xié)作定位框架圖Fig.1 Framework of cooperative localization for heterogeneous multi robots

1.2 機器人運動模型

考慮兩輪差速輪式移動機器人,假設(shè)機器人的質(zhì)地均勻,質(zhì)心為幾何中心點,且機器人運動過程中左右過程中左右兩輪與地面純滾動無滑動(圖2)。圖2中:R為車體運動的圓弧半徑;Δx,Δy,Δθ分別為單位時間Δt內(nèi)車體位置橫、縱坐標和方位角的變化量。當走直線或采樣時間足夠短時,里程計通過記錄左右輪的轉(zhuǎn)動里程,得到移動機器人的角速度與速度。

圖2 運動模型Fig.2 Motion model

定義移動機器人的位姿為Xk=[xk,yk,θk]T為k時刻移動機器人的橫坐標、縱坐標和航向角。如圖2所示,狀態(tài)方程[16]可描述為

X(k+1)=f(X(k),u(k))

(1)

(2)

式中:u(k)=[v(k),w(k)]T，其中v(k)=ΔL=(ΔSl+ΔSr)/2，w(k)=Δθ=(ΔSl-ΔSr)/d，而ΔSl,ΔSr分別為移動機器人在k時刻的左右輪里程計的變化值，d為2輪軸距。

2 協(xié)作定位方法

圖3 協(xié)同定位算法框架Fig.3 Framework of cooperative location algorithm

2.1 基于RGB-D相機的局部估計器1

2.1.1 RGB-D相機傳感器模型

使用RGB-D相機的紅外相機可以獲得場景的深度信息,從而求得空間中某點的坐標,如圖4所示。

圖4 RGB-D測距原理Fig.4 RGB-D camera measuring principle

以RGB-D紅外相機中心為坐標原點建立坐標系,取紅外相機指向紅外發(fā)射的方向為X軸,Z軸垂直于圖像平面,當物體恰好位于參考平面時,記錄下它的散斑圖案。當物體位于平面時,它的散斑圖像在X軸上產(chǎn)生一個視差,通過三角形相似原理可知表達式為

(3)

(4)

式中:Z2為物體在物理平面的深度值;b為基線長度;f為紅外相機的焦距;D為物體在物理平面的位移;d為散斑圖案在圖像平面的視差。由式(3,4)可得

(5)

式中:Z1,f,b通過校準得到。場景中某點的深度值和相機的焦距f共同決定了該點的成像度,可得出X和Y軸的坐標為

(6)

(7)

式中:x,y分別為該點在圖像坐標系上的坐標;x0,y0分別為圖像坐標系的原點坐標。

x0,y0可以通過上述原理求出(x0,y0是以RGB-D為原點的局部坐標),如圖5所示可得

(8)

(9)

所以局部估計器1在k+1時刻對機器人j的量測信息為

(10)

圖5 RGB-D觀測模型Fig.5 The observation model of RGB-D camera

2.1.2 RGB-D局部估計器設(shè)計

UKF濾波方法是一種基于UT(Unscented transform)變換的非線性濾波方法。UKF是一種遞歸式貝葉斯估計方法。相比于EKF,UKF不必計算Jacobi矩陣并且估計精度和收斂速度都得到了提高,也更容易實現(xiàn)。

算法1RGB-D相機局部估計器

步驟1狀態(tài)預(yù)測。其表達式為

(11)

Xj(k+1|k)=f(X(k),u(k))

(12)

(13)

(14)

步驟2量測更新。其表達式為

Zj(k+1|k)=H(Xj(k+1|k))

(15)

(16)

(17)

P1(k+1|k+1)=P1(k+1|k)-KPZZKT

(18)

(19)

P1(k+1|k+1)=P1(k+1|k+1)

(20)

(21)

(22)

(23)

根據(jù)RGB-D測距原理將k+1時刻機器人i的位置[x(k+1)i,y(k+1)i]T代入RGB-D觀測模型中得到k+1時刻的量測信息Z(k+1)=[ρij,βij]T。

2.2 基于激光傳感器的局部估計器2

2.2.1 激光傳感器模型

如圖6所示,由于激光傳感器坐標系的形式為極坐標系統(tǒng),采集的數(shù)據(jù)用T={ρi,i|i=1,2,…,n},其中ρi為激光測量的長度值,i為每束激光的數(shù)據(jù)集的下標。

圖6 激光測量模型Fig.6 Laser measurement model

(24)

2.2.2 激光局部估計器設(shè)計

算法2激光局部估計器

步驟1狀態(tài)預(yù)測。其表達式為

(25)

Xj(k+1|k)=f(X(k),u(k))

(26)

(27)

(28)

步驟2量測更新。其表達式為

Zj(k+1|k)=H(Xj(k+1|k))

(29)

(30)

(31)

(32)

P2(k+1|k+1)=P2(k+1|k)-KPZZKT

(33)

(34)

P1(k+1|k+1)=P1(k+1|k+1)

(35)

式中：Zj(k+1|k)=H(Xj(k+1|k))因傳感器的測量原理不同,計算的方法也不同。激光觀測模型的量測信息通過下面方式進行更新:

1) 在k+1時刻得到機器人i的位置[x(k+1)i,y(k+1)i]T。

5) 得到[ρ(k+1)ij,β(k+1)ij]T,其中ρ(k+1)ij=ρcenter,β(k+1)ij=icenterΔθ-π/2。

6) 將所得到的量測信息Z(k+1)=[ρij,βij]T以及將k+1時刻機器人i的位置信息[x(k+1)i,y(k+1)i]T代入激光觀測模中。

2.3 全局融合算法

得到兩個局部估計之后,將兩個局部估計值進一步融合。由于受到共同的過程噪聲影響,兩個局部估計結(jié)果是相關(guān)的,而各局部估計間的互協(xié)方差計算復(fù)雜,考慮到移動機器人的實時性要求高的特點,采用CI融合算法得到更精確的估計?；诩す鈧鞲衅髋cRGB-D相機的全局估計器設(shè)計算法為

PCI(k+1|k+1)=[ωP1(k+1|k+1)-1+
(1-ω)P2(k+1|k+1)-1]-1

(35)

xCI(k+1|k+1)=PCI(k+1|k+1)[ωP1(k+1|k+
1)-1×x1(k+1|k+1)+(1-ω)P2(k+1|k+
1)-1x2(k+1|k+1)]

(36)

式中:x1(k+1|k+1),P1(k+1|k+1)分別通過RGB-D相機局部估計器算法得到;x2(k+1|k+1),P2(k+1|k+1)分別通過激光局部估計器算法得到。使用黃金分割法[17]等方法快速求最小值點ω=0.542 5。得到機器人j最優(yōu)估計位姿為xCI(k+1|k+1),與估計的方差陣PCI(k+1|k+1)。

3 實 驗

3.1 實驗硬件與環(huán)境

本實驗使用的是兩臺DFROBOT，如圖7所示.2臺DFROBT都裝備里程計,其中一臺DFROBOT為機器人i裝備RGB-D相機;型號為華碩Xtion-pro live,裝備型號為HOKUYO URG-04LX-UG01的激光雷達;使用的系統(tǒng)環(huán)境是ubuntu 14.04,采用indigo版本的ROS系統(tǒng)[18-19],opencv 3.0,深度相機的驅(qū)動庫openNI。為了能夠有良好的人機交互,將ROS與QT5.7結(jié)合使用,編寫上位機程序,直觀的實時體現(xiàn)機器人的位姿信息。

圖7 DFROBOT機器人Fig.7 DFROBOT

首先建立1個名為/Robot的ROS節(jié)點(用于接受ROS發(fā)布的其他節(jié)點信息),利用ROS訂閱/M1Speed的左輪線速度、/M2Speed右輪線速度的節(jié)點信息。然后為了方便識別目標機器人j,在機器人j尾部安置1個綠色矩形標志物。通過圖像處理檢測出綠色標志物(圖8),提取RGB-D量測信息。接著訂閱激光節(jié)點/hokuyo_node的/scan話題,提取激光量測信息。最后使用所提方法估計出機器人j的位姿,并在上位機上實時顯示。

圖8 標志物檢測與選定Fig.8 Detection and selection of signs

3.2 實驗場景與結(jié)果

實驗選擇為空曠的室內(nèi)環(huán)境,兩臺DFROBOT都按照指定的軌跡進行移動,兩個移動機器人移動速度均≤200 mm/s,機器人j從坐標(0,-1)開始做半徑為1.2 m的圓周運動;機器人i從坐標(-1,-1.5)開始,跟蹤并觀測機器人j進行實驗。實驗對比如圖9所示。

圖9 實驗結(jié)果Fig.9 Experimental results

圖9(a)為融合估計與RGB-D估計器估計軌跡,RGB-D濾波器估計軌跡一直在真實軌跡周圍。說明RGB-D相機識別出了機器人j,沒有丟失目標。因為受到光照強度的影響,在提取機器人j尾部標志物中心點時數(shù)據(jù)不穩(wěn)定,容易跳變,所以導(dǎo)致了RGB-D濾波器估計值方差大,軌跡波動明顯。而融合后的軌跡方差比RGB-D濾波器的估計方差要小,融合后軌跡更加貼合真實軌跡。

圖9(b)為融合估計與激光估計器估計軌跡,雖然激光雷達沒有大的數(shù)據(jù)跳變,但是激光只能測量到機器人的邊框。因為需要定位的是機器人的幾何中心點,機器人本身寬28 cm,所以無法準確地定位到目標中心,從而導(dǎo)致了定位結(jié)果有一定的偏差。雷達精度比視覺高,所估計的軌跡波動不大。融合后的軌跡比激光濾波器估計的軌跡更加貼近真實軌跡,很好地結(jié)合了兩個子濾波器的優(yōu)點,抑制了其缺點。

圖9(c)為RGB-D濾波器、激光濾波器與融合估計的RMS均方根誤差。RGB-D濾波器的均方根誤差最大E(RMS)=0.391 6 m,激光濾波器E(RMS)=0.196 3 m,運用全局融合算法(CI)計算后的E(RMS)=0.178 1 m。

4 結(jié) 論

提出的基于激光與RGB-D相機的異構(gòu)多機器人協(xié)作定位方法達到了精確定位的目的,驗證了算法的可行性。融合視覺與激光傳感器,獲得了視覺提取目標容易的優(yōu)點,又抑制了視覺收到光照等環(huán)境影響導(dǎo)致的估計方差過大的缺點,同時擁有激光估計器的方差小的特點,克服了激光識別目標困難以及無法定位到機器人幾何中心的缺點。全局融合(CI)算法比其他的融合方法計算簡單,實現(xiàn)容易,在保證定位精度的同時還提高了系統(tǒng)的實時性。