融合圖像與點云處理的托盤定位系統

徐 斌，趙 鵬，劉 偉，耿牛牛，康照奇

（機科發展科技股份有限公司，北京 100044）

0 引言

現代物流的快速發展，使得自動導引運輸車(Automated Guided Vehicle，AGV)等倉儲搬運機器人發揮著更加重要的作用。托盤定位是指在當前復雜的倉儲環境中檢測出托盤目標，并計算其坐標位置與姿態，是AGV實現自動搬運貨物的核心技術之一。現有的倉儲托盤定位方法主要包括兩種，基于視覺或者基于激光雷達的方法。激光雷達傳感器價格昂貴，且其采集的數據較為稀疏，相較于此，視覺傳感器具有成本低，采集數據全面、可提取特征豐富的特點，更適用于實際應用。

基于視覺的托盤定位主要通過人工定義特定特征的方式從圖像中分割出托盤。文獻[1]提出了一種基于顏色精確分割的托盤視覺識別系統，通過獲取托盤顏色特征，然后采用形態學濾波、Sobel邊緣檢測和Hough變換等算法獲取托盤目標。但復雜背景與外部環境光照等因素，會對顏色特征提取造成干擾，算法的魯棒性較差。Seelinger等人[2]使用視覺標簽檢測托盤的方法，此類方法一般在托盤的立柱面貼二維碼或條碼等視覺標志，檢測系統通過尋找視覺標志確定托盤位置，一旦視覺標志受到污損就會影響托盤檢測結果。SYU等人[3]提出基于Harr-Like特征的方法檢測托盤，算法魯棒性較強，但受限于視覺傳感器與托盤的相對位姿，算法在實際應用有很大的局限性。

當前，三維視覺定位方法主要包含基于局部與全局特征兩種。局部特征的方法通過對每個點計算與其周圍鄰域相關的特征向量，這使得局部特征描述子不對場景分割也可描述復雜場景。點特征直方圖(FPH)[4]與快速點特征直方圖(FPFH)[5]為相對早期提出的局部特征描述子。具有更好的平移、旋轉不變性的SHOT[6]特征描述子在后期也被提出?；诰植棵枋鲎?，逐漸形成了基于局部特征匹配的目標識別框架。對于基于全局特征的方式，是將視角作為特征構建多視角下的2.5D模型特征庫，目標識別結果以及位姿根據模板庫對應給出，VFH[7]全局特征即為帶視角的特征直方圖。吳等人[8]提出一種基于描述全局表面曲率變化信息的特征描述子PPF的托盤定位方法，但只能實現近距離范圍的托盤定位。武等人[9]提出一種基于點云平面輪廓匹配的檢測方法，該方法依賴于原始點云濾波的結果，包括基于距離的直通濾波與基于顏色分量的濾波，算法泛化能力不強。相比于局部特征的方式，全局特征方式的缺陷是場景分割結果的好壞直接影響目標識別結果。

綜上所述，二維圖像處理技術無法獲取目標的空間深度信息，且因倉儲環境背景復雜、光線變化大、托盤位姿不確定性等難點，使得算法無法準確提取托盤目標特征。三維點云處理技術，因點云數據冗余使得目標識別效率較低，且當環境復雜時易對特征匹配造成干擾?；诖耍瑸閷崿F倉儲環境下托盤的精確定位，本文進一步進行研究，提出了融合圖像與點云處理的托盤定位系統。

1 融合圖像與點云處理的托盤定位系統

托盤定位系統首先由基于飛行時間(ToF)原理的Basler blaze-101相機采集數據，包括像素格式為640×480的灰度圖像以及與灰度圖像所示場景對應的3D點云數據，并將其傳輸給托盤定位系統；其次，托盤定位系統通過深度學習目標檢測算法獲取托盤在場景圖像中的像素位置，基于此結合3D點云數據提取感興趣托盤點云。然后，托盤點云模板與感興趣托盤點云作為輸入數據，經粗配準與二次校正得到精確托盤點云。最后，基于精確托盤點云計算托盤的位姿數據。本系統應用于倉儲環境下AGV的托盤叉取任務，具有精度高、效率高、魯棒性強的優點。

1.1 感興趣托盤點云提取算法

倉儲環境的復雜使得采集的3D點云數據包含大量冗余無效的環境數據，因此無論采用基于局部特征匹配還是基于全局特征匹配的目標識別，算法的計算效率均較低，且會影響對托盤的特征描述。因此，本文提出一種基于深度學習目標檢測的感興趣托盤點云提取算法。深度學習目標檢測出色的檢測準確率保證了倉儲環境下托盤目標的精確檢測，得到托盤在圖像中的像素位置也將與原始3D點云數據對應，為后續點云配準提取較為準確的輸入數據，同時有效地過濾了大量冗余的環境數據、提高了計算效率。感興趣托盤點云提取算法主要包含托盤目標檢測與點云提取兩部分。

1.1.1 托盤目標檢測

當前，深度學習目標檢測已經被廣泛應用于自動駕駛、機器人視覺、工業質檢等領域。高性能的二階段目標檢測模型[10，11]已發展成熟，但其本身的網絡結構對硬件資源的消耗，使得其難以實現實時性檢測，因此并不能最優地應用于實際工業中。一階段目標檢測模型，如YOLO系列[12]網絡模型能夠更好地平衡檢測精度與效率，并逐步在工業中得到廣泛應用，但其本身存在兩個問題：對不同的數據集需要重新設置錨框以及正負樣本不均衡。因此，本研究采用高效率高精度的無錨框輕量級檢測模型PicoDet[13]進行工業倉儲環境下的托盤圖像目標檢測，獲取托盤在圖像中的像素位置。

1）BackBone網絡

PicoDet 模型采用超輕量、高精度骨干網絡ESNet(Enhanced ShuffleNet)，ESNet骨干網絡基于ShuffleNetV2網絡，引入了SE模塊，SE模塊可以對網絡通道權重進行調整以獲取更優的特征。SE模塊新增了通道卷積和點卷積，通過融合不同通道信息實現了降低實際參數量的同時保證了模型精度，如圖2(a)所示。同時，為進一步增強ESNet網絡的性能，在步長為1的SE中引入Ghost模塊，實現以更少的參數生成更多的特征圖以此來提高網絡的特征學習能力。另外，ESNet網絡采用神經網絡搜索Neural Architecture Search(NAS)搜索更高效的模型結構?；诖?，ESNet骨干網絡在精度、速度上得到了全方面提升，整個目標檢測模型計算量更小、延遲更低、精度更高。

圖1 PicoDet網絡結構

圖2 ES模塊結構

2）Neck網絡和Head

在Neck部分，PicoDet提出了CSP-PAN網絡結構，使用1×1卷積將所有特征的通道數與最小通道數保持相同，計算量減少的同時保證特征融合性能不受影響，而后通過CSP結構實現自頂向下和自底向上的特征融合。同時，在CSP-PAN的頂部增加了一個特征圖尺度，以此檢測更多尺度的目標物體。PicoDet在Neck和Head除了1×1卷積外均采用卷積核為5×5深度可分離卷積來擴大感受野，在使用參數量較少的情況下大幅度地提升了精度。整體的網絡結構如圖1所示。

PicoDet模型的Neck和head部分都有四個尺度分支，且head部分的通道數與neck部分保持一致，之后將分類和回歸分支耦合在一起。PicoDet還使用了SimOTA動態標簽分配策略優化訓練過程，有效加速模型收斂。

1.1.2 托盤點云提取

基于PicoDet目標檢測模型出色的檢測準確率保證了倉儲圖像托盤的精確檢測，得到了托盤在圖像中的像素位置(X，Y，W，H)，(X，Y)表示托盤左上角在圖像中的像素位置，(W，H)分別表示托盤的像素寬度和像素高度。由于Basler ToF相機獲取640×480像素分辨率的圖像，且對應點云數據按行保存，因此，托盤左上角像素點對應的點在三維點云數據存儲中的索引號為i=640×Y+X。

圖3 倉儲環境二維圖像與點云數據

考慮目標檢測所得像素位置存在誤差，因此增加補償量v=10，即將托盤左上角的起始點設置為i=640×(Y-10)+(X-10)，盡可能保證托盤點云包含在提取的感興趣點云中。同時，托盤目標像素位置對應的參數范圍設置為(X-10，Y-10，H+20，W+20)，即托盤目標的像素寬度和像素高度擴大為H+20和W+20?；诖?，結合采集的三維點云數據進行感興趣托盤點云提取，此算法有效地過濾了大量冗余的環境數據，提高了后續托盤目標配準的計算效率。具體提取效果見實驗部分。

1.2 托盤定位算法

托盤模板點云與感興趣托盤點云作為輸入數據，通過粗配準與二次校正的點云配準算法，獲取精確托盤目標點云，然后計算托盤目標的中心坐標與位姿。本文采用剛性位姿估計[14](Sample Consensus Prerejective，SCP)作為粗配準方法，迭代最近點算法[15](Iterative Closest Point，ICP)進行二次校正。

1.2.1 粗配準與二次校正

本研究采用基于快速點特征直方圖FPFH局部特征描述子的托盤識別方法，首先將感興趣托盤點云及其FPFH特征描述子、托盤模板點云及其特征描述子作為粗配準算法SCP的輸入，得到托盤模板點云與感興趣托盤點云之間的變換矩陣。而后將感興趣托盤點云、托盤模板點云，以及粗配準所得變換矩陣作為二次校正算法ICP的輸入，得到精確托盤點云。

本文基于托盤M型立柱面制作托盤模板點云，一方面其明顯的結構特征有助于托盤特征提取，另一方面大大減少了表征托盤的點云數據量，提高了托盤點云配準的計算效率。圖4分別是托盤的實物圖與制作的M型立柱面點云圖。另外，對制作的托盤模板點云，執行點云體素濾波，體素濾波使用體素化網絡實現點云的下采樣，在減少點的數量的同時，能夠保持點云的形狀特征。

圖4 托盤實物圖及其立柱面M型點云模板

1）粗配準算法SCP

在托盤定位過程中，首先采用剛性物體位姿估計(Sample Consensus Prerejective，SCP)算法完成托盤目標的粗配準。SCP算法具體步驟為：

第一步，定義變換矩陣，見上方公式，使得托盤模板點云P上的任意一點p與感興趣托盤點云Q中的任意一點q之間的距離平方和最小化。

第二步，通過最近鄰匹配，在托盤模板點云P中找到n≥3個隨機對象點，并在感興趣托盤點云Q中找到它們對應的點。

第三步，使用n個采樣對應點估計一個假設變換T，并將假設變換矩陣T應用于托盤模板點云P上。

第四步，采用空間最近鄰域搜索方法在變換后的托盤模板點云與感興趣托盤點云Q之間找到內點，然后采用歐氏距離閾值進行判斷，如果內點數低于設定閾值，返回第二步。

第五步，使用內點對應重新估計一個假設變換。

第六步，通過將內點代入上述公式，計算∈(T)的值，如果值為目前為止的最小值，設置當前T作為托盤模板點云與感興趣托盤點云之間的變換矩陣。

2）二次校正算法ICP

在粗配準之后，使用迭代最近點算法ICP進行二次校正，下面是ICP算法的具體步驟：

第一步，定義均方誤差，其中Ps和Pt是托盤模板點云和感興趣托盤點云中的對應點，R和t分別表示旋轉矩陣和平移向量。

第二步，根據SCP獲得的對應關系“變換矩陣”，對托盤模板點云施加初始變換T，得到新的點云。

第三步，在感興趣托盤點云Pt中尋找與Ps最近的點云，形成對應點對。

第四步，計算ε(T)的值。

第五步，設置誤差閾值和迭代次數閾值，直到均方誤差值小于誤差閾值則收斂，若不收斂返回第一步。

1.2.2 托盤位姿計算

本文基于二次校正后得到的精確托盤點云，計算托盤的定位信息，包括托盤的中心坐標以及姿態。首先，計算精確托盤點云在相機坐標系下各坐標軸對應的最大值和最小值：Xmax、Xmin、Ymax、Ymin、Zmax、Zmin。然后，計算各個坐標軸對應的最大值和最小值的平均值，確定為托盤點云的中心坐標，即

對于托盤目標的姿態，將精確托盤點云投影于各個坐標軸對應的坐標平面，利用最小二乘法將投影而得的散點擬合成一條直線，確定坐標軸與直線的夾角，即為托盤與各個坐標軸的夾角。

1.3 路徑規劃

AGV取貨路徑規劃主要是解決取貨端由于托盤位姿發生變化而導致AGV無法加載貨物的問題。如圖5所示，AGV在進行路徑規劃時，需要以自身位姿狀態XI和托盤目標位姿狀態XT分別作為起始狀態和終止狀態進行路徑規劃。根據貝塞爾曲線的幾何不變性，將AGV的位姿作為規劃原點，即；托盤在車身坐標系下的位姿作為目標狀態，即XT=[x，y，θ]。從而將自主路徑規劃問題轉化為對貝塞爾曲線的規劃。

圖5 貝塞爾曲線路徑規劃

2 實驗結果與數據分析

基于PicoDet模型的二維圖像托盤目標檢測，實驗訓練環境基于Windows10平臺系統，32GB內存，以及NVIDIA Geforce RTX 3080上完成。測試實驗基于Windows10、Inter i7-10510U，16G內存的處理器環境下完成的。

2.1 效率評估

為了使得本文提出的托盤定位系統滿足工業應用對實時性的要求，針對第一階段的感興趣托盤點云提取部分，本文選擇了當前深度學習目標檢測領域高精度、高效率的三種模型，分別是yoloV3、ppyolo以及picodet模型，進行了托盤目標檢測效率評估實驗，如表1所示。分別對三種目標檢測模型進行了訓練，并在測試實驗環境中測試圖像檢測效率。經過評估，picodet模型的檢測效率最高，因此，本研究基于該模型完成感興趣點云提取算法。

表1 托盤圖像目標檢測時間

基于感興趣點云提取算法所得的感興趣托盤點云，是濾除了大量冗余的環境點云數據后所得。本研究基于感興趣托盤點云，對粗配準SCP算法與二次校正ICP算法進行了效率評估，如表2所示。

表2 托盤粗配準與二次校正時間

可以看出，感興趣托盤點云提取與兩次配準進行托盤識別的時間總和不超過1s，滿足工業實時性要求。

2.2 感興趣托盤點云提取實驗評估

2.2.1 數據集

本文將采集的原始倉儲環境的二維圖像，進行縮放、對比度增強、噪聲等操作，來增大目標檢測模型訓練的數據量，以此來提高模型對托盤的特征學習能力，并抑制復雜背景對托盤檢測的干擾。數據集的所有圖像都采用LabelImg軟件進行標注。采用不同尺寸矩形框手動標記托盤位置，并給定類別標簽，制作了托盤圖像數據集，隨機分配構成檢測模型的訓練集和測試集。表3顯示了數據集配置。

表3 托盤圖像數據集

2.2.2 目標檢測評估

對于圖像的托盤目標檢測，本文采用平均精度（Average presion，AP）評估模型的性能。AP的計算與準確率（precision，ppre用表示）、召回率（recall，用ppre表示）有關，對于給定的類別，其分別定義為：

其中TP，為分類正確的托盤目標的個數，FP為背景分類為托盤目標的個數，FN是托盤正樣本被錯誤地分為背景的個數。

本文在測試實驗環境下，當檢測的托盤目標的IOU大于設定閾值0.5時，認定目標為托盤，實驗測試結果為模型的AP值達到了98.6%。圖6是托盤檢測結果，可以看到，在光線較暗的倉儲環境下依舊可以檢測到托盤目標，實驗結果表明該模型的魯棒性較好。

圖6 圖像目標檢測托盤結果

2.2.3 點云提取評估

感興趣托盤點云提取可以有效地濾除大量冗余的環境點云數據，這些數據不僅會增大后期點云配準的計算效率，而且會對托盤點云的特征提取造成干擾。表4進行了幾組感興趣點云提取前后的點云數量對比，可以看出，點云數量在使用感興趣托盤點云提取算法之后，點數量驟減。

表4 托盤點數量前后對比

圖7是感興趣托盤點云提取效果，其中(a)和(b)表示深度學習檢測所得的托盤目標位置，(c)和(d)表示與其對應提取的托盤點云數據?？梢钥吹?，托盤點云數據可以被準確的提取出來。

圖7 感興趣點云提取

2.3 點云配準實驗

圖8展示了托盤圖像目標檢測、感興趣托盤點云提取、以及兩次配準后得到的精確托盤點云結果。其中，綠色點云表示當前倉儲環境的點云數據，紅色表示最終獲取的精確托盤點云，可以看出，精確托盤點云基本可以與環境點云數據中的托盤點云完全重合。實驗結果表明，融合圖像與點云處理的托盤識別定位系統可以準確地實現托盤定位，并滿足工業應用的實時性要求。

圖8 托盤定位實驗過程

另外，通過實驗進行了分析驗證。對固定同一位置的托盤重復進行了十次實驗，對托盤定位的實驗結果，即中心點坐標以及姿態數據，進行了記錄，如表5所示。實驗結果表明，十次數據之間的重復定位誤差相差不大，滿足精度要求，算法具有一定的穩定性。

表5 重復定位誤差計算表

3 結語

本文采用高精度、高效率的深度學習目標檢測模型PicoDet對二維倉儲環境圖像進行托盤檢測，將獲取的托盤像素位置與3D點云數據對應，提取感興趣托盤點云，有效地濾除了冗余的環境點云，為托盤點云配準提供精確輸入數據的同時，有效地提高了算法效率。此外，采用粗配準方法與二次校正的點云配準算法進行托盤識別，并基于得到的精確托盤點云進行位姿估計。實驗結果表明，本文提出的方法能夠克服復雜環境的干擾，高精度高效地完成托盤目標定位，滿足工業應用需求，為AGV叉取托盤提供有效的方法。