移動(dòng)機(jī)器人基于激光點(diǎn)云定位建圖的汽車寬度與方位估計(jì)

， ，霄遠(yuǎn)，

(上海交通大學(xué)機(jī)械與動(dòng)力工程學(xué)院機(jī)械系統(tǒng)與振動(dòng)國(guó)家重點(diǎn)實(shí)驗(yàn)室， 上海 200240)

0 引言

在移動(dòng)機(jī)器人抓取搬運(yùn)作業(yè)中，識(shí)別待操作目標(biāo)物體的寬度和擺放方位是成功的關(guān)鍵。文獻(xiàn)[1]采用二維圖像進(jìn)行物體檢測(cè)和定位。隨著深度傳感器的發(fā)展，基于三維點(diǎn)云的物體參數(shù)識(shí)別方法越來(lái)越多，相比于基于二維圖像的方法，其更適合于識(shí)別物體的幾何參數(shù)。基于三維點(diǎn)云識(shí)別物體幾何參數(shù)，首先需要構(gòu)建整體環(huán)境的三維點(diǎn)云結(jié)構(gòu)，然后從包含地面和周邊環(huán)境信息的數(shù)據(jù)中分離出物體，最后識(shí)別其寬度和擺放方位。

文獻(xiàn)[2]采用旋轉(zhuǎn)的二維激光雷達(dá)生成三維點(diǎn)云構(gòu)建三維環(huán)境地圖；文獻(xiàn)[3]采用三維激光雷達(dá)實(shí)時(shí)建立高精度三維點(diǎn)云地圖，但是需要借助相機(jī)傳感器。文獻(xiàn)[2-3]主要研究定位建圖技術(shù)，并沒(méi)有識(shí)別物體幾何參數(shù)。在物體與環(huán)境信息分離方法方面，文獻(xiàn)[4-6]提取環(huán)境中的平面，實(shí)現(xiàn)物體與地面、墻面或桌面的分割。文獻(xiàn)[7]首先將物體三維點(diǎn)云投影到二維平面，然后采用連通成分分析進(jìn)行物體分割。在物體幾何參數(shù)識(shí)別方面，文獻(xiàn)[8]先建立汽車點(diǎn)云的包圍盒, 然后估計(jì)包圍盒的方位和尺寸參數(shù)。文獻(xiàn)[9]提出建立邊緣點(diǎn)凸包，然后通過(guò)最小化凸包頂點(diǎn)與包圍盒各個(gè)邊的平均距離確定出包圍盒。文獻(xiàn)[10]采用迭代端點(diǎn)算法擬合出邊緣折線，以最長(zhǎng)邊緣折線的2個(gè)端點(diǎn)作為包圍盒的相對(duì)頂點(diǎn)，然后以距離兩者連線的最遠(yuǎn)點(diǎn)作為第3個(gè)頂點(diǎn)來(lái)最終確定包圍盒，進(jìn)而估計(jì)物體尺寸參數(shù)。

在此，針對(duì)室外環(huán)境下停放的汽車物體搬運(yùn)作業(yè)需求，提出一種識(shí)別待搬運(yùn)汽車寬度和方位的方法。

1 移動(dòng)機(jī)器人坐標(biāo)系定義

移動(dòng)機(jī)器人的坐標(biāo)系定義如圖1所示。移動(dòng)機(jī)器人本體坐標(biāo)系為Or-XYZ，其中原點(diǎn)Or為兩側(cè)驅(qū)動(dòng)輪軸向連線的中點(diǎn)，X軸水平指向機(jī)器人右側(cè)，Y軸指向機(jī)器人正前方，Z軸垂直地面豎直向上。機(jī)器人前上方安裝有三維激光雷達(dá)傳感器，用來(lái)采集周圍環(huán)境的信息，該傳感器測(cè)量坐標(biāo)系為Os-XYZ，其3個(gè)坐標(biāo)軸與機(jī)器人本體坐標(biāo)系平行，兩者之間存在1個(gè)平移變換T。世界坐標(biāo)系Ow-XYZ與初始時(shí)刻機(jī)器人本體坐標(biāo)系Or-XYZ重合。

圖1 移動(dòng)機(jī)器人坐標(biāo)系定義

圖2 移動(dòng)機(jī)器人運(yùn)動(dòng)中地圖構(gòu)建過(guò)程

2 基于激光點(diǎn)云的定位建圖

2.1 定位建圖中主要算法

ICP(iterativeclosestpoint)[11]算法是Besl和McKay在1992年首次提出的，主要用來(lái)計(jì)算兩幀具有部分或者全部重疊點(diǎn)云之間的位姿變換關(guān)系。GICP(generalized-ICP)作為改進(jìn)算法之一，憑借其出色的速度和魯棒性能被廣泛使用。

標(biāo)準(zhǔn)ICP算法主要分為2個(gè)部分，一是計(jì)算兩幀點(diǎn)云之間的匹配關(guān)系；二是計(jì)算使得具有匹配關(guān)系的點(diǎn)對(duì)之間歐氏距離最小的變換矩陣。標(biāo)準(zhǔn)ICP算法過(guò)程如下所述。

設(shè)有兩幀點(diǎn)云數(shù)據(jù)A={ai}，B={bi}，i=1，2，…，N，兩者之間的變換矩陣為T0。

a.令T=T0。

b.對(duì)于i=1∶N，搜索mi∈A使得‖mi-T·bi‖最小。

GICP算法考慮到點(diǎn)云中每個(gè)點(diǎn)的概率模型，通過(guò)對(duì)點(diǎn)云中的點(diǎn)進(jìn)行權(quán)重分配來(lái)使得整個(gè)算法更加精細(xì)化，同時(shí)又不增加算法復(fù)雜度，其與標(biāo)準(zhǔn)ICP主要不同在于求解T時(shí)加入了協(xié)方差矩陣，即

(1)

Ci為距離的協(xié)方差矩陣。

因子圖[12]是同時(shí)定位與建圖方法中后端優(yōu)化架構(gòu)，該框架將待優(yōu)化的變量表示為頂點(diǎn)，將變量之間的約束關(guān)系表示為因子。因子圖求解定位與建圖問(wèn)題可以轉(zhuǎn)化為求解最大后驗(yàn)估計(jì)的問(wèn)題，即通過(guò)調(diào)整待優(yōu)化變量的值使得所有因子乘積達(dá)到最大。對(duì)于待優(yōu)化的圖是不斷增長(zhǎng)的情況，因子圖可以通過(guò)精細(xì)化的處理，對(duì)于新添加進(jìn)來(lái)的節(jié)點(diǎn)和邊，近似認(rèn)為只有與新添加進(jìn)來(lái)的直接關(guān)聯(lián)的節(jié)點(diǎn)需要調(diào)整更新，而不必對(duì)整個(gè)圖進(jìn)行重新計(jì)算，以節(jié)省掉大量的計(jì)算[13]。

2.2 激光點(diǎn)云定位建圖

激光點(diǎn)云定位建圖算法架構(gòu)如圖3所示。該方法采用單一的三維激光傳感器，通過(guò)幀與幀之間的重疊關(guān)系，采用GICP算法估計(jì)傳感器的運(yùn)動(dòng)里程計(jì)；采用基于關(guān)鍵幀的策略來(lái)避免冗余；后端采用構(gòu)建因子圖的方式進(jìn)行優(yōu)化，調(diào)整三維點(diǎn)云關(guān)鍵幀的空間位置使其更好地滿足約束關(guān)系；采用回環(huán)檢測(cè)策略提供額外的約束添加到因子圖中，減小運(yùn)動(dòng)過(guò)程中由幀間匹配造成的累積誤差。

圖3 激光定位建圖算法

里程計(jì)部分將每一幀新讀取到的點(diǎn)云先與上一幀進(jìn)行GICP迭代，初步估計(jì)出當(dāng)前幀機(jī)器人位姿。然后根據(jù)初始估計(jì)出的位姿結(jié)果在地圖中尋找出與當(dāng)前幀每一點(diǎn)最近的三維點(diǎn)組成新的點(diǎn)云，將當(dāng)前幀與從地圖中取出的點(diǎn)云再次進(jìn)行GICP迭代，得到更加精確的當(dāng)前幀位姿。將此估計(jì)結(jié)果添加進(jìn)后端優(yōu)化因子圖中，執(zhí)行優(yōu)化。根據(jù)優(yōu)化后的位姿平移量的大小確定是否生成關(guān)鍵幀。若生成關(guān)鍵幀則將關(guān)鍵幀插入到回環(huán)關(guān)鍵幀序列中，并將其對(duì)應(yīng)點(diǎn)云加入地圖中。

回環(huán)優(yōu)化部分對(duì)每個(gè)新生成的關(guān)鍵幀，都會(huì)在已經(jīng)存在的回環(huán)關(guān)鍵幀序列中尋找出與當(dāng)前幀相似的關(guān)鍵幀，通過(guò)同樣的GICP方法使得兩幀之間具有額外的位姿約束關(guān)系，并添加到因子圖中。每次回環(huán)成功即進(jìn)行1次全局因子圖優(yōu)化，通過(guò)調(diào)整變量節(jié)點(diǎn)的取值來(lái)滿足所有的因子節(jié)點(diǎn)約束關(guān)系：

(2)

X為待估計(jì)的移動(dòng)機(jī)器人關(guān)鍵幀處位姿；Z為幀間匹配和回環(huán)檢測(cè)得到的關(guān)鍵幀之間位姿約束。

3 汽車寬度和方位估計(jì)

3.1 點(diǎn)云分割

激光定位建圖算法得到的三維點(diǎn)云中混有地面和環(huán)境信息，下一步需要進(jìn)行汽車點(diǎn)云與環(huán)境的分割，從而只針對(duì)汽車數(shù)據(jù)進(jìn)行處理。主要包括2步，點(diǎn)云地面去除和多個(gè)汽車之間點(diǎn)云分離。

由于激光傳感器相對(duì)于移動(dòng)機(jī)器人的變換矩陣T已知，可以根據(jù)變換矩陣T將地面點(diǎn)云數(shù)據(jù)從傳感器坐標(biāo)系轉(zhuǎn)換到機(jī)器人坐標(biāo)系下，這樣地面點(diǎn)云在機(jī)器人坐標(biāo)系下Z軸方向坐標(biāo)已知，因此地面點(diǎn)云數(shù)據(jù)的分割就比較容易，只需要將機(jī)器人坐標(biāo)系下Z坐標(biāo)為-r的點(diǎn)云去除即可，r為機(jī)器人驅(qū)動(dòng)輪半徑。

地面分割后的物體數(shù)據(jù)中包含多個(gè)目標(biāo)點(diǎn)云，為此需要進(jìn)行目標(biāo)之間的分割，從而分別識(shí)別其方位和寬度信息。同一物體的點(diǎn)云總是緊鄰的，不同物體的點(diǎn)云之間存在間距。為此，可以根據(jù)空間點(diǎn)的歐氏距離采用點(diǎn)云聚類算法進(jìn)行物體分割，然后分別對(duì)不同的類別進(jìn)行處理。對(duì)于P中的2組點(diǎn)云集PI和PJ，其中PI∩PJ=?，且令d為分類距離閾值，若滿足

(3)

則2組點(diǎn)云集可認(rèn)為分屬不同類別。聚類過(guò)程中需要根據(jù)距離對(duì)數(shù)據(jù)點(diǎn)進(jìn)行大量的臨近點(diǎn)搜索，采用遍歷方法效率太低，因此采用構(gòu)建K-D樹的數(shù)據(jù)結(jié)構(gòu)的方法加快搜索速度，其中K代表數(shù)據(jù)點(diǎn)的維數(shù)，本文中K=3。

3.2 汽車寬度和方位估計(jì)

在對(duì)汽車進(jìn)行寬度識(shí)別前先將三維的汽車點(diǎn)云投影到二維，這樣使得汽車減少了一個(gè)不重要的高度信息，也很好地保留對(duì)搬運(yùn)作業(yè)比較關(guān)鍵的長(zhǎng)寬和方位信息，如圖4所示。

圖4 汽車寬度和方位識(shí)別方法

識(shí)別算法如下所述。

設(shè)二維投影后的汽車點(diǎn)云為C(共N個(gè)點(diǎn))；第i個(gè)點(diǎn)的坐標(biāo)為Pi(xi，yi)；汽車點(diǎn)云的包圍盒長(zhǎng)度為L(zhǎng)，寬為W；P0(x0,y0)為包圍盒距離機(jī)器人坐標(biāo)系原點(diǎn)最近的點(diǎn)；包圍盒4條邊分別為l1,l2,l3,l4,其對(duì)應(yīng)的法向量分別為n1,n2,n3,n4,其中l(wèi)1與X軸的夾角為θ。

方形框4條邊的直線方程分別為：

(4)

每條邊的法向量分別為：

(5)

Pi與4條邊的距離分別為：

(6)

目標(biāo)函數(shù)為：

(7)

(8)

即確定了汽車輪廓的長(zhǎng)度和寬度，以及汽車在移動(dòng)機(jī)器人坐標(biāo)系下的擺放方位，對(duì)于搬運(yùn)作業(yè)只需要從寬度面進(jìn)行規(guī)劃搬運(yùn)路徑，因此只針對(duì)寬度進(jìn)行處理。

4 實(shí)驗(yàn)驗(yàn)證

4.1 移動(dòng)機(jī)器人實(shí)驗(yàn)平臺(tái)

實(shí)驗(yàn)設(shè)備為差動(dòng)輪移動(dòng)機(jī)器人，如圖5所示。該機(jī)器人下位機(jī)為工控機(jī)，位于機(jī)器人本體內(nèi)部，主要負(fù)責(zé)接收上位機(jī)的指令控制機(jī)器人運(yùn)動(dòng)。上位機(jī)為筆記本電腦，配置為 Core i5 四核處理器、主頻 2.3 GHz、4 GB內(nèi)存，主要負(fù)責(zé)接收激光雷達(dá)數(shù)據(jù)，并完成定位建圖及汽車寬度和方位估計(jì)的任務(wù)。

圖5 移動(dòng)機(jī)器人實(shí)驗(yàn)平臺(tái)

三維激光傳感器為VLP-16激光雷達(dá)，位于機(jī)器人頂部。它支持16個(gè)通道，每秒向四周360°發(fā)射激光束，采集30萬(wàn)個(gè)三維點(diǎn)，并將采集到的數(shù)據(jù)通過(guò)百兆以太網(wǎng)與上位機(jī)通信。VLP-16激光雷達(dá)垂直視角為±15°。

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

實(shí)驗(yàn)場(chǎng)景如圖6所示。移動(dòng)機(jī)器人在運(yùn)動(dòng)中實(shí)時(shí)構(gòu)建周圍環(huán)境的三維點(diǎn)云地圖，并識(shí)別出各個(gè)汽車寬度信息和擺放角度。三維點(diǎn)云圖結(jié)果如圖7所示，測(cè)量結(jié)果與真實(shí)值對(duì)比如表1所示。由表1可知，該方法識(shí)別結(jié)果寬度誤差約為0.007 m。汽車停放角度測(cè)量值如表2所示，角度誤差約為1°，可以滿足室外搬運(yùn)作業(yè)精度要求。

圖6 實(shí)驗(yàn)場(chǎng)景

圖7 三維點(diǎn)云圖

表1 汽車寬度測(cè)量結(jié)果 m

表2 停放角度測(cè)量結(jié)果 (°)

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

提出的基于激光點(diǎn)云定位建圖的汽車寬度和方位估計(jì)方法，適用于室外環(huán)境下移動(dòng)機(jī)器人汽車搬運(yùn)作業(yè)。該方法通過(guò)構(gòu)建點(diǎn)云地圖將汽車外形部分重構(gòu)出來(lái)，然后對(duì)點(diǎn)云進(jìn)行分割，提取出汽車點(diǎn)云，最后通過(guò)投影處理后進(jìn)行包圍盒估計(jì)，進(jìn)而識(shí)別出汽車的寬度和方位信息。通過(guò)實(shí)驗(yàn)驗(yàn)證了該方法的識(shí)別效果。

參考文獻(xiàn)：

[1] Calonder M, Lepetit V, Strecha C, et al. BRIEF: binary robust independent elementary features[C]// European Conference on Computer Vision,2010:778-792.

[2] Zhang J, Singh S. LOAM : lidar odometry and mapping in real-time[C]// Robotics: Science and Systems Conference,2014.

[3] Zhang J, Singh S. Enabling aggressive motion estimation at low-drift and accurate mapping in real-time[C]// IEEE International Conference on Robotics and Automation, 2017：5051-5058.

[4] 孫小凱.基于RGB-D信息的物體定位與識(shí)別[D].杭州：浙江大學(xué), 2014.

[5] 柯翔. 面向室內(nèi)環(huán)境的服務(wù)機(jī)器人物體檢測(cè)與識(shí)別研究[D].合肥:中國(guó)科學(xué)技術(shù)大學(xué), 2013.

[6] 鄢武. 基于ROS和點(diǎn)云庫(kù)的室內(nèi)三維物體識(shí)別與姿態(tài)估計(jì)[D].廣州: 廣東工業(yè)大學(xué), 2016.

[7] Marton Z C, Goron L C, Rusu R B, et al. Reconstruction and verification of 3D object models for grasping[C]// Robotics Research - the International Symposium, 2009:315-328.

[8] Azim A, Aycard O. Detection, classification and tracking of moving objects in a 3D environment[C]//Intelligent Vehicles Symposium, 2012:802-807.

[9] B?rcs A, Nagy B, Baticz M, et al. A model-based approach for fast vehicle detection in continuously streamed urban LIDAR point clouds[C]//Asian Conference on Computer Vision, 2014:413-425.

[10] Ye Y, Fu L, Li B. Object detection and tracking using multi-layer laser for autonomous urban driving[C]// IEEE International Conference on Intelligent Transportation Systems, 2016:259-264.

[11] Besl P J, McKay N D. A method for registration of 3-D shapes[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence,2002, 14(2):239-256.

[12] Dellaert F, Kaess M. Factor graphs for robot perception[J].Foundations and Trends in Robotics 2017, 6(1/2):1-139.

[13] Kaess M, Johannsson H, Roberts R, et al. iSAM2: incremental smoothing and mapping using the Bayes tree[J].International Journal of Robotics Research, 2012, 31(2):216-235.