Kinect與二維激光雷達(dá)結(jié)合的機(jī)器人障礙檢測

肖宇峰，黃 鶴，鄭 杰，劉 冉

(西南科技大學(xué)信息工程學(xué)院 四川 綿陽 621010)

作為移動(dòng)機(jī)器人感知環(huán)境、自主導(dǎo)航的基礎(chǔ)，障礙物檢測是實(shí)現(xiàn)正確運(yùn)動(dòng)控制的重要依據(jù)。傳統(tǒng)的障礙物探測通常采用激光傳感器、超聲傳感器或紅外傳感器等非視覺器件，具有抗干擾能力強(qiáng)、可全天候工作的優(yōu)點(diǎn)，然而這些傳感器采集的數(shù)據(jù)單一，機(jī)器人很難準(zhǔn)確探測周圍環(huán)境的三維信息[1-2]。作為無人駕駛系統(tǒng)中環(huán)境感知的重要功能單元，三維激光雷達(dá)可較好地檢測周圍環(huán)境三維信息，但因其成本較高，目前較難大范圍應(yīng)用[3]。

相比之下，基于視覺的環(huán)境感知技術(shù)具有成本低、信息豐富的優(yōu)點(diǎn)，并得到了廣泛研究[4-5]。然而，視覺傳感器易受環(huán)境影響、檢測精度低：基于單目視覺的環(huán)境感知方法具有受環(huán)境影響較大、檢測精度低和視野范圍小的缺點(diǎn)[6]；而基于雙目視覺的環(huán)境感知方法易出現(xiàn)多義性[7-8]。鑒于這些問題，有研究者把視覺和激光檢測方法結(jié)合起來，在增強(qiáng)環(huán)境適應(yīng)性的同時(shí)提高檢測精度[9]：文獻(xiàn)[10]把車載相機(jī)和二維激光雷達(dá)結(jié)合起來，較好地探測了障礙物位置和形狀，以較低成本輔助智能車無人駕駛；文獻(xiàn)[11]把二維激光雷達(dá)與視覺融合起來，較好地解決了車輪打滑時(shí)的位姿丟失問題，并能較好檢測出地面負(fù)障礙物。

相比單目和雙目相機(jī)，Kinect相機(jī)能以簡捷方式和較低成本提供環(huán)境深度數(shù)據(jù)[12]。利用這一優(yōu)點(diǎn)，本文提出在移動(dòng)機(jī)器人上同時(shí)安裝二維激光雷達(dá)和Kinect相機(jī)，把激光掃描和深度圖像結(jié)合起來檢測障礙物。這種障礙物檢測方法不僅具備二維激光雷達(dá)平面探測范圍廣、精度高、實(shí)時(shí)性好的特點(diǎn)，而且兼有Kinect深度相機(jī)三維探測的優(yōu)勢。具體步驟是：首先，二維激光雷達(dá)與深度相機(jī)分別從周圍環(huán)境中獲取二維激光數(shù)據(jù)和三維深度圖像；然后，將深度圖像轉(zhuǎn)換成虛擬二維激光數(shù)據(jù)；最后，對虛擬二維激光數(shù)據(jù)與二維激光雷達(dá)數(shù)據(jù)進(jìn)行融合，得到障礙物的三維位置。本文通過實(shí)物測試驗(yàn)證了該方法正確有效，可用于移動(dòng)機(jī)器人對環(huán)境障礙物的判斷。

1 整體系統(tǒng)框架

圖1為本文的研究平臺(tái)，在改造的Turtlebot機(jī)器人平臺(tái)上搭載一臺(tái)Kinect相機(jī)，并在其上方安裝一臺(tái)Hokuyo UTM-30LX激光雷達(dá)。激光雷達(dá)和Kinect相機(jī)結(jié)合的工作思路如圖2所示，Kinect相機(jī)從周圍環(huán)境中采集深度圖像，經(jīng)過“深度圖像轉(zhuǎn)換模塊”轉(zhuǎn)換成虛擬激光測距數(shù)據(jù)；二維激光雷達(dá)從周圍環(huán)境中采集二維平面的激光測距數(shù)據(jù)；“測距數(shù)據(jù)融合”模塊把虛擬激光測距數(shù)據(jù)和激光測距數(shù)據(jù)整合得到障礙物位置，并提交給“機(jī)器人導(dǎo)航模塊”。

圖1 系統(tǒng)工作平臺(tái)

圖2 系統(tǒng)工作思路

2 二維激光雷達(dá)與Kinect相機(jī)聯(lián)合標(biāo)定

對于環(huán)境中的障礙物，二維激光雷達(dá)和Kinect相機(jī)各有不同的表示形式。圖3給出了同時(shí)處在激光雷達(dá)坐標(biāo)系(OLXL YLZL)與Kinect相機(jī)坐標(biāo)系(OKXK YKZK)的目標(biāo)點(diǎn)P，坐標(biāo)分別為(xL,yL,zL)和(xK,yK,zK)，P點(diǎn)在圖像平面坐標(biāo)系(XPOP YP)中的投影P′坐標(biāo)為(u,v)。通過標(biāo)定找到激光雷達(dá)坐標(biāo)系與Kinect圖像坐標(biāo)系之間的關(guān)系，才能準(zhǔn)確融合激光雷達(dá)測距數(shù)據(jù)與Kinect深度圖像數(shù)據(jù)。

圖3 激光雷達(dá)與Kinect相機(jī)坐標(biāo)系

2.1 激光雷達(dá)坐標(biāo)系與Kinect坐標(biāo)系的關(guān)系

P點(diǎn)在(OLXL YLZL)下的坐標(biāo)為(xL,yL,zL)，根據(jù)激光雷達(dá)坐標(biāo)系與Kinect坐標(biāo)系的幾何關(guān)系，得到坐標(biāo)關(guān)系為：

式中，R是旋轉(zhuǎn)矩陣；T是平移矩陣。Kinect相機(jī)采集得到的數(shù)據(jù)不是P點(diǎn)坐標(biāo)(xK,yK,zK)，而是P′點(diǎn)像素坐標(biāo)(u,v)和對應(yīng)的深度值zK；激光雷達(dá)采集得到的不是P點(diǎn)坐標(biāo)(xL,yL,zL)，而是距離r和角度α?？梢酝ㄟ^下面兩小節(jié)分析得到(xK,yK,zK)與像素點(diǎn)三元組(u,v,zK)的對應(yīng)關(guān)系、(xL,yL,zL)與測距點(diǎn)二元組(r,α)的對應(yīng)關(guān)系。

根據(jù)經(jīng)典相機(jī)模型[6]，P點(diǎn)在(OKXK YKZK)下的坐標(biāo)為(xK,yK,zK)，其在圖像平面投影為P′，用像素表示坐標(biāo)為(u,v)，可以得到Kinect坐標(biāo)與像素點(diǎn)三元組的關(guān)系為：

2.2 Kinect坐標(biāo)與像素點(diǎn)三元組的關(guān)系

式中，fx、fy是X軸和Y軸方向上以像素為單位的等效焦距；cx、cy是基準(zhǔn)點(diǎn)。這些都是相機(jī)參數(shù)，可以采用標(biāo)準(zhǔn)的相機(jī)參數(shù)標(biāo)定方法計(jì)算出。

2.3 激光雷達(dá)坐標(biāo)與像素點(diǎn)三元組的關(guān)系

綜合式(1)與式(2)，可以得到像素點(diǎn)三元組與激光雷達(dá)坐標(biāo)的關(guān)系為：

對于激光雷達(dá)，保持極坐標(biāo)系與平面直角坐標(biāo)系(OLXLZL)在同一個(gè)平面內(nèi)，原點(diǎn)同為激光雷達(dá)中心，激光束與ZL軸正向夾角為極坐標(biāo)角度α(激光束偏角值)，激光束測距值為極坐標(biāo)徑向值r(激光雷達(dá)當(dāng)前測距值)，如圖4所示。

圖4 激光雷達(dá)的直角坐標(biāo)與極坐標(biāo)關(guān)系

激光測距點(diǎn)二元組(r,α)與直角坐標(biāo)(xL,yL,zL)的關(guān)系為：

對于像素點(diǎn)P′的圖像坐標(biāo)(u,v)，保持激光雷達(dá)坐標(biāo)原點(diǎn)與Kinect相機(jī)坐標(biāo)原點(diǎn)在Y軸方向的垂直高度差為l，則有像素點(diǎn)三元組(u,v,zK)與激光雷達(dá)測距點(diǎn)二元組(r,α)的關(guān)系為：

在確定相機(jī)參數(shù)后，把多對像素點(diǎn)三元組和測距點(diǎn)二元組代入式(5)，通過求解線性方程組得到未知的R、T矩陣，進(jìn)而確定激光雷達(dá)坐標(biāo)系與Kinect相機(jī)坐標(biāo)系的旋轉(zhuǎn)平移關(guān)系，完成聯(lián)合標(biāo)定。

3 深度圖像轉(zhuǎn)換

本文中，Kinect相機(jī)采集得到深度圖像后，深度圖像轉(zhuǎn)換模塊把圖像中轉(zhuǎn)換窗口的每個(gè)像素列上深度值最小的像素點(diǎn)距離和角度提取出來，相當(dāng)于將部分或者全部深度圖像向中心平面壓縮，在中心平面上形成一系列虛擬激光點(diǎn)。圖5a為Kinect相機(jī)采集到的RGB圖像；圖5b為深度圖像與轉(zhuǎn)換窗口，s表示窗口第一個(gè)轉(zhuǎn)換像素行號(hào)，h表示轉(zhuǎn)換窗口高度(單位為像素行)，RGB圖像 (u0,v0)點(diǎn)對應(yīng)深度圖像的(u,v)點(diǎn)。

假設(shè)當(dāng)前需要轉(zhuǎn)換的像素在第u個(gè)像素列，該列所有像素點(diǎn)被轉(zhuǎn)換成一個(gè)虛擬激光點(diǎn)，該點(diǎn)在所有虛擬激光點(diǎn)中的序號(hào)是j，激光點(diǎn)距離值為：

式中，表示OK與目標(biāo)點(diǎn)P的虛擬激光點(diǎn)(P點(diǎn)在中心平面上的投影)距離，具體計(jì)算方法參照圖6所示模型。P在深度圖像的對應(yīng)點(diǎn)P′(u,v)，其在中心平面的投影點(diǎn)A就是虛擬激光點(diǎn)，C是OK的投影，那么，線段AO的長度就是。

圖5 Kinect相機(jī)圖像

圖6 Kincet相機(jī)的虛擬激光測距

具體計(jì)算(i)的步驟如下：

1) 獲取P在 Kinect坐標(biāo)系下坐標(biāo)P(xK,yK,zK)，zK是P′的深度值；

2) 計(jì)算直線AOK和COK夾角β為：

3) 二維激光點(diǎn)的計(jì)算。

如果Kinect相機(jī)觀測角范圍為 (βmin,βmax)，沿AC軸向可轉(zhuǎn)換成M個(gè)虛擬激光點(diǎn)，那么，P對應(yīng)虛擬激光束的序號(hào)為：

4) 計(jì)算A到相機(jī)原點(diǎn)KO的距離為：

4 深度圖像與二維激光雷達(dá)數(shù)據(jù)融合

設(shè)激光雷達(dá)掃描得到的測距二元組集合為DL，融合Kinect采集得到的三元組信息后的測距二元組集合為DF。圖3中，P為DL中第i個(gè)測距點(diǎn)，其測距二元組為(dL,α) ，由式(1)和式(4)可得P在(OKXK YKZK)下的坐標(biāo)為：

利用第3節(jié)的方法，由(xK,yK,zK)得到P對應(yīng)虛擬激光雷達(dá)測距數(shù)據(jù)d。于是，可得到融合后第i束激光測距二元組為：

當(dāng)Kinect相機(jī)與二維激光雷達(dá)同時(shí)工作，按照上述方法得到所有融合后的測距二元組集合DF。一般情況下二維激光雷達(dá)的激光測量角度大于Kinect相機(jī)觀測角度，即 [βmin,βmax]? [αmin,αmax]，從而DF為：式中，Δβ表示Kinect相機(jī)的虛擬二維激光角增量；Δα代表激光雷達(dá)的角增量。深度圖像與二維激光雷達(dá)數(shù)據(jù)融合算法偽代碼為：

輸入：DL//原始激光掃描二元組集合

輸出：DF//融合Kinect數(shù)據(jù)的二元組集合

其中，F(xiàn)useRangeKinect()是主框架；GetNextElement()為從原始激光掃描點(diǎn)集LD返回下一個(gè)二元組；LaserTOKinect()實(shí)現(xiàn)式(10)；Insert()向融合激光點(diǎn)集FD中添加一個(gè)二元組。

5 實(shí)驗(yàn)測試

為驗(yàn)證本文方法正確有效，在圖1的機(jī)器人平臺(tái)上實(shí)現(xiàn)了上述環(huán)境障礙檢測算法：主控計(jì)算機(jī)處理器為酷睿六代i7，主頻為2.5 GHz，操作系統(tǒng)采用Ubuntu并運(yùn)行了ROS系統(tǒng)；二維激光雷達(dá)為Hokuyo UTM-30LX，最大測距值30 m，角分辨率是0.25°，測距精度能達(dá)到30 mm，最大掃描角度是270°。

圖7給出了本文方法的測試結(jié)果，圖7a為Kinect相機(jī)采集的環(huán)境RGB圖像，圖7b為相機(jī)采集的深度圖像。本文通過4組測試對比二維激光雷達(dá)、Kinect相機(jī)和本文方法的障礙物檢測結(jié)果。

1) 第1組測試：圖7c表示h=50的測量區(qū)域，此時(shí)轉(zhuǎn)換窗口很小，只能檢測到桌面上的物品；圖7d表示h=50的障礙物邊緣比較圖，其中點(diǎn)劃線為激光雷達(dá)檢測到的障礙物邊緣(來自LD)，點(diǎn)線為Kinect相機(jī)檢測到的障礙物邊緣，細(xì)實(shí)線為融合得到的障礙物邊緣(來自于FD)。對比可發(fā)現(xiàn)，因?yàn)镵inect相機(jī)檢測角度小于激光雷達(dá)，其檢測出的障礙物邊緣范圍也小于激光雷達(dá)和融合方法；因?yàn)檗D(zhuǎn)換窗口很小，融合方法跟激光雷達(dá)一樣沒有發(fā)現(xiàn)桌子邊沿、垃圾桶和紙箱。

2) 第2組測試：圖7e表示h=100的測量區(qū)域，轉(zhuǎn)換窗口高度有所增大，檢測到了桌面弧形邊沿，圖7f表示h=100的障礙物邊緣比較圖。對比可發(fā)現(xiàn)，隨著轉(zhuǎn)換窗口增大，融合方法和Kinect相機(jī)可發(fā)現(xiàn)桌子邊沿，但激光雷達(dá)依然不行。

3) 第3組測試：圖7g表示h=180的測量區(qū)域，轉(zhuǎn)換窗口高度繼續(xù)增大，檢測到紙箱；圖7h表示h=180障礙物邊緣比較圖。對比發(fā)現(xiàn)，隨著窗口繼續(xù)增大，融合方法和Kinect相機(jī)可發(fā)現(xiàn)地面紙箱，相比Kinect，融合方法還能檢測更寬的范圍。

圖7 障礙物檢測實(shí)驗(yàn)

4) 第4組測試：圖7i表示h=240的測量區(qū)域，轉(zhuǎn)換窗口高度進(jìn)一步增大，進(jìn)而檢測到地面垃圾桶；圖7j表示h=240的障礙物邊緣比較圖。對比可發(fā)現(xiàn)，隨著轉(zhuǎn)換窗口進(jìn)一步增大，融合方法和Kinect相機(jī)可發(fā)現(xiàn)地面紙簍，相比Kinect相機(jī)，融合方法還能檢測更寬的范圍。

由4次實(shí)驗(yàn)對比可見，本文的融合方法不僅能更準(zhǔn)確地檢測到環(huán)境中的障礙物，而且還具有較寬的檢測范圍，是一種成本較低的三維障礙檢測方法，適合移動(dòng)機(jī)器人在局部區(qū)域移動(dòng)時(shí)避障。

6 結(jié)束語

針對現(xiàn)有視覺傳感器探測障礙物時(shí)易受環(huán)境影響、檢測精度低的問題，本文提出了一種結(jié)合Kinect相機(jī)和二維激光雷達(dá)判斷障礙物的方法，具備了二維激光雷達(dá)平面探測范圍廣、精度高、實(shí)時(shí)性好的特點(diǎn)，同時(shí)兼有Kinect相機(jī)廉價(jià)的三維測量優(yōu)勢。在改造的Turtlebot機(jī)器人平臺(tái)上，驗(yàn)證了該方法的正確性和有效性。

[1]MANSOUR S M B, SUNDARAPANDIAN V, NACEUR S M. Design and control with improved predictive algorithm for obstacles detection for two wheeled mobile robot navigation[J]. International Journal of Control Theory &Applications, 2016, 9(38)： 37-54.

[2]彭夢. 基于多傳感器融合的移動(dòng)機(jī)器人障礙物檢測研究[D]. 長沙： 中南大學(xué), 2007.PENG Meng. Research on obstacle detection of mobile robots based on multi-sensor information fusion[D].Changsha： Central South Uuniversity, 2007.

[3]鄒斌, 劉康, 王科未. 基于三維激光雷達(dá)的動(dòng)態(tài)障礙物檢測和追蹤方法[J]. 汽車技術(shù), 2017(8)： 19-25.ZOU Bin, LIU Kang, WANG Ke-wei. Dynamic obstacle detection and tracking method based on 3D lidar[J].Automobile Technology, 2017(8)： 19-25.

[4]KELLER C G, ENZWEILER M, ROHRBACH M, et al. The benefits of dense stereo for pedestrian detection[J]. IEEE Transactions on Intelligent Transportation Systems, 2011,12(4)： 1096-1106.

[5]YU Q, ARAUJO H, WANG H. A stereovision method for obstacle detection and tracking in non-flat urban environments[J]. Autonomous Robots, 2005, 19(2)：141-157.

[6]李啟東. 基于單目視覺的機(jī)器人動(dòng)態(tài)障礙物檢測與壁障方法研究[D]. 長春： 吉林大學(xué), 2016.LI Qi-dong. Research on robot dynamic obstacle detection and obstacle avoidance method based on monocular vision[D]. Changchun： Jilin University, 2016.

[7]馮瑾. 基于雙目立體視覺的移動(dòng)機(jī)器人障礙物檢測技術(shù)研究[D]. 徐州： 中國礦業(yè)大學(xué), 2015.FENG Jin. Research on technology of mobile robot obstacle detection based on binocular stereo vision[D]. Xuzhou：China University of Mining and Technology, 2015.

[8]MANE S B, VHANALE S. Real time obstacle detection for mobile robot navigation using stereo vision[C]//Proceedings of the International Conference on Computing, Analytics and Security Trends. [S.l.]： IEEE, 2016： 101-105.

[9]CHEN X, REN W, LIU M, et al. An obstacle detection system for a mobile robot based on radar-vision fusion[C]//Proceedings of the 4th International Conference on Computer Engineering and Networks. [S.l.]： Springer International Publishing, 2015： 677-685.

[10]張雙喜. 基于雷達(dá)與相機(jī)的無人駕駛智能車障礙物檢測技術(shù)研究[D]. 西安： 長安大學(xué), 2013.ZHANG Shuang-xi. Research on obstacle detection technology based on radar and camera of driverless smart vehicles[D]. Xi’an： Chang’an University, 2013.

[11]汪佩, 郭劍輝, 李倫波, 等. 基于單線激光雷達(dá)與視覺融合的負(fù)障礙檢測算法[J]. 計(jì)算機(jī)工程, 2017, 43(7)：303-308.WANG Pei, GUO Jian-hui, LI Lun-bo, et al. Negative obstacle detection algorithm based on single line laser radar and vision fusion[J]. Computer Engineering, 2017,43(7)： 303-308.

[12]薛彥濤, 呂洪波, 孫啟國. 基于Kinect深度數(shù)據(jù)的移動(dòng)機(jī)器人障礙檢測方法研究[J]. 計(jì)算機(jī)測量與控制, 2017,25(2)： 61-63.XUE Yan-tao, Lü Hong-bo, SUN Qi-guo. Mobile robot obstacle detection method based on depth data of Kinect[J]. Computer Measurement & Control, 2017, 25(2)：61-63.