基于FPGA的三維掃描測(cè)頭反光標(biāo)志點(diǎn)中心定位及匹配

王文祥，柳燕飛，張 瑞，李維詩

(合肥工業(yè)大學(xué)儀器科學(xué)與光電工程學(xué)院，安徽合肥 230009)

0 引言

為了實(shí)現(xiàn)三維測(cè)量過程的自動(dòng)化，工業(yè)制造領(lǐng)域通常將三維掃描測(cè)頭與關(guān)節(jié)臂式機(jī)器人結(jié)合，稱作三維掃描測(cè)量機(jī)器人。在測(cè)量過程中掃描測(cè)頭的位置和姿態(tài)由關(guān)節(jié)臂式機(jī)器人確定，因此三維掃描測(cè)量機(jī)器人的測(cè)量精度依賴于關(guān)節(jié)臂式機(jī)器人的定位精度。關(guān)節(jié)臂式機(jī)器人定位精度容易受關(guān)節(jié)臂幾何精度、裝配誤差、關(guān)節(jié)間隙等因素的影響。這些因素帶來的機(jī)器人定位誤差使得三維掃描測(cè)量機(jī)器人難以滿足高精度場(chǎng)合的測(cè)量要求，特別是在檢測(cè)領(lǐng)域。

為了提高三維掃描測(cè)量機(jī)器人的測(cè)量精度，本課題增加一套基于多相機(jī)的三維掃描測(cè)頭視覺定位跟蹤系統(tǒng)。在掃描測(cè)量過程中，視覺定位跟蹤[1-2]系統(tǒng)同步測(cè)量掃描測(cè)頭的位置和姿態(tài)。本課題采用的基于多相機(jī)(4臺(tái))的視覺定位跟蹤系統(tǒng)，相較于傳統(tǒng)的雙相機(jī)視覺定位跟蹤系統(tǒng)，在消除關(guān)節(jié)臂式機(jī)器人定位誤差對(duì)系統(tǒng)測(cè)量精度影響的同時(shí)，大幅提高測(cè)量范圍并降低遮擋現(xiàn)象出現(xiàn)的機(jī)率。但是由于多相機(jī)同時(shí)采樣的數(shù)據(jù)量大，傳統(tǒng)的串行處理器如(PC、DSP等)難以滿足數(shù)據(jù)處理的實(shí)時(shí)性要求。提升系統(tǒng)處理速度的關(guān)鍵之一是提高對(duì)圖像數(shù)據(jù)的處理能力。本課題以并行運(yùn)行方式的FPGA作為核心數(shù)據(jù)處理單元，并對(duì)傳統(tǒng)反光標(biāo)志點(diǎn)中心定位算法加以改進(jìn)，使其能夠運(yùn)用在FPGA上，同時(shí)采用高效的標(biāo)志點(diǎn)編碼方案使標(biāo)志點(diǎn)匹配算法易于在FPGA上實(shí)現(xiàn)。以流水線的方式實(shí)現(xiàn)對(duì)三維掃描測(cè)頭反光標(biāo)志點(diǎn)中心的定位及匹配，實(shí)驗(yàn)結(jié)果表明系統(tǒng)數(shù)據(jù)處理的速度能夠滿足三維掃描測(cè)頭位姿定位跟蹤系統(tǒng)的實(shí)時(shí)性要求(>30 fps)。

1 系統(tǒng)方案

三維掃描測(cè)量機(jī)器人系統(tǒng)結(jié)構(gòu)如圖1所示。

圖1 系統(tǒng)結(jié)構(gòu)圖

三維掃描測(cè)量機(jī)器人系統(tǒng)由關(guān)節(jié)臂式機(jī)器人、激光掃描測(cè)頭、多相機(jī)定位跟蹤系統(tǒng)(4臺(tái)紅外攝像機(jī)、數(shù)據(jù)處理單元)及PC組成。系統(tǒng)工作時(shí)關(guān)節(jié)臂式機(jī)器人帶動(dòng)激光掃描測(cè)頭，按照規(guī)劃好的路徑，對(duì)被測(cè)工件的表面輪廓進(jìn)行三維掃描測(cè)量。紅外攝像機(jī)實(shí)時(shí)采集三維掃描測(cè)量機(jī)器人運(yùn)動(dòng)過程中測(cè)頭的高清圖像(2 048×2 048×8 bits)，輸至數(shù)據(jù)處理單元。數(shù)據(jù)處理單元由Camera Link采集子卡、FPGA主板及NET數(shù)據(jù)傳輸子卡組成。子卡與FPGA主板通過180-pin Samtec Connector接口相連接。Camera Link采集子卡將低壓差分信號(hào)(LVDS)轉(zhuǎn)換成并行信號(hào)。FPGA主板進(jìn)行數(shù)據(jù)的處理，實(shí)現(xiàn)對(duì)掃描測(cè)頭反光標(biāo)志點(diǎn)中心定位及匹配。NET子卡通過雙絞網(wǎng)線采用UDP協(xié)議將處理結(jié)果上傳至PC端。PC端接收匹配結(jié)果并做進(jìn)一步處理。

2 編碼標(biāo)志點(diǎn)匹配算法

2.1 交比不變性

交比是攝影幾何學(xué)中基本的攝影不變量之一[3]。如圖2所示，a,b,c,d及a′，b′，c′，d′分別為空間中任意的共線4點(diǎn)及其成像。2組共線點(diǎn)集a′，b′，c′，d′與點(diǎn)集a,b,c,d的交比分別如式(1)、式(2)所示。

圖2 交比不變性

(1)

(2)

在攝影變換中點(diǎn)集的交比保持不變，即

R(a′，b′，c′，d′)=R(a,b,c,d)

(3)

2.2 編碼方案設(shè)計(jì)

現(xiàn)有的編碼方案[4]存在一個(gè)問題：當(dāng)同時(shí)拍攝到2組編碼標(biāo)志點(diǎn)時(shí)難以分離2組編碼標(biāo)志點(diǎn)，導(dǎo)致誤匹配。本文在現(xiàn)有編碼方案的基礎(chǔ)上，基于攝影測(cè)量的交比不變性設(shè)計(jì)新的編碼方案，更易于2組標(biāo)志點(diǎn)的分割，降低誤匹配的概率。

編碼原理如圖3(a)所示，每組編碼方案由6個(gè)相同的圓形反光標(biāo)志點(diǎn)組成。其中O、C、D、E4點(diǎn)共線，由A、B點(diǎn)確定的直線與O、E點(diǎn)確定的直線相交于Q點(diǎn)。點(diǎn)O、C、D、E、Q按照特定的順序取其中4點(diǎn)可以組合成2組不同的4點(diǎn)共線點(diǎn)集(R1、R2)，每組點(diǎn)集有不同的固定的交比。依據(jù)交點(diǎn)Q的位置(在O、C之間，還是C、D之間)組合成4種不同的編碼圖案：

(1)點(diǎn)集R1、R2交比分別為：1.5、1.125，且Q位于C、D之間，如圖3(b)所示；

(2)點(diǎn)集R1、R2交比分別為：2.0、1.125，且Q位于C、D之間，如圖3(c)所示；

(3)點(diǎn)集R1、R2交比分別為：1.5、1.125，且Q位于O、C之間，如圖3(d)所示；

(4)點(diǎn)集R1、R2交比分別為：2.0、1.125，且Q位于O、C之間，如圖3(e)所示。

(a)

(b) (c) (d)(e)圖3 編碼原理

3 FPGA硬件系統(tǒng)

實(shí)現(xiàn)對(duì)大量高清圖像數(shù)據(jù)實(shí)時(shí)處理的關(guān)鍵在于充分利用FPGA處理器并行運(yùn)算的優(yōu)勢(shì)。以流水線[5]方式實(shí)現(xiàn)4路攝像機(jī)圖像數(shù)據(jù)的同步解析、預(yù)處理、中心點(diǎn)定位及編碼標(biāo)志點(diǎn)匹配等算法，極大提升系統(tǒng)的運(yùn)算速度。對(duì)流水線上各模塊的數(shù)據(jù)采用“乒乓結(jié)構(gòu)”的處理方式，避免存取數(shù)據(jù)的沖突。FPGA硬件系統(tǒng)如圖4所示。開發(fā)平臺(tái)配置有DDR3存儲(chǔ)器用于系統(tǒng)工作過程中緩存圖像數(shù)據(jù)。DDR3上每個(gè)內(nèi)存單元為4 MB存儲(chǔ)空間，用于緩存4路通道的處理結(jié)果。其中每路通道(下文簡(jiǎn)稱：CH)分配有2個(gè)內(nèi)存空間，用于實(shí)現(xiàn)“乒乓結(jié)構(gòu)”。以CH1為例，假設(shè)當(dāng)前時(shí)刻step1的處理結(jié)果存儲(chǔ)在#1 CH1內(nèi)存空間，此時(shí)step2將從#2 CH1內(nèi)存空間中讀取數(shù)據(jù)，以此類推。

圖4 FPGA硬件結(jié)構(gòu)圖

3.1 數(shù)據(jù)解析

Camera Link是一種串行通信協(xié)議，使用低壓差分信號(hào)(LVDS)進(jìn)行數(shù)據(jù)傳輸。共有3種工作模式，分別為Base、Medium和Full。紅外高速攝像機(jī)的圖像為2 048×2 048×8 bits的灰度圖。經(jīng)過Camera Link采集子卡工作在Base模式，解析出的圖像數(shù)據(jù)時(shí)序如圖5所示。其中FVAL、LVAL分別為幀有效和行有效信號(hào)，當(dāng)FVAL、LVAL信號(hào)同時(shí)有效，DATA[15∶0]數(shù)據(jù)有效。解析出的圖像存儲(chǔ)到DDR3對(duì)應(yīng)的內(nèi)存單元。

圖5 Camera link 時(shí)序圖

3.2 預(yù)處理

解析出的圖像數(shù)據(jù)需要經(jīng)過濾波以便后續(xù)步驟進(jìn)一步處理。為了充分利用FPGA處理器并行運(yùn)算的優(yōu)勢(shì)以及其特性，綜合考慮后，采用8鄰域均值濾波算法。即每個(gè)像素點(diǎn)濾波后的灰度由該像素點(diǎn)相鄰的8個(gè)像素灰度平均值代替。FPGA平臺(tái)上實(shí)現(xiàn)方式如圖6所示，通過3×3窗口在圖像上依次滑動(dòng)計(jì)算每個(gè)像素點(diǎn)濾波后的灰度值。

圖6 8鄰域均值濾波

3.3 中心定位

上位機(jī)對(duì)于類圓形成像標(biāo)志點(diǎn)的中心定位[6]一般流程為：先進(jìn)行邊緣檢測(cè)，再通過橢圓擬合或者Hough變換[7]的方式，獲得標(biāo)志點(diǎn)中心位置。然而由于算法計(jì)算量大，這兩種方式都不易于在FPGA處理器上實(shí)現(xiàn)。針對(duì)本系統(tǒng)類圓形成像標(biāo)志點(diǎn)的圖像特點(diǎn)，對(duì)傳統(tǒng)的兩步法連通域標(biāo)記算法[8]進(jìn)行改進(jìn)，再通過灰度重心法求標(biāo)志點(diǎn)中心的像素坐標(biāo)。

兩步法連通域標(biāo)記算法的核心在于設(shè)計(jì)合適的掃描窗口，減少標(biāo)記沖突的發(fā)生從而提高處理速度。因?yàn)橄到y(tǒng)的標(biāo)志點(diǎn)成像均為類圓形，所以采用圖7所示的改進(jìn)掃描窗口，可以有效降低標(biāo)記沖突的發(fā)生。

圖7 改進(jìn)連通域標(biāo)記掃描窗口

上圖中P為當(dāng)前像素灰度值，L1、L2、L3、L4是與當(dāng)前像素值鄰近像素的連通域標(biāo)號(hào)。

在連通域標(biāo)記的基礎(chǔ)上，通過灰度重心法[9]求得標(biāo)志點(diǎn)中心在圖像上的像素坐標(biāo)。計(jì)算公式如式(4)所示。

(4)

式中：f(x,y)為連通域內(nèi)坐標(biāo)為(x,y)的像素的灰度值；Ω為連通域集合；(x′，y′)為連通域的中心坐標(biāo)。

3.4 標(biāo)志點(diǎn)匹配

在Step3中心定位得到的點(diǎn)集基礎(chǔ)上。根據(jù)第3節(jié)所描述的標(biāo)志點(diǎn)匹配算法，首先搜尋4點(diǎn)共線的標(biāo)志點(diǎn)。其次通過交比約束確定點(diǎn)A、B坐標(biāo)。最后通過直線AB與直線OE的交點(diǎn)位置確定圖案編號(hào)及6個(gè)編碼點(diǎn)的坐標(biāo)。

3.5 Channel多路選擇器(MUX)

Channel多路選擇器(MUX)區(qū)分不同數(shù)據(jù)通道對(duì)外部存儲(chǔ)器DDR3的讀寫訪問，防止由同時(shí)對(duì)DDR3發(fā)起的多條讀寫操作引發(fā)的讀寫沖突。MUX狀態(tài)機(jī)如圖8所示，包括：空閑(Idel)、通道1讀(Rd_CH1) 、通道1寫(Wr_CH2)、通道2讀(Rd_ CH2) 、通道2寫(Wr_ CH2)等狀態(tài)。在Idel狀態(tài)下按優(yōu)先級(jí)響應(yīng)來自各通道的讀寫請(qǐng)求，進(jìn)入對(duì)應(yīng)的讀寫狀態(tài)，在讀寫過程中屏蔽其他讀寫請(qǐng)求，讀寫完成后返回Idel狀態(tài)，等待響應(yīng)讀寫請(qǐng)求。

圖8 MUX狀態(tài)機(jī)

3.6 DDR3仲裁控制

為避免系統(tǒng)中多模塊可能同時(shí)對(duì)DDR3的讀寫操作而產(chǎn)生讀寫沖突，需由仲裁[10]模塊合理地控制和管理系統(tǒng)中的讀寫請(qǐng)求。在多個(gè)模塊同時(shí)提出讀寫請(qǐng)求時(shí)，以一定的優(yōu)先級(jí)仲裁讀寫請(qǐng)求。仲裁控制方案一般可分為2種：一般仲裁方案和環(huán)形羅賓仲裁方案。一般仲裁方案是簡(jiǎn)單優(yōu)先級(jí)仲裁器，每個(gè)請(qǐng)求者被分配一個(gè)固定的優(yōu)先級(jí)。環(huán)形羅賓仲裁方案是按一定順序依次查詢請(qǐng)求信號(hào)。如果當(dāng)前被查詢請(qǐng)求信號(hào)被置位則響應(yīng)請(qǐng)求，執(zhí)行完當(dāng)前讀寫操作后繼續(xù)向下查詢請(qǐng)求信號(hào)。由于環(huán)形羅賓仲裁方案布置于FPGA上邏輯復(fù)雜，硬件資源消耗大，所以本系統(tǒng)選用一般仲裁方案。

4 實(shí)驗(yàn)

4.1 實(shí)驗(yàn)平臺(tái)配置

搭建如圖9(a)所示的系統(tǒng)，包括：關(guān)節(jié)臂式機(jī)器人、激光測(cè)頭、紅外攝像機(jī)、數(shù)據(jù)處理單元及PC等。圖9(b)為Terasic TR4數(shù)據(jù)處理單元。

(a)系統(tǒng)總圖

(b)TR4數(shù)據(jù)處理單元圖9 系統(tǒng)實(shí)物圖

4.2 實(shí)驗(yàn)結(jié)果分析

將本系統(tǒng)布置于Terasic TR4開發(fā)平臺(tái)上，該開發(fā)平臺(tái)以Altera Stratix IV FPGA (EP4SGX530C2)作為核心處理器，系統(tǒng)資源消耗如表1所示。

表1 系統(tǒng)資源消耗

為驗(yàn)證系統(tǒng)運(yùn)行效果，將上述的標(biāo)志點(diǎn)中心定位算法和標(biāo)志點(diǎn)匹配算法在Intel Core i7-6700 3.4 GHz CPU，8 GB內(nèi)存，Windows 10操作系統(tǒng)的PC上基于OpenCV3.0庫函數(shù)實(shí)現(xiàn)。系統(tǒng)運(yùn)行過程中編碼標(biāo)志點(diǎn)分布的2種情況如圖10所示。對(duì)應(yīng)的匹配結(jié)果如表2和表3所示。由表2可以看出，對(duì)于單組編碼標(biāo)志點(diǎn)，TR4開發(fā)平臺(tái)4路圖像數(shù)據(jù)處理速度達(dá)到35.7 fps，而PC平臺(tái)為3.3 fps。而對(duì)于2組編碼標(biāo)志點(diǎn)，根據(jù)表3的數(shù)據(jù)，TR4開發(fā)平臺(tái)對(duì)4路圖像數(shù)據(jù)處理速度仍為35.7 fps，而PC平臺(tái)為2.9 fps。

(a)單組編碼標(biāo)志點(diǎn)

(b)2組編碼標(biāo)志點(diǎn)

表2 單組編碼標(biāo)志點(diǎn)匹配結(jié)果

表3 2組編碼標(biāo)志點(diǎn)匹配結(jié)果

5 結(jié)束語

針對(duì)多相機(jī)定位跟蹤系統(tǒng)數(shù)據(jù)量大，數(shù)據(jù)處理實(shí)時(shí)性要求高，普通串行通用處理器難以滿足系統(tǒng)實(shí)時(shí)性要求等問題。選用FPGA處理器作為核心處理單元，充分利用其并行運(yùn)算的特性，并改進(jìn)反光標(biāo)志點(diǎn)中心定位算法及標(biāo)志點(diǎn)匹配算法。實(shí)驗(yàn)結(jié)果表明對(duì)于4路高清圖像(2 048×2 048×8 bits)，無論單組標(biāo)志點(diǎn)圖像，還是2組標(biāo)志點(diǎn)圖像，均能夠?qū)崿F(xiàn)35.7 fps的處理速度，滿足系統(tǒng)實(shí)時(shí)性要求(>30 fps)，相對(duì)于PC平臺(tái)的處理速度(單組標(biāo)志點(diǎn)圖像3.3 fps，2組標(biāo)志圖像2.9 fps)提高了近10倍。