本發(fā)明涉及計(jì)算機(jī)視覺及工業(yè)自動化技術(shù)領(lǐng)域，具體涉及一種在線cad模型驅(qū)動的級聯(lián)式機(jī)械臂視覺引導(dǎo)逼近方法。

背景技術(shù)：

機(jī)械臂的精確引導(dǎo)一直以來都是工業(yè)自動化的一個重要組成部分，但目前很多工業(yè)流水線上的機(jī)器人只能完成重復(fù)單調(diào)的動作，在使用機(jī)械臂對隨機(jī)擺放的零件進(jìn)行接近并執(zhí)行動作這一方面做得還不夠充分，當(dāng)零件相對于機(jī)械臂末端執(zhí)行器的位姿不確定時，機(jī)械臂就無法完成一些精確的工作，因此研究一種可靠的機(jī)械臂精確引導(dǎo)逼近方法就顯得尤為重要。

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

針對目前方法的不足，本發(fā)明考慮結(jié)合使用這兩種方法以避免各自的缺點(diǎn)，并利用雙視圖三維重建方法引導(dǎo)機(jī)械臂按設(shè)定路徑運(yùn)動。通過基于模板的位姿匹配定位大致位姿，之后在線生成路徑上的零件虛擬圖像引導(dǎo)機(jī)械臂按指定軌跡接近零件，最后利用視覺伺服方法完成精確引導(dǎo)。如此，既避免了單一使用視覺伺服方法可能使零件脫離攝像機(jī)的視域范圍丟失圖像特征，又避免了單一使用位姿估計(jì)方法精度不高的問題，同時使得軌跡可控，滿足實(shí)際工業(yè)應(yīng)用的需求。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明為解決上述機(jī)械臂引導(dǎo)方法的不足，提出一種在線cad模型驅(qū)動的級聯(lián)式機(jī)械臂視覺引導(dǎo)逼近方法，

如圖2所示，本發(fā)明的技術(shù)方案包括以下步驟：

方法實(shí)施前，機(jī)械臂末端安裝攝像機(jī)，并進(jìn)行手眼標(biāo)定。

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

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

步驟3，接近階段：以預(yù)估位姿設(shè)定機(jī)械臂的運(yùn)動軌跡，在運(yùn)動軌跡上生成多張路徑模板圖像，通過路徑模板圖像引導(dǎo)機(jī)械臂按運(yùn)動軌跡接近被抓取零件；

步驟4，精確引導(dǎo)階段：接近被抓取零件后，用機(jī)械臂末端的攝像機(jī)實(shí)時采集獲得實(shí)際圖像，根據(jù)被抓取零件的目標(biāo)位姿實(shí)際圖像和實(shí)際圖像利用基于圖像的視覺伺服方法進(jìn)行最后精確引導(dǎo)，進(jìn)行抓取或裝配操作。

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

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

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

所述的步驟1中，對模板邊緣圖像進(jìn)行聚類構(gòu)造位姿層次樹具體是：對模板邊緣圖像按相似度進(jìn)行層次聚類，每完成一層的聚類，對模板圖像進(jìn)行向下采樣，再次進(jìn)行邊緣檢測及聚類，直至類別數(shù)達(dá)到設(shè)定數(shù)量，即完成了位姿姿態(tài)樹的構(gòu)造。具體包括：

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

1.2先對模板邊緣圖像采用步驟1.1進(jìn)行處理獲得第一層分類；

1.3再針對第一層分類后的每一類，取位姿位于中心的模板邊緣圖像作為該層該類的中心圖像，接著將該層的所有中心圖像采用步驟1.1進(jìn)行處理

1.4獲得下一層分類，再迭代重復(fù)步驟1.3直至類別數(shù)達(dá)到預(yù)設(shè)數(shù)量閾值，完成位姿姿態(tài)樹構(gòu)建。

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

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

未加特殊說明的情況下，所述位姿均為機(jī)械臂末端執(zhí)行器與零件坐標(biāo)系的相對位姿，包括位置和姿態(tài)。

所述的預(yù)估位姿目的是便于進(jìn)行后續(xù)的路徑規(guī)劃，并非僅依靠該方法引導(dǎo)機(jī)械臂全程的運(yùn)動。

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

所述的相似度采用以下公式計(jì)算：

其中，為一圖像中的任意一點(diǎn)p處的梯度向量，為另一圖像中點(diǎn)p’處的梯度向量，點(diǎn)p與點(diǎn)p’在各自圖像中的坐標(biāo)相同，n為圖像中的所有像素點(diǎn)的總數(shù)，i代表計(jì)算的是第i對點(diǎn)，s為兩張圖像之間的相似度；

若相似度大于相似閾值tm，則認(rèn)為兩張圖像相似；否則不相似；

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

第k對點(diǎn)時的相似度值sk采用以下公式計(jì)算：

若此時即便之后所有點(diǎn)對的兩個邊緣向量的內(nèi)積均為1，最終的相似度s也將小于tm。因此在滿足條件時，則認(rèn)為兩張圖像不相似，以終止計(jì)算、加速整體的計(jì)算速度。

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

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

在每次需要移動到下一個位置點(diǎn)時，提取當(dāng)前位置位姿下的實(shí)際圖像和下一位置位姿下的路徑模板圖像的特征點(diǎn)，根據(jù)特征點(diǎn)對兩幅圖像的特征點(diǎn)進(jìn)行匹配得到匹配點(diǎn)對，用匹配點(diǎn)對采用求解計(jì)算獲得本質(zhì)矩陣e，然后對本質(zhì)矩陣e進(jìn)行奇異值分解得到旋轉(zhuǎn)矩陣r和平移向量t，使機(jī)械臂末端按照旋轉(zhuǎn)矩陣r進(jìn)行旋轉(zhuǎn)并按照平移向量t進(jìn)行平移。

本發(fā)明僅在機(jī)械臂末端執(zhí)行器接近零件之后使用基于圖像的視覺伺服方法，并非在機(jī)械臂全程的運(yùn)動使用。

本發(fā)明的有益效果是：

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

2)本發(fā)明通過cad模型生成模板圖像和路徑模板圖像進(jìn)行位姿估計(jì)與路徑引導(dǎo)。

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

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

3)本發(fā)明僅通過單目視覺信息并結(jié)合已知模型的cad信息就能完成視覺引導(dǎo)逼近。本發(fā)明引導(dǎo)精度高，軌跡可控，避免了單一使用視覺伺服方法可能使零件脫離攝像機(jī)的視域范圍丟失圖像特征和單一使用位姿估計(jì)方法精度不高的問題，同時使得軌跡可控，可滿足實(shí)際工業(yè)應(yīng)用的需求。

附圖說明

圖1為本發(fā)明實(shí)施例的機(jī)器人組件設(shè)備示意圖；

圖2為本發(fā)明方法流程圖；

圖3為實(shí)施例四種可能的攝像機(jī)相對位姿示意圖。

具體實(shí)施方式

下面結(jié)合附圖和實(shí)施例對本發(fā)明作進(jìn)一步說明。

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

方法實(shí)施前，對機(jī)械臂末端安裝的攝像機(jī)使用棋盤格標(biāo)定板通過tsai-lenz方法進(jìn)行手眼標(biāo)定。

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

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

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

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

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

第三步接近階段。

首先按照路徑最短的條件規(guī)劃出軌跡：在第二步中估計(jì)出的與零件的相對位置與預(yù)設(shè)的最終的與零件之間的相對位置之間沿直線均勻取n個位置點(diǎn)；

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

利用基于尺度不變、旋轉(zhuǎn)不變的特征提取算法，提取當(dāng)前位姿下攝像機(jī)實(shí)際圖像和第1張路徑模板圖像中的特征點(diǎn)，根據(jù)特征點(diǎn)的描述信息對這兩幅圖像的特征點(diǎn)進(jìn)行匹配，得到若干對匹配點(diǎn)對。

為了提高最后的計(jì)算得到的本質(zhì)矩陣e的精度，可使用ransac方法進(jìn)行優(yōu)化。本發(fā)明使用opencv自帶的findessentialmat()函數(shù)進(jìn)行本質(zhì)矩陣e的求解，findessentialmat()函數(shù)中使用ransac方法進(jìn)行優(yōu)化求解過程。

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

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

第四步精確引導(dǎo)階段。將最終位姿時刻機(jī)械臂末端攝像機(jī)拍攝到的實(shí)際圖像作為目標(biāo)圖像(手動獲得第一個零件的目標(biāo)圖像，由于機(jī)械臂接近的對象是若干相同的零件，所以該圖像可以用于之后零件的引導(dǎo)，因?yàn)闄C(jī)械臂末端執(zhí)行器在執(zhí)行動作時相對于每個零件的相對位姿都是相同的)，利用基于圖像的視覺伺服方法完成最后的精確引導(dǎo)，并執(zhí)行動作。

雖然本發(fā)明已以較佳實(shí)施例揭露如上，然其并非用以限定本發(fā)明。本發(fā)明所屬技術(shù)領(lǐng)域中具有通常知識者，在不脫離本發(fā)明的精神和范圍內(nèi)，當(dāng)可作各種的更動與潤飾。因此，本發(fā)明的保護(hù)范圍當(dāng)視權(quán)利要求書所界定者為準(zhǔn)。