一種在線CAD模型驅動的級聯式機械臂視覺引導逼近方法與流程

2023-05-28 18:36:31

本發明涉及計算機視覺及工業自動化技術領域，具體涉及一種在線cad模型驅動的級聯式機械臂視覺引導逼近方法。

背景技術：

機械臂的精確引導一直以來都是工業自動化的一個重要組成部分，但目前很多工業流水線上的機器人只能完成重複單調的動作，在使用機械臂對隨機擺放的零件進行接近並執行動作這一方面做得還不夠充分，當零件相對於機械臂末端執行器的位姿不確定時，機械臂就無法完成一些精確的工作，因此研究一種可靠的機械臂精確引導逼近方法就顯得尤為重要。

目前對於零件進行位姿識別並引導機械臂接近的方法主要分兩大類：其一是通過視覺伺服的方法，計算當前圖像(或位置)與目標圖像(或位置)之間的誤差，利用該誤差信號作為反饋控制機械臂的運動，這種方法的優點在於控制的精度高，缺點在於運動過程中可能使零件脫離攝像機的視域範圍丟失圖像特徵，致使機械臂無法收斂到期望目標狀態；其二是直接通過當前圖像與目標圖像計算相對位姿，之後控制機械臂完成接近，這種方法的優點在於能夠僅依靠起始位置的圖像計算得到零件的位姿，將路徑規劃的部分交由機械臂控制器完成，缺點在於兩張圖像之間的差別不能太大、不能對路徑進行規劃，並且其控制方式為開環控制，無法對最後的精度做出保證。

針對目前方法的不足，本發明考慮結合使用這兩種方法以避免各自的缺點，並利用雙視圖三維重建方法引導機械臂按設定路徑運動。通過基於模板的位姿匹配定位大致位姿，之後在線生成路徑上的零件虛擬圖像引導機械臂按指定軌跡接近零件，最後利用視覺伺服方法完成精確引導。如此，既避免了單一使用視覺伺服方法可能使零件脫離攝像機的視域範圍丟失圖像特徵，又避免了單一使用位姿估計方法精度不高的問題，同時使得軌跡可控，滿足實際工業應用的需求。

技術實現要素：

本發明為解決上述機械臂引導方法的不足，提出一種在線cad模型驅動的級聯式機械臂視覺引導逼近方法，

如圖2所示，本發明的技術方案包括以下步驟：

方法實施前，機械臂末端安裝攝像機，並進行手眼標定。

步驟1，離線準備階段：建立被抓取零件的cad模型，使用opengl圖形程序接口讀取該模型，使用虛擬攝像機對零件的cad模型在不同位姿下拍攝生成一系列模板圖像，對模板圖像進行邊緣檢測提取獲得模板邊緣圖像，然後對模板邊緣圖像進行聚類，構造位姿層次樹；

步驟2，位姿預估階段：將實際拍攝的實際邊緣圖像和位姿層次樹中的模板邊緣圖像進行相似度計算，以相似度最高的模板邊緣圖像拍攝時對應的位姿作為預估位姿；

步驟3，接近階段：以預估位姿設定機械臂的運動軌跡，在運動軌跡上生成多張路徑模板圖像，通過路徑模板圖像引導機械臂按運動軌跡接近被抓取零件；

步驟4，精確引導階段：接近被抓取零件後，用機械臂末端的攝像機實時採集獲得實際圖像，根據被抓取零件的目標位姿實際圖像和實際圖像利用基於圖像的視覺伺服方法進行最後精確引導，進行抓取或裝配操作。

本發明融合了模板位姿估計方法(步驟1+2)、雙視圖三維重建方法(步驟3)和基於圖像的視覺伺服方法(步驟4)進行機械臂視覺引導逼近。

所述步驟1中，opengl圖形程序接口中的光源設置應與實際情況相同，使模板圖像中的零件不同表面之間的灰度變化與機械臂攝像機拍攝到的實際零件不同表面之間的灰度變化相同。

所述步驟1和2中進行邊緣檢測提取具體是：對圖像採用邊緣sobel算子進行卷積(即進行邊緣檢測)，得到邊緣圖像，並將邊緣圖像中低於閾值的像素點全部置0，避免捕捉到圓弧面上由多邊形擬合出的錯誤邊緣。

所述的步驟1中，對模板邊緣圖像進行聚類構造位姿層次樹具體是：對模板邊緣圖像按相似度進行層次聚類，每完成一層的聚類，對模板圖像進行向下採樣，再次進行邊緣檢測及聚類，直至類別數達到設定數量，即完成了位姿姿態樹的構造。具體包括：

1.1針對所有圖像，相似計算每兩個圖像之間的相似度，將相互之間均相似的多張圖像歸為一類，從而進行分類；

1.2先對模板邊緣圖像採用步驟1.1進行處理獲得第一層分類；

1.3再針對第一層分類後的每一類，取位姿位於中心的模板邊緣圖像作為該層該類的中心圖像，接著將該層的所有中心圖像採用步驟1.1進行處理

1.4獲得下一層分類，再迭代重複步驟1.3直至類別數達到預設數量閾值，完成位姿姿態樹構建。

從第一層到最高一層的圖像大小作逐漸減小處理，使得能夠快速完成層次聚類。

所述的步驟2具體為：用機械臂末端的攝像機實時採集獲得實際圖像，並進行邊緣檢測提取獲得實際邊緣圖像，將實際邊緣圖像和位姿層次樹中的模板邊緣圖像進行相似度計算，並使用終止條件加速相似度計算，獲得相似度最高的模板邊緣圖像，以模板邊緣圖像拍攝時對應的位姿作為預估位姿。

未加特殊說明的情況下，所述位姿均為機械臂末端執行器與零件坐標系的相對位姿，包括位置和姿態。

所述的預估位姿目的是便於進行後續的路徑規劃，並非僅依靠該方法引導機械臂全程的運動。

更具體地：先將實際邊緣圖像與位姿層次樹中最高層的模板邊緣圖像進行相似度計算，再根據最高層相似度結果將實際邊緣圖像與位姿層次樹中第二高層中包含最高層匹配成功的模板圖像的那一類中的模板邊緣圖像進行相似度計算，以此類推直到與第一層的模板邊緣圖像進行相似度計算後，以相似度最高的模板邊緣圖像拍攝時對應的位姿作為預估位姿。

所述的相似度採用以下公式計算：

其中，為一圖像中的任意一點p處的梯度向量，為另一圖像中點p』處的梯度向量，點p與點p』在各自圖像中的坐標相同，n為圖像中的所有像素點的總數，i代表計算的是第i對點，s為兩張圖像之間的相似度；

若相似度大於相似閾值tm，則認為兩張圖像相似；否則不相似；

並且在相似度值計算過程中，為了加快模板邊緣圖像與實際邊緣圖像之間相似度計算的速度，第k對點時的相似度值sk滿足條件時，則立即終止計算，認為兩張圖像不相似，以加速整體的計算速度。

第k對點時的相似度值sk採用以下公式計算：

若此時即便之後所有點對的兩個邊緣向量的內積均為1，最終的相似度s也將小於tm。因此在滿足條件時，則認為兩張圖像不相似，以終止計算、加速整體的計算速度。

所述的步驟3具體為：以預估位姿和目標位姿之間的連接直線作為機械臂的運動軌跡，在運動軌跡上設置多個位置點，用虛擬攝像機沿軌跡運動在每個位置點生成零件的路徑模板圖像，利用路徑模板圖像控制機械臂進行逐位置點移動；在每次需要移動到下一個位置點時，將實際圖像與下一個位姿的路徑模板圖像進行特徵點匹配，計算兩張圖像之間的本質矩陣，進而計算旋轉和平移向量來移動機械臂，如此逐位置點移動直至機械臂末端到達目標位姿。

所述的步驟3包括：以預估位姿和目標位姿的位置之間的連接直線作為機械臂的運動軌跡，在連接直線上均勻取n個位置點，在預估位姿和目標位姿的姿態(朝向方向)之間插入n個姿態(這裡的每個姿態，其視線方向均為由當前位置點指向零件坐標系原點，其上方方向可以在始末兩個姿態的上方方向之間均勻插值得到)，n個姿態分別作為n個位置點的姿態，用虛擬攝像機在每個位置點以對應的位姿拍攝獲得n張路徑模板圖像；

在每次需要移動到下一個位置點時，提取當前位置位姿下的實際圖像和下一位置位姿下的路徑模板圖像的特徵點，根據特徵點對兩幅圖像的特徵點進行匹配得到匹配點對，用匹配點對採用求解計算獲得本質矩陣e，然後對本質矩陣e進行奇異值分解得到旋轉矩陣r和平移向量t，使機械臂末端按照旋轉矩陣r進行旋轉並按照平移向量t進行平移。

本發明僅在機械臂末端執行器接近零件之後使用基於圖像的視覺伺服方法，並非在機械臂全程的運動使用。

本發明的有益效果是：

1)本發明結合使用基於模板的位姿估計、基於雙視圖的三維重建方法及基於圖像的視覺伺服以對零件進行視覺引導逼近，避免了單一使用視覺伺服方法可能使零件脫離攝像機的視域範圍丟失圖像特徵，又避免了單一使用位姿估計方法精度不高的問題，同時使得軌跡可控。

2)本發明通過cad模型生成模板圖像和路徑模板圖像進行位姿估計與路徑引導。

通過使用cad模型生成模板圖像和路徑模板圖像，避免實際操作中需要拍攝大量模板圖像的問題，簡化了工作的流程，並且提高了模板圖像的精度。

通過單目圖像信息與cad模型生成的模板圖像進行相似度計算可以獲得預估位姿信息，在機械臂接近零件的過程中，位姿的誤差隨著距離的縮短而減小，能夠更精確地完成引導任務。

3)本發明僅通過單目視覺信息並結合已知模型的cad信息就能完成視覺引導逼近。本發明引導精度高，軌跡可控，避免了單一使用視覺伺服方法可能使零件脫離攝像機的視域範圍丟失圖像特徵和單一使用位姿估計方法精度不高的問題，同時使得軌跡可控，可滿足實際工業應用的需求。

附圖說明

圖1為本發明實施例的機器人組件設備示意圖；

圖2為本發明方法流程圖；

圖3為實施例四種可能的攝像機相對位姿示意圖。

具體實施方式

下面結合附圖和實施例對本發明作進一步說明。

圖1顯示了能夠使用本發明的實施方式的機器人組件設備。系統包括六軸機械臂1，單目攝像機2及末端執行器3，需要抓取的零件為4。其中單目攝像機2固連在末端執行器3上。

方法實施前，對機械臂末端安裝的攝像機使用棋盤格標定板通過tsai-lenz方法進行手眼標定。

第一步離線準備。建立被抓取零件的三維cad模型。為了獲得三維cad模型的二維圖像，本發明優選地使用opengl讀取該模型，但不限於該圖形程序接口，可以使用諸如directx等其他的圖形程序接口。視線方向對準零件坐標系原點，保證零件處於圖像中心，均勻改變視點位置(3個參數)及繞視線旋轉角度(1個參數)共4個參數的值，保存在這些位姿下所獲得的零件二維圖像,即模板圖像。採用sobel算子對模板圖像進行卷積(即進行邊緣檢測)，獲得模板圖像在水平方向及豎直方向的梯度值，也即提取出了邊緣。由於在opengl中的零件表面是由面片組成的，因此如圓弧之類的表面也是由許多面片來近似的，所以圓弧表面相鄰面片之間的灰度值會略有不同，這就會被邊緣算子所捕捉到。因此之後需要對獲得的梯度值進行閾值處理，將低於閾值的梯度全部置0，因為圓弧面上相鄰面片的灰度值相差不大，所以經過卷積得到的值也很小，閾值處理就可以將圓弧面上的錯誤邊緣除去，經過處理後的該圖像稱為模板邊緣圖像。

為加速單目攝像機2拍攝到的實際圖像提取邊緣後的實際邊緣圖像與模板邊緣圖像的匹配過程，需要對模板邊緣圖像構造位姿層次樹，減少搜索的時間。位姿層次樹構造方法如下：首先任取一張模板邊緣圖像，計算位姿相鄰的另一張模板邊緣圖像與其的相似度，若該相似度大於相似閾值tm，則將其歸為一類。

計算與該類中的模板的位姿相鄰(還沒有被劃入其他類)的另一個模板邊緣圖像與類內所有圖像的相似度，若所有相似度均大於tm，則將其加入該類；如此往復，直至與該類中的模版的位姿相鄰的模板邊緣圖像均不能滿足前述的條件。然後再任取一個還未被分類的模板邊緣圖像，重複上述過程，直至所有位姿的模板邊緣圖像均被分類完畢，完成第一層的分類。

將處於每一類中心位置的模板邊緣圖像和模板圖像作為該類的模板邊緣圖像和模板圖像。隨後將每一類的模板圖像進行向下採樣(圖像大小縮小為原先的四分之一)，獲得第二層的模板圖像，之後提取邊緣獲得第二層的模板邊緣圖像，以同樣的方式對第二層的模板邊緣圖像進行分類。如此往復，直至某層的類別數量減小至設定的閾值。

第二步位姿預估階段。獲取機械臂起始位置處拍攝到的實際圖像，對實際圖像進行若干次向下採樣操作(該操作次數取決於之前生成的位姿層次樹的層數)，對這些圖像進行邊緣提取，獲得若干實際邊緣圖像，將這些實際邊緣圖像與位姿層次樹內的模板邊緣圖像進行相似度計算，從上層至下層逐步搜索。例如：預先構造完畢的位姿層次樹有2層，第一層有30張模板邊緣圖像(100*100大小)(共分為5類，每類6張模板)，第二層有5張模板邊緣圖像(50*50大小)。則對實際圖像(100*100大小)首先進行1次向下採樣，得到50*50的圖像，再對這兩張圖像提取邊緣。首先使用50*50的實際邊緣圖像與第二層的5張模板邊緣圖像計算相似度，假設與第一張匹配，則之後再使用100*100的實際邊緣圖像與第一層中包含第二層第一張模板邊緣圖像的那一類中的圖像計算相似度，假設與第三張圖像匹配，則返回該圖像所對應的位姿信息作為預估位姿。

第三步接近階段。

首先按照路徑最短的條件規劃出軌跡：在第二步中估計出的與零件的相對位置與預設的最終的與零件之間的相對位置之間沿直線均勻取n個位置點；

在第二步中估計出的與零件的相對姿態與預設的最終的與零件之間的相對姿態之間插入n個姿態(這裡的每個姿態，其視線方向均為由當前位置點指向零件坐標系原點，其上方方向可以在始末兩個姿態的上方方向之間均勻插值得到)，分別作為之前n個位置點的姿態，這樣就獲得了n個相對零件的位姿，在計算機中將虛擬攝像機的位姿設定為這n個位姿，就獲得了n張路徑模板圖像。

利用基於尺度不變、旋轉不變的特徵提取算法，提取當前位姿下攝像機實際圖像和第1張路徑模板圖像中的特徵點，根據特徵點的描述信息對這兩幅圖像的特徵點進行匹配，得到若干對匹配點對。

為了提高最後的計算得到的本質矩陣e的精度，可使用ransac方法進行優化。本發明使用opencv自帶的findessentialmat函數進行本質矩陣e的求解，findessentialmat函數中使用ransac方法進行優化求解過程。

在得到本質矩陣e之後，對它進行奇異值分解得到兩個旋轉矩陣r和兩個平移向量t，由此可以組合出4種情況，如圖3所示。圖中，a和b分別代表攝像機的兩個位置，「t」字型符號的橫線代表成像平面，豎線代表攝像機的光軸方向，豎線的下端點代表攝像機的光心位置。可以發現，除了(a)以外，物體均會出現在某個(或兩個)攝像機的後方，據此，可以排除三種錯誤的情況，獲得唯一的r和t的解。在本發明中通過調用opencv中的recoverpose函數得到正確的r和t。

之後使機械臂末端按照矩陣r進行旋轉並按照向量t進行平移，即可運動至第1張路徑模板圖像拍攝時所處的相對位姿。之後再讀取第2張路徑模板圖像，進行類似的動作，如此往復，直至接近階段完成。

第四步精確引導階段。將最終位姿時刻機械臂末端攝像機拍攝到的實際圖像作為目標圖像(手動獲得第一個零件的目標圖像，由於機械臂接近的對象是若干相同的零件，所以該圖像可以用於之後零件的引導，因為機械臂末端執行器在執行動作時相對於每個零件的相對位姿都是相同的)，利用基於圖像的視覺伺服方法完成最後的精確引導，並執行動作。

雖然本發明已以較佳實施例揭露如上，然其並非用以限定本發明。本發明所屬技術領域中具有通常知識者，在不脫離本發明的精神和範圍內，當可作各種的更動與潤飾。因此，本發明的保護範圍當視權利要求書所界定者為準。