基于PatMax 視覺引導的機械臂末端軌跡規劃與控制

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

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

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

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

1 視覺定位系統硬件設計

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

圖1 視覺定位系統硬件圖Fig.1 Visual positioning system hardware diagram

3D 相機和工控機之間通過BCM 萬兆以太網口建立連接，以確保圖像傳輸的速度和質量。機械臂控制柜和工控機之間建立了TCP/IP 通信[3-4]，通過網口進行數據傳輸。機械臂控制柜在上位機的角色下，向工控機發出拍照指令，實現了機械臂的拍照功能。之后，工業控制計算機指揮相機進行拍照操作，并將捕獲的圖像數據傳遞給工業控制計算機內部的程序進行處理。將經過處理的數據轉化為信號，然后傳輸給機械臂，以便機械臂能夠執行相應的動作。

2 視覺定位系統軟件設計

2.1 圖像采集（獲取）

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

圖像采集步驟：

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

（2）確認相機是否連接；

（3）為康耐視相機設置視頻格式；

（4）利用FIFO 對象，設置相機的參數；

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

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

2.2 手眼標定

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

圖2 手眼標定示意圖Fig.2 Hand-eye calibration diagram

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

圖3 坐標變換示意圖Fig.3 Coordinate transformation diagram

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

4 個坐標系對應3 個變換矩陣：

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

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

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

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

定義坐標系A-B 的變換矩陣為T（A-B），根據坐標系之間變換關系：

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

變換后，手眼矩陣為

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

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

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

2.3 圖像定位

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

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

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

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

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

工件坐標系相對于機械臂末端坐標系的位姿為

3 實驗

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

3.1 實驗平臺

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

圖6 視覺系統操作界面Fig.6 Visual system operating interface

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

3.2 2D 坐標與3D 坐標轉換

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

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

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

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

3.3 實驗結果

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

圖7 點云標定圖Fig.7 Point cloud calibration map

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

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

4 結語

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