基於PCL的棧板點雲識別與定位

棧板是為方便物品的擺放和出貨要求而製成的底座，中間有槽專門用於叉車搬運。為實現更高程度的自動化，叉車除了利用SLAM自主定位導航外，還需要識別棧板並確定位置。這裡使用德國易福門產的O3D303 3D ToF攝像頭進行成像，該攝像頭能獲得前方物體上各點的位置xyz、信號前度a和置信度信息，z軸定位精度約為8mm。

在使用深度學習缺少數據，使用特徵描述子效果極差的情況下，選擇使用PCL庫中經典的機器學習演算法進行棧板的識別與定位，作為叉車Pick-and-Drop System（PDS）的主要演算法。

目前國際上做得比較成功的是Love Park Robotics公司，他們有一套基於O3D303攝像頭、PCL、OpenCV、CUDA、ROS的成熟工業自動化方案，雖然軟體有部分開源，但是僅開源了3D點雲圖像獲取部分，棧板識別定位和GPU優化演算法作為其核心演算法未開源。其PDS系統的一整套設備（攝像頭+筆記本電腦）官方售價為5000美元，而硬體成本（零售價）在國內約為1600美元。

本文介紹一種不成熟的棧板識別與定位方法。

首先使用O3D303攝像頭官方的SDK採集點雲圖像，約有16000個點，保存為點雲格式的pcd文件作為演算法測試數據。

演算法首先讀取該pcd文件，使用VoxelGrid濾波器過濾的點雲，並設置濾波時創建的體素體積為1cm的立方體。該體素格濾波器可以達到向下採樣同時不破壞點雲本身幾何結構的功能，能夠有效減少點雲數量，提高後續演算法的運算速度。

然後，對體素格濾波器濾波後的點雲使用StatisticalOutlierRemoval濾波器，對每個點分析的臨近的100個點，並將標準差的倍數設置為0.1，這意味著如果一個點到分析點的距離超出了平均距離0.1個標準差以上，則該點被標記為離群點，並將它移除。該統計濾波器能濾除大部分由反射、曝光時間不足、點離攝像頭較遠等原因產生的異常點，這些異常點會影響後面的表面法向量、曲率變化等值的計算。

估計二次濾波後點雲中每個點的法向量，使用KdTree作為搜索的演算法，設置K最近鄰搜索時的參數K為20。利用計算得到的法向量，採用隨機採樣一致性（RANSAC）演算法提取平面，設置表面法線權重係數為0.1，迭代的最大次數為500次，點到模型的距離允許最大值為0.3。該演算法原本能夠提取點雲中的平面，但在實際測試數據上表現較差（可能沒有調到合適的參數）。因此使用RANSAC只是作為濾波器，從依然帶有雜訊的點雲中提取平面特徵，方便後續的計算。在經過該步驟後，得到的點雲幾乎沒有肉眼看見的雜訊了（RANSAC就是牛逼）。再用相同的演算法和參數估算一次點雲中每個點的法向量，得到更為準確的法向量值。

最後使用區域增長（Region Growing）演算法，獲得地面和棧板的前表面。設置最小的聚類數為200，也就是說每個平面至少含有200個點（不會出現很小的奇怪的面），設置搜索演算法為KdTree，近鄰數為150（該參數越大，法向量相似的面會合併），平滑閾值為7度（能夠區分地面和棧板前表面，又不會受表面不平的影響），曲率閾值為1（曲率太大的曲面不會繼續延展生長）。這樣整個點雲被聚類兩部分：地面與棧板上表面（法向量都向上）、棧板前表面（法向量向前），當然還有棧板邊緣一些噪音點沒有被聚類。

分別求出上述兩個聚類中所有點的平均法向量，再做歸一化，作為地面和棧板前表面的單位法向量，理論上兩個法向量應該垂直，即點乘為0，實際誤差在2~5度，誤差較小。

分別求出棧板前表面點雲xyz坐標的最大、最小值，再取平均，作為棧板前表面幾何中心。

如果修改區域增長演算法的參數，例如將條件改得更加嚴格，這樣算出來兩個法向量更加準確，垂直的誤差能小於1度，但得到的棧板前表面區域就不準確，比理論的區域小，並且更加不對稱，因此計算幾何中心的誤差將增大（但誤差能控制在1cm範圍內）。更好的方法是分別用兩組參數算兩次區域增長，分別得到準確的法向量和幾何中心，但這會增加運算時間。未來可重寫區域增長演算法，修改增長結束的條件，使其適用於本例。

最終，我們依據棧板前表面法向量和棧板前表面幾何中心坐標，以及攝像頭安裝的位置和姿態這些先驗條件，來控制叉車運動，使其完成Pick and Drop任務。

喜歡這篇文章嗎？立刻分享出去讓更多人知道吧！