無反射板激光導航機器人地圖創建的特征提取

李丹陽 張 軻 許如清 羅志峰 吳佳謙 桂 豪

1.上海交通大學上海市激光制造與材料改性重點實驗室，上海，2002402.高新船舶與深海開發裝備協同創新中心，上海，200240

0 引言

無反射板激光導航(自然導航)不需要設置繁瑣的反光標識板，行走路徑可根據實際需求隨時改動，比有反光標識板的激光導航更加靈活,具有更低的人力和物力成本、更高的效率，代表激光導航技術的未來發展方向。無反射板激光導航最為關鍵的是地圖的構建以及導航過程中對特征信號的準確識別和定位[1]。

真實環境的抽象描述稱為地圖構建。測距式傳感器采集環境得到的數據是點的集合，如何處理這些離散的點獲得環境的抽象描述呢？文獻[1-3]詳細比較了幾種典型的數據點提取特征直線的方法：Hough變換法特征精度較高[4-5]，但計算量較大，難以保證局部地圖構建的實時性；數據分裂與合并(split-and-merge)算法精度和實時性都較好[6-7]。

通常情況下，地圖構建和導航定位提取的特征是直線或圓。直線是地圖構建最典型的特征，本文采用逐步分解的方法，將直線特征提取分為斷點檢測、線段分割以及直線提取三個步驟逐步分離點集，來提高特征提取的精確度。

斷點檢測法有固定閾值法、自適應閾值法以及卡爾曼濾波斷點檢測法等[8]。自適應閾值法不受地圖尺寸影響，且將點之間的關系納入考慮范圍，因此稀疏的線段不會被判定為斷點。

線段分割的本質就是提取多線段中的拐點，目前較為成熟的線段分割方法主要有LT(line tracking)算法[9]、模糊聚類(prototype-based fuzzy)算法[10]和迭代適應點(iterative end-point fit，IEPF)[11-12]算法。

LT算法計算速度非常快且具有普適性，但難以確定一個合適的閾值，尤其在地圖尺寸和誤差量未知的情況下。此外，LT算法只考慮一個點的情況，而沒有考慮所有已被納入直線的點的殘差，易將曲率較大的曲線誤判斷為直線特征。

模糊聚類算法的核心思想是通過遞歸計算，不斷減小成本函數(cost function)來進行線段分割。模糊聚類算法有比較大的局限性，一是算法需要已知原型，采用模糊聚類算法必須把原型的數量作為已知條件，而在大多數應用情況下，線段的數量是未知量；二是模糊聚類算法容易受到異常值干擾，算法的魯棒性差。

IEPF算法的核心是遞歸思想[11-12]，計算速度快且對局部地圖的適應性好。

本文所涉及的環境為紡織倉庫，環境特點以直線特征為主，因此對無反射板激光導航AGV地圖構建的直線特征提取進行研究。針對環境地圖掃描范圍多變、掃描數據點遠近稀疏不均，采用自適應閾值法進行斷點檢測，考慮到運算速度和局部適應性，采用IEPF算法進行線段分割，最后采用最小二乘法擬合直線及區域搜索法進行優化，進一步提高直線特征提取的精度。

1 實驗系統及參數

圖 1 開發的無反射激光導航AGV系統架構Fig.1 Developed AGV architecture based on laser navigation without reflector

圖1所示為研發的無反射板激光導航AGV系統架構。激光測距儀實時掃描外部場景并將數據傳輸給工控機；工控機進行地圖構建、任務規劃、導航，對搬運任務執行過程中的AGV進行實時定位，并將位置信息反饋給PLC系統；PLC主要完成貨物的裝卸以及安全檢測和控制。通過遠程調度系統(通過wireless signal 與AGV進行通信)對AGV進行遠程調度。本研究中，激光測距儀為SICK公司的LMS511 2D型激光掃描測距儀，掃描頻率為50 Hz，掃描角度范圍為180°，每0.25°掃描一次，掃描高度為2.5 m。一次掃描后得到721個極坐標系下的數據點。

2 特征直線提取

2.1 自適應斷點檢測

在激光掃描地圖匹配中，斷點指在連續的掃描點中數據特征出現突變的情況。這種情況通常對應被掃描區域內的物體邊緣、門、窗，或者環境中的障礙物等物體不連續的部分。斷點檢測是地圖特征提取中非常重要的一個步驟，有助于辨識環境地圖中的門、窗、立柱等重要的特征，方便隨后的地圖匹配以及路徑規劃等。

為方便問題描述，下文將連續的2個點統一命名為Pn-1、Pn，旗標Kn-1和Kn表征Pn-1和Pn是否為斷點。如果Pn-1和Pn不連續，則Pn-1和Pn的對應值都被賦值為TRUE。

自適應閾值的判定標準：若|Pn-Pn-1|>Dmax，則判定Kn-1和Kn為TRUE。本文采用MORAVEC[8]提出的一種自適應閾值算法，原理如圖2所示。首先假設一條通過Pn-1點的直線l，并假設這條直線是能被算法提取的最極端的情況，以此來外推最極端可接受情況下Pn點閾值圓的半徑。直線l與Pn-1點的激光入射線Φn-1的夾角為λ，根據圖2所示的幾何關系可得

(1)

圖2 自適應閾值的原理圖Fig.2 Schematic of adaptive detection

(2)

(3)

式中，σr為設定的傳感器的測量誤差，根據實際測試情況進行確定。

測試發現，在σr取25 mm，λ取5°的情況下，可以取得比較理想的結果。

自適應閾值法不受地圖尺寸的影響，而且自適應閾值將點之間的關系納入考慮范圍，因此稀疏的線段不會被誤判為斷點。

2.2 線段分割

2.2.1點集分離

斷點檢測后，需先將點群按斷點分為不同的點集，然后再進行線段提取。有效的特征直線應滿足以下幾個基本條件：①線段的長度應該超過最小值Lmin；②線段所包含的點的數量應該超過最小值Nmin；③不能有斷點。因此，按這三個原則根據斷點分離成不同點集，為后續進一步的線段分割奠定基礎。

2.2.2迭代適應點算法

IEPF算法利用遞歸的思想，不斷地將點集P={Pni,Pni+1,…,Pne}劃分為子集P′={Pni,Pni+1,…,Pna}、P″={Pna,Pna+1,…,Pne}。如圖3所示，首先連接起點Pni和終點Pne，得到線段l，然后計算點集P中所有點到線段l的距離，將距離線段l最遠的點標記為Pna，最遠距離記作Tmax。然后將點集P按順序從Pna處劃分為子集P′和P″。劃分將一直迭代，直至子集P′和P″滿足了兩個終止條件之一：①點集中的點的數量小于閾值Nmin；②Tmax的值小于閾值Tthreshold。

圖 3 IEPF算法原理Fig.3 Scheme of IEPF algorithm

2.2.3算法實現

算法的主程序部分負責控制迭代的進行，并記錄一輪迭代后所輸出的拐點索引值。初始的搜索范圍是整個點集，然后開始進行迭代計算，每一輪迭代后，輸出下一輪的搜索列表和本次迭代后所得到的拐點。計算一直循環進行，直到某一次迭代后輸出的搜索列表為空，這意味著點集中所有的分段都已滿足上文所說的終止條件之一。最后的輸出將包括所有拐點的索引值。具體過程如下：首先計算點集中所有的點到直線的距離，求其中的最大值Tmax以及對應的索引值(記為Pspinodal)。如果Tmax大于所設定閾值，則Pspinodal被判定為拐點，并將點集P分割為P′和P″兩個子集。然后創建下一輪迭代的搜索列表。根據子集P′和P″內點的數量判斷搜索列表函數，如果子集P′或P″點的數量大于閾值Nmin，則將子集P′或P″加入下一輪迭代的搜索列表；如果Tmax小于所設定閾值，則意味著點集P′或P″無需再分割。

2.3 特征直線提取及優化

線段分割后，在斷點和拐點之間還有一些離散的點，它們不具有線段的特征信息。直線擬合的本質就是確定直線的參數特征，即將前文中所提取的斷點和拐點所夾的中間點直線擬合為y=kx+b的形式。除了Hough變換算法，其他算法得到的線段分割結果都為點的信息，都需要進行直線擬合，本文采用最小二乘法進行特征直線提取。

直線擬合的過程將前文提取的結果(斷點和拐點)有機地結合在一起，生成用于直線擬合的點集，即兩個斷點間以拐點為分割點的子集。程序首先嘗試擬合直線，如果擬合殘差超過設定的閾值threshold，需再進行擬合優化。直線擬合的關鍵算法如下：

/*生成直線擬合的點集*/

fitting_point = [ni sort(Pspinodal) ne]

for i in fitting_point do

/*嘗試擬合直線*/

[parameter(i) error_value(i)] = LineFit (fitting_point)

/*當擬合殘差超過閾值*/

if error_value(i) > threshold do

/*進行擬合優化*/

[parameter(i)] = LineFitOptimization (fitting_point)

end

end

進行擬合優化的原因是某些直線的擬合結果不理想，由IEPF算法的自身缺陷所致。圖4是IEPF算法所導致的缺陷示意圖，P1為程序所計算出的拐點，而P1(3 349，2 175)(mm)與P2(3 417，2 177)(mm)之間的距離僅為32 mm，Tmax小于兩點之間的距離，遠小于所設置的閾值75 mm，導致程序無法判定P2為拐點。減小閾值有利于程序識別P2為拐點，但閾值設定過小(在本例中，小于30 mm)時，迭代適應點將陷入時間復雜度為O(n2)的最糟糕情況，導致程序進入長時間的迭代。因此圖4所示的缺陷情況難以通過改進IEPF算法加以改善，而應通過擬合優化的方式來解決。

圖 4 IEPF算法所導致的缺陷示意圖Fig.4 Scheme of the defects caused by IEPF algorithm

優化擬合的思路如下：由于該缺陷一般僅出現在線段起點Pni或終點Pne附近，因此只需考慮在兩端小范圍內搜索從而尋找最優值即可。在本研究中，將搜索范圍限定為[Pni+3,Pne-3]可以取得比較理想的結果。

算法如下：首先創建一個搜索列表；然后計算不同組合情況下的擬合殘差，并以矩陣的形式儲存；最后，求出最小殘差及其對應的起點和終點的索引值，并輸出此條件下的最優參數。

擬合優化的關鍵算法如下：

Function [parameter] =LineFitOptimization (fitting_point)

/*設置搜索區域*/

search_area = CreatSearchArea(fitting_point)

/*計算不同組合下的擬合殘差*/

error_value_matrix = LineFit(search_area)

/*求最小殘差對應的索引值*/

[error_value_min index]= min(error_value_matrix)

/*輸出最優的參數*/

parameter = fitting_point(index)

3 結果和討論

3.1 算法結果討論

為驗證特征直線提取算法的有效性，使用移動機器人在某廠的紗包倉庫進行環境掃描、特征提取和地圖構建測試。測試場景如圖 5所示，倉庫寬約20 m，長約110 m(圖示長約60 m)，圖中兩側的立柱、支架等以直線特征為主，可作為特征標識物方便定位。首先沿著預先規劃的路徑移動機器人，用激光測距儀掃描環境，進行特征直線提取、地圖構建。分別對本文提出的自適應斷點檢測、IEPF線段分割、最小二乘法特征提取及優化算法進行測試和效果驗證。

圖 5 紡織車間紡織包堆放倉庫應用場景Fig.5 Warehouse for textile spinning package

由圖6可知，采用自適應閾值法可以比較理想地進行斷點檢測，在線段上的點排列稀疏程度不同的情況下，自適應閾值法都能成功識別出斷點特征。

圖 6 自適應閾值斷點檢測實例(σr=25 mm，λ=5°)Fig.6 Breakpoint detection used by adaptive thresholds(σr=25 mm，λ=5°)

圖7所示為采用IEPF算法進行線段分割的實例。可以看出，該算法的效果較理想，可以精確地提取出一些非常細節的拐點，這非常有利于后續的特征提取和地圖匹配。

圖7 IEPF算法線段分割實例Fig.7 Line segmentation based on the IEPF algorithm

實驗中，采用IEPF算法的運算時間小于0.02 s，滿足AGV在行駛過程中實時定位的速度要求。

圖8所示為應用上述的斷點檢測、線段分割和線段擬合算法的直線特征提取實例，程序一共提取出12條直線特征，地圖中主要的直線特征都被準確提取，尤其是在一些細節處的直線提取也得到了理想的結果。

圖 8 直線特征提取實例Fig.8 Line extraction by using least square method

圖9是進行直線擬合優化前后的對比圖，可以看出，優化過程可以使地圖的直線特征提取更為精確。這說明由IEPF算法所引起的缺陷可以通過優化擬合過程得以消除。

擬合優化就是將擬合殘差的影響納入直線特征提取的考慮范圍。經過斷點檢測和線段分割的步驟后，原先接近千個數據點集被算法簡化為由斷點和拐點組成的簡單拓撲關系，所包含的信息量急劇減少，這雖然減輕了計算機的運算量，但增加了程序整體的盲目性。一旦斷點檢測和直線分割的算法出現遺漏斷點或拐點的情況，程序將忽視原始點集而提取出地圖上并不存在的直線。因此，在直線擬合的步驟中加入優化程序是十分有必要的。

3.2 重復定位精度測試

應用本文所提出的算法進行特征直線提取，然后進行地圖匹配和構建，建立全局地圖。將激光導航機器人移動到測試倉庫的任意位置，并采用激光導航儀進行掃描，建立局部地圖并將其與全局地圖進行匹配，確定機器人的當前位置。在應用場景中任取了6個點進行重復定位精度測試，每個位置進行多次重復掃描。其測試結果如表1所示，由測試結果可知，X方向、Y方向的定位精度在±6 mm以內，而方位角的定位偏差則在±0.007 rad以內。

圖10所示為表1第一個測試位置經過260次重復定位測試時的位置和方位角的變化。

由圖10可知，X方向和Y方向的偏差始終在±6 mm以內，方位角變化始終在±0.003 5 rad以內，即使考慮小車在運動過程中產生的偏差，其整體偏差也可控制在±10 mm以內，該定位精度滿足紡織紗包搬運重載機器人實際應用需求。

圖9 直線擬合優化前后對比Fig.9 Comparison of line fitting before and after optimization

位置(x,y,θ)(xmax,xmin) (mm)(ymax,ymin) (mm)(θ max,θ min) (rad)重復次數(-6.00,15 326.0,0.08)(-13.2,-0.6)(15 322,15 332)(0.080 7,0.087 3)260(186.4,12 869.0,0.100)(180.2,193.4)(12 865,12 875)(0.099 1,0.104 1)120(54.90,15 249.0,0.06)(50.8,59.5)(15 245,15 254)(0.061 7,0.065 7)120(225.0,19 810.0,-0.030)(219.6,231.4)(19 807,19 815)(-0.029 4,-0.034 4)120(2 120,16 215.0,1.569)(2 114.9,2 124.6)(16 209,16 220)(1.566,1.575)120(-1 620,22 320,-1.568)(-1 625.2,1 615.6)(22 315,22 326)(-1.573,-1.563)120

圖10 位置和方位角的變化Fig.10 Variation of the position and orientation angle

3.3 現場應用測試

將上述特征直線提取算法以及地圖匹配算法等植入無反射板激光導航自主運動機器人，進行實際搬運任務測試。首先，激光導航儀掃描放置紗包的倉庫，建立全局環境地圖，機器人根據搬運任務規劃運動路徑。運動過程中，激光導航傳感器實時掃描現場場景，提取特征直線后，創建當前位置的局部地圖。AGV通過與全局地圖中的期望位置相比較不斷調整運動路徑，這個過程被反復進行，直到搬運任務完成。圖11所示為實驗場景，紡織紗包被準確地取貨，行進之后，在正確的位置放貨。實驗結果表明本文的算法滿足實際應用要求。

圖11 開發的AGV搬運紡織紗包實驗Fig.11 Experiment of carrying textile spinning package for the AGV

4 結論

(1)針對特征直線提取，提出了逐步分解的方法，將直線特征提取分為斷點檢測、線段分割以及直線提取三個步驟來分離點集。

(2)在斷點檢測部分，提出自適應閾值斷點檢測法。針對線段分割，提出的IEPF線段分割方法運算快且效果較好。

(3)對最后分離的直線特征點集，采用最小二乘法并結合局部區域搜索的方法進行直線特征提取，提高了精度并消除了線段分割中IEPF算法自身缺陷帶來的不利影響。

(4)實驗結果表明，所提出的逐步分解的直線特征提取方法可以精確地提取直線特征，位置重復定位精度在±6 mm以內，方位角偏差在±0.007 rad以內，計算時間在0.02 s以內。現場應用測試表明該算法滿足大部分以直線特征為主的工業應用場合。