蘇麗穎，宋華磊

（北京工業(yè)大學(xué)機(jī)電學(xué)院，北京 100124）

0 引言

地圖構(gòu)建是移動(dòng)機(jī)器人在未知環(huán)境中實(shí)現(xiàn)導(dǎo)航任務(wù)的基礎(chǔ)。移動(dòng)機(jī)器人導(dǎo)航領(lǐng)域中常用的平面環(huán)境模型主要有柵格地圖和拓?fù)涞貓D2種。柵格地圖易于創(chuàng)建和維護(hù)，但需要巨大的存儲(chǔ)空間和計(jì)算量，難以滿足實(shí)時(shí)性的要求;拓?fù)涞貓D忽略大量的環(huán)境細(xì)節(jié)，將環(huán)境表示為一個(gè)僅由線和節(jié)點(diǎn)組成的圖，節(jié)點(diǎn)表示環(huán)境的特征狀態(tài)、地點(diǎn)等，連接對應(yīng)其間的關(guān)系。拓?fù)涞貓D能緊湊地表示環(huán)境，并且所需要的存儲(chǔ)空間和計(jì)算量小，有利于進(jìn)行有效的全局路徑規(guī)劃。

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

針對未知環(huán)境拓?fù)涮卣魈崛栴}，首先利用德國SICK激光傳感器LMS100掃描機(jī)器人所處的環(huán)境，并構(gòu)建環(huán)境地圖。在構(gòu)建的幾何地圖基礎(chǔ)上，利用中線法抽取地圖環(huán)境自由空間的中線，實(shí)現(xiàn)拓?fù)涞貓D的構(gòu)建。該方法構(gòu)建的地圖具有緊湊性、為有效進(jìn)行路徑規(guī)劃提供必要的保障。

1 基于幾何地圖自由空間中線法構(gòu)建拓?fù)涞貓D

利用激光傳感器掃描的數(shù)據(jù)構(gòu)建環(huán)境幾何地圖，將激光傳感器的掃描區(qū)域劃分為4個(gè)區(qū)域，由提出的自由空間中線法提取環(huán)境的拓?fù)浣Y(jié)構(gòu)，并將其作為拓?fù)涞貓D。

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

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

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

1．2 距離傳感器構(gòu)建地圖

移動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)模型如圖2所示。計(jì)算單位采樣時(shí)間（ΔT）內(nèi)車體內(nèi)位姿的變化，進(jìn)行累加，求出車體在世界坐標(biāo)系中的位姿

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

圖2 移動(dòng)機(jī)器人的運(yùn)動(dòng)模型Fig 2 Motion model of mobile robot

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

將激光測量的距離信息映射到世界坐標(biāo)中

其中，xr，yr，θr為機(jī)器人參考中心在世界坐標(biāo)中的位置。

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

在由激光傳感器構(gòu)建的環(huán)境地圖上，提出中線法提取環(huán)境自由空間的骨架，并以其為環(huán)境的拓?fù)涞貓D。

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

高中物理演示實(shí)驗(yàn)教學(xué)時(shí)，教師應(yīng)多傾聽學(xué)生的意見，對相關(guān)實(shí)驗(yàn)環(huán)節(jié)進(jìn)行優(yōu)化，不斷激發(fā)學(xué)生聽課的熱情，尤其應(yīng)鼓勵(lì)學(xué)生參與到實(shí)驗(yàn)中來，使其切身感受物理規(guī)律，真正從實(shí)驗(yàn)中有所學(xué)、有所獲，加深學(xué)生印象的同時(shí)，保證演示實(shí)驗(yàn)教學(xué)的有效性.

將前方掃描數(shù)據(jù)Zfront以機(jī)器人的運(yùn)動(dòng)方向分為左右兩組測量數(shù)據(jù):Zfront=［L，R］。其中，L，R分別為Zfront中的左前方掃描數(shù)據(jù)和右前方掃描數(shù)據(jù)

分別取L，R中測量的環(huán)境邊界到機(jī)器人的最小距離Df_l，Df_r為

從而機(jī)器人所測量兩側(cè)最短距離的中點(diǎn)

其中，Dl和Dr分別為環(huán)境邊界距機(jī)器人最短距離的測量最小距離。

機(jī)器人移動(dòng)過程中，連續(xù)掃描得到的左右兩側(cè)環(huán)境邊界到機(jī)器人最短距離點(diǎn)連線的中點(diǎn)Midpt點(diǎn)之間的連線將形成環(huán)境自由空間的中線，并將其作為環(huán)境的拓?fù)涞貓D的弧線。以上算法在室內(nèi)走廊環(huán)境下總是有效的。

1．4 節(jié)點(diǎn)的構(gòu)建

將抽取的中線的分支點(diǎn)和拐道處作為環(huán)境拓?fù)涞貓D的拓?fù)涔?jié)點(diǎn)，從而完成環(huán)境自由空間拓?fù)浣Y(jié)構(gòu)的提取，構(gòu)成拓?fù)涞貓D。通常情況下，中線分支發(fā)生在走廊與房間的連接處，或走廊之間的連接處（拐道）。取機(jī)器人左側(cè)掃描區(qū)域測量數(shù)據(jù)Zleft，右側(cè)掃描區(qū)域測量數(shù)據(jù)Zright

若機(jī)器人左前方掃描數(shù)據(jù)L和左側(cè)掃描數(shù)據(jù)Zleft中分別存在2個(gè)測量點(diǎn)，使Dl=Df_l，則該兩區(qū)域中相應(yīng)測量點(diǎn)連線中點(diǎn)Midpt為中線分支點(diǎn)，并將其作為拓?fù)涔?jié)點(diǎn)

其中，i和j分別為測量的左側(cè)和左前方距機(jī)器人最短距離相等的環(huán)境測量點(diǎn)的序號(hào)。

同理，若機(jī)器人右前方掃描數(shù)據(jù)R和右側(cè)掃描數(shù)據(jù)Zright中分別存在2個(gè)測量點(diǎn)使Dr=Df_r，則該兩區(qū)域中相應(yīng)測量點(diǎn)連線中點(diǎn)Midpt為中線分支點(diǎn)，將其作為拓?fù)涔?jié)點(diǎn)。為了便于節(jié)點(diǎn)的識(shí)別，將中線分支點(diǎn)處的部分度量信息如機(jī)器人朝向、位置坐標(biāo)，拓?fù)涔?jié)點(diǎn)的絕對坐標(biāo)（θrctop）加入到拓?fù)涔?jié)點(diǎn)。

2 實(shí)驗(yàn)

2．1 激光構(gòu)建環(huán)境幾何地圖

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

移動(dòng)機(jī)器人運(yùn)行的環(huán)境是北京工業(yè)大學(xué)基礎(chǔ)樓機(jī)器人實(shí)驗(yàn)室。

實(shí)驗(yàn)采用LMS100激光傳感器掃描環(huán)境，利用掃描障礙物邊界所采集的數(shù)據(jù)構(gòu)建環(huán)境障礙物邊界的幾何地圖，如圖3所示。

2．2 中線法提取環(huán)境地圖拓?fù)鋱D

環(huán)境的拓?fù)涞貓D形式有多種多樣。由于室內(nèi)環(huán)境的特征容易提取，有的學(xué)者將每個(gè)房間作為一個(gè)節(jié)點(diǎn)，聯(lián)通每個(gè)房間的通道作為連接節(jié)點(diǎn)的弧線;也有的抽取自由空間的骨架，例如:Voronoi圖，作為環(huán)境的拓?fù)涞貓D。在構(gòu)建環(huán)境幾何地圖的基礎(chǔ)上，抽取地圖中可行區(qū)域（自由空間）的中線作為拓?fù)鋱D的連接線，各個(gè)可行區(qū)域中線的交點(diǎn)作為節(jié)點(diǎn)，構(gòu)成拓?fù)涞貓D。構(gòu)建的拓?fù)鋱D如圖4所示。

圖4 幾何地圖與抽取的拓?fù)鋱DFig 4 Diagram of geometric map and extracted topology

該算法構(gòu)建的拓?fù)鋱D拓?fù)涔?jié)點(diǎn)1處保存了機(jī)器人位置坐標(biāo)和朝向，其中，節(jié)點(diǎn)1為（105°2950 2560）;節(jié)點(diǎn)2為（60°945 2795）。導(dǎo)航過程中，移動(dòng)機(jī)器人沿著中線移動(dòng)，并在節(jié)點(diǎn)處附加（θrctop）度量特征信息，便于移動(dòng)機(jī)器人在節(jié)點(diǎn)位置進(jìn)行節(jié)點(diǎn)識(shí)別。采用該種方法構(gòu)建環(huán)境拓?fù)涞貓D，計(jì)算量小，實(shí)時(shí)性好。

3 結(jié)論

利用SICK LMS100激光傳感器掃描環(huán)境，并構(gòu)建幾何地圖。在處理激光傳感器掃描數(shù)據(jù)時(shí)，掃描數(shù)據(jù)以機(jī)器人為中心，劃分為左、左前、右、右前4個(gè)部分。根據(jù)提出的基于幾何地圖自由空間中線法提取出環(huán)境的拓?fù)浣Y(jié)構(gòu)，將中線的分支點(diǎn)作為拓?fù)鋱D的節(jié)點(diǎn)，并在節(jié)點(diǎn)處加入機(jī)器人的位置、朝向等度量信息以便于節(jié)點(diǎn)的識(shí)別。經(jīng)實(shí)驗(yàn)證明:該方法能快速有效地完成環(huán)境拓?fù)涞貓D的構(gòu)建，并能緊湊方便地表示環(huán)境。

在節(jié)點(diǎn)處雖然加入節(jié)點(diǎn)位置坐標(biāo)、機(jī)器人朝向等少量的度量信息，但仍然會(huì)出現(xiàn)識(shí)別錯(cuò)誤的情況。另外，由于提出的中線法是基于室內(nèi)結(jié)構(gòu)化環(huán)境下提出的，并不適應(yīng)一般的自然環(huán)境。

［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，F(xiàn)rederic 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．