基于視覺和激光傳感器信息融合的樓梯結構參數估計*

李艷杰, 劉 宸, 眭 晉

0 引 言

目前樓梯識別方法主要分為基于視覺傳感器的樓梯識別方法和基于激光傳感器的樓梯識別方法。Se S等人采用基于單目視覺傳感器的方法，對圖像進行Gabor濾波，進而用消失點約束來尋找階梯邊緣特征直線，最后利用單一性搜索方法估計樓梯偏移角。Lu X等人采用了融合立體視覺提取的三維邊緣點信息和單目視覺提取的圖像邊緣線的方法，獲得了樓梯的結構參數，但精度有限，識別效果受環境影響比較大。與攝像頭對比，激光測距儀有測量精度高、受光照條件影響小、測量范圍大等優點，在樓梯參數估計方面被大量使用。Theeravithayangkura C等人采取將單線激光雷達進行旋轉的方式獲得稠密的三維點云，再將三維點云逐列提取線段并將線段聚合后提取樓梯的豎直平面，最后依據提取出的平行的豎直平面來構建樓梯模型,但識別效果非常依賴提取線段的斷點位置，如果樓梯比較老舊、形狀不夠規則，無法保證成功構建樓梯模型。O?wald S等人通過單線激光俯仰的方式獲得三維點云，采用改進的線段分組方法進行平面提取，進而估計樓梯結構參數。該方法依然對于樓梯有較高的要求，只有樓梯平面比較規則時才會有較好的效果。

本文提出了一種新的樓梯結構參數估計方法,該方法精度較高、通用性強、對樓梯形狀是否規則沒有過高要求，可以通過后期校正處理光照等影響。

1 樓梯結構參數確定

1.1 技術方案

本文根據室內樓梯環境特點，考慮其他應用(如自主避障)需求，以及前人經驗，應用Pointgrey FL3-GE-03S2C-C工業千兆以太網相機和Velodyne VLP-16三維激光雷達組成的測距系統采集樓梯地形環境信息。工業相機采集的數據為分辨率為640×480的彩色圖片；激光雷達采集的數據為雷達坐標系下的坐標值。執行攀爬任務前，機器人位于樓梯不遠處且大致正對樓梯。圖1(a)為使用工業相機采集的樓梯模型圖像數據。圖1(b)為激光雷達采集圖像的俯視圖，其中緊密排列的矩形即為樓梯，特征明顯，且容易用數學方法表示和計算相關參數。

圖1 實驗用樓梯及點云數據

如圖2所示為技術方案。

圖2 技術方案

定義機器人正前方為Y正軸，正右方為X正軸，正上方為Z正軸，將激光點云數據變換到機器人坐標系下表示。

1.2 臺階深度與寬度確定

將圖1(b)點云數據投影到XY平面，發現在激光雷達位置高于臺階時，有相當大的概率會使激光點落到臺階水平面上，給直線的提取帶來問題，所以，保留激光雷達坐標系下Z>0的激光點。

為了利用霍夫變換算法，本文將激光點云俯視圖處理為二值圖像。處理時要記錄下每一個像素與點云數據的對應關系，目的是利用從圖像中提取出的特征線段坐標選出落到臺階立面上的點云數據。

利用霍夫變換提取直線。提取出樓梯特征直線后，首先區分樓梯臺階直線和扶手直線。

如果機器人與樓梯呈一個較小的角度，為了能夠融合視覺和激光傳感器進行識別，必須要調整姿態到近似正對樓梯。此時，扶手直線與臺階直線的斜率乘積近似為-1，則提取出的直線可用斜率的正負區分為2組。根據直線幾何關系挑選出特征直線，進而估算出機器人相對樓梯的位置，并調整機器人位姿，使其正對樓梯。

當機器人正對樓梯，將提取出的直線根據斜率絕對值大小分為2組，各組結合斜率、相鄰直線間距離等信息去除干擾直線并確定特征直線。挑選出與各直線對應的點云，根據各直線對應的點云在機器人坐標系中的坐標，即可計算出樓梯的寬度和深度。

1.3 臺階高度確定

通過相機與激光雷達進行聯合標定，確定了點云坐標系與圖像坐標系的映射關系，通過計算獲得激光點云映射到圖像坐標系中的坐標，點云直線的實際距離可用激光點云數據計算得出，臺階高度根據映射到圖像中的點云直線距離與臺階邊緣直線距離的比例關系計算，如圖3所示。

圖3 點云直線與臺階邊緣直線比例關系示意

圖4 臺階點云三維圖

1.4 臺階高度校正

由于基于圖像信息進行結構參數確定的效果受光照條件、邊緣和特征提取效果等因素影響較大，并不一定能夠準確估計出結構參數，所以，在攀爬樓梯過程中要對臺階高度進行校正。本文采用了GNSS組合慣導系統NV-GI310實時獲得機器人的運動信息。該傳感器在室外和室內條件可保持高精度數據輸出。利用機器人的俯仰角，根據直角三角形的性質進行臺階高度校正。

機器人在逐漸由擺臂引導到樓梯上到完全脫離地面的時間內，俯仰角隨時間增加，直到完全脫離地面，俯仰角趨于穩定，如圖5所示(圖中由于傳感器安裝問題，機器人俯仰角為傳感器測量值的相反數。橫坐標長度不隨著時間的增加而增加，只顯示固定時間內的俯仰角變化情況。下圖為上圖的延續，其中,上圖橫坐標為42 s處的點與下圖橫坐標為8 s的點為同一點)。

圖5 俯仰角變化曲線

如圖6所示，W為采用文中方法獲得的臺階寬度，α為機器人攀爬過程中的俯仰角增量，臺階高度h為

h=W·tanα

(1)

圖6 臺階高度校正幾何示意

此樓梯所有結構參數都確定完畢，攀爬過程中可根據參數自主進行構型優化，改變重心，增加攀爬穩定性。

2 試驗結果與誤差分析

在如圖7所示無人平臺上進行了測試，傳感器系統包括Pointgrey FL3-GE-03S2C-C工業千兆以太網相機、Velodyne VLP—16三維激光雷達和NV-GI310組合慣導系統，搭載平臺為中科院沈陽自動化研究所研發的“單人攜帶型”履腿復合式機器人。該系統中激光雷達共有16個激光束，垂直分辨率為0.2°，水平分辨率為0.1°～0.4°，最遠探測距離為100 m并且能保證典型距離下誤差在±3 cm以內。相機分辨率為640×480，鏡頭為焦距4 mm的工業低畸變鏡頭。組合慣導系統水平姿態精度保持在0.05°以內。對前文介紹過的試驗樓梯進行了不同距離的場景下進行了測試。

圖7 試驗無人平臺

圖8(a)為經過圖像處理后，提取出的特征直線。圖8(b)為點云直線映射到可見光圖像中。

圖8 數據處理過程

表1 為機器人正對樓梯，在不同距離下的測試結果。

表1 不同距離下的結構參數測量結果及誤差 m

由于機器人自身結構問題，距離樓梯距離需大于等于0.4 m。可以看出，扶手距離平均誤差為0.023 18 m，最大誤差為0.035 6 m；臺階寬度平均誤差為0.017 71 m，最大誤差為0.032 2 m；臺階高度平均誤差為0.018 78 m，最大誤差為0.021 8 m。表中表明的扶手距離和臺階寬度的測量值普遍大于實際值(臺階寬度為0.281 0 m；臺階高度為0.162 3 m；樓梯扶手距離為0.673 2 m)，而不是在實際值附近上下擺動，經分析認為是由樓梯模型平面度不夠或激光雷達測量誤差引起的，如圖8(a)所示，點云圖像提取出的邊緣是一個包圍住了特征直線的輪廓，在此基礎上進行特征提取就只能在輪廓上進行，這必然會帶來誤差，誤差的大小受模型平面度和激光測量誤差影響；而臺階高度測量值則普遍小于實際值的情況，經分析，主要原因是模型中臺階邊緣靠下約0.02 m處有一個由不同木板貼合出現的規則的直線邊緣，造成了偽樓梯邊緣的產生。

由于臺階高度、寬度精度在0.02 m左右，扶手距離精度在0.023 m左右，可以滿足履腿復合式機器人自主攀爬樓梯需要的結構參數精度要求。

3 結 論

本文提出了一種基于視覺和激光傳感器的樓梯目標檢測與結構參數確定方法,能夠可靠、快速地識別樓梯目標并獲得樓梯的結構參數，為履帶式機器人自主安全通過樓梯提供了有效參考。方法中結合傳感器系統采用了將三維點云作為圖像的處理方法，有一定創新性。

參考文獻:

[1] Se S,Brady M.Vision-based detection of staircases[C]∥Proceedings of the Fourth Asian Conference on Computer Vision,Taipei:Springer,2000:535-540.

[2] Lu X,Manduchi R.Detection and localization of curbs and stairways using stereo vision[C]∥Proceedings of the 2005 IEEE International Conference on Robotics and Automation,Barcelona,Spain:IEEE,2005:4648-4654.

[3] 朱萬彬，鐘 俊，莫仁蕓.激光位移傳感器在角度測量中的應用[J].傳感器與微系統，2010,29(6)131-133.

[4] 曹 鵬，尹文慶，吳林華，等.基于激光傳感器的稻種輪廓形狀測量方法研究[J].傳感器與微系統，2017,36(3)：29-32.

[5] Theeravithayangkura C,Takubo T,Mae Y,et al.Stair recognition of with laser range scanning by limb mechanism robot ‘ASTERISK’[C]∥Proceedings of IEEE International Conference on Robotics and biomimetics,Bangkok,Thailand:IEEE,2009:915-920.

[6] O?wald S,Gutmann J S,Hornung A,et al.From 3D point clouds to climbing stairs: A comparision of plane segmentation approaches for humanoids[C]∥The 11th IEEE/RAS International Conference on Humanoid Robots,Bled,Slovenia:IEEE,2011:93-98.