基于深度學習的物體點云六維位姿估計方法

李少飛，史澤林，莊春剛

（上海交通大學機械與動力工程學院，上海 200240）

0 概述

散亂場景中的三維物體拾取是機器人操作中的一類經典問題，利用機械臂將箱子中無序擺放、堆疊的物體取出對機器人實現自動化具有重要意義。該問題的難點在于散亂堆疊的物體之間存在大量的遮擋，這不僅影響了對物體的識別，而且使得拾取過程中的碰撞檢測更加復雜。物體六維位姿識別是散亂場景中三維物體拾取的重點和難點。近年來，深度學習技術在六維位姿估計任務中得到廣泛應用。文獻［1-3］根據RGB 數據對紋理豐富的物體實例進行六維位姿估計。文獻［4］擴展二維目標檢測器，提出一種基于分類離散視點的旋轉位姿估計方法，但該方法僅預測真實姿態的粗略離散近似值，為達到更好的效果，還需對輸出結果進行位姿細化。文獻［5］先將RGB 圖像在2 個網絡中進行由粗到細的分割，再將分割結果反饋給第3 個網絡得到待檢測目標邊界框點的投影，最終利用PnP 算法估計六維位姿，但該方法由于將網絡分為多個階段，因此導致運行時間非常長。文獻［6］針對通過CNN 檢測二維關鍵點并利用PnP 回歸六維位姿的方法在遮擋和截斷樣本中存在的問題進行改進，對于每一個像素計算一個指向二維關鍵點的方向向量，并通過投票策略得到魯棒的二維關鍵點，減少了物體局部缺失對位姿估計的影響。文獻［7］通過訓練獲取輸入RGB 圖像的六維隱變量表示，然后在數據庫中查找和其最相近的位姿作為估計結果。然而，在低紋理的情況下，僅通過RGB 信息估計的六維位姿準確率較低。文獻［8-10］將RGB 信息和深度信息相結合估計目標的六維位姿。文獻［11-12］均是利用CNN 學習特定的描述子進行目標檢測和六維位姿估計。從RGB-D圖像進行六維目標位姿估計的關鍵是充分利用兩個互補的數據源，文獻［13］提出一種新的稠密融合網絡，該網絡將分別處理后的兩種數據源進行像素級別的特征嵌入，從而估計出物體的六維位姿。

近幾年，基于深度學習的六維位姿估計方法多數將RGB 圖和深度圖作為輸入。然而，一個物體處于不同的位姿卻有著相似的二維圖像這一現象是很常見的，這限制了基于二維圖像的位姿估計的準確率。在一些工業應用中，為了獲取完整場景、高精度的三維信息，通常會采用三維掃描儀獲取場景點云，而有些掃描儀由于成像原理不同，不能獲取RGB 圖和深度圖。隨著傳感器技術的發展，獲取三維點云的速度得到了大幅提升，這使得基于點云研究的實時性得到了保障。因此，基于點云的物體六維位姿估計引起了研究人員的關注。DROST 等［14-15］提出基于物體點對特征（Point Pair Feature，PPF）的位姿估計算法及其變體算法，并將其成功應用于工業機器人分揀任務，然而此類算法的局限性在于：一方面，如果模板點云和場景點云的采樣疏密程度不一致，將難以發現相似點對特征，從而導致匹配錯誤；另一方面，出現了一些先分割后配準的算法，將點云進行聚類分割后，利用點云配準的流程得到物體的位姿［16］，但是此類算法計算量大，且在堆疊嚴重的場景中表現較差。在深度學習領域，QI 等［17］基于對稱函數思想，將原始點云輸入網絡進行訓練實現分類和分割任務，并在網絡中加入分層多尺度特征學習［18］，該方法相比已有方法在精度上有了顯著提升。之后研究人員將該方法應用于自動駕駛的目標檢測提出F-PointNet［19］，F-PointNet 雖然在一定程度上解決了三維目標檢測問題，但是激光雷達獲得的點云是稀疏和不規律的，在自動駕駛場景中的物體也鮮有遮擋的情況，并且包圍框的位姿也僅考慮垂直于地面的旋轉，這與散亂場景中堆疊的工件有很大的差別，因此此類方法的實用性不強。

針對現有點云位姿估計方法計算量較大且在復雜場景中結果魯棒性較差的問題，本文提出基于深度學習的物體點云六維位姿估計方法，將三維點云映射到二維平面，生成深度特征圖和法線特征圖，提取位姿特征。

1 數據集生成

1.1 工業零件建模

現有基于深度學習的六維位姿估計方法多數是在已有的LINEMOD、OCCLUSION 等數據集上進行測試。但是，由于工業零件的特殊性，在這些數據集上測試效果很好的神經網絡并不能適用于一些低紋理的機械零件，因此本文提出了一種用于工業零件位姿估計的數據集生成方法。

在對數據集進行標簽標注時，點云的標簽標注相比二維圖像標注更加困難。每訓練一個新的工件，如果用真實點云生成數據集，則工作量會非常巨大，因此在仿真環境下生成數據集用于訓練是很有必要的。文獻［20］考慮了環境光反射的影響，利用Unity3D 游戲引擎生成散亂堆疊場景的深度圖數據集。文獻［21］利用Blender API 將提前建好的日常用品的三維模型放入仿真環境，設置模型初始位姿，并通過重力掉落以及剛體碰撞模擬真實環境。上述仿真方法均能達到較好的效果，但是所仿真模型的幾何結構都是類似于圓柱體、立方體等簡單的模型，而對于一些復雜的工件，首先建模精確度較低，其次仿真會出現穿模現象。

本文對文獻［21］所采用的物理仿真方法進行改進，在Blender API 中根據模型紋理、矩形包絡、球包絡等方式選擇物理的碰撞類型。基于模型紋理的物理仿真方法會在模型面數較多時出現計算復雜度高的問題，從而引起穿模，而基于矩形包絡、球包絡等的物理仿真方法雖然可以避免模型之間產生穿模現象，但是模型形狀的簡化會使工件之間的堆疊不能反映真實場景中的碰撞堆疊效果。因此，本文首先利用高精度的三維掃描儀，拍攝工件多個角度的三維點云并進行配準，得到工件的完整點云；接著采用貪婪投影三角法進行曲面重建，得到復雜工業零件的精確模型，如圖1（a）所示。為了盡可能減少模型面數從而減少仿真計算量，并保證物理碰撞盡可能與真實場景相似，本文對每一個特定的工業零件，實心化對物理碰撞不會產生影響的局部區域，而對于產生碰撞的區域，使用相對簡單的形狀進行包絡擬合，如圖1（b）所示。在圖1 中，本文采用的4 種工件從上到下依次為軸承座1、軸承座2、連桿和榔頭。

圖1 精確模型與簡化模型Fig.1 Exact model and simplified model

1.2 基于物理仿真的數據集生成

本文數據集生成的步驟如下：1）將多個簡化的工件模型預設置隨機位姿并置于環境上方；2）工件依靠重力下落，基于模型紋理產生碰撞散亂堆疊在相機視野下，然后渲染得到每個工件的掩碼與之后生成的深度圖對應得到點云的類別標簽；3）在獲取堆疊工件位姿后，在Bullet 中用重建的精確點云模型代替簡化模型，渲染得到深度圖，進而獲得散亂場景的點云，如圖2 所示。這樣就可以使得仿真生成的散亂堆疊工件的點云以及工件之間的碰撞效果和真實場景盡可能相似，防止由于模型面數過多造成穿模問題。由于Blender 中的工件在世界坐標系下的坐標變換為因此需要將其轉換到相機坐標系下，已知相機在世界坐標系下的坐標變換為則工件在相機坐標系下的六維位姿為：

圖2 散亂場景的點云仿真Fig.2 Simulation of point clouds in scattered scene

2 基于深度學習的點云位姿估計方法

直接將學習得到的原始點云特征輸入全連接層進行訓練可以達到很好的分類效果［17-18］，但對于六維位姿估計效果并不理想，因為訓練得到的全局特征和每個點的局部特征更多的是表現該工件的類別特征，而用于估計六維位姿的局部表面特征和幾何特征并未進行有效提取，僅依靠神經網絡本身參數的調整和訓練效果較差。另外，神經網絡的數據輸入維度需要保持一致，而從場景分割得到的單個點云的點數是不確定的，為了使其能夠輸入網絡，需要采樣成固定點數，這會使得工件點云變得稀疏，從而損失一定的特征。近年來研究人員提出了許多成熟的處理二維圖像的深度學習方法，因此本文將三維點云映射到二維平面，生成深度特征圖和法線特征圖并提取位姿特征，不僅保證了網絡輸入維度一致，而且大幅提高了基于點云的位姿估計準確率。

2.1 點云二維深度特征生成

在位姿估計前，本文利用ASIS 方法［22］對散亂場景的點云進行分割預處理。對于每一個分割后的單個工件點云，計算其xyz坐標的平均值xm、ym、zm，記為點云的中心，并將點云中心移動到相機坐標系原點，如圖3中A 所示，記為tO=(-xm，-ym，-zm)T并得到：

圖3 點云二維特征生成Fig.3 2D feature generation of point clouds

將點云平移到坐標原點附近可以有效減小圖像尺寸，使樣本點所占二維圖像的比例盡可能大，增加圖像特征的顯著度。點云到二維圖像的具體映射方法為：1）設定分辨率及寬度方向的像素個數，按照圖像寬高尺寸的比例設定高度方向的像素個數；2）將點投影到圖像中時，會出現一個像素中存在多個點的情況，此時僅保留z值最小的點，該點離觀測視野最近，識別度最高；3）由于二維圖像是單通道的灰度圖，因此得到點像素的灰度值為：

由于設定的分辨率不同，因此每個像素包含點的數量也會發生變化，而二維圖像的特征也會有所差別。圖4 給出了在不同分辨率下工件僅通過二維深度特征進行位姿估計的準確率。可以看出，分辨率從起始到80 像素×80 像素時，位姿估計的準確率提升得很快，再提高分辨率時，位姿估計準確率的提升開始減緩，并且約在100 像素×100 像素時達到最大，此時進一步提高分辨率，準確率開始緩慢下降。由于分辨率過大或者過小都會造成點云二維特征不夠明顯，因此在實驗階段，本文將特征圖的分辨率設置為峰頂處的100 像素×100 像素。同時，本文工件的尺寸設置為10～20 cm，如果物體尺寸大于實驗采用的工件尺寸，可以適當提高分辨率，反之亦然。筆者認為應謹慎降低特征圖的分辨率，因為從實驗結果可以看出，過大的分辨率對實驗結果的影響遠小于過小的分辨率。

圖4 不同分辨率下位姿估計的準確率Fig.4 Accuracy of pose estimation at different resolutions

2.2 點云二維法線特征生成

點云生成的二維深度特征能夠有效提取出工件的幾何特征，但是一些不同的工件或者一個工件的不同局部投影到二維平面，有可能呈現類似的形狀，即使深度不一致，也會影響最終的估計結果。如圖5（a）、圖5（b）所示，軸承座2 的正反面投影到二維平面會產生上述問題。而點云法線作為點云的一種重要的幾何屬性，已廣泛應用于特征點檢測、三維重建、薄板正反面區分等場景。傳統位姿估計算法的點對特征［14］就是運用兩點的法線特征構建特征算子，而近年來許多基于點云分類分割的深度學習研究［17-18］也將點云的表面法線作為點云的額外信息輸入網絡進行訓練，經過實驗證明，分割準確度有了明顯提升。因此，本文類比二維深度特征圖的生成方式生成點云的二維法線特征圖，用于增加二維特征的區分度，即使不同位姿樣本的二維深度圖相似，最終的位姿估計結果也不會產生誤匹配的情況。

在將點云投影到二維平面生成的深度特征圖前，利用Open3D 庫計算點云的法線，這樣二維深度特征圖中任意點像素都會包含這個點的深度值及其法線。將各點的三維法線特征和深度值分離，即可得到二維法線特征圖。本文思想是將法線特征和深度特征分成兩條支路，各自學習對應的特征，最終將網絡學到的特征信息進行融合輸出六維位姿。在二維法線特征圖生成的過程中存在兩方面的問題。一方面，通過上述方法計算出的法線并沒有經過全局定向，這會極大地影響模型對工件位姿的訓練。本文將所有法線的方向統一至與z軸負方向呈小于90°的夾角，解決了全局定向的問題，將二維法線特征圖中計算得到的法線以及該像素緩存的三維點還原成空間點云，可以看到法線的取向是統一的，如圖5（c）、圖5（d）所示。另一方面，在二維法線特征計算的過程中引入了分割后的噪聲，特別是在工件的邊緣位置處，法線的估計會因為噪聲產生很大的誤差，因此本文在實驗部分將噪聲對位姿估計結果的影響進行實驗驗證。

圖5 軸承座2 的正反面及其點云法線Fig.5 Front and back sides and their point cloud normals of bearing pedestal 2

2.3 特征融合網絡

本文提出的特征融合網絡框架如圖6 所示。特征融合網絡主要包括：1）二維深度特征提取，將點云映射為二維深度特征圖，經過預處理后輸入resnet50 預訓練模型進行預訓練，每個樣本得到2 048 維特征，經過多個全連接層后得到256 維特征；2）二維法線特征提取，投影生成二維法線特征圖后，經過多個卷積層得到通道數為1 024的特征圖，通過多個卷積核為2×2 與5×5的卷積層得到通道數為1 024 的特征圖，并經過最大池化處理平鋪生成1 024 維的全連接層，之后分為2 個支路經過全連接層分別得到256 維特征，該網絡采用Relu激活函數；3）將二維深度特征提取過程中得到的特征分別于二維法線特征提取過程中的兩條支路進行特征拼接，經過多個全連接層后，兩支路分別得到三維特征和四維特征，代表工件位姿的xyz值以及表示旋轉的四元數，將四元數轉換為旋轉矩陣后即可得到4×4 的六維位姿矩陣。

圖6 特征融合網絡框架Fig.6 Framework of feature fusion network

2.4 損失函數

在基于深度學習的位姿回歸中，常見的一種損失函數是計算使用真實位姿回歸得到的點云和使用估計位姿回歸得到的點云中對應點距離的平均值［5］，記為CPLoss，計算公式如下：

其中：M表示已事先采樣的模型點云；n表示采樣點個數；Tg、Tp分別表示標簽位姿和估計位姿。需要注意的是，網絡估計的位姿是分割后的局部點云到相機坐標系原點的模板點云的變換位姿，而計算損失函數使用模型點云到場景點云中的變換位姿，因此需要對變換矩陣求逆。

CPLoss 損失函數可以有效地表示估計位姿回歸的準確程度，但是對于一些對稱物體而言，多個位姿可能對應同一個正確的姿態，從而使網絡回歸到另一個可代替的位姿上，造成損失函數給出不一致的訓練信號。針對這一問題，本文采用類似于迭代最近點（Iterative Closest Point，ICP）算法的損失函數ICPLoss，計算估計位姿回歸得到的點云中的每一個點離真實位姿回歸得到點云的最近點的距離并取平均值，計算公式如下：

3 實驗驗證

在進行位姿估計前，需要對獲取的場景點云進行實例分割。本文采用ASIS［21］實例分割算法，根據同類實例點的特征向量相近、不同類實例點的特征向量相差較遠的原則進行實例分割。因此，工件在無遮擋堆疊的情況下，分割效果是非常理想的，而由于本文在抓取過程中每次僅對場景中的一個實例進行位姿估計，對于遮擋堆疊嚴重的場景點云，將最上層實例分割分數最為理想的工件作為待抓取工件，可以避免遮擋堆疊帶來的分割誤差。圖7（a）是真實場景的散亂堆疊工件，圖7（b）、圖7（c）是真實場景點云的兩個分割實例。圖8 是針對圖7（a）的真實場景位姿估計實例，通過網絡估計工件位姿并利用ICP 進行位姿細化得到可抓取工件的精確六維位姿，接著通過機器人進行工件的抓取，重復以上過程即是一次完整的散亂工件抓取的流程。圖8（a）～圖8（h）顯示了將模型點云基于估計得到的精確六維位姿變換回場景中，可以看出模型點云和場景中的目標點云基本重合。

圖7 真實場景的點云分割實例Fig.7 Examples of point clouds segmentation of real scene

圖8 真實場景的位姿估計實例Fig.8 Examples of pose estimation of real scene

3.1 實驗參數設置

本文針對4 種不同的工業零件進行六維位姿估計實驗。在數據集中，每類工件都有8 000 個分割后的點云樣本作為訓練集，2 000 個樣本作為測試集，每個樣本包含2 048 個采樣點。對于非對稱工件，由于本文采用的工件尺寸為10～20 cm，因此將CPLoss 小于工件尺寸最大直徑的1/10視為位姿估計正確。對于對稱工件，判別標準是ICPLoss 的大小，經過實驗評估，軸承座1和連桿的回歸損失ICPLoss 分別小于2 mm 和1.4 mm時，可視為位姿回歸正確。如果訓練的工件尺寸和本文相差很大，則需重新選定合理的閾值。

3.2 與傳統位姿估計方法的性能對比

將本文方法與粗配準+ICP、PPF、深度+ICP 方法進行對比，如表1 所示，其中最優指標值用加粗字體標示。可以看出，使用深度特征和法線特征相融合的位姿估計方法比僅使用深度特征的位姿估計方法具有更高的估計準確率。對于對稱工件而言，即軸承座1 和連桿，PPF 和本文方法均能達到很高的估計準確率，而粗配準+ICP 方法效果較差；對于非對稱工件而言，即軸承座2 和榔頭，本文方法在準確率上遠超粗配準+ICP 和PPF 方法。

表1 工業零件在不同方法下的位姿估計準確率Table 1 Accuracy of pose estimation of industrial parts with different methods %

圖9 給出了PPF 匹配錯誤的兩種情況，可以看出榔頭正反面是兩個類似的平面，而當分割后的輸入點云是類似于圖中這樣的局部平面時，PPF 或者粗配準+ICP 方法很可能會將其匹配到工件的一個類似平面上，方向和位置完全錯誤。由此得出，傳統方法是通過計算特征點對的方式進行匹配的，它們沒有獲取輸入點云的局部外形特征和幾何特征，在有相似特征的情況下很容易匹配錯誤，而本文方法沒有出現這方面的問題。

圖9 PPF 錯誤匹配樣本Fig.9 PPF error matching samples

表2 給出了3 種方法的平均位姿估計時間對比結果。本文所有涉及ICP 位姿細化的地方，均將終止條件定為兩次迭代的結果之差小于10-6m。可以看出，本文方法非常高效，估計一次的時間遠少于粗配準+ICP方法的時間，也略快于PPF 方法。同時，對于增加的法線特征支路，其浮點運算量為2.1×108，而深度特征支路resnet50 的浮點運算量為3.8×109，約為前者的1.8 倍。可見，特征融合網絡相比單特征網絡運算復雜度和位姿估計時間并未明顯增加，這是因為整個網絡的運算復雜度主要由深度特征支路以及之后的全連接層決定。

表2 3 種方法的平均位姿估計時間對比Table 2 Comparison of the average pose estimation time of three methods s

3.3 其他因素對估計結果的影響

由于真實場景中分割得到的單個點云的點個數是不確定的，而本文訓練采用的數據集中每個樣本都是2 048 個點，因此本文將各種采樣點數的點云分別輸入訓練好的模型進行位姿預測并統計各種方法在不同采樣點數下的位姿估計準確率。圖10 給出了采用3 種方法的榔頭工件位姿估計準確率對比結果。可以看出，在不同采樣點數下，本文方法在估計準確率上未有明顯變化，說明本文訓練的模型可以針對不同點數的點云進行位姿估計，而其他兩種方法在點數變少時準確率出現遞減的情況。

圖10 不同采樣點數下位姿估計準確率的對比Fig.10 Comparison of accuracy of pose estimation under different sampling points

針對噪聲對法線特征圖的影響，本文對測試數據的每一個點加入隨機噪聲Δ：其中：β是比例系數，為使噪聲的影響更加顯著，本文將其設定為0.05。表3 給出了本文方法在無噪聲的測試集、添加噪聲樣本的測試集以及添加噪聲樣本的訓練集上進行訓練后得到的測試結果。可以看出，噪聲對位姿估計準確率的影響較小，并且將一些帶有噪聲的樣本加入訓練集后可以避免該影響。因此，經過實驗證實，本文方法對噪聲的魯棒性較強。

表3 噪聲對本文方法位姿估計準確率的影響Table 3 The effect of noise on the accuracy of the proposed pose estimation method %

4 結束語

本文提出一種基于深度學習的點云位姿估計方法，將分割后的單個點云投影到二維平面，生成深度特征圖和法線特征圖，用于提取點云的局部表面特征和幾何特征，從而估計出準確的六維位姿。在仿真數據集和真實數據集上的實驗結果驗證了該方法的有效性，并表明其在一定程度上解決了傳統位姿估計方法計算量大且魯棒性差的問題。但由于本文方法是基于點云的實例分割，位姿估計的準確率依賴于實例分割的準確率，因此下一步將對分割和位姿估計進行有效結合形成端到端模型，在保證點云語義實例分割準確率的前提下進一步提升算法實時性。