[發明專利]一種基于深度相機的托盤檢測定位方法

在審

• 申請號: 202010866470.1
• 申請日: 2020-08-25
• 公開（公告）號: CN111986185A
• 公開（公告）日: 2020-11-24
• 發明（設計）人: 高飛;邱琪;盧書芳;翁立波;張元鳴
• 申請（專利權）人: 浙江工業大學
• 主分類號: G06T7/00
• 分類號: G06T7/00;G06K9/46;G06K9/40;G06K9/62;G06T7/11;G06T7/70
• 代理公司: 杭州浙科專利事務所(普通合伙) 33213
• 代理人: 周紅芳;朱盈盈

1.一種基于深度相機的托盤檢測定位方法，其特征在于，包括如下步驟：

步驟1：根據托盤貨架橫梁和立柱的寬度，選取直徑3-5厘米的圓形紅外反光標記粘貼在橫梁和立柱外表面；

步驟2：使用深度相機獲取包含目標托盤的點云和紅外圖像，記該點云為原始點云P₀，紅外圖像為I₀，并對點云和紅外圖像進行預處理；

步驟3：采用直通濾波算法去除點云P₀中的無效區域，同時剔除離群點；采用基于法向量約束的區域生長算法對點云進行分割，首先使用基于主成分分析的法線估計方法，設置方法中種子點的鄰域半徑為30毫米，設置區域生長點數下限為50，上限為2000，對點云中任意種子點進行法線估計，將點云聚類為n個點簇集合，其中第j個聚類點集D_j如式(5)所示，j＝1,2,…,n，點集D_j被視為一個平面；篩選出點云中待處理的托盤立柱平面點云并構成如式(6)所示的點云集合P_plane，該點云集合對應的平面記為α；

D_j＝{d_i||A_i|＜5°,i＝1,2,…,v} (5)

其中，d_i表示點云P₀中第i個種子點，A_i表示種子點d_i與其鄰域內點的法向量角度差；A_jk表示平面點集D_j與D_k的法向量角度差，表示平面點集D_j的法向量與深度相機z軸正方向向量的角度差，η(D_j)表示平面點集D_j整體的長寬比；

步驟4：將原始點云P₀中滿足x坐標大于x_np且小于x_xp、y坐標大于y_np且小于y_xp、z坐標大于z_np且小于z_xp的w個點構成點云P_seg，其中，x_np和x_xp分別表示點云P_plane中x坐標的最小值和最大值，y_np和y_xp分別表示點云P_plane中y坐標的最小值和最大值，z_np和z_xp分別表示點云P_plane中z坐標的最小值和最大值；獲取P_seg中屬于平面α的切面點云并投影形成如式(7)所示點云集合P_pro；根據式(8)得到P_pro旋轉后的切面點云P_tran，該點云中的點的z坐標大小記為z_tran；

其中，d_s表示點云P_seg中任意點，表示點d_s在平面α上的投影點，Dist(α,d_s)表示點d_s到平面α的距離，單位為毫米，R_proz為點云P_pro到相機z軸正方向的旋轉矩陣，(x_pro,y_pro,z_pro)表示點云P_pro中點的坐標，(x_tran,y_tran,z_tran)表示將點(x_pro,y_pro,z_pro)旋轉到點云P_tran中對應點的坐標；

步驟5：測量出托盤實物的長為L₀，寬為W_e，單位為毫米，然后將柵格圖G的長設置為L₀、寬設置為W_e，將平面點云P_tran轉換為柵格圖G，具體為：根據式(9)設置柵格圖像素坐標的灰度值，然后采用閉運算算法將柵格圖稀疏區域的灰度值設置為255，使柵格圖中白色區域表示托盤，黑色區域表示背景或叉孔；

其中，(x_G,y_G)表示柵格圖G的像素坐標，G(x_G,y_G)表示坐標(x_G,y_G)處的像素的灰度值，x_min、y_min分別為點云P_tran中點的x、y坐標的最小值，x_tran、y_tran分別表示點云P_tran中任意點的x、y坐標；

步驟6：使用輪廓檢測算法，檢測出柵格圖G中t個無內部輪廓的輪廓并篩選得到如式(10)所示的候選輪廓集合H；獲取柵格圖G中托盤叉孔輪廓對應的兩個外接矩形并得到如式(11)所示的集合R_f，記集合R_f中的兩個外接矩形的八個角點的像素坐標為(x_re,y_re)，re＝1,2,…,8，角點對應點云P_tran上的坐標為(x_min+x_re,y_min+y_re,z_tran)；

H＝{C_i|i＝1,2,…,t,1≤η(C_i)≤4} (10)

R_f＝{R_C|C∈min2(H)} (11)

其中，C_i表示柵格圖G中的任意輪廓，η(C_i)表示輪廓C_i的外接矩形的長寬比；式(11)中min2(H)表示候選輪廓集合H中與托盤叉孔實際長寬比差值最小的兩個輪廓，R_C為輪廓C的外接矩形；

步驟7：根據式(12)將八個角點在點云P_tran上的坐標(x_min+x_re,y_min+y_re,z_tran)旋轉回原始點云P₀，得到原始點云P₀中對應的八個坐標(x_0i,y_0i,z_0i)，i＝1,2,…,8，再根據式(13)計算兩個叉孔中心點對應的原始點云坐標(x_cen,y_cen,z_cen)，最終根據式(14)計算叉車與托盤的距離l_fork；托盤立柱平面法向量與相機正方向夾角為托盤與叉車夾角，記為angle，由式(15)得到；計算得到l_fork和angle，即實現了托盤的精確的定位；

l_fork＝min(l(d_cen1,d_fork),l(d_cen2,d_fork)) (14)

其中，R_tran為點云P_tran到點云P₀的旋轉矩陣，式(13)中s取1、k取4和s取5、k取8時，分別計算兩個叉孔中心點的坐標并記為d_cen1和d_cen2；式(14)中，d_fork表示叉車上預先標定點的坐標，l(d_cen1，d_fork)、l(d_cen2，d_fork)分別為兩個叉孔中心點與預先標定點的距離，min()取兩者最小值；表示托盤立柱平面點云方向向量，表示相機正方向向量，表示與的方位角，×表示向量叉乘，為的向量范數，·表示向量點乘。