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

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

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

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

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

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

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

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

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

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

1.1 二維編碼點的設計

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

圖1 編碼點Fig.1 Coded points

1.2 二維編碼點的檢測

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

1.2.1 圖像預處理

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

1.2.2 四邊形檢測

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

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

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

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

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

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

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

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

1.3 二維編碼點的解碼

如圖2(g)所示，解碼通過二維平面點的單應變換實現(xiàn)。首先在理想二維編碼點上建立坐標系，同時獲得所有編碼位置的坐標，并將其投影到實際圖像中。理想二維編碼點的坐標系建立在中心，從左下角開始，按順時針順序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。

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

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

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

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

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

2 全局稀疏地圖重建算法

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

圖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 分參數(shù)塊優(yōu)化地圖參數(shù)

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

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

(7)

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

f=Fκ

(8)

算法1優(yōu)化重投影誤差算法。

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

輸出:參數(shù)優(yōu)化結果。

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

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

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

AGV的定位通過提取圖像中的二維編碼點和稀疏地圖進行數(shù)據(jù)關聯(lián)，求解PNP實現(xiàn)。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 大規(guī)模地圖構建實驗

在55 m×15 m的工業(yè)環(huán)境中以0.2 m的間距鋪設二維編碼點，共20 671個。為了減少相機噪聲帶來的影響，提高二維編碼點角點提取的精度，使用尼康D810單反拍攝高分辨圖片(7 360像素×4 912像素)。相機使用焦距為24 mm鏡頭，視覺感器有效像素為3 635萬，尺寸大小為35.9 mm×24 mm，計算得到相機內參初值fx=fy=4 979.512。設置主點的初始參數(shù)為Cx=3 680，Cy=2 456.5。共順序拍攝540張照片(圖5)。建圖采用離線的方式，整個過程需要5 h左右。最后以場地的短邊作為參照恢復尺度，并將稀疏地圖轉換到以0號編碼點為原點，xy平面建立在地面，x為場地長邊，z軸朝上的坐標系。重建結果如圖6所示。為了評定重建結果，使用地面的二維編碼點邊長作為參考，二維編碼點使用360 dpi(dpi為每英寸點數(shù))的打印機打印，邊長的理論值為(28.00±0.07) mm。使用統(tǒng)計方法的評定結果見表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 二維編碼點邊長重構統(tǒng)計結果Table 1 Statistical results of two-dimensional code side length reconstitution mm

4.2 AGV定位實驗

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

為了驗證該定位方法的可行性，在真實環(huán)境中分別對AGV的主要狀態(tài)進行定位實驗。

4.2.1 靜態(tài)定位實驗

AGV的最常見的狀態(tài)停在某個車位長時間靜止。將AGV停在某一車位，并記錄x、y和角度的數(shù)據(jù)，結果如圖9所示。由圖9可知，定位結果穩(wěn)定且沒有漂移。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 搭載視覺定位系統(tǒng)的AGVFig.8 AGV equipped with visual localization system

圖9 靜態(tài)定位實驗Fig.9 Experiment of static localization

4.2.2 動態(tài)定位實驗

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

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