基于ROS與三維點(diǎn)云圖像的室內(nèi)物體精準(zhǔn)定位

于 洋，樸 燕，倪 焱，佀同嶺

(長(zhǎng)春理工大學(xué) 電子信息工程學(xué)院，吉林 長(zhǎng)春 130022)

1 引 言

由于GPS衛(wèi)星定位的小環(huán)境局限性和室內(nèi)環(huán)境的復(fù)雜性，室內(nèi)物體定位技術(shù)便有了大展拳腳的機(jī)會(huì)。室內(nèi)小型激光雷達(dá)與KINECT深度相機(jī)的出現(xiàn)和發(fā)展使得我們可以輕而易舉地獲得物體高清RGB圖像、室內(nèi)場(chǎng)景點(diǎn)云和三維特征點(diǎn)云圖像信息，為能夠?qū)崿F(xiàn)物體更好的定位提供了技術(shù)手段。

使用三維點(diǎn)云圖像數(shù)據(jù)來(lái)描述物體特征簡(jiǎn)單直觀，而且數(shù)據(jù)獲取方便。它主要是通過(guò)三維掃描獲取設(shè)備，記錄被檢測(cè)物體表面上大量高度密集的點(diǎn)的三維坐標(biāo)、反射率等信息，從而描述物體特征。

基于ROS的移動(dòng)平臺(tái)[1]由于其開源性近年發(fā)展迅速。許多知名機(jī)器人開源庫(kù)，如TF坐標(biāo)轉(zhuǎn)換、3D點(diǎn)云處理驅(qū)動(dòng)、SLAM等都是基于ROS開發(fā)的。

本研究以基于ROS的移動(dòng)平臺(tái)為基礎(chǔ)，在其上面搭載了RPLIDAR A2激光雷達(dá)與KINECT深度相機(jī)，通過(guò)SLAM算法對(duì)室內(nèi)地圖進(jìn)行實(shí)時(shí)構(gòu)建，通過(guò)RPLIDAR A2激光雷達(dá)構(gòu)建的二維地圖與KINECT獲得的物體的三維點(diǎn)云進(jìn)行復(fù)合坐標(biāo)定位來(lái)實(shí)現(xiàn)物體的精準(zhǔn)定位。

2 定位系統(tǒng)整體設(shè)計(jì)

2.1 軟硬件平臺(tái)的搭建

為了完成室內(nèi)物品的精準(zhǔn)定位研究，在實(shí)驗(yàn)室自主研發(fā)的基于ROS的智能數(shù)據(jù)采集移動(dòng)底盤的基礎(chǔ)上放置了RPLIDAR A2激光雷達(dá)與KINECT相機(jī)。其硬件系統(tǒng)構(gòu)成如圖1所示。

圖1 硬件系統(tǒng)構(gòu)成圖Fig.1 Hardware system diagram

這里重點(diǎn)介紹一下負(fù)責(zé)全局地圖構(gòu)建的RPLIDAR A2激光雷達(dá)與負(fù)責(zé)三維點(diǎn)云采集的KINECT深度相機(jī)。RPLIDAR A2具有高速激光測(cè)距采樣能力，可對(duì)在米半徑范圍內(nèi)物體進(jìn)行激光測(cè)距掃描，并產(chǎn)生點(diǎn)云地圖信息[2]。這些點(diǎn)云信息可以對(duì)室內(nèi)全局進(jìn)行地圖構(gòu)建。由于我們的研究地點(diǎn)為室內(nèi)環(huán)境，該環(huán)境的特征是整體面積偏小，但是局部物品堆放復(fù)雜，要求物體定位信息精準(zhǔn)。ROLIDAR A2的工作性能指標(biāo)剛好滿足我們的要求。

三維掃描設(shè)備可以快速精確地獲得物體表面的形狀和空間拓普信息。微軟研發(fā)的KINECT深度相機(jī)采集到的物體三維點(diǎn)云信息更為精確且很難受到光照、紋理等條件影響，不但外觀小巧方便裝載，而且成本也不高[3]。因此本研究采用KINECT來(lái)獲得物體的三維特征點(diǎn)云信息。

在構(gòu)建了硬件平臺(tái)后，想要完成本文研究還需要軟件平臺(tái)的支持。本研究主要采用了基于LINUX的Ubuntu16.04系統(tǒng)與開源機(jī)器人操作系統(tǒng)ROS(Robot Operating System)。ROS有很多優(yōu)點(diǎn)，它是開源的，提供底層驅(qū)動(dòng)程序管理、共用功能的執(zhí)行、程序間的消息傳遞、程序發(fā)行包管理等，它也提供一些工具程序和庫(kù)用于獲取、建立、編寫和運(yùn)行多機(jī)整合的程序[4]。

2.2 融合算法實(shí)現(xiàn)流程

本文通過(guò)Gmapping算法結(jié)合了SLAMTEC發(fā)布的關(guān)于RPLIDAR激光雷達(dá)的ROS功能包rplidar_ros獲取室內(nèi)地圖，然后使用KINCET深度相機(jī)結(jié)合orb_slam包來(lái)獲取物體三維特征點(diǎn)云圖像，最后通過(guò)點(diǎn)云圖像與室內(nèi)地圖對(duì)物體進(jìn)行復(fù)合坐標(biāo)定位，從而實(shí)現(xiàn)室內(nèi)物體的精準(zhǔn)定位。其具體算法實(shí)現(xiàn)流程圖如圖2所示。

圖2 算法實(shí)現(xiàn)流程圖Fig.2 Algorithm realization flow chart

3 復(fù)合坐標(biāo)的精準(zhǔn)定位

圖3 復(fù)合坐標(biāo)精準(zhǔn)定位流程圖Fig.3 Flow chart for precise positioning of composite coordinates

我們知道二維坐標(biāo)可以鎖定平面內(nèi)物體的位置，三維坐標(biāo)可以表示空間內(nèi)物體的位置，而本文采用的是四維復(fù)合坐標(biāo)的方式來(lái)進(jìn)行物體的精準(zhǔn)定位，定位精度高。其完成復(fù)合四維坐標(biāo)精準(zhǔn)定位流程圖如圖3所示。

為完成本定位研究，主要研究?jī)?nèi)容為：室內(nèi)地圖構(gòu)建、三維點(diǎn)云圖像的獲取、三維坐標(biāo)信息計(jì)算與復(fù)合坐標(biāo)系定位等步驟。

3.1 Gmapping算法選取與室內(nèi)地圖構(gòu)建

室內(nèi)物體精準(zhǔn)定位的關(guān)鍵點(diǎn)在于獲取物體基于某一坐標(biāo)系的三維坐標(biāo)。多室內(nèi)的地圖構(gòu)建正是獲取物體位置的關(guān)鍵一環(huán)。選取建圖算法就更是重中之重。

Gmapping算法是基于RBPF的粒子濾波算法[5]，它在RBPF算法上做了兩個(gè)重要的改進(jìn)：為了減小粒子數(shù)Gmapping算法提出了改進(jìn)提議分布，為了減少重采樣的次數(shù)Gmapping算法提出了選擇性重采樣[6]。

這兩個(gè)改進(jìn)對(duì)構(gòu)建室內(nèi)地圖來(lái)說(shuō)影響重大。首先說(shuō)一下改進(jìn)提議分布。改進(jìn)提議分布主要由兩部分組成，一部分有計(jì)算出數(shù)據(jù)的提議分布，另一部分是權(quán)重計(jì)算。以圖4為例，圖中虛線概率分布是里程計(jì)采樣的高斯分布，實(shí)線的概率分布也是使用激光進(jìn)行觀測(cè)后獲得狀態(tài)的高斯分布。由圖可知觀測(cè)提供的信息的準(zhǔn)確度(方差小)相比控制的準(zhǔn)確度要高很多。

圖4 概率分布圖Fig.4 Probability map

如果通過(guò)從提議分布中采樣得到下一時(shí)刻移動(dòng)平臺(tái)的位姿，那么提議分布與目標(biāo)分布越接近的話所使用的粒子越少。如果粒子是直接從目標(biāo)分布采樣的話只需要一個(gè)粒子就可以獲得移動(dòng)平臺(tái)的位姿估計(jì)了。因此Gmapping算法要做的是改進(jìn)提議分布，如果只從里程計(jì)中采樣的，粒子的權(quán)重迭代公式見式(1)。

(1)

激光的分布相比里程計(jì)分布更接近真正的目標(biāo)分布，因此如果可以把激光的信息融入到提議分布中的話那樣提議分布就會(huì)更接近目標(biāo)分布。因此本文采用RPLIDAR A2激光雷達(dá)來(lái)獲取激光信息，分布計(jì)算公式變成式(2)，而粒子的權(quán)重公式計(jì)算公式變成了式(3)。

(2)

(3)

目標(biāo)分布在大多數(shù)情況下只有一個(gè)峰值。為了簡(jiǎn)化計(jì)算量在峰值附近采K個(gè)值來(lái)模擬出提議分布。Gmapping算法使用高斯函數(shù)來(lái)構(gòu)建提議分布，因此有了K個(gè)數(shù)據(jù)后就可以模擬出一個(gè)高斯函數(shù)作為提議分布:

.

(4)

Gmapping在計(jì)算權(quán)重時(shí)就是計(jì)算模擬出的提議分布和目標(biāo)分布的不同。而這種不同體現(xiàn)在我們是由有限的采樣點(diǎn)模擬出目標(biāo)分布，因此權(quán)重的計(jì)算公式為:

.

(5)

至此Gmapping改進(jìn)提議分布完成。接下來(lái)是重采樣部分。這部分來(lái)說(shuō)相對(duì)比較簡(jiǎn)單，Gmmaping算法通過(guò)設(shè)定一個(gè)閾值，當(dāng)粒子的權(quán)重變化大于我們?cè)O(shè)定的閾值時(shí)就會(huì)執(zhí)行重采樣，這樣減少了采樣的次數(shù)，也就減緩了粒子退化，從而提高地圖精度。

考慮到Gmapping算法在構(gòu)建小場(chǎng)景地圖所需計(jì)算量較小，而且精度較高，因此本研究采用Gmmaping算法對(duì)地圖進(jìn)行構(gòu)建。

本文的實(shí)驗(yàn)地點(diǎn)為實(shí)驗(yàn)室。理論上，構(gòu)圖激光粒子數(shù)目越多，構(gòu)建出的地圖越準(zhǔn)確。但實(shí)際操作中由于激光粒子的數(shù)目增多，計(jì)算機(jī)負(fù)載驟升，構(gòu)圖噪聲也會(huì)更加明顯。圖5為構(gòu)圖粒子數(shù)對(duì)構(gòu)圖的影響。在構(gòu)圖激光粒子數(shù)目P由30增加到150的過(guò)程中，生成的二維地圖精度符合似正態(tài)分布。通過(guò)試驗(yàn)比對(duì)，構(gòu)圖粒子選擇為150。在選擇最優(yōu)粒子數(shù)后，Gmapping的Linear Update和Angular Update參數(shù)對(duì)構(gòu)圖效果也會(huì)產(chǎn)生影響。最終通過(guò)比對(duì)我們選擇了Angular Update參數(shù)為0.2 ，且Linear Update參數(shù)為0.2 時(shí)地圖構(gòu)建效果最好。對(duì)比效果如圖6所示。

圖5 構(gòu)圖粒子數(shù)對(duì)構(gòu)圖的影響Fig.5 Influence of composition number on composition

圖6 Linear Update和Angular Update對(duì)構(gòu)圖的影響Fig.6 Influence of Linear Update and Angular Update on composition

最終本文選擇激光粒子數(shù)P=150，Linear Update與Angular Update都為0.2進(jìn)行構(gòu)圖和后續(xù)實(shí)驗(yàn)。

3.2 二維坐標(biāo)的標(biāo)定

二維坐標(biāo)的標(biāo)定是三維外部坐標(biāo)系φ建立的基礎(chǔ)。通過(guò)在虛擬機(jī)Rviz軟件中可以加載已經(jīng)構(gòu)建成功的室內(nèi)2D地圖。取構(gòu)建好的地圖左下角為二維坐標(biāo)系原點(diǎn)，室內(nèi)長(zhǎng)寬分別為二維坐標(biāo)系x，y軸。首先需要測(cè)量室內(nèi)的長(zhǎng)寬，然后對(duì)Rviz軟件柵格等比例縮放來(lái)獲得單位柵格長(zhǎng)寬大小。用式(6)可以算出任意一點(diǎn)的坐標(biāo)，其中P點(diǎn)為移動(dòng)平臺(tái)在Rviz中的實(shí)時(shí)顯示的點(diǎn)。

.

(6)

室內(nèi)2D地圖的坐標(biāo)標(biāo)定至此完成，其效果如圖7所示。

圖7 任一點(diǎn)的二維標(biāo)定Fig.7 Two dimensional calibration of any point

3.3 物體三維特征點(diǎn)云圖像獲取

本文用KINECT深度相機(jī)獲取待定位物體點(diǎn)云圖像。在PC端Rviz仿真軟件點(diǎn)選待移動(dòng)目標(biāo)點(diǎn)，此時(shí)移動(dòng)底盤將自主移動(dòng)避障前往目標(biāo)點(diǎn)。從高清攝像頭中觀測(cè)到待定位物體之后，我們讓數(shù)據(jù)采集底盤在待定位物體周圍反復(fù)移動(dòng)，從不同的視覺獲取特征點(diǎn)云從而獲得物體三維點(diǎn)云圖像。

圖8 高清圖像與特征點(diǎn)云Fig.8 High definition image and feature point cloud

圖9 物體三維點(diǎn)云Fig.9 Object 3D point cloud

圖7中，左側(cè)圖像是攝像頭采集的原始圖像，含有紅綠特征點(diǎn)的黑白圖像。對(duì)比兩圖，在左圖中選取待定位物體之后，在待定位物體附近讓數(shù)據(jù)采集底盤多次來(lái)回移動(dòng)以保證采集足夠多的特征點(diǎn)。最后在本地虛擬機(jī)中打開Rviz便可以看到采集到的物體三維點(diǎn)云圖像了。如圖9所示，其中藍(lán)色方框?yàn)橐苿?dòng)底盤行駛軌跡，紅色點(diǎn)云為采集到的物體三維點(diǎn)云圖像。

3.4 復(fù)合坐標(biāo)精準(zhǔn)定位

首先進(jìn)行四維定位坐標(biāo)系的構(gòu)建。首先由構(gòu)建地圖獲得的二維坐標(biāo)結(jié)合空間信息組合成三維外部坐標(biāo)系φ，然后通過(guò)對(duì)不同室內(nèi)進(jìn)行分類Ⅰ、Ⅱ、Ⅲ,結(jié)合外部坐標(biāo)系構(gòu)成四維定位坐標(biāo)系。

四維坐標(biāo)系構(gòu)建完成后，為了獲取到待定位物體的三維信息，要對(duì)獲得物體三維特征點(diǎn)云圖像進(jìn)行濾波、聚類、坐標(biāo)變換等過(guò)程。

濾波這里我們采取雙邊濾波器[7]對(duì)點(diǎn)云進(jìn)行濾波。雙邊濾波器的好處是可以做到邊緣保存，它是基于空間分布的高斯濾波函數(shù)，所以在點(diǎn)云邊緣附近，離得較遠(yuǎn)的像素不會(huì)太多影響到邊緣上的像素值[8]。

由KINECT視覺傳感器的工作原理可知，本文獲得的點(diǎn)云數(shù)據(jù)坐標(biāo)都是相對(duì)于KINECT視覺傳感器得到的，KINECT的攝像頭坐標(biāo)系和我們所定義的外部的坐標(biāo)系不同。因此我們根據(jù)特征點(diǎn)云進(jìn)行參數(shù)優(yōu)化的方法，建立了坐標(biāo)變換矩陣(7)。

(7)

其中表示旋轉(zhuǎn)的為3乘3矩陣Ri，表示平移的為三維向量ti。我們?cè)讦罩羞x組坐標(biāo)已知的M個(gè)點(diǎn)，坐標(biāo)記為pk,然后測(cè)量這些點(diǎn)在φ中的坐標(biāo)Q，根據(jù)變換關(guān)系就可以計(jì)算出他們?cè)讦罩械淖鴺?biāo)TiQk。最后計(jì)算出變換矩陣為：

.

(8)

為了計(jì)算兩個(gè)點(diǎn)之間的距離誤差，構(gòu)造了函數(shù)E。在實(shí)際中取三維歐幾里得距離平方求得。

E(P,Q)=(Px-Qx)2+(Py-Qy)2+
(Pz-Qz)2

，

(9)

其中：P表示P對(duì)應(yīng)于KINECT的三維坐標(biāo)，Ti為坐標(biāo)變換矩陣，求和過(guò)程相當(dāng)于將所有計(jì)算后的點(diǎn)云坐標(biāo)合并，其坐標(biāo)值是基于我們定義的外部坐標(biāo)系。

上述坐標(biāo)變換校準(zhǔn)過(guò)程的關(guān)鍵點(diǎn)是在于要有準(zhǔn)確的坐標(biāo)值P。這里基于KINECT三維坐標(biāo)值的點(diǎn)云數(shù)據(jù)通過(guò)讀取每一個(gè)像素點(diǎn)的三維數(shù)據(jù)獲得。

Pi中除了含有待定位物體的點(diǎn)云信息之外，還有很多不需要的點(diǎn)的信息。由于待定位物體只會(huì)聚類在一定范圍之內(nèi)，因此我們需要將不需要點(diǎn)的信息去除[10]。本文結(jié)合高清攝像頭采集的圖像與三維點(diǎn)云信息對(duì)比，考慮待檢測(cè)物體對(duì)應(yīng)點(diǎn)云密集的關(guān)系，分割點(diǎn)云數(shù)據(jù)、由于每個(gè)物體表面產(chǎn)生的點(diǎn)云數(shù)據(jù)距離很近。因此我們可以計(jì)算相鄰點(diǎn)的距離，通過(guò)距離的大小值對(duì)點(diǎn)云進(jìn)行聚類，從而得到只有待測(cè)物體的點(diǎn)云P。這些點(diǎn)云信息最終用于待定位物體的位置坐標(biāo)計(jì)算?？紤]到本文獲得的點(diǎn)云是在物體表面均勻采樣得到的，因此點(diǎn)云數(shù)據(jù)基于KINECT三維坐標(biāo)值如下：

.

(10)

基于優(yōu)化理論，為了減少計(jì)算誤差，特征點(diǎn)的數(shù)目需要足夠多。位置選取應(yīng)具有代表性[11]。把基于KINECT的三維坐標(biāo)通過(guò)式(11)經(jīng)過(guò)仿射矩陣獲得物體基于外部坐標(biāo)系φ的坐標(biāo)。

.

(11)

再結(jié)合第一維坐標(biāo)，獲得物體的四維坐標(biāo)數(shù)據(jù)，達(dá)到精準(zhǔn)定位的效果。

4 實(shí)驗(yàn)結(jié)果

這里截取了兩張典型實(shí)驗(yàn)的效果圖，多室多物體精準(zhǔn)定位實(shí)驗(yàn)結(jié)果如圖10所示，實(shí)測(cè)兩物體室內(nèi)的位置平均測(cè)量誤差只有4.2 cm。

之后本文試驗(yàn)了其他3種室內(nèi)物體定位系統(tǒng)：超寬帶定位系統(tǒng)、超聲波與空外線定位系統(tǒng)、基于藍(lán)牙角度測(cè)量的定位系統(tǒng)對(duì)相同室內(nèi)環(huán)境下的大物體瓶子與小物體玩偶進(jìn)行定位。對(duì)比其他3種常見的室內(nèi)定位方法，本文方法精度更高，效果更好。其定位結(jié)果對(duì)比如表1所示。

(a)Ⅰ室瓶子四維坐標(biāo)定位(a) Four-dimensional coordinate location of bottle in room Ⅰ

(b)Ⅱ室玩偶四維坐標(biāo)標(biāo)定(b)Four-dimensional coordinate location of doll in room Ⅱ圖10 定位實(shí)驗(yàn)結(jié)果圖Fig.10 Location experiment result diagram

表1 定位效果對(duì)比Tab.1 Comparison of positioning results

由實(shí)驗(yàn)結(jié)果可以看到，本文提出的定位系統(tǒng)對(duì)室內(nèi)物體定位更加精準(zhǔn)，尤其是在更困難的小物體的定位精度上更高。

5 結(jié) 論

本文提出了一種融合基于ROS的SLAM算法和三維特征點(diǎn)云的物體精準(zhǔn)定位研究系統(tǒng)，克服了ROS SLAM只能描述二維信息，三維點(diǎn)云只能描述物體獨(dú)立的三維信息等缺點(diǎn)。通過(guò)實(shí)驗(yàn)結(jié)果來(lái)看，其定位精度比起常見的超聲波與紅外線定位系統(tǒng)提高6.7%，比基于藍(lán)牙角度測(cè)量的定位系統(tǒng)定位精度提高20%，比超寬帶定位系統(tǒng)提高72%，對(duì)室內(nèi)物體的精準(zhǔn)定位效果顯著。