基于激光與RGB-D相機的異構多機器人協作定位

,, ,

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

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

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

1 問題描述與建模

1.1 系統描述

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

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

圖1 異構多機器人協作定位框架圖Fig.1 Framework of cooperative localization for heterogeneous multi robots

1.2 機器人運動模型

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

圖2 運動模型Fig.2 Motion model

定義移動機器人的位姿為Xk=[xk,yk,θk]T為k時刻移動機器人的橫坐標、縱坐標和航向角。如圖2所示,狀態方程[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 協作定位方法

圖3 協同定位算法框架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紅外相機中心為坐標原點建立坐標系,取紅外相機指向紅外發射的方向為X軸,Z軸垂直于圖像平面,當物體恰好位于參考平面時,記錄下它的散斑圖案。當物體位于平面時,它的散斑圖像在X軸上產生一個視差,通過三角形相似原理可知表達式為

(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局部估計器設計

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

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

步驟1狀態預測。其表達式為

(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)

根據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={ρi,i|i=1,2,…,n},其中ρi為激光測量的長度值,i為每束激光的數據集的下標。

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

(24)

2.2.2 激光局部估計器設計

算法2激光局部估計器

步驟1狀態預測。其表達式為

(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 全局融合算法

得到兩個局部估計之后,將兩個局部估計值進一步融合。由于受到共同的過程噪聲影響,兩個局部估計結果是相關的,而各局部估計間的互協方差計算復雜,考慮到移動機器人的實時性要求高的特點,采用CI融合算法得到更精確的估計。基于激光傳感器與RGB-D相機的全局估計器設計算法為

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最優估計位姿為xCI(k+1|k+1),與估計的方差陣PCI(k+1|k+1)。

3 實 驗

3.1 實驗硬件與環境

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

圖7 DFROBOT機器人Fig.7 DFROBOT

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

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

3.2 實驗場景與結果

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

圖9 實驗結果Fig.9 Experimental results

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

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

圖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 結 論

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