許國玉， 曹虎辰， 劉少剛

（哈爾濱工程大學(xué)機(jī)電工程學(xué)院，黑龍江 哈爾濱 150001）

目前，救援機(jī)器人地圖創(chuàng)建主要采用的是基于特征線段的SLAM算法，通過激光雷達(dá)掃描出的信息得到環(huán)境輪廓特征及機(jī)器人位置信息。所提取的環(huán)境特征主要是能夠反映大范圍環(huán)境的結(jié)構(gòu)化特征（如點(diǎn)、角和線段等），分別與凸角、墻角和墻邊等實(shí)際特征一一對應(yīng)?；谔卣鱏LAM算法發(fā)展迅速，主要是以點(diǎn)特征為研究對象。點(diǎn)特征的缺點(diǎn)是數(shù)量龐大、計(jì)算復(fù)雜，其算法本身存在一定的缺陷，盡管可以通過使用卡爾曼濾波或者路標(biāo)提取等附加方法來解決其問題，但仍存在不少問題[1-4]。

根據(jù)救援環(huán)境的特殊性可以發(fā)現(xiàn)，相比點(diǎn)特征的復(fù)雜特性，在大多數(shù)情況下直線特征更容易被提取和匹配。因此，研究提出了一種基于線段特征匹配的SLAM算法，將點(diǎn)和線的特征相融合，提高救援機(jī)器人的建圖性能。

1 激光雷達(dá)掃描中特征線段識別

特征識別過程主要分為兩個(gè)步驟，區(qū)域分割和特征提取。

1.1 初始區(qū)域分割

假設(shè)激光雷達(dá)可得到的數(shù)據(jù)點(diǎn)個(gè)數(shù)為n，并以其n個(gè)點(diǎn)為一個(gè)區(qū)域，那么這個(gè)區(qū)域可以表示A1{(Xi,Yi)|i=1,2…n}。利用兩點(diǎn)間歐式距離公式可求得相鄰兩點(diǎn)間距離Dj，同時(shí)確定一個(gè)閾值δ，通過判斷Dj和閾值δ的關(guān)系，確定相鄰的兩個(gè)點(diǎn)是否在一個(gè)直線特征集合范圍內(nèi)。

當(dāng)Dj大于δ時(shí)，確定點(diǎn)(Xj,Yj)是該區(qū)域的一個(gè)分割點(diǎn)，以此點(diǎn)為分割點(diǎn)將區(qū)域A中的激光數(shù)據(jù)點(diǎn)劃分成兩個(gè)部分，分別得到區(qū)域A1{(Xi,Yi)|i=1,2…j}和區(qū)域A'{(Xi,Yi)|i=j+1,j+2…n}。依次類推，按照同樣方法對區(qū)域A1和A'繼續(xù)進(jìn)行劃分，最后能夠得出N個(gè)相互不連的區(qū)域{A1,A2,…AN}。

1.2 擬合線段方程

為了從激光雷達(dá)檢測到的數(shù)據(jù)點(diǎn)中識別出直線，使用最小二乘法將點(diǎn)簇?cái)M合成直線y=kx+b形式，并根據(jù)轉(zhuǎn)換公式將(k,b)轉(zhuǎn)換成極坐標(biāo)中的(r,α)形式，如圖1所示。

圖 1 參數(shù)關(guān)系示意圖

1.3 基于Split-and-Merge算法的線段分割聚合

Split-and-Merge算法，即“分割——聚合”方法[1-3]，是一種較為常用的直線提取方法。先用區(qū)域的兩個(gè)端點(diǎn)擬合一條直線Li，計(jì)算區(qū)域中各個(gè)掃描點(diǎn)距離線段Li的距離，然后設(shè)定閾值dT，最后通過比較最大距離dmax與閾值dT來進(jìn)行分割。經(jīng)過區(qū)域分割，得到了可以用線段描述的區(qū)域Al{ali|i=1, 2…n}，每個(gè)ali區(qū)域可能是一條或多條直線段的組合。在不同區(qū)域內(nèi)分別進(jìn)行最小二乘法線段擬合，可以得到對應(yīng)的特征線段。

2 基于特征線段匹配的地圖構(gòu)建

當(dāng)機(jī)器人從一個(gè)位置水平移動到另一個(gè)位置時(shí)，激光雷達(dá)可得到位于同一個(gè)環(huán)境平面上的兩組測量點(diǎn)，由此能得到相應(yīng)的特征線段。因此，救援機(jī)器人地圖構(gòu)建(SLAM)的問題可以轉(zhuǎn)化為同一平面兩組特征線段的匹配問題[4-7]，在全局地圖建立過程中將邊界周圍的可觀測特征線段記為特征地圖；局部地圖覆蓋的過程中使用已知特征線段定位，逐步完善特征地圖。

由于救援機(jī)器人運(yùn)動會改變激光雷達(dá)的測量環(huán)境，所以在相鄰兩時(shí)刻機(jī)器人可以得到兩組激光數(shù)據(jù)點(diǎn)。經(jīng)過特征線段提取可以得到兩組特征線段，兩組線段中數(shù)據(jù)點(diǎn)之間的對應(yīng)關(guān)系未知。選取當(dāng)前時(shí)刻的某一個(gè)特征線段，與上一時(shí)刻特征線段逐一比較，當(dāng)與其中一個(gè)線段正確匹配后，可以確定兩個(gè)線段在環(huán)境中表示同一個(gè)邊界，并保存建圖。

2.1 機(jī)器人位置初始化

在數(shù)據(jù)測量過程中，由于救援機(jī)器人的中心和激光雷達(dá)的中心可能不在一個(gè)點(diǎn)上，所以在機(jī)器人系統(tǒng)中存在著兩個(gè)坐標(biāo)系，分別是救援機(jī)器人坐標(biāo)系(xr,or,yr)和以激光雷達(dá)為中心的坐標(biāo)系(xs,os,ys)，兩者之間的坐標(biāo)和參數(shù)可以相互轉(zhuǎn)化，如圖2(a)所示。在特征線段的匹配過程中，救援機(jī)器人的體積大小不會影響建圖結(jié)果，而且激光雷達(dá)是固定在救援機(jī)器人本體上隨之移動，兩者坐標(biāo)同時(shí)變化。因此，將救援機(jī)器人整體轉(zhuǎn)化為一個(gè)點(diǎn)，即忽略救援機(jī)器人的坐標(biāo)系而取激光雷達(dá)坐標(biāo)系為局部坐標(biāo)系(xs,os,ys)，如圖2(b)所示。

圖 2 建立點(diǎn)化模型

2.2 特征線段的坐標(biāo)轉(zhuǎn)換

由激光雷達(dá)所得到得采樣點(diǎn)信息都是基于救援機(jī)器人的局部坐標(biāo)系，將前后兩次得到的環(huán)境特征進(jìn)行匹配，并將每次得到的環(huán)境特征轉(zhuǎn)換到全局坐標(biāo)系中。

當(dāng)救援機(jī)器人在全局坐標(biāo)系中的位姿是Zs= （xs,ys,αs）時(shí)，特征線段在全局坐標(biāo)系中的參數(shù)表示Z=（r,α）與在機(jī)器人運(yùn)動坐標(biāo)系（局部坐標(biāo)系）中的參數(shù)有如下關(guān)系：

其中：αs,α∈[0 , π]，如圖3所示。

圖 3 特征線段的坐標(biāo)轉(zhuǎn)換

2.3 特征線段匹配方法

在特征線段匹配過程中，主要是計(jì)算線段之間的轉(zhuǎn)動偏差和位移偏差，確定兩個(gè)不同時(shí)刻下同一個(gè)邊界的線段特征。

對提取出來的線段特征點(diǎn)集進(jìn)行基于距離誤差的最小二乘法擬合，得到線段y=kx+b。為使線段能有一個(gè)完整的描述，選擇特征參數(shù)

其中：α表示線段在極坐標(biāo)系下的角度；r表示線段到原點(diǎn)的距離；L表示線段的長度；start(x)和end(y)分別表示線段起點(diǎn)和終點(diǎn)的坐標(biāo)；cross(x,y)表示交點(diǎn)的坐標(biāo)；N表示該線段的點(diǎn)集中點(diǎn)的個(gè)數(shù)。

起點(diǎn)和終點(diǎn)坐標(biāo)的計(jì)算方式如下：設(shè)(x1,y1)為原始數(shù)據(jù)的起點(diǎn)或者終點(diǎn)，直線段為數(shù)據(jù)點(diǎn)集擬合出來的特征線段，將x1和y1分別代入直線公式，可以得到x=(y1-b)/k和y=k x1+b，即為點(diǎn)(x1,y1)關(guān)于特征線段的對應(yīng)點(diǎn)(x,y)的坐標(biāo)值，則五角星為該兩點(diǎn)的中點(diǎn)，也就是擬合后的線段端點(diǎn)，如圖4所示。

圖 4 擬合后的線段端點(diǎn)

2.4 特征參數(shù)與運(yùn)動模式的關(guān)系

救援機(jī)器人運(yùn)動模式主要有兩種，即旋轉(zhuǎn)運(yùn)動和直線移動。在特征線段匹配過程中，需要分別考慮救援機(jī)器人的移動距離和轉(zhuǎn)動角度，從而對不同運(yùn)動模式下的激光雷達(dá)線段特征數(shù)據(jù)進(jìn)行不同方式的匹配。

2.4.1 旋轉(zhuǎn)運(yùn)動

當(dāng)救援機(jī)器人進(jìn)行旋轉(zhuǎn)運(yùn)動時(shí)，環(huán)境中特征線段到激光雷達(dá)的距離r不變，但特征線段的旋轉(zhuǎn)角度與載體旋轉(zhuǎn)的角度相同，如圖5所示。

圖 5 旋轉(zhuǎn)運(yùn)動下的參數(shù)變換

在這種情況下，相鄰兩時(shí)刻匹配的閾值主要決定于載體旋轉(zhuǎn)的速度。設(shè)載體旋轉(zhuǎn)的角速度為ω(rad/s)，激光雷達(dá)掃描周期為t，則在一個(gè)掃描周期內(nèi)載體旋轉(zhuǎn)角度為ωt。設(shè)置閾值應(yīng)該略大于全速旋轉(zhuǎn)的載體所轉(zhuǎn)過的弧度，取αt=1.2ωt。若相鄰兩時(shí)刻的某一對匹配線段滿足條件

則認(rèn)為這對線段為不同時(shí)間上的同一環(huán)境特征。

2.4.2 直線移動

當(dāng)救援機(jī)器人進(jìn)行直線移動時(shí)，各線段特征在激光雷達(dá)坐標(biāo)系下的角度α不變，但與激光雷達(dá)的距離r隨著激光雷達(dá)前進(jìn)的距離的不同而有著不同的變化值，如圖6所示。

圖 6 直線運(yùn)動下閾值的求取

為了保證地圖中某一時(shí)刻的特征線段都能夠匹配上一時(shí)刻所對應(yīng)的特征線段，需要設(shè)定一個(gè)動態(tài)閾值。設(shè)Δd為激光雷達(dá)前進(jìn)的距離，α為特征線段在極坐標(biāo)系下的角度，則Δr= Δds in （π-α）。取閾值rt=1.2Δr，對于上一時(shí)刻的每一條特征線段i，存在ri= 1 .2 Δds in(π -αi)，進(jìn)行匹配的特征線段只有滿足了r的取值閾值范圍，才有可能是正確的匹配。

2.5 特征線段匹配

為找到兩個(gè)相鄰時(shí)刻對應(yīng)的特征線段，需要對兩個(gè)時(shí)刻的線段特征進(jìn)行匹配。通過2.4節(jié)對救援機(jī)器人的運(yùn)動分析，可以得到相應(yīng)的匹配閾值。若當(dāng)前時(shí)刻與上一時(shí)刻的一組線段特征的參數(shù)之差小于其閾值，則可以說明這一組線段特征有可能來源于環(huán)境中的同一障礙物。

2.5.1 基于特征線段匹配算法

對于上一時(shí)刻的每一個(gè)線段特征，與當(dāng)前時(shí)刻的所有線段特征作對比。根據(jù)公式(3)計(jì)算其特征參數(shù)之差，可以得到一個(gè)誤差向量

其中：dist()表示對該組特征中的點(diǎn)求距離；NΔ表示線段特征中包含的點(diǎn)數(shù)之差。

對誤差向量進(jìn)行分析，其中Δα和Δr按照計(jì)算閾值進(jìn)行匹配。若誤差向量小于設(shè)定閾值，則將所計(jì)算的相鄰時(shí)刻的線段的索引計(jì)入匹配矩陣中保存。根據(jù)所匹配好的特征線段，推算出救援機(jī)器人的在大環(huán)境下的移動方式，并得出局部地圖中特征線段所處于全局地圖中的位置，以此疊加局部地圖得到全局地圖。

2.5.2 誤差向量作用

在救援機(jī)器人移動過程中，可能由于速度過快導(dǎo)致激光雷達(dá)無法掃描完整，而且對兩張局部地圖的特征線段進(jìn)行匹配的同時(shí)會出現(xiàn)旋轉(zhuǎn)運(yùn)動和直線移動。為使匹配能順利進(jìn)行，需要設(shè)定一定的條件范圍。

在匹配救援機(jī)器人直線移動的局部地圖時(shí)，Δα用來限制機(jī)器人在移動中角度變化過大。在直線運(yùn)動中有可能出現(xiàn)角度變化，小角度變化在匹配中可以忽略不計(jì)；過大的角度變化應(yīng)剔除掉，以避免產(chǎn)生誤差。

在匹配救援機(jī)器人旋轉(zhuǎn)運(yùn)動的局部地圖時(shí)，Δr用來限制移動中位移變化過大。在轉(zhuǎn)動運(yùn)動中有可能出現(xiàn)位移變化，小位移變化在匹配中可以忽略不計(jì)，過大的位移變化應(yīng)剔除掉，以避免產(chǎn)生誤差。

在相鄰兩個(gè)時(shí)刻中，局部地圖的變化不會差距很大，也有可能出現(xiàn)特殊情況導(dǎo)致相鄰兩時(shí)刻局部地圖突變，這種情況的發(fā)生會對地圖匹配造成很大的影響，增加匹配難度，導(dǎo)致誤差過大。因此，設(shè)立dist(strat)，dist(end)和dist(cross)用于剔除掉變化過大的情況，確保順利匹配；設(shè)置點(diǎn)數(shù)差ΔN，剔除數(shù)據(jù)點(diǎn)過少的情況，減少誤差。

2.5.3 特征線段誤匹配

在特征線段匹配過程中，能夠滿足匹配條件的有以下3種情況：

1） 兩特征線段近似重疊；

2） 兩特征線段屬于同一特征的不同部分；

3） 兩特征線段是共線的不同特征。

對于前兩種匹配情況，稱為真匹配；而對于第3種匹配情況，稱為誤匹配。

經(jīng)特征線段匹配后，絕大部分的特征線段會得到一對一的匹配關(guān)系，即對于tk時(shí)刻的特征線段lk；在tk+1時(shí)刻有且只有一條特征線段lk+1與之對應(yīng)。但由于實(shí)際地形中的非理想地形、運(yùn)動誤差和測量誤差等因素，會有小部分特征線段出現(xiàn)一對多，或者多對多的匹配關(guān)系，此時(shí)則出現(xiàn)誤匹配情況。

如圖7所示，tk時(shí)刻經(jīng)過特征提取和擬合后獲得線段l1k和l2k，tk+1時(shí)刻經(jīng)過特征提取和擬合后獲得線段l1k+1和l2k+1。根據(jù)特征線段匹配條件，將l1k與l1k+1和l2k+1進(jìn)行匹配判定。tk+1時(shí)刻與l1k正確匹配的特征線段是l1k+1，但此時(shí)l1k與l2k+1也很相似，接近于一條線段，則l1k與l1k+1和l2k+1匹配，即發(fā)生了特征誤匹配。

圖 7 誤匹配示意圖

當(dāng)出現(xiàn)誤匹配的對應(yīng)關(guān)系時(shí)，需要進(jìn)一步地判斷，即誤匹配處理。在處理過程中，需要計(jì)算各端點(diǎn)間距，其中起點(diǎn)間距加終點(diǎn)間距最小的線段為正確匹配的線段對。經(jīng)特征提取后tk時(shí)刻的特征線段l1k起點(diǎn)為a，終點(diǎn)為b；l2k起點(diǎn)為c，終點(diǎn)為d；tk+1時(shí)刻的特征線段l1k+1起點(diǎn)為a'，終點(diǎn)為b'；l2k+1起點(diǎn)為c'，終點(diǎn)為d'。計(jì)算a與a'的間距Daa'、b與b'的間距Dbb'，a與c'的間距Dac'、b與d'間距的Dbd'。通過對比可以得出Daa'+Dbb'

3 全局地圖線段優(yōu)化

假設(shè)有兩特征線段lm和ln，并且lm先匹配進(jìn)入到全局地圖中，為提高建圖質(zhì)量可以進(jìn)行以下兩點(diǎn)優(yōu)化：

1） 為了保證制圖的連貫性，計(jì)算lm始端點(diǎn)與ln末端點(diǎn)之間的距離Δl。若Δl小于某一給定極小閾值，則認(rèn)為兩個(gè)端點(diǎn)是同一個(gè)；

2） 將特征線段看成若干點(diǎn)的集合。當(dāng)lm與ln為真匹配時(shí)，只需將任意一個(gè)特征線段融入到地圖中。當(dāng)兩者屬于同一特征線段的不同部分時(shí)，融入到地圖中的特征線段lr應(yīng)滿足lr=lm∪ln。

在地圖構(gòu)建中，計(jì)算時(shí)間的消耗主要來自于狀態(tài)更新階段。由于點(diǎn)特征本身的離散特性，要表示出一個(gè)線特征至少需要屬于該線段特征的3個(gè)點(diǎn)特征參與，而采用本算法則避免了對屬于同一直線的所有點(diǎn)進(jìn)行更新計(jì)算，計(jì)算負(fù)擔(dān)減小，實(shí)時(shí)性提高。

當(dāng)相鄰兩個(gè)時(shí)刻局部地圖中的特征線段匹配成功后，將相應(yīng)的特征線段對應(yīng)標(biāo)記在全局地圖中，能夠看到局部坐標(biāo)的變化，然后根據(jù)局部坐標(biāo)原點(diǎn)位置的變化，可以得出救援機(jī)器人在全局地圖中的位置以及位姿變化情況。

4 地圖構(gòu)建仿真

地圖構(gòu)建仿真實(shí)驗(yàn)平臺為自主研發(fā)的履帶式救援機(jī)器人，采用Matlab軟件編寫算法程序，實(shí)現(xiàn)對救援機(jī)器人的特征識別與地圖構(gòu)建的仿真。

在圖8所示救援機(jī)器人仿真建圖界面中，左地圖為特征線段在全局坐標(biāo)系下建立的地圖，其中邊界線段為救援場地的墻壁，地圖中的曲線是救援機(jī)器人的運(yùn)動軌跡；右地圖顯示在局部地圖下的特征線段。

圖 8 救援機(jī)器人仿真建圖界面

系統(tǒng)最后生成的環(huán)境地圖，如圖9所示。

圖 9 仿真環(huán)境地圖

5 結(jié) 論

利用基于特征線段匹配的方法，在未知救援環(huán)境中解決救援機(jī)器人地圖構(gòu)建（SLAM）問題。以機(jī)器人運(yùn)動前的位姿確定的坐標(biāo)系作為全局坐標(biāo)系，機(jī)器人在初始位置觀測到的特征線段直接記入環(huán)境地圖用于定位，在雷達(dá)數(shù)據(jù)采集過程中，將邊界周圍的可觀測特征線段記為環(huán)境邊界，最后，在建圖過程中使用已知特征線段定位的同時(shí)完善全局地圖。仿真過程表明，特征線段匹配算法在救援機(jī)器人地圖構(gòu)建研究中完全可行。

[1]Borges G A, Aldon M J. Line extraction in 2D range images for mobile robotics [J]. Journal of Intelligent and Robotic Systems, 2004: 267-297.

[2]Einsele T. Real-time self-localization in unknown indo or environments using a panorama laser range finder [J]. IEEE/RSJ International Conference on Intelligent Robots and Systems, 1997: 697-702.

[3]謝 鈞, 俞 璐, 吳樂南. 一種改進(jìn)的Split—Merge圖像分割算法[J]. 計(jì)算機(jī)應(yīng)用, 2008, 28(7):1744-1746.

[4]Jose Guivant, Eduardo Nebot. Optimization of the simultaneous localization and map-building algorithm for real-time implementation [J]. IEEE Journal of Robotic and Automation, 2001, 17(3): 242-257.

[5]Choi Y, Lee T, Oh S. A line feature based SLAM with low grade range sensors using geometric constraints and active exploration for mobile robot [J].Autonomous Robots, 2008, 24 (1): 13-27.

[6]季秀才, 鄭志強(qiáng), 張 輝. SLAM問題中機(jī)器人定位誤差分析與控制[J]. 自動化學(xué)報(bào), 2008, 34(3):323-330.

[7]趙一路, 陳 雄, 韓建達(dá). 基于掃描匹配的室外環(huán)境SLAM方法[J]. 機(jī)器人, 2010, 32(5): 655-660.