基于全局稀疏地圖的AGV視覺定位技術

2019-01-30 01:36:46張浩悅程曉琦劉暢孫軍華

北京航空航天大學學報 2019年1期

張浩悅，程曉琦，劉暢，孫軍華

(北京航空航天大學儀器科學與光電工程學院, 北京 100083)

自動導引車(Automated Guided Vehicle，AGV) 是一種用于搬運物料的無人駕駛導航車，目前已廣泛應用于碼頭、倉庫、車站等各種場所。定位系統是AGV的關鍵組成部分，為AGV的自動導航提供實時的位姿信息。目前AGV 存在多種定位方式，采用不同定位方式的AGV 的性能指標、成本及應用領域是不同的[1]。傳統的定位方式有電磁定位、激光定位、GPS定位和慣導定位等。電磁定位方式預先埋設電線，通過感應高頻交變電流的電磁場實現定位，成本低且易于實現但精度較低，且路徑固定不易更改。采用激光定位AGV 的定位精度高，有更高的測量距離，對外部光照的變化不敏感，但技術較為復雜，成本也比較高，容易受到環境中透明和反光物體的影響[2]。GPS定位能提供絕對坐標，但在室內場景中難以使用且定位誤差較大。慣導定位通過積分計算狀態，存在積累誤差，同時存在靜態漂移。近年來，利用低成本相機實現導航定位的技術受到了越來越多的關注，機器視覺的飛速發展為工業AGV 的自動導航提供了新的可能。視覺定位方式通過視覺傳感器動態感知四周環境，采用圖像處理、立體視覺等相關技術，實現AGV的定位。

為了實現AGV的視覺定位，需要事先給AGV提供一張三維“地圖”，供AGV搜索匹配，以此完成位姿的求解任務。地圖是一種包含環境空間幾何信息的結構，有多種表示方法[3]。其中，基于路標的稀疏地圖是常用的一種表示法。稀疏地圖將環境抽象為一些表示空間三維坐標的3D路標，利用“描述子”對這些3D路標進行區分。視覺傳感器根據這些獨一無二的描述子對檢測到的圖像特征點和地圖進行匹配，從而實現數據關聯，實現位姿的求解。對于3D路標的表示，點特征是最常用的描述方法，如ORB(Oriented fast and Rotabed Brief)特征[4]和SIFT(Scale-Invariant Feature Transform)特征[5]等，另外一些更復雜的幾何特征也被用于描述3D路標，如線特征、平面和滅點[6]。

全局定位算法要求在任何位置，都能根據傳感器的數據，唯一地、重復地解算出AGV的位姿。這極大地簡化了定位策略，然而這通常是困難的[7]。基于視覺的全局定位方式面臨噪聲和混淆兩大問題的挑戰。視覺傳感器的噪聲來自于光照變化、圖像抖動和模糊、遮擋等多個方面，極大地影響到3D特征的檢測和匹配。混淆即通過視覺無法分辨相似場景(如走廊)[8]。在AGV作業的工業環境中，環境通常是動態變化的(如白天和夜晚光照的變化，托盤的移動等)，同時存在大量的相似場景(如貨架等)，使得采用自然特征構建地圖的策略變得困難。一方面動態環境使得難以建立穩定不變的3D路標，另一方面是大量重復的場景給數據關聯帶來挑戰。Wulf等[9]為了克服動態場景的影響，使用倉庫天花板的結構和特征作為導航定位依據。但是在智能泊車的應用場景中，AGV需要在上方放置托盤運送汽車，同時AGV自身高度也極低(30 cm左右)，很難看到工業環境的天花板。在環境中鋪設位置已知、易于識別、不會變化的人工路標，可以輔助機器人的定位，如在激光定位中使用的反光板。然而，在工業應用中，為了能覆蓋AGV行駛的所有路徑，往往需要設置大量的人工路標，如何準確無誤地匹配這些路標是具有挑戰性的問題[10]。

本文針對以上問題，考慮AGV車身高度低，上方運載貨物的限制，提出了一種新的二維編碼點作為人工路標，并以此為基礎實現魯棒的特征匹配，構建了一種稀疏地圖，供AGV搜索匹配，實現AGV精確的全局定位。二維編碼點設計為方形棋盤格樣式，通過黑白色塊編碼ID信息，作為二維編碼點的區分標志；二維編碼點作為人工路標鋪設于地面，使用單反相機進行拍攝，通過一種分參數塊的三維重建方法，實現廠房的大規模地圖構建，生成一種稀疏電子地圖。實驗結果表明，使用本文方法，AGV視覺傳感器能準確識別每個編碼點的編碼信息，并和提供的電子地圖魯棒匹配，完成定位任務，速度和精度都能滿足工業應用要求。

1 二維編碼點的設計、檢測及解碼

1.1 二維編碼點的設計

工業環境中使用的人工路標要求能夠易于識別，同時，如果每個編碼點都有自己獨一無二的編碼信息，可大大簡化多幅圖像之間的匹配[11]。常用的人工編碼點為環狀編碼點，如圖1(a)所示[12]。此種編碼點利用環帶編碼信息。由于圓形的周期性，解碼時需要以每一位為起點進行解碼，然后進行選擇；同時周期性使得編碼容量較小。本文設計一種新的大容量二維編碼點，如圖1(b)所示。編碼點設計為底色為黑色的正方形，使用時印刷在白色背景上。方形二維編碼點僅有4種可能的解碼方向，提高了解碼效率。編碼區域劃分為7×7的棋盤格，每一格可為黑色或者白色，分別對應二進制編碼0和1。49位二進制數字作為二維編碼點的編碼結果，為了方便起見，二進制編碼結果會映射到從0開始編碼的十進制數字。理論上可以有249種編碼方式，容量滿足工業大規模應用的需求。

圖1 編碼點Fig.1 Coded points

1.2 二維編碼點的檢測

二維編碼點檢測的關鍵是識別編碼點的黑色四邊形邊框。基于此提出了一種四邊形區域的檢測算法，以提取出二維編碼點可能存在的區域。檢測算法的流程如下。

1.2.1 圖像預處理

圖像預處理包括濾除圖像噪聲和提取顯著的圖像特征。首先對原始圖像(圖2(a))進行平滑處理，使用高斯模板進行濾波，減小噪聲帶來的影響。然后使用傳統的Canny算子[13]檢測圖像梯度顯著變化的點，并對這些點進行連接和聚類，獲得圖像中的輪廓(圖2(b))。同時對圖像進行Harris角點檢測[14]，獲得整幅圖像每個像素點的角點響應值(圖2(c))。

1.2.2 四邊形檢測

對于四邊形的檢測基于如下的假設：具有4個頂點的輪廓為四邊形。對于每一組輪廓，判斷是否為四邊形的算法步驟如下：

1) 將輪廓的每個像素擴大至3×3的區域作為圖像掩模。

2) 對Harris角點的響應圖使用掩模，將掩模以外的區域設置為0(圖2(d))。

圖2 二維編碼點的檢測和識別Fig.2 Detection and recognition of two-dimensional coded points

3) 尋找全圖響應值最大的點，作為一個頂點并記錄；并對角點響應圖該點處進行非極大值抑制(NMS)。

4) 重復步驟3)，直至最大響應值小于一定的閾值。

5) 若記錄的頂點數目為4，則判斷該輪廓為四邊形；否則舍棄該輪廓。

由此可判斷該輪廓是否為四邊形，如果是四邊形，還記錄保存了四邊形的4個頂點。檢測結果中會包含一些凹四邊形，如圖2(e)左上角，凹四邊形明顯不符合二維編碼點的透視投影規律，因此對所有檢測結果用Graham scan算法[15]進行篩選，保留下僅為凸包的四邊形(圖2(f))。

1.3 二維編碼點的解碼

如圖2(g)所示，解碼通過二維平面點的單應變換實現。首先在理想二維編碼點上建立坐標系，同時獲得所有編碼位置的坐標，并將其投影到實際圖像中。理想二維編碼點的坐標系建立在中心，從左下角開始，按順時針順序4個角點坐標設為(-1，-1)，(-1，1)，(1,1)，(1，-1)。設理想二維編碼點角點齊次坐標為X1=(u1,v1,1)T，角點圖像坐標為X2=(u2,v2,1)T，兩者之間的變換關系為

X2=HX1

(1)

式中：H=(h1,h2,h3)T為3×3的單應矩陣，變換式(1)有

[X2]×HX1=0

(2)

其中：[]×表示向量之間的叉乘。由式(2)得

(3)

即單應矩陣的求解轉換為最小二乘問題AX=0，其中hi表示H矩陣的第i行。對A做奇異值解得A=UDVT取V最后一列，即為單應矩陣H。

得到單應矩陣后，使用單應矩陣把每一位編碼投影到圖像中得到相應的坐標，根據圖像中該像素點的灰度值和閾值，決定該點編碼為0還是1對49個位置解碼得到最終的二進制編碼串，從而實現二維編碼點的解碼。把解碼結果和編碼數據庫進行對照匹配，獲得最終的編碼ID。如果在數據庫中沒有該編碼，則舍棄該四邊形。為了使識別結果更加魯棒，在設計二維編碼點時應遵循以下原則：

1) 同一編碼點一次順時針旋轉90°，可以獲得4種不同的編碼(沒有對稱的情況)。為了使二維編碼點具有旋轉不變性，4種編碼點只使用一種，其余編碼情況舍棄。同時避免對稱的二維編碼點設計。

2) 舍棄簡單的二維編碼點設計，如全黑和全白。

3) 二維編碼點編碼值之間漢明距離大于一定的閾值，使之易于區分。

二維編碼點的最終檢測結果包含4個有序角點像素坐標和編碼ID，同時可以通過4個角點坐標計算出編碼點的中心坐標。因此，結合編碼ID，每個二維碼可以提供5個特征點用于特征匹配。每個特征點的ID和像素坐標使用散列表存儲，匹配通過散列表的查找實現，在速度和準確率上都優于依靠特征描述子的特征匹配方法。

2 全局稀疏地圖重建算法

基于上述的特征點匹配方法，提出一種全局稀疏地圖的重建算法，流程如圖3所示。重建算法將相機內參數加入到參數列表中，省去了相機標定的過程。

圖3 全局稀疏地圖重建流程圖Fig.3 Flowchart of global sparse map reconstruction

2.1 基于多模型的兩視圖初始化

初始化是指由最初兩視圖建立世界坐標原點和初始三維點的過程，其結果的好壞直接影響隨后重建算法的收斂性。初始化必須被謹慎處理，因此同時采用單應矩陣初始化模型和本質矩陣初始化模型[16]。用H表示初始化的平面單應矩陣，E表示初始化的本質矩陣。對于初始化結果，采用如下的評價方式：

SH=∑d(x1,Hx2)+∑d(x2,H-1x1)

(4)

(5)

(6)

若R>0.6，則選擇平面單應模型；反之，則選擇本質矩陣的初始化模型。由經驗值可以看出，因為是在平坦地面鋪設的二維編碼點，所以更傾向于使用平面單應模型進行初始化。

2.2 分參數塊優化地圖參數

初始化過后，對于每一幀新到來得圖片，使用三角化算法和迭代PNP算法[16]，進行新的三維點生成和新相機的注冊。值得注意的是，為了在AGV行駛過程中沒有盲區，每個二維碼提供的特征點都需要記錄在電子地圖中。因此在三維重建過程中，并不判斷新恢復的特征點質量的好壞，也不舍棄任何特征點，對于三維重建結果較差的點，通過優化算法進行優化更新。

三維地圖重建的過程中，相機參數并沒有被標定，而是被賦予了一定的初值，三維點和相機姿態的初始值也是不準確的，需要不斷對相機內參、相機姿態和三維點進行優化。優化的指標通常為重投影誤差，即三維點通過相機投影模型投影到圖像中的坐標，和拍攝圖片中對應點像素觀測值的殘差。用數學表達式為

(7)

式中：π(·)表示相機的投影模型，采用針孔成像模型；Pin為相機內參數，包括焦距、主點和畸變，拍攝過程中相機內參保持不變；Pex={Ri,ti}i∈φ為相機的位姿參數，包括旋轉和平移，φ為所有的圖片集合；Xw={xk,yk,zk}k∈p為所有三維點的坐標，p為所有的三維點集合。對于該問題，待優化參數非常多，在優化求解式(7)的過程中非常容易發散。為了使算法收斂，需要給每個待估計的狀態量賦予良好的初值。對于相機外參數Pex，使用迭代PNP算法的結果作為初值；對于三維點Xw，使用三角化的結果作為初值；對于相機內參數Pin，以像素為單位的焦距f由式(8)確定：

f=Fκ

(8)

算法1優化重投影誤差算法。

輸入:相機內參數Pin，相機外參數Pex，三維點坐標Xw。

輸出:參數優化結果。

步驟7返回步驟2，直至所有圖片處理完畢。

分參數塊優化的策略使得算法對于較差的初始值魯棒性更強。通過不斷剔除參數，避免了參數規模的無限制增長，使得算法更容易收斂。

3 基于稀疏地圖的定位技術

AGV的定位通過提取圖像中的二維編碼點和稀疏地圖進行數據關聯，求解PNP實現。AGV位姿的解算結果包括航向(和世界坐標系x軸的夾角)和位置(x,y坐標)2部分。如圖4所示，一共有世界坐標系(Ow)、相機坐標系(Oc)和AGV坐標系(Ov) 3個坐標系。世界坐標系的xy平面建立在地平面上；使用激光跟蹤儀將AGV坐標系建立在AGV正中心，并使得x軸朝前、z軸垂直向上。Tcw為PNP解算結果，Tcv由事先標定得到。Tab為將b系坐標下的點轉換到a系下所使用的變換。

圖4 坐標系轉換關系Fig.4 Transformation relationship of coordinate system

由坐標轉換關系，推導AGV小車在世界坐標系下的平移和旋轉為

(9)

(10)

AGV正方向為rv=(1,0,0)T，將其轉換到世界坐標系下：

rw= (Xrw,Yrw,Zrw)T=Rwvrv

(11)

設正航向和世界坐標系x軸的夾角為θ，得到航向角度為

(12)

因為世界坐標系xy平面建立在地平面上，取tvw的前兩維坐標即為AGV的x坐標和y坐標。

4 實驗和實際應用

4.1 大規模地圖構建實驗

在55 m×15 m的工業環境中以0.2 m的間距鋪設二維編碼點，共20 671個。為了減少相機噪聲帶來的影響，提高二維編碼點角點提取的精度，使用尼康D810單反拍攝高分辨圖片(7 360像素×4 912像素)。相機使用焦距為24 mm鏡頭，視覺感器有效像素為3 635萬，尺寸大小為35.9 mm×24 mm，計算得到相機內參初值fx=fy=4 979.512。設置主點的初始參數為Cx=3 680，Cy=2 456.5。共順序拍攝540張照片(圖5)。建圖采用離線的方式，整個過程需要5 h左右。最后以場地的短邊作為參照恢復尺度，并將稀疏地圖轉換到以0號編碼點為原點，xy平面建立在地面，x為場地長邊，z軸朝上的坐標系。重建結果如圖6所示。為了評定重建結果，使用地面的二維編碼點邊長作為參考，二維編碼點使用360 dpi(dpi為每英寸點數)的打印機打印，邊長的理論值為(28.00±0.07) mm。使用統計方法的評定結果見表1。隨機選取地面上的編碼點，用精度為1 mm卷尺進行小、中和大3種尺度的測量，并和重建稀疏地圖中相應的距離比較，比較結果如圖7(a)～(c)所示。小、中、大3種尺度的平均誤差分別為0.5、5.2、6.7 mm,誤差的標準差分別為0.4、2.1、4.8 mm。

圖5 用于重構的原始圖像Fig.5 Original image for reconstruction

圖6 用于定位的稀疏地圖Fig.6 Sparse map for localization

表1 二維編碼點邊長重構統計結果Table 1 Statistical results of two-dimensional code side length reconstitution mm

4.2 AGV定位實驗

搭載視覺定位系統的AGV如圖8所示，矩形標注的部分即為視覺定位系統。AGV使用麥克納姆輪，能在擁擠的工業環境中最大限度地減少轉彎半徑。車體高度為293 mm，中間部留有720 mm×800 mm的空間，用于安裝視覺定位系統。由于高度較低，相機視場較小，因此使用2個相機，其中一個相機作為另一個相機的輔助相機，當主相機視野中沒有檢測到二維編碼點時，使用輔助相機進行檢測。該系統選用2個大恒MER-200-20GM-P工業相機，該型號相機幀率為20幀/s，分辨率為1 628像素×1 236像素，使用觸發器控制2個相機同時獲取圖像；在側面下部安裝2個條形光源，減少光照變化帶來的影響。

為了驗證該定位方法的可行性，在真實環境中分別對AGV的主要狀態進行定位實驗。

4.2.1 靜態定位實驗

AGV的最常見的狀態停在某個車位長時間靜止。將AGV停在某一車位，并記錄x、y和角度的數據，結果如圖9所示。由圖9可知，定位結果穩定且沒有漂移。x坐標最大變化范圍為0.21 mm，重復定位精度為0.03 mm；y坐標最大變化范圍為0.19 mm，重復定位精度為0.01 mm；角度最大變化范圍為0.12°，重復定位精度為0.01°。

圖7 測量值和重建值結果的對比Fig.7 Comparison of measurement and reconstruction results

圖8 搭載視覺定位系統的AGVFig.8 AGV equipped with visual localization system

圖9 靜態定位實驗Fig.9 Experiment of static localization

4.2.2 動態定位實驗

使用控制器控制AGV在B201、B104兩個車位之間反復行駛。每次使AGV停在B201車位相同的起點，設定路徑如下：從B201車位出發，前進6.23 m到達行車道，左轉90°前進2.80 m，右轉90°前進6.12 m進入B104車位。使用控制器控制AGV在該預定路徑行駛，并記錄定位系統的輸出。記錄的10次往返過程中起點和終點的停車定位數據見表2。x坐標最大變化范圍為1.0 mm，重復定位精度為0.3 mm；y坐標數據最大變化范圍為0.9 mm，重復定位精度為0.2 mm；角度最大變化范圍為0.9°，重復定位精度為0.3°。

為了能讓AGV每次都準確進入狹窄的車位，需要很高的軌跡重復精度。對于整條行駛路徑的重復定位精度，采用KITTI[17]中視覺里程計的評價方式。該方法用于評價2條軌跡之間的相近程度，最后結果用2條軌跡之間的位置誤差表示，越小則說明2條軌跡越相近。按照原始方法，設置評價尺度為10 m。對于記錄的定位數據進行處理，每一組數據從第10幀開始運動，直到終點停止，共記錄951幀數據。記錄的4組軌跡如圖10所示，對記錄的4組定位數據兩兩進行比較，獲得最終結果見表3。軌跡重復定位精度(即平均位移誤差)為0.082%，軌跡的重復精度很高。