郝增號，丁偉利,2,*，楊 慶

(1.燕山大學(xué) 電氣工程學(xué)院，河北 秦皇島 066004；2.燕山大學(xué) 智能控制系統(tǒng)與智能裝備教育部工程中心，河北 秦皇島 066004)

0 引言

作為一種典型的工業(yè)機器人，龍門機器人被廣泛應(yīng)用于點膠、滴塑、噴涂、碼垛、分揀、包裝、焊接、金屬加工、搬運、上下料、裝配、印刷等領(lǐng)域。工業(yè)4.0大跨距龍門式機器人的研制，是我國工業(yè)機器人領(lǐng)域的一個挑戰(zhàn)性問題。

為了保證龍門機器人的定位精度，可以使用不同類型的外接傳感器進行誤差補償。工業(yè)上用于機械臂定位的傳感器有視覺傳感器、激光傳感器[1]、編碼器[2]、光柵尺[3]等。其中用于定位補償?shù)囊曈X傳感器可以是單目相機[4]、雙目相機[5]、結(jié)構(gòu)光相機[6]。單目相機測量方法由于結(jié)構(gòu)簡單、使用方便,僅使用一臺攝像機即可測量出目標相對于攝像機的三維姿態(tài)數(shù)據(jù)和三維位移數(shù)據(jù),在工業(yè)領(lǐng)域得到了廣泛的研究和應(yīng)用[7-9]。

大型龍門機器人的研制生產(chǎn)，耗時長、耗資巨大，為了滿足龍門機器人運動學(xué)、動力學(xué)、剛度、定位精度等方面的設(shè)計要求，通常的研究方法是利用各種仿真軟件對機器人進行運動學(xué)、動力學(xué)分析、軌跡、路徑規(guī)劃和定位實驗。根據(jù)所介入的模型不同,通常將仿真技術(shù)分為數(shù)字仿真、半物理仿真以及全物理仿真[10]。其中全物理仿真真實性最好,實驗結(jié)果具有較高的可信度,但進行全物理仿真所用的設(shè)備很復(fù)雜,有些條件還無法模擬,且需要相當高的經(jīng)費投入[11]。數(shù)字仿真也是機器人研究的常用方法，如文[7]針對Delta并聯(lián)機械手對目標物體的拾放任務(wù)，建立了 Simulink/Simmechanics聯(lián)合仿真系統(tǒng)，采用Simulink完成多項式軌跡規(guī)劃并得出驅(qū)動臂的實時目標角度，傳遞Simmechanics模擬機械手的連續(xù)軌跡運動，得到機械手的動力學(xué)參數(shù)。這種仿真方法實驗方便，但由于實驗條件經(jīng)常與真實情況相差較大，更適合于理論推導(dǎo)階段使用，將其實驗結(jié)果應(yīng)用于大型機器人的研發(fā)時通常還需進一步實驗。半物理仿真實驗方法兼有數(shù)學(xué)仿真和物理仿真的優(yōu)點，既能較好地監(jiān)測機器人的運行狀態(tài)，又減少了實驗投入，如文[11]針對空間機器人目標捕獲過程進行微重力環(huán)境下的真實實驗問題,建立了半物理仿真系統(tǒng)，用真實視覺對虛擬環(huán)境進行3D重構(gòu)，通過計算目標的相對位置和姿態(tài)，實現(xiàn)對視覺伺服控制的閉環(huán)仿真，系統(tǒng)可靠，對空間機器人的真實實驗具有很高的參考價值。

針對大跨距龍門機器人的視覺定位問題，本文設(shè)計了基于半物理仿真的龍門機器人視覺測量實驗系統(tǒng)。首先利用Unity軟件構(gòu)建了龍門機器人虛擬木工加工場景，并用投影儀和相機搭建了半物理視覺定位實驗平臺，使用Unity中的真實位姿和測量值進行比較，驗證半物理仿真模擬龍門機器人視覺定位的實驗精度，并分析了進行半物理仿真視覺定位實驗的誤差來源與改進方案。

1 龍門機器人半物理仿真視覺測量系統(tǒng)總體設(shè)計

如圖1所示，龍門機器人視覺定位半物理仿真視覺測量系統(tǒng)包含3個部分:虛擬場景生成、相機標定和視覺定位。本文利用Unity軟件仿真生成龍門機器人木工加工場景，同時在虛擬場景中設(shè)置虛擬相機、虛擬標志物和虛擬棋盤格標定板，模擬真實的機械臂抓取工作時視覺定位的整個流程。工業(yè)相機和投影儀分別連接計算機主機，計算機將Unity中生成的仿真圖像通過投影儀投影到投影屏幕上，然后相機采集投影屏幕上的圖片，采集的圖片返回到計算機中。在計算機中先進行相機標定，最后利用視覺定位算法計算出所需龍門的機械臂所需要移動的位姿。

圖1 半物理仿真系統(tǒng)硬件示意圖Fig.1 Hardware diagram of semi physical simulation system

相機標定的目的是標定出虛擬相機、工業(yè)相機與投影儀的內(nèi)參和畸變系數(shù)。標定場景如圖2所示。首先，在Unity中設(shè)置大小為9×8的黑白棋盤格，棋盤格的每一個格子都是2 cm。然后將其固定在虛擬場景的工作臺上，調(diào)整虛擬場景中的龍門機器人機械臂末端，使末端連接的虛擬相機視野對準設(shè)置的虛擬棋盤格標定板，此時虛擬的標定板場景會出現(xiàn)在真實的工業(yè)相機視野中。驅(qū)動虛擬場景中的龍門機器人機械臂移動到不同的位置，固定在機械臂上的虛擬相機采集圖像，虛擬相機的圖像經(jīng)過投影后，工業(yè)相機在各個位置采集虛擬標定板圖片，即可利用標定算法求出此時虛擬相機、投影儀和工業(yè)相機的聯(lián)合內(nèi)參與畸變系數(shù)。

圖2 相機標定場景圖Fig.2 Camera calibration scene

龍門機器人的視覺定位是利用相機采集到的圖像通過圓形特征提取和pnp定位算法得到機械臂所需要移動的位姿，從而到達期望位置。定位標志物如圖3所示，其中左側(cè)的6個圓形位于虛擬場景中機械臂末端夾持的木門上，稱之為機械臂標志物，主要用于確定木門和機械臂之間的位置偏移。右側(cè)的標志物固定在場景中的工作臺上，其位置保持不變，稱之為工作臺標志物，用于定位工作臺。通過相機視野中機械臂標志物跟工作臺標志物的特征點信息可分別求得兩組標志物在相機坐標系下的位姿信息，繼而獲得機械臂標志物與工作臺標志物的相對位姿。獲得相對位姿后，即可驅(qū)動機械臂到達指定位置從而完成達到精準上下料的目的。

圖3 定位標志物圖示Fig.3 Positioning mark

2 關(guān)鍵技術(shù)

2.1 虛擬場景建模與仿真

本文使用Unity 3D軟件對龍門機器人木工加工作業(yè)流程進行虛擬場景建模與仿真。木工加工現(xiàn)場以及龍門機器人按照1∶1建模，虛擬場景如圖4所示。在仿真中，對龍門機器人作業(yè)空間建立三維空間坐標系，然后控制末端機械臂向目標位置行進，到達指定位置后再按照實驗條件進行視覺定位測試。在仿真過程中，在虛擬場景中設(shè)置手動控制模式，使用按鍵上、下、左、右來控制龍門機器人在X、Y軸方向以2 m/s的速度運動；使用按鍵PgUp和PgDn來控制龍門機器人在Z軸方向以1 m/s的速度上下運動；使用按鍵W、A、S、D來控制標定板繞X、Y軸旋轉(zhuǎn)，每檢測到按鍵輸入，標定板在相應(yīng)角度傾斜10°。

圖4 虛擬場景Fig.4 Virtual scene

Unity 3D中通過虛擬相機可以捕獲搭建的虛擬場景圖像。實驗時，創(chuàng)建虛擬相機并放置在龍門機器人末端機械臂上，虛擬相機捕獲到的畫面不存在畸變，參數(shù)面板中的視場角(Field of View，F(xiàn)OV)可以用來調(diào)整相機視野，F(xiàn)OV參數(shù)表示相機的垂直視野，以度為單位，其與相機參數(shù)關(guān)系為

(1)

式中，f表示相機焦距，hsensor表示相機芯片的高。

2.2 半物理仿真平臺相機標定

進行位姿計算時需要獲得相機的內(nèi)參。由于在龍門機器人半物理仿真實驗中，虛擬場景中的圖像為先后經(jīng)過虛擬相機、投影儀、工業(yè)相機，因此在標定相機時需要同時考慮虛擬相機、投影儀與工業(yè)相機模型。相機標定的過程如下：

1)放置投影儀使之正對投影幕布，并通過投影儀的梯形校正功能調(diào)整出方正的投影圖像。

2)測量投影圖像寬高分W0和H0，設(shè)置工業(yè)相機的長寬比等于工業(yè)相機的長寬比W0/H0。例如，當工業(yè)相機采集圖像的寬度為WC時，設(shè)置圖像的高度HC為λWC×(W0/H0)，其中λ為變換后的比例關(guān)系。

3)調(diào)節(jié)工業(yè)相機的工作距離，使相機視野剛好對齊投影圖像。

4)在虛擬場景中設(shè)置標定板，控制機械臂末端移動，使虛擬相機對齊虛擬標定板，并采集圖像。

5)工業(yè)相機采集投影圖像后結(jié)合已知的標定板尺寸信息計算相機的內(nèi)參系數(shù)。

為了實現(xiàn)從工業(yè)相機的圖像坐標系到虛擬場景中的相機坐標系的轉(zhuǎn)換。需要將虛擬場景下的相機坐標系轉(zhuǎn)化為虛擬相機的圖像坐標系，然后將虛擬相機的圖像坐標系轉(zhuǎn)化為投影儀投影到投影幕布上的圖像坐標系，最后將投影幕布的圖像坐標系轉(zhuǎn)化為工業(yè)相機的圖像坐標系。

由于虛擬標定板的單位已知，結(jié)合圖像轉(zhuǎn)換過程的中間圖像可以順序計算每一次轉(zhuǎn)化過程的旋轉(zhuǎn)、平移矩陣，得到工業(yè)相機的圖像坐標系到虛擬場景中相機的圖像坐標系的轉(zhuǎn)換矩陣，具體過程下：

1)虛擬相機的相機坐標系到虛擬相機圖像坐標系的轉(zhuǎn)換：相機采集圖像的成像模型如圖5。

圖5 相機成像模型Fig.5 Camera imaging model

O-xyz為虛擬場景中虛擬相機的相機坐標系，O′-uv是虛擬相機的圖像坐標系，則虛擬場景下相機坐標系中的點P0(X,Y,Z)與虛擬相機的圖像坐標系的圖像點P(u,v)之間的關(guān)系為

(2)

其中，fx，fy，cx，cy為虛擬場景中虛擬相機的內(nèi)參。

2)虛擬相機的圖像坐標系到投影幕布上圖像坐標系的轉(zhuǎn)換：如圖6所示，投影儀在投影時，原圖像(a)由于無法保證投影儀正對幕布，所以投影圖像通常不是方正的矩形，而是如圖6(b)的梯形，造成圖像的失真。若要求解失真圖像與原圖像的變換關(guān)系，需要單獨標定投影模型。為了簡化實驗過程，本文利用投影儀自帶的梯形矯正功能簡化投影模型，梯形校正后的圖像如圖6(c)。

圖6 投影失真示意圖Fig.6 Schematic diagram of projection distortion

經(jīng)過投影儀梯形校正后，虛擬相機圖像坐標系的圖像點P′(u,v)到投影幕布上的圖像坐標系的對應(yīng)點P(x,y)之間的關(guān)系為

(3)

其中，λ為投影儀投影比例，a為投影儀的橫向縮放比例，b為縱向縮放比例。

3)投影幕布上的投影圖像坐標系到工業(yè)相機圖像坐標系的轉(zhuǎn)換：

調(diào)整工業(yè)相機視野的長寬比例，使相機視野對齊投影幕布上的投影圖像。此時工業(yè)相機采集到圖像P″(u′,v′)與投影圖像點P(x,y)關(guān)系為

(4)

其中，?為工業(yè)相機采集圖像相對投影圖像的比例因子。

4)工業(yè)相機的圖像坐標系到虛擬相機的相機坐標系的轉(zhuǎn)換：

由于工業(yè)相機采集的圖像點P″(u′,v′)與虛擬相機采集的圖像P(u,v)的關(guān)系為

(5)

則工業(yè)相機采集到圖像點坐標為P″(u′,v′)與虛擬場景中虛擬相機的相機坐標系對應(yīng)點P0(X,Y,Z)的關(guān)系為

(6)

令fax=?λafx,fby=?λbfy，cax=?λacx，cby=?λbcy，得

(7)

經(jīng)過上述簡化后，由式(7)可以發(fā)現(xiàn)：經(jīng)過投影儀投影，從工業(yè)相機圖像坐標系到虛擬相機的相機坐標系的投影關(guān)系與式(2)所示的相機模型形式一致，因此可以不需要對過程中的每次轉(zhuǎn)換單獨求解，式(7)中的fax，fby，cax，cby即為要獲得的內(nèi)參。可以直接利用已有的標定算法如張正友標定法[12]進行相機標定，求出工業(yè)相機圖像坐標系到虛擬相機的相機坐標系的內(nèi)參。由于此時投影成像模型與相機模型一致，畸變系數(shù)也用相同的方法求解即可。

2.3 基于圓形特征的目標位姿測量方法

2.3.1標志物設(shè)置與圓形特征提取

已知被觀察物體上的n個特征點以及這n個特征點的相對位置時，求解此時被觀察物體相對相機的位姿，即pnp問題。Fichler 等[17]用投影幾何[18]二的方法證明了為保證pnp問題解的唯一性,在特征點共面的情形下需要4個點,在非共面情形下需要6個點。針對木工加工現(xiàn)場要求，本實驗在Unity的場景中設(shè)置了兩組標志物，每一組標志物由三行兩列的6個圓組成，標志物如圖7(a)。

由于機械臂標志物固定于木門上隨著機械臂一起運動，每次抓取木門后機械臂標志物與相機的位置固定不變，運動時機械臂標志物始終位于相機視野偏左的位置。工作臺標志物固定于場景中的工作臺上，機械臂抓取木門移動時工作臺標志物在移動時在相機視野是由遠及近，標志物的位置示例見圖3。

為了從標志物上獲取目標點的信息，本文采用EDCircle[14]算法與AAMED算法[15]檢測圓，其中EDCircle算法檢測圓的步驟如圖7。

圖7 EDCircle圓提取流程圖Fig.7 Extraction process of EDCircle

機械臂末端移動時，起始視野中應(yīng)該為機械臂標志物中的6個圓。若檢測到的起始圓的數(shù)量大于6，則需要篩選出所需的圓。由于標志物上的圓距離固定，且標志物四周背景簡單，因此其他圓形物體擬合得到的圓與標志物上的圓之間的距離較大。通過最小二乘法擬合得到圓心坐標為x1,y1,x2,y2，…，xn,yn。按照式(8)計算每個圓到其他圓的距離和Si，將Si按照大小進行排序，取出Si最小的6個圓即為標志物上的圓。

(8)

得到6個標志物圓心后，記錄為機械臂標志物，對這6個圓的圓心坐標進行排序。標志物中的圓分3行，第一行圓心的坐標為B1(u7,v7)、B2(u8,v8)，第二行圓心的坐標為B3(u9,v9)、B4(u10,v10)，第三行圓心的坐標為B5(u11,v11)、B6(u12,v12)。

隨著機械臂抓取木門開始移動，當相機視野中新出現(xiàn)的圓達到6個時，記新出現(xiàn)的6個圓為工作臺標志物。按同樣的圓心位置排序后的坐標為A1(u1,v1)、A2(u2,v2)、A3(u3,v3)、A4(u4,v4)、A5(u5,v5)、A6(u6,v6)。

2.3.2 求解機械臂在工作臺坐標系下的位置

當2組標志物都出現(xiàn)在相機視野時，需要計算機械臂標志物在工作臺標志物坐標系下的位置。設(shè)相機坐標系為O-xyz，以A1點為坐標系原點，A1A2為x軸，A1A3為y軸，A1向上為z軸建立工作臺坐標系OA-xyz，以B1為坐標系原點，B1B2為x軸，B1B3為Y軸建立的機械臂坐標系OB-XYZ。

圖8 坐標系轉(zhuǎn)換圖Fig.8 Coordinate system conversion diagram

在Unity中設(shè)置的兩個圓之間的間距為2 cm，因此標志物上6個點在工作臺坐標系下的坐標依次為(0,0,0)、(0,2,0)、(2,0,0)、(2,2,0)、(4,0,0)、(4,2,0)。

3 實驗與結(jié)果

實驗中使用的投影儀為極米Z6投影儀，相機使用大恒MARS系列MER-503-20GM-P，相機采用Sony IMX178 CMOS 感光芯片，通過USB 3.0數(shù)據(jù)接口進行圖像數(shù)據(jù)的傳輸，集成I/O(GPIO)接口，相機的最大分辨率為2 448×2 048，采集的圖像大小為2 120×1 500，工作環(huán)境為Win10系統(tǒng)。

3.1 標定實驗及結(jié)果

在Unity中設(shè)置用每個格子2 cm，9×8的黑白棋盤格作為標定板。然后調(diào)整相機正對投影儀，為了保證工業(yè)相機采集的圖像與真實的圖像同比例變化，調(diào)整投影儀的投影圖像位置，使之剛好充滿相機的視野?？刂铺摂M場景中的相機在不同的角度采集場景中標定板的圖像。經(jīng)投影儀投影后，工業(yè)相機共采集40幅棋盤格圖像，40幅圖像的標定誤差見表1，標定計算后得到的內(nèi)參矩陣為[1 102.50,0,1 029.98;0,963.69,594.97;0,0,1],畸變系數(shù)為[-0.002 098 8,-0.000 350 8,0.002 281 5,-1.002 520 9e-005,0.000 209 6]。

表1 相機標定誤差Tab.1 Camera calibration error像素

由測量數(shù)據(jù)可得，40張照片的平均誤差為0.272 94像素。

3.2 位姿測量實驗及結(jié)果

開始測量時，機械臂抓取木門開始移動，此時工作臺坐標系的位置應(yīng)接近相機的中心偏上位置，如圖3。當兩組標志物都出現(xiàn)在相機視野時，開始記錄兩組標志物的圖像位置。

進行測量實驗時，工作臺標志物原點A1在世界坐標系下的位置為(82，151，517)，機械臂坐標系B1在世界坐標系下的起始位置為(76，146，519)。移動機械臂，使機械臂坐標系在x，y，z軸正方向分別移動5次，每次移動10 mm后采集實驗圖像。

計算出機械臂標志物原點在工作臺坐標系下的位置后與Unity中記錄的真實位姿比較，即得到測量誤差。使用EDCircle作為圓心提取算法時，沿著x軸，y軸、z軸移動時的3個方向的誤差統(tǒng)計見圖9。然后增加相機的工作距離，重新移動并計算誤差，此時沿著3個方向移動時的誤差統(tǒng)計如圖10。

標志物圓心橫向間隔2 cm，增加工作距離前后圓心間隔所占據(jù)的像素數(shù)量分別約為150、100，得到圖像中每個像素的大小分別約為0.013 cm、0.02 cm。相機在兩種工作距離下各采集15幅定位圖像，使用EDCircle算法與AAMED算法分別作為圓心提取算法時，3個方向的平均誤差見表2前兩行；提高工作距離后3個方向的平均誤差見表2后兩行。

表2 誤差統(tǒng)計表Tab.2 Error statisticscm

觀察以上實驗結(jié)果，分析得誤差來源主要有3個方面：

1)圓心擬合誤差：圓心擬合誤差指的是圓檢測算法不能得到完全精確的特征點產(chǎn)生的誤差。如表2，采用EDCircle算法提取特征時的平均位姿測量誤差為0.30 cm，采用AAMED算法提取特征時的平均位姿測量誤差為0.23 cm。EDCircle算法的平均誤差略大于AAMDE算法。由于EDCircle算法需先將弧線處理為線段，如圖7，因此擬合得到的圓心精度略低于AAMED算法。

2)工作距離放大誤差：工作距離放大誤差指的是在增加工作距離后被放大的誤差。如圖9，當機械臂沿著某一方向移動時，表中的誤差呈逐漸增大的趨勢，即視覺定位的精度逐漸下降，此誤差產(chǎn)生的原因有兩個：一是由于每次移動時，定位標志會沿坐標軸的方向偏離視野中心，定位標志中的圓變形、變小，導(dǎo)致提取的用于定位的圓心誤差增大；二是由于在相機采集模型中近小遠大，因此當標志位偏離相機時，誤差也會成比例放大了。

圖9 每個方向的誤差統(tǒng)計Fig.9 Error statistics in each direction

圖10 提高工作距離后每個方向的誤差統(tǒng)計Fig.10 Moving error in three directions after increasing working distance

3)旋轉(zhuǎn)誤差：旋轉(zhuǎn)誤差指的是由于旋轉(zhuǎn)矩陣計算的不完全準確導(dǎo)致的其他方向上的誤差。由表2可見，當在一個方向上移動時，不僅該方向上的誤差增大，另外兩個方向的誤差也會變化。原因主要是工業(yè)相機視野無法完全對齊投影圖像，導(dǎo)致計算得到的旋轉(zhuǎn)矩陣模型無法對應(yīng)真實的旋轉(zhuǎn)矩陣模型，因此在本該在一個方向上的位移計算時會導(dǎo)致其他兩個方向也會產(chǎn)生位移。

由以上分析得到，可以從以下幾個方面提高實驗精度：

1)針對圓心提取算法產(chǎn)生的誤差，可以采用其他精度更高的圓提取算法以減小此部分圓心擬合誤差。

2)針對標志物偏離相機視野誤差增大的情況，可以進一步調(diào)節(jié)相機視野，減小工作距離使標志物靠近相機，以減小此工作距離放大誤差。

3)針對相機視野與投影圖像無法完全對齊產(chǎn)生的誤差，可以通過機械工裝等方式對齊相機視野，以減小旋轉(zhuǎn)誤差。

3.3 修訂實驗及結(jié)果

重新固定投影儀，通過梯形校正得到正對的投影圖像。如圖11所示，將相機固定到微動相機調(diào)節(jié)架上，精細調(diào)節(jié)相機視野，使相機視野與投影視野矯正到幾乎完全對齊，以減小旋轉(zhuǎn)誤差。

圖11 微動相機調(diào)節(jié)架Fig.11 Adjusting frame of micro motion camera

按照同樣的流程完成相機標定后，為了減小工作距離放大誤差，降低虛擬相機工作距離，進行位姿測量實驗。由于2 cm的圓心橫向間隔在圖像中占據(jù)約450個像素，此時標志物圓心每個像素的大小為0.004 cm，相比于初次實驗每個像素0.013 cm、0.02 cm，每個像素產(chǎn)生的誤差大大降低。即本來一個像素導(dǎo)致的誤差現(xiàn)在需要偏差七八個像素才會產(chǎn)生同樣大小的真實誤差。然后使用比EDCircle略精確的AAMED算法作為圓檢測算法進行位姿測量實驗以控制圓心擬合誤差。

由于此時相機視野較近，使機械臂坐標系在x，y，z軸正方向分別移動5次，每次僅移動0.1 cm，此時的位姿測量偏差統(tǒng)計見表3。

表3 修訂實驗誤差統(tǒng)計表Tab.3 The statistical table ofrevised experimental error cm

木工家具制造場景中龍門機器人設(shè)計測試需要毫米級別的位姿測量精度。經(jīng)過修訂實驗后，實驗位姿誤差從0.296 cm降調(diào)0.040 cm，可以基本滿足龍門機器人方案設(shè)計測試時的測量要求。在真實實驗時，由于投影誤差被去除，此誤差可以被進一步降低。

4 結(jié)論

本文提出了一種面向木工家具制造的龍門機器人視覺測量半物理仿真實驗系統(tǒng)，可用于在實驗條件受限時方便地進行木工加工場景中龍門機器人視覺定位方案的理論研究測試。為了得到相機參數(shù)并去除投影和相機畸變，本文先設(shè)計了半物理仿真標定方法對相機進行標定，標定實驗結(jié)果驗證了本標定方法的有效性。標定完成后使用基于圓形特征的目標位姿測量方法進行了位姿測量精度實驗，并根據(jù)實驗結(jié)果分析了誤差來源，最后根據(jù)所分析誤差來源修訂實驗流程后重新測試后顯著降低了測量誤差。實驗結(jié)果表明本文所使用的半物理仿真實驗可以實現(xiàn)算法的測試和誤差分析，對設(shè)計龍門機器人在木工家具制造場景下的視覺定位系統(tǒng)具有指導(dǎo)意義。