基于PatMax 視覺引導(dǎo)的機械臂末端軌跡規(guī)劃與控制

陳策博，萬 琴，蘇宏博，劉 波

（1.湖南工程學(xué)院 電氣與信息工程學(xué)院，湘潭 411100；2.湖南中南智能裝備有限公司，長沙 410117）

近年來，機器人技術(shù)在工業(yè)自動化、醫(yī)療保健、軍事防務(wù)等領(lǐng)域得到了廣泛的應(yīng)用。機械臂作為一種典型的機器人，具有靈活性、精度高等優(yōu)點，已經(jīng)成為各種工業(yè)機器人和服務(wù)機器人中不可或缺的部分。機械臂的末端軌跡規(guī)劃和控制是機械臂控制中的重要問題，對于機械臂的操作精度和穩(wěn)定性有著重要的影響。

因動力學(xué)和摩擦等因素，傳統(tǒng)的數(shù)學(xué)模型機械臂末端軌跡規(guī)劃和控制難以準(zhǔn)確描述機械臂運動特性。基于視覺引導(dǎo)的機械臂末端軌跡規(guī)劃和控制備受關(guān)注。視覺引導(dǎo)方法可實現(xiàn)目標(biāo)物體的實時識別和跟蹤，提高機械臂末端定位和運動控制精度。在制造業(yè)中，早期的焊接通常采用機械定位和人工示教，精度低、瑕疵率高、產(chǎn)品部件不匹配，甚至產(chǎn)生結(jié)構(gòu)畸變。當(dāng)人工示教無法滿足工廠的精度要求時，機器視覺系統(tǒng)的定位工作備受關(guān)注。因此，本文提出了一種基于PatMax 視覺引導(dǎo)的機械臂末端軌跡規(guī)劃與控制系統(tǒng)。該系統(tǒng)采用了Cognex 公司旗下的VisionPro 視覺軟件，對已有的視覺規(guī)劃和控制系統(tǒng)進行了二次開發(fā)[1-2]，可以更好地實現(xiàn)機械臂末端的軌跡規(guī)劃和精確定位。

1 視覺定位系統(tǒng)硬件設(shè)計

視覺技術(shù)硬件組件的設(shè)計是其使用的關(guān)鍵之一。光源、鏡頭、攝像頭以及攝像頭與計算機之間的接口是視覺定位系統(tǒng)的主要硬件元件。康耐視3DA5120 點云相機是本文在研究中使用的主要設(shè)備之一。該相機具有超過150 萬個3D 數(shù)據(jù)點，可檢測元件上的細(xì)微特征，構(gòu)建高分辨率的3D 數(shù)據(jù)點云圖像。另外，采用了適應(yīng)性強的協(xié)作式工業(yè)機器人UR5，通過編寫程序，可以沿著所需的運動軌跡來精確地移動，也可以使用電信號與參與其他機器設(shè)備工作時進行通信。在數(shù)據(jù)處理方面，配備了具有獨立顯卡RTX2060 的視覺處理工控機。為保證數(shù)據(jù)傳輸?shù)姆€(wěn)定性，使用四口網(wǎng)線交換機和五類通訊網(wǎng)線（30 m）。視覺定位系統(tǒng)硬件如圖1 所示。

圖1 視覺定位系統(tǒng)硬件圖Fig.1 Visual positioning system hardware diagram

3D 相機和工控機之間通過BCM 萬兆以太網(wǎng)口建立連接，以確保圖像傳輸?shù)乃俣群唾|(zhì)量。機械臂控制柜和工控機之間建立了TCP/IP 通信[3-4]，通過網(wǎng)口進行數(shù)據(jù)傳輸。機械臂控制柜在上位機的角色下，向工控機發(fā)出拍照指令，實現(xiàn)了機械臂的拍照功能。之后，工業(yè)控制計算機指揮相機進行拍照操作，并將捕獲的圖像數(shù)據(jù)傳遞給工業(yè)控制計算機內(nèi)部的程序進行處理。將經(jīng)過處理的數(shù)據(jù)轉(zhuǎn)化為信號，然后傳輸給機械臂，以便機械臂能夠執(zhí)行相應(yīng)的動作。

2 視覺定位系統(tǒng)軟件設(shè)計

2.1 圖像采集（獲取）

本文采用了基于康耐視A5000 點云相機的視覺系統(tǒng)，該系統(tǒng)使用了Gigabit Ethernet 萬兆接口，利用VisionPro[5]軟件內(nèi)原有的CogAcqfifotool 工具來進行采集并處理拍攝的圖像。在這個軟件的視覺系統(tǒng)中，首先創(chuàng)建了一個圖像抓取工具用來連接，此工具是維持先進先出隊列的Acquisition FiFo 對象，可以設(shè)置視頻格式、亮度、對比度、曝光、對等參數(shù)[6]，從而獲取含有深度信息的圖像。將采集的深度圖像信息傳輸給輸入圖像，并且同時進行后續(xù)工作的處理。通過這樣的設(shè)計，可以實現(xiàn)對圖像的高效準(zhǔn)確采集和處理，并為后續(xù)的數(shù)據(jù)分析提供有效的支持。

圖像采集步驟：

（1）通過Cognex GigE Vision Configuration 工具配置相機連接環(huán)境；

（2）確認(rèn)相機是否連接；

（3）為康耐視相機設(shè)置視頻格式；

（4）利用FIFO 對象，設(shè)置相機的參數(shù)；

（5）利用Acquire（）方法獲取圖像；

（6）將獲取圖像進行保存。

2.2 手眼標(biāo)定

一般來說，相機與機械臂之間的位置關(guān)系有兩種：一種是“眼在手外”（eye-to-hand），即相機攝像頭的位置在機械臂的外邊，并與機器人的基座的位置（即世界坐標(biāo)系）保持固定，不隨著其位置的改變而發(fā)生移動，這種模式適用于需要穩(wěn)定的視野，或是需要在機械臂的工作空間之外進行視覺檢測和控制的情況；另一種是“眼在手上”（eye-in-hand），即相機攝像頭的位置安裝在機械臂上，并會隨著機械臂位置的變化而發(fā)生移動，需要進行復(fù)雜的姿態(tài)計算和控制，這種模式適用于需要機械臂在其工作空間內(nèi)對目標(biāo)物體進行精確定位和操作的情況，手眼標(biāo)定示意圖如圖2 所示。

圖2 手眼標(biāo)定示意圖Fig.2 Hand-eye calibration diagram

本文采用的是眼在手外的手眼標(biāo)定系統(tǒng)，在機械臂外的架子上固定住Cognex3D 點云相機，并固定機械臂的基座，使其不會隨著機械臂位置的變化而移動。這樣就保證了相機和機械臂底座的位置相對固定。坐標(biāo)變換的示意圖如圖3 所示。

圖3 坐標(biāo)變換示意圖Fig.3 Coordinate transformation diagram

圖3 中：機械臂基座坐標(biāo)系（W）；相機坐標(biāo)系（C）；機械臂末端坐標(biāo)系（B）；工件坐標(biāo)系（A）。

4 個坐標(biāo)系對應(yīng)3 個變換矩陣：

（1）機械臂基座坐標(biāo)系（W）—機械臂末端坐標(biāo)系（B），T（W-B）該變換矩陣可以直接從機械臂坐標(biāo)系得到。

（2）相機坐標(biāo)系（C）—機械臂基座坐標(biāo)系（W），T（C-W）該變換矩陣即為待求取的位置固定矩陣—手眼標(biāo)定矩陣。

（3）工件坐標(biāo)系（A）—相機坐標(biāo)系（C），T（A-C）該變換矩陣是一個相對固定的矩陣，通過視覺定位求取。

3D 相機不需要單獨標(biāo)定，但是需要和機械臂標(biāo)定。待求量是機械臂底座坐標(biāo)系（W）和相機坐標(biāo)系（C）之間的位姿關(guān)系：T（C-W）。

定義坐標(biāo)系A(chǔ)-B 的變換矩陣為T（A-B），根據(jù)坐標(biāo)系之間變換關(guān)系：

在標(biāo)定的過程中，固定住了標(biāo)定板坐標(biāo)系（A）和機械臂的末端坐標(biāo)系（B），所以在機械臂不同姿態(tài)下的T（A-B）均不會發(fā)生變化，這里令X=T（C-W），E=T（A-C），F(xiàn)=T（W-B），所以機械臂姿態(tài)下的X 值不會發(fā)生變化，但Ei、Fi 不同，E 與機械臂相關(guān)，F(xiàn)與相機相關(guān)。

變換后，手眼矩陣為

用平移向量T 和旋轉(zhuǎn)矩陣R 來表示矩陣方程，可以得到：

式中：T 是大小為3×1 的平移矢量；R 是大小為3×3的旋轉(zhuǎn)矩陣。手眼標(biāo)定的基本方程可以由式（6）得到：

進行多次變換機械臂末端位姿，對手眼標(biāo)定方程式（4）求解，即可得到手眼轉(zhuǎn)化矩陣X 的值。為了提高實驗的精準(zhǔn)度，標(biāo)定時共拍攝6 組照片。

2.3 圖像定位

圖像定位前需要首先對圖像進行預(yù)處理，首先使用3DPatMaxTool 工具對目標(biāo)物進行尋找，以獲取物體的坐標(biāo)信息，并將其存儲到數(shù)據(jù)庫中。然后通過在目標(biāo)圖像中尋找相近的形狀來確定目標(biāo)物體的坐標(biāo)信息，該方法不受特定圖像灰度級別的限制，提高了精度和抗干擾性。此外，通過設(shè)定參數(shù)特征閾值，該算法能夠旋轉(zhuǎn)、縮放目標(biāo)，具有較高的靈活性和適應(yīng)性。定位流程如圖4 所示。

圖4 PatMax 定位流程Fig.4 PatMax positioning flow chart

在使用3DPatMaxTool 工具時，首先需要拍攝一張所需的3D 照片，并框選出所需的具體幾何特征。在選框的中心設(shè)定好訓(xùn)練模板原點，用來定位Mark點的位置，這樣可以實現(xiàn)最高精度的定位。經(jīng)過訓(xùn)練后，PatMax[7]利用模板對Mark 點進行圖像模板3D匹配，并返回該圖像的Mark 點的中心坐標(biāo)，以實現(xiàn)精確定位，模板定位如圖5 所示。

圖5 PatMax 特征的模板定位Fig.5 PatMax feature template localization

通過手眼標(biāo)定得到了相機坐標(biāo)系下機械臂底座的位姿H（C-W），模板匹配定位是確定了工件坐標(biāo)系相對于相機的位姿H（A-C），通過讓機械臂直接讀取H（W-B），可以獲得機械臂末端坐標(biāo)系相對于機械臂基坐標(biāo)系的位姿。

工件坐標(biāo)系相對于機械臂末端坐標(biāo)系的位姿為

3 實驗

在本實驗中，機械臂末端需要在2 塊焊板之間移動，利用基于VisionPro 軟件完成的視覺定位技術(shù)，對機械臂末端進行精確定位。而后，通過對機械臂控制系統(tǒng)的編程來控制機械臂末端按照預(yù)先設(shè)定的軌跡進行運動，實現(xiàn)對焊板的自動焊接。

3.1 實驗平臺

本試驗在Win10 系統(tǒng)下，采用C# 的語言環(huán)境，利用VisionPro 開發(fā)軟件進行開發(fā)，手眼標(biāo)定的目標(biāo)物體三維特征明顯，視覺系統(tǒng)操作界面如圖6 所示。

圖6 視覺系統(tǒng)操作界面Fig.6 Visual system operating interface

視覺引導(dǎo)的機械臂末端軌跡規(guī)劃與控制的目的之一是獲取機械臂底座坐標(biāo)系上目標(biāo)物的坐標(biāo)，通過相機拍攝的標(biāo)定，對圖像上的坐標(biāo)與機械臂的底座坐標(biāo)系進行匹配，定位到目標(biāo)物便可以獲取其位置。

3.2 2D 坐標(biāo)與3D 坐標(biāo)轉(zhuǎn)換

為了找到所需的目標(biāo)物，采用了VisionPro 開發(fā)軟件中的3DPatMaxTool 工具，該工具通過內(nèi)置的模板匹配技術(shù)來辨識目標(biāo)物，3DPatMaxTool 工具的輸入圖像是攝像機標(biāo)定后的圖像，目標(biāo)物的位置可以在工控機上直接輸出機械臂底座坐標(biāo)系上的坐標(biāo)。接著繼續(xù)使用以下工具Cog3D Range Grey Splitter Script3D 范圍灰色拆分器腳本，目的是將高度和灰度圖像（16 位Range）分離為高度圖像（16 位Range）和灰度圖像（16 位Grey），以便于軟件進行模板匹配工作。

康耐視3D 點云相機拍攝一次照片會同時得到2D 成像照片和3D 點云圖兩種數(shù)據(jù)。然而在視覺定位軟件系統(tǒng)中，僅能直接使用2D 成像照片數(shù)據(jù)，而無法直接讀取3D 點云圖數(shù)據(jù)。因此，需要通過代碼編寫的方式，將2D 成像照片中的坐標(biāo)點轉(zhuǎn)換為3D點云的坐標(biāo)，增加一個坐標(biāo)軸的坐標(biāo)信息，以便于實現(xiàn)3D 點云圖像的使用。

文中所拍攝的6 幀點云圖片同時記錄了機械臂末端執(zhí)行器在機器人坐標(biāo)系下的位置，如表1 所示。表1 中X，Y，Z，UX，UY，UZ 分別代表六軸機械臂執(zhí)行器在機械臂基座坐標(biāo)系的坐標(biāo)。

表1 機器人六軸坐標(biāo)（相對于機械臂基座坐標(biāo)系）Tab.1 Robot six-axis coordinates（relative to arm base coordinates）

3.3 實驗結(jié)果

完成所有訓(xùn)練之后，進行機械臂末端移動實驗，改變工件的位置并使用相機進行拍照定位，識別出的目標(biāo)物與模板進行匹配，以坐標(biāo)的形式發(fā)送到工控機，保存在設(shè)置好的路徑，然后傳輸給機械臂，機械臂末端可到達所需的位姿。標(biāo)定時共拍攝6 組照片，如圖7 所示。

圖7 點云標(biāo)定圖Fig.7 Point cloud calibration map

將訓(xùn)練好的1 組圖像與其他的5 組圖像使用3DPatMax 能快速匹配出標(biāo)定物的點云特征，按照式（5）可以得到5 個F 矩陣。通過對機械臂基座的位置信息和相機的外參矩陣，對EX=XF 進行求解，最終得到手眼關(guān)系矩陣X：

再移動工件的位置，進行多次重復(fù)試驗，定位準(zhǔn)確率高達99%，3DPatMax 圖像定位得分為0.98。結(jié)果表明，每次都能定位到工件所需要到達的位置。

4 結(jié)語

本文采用旋轉(zhuǎn)矩陣和平移向量以及3DPat-MaxTool 工具對眼在手外的手眼標(biāo)定系統(tǒng)康耐視3D點云相機進行了圖像匹配定位和標(biāo)定，隨后基于成熟的可視化開發(fā)軟件，采用標(biāo)定好的相機進行對視覺系統(tǒng)的二次開發(fā)，以確保其可靠性。實驗結(jié)果表明，采用PatMax 視覺引導(dǎo)的機械臂末端軌跡規(guī)劃與控制系統(tǒng)能夠快速地進行應(yīng)用程序開發(fā)，并準(zhǔn)確地定位訓(xùn)練并識別特征物品的位置，在實現(xiàn)工業(yè)自動化，特別是機器人的精確控制方面具有重要實際應(yīng)用意義。