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

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

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

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

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

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

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

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

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

2 二維激光雷達與Kinect相機聯(lián)合標定

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

圖3 激光雷達與Kinect相機坐標系

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

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

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

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

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

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

2.3 激光雷達坐標與像素點三元組的關(guān)系

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

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

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

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

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

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

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

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

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

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

圖5 Kinect相機圖像

圖6 Kincet相機的虛擬激光測距

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

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

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

3) 二維激光點的計算。

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

4) 計算A到相機原點KO的距離為：

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

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

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

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

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

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

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

5 實驗測試

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

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

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

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

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

圖7 障礙物檢測實驗

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

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

6 結(jié)束語

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

[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]. 長沙： 中南大學, 2007.PENG Meng. Research on obstacle detection of mobile robots based on multi-sensor information fusion[D].Changsha： Central South Uuniversity, 2007.

[3]鄒斌, 劉康, 王科未. 基于三維激光雷達的動態(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]李啟東. 基于單目視覺的機器人動態(tài)障礙物檢測與壁障方法研究[D]. 長春： 吉林大學, 2016.LI Qi-dong. Research on robot dynamic obstacle detection and obstacle avoidance method based on monocular vision[D]. Changchun： Jilin University, 2016.

[7]馮瑾. 基于雙目立體視覺的移動機器人障礙物檢測技術(shù)研究[D]. 徐州： 中國礦業(yè)大學, 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]張雙喜. 基于雷達與相機的無人駕駛智能車障礙物檢測技術(shù)研究[D]. 西安： 長安大學, 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]汪佩, 郭劍輝, 李倫波, 等. 基于單線激光雷達與視覺融合的負障礙檢測算法[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ù)的移動機器人障礙檢測方法研究[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.