基于激光傳感器構建環境拓撲地圖*

蘇麗穎，宋華磊

（北京工業大學機電學院，北京 100124）

0 引言

地圖構建是移動機器人在未知環境中實現導航任務的基礎。移動機器人導航領域中常用的平面環境模型主要有柵格地圖和拓撲地圖2種。柵格地圖易于創建和維護，但需要巨大的存儲空間和計算量，難以滿足實時性的要求;拓撲地圖忽略大量的環境細節，將環境表示為一個僅由線和節點組成的圖，節點表示環境的特征狀態、地點等，連接對應其間的關系。拓撲地圖能緊湊地表示環境，并且所需要的存儲空間和計算量小，有利于進行有效的全局路徑規劃。

構建拓撲地圖方法的不同主要體現在如何構成節點、如何識別節點上。針對不同的傳感器，國內外學者提出很多不同的構建拓撲地圖的方法。針對利用機器視覺采集的圖像構建基于外觀的拓撲地圖問題，Booij O等人［1］利用對極幾何和平面約束計算必要的方向信息，使機器人系統具有較強的魯棒性;Werner F，Sitte J等人［2］利用連續圖像視覺外觀的連續性，對環境顏色的直方圖加以聚類，每一類作為一個拓撲節點，構成拓撲地圖;Bacca B等人［3］在每個拓撲節點上建立特征穩定性直方圖（FSH），FHS寄存著由投票機制決定的局部特征穩定性，最穩定的特征用來構建地圖并進行定位。Goedeme T［4］通過檢測 visual loop closing，建立拓撲地圖;聯合采用視覺比較技術和基于D-S推理概率理論的證據收集方法解決visual loop closing難以檢測的問題。在利用距離傳感器構建柵格地圖后，抽取拓撲結構的問題上，Beeson P N，Jong K 和 Kuipers B［4］運用位置自動檢測算法，實時計算局部柵格地圖自由空間的Voronoi圖實現拓撲地圖的構建，并采用拓展Voronoi圖（EVG）實現從走廊中線到局部區域骨架無縫過度;Kwon T與Song J［5］運用細化算法由局部柵格地圖自動生成拓撲地圖;Iser R和Wahl F M［6］針對SLAM問題，將建立的局部柵格地圖融合成一個圖形結構，從而生成拓撲地圖;Cheong H等人［7］利用激光傳感器采集環境幾何特征數據，通過相關圖像處理算法抽取骨架，從分支點中決定節點，構建混合地圖?；诎脊濣c的概念，提出一種有效算法，解決目標節點的選擇問題。Shi Chaoxia和Wang Yanqing等人［8］綜合利用激光傳感器和視覺傳感器采用位置識別取代計算全局位姿進行定位，結合反應式導航策略:BCM，實時在線構建環境拓撲地圖。另外，Lin Weiguo等人針對利用ID標簽和網絡攝像機導航問題，引入含有node IDs和方向角的拓撲地圖表示環境。

針對未知環境拓撲特征提取問題，首先利用德國SICK激光傳感器LMS100掃描機器人所處的環境，并構建環境地圖。在構建的幾何地圖基礎上，利用中線法抽取地圖環境自由空間的中線，實現拓撲地圖的構建。該方法構建的地圖具有緊湊性、為有效進行路徑規劃提供必要的保障。

1 基于幾何地圖自由空間中線法構建拓撲地圖

利用激光傳感器掃描的數據構建環境幾何地圖，將激光傳感器的掃描區域劃分為4個區域，由提出的自由空間中線法提取環境的拓撲結構，并將其作為拓撲地圖。

1．1 激光傳感器工作原理

激光傳感器的工作原理見圖1，激光發射器發射的激光脈沖經過分光鏡后，分為2路，一路進入接收器;另一路則由反射鏡面反射到被測障礙物表面，障礙物表面反射光，再經過反射鏡返回接收器。通過反射鏡的旋轉，激光傳感器得以在一個角度范圍內獲得掃描的扇形區域。由于發射光與反射光的頻率完全相同，故通過測量發射脈沖與反射脈沖之間的時間間隔并與光速的乘積來測定被測障礙物體與激光傳感器的距離。

圖1 激光傳感器工作原理Fig 1 Working principle of laser sensor

1．2 距離傳感器構建地圖

移動機器人的運動學模型如圖2所示。計算單位采樣時間（ΔT）內車體內位姿的變化，進行累加，求出車體在世界坐標系中的位姿

其中，xr（k），yr（k），θr（k）分別為機器人在k時刻的坐標和方向，Δd（k）為根據光電編碼器測量的相鄰采樣時間間隔的相對位移增量，Δ θ（k）為根據陀螺儀測量的相鄰采樣時間間隔內相對偏移角度。若D為機器人車輪直徑，減速器的減速比為1/P，編碼器的精度為η，單位采樣時間ΔT內光電編碼器輸出的脈沖數為N，陀螺儀測量的偏轉角速度為ω，則

圖2 移動機器人的運動模型Fig 2 Motion model of mobile robot

機器人用激光掃描周圍環境，采用極坐標（ρn，φn）表示一次掃描的所有距離信息，極點位于掃描中心，極軸為激光的主掃描方向（0°），n為掃描到的第n個障礙物。

將激光測量的距離信息映射到世界坐標中

其中，xr，yr，θr為機器人參考中心在世界坐標中的位置。

1．3 中線法提取自由空間中線

在由激光傳感器構建的環境地圖上，提出中線法提取環境自由空間的骨架，并以其為環境的拓撲地圖。

記激光傳感器單次掃描的數據為Z，所得的數據為從環境邊界到傳感器的距離值。將機器人單次掃描數據以機器人為掃描中心分別分為左右兩側的掃描數據和前方掃描數據3 個部分，分別表示為Zleft，Zright和Zfront，即有

高中物理演示實驗教學時，教師應多傾聽學生的意見，對相關實驗環節進行優化，不斷激發學生聽課的熱情，尤其應鼓勵學生參與到實驗中來，使其切身感受物理規律，真正從實驗中有所學、有所獲，加深學生印象的同時，保證演示實驗教學的有效性.

將前方掃描數據Zfront以機器人的運動方向分為左右兩組測量數據:Zfront=［L，R］。其中，L，R分別為Zfront中的左前方掃描數據和右前方掃描數據

分別取L，R中測量的環境邊界到機器人的最小距離Df_l，Df_r為

從而機器人所測量兩側最短距離的中點

其中，Dl和Dr分別為環境邊界距機器人最短距離的測量最小距離。

機器人移動過程中，連續掃描得到的左右兩側環境邊界到機器人最短距離點連線的中點Midpt點之間的連線將形成環境自由空間的中線，并將其作為環境的拓撲地圖的弧線。以上算法在室內走廊環境下總是有效的。

1．4 節點的構建

將抽取的中線的分支點和拐道處作為環境拓撲地圖的拓撲節點，從而完成環境自由空間拓撲結構的提取，構成拓撲地圖。通常情況下，中線分支發生在走廊與房間的連接處，或走廊之間的連接處（拐道）。取機器人左側掃描區域測量數據Zleft，右側掃描區域測量數據Zright

若機器人左前方掃描數據L和左側掃描數據Zleft中分別存在2個測量點，使Dl=Df_l，則該兩區域中相應測量點連線中點Midpt為中線分支點，并將其作為拓撲節點

其中，i和j分別為測量的左側和左前方距機器人最短距離相等的環境測量點的序號。

同理，若機器人右前方掃描數據R和右側掃描數據Zright中分別存在2個測量點使Dr=Df_r，則該兩區域中相應測量點連線中點Midpt為中線分支點，將其作為拓撲節點。為了便于節點的識別，將中線分支點處的部分度量信息如機器人朝向、位置坐標，拓撲節點的絕對坐標（θrctop）加入到拓撲節點。

2 實驗

2．1 激光構建環境幾何地圖

采用德國SICK激光傳感器LMS100。最大測量范圍為20 m，掃描精度為0．5°，最大掃描角度范圍270°，掃描周期為20 ms。傳感器采用以太網接口，傳輸速率10/100 Mbit。移動機器人采用美國Pioneer 3—DX機器人。

移動機器人運行的環境是北京工業大學基礎樓機器人實驗室。

實驗采用LMS100激光傳感器掃描環境，利用掃描障礙物邊界所采集的數據構建環境障礙物邊界的幾何地圖，如圖3所示。

2．2 中線法提取環境地圖拓撲圖

環境的拓撲地圖形式有多種多樣。由于室內環境的特征容易提取，有的學者將每個房間作為一個節點，聯通每個房間的通道作為連接節點的弧線;也有的抽取自由空間的骨架，例如:Voronoi圖，作為環境的拓撲地圖。在構建環境幾何地圖的基礎上，抽取地圖中可行區域（自由空間）的中線作為拓撲圖的連接線，各個可行區域中線的交點作為節點，構成拓撲地圖。構建的拓撲圖如圖4所示。

圖4 幾何地圖與抽取的拓撲圖Fig 4 Diagram of geometric map and extracted topology

該算法構建的拓撲圖拓撲節點1處保存了機器人位置坐標和朝向，其中，節點1為（105°2950 2560）;節點2為（60°945 2795）。導航過程中，移動機器人沿著中線移動，并在節點處附加（θrctop）度量特征信息，便于移動機器人在節點位置進行節點識別。采用該種方法構建環境拓撲地圖，計算量小，實時性好。

3 結論

利用SICK LMS100激光傳感器掃描環境，并構建幾何地圖。在處理激光傳感器掃描數據時，掃描數據以機器人為中心，劃分為左、左前、右、右前4個部分。根據提出的基于幾何地圖自由空間中線法提取出環境的拓撲結構，將中線的分支點作為拓撲圖的節點，并在節點處加入機器人的位置、朝向等度量信息以便于節點的識別。經實驗證明:該方法能快速有效地完成環境拓撲地圖的構建，并能緊湊方便地表示環境。

在節點處雖然加入節點位置坐標、機器人朝向等少量的度量信息，但仍然會出現識別錯誤的情況。另外，由于提出的中線法是基于室內結構化環境下提出的，并不適應一般的自然環境。

［1］ Booij O，Terwijn B，Zivkovic Z，et al．Navigation using an appearance based topological map［C］∥2007 IEEE International Conference on Robotics and Automation，ICRA’07，Rome，Italy，2007:3927－3932．

［2］ Felix W，Joaquin S，Frederic M．Visual topological mapping and localisation using colour histograms［C］∥2008．the 10th International Conference on Control，Automation，Robotics and Vision，ICARCV 2008，Hanoi，Vietnam:IEEE Computer Society，2008:341－346．

［3］ Bacca B，Salvi J，Batlle J，et al．Appearance-based mapping and localization using feature stability histograms［J］．Electronics Letters，2010，46（16）:1120 －1121．

［4］ Beeson P，Jong N K，Kuipers B．Towards autonomous topological place detection using the extended voronoi graph［C］∥2005 IEEE International Conference on Robotics and Automation，Barcelona，Spain，2005:4373 －4379．

［5］ Kwon Tae-bum，Song Jae-bok．Thinning-based topological exploration using position possibility of topological nodes［J］．Advanced Robotics，2008，22（2 －3）:339 －359．

［6］ Iser R，Wahl F M．Building local metrical and global topological maps using efficient scan matching approaches［C］∥2008 IEEE/RSJ International Conference on Intelligent Robots and Systems，Piscataway，NJ，USA，IEEE，2008:1023 －1030．

［7］ Cheong H，Park S，Park Sung-kee．Topological map building and exploration based on concave nodes［C］∥2008 IEEE International Conference on Control，Automation and Systems（ICCAS），Piscataway，NJ，USA，2008:1115 －1120．

［8］ Shi Chaoxia，Wang Yanqing，Yang Jingyu．Online topological map building and qualitative localization in large-scale environment［J］．Robotics and Autonomous Systems，2010，58（5）:488 －496．

［9］ Lin Weiguo，Jia S，Yang Fei，et al．Topological navigation of mobile robot using ID tag and Web camera．Proceedings［C］∥2004 IEEE International Conference on Intelligent Mechatronics and Automation，Piscataway，NJ，USA，2004:644 －649．

［10］ Toon G，Tinne T，Luc V G．Visual topological map building in self-similar environments［C］∥The 3rd International Conference on Informatics in Control，Automation and Robotics，ICINCO 2006，Setubal，Portugal，2006:341 － 346．