基于雙目視覺的機械臂位姿估計及目標自動定位系統

曹陽，丁一岷

（國網嘉興供電公司，浙江嘉興 314001）

當前機器人在自動化生產線上得到了廣泛的應用，機械臂位姿有效檢測是生產線自動化程度和生產效率提高的關鍵技術之一[1]。若要對工件進行自動化抓取和包裝，必須實現機械臂位姿的自動估計。傳統使用的關鍵性技術是基于目標特性約束的激光測距和單目攝像機估計技術，該技術利用機械臂末端的三點式激光測距傳感器進行位姿估計，但易受距離影響，估計方法穩定性差；使用基于目標特征約束的單眼照相機位姿估計法，根據提取的對接圓和輪廓特征估計出機械臂的位置和姿態初始值，但易受噪聲影響，估計精度低。針對這一問題，設計了基于雙目視覺的機械臂位姿估計及目標自動定位系統。

1 系統硬件結構設計

雙目視覺采用非經典收斂式雙目結構，系統由上、下兩個處理單元組成，每個處理單元處理通道的圖像數據[2]。下處理器采用DSP 智能攝像機，上處理器采用PC 主機[3]。系統硬件結構如圖1 所示。

圖1 系統硬件結構

如圖1 所示，該攝像機是以DSP 為核心的圖像處理平臺，可以完成圖像的采集和處理[4]。相機采用FPGA 實現CCD 的時序控制和數據采集，同一外部控制信號觸發兩相機進行同步圖像采集。采用以太網技術，實現了攝像機與主機之間的通信，即攝像機之間的圖像傳輸，最后由上位機獲得機械臂的姿態數據并控制其移動[5-7]。

1.1 高壓柜上按鈕

高壓柜上的按鈕是低壓電器，可手動或自動復位[8-9]。按鈕通常是在電路中發出一條啟動或停止指令，以控制接觸器、繼電器等線圈的電流，圖2 中顯示了它的結構[10]。

由圖2 可知，按鈕會有不同的標志和顏色。一般來說，紅色代表停止，綠色代表開啟。急停鍵是紅色的蘑菇頭，必須配備金屬保護環[11]?？廴哂诎粹o蓋，以防止誤觸按鈕導致誤操作。高壓柜上按鈕操作過程為：通電后按停止鍵，松開后再關閉。按下啟動鍵關閉，釋放后繼續斷開[12]。

圖2 高壓柜上按鈕結構

1.2 FPGA

FPGA 是適用于數字集成電路的通信協議芯片，其基本結構如圖3 所示。

圖3 FPGA芯片基本結構

由圖3 可知，FPGA 的內部組件包含了可配置邏輯塊（CLB）、寄存器（IOB）、顏色轉換模塊（CB）、布線資源結構模塊（SB）等，這些模塊可以組合使用，通過可編程序邏輯資源連接多個模塊，形成FPGA 電路，具有高速并行計算速度快、功耗低的優點。

2 系統軟件部分設計

2.1 紋理圖像和三維點云配準

為了引導機械臂抵達指定目標位置并完成操作任務，需要利用雙目視覺將操作目標的空間位置信息傳遞給機械臂[13-14]。利用雙目攝像機進行立體定標，獲得系統內外參數。通過攝像機捕獲雙目圖像，根據系統內外參數進行立體校正[15]。經修正的雙目圖像進行立體匹配，由此獲得視差圖[16]。由此構建雙目視覺模型如圖4 所示。

圖4 雙目視覺模型

由圖4 可知，兩臺相機像平面嚴格向前方平行線對準，兩臺物理焦距f一致的相機之間的基線距離為T，像元大小為a。在三維立體空間中的某點A(x,y,z)在兩臺相機成像平面中的映射點分別為O1和O2，對應雙目視覺模型中的坐標分別為(a1,v)和(a2,v)，由此可得到雙目視覺差為：

采用二維特征匹配方法，建立紋理圖像和校正后的左雙目圖像的對應關系。具體的匹配過程如下：從圖像中提取特征點，通過特征點的匹配來求解單應性矩陣，并將其轉化為同一坐標系。

1）特征點匹配

設紋理圖像中的某點像素坐標為(x,y)，與其相對應的左目像素坐標為(x′,y′)，由此得到紋理圖像與左目像素齊次坐標模式下對應關系：

式（2）中，W表示單應性矩陣；λ表示映射尺度因子，是一個常數。

2）單應性矩陣求解

單應性矩陣的計算公式為：

式（3）中，單應性矩陣W是通過特征點匹配后計算得到的，通過式（2）可得到紋理圖像與雙目視覺對應的關系。

3）三維坐標與紋理圖像匹配

三維立體空間中的某點A(x,y,z)坐標與紋理圖像中二維坐標(x,y)匹配關系為：

式（4）中，Q-1表示雙目視覺重建的投影逆矩陣；WD表示單應性矩陣中的逆投影結構，計算公式為：

式（5）中，采用左雙目匹配的橋形圖，可以得到匹配紋理圖像的二維坐標和雙目三維坐標點云模型的關系。通過對匹配關系的確定，將單視紋理圖像映射到雙目視覺模型，得到目標中心的姿態信息。

2.2 目標旋轉中心和方向向量確定

由于機器人本身位置定位存在一定誤差，變電站所內光照情況也會發生變化，因此不同操作任務下拍攝的圖像會存在較大差別。為了排除光線變化、位置變動以及柜體表面相似外觀測點的干擾，采用基于模板匹配的粗定位在紋理圖中定位目標的大致位置，再進一步根據目標模板圖像，識別物體位姿。

2.2.1 目標模板圖像形成

構建目標模板圖像庫模塊，針對目標的三維模型，利用球形攝像機對目標視場進行多角度、多距離的投影。以目標三維模型的重心為基準，設定坐標系的原點，該點記為P。轉化空間坐標，將基準坐標系轉化為以觀測點為P′原點的坐標系，兩個坐標系原點之間的距離為Z軸。之后從以P′點為原點的坐標系中獲取三維模型投影目標模板圖像，記錄該目標的三維位置P′點到目標三維模型之間的距離，由此生成目標模板圖像數據集。

2.2.2 機械臂位姿識別

將目標模板圖像放入存儲中，然后，對經過預處理的二維目標圖像進行目標檢測和識別，并采用相似性度量方法將模板圖像與源圖像進行比較，由此計算機械臂位姿坐標：

式（6）中，ori(O,r)表示目標在模板圖像上r橫坐標梯度；ori(I,c+r)表示目標在二維圖像上c+r橫坐標梯度；c表示目標模板圖像中心點。在堆疊相同對象的二維圖像時，根據最終計算結果的值來選擇得分最高的對應位置。

2.3 剔除噪點區域

利用3×3 鄰域高斯模板對圖像數據進行平滑輸入，對二維高斯函數進行采樣得到高斯模版，在圖像處理中得到廣泛應用。給出圖像數據g(i,j)的特定表達式如下：

經過濾波平滑后的圖像數據g(i,j)對圖像中噪聲的侵襲具有一定的魯棒性，利用該方法提取方向紋理特征，機械臂位姿估計及定位結果更加精準。

3 實 驗

3.1 實驗裝置

實驗裝置是基于數字處理器智能相機的雙目視覺平臺，使用UR5 機械臂，如圖5 所示。

圖5 機器人機械臂實物圖

使用數字處理器智能相機采集相關圖像內容，利用標定參數校正，將校正后的圖像發送到相機中，由此完成雙目圖像視差估計，并將結果傳回到PC 主機。利用數據統計結果，實現目標位姿估計。

3.2 實驗數據分析

使用機械臂精確控制目標的位移和旋轉運動，得到姿態剛體變換參數信息，即位姿。實驗選用焦距為8 mm 的鏡頭，工作距離為450 mm，檢測的物體為35 mm×20 mm×20 mm 矩形塊，一共獲取60 張圖。實驗測試集如表1 所示。

表1 實驗測試集

3.3 實驗結果與分析

分別使用激光測距和單目攝像機估計技術Q1、單眼照相機位姿估計法Q2 和基于雙目視覺機械臂位姿估計及目標自動定位系統Q3 對機械臂運動坐標展開對比分析，結果如圖6 所示。

圖6 機械臂運動坐標估計精度對比分析

由圖6 可知，使用激光測距和單目攝像機估計技術在從1 處到2 處的運動軌跡是曲線型的，變化幅度較大，尤其是在坐標點為（40.5,-17,26.5）時，差別最大；使用單眼照相機位姿估計法在從1 處到2 處的運動軌跡是曲線型的，變化幅度比第一種方法要小，但與預期軌跡差別較大，尤其在坐標點為（41.2,-16,28）時，差別最大；使用基于雙目視覺機械臂位姿估計及目標自動定位系統從1 處到2 處的運動軌跡與預期軌跡基本一致，說明使用該系統機械臂運動坐標估計結果較為精準。

4 結束語

文中基于雙目視覺的機械臂位姿估計及目標自動定位系統，利用雙目視覺引導機械臂完成操作任務，采用基于模板匹配的粗定位在紋理圖中定位目標的大致位置，根據目標模板圖像，識別物體位姿，解決了傳統定位系統定位精準度低的問題。