◆彭城 楊洋 樊玉琦通訊作者

（1.合肥工業(yè)大學(xué)智能互聯(lián)系統(tǒng)安徽省實(shí)驗(yàn)室 安徽 230601；2.中國科學(xué)院等離子體物理研究所 安徽 230031）

1 引言

在EAST 托克馬克中采用了多種光學(xué)診斷技術(shù)來分析等離子體的可見光譜，例如電荷交換重組光譜（CXRS）[1]、濾鏡[2]、運(yùn)動(dòng)斯塔克效應(yīng)（MSE）[3]。其中MSE 診斷技術(shù)能夠快速提供局部高精度的等離子體內(nèi)部的磁場信號(hào)，結(jié)合等離子體平衡反演能獲得可靠的電流密度分布。MSE 診斷系統(tǒng)在EAST 上的布局如圖 1 所示，觀察區(qū)集中在中性束注入裝置（NBI）的中心軸上，觀察區(qū)和NBI 中心軸都在EAST 的赤道面附近。觀察區(qū)是一個(gè)窄條帶，尺寸是855mm（長度）*50mm（寬度）。MSE 觀察鏡視野范圍是18.48°，并劃分為17個(gè)方向，以此來收集不同位置的等離子體輻射譜。

圖1 EAST 托卡馬克上的MSE 診斷系統(tǒng)布局圖[4]

良好的光學(xué)診斷性能依賴于高精度的系統(tǒng)校準(zhǔn)。傳統(tǒng)校準(zhǔn)方法是利用一個(gè)白板手動(dòng)去捕捉光斑，在獲取到光斑后，再使用激光測距儀或者高精度測量臂計(jì)算光斑的空間位姿。在整個(gè)測量過程中，需要手動(dòng)移動(dòng)白板，并需要在目視情況下，手動(dòng)將測量臂末端的激光測距儀對(duì)準(zhǔn)白板上的光斑中心，不僅費(fèi)時(shí)費(fèi)力，而且測量精度也難以保證。Huang 等人設(shè)計(jì)了一個(gè)高精度的四維標(biāo)定轉(zhuǎn)臺(tái)，在水平軸向、俯仰、偏航和橫滾四個(gè)維度利用高精度電機(jī)加上偏振光發(fā)生器產(chǎn)生已知空間方向的光束來校準(zhǔn)MSE 診斷設(shè)備。該系統(tǒng)需要先手動(dòng)控制水平軸向、俯仰和偏航三個(gè)維度的電機(jī)將偏振光發(fā)生器對(duì)準(zhǔn)MSE 診斷設(shè)備，再控制橫滾軸和偏振光發(fā)生器來校準(zhǔn)MSE 診斷設(shè)備。此方法采用手動(dòng)對(duì)準(zhǔn)，所以效率不高[5]。Yang 等人提出了基于手眼系統(tǒng)的光束定向方法。在MSE 診斷設(shè)備處放置一個(gè)激光源，用機(jī)械臂攜帶一個(gè)專用的相機(jī)和投影白板裝置，去截取激光束，在白板上生成投影點(diǎn)。然后用直線擬合算法計(jì)算出光束的方向，再將機(jī)械臂末端的偏振光發(fā)生器對(duì)準(zhǔn)MSE 診斷設(shè)備[4]。該方法通過手眼系統(tǒng)將偏振光發(fā)生器對(duì)準(zhǔn)MSE 診斷設(shè)備，提高了效率，減少了校準(zhǔn)時(shí)間。

在本文中，整個(gè)光束定向方法主要參考了Yang 的設(shè)計(jì)，但采用更輕的固定架，以減少末端負(fù)載產(chǎn)生的誤差，并且引入了視覺伺服功能來保證光斑的位置，較前期研究提升了精度和效率。事先給定的NBI 軸方向以及激光束大致方向可以保證相機(jī)獲取到初始光斑位置，視覺伺服功能可以保證光斑在整個(gè)校準(zhǔn)過程中始終處于白板的中心區(qū)域。工作人員僅需要控制機(jī)械臂靠近或遠(yuǎn)離激光源以獲取不同位置的光斑即可。相比較傳統(tǒng)的手動(dòng)校準(zhǔn)的方式，在精度和效率上有了極大的提升。最后通過搭建的物理實(shí)驗(yàn)裝置得出，視覺伺服功能可以很好保證光斑的位置，光束定向的誤差控制在0.32°以內(nèi)，滿足MSE系統(tǒng)光束定向所需要的精度。

圖2 光束定向方法原理示意圖

2 基于視覺伺服的光束定向方法

本文提出了一種基于視覺伺服的光束定向方法，原理如下。機(jī)械臂末端攜帶一個(gè)鋁合金固定架，固定架中間固定著一個(gè)相機(jī)，相機(jī)前方固定著一個(gè)半透明的平面玻璃白板。距離機(jī)械臂基座L 的地方，放置一個(gè)激光筆，以模仿MSE 診斷設(shè)備的激光源。激光打在白板上，相機(jī)就可以采集到光斑圖片，對(duì)光斑圖片進(jìn)行處理獲取光斑中心點(diǎn)坐標(biāo)，再經(jīng)過坐標(biāo)變換得到光斑在機(jī)械臂基座坐標(biāo)系下的空間坐標(biāo)。為了實(shí)現(xiàn)坐標(biāo)變換，需要對(duì)相機(jī)、白板位置以及手眼關(guān)系進(jìn)行標(biāo)定。最后通過直線擬合算法將多個(gè)光斑的空間坐標(biāo)擬合，得到光束的空間方向。視覺伺服功能則保證在機(jī)械臂運(yùn)動(dòng)過程中，光斑始終投射在白板上。

2.1 光斑中心點(diǎn)坐標(biāo)獲取

首先將獲取到的光斑彩色圖轉(zhuǎn)換為灰度圖，如 (a)。由灰度圖可以看出光斑的亮度明顯高于其他區(qū)域，使用OpenCV 開源庫的閾值二值化函數(shù)可以提取出光斑區(qū)域，如(b)。確定光斑區(qū)域后，再用輪廓提取函數(shù)提取出光斑的輪廓?？紤]到激光大部分情況不是垂直照射白板的，形成的光斑多數(shù)是橢圓的，所以選擇使用橢圓擬合算法來擬合提取到的輪廓。最后將擬合到的橢圓加到原圖查看擬合效果，如(c)?？梢园l(fā)現(xiàn)擬合的橢圓可以很好的包裹著整個(gè)光斑。最后可以使用橢圓中心來代表光斑的中心點(diǎn)像素坐標(biāo)md[ud,vd]T。

圖3 獲取光斑中心點(diǎn)像素坐標(biāo)

2.2 坐標(biāo)變換

空間點(diǎn)投影到圖像上經(jīng)歷四個(gè)坐標(biāo)系的變換：世界坐標(biāo)系、相機(jī)坐標(biāo)系、圖像坐標(biāo)系、像素坐標(biāo)系，如所示。

世界坐標(biāo)系是客觀三維世界的絕對(duì)坐標(biāo)系。相機(jī)和光束在三維空間中，需要在世界坐標(biāo)系這個(gè)基準(zhǔn)坐標(biāo)系中來描述它們的位置，在本文中用機(jī)械臂基座坐標(biāo)系（以下簡稱基坐標(biāo)系）[XB,YB,ZB]T來表示。相機(jī)坐標(biāo)系是以相機(jī)的光心為坐標(biāo)原點(diǎn)，XC軸和YC軸分別平行于圖像坐標(biāo)系的X 軸和Y 軸，相機(jī)的光軸為ZC軸，用[XC,YC,ZC]T表示。圖像坐標(biāo)系是以CCD 圖像平面的中心為坐標(biāo)原點(diǎn)，X軸和Y軸分別平行于圖像平面的兩條垂直邊，用[x,y]T表示。圖像坐標(biāo)系是用物理單位（mm）表示像素在圖像中的位置。像素坐標(biāo)系是以CCD 圖像平面的左上角頂點(diǎn)為原點(diǎn)，u軸和v軸分別平行于圖像坐標(biāo)系的X 軸和Y 軸，用[u,v]T表示。像素坐標(biāo)系是以像素為單位的坐標(biāo)系。

圖4 四坐標(biāo)系關(guān)系圖

像素坐標(biāo)系與圖像坐標(biāo)系之間屬于平面坐標(biāo)變換，它們之間的轉(zhuǎn)換關(guān)系為：

其中，[uo,vo]是圖像坐標(biāo)系原點(diǎn)在像素坐標(biāo)系中的坐標(biāo)，dx和dy分別是每個(gè)像素在圖像平面X 軸和Y 軸方向上的物理尺寸。

在針孔相機(jī)模型中，圖像坐標(biāo)系與相機(jī)坐標(biāo)系之間是相似三角形變換，公式為：

其中f為焦距（圖像平面與相機(jī)坐標(biāo)系原點(diǎn)的距離）。故像素坐標(biāo)系到相機(jī)坐標(biāo)系之間的變換可以寫成：

為相機(jī)的內(nèi)參，用K表示。

相機(jī)坐標(biāo)系與基坐標(biāo)系的變換為空間坐標(biāo)變換，公式為：

其中，R為3×3正交旋轉(zhuǎn)矩陣，t為三維平移向量。也叫相機(jī)的外參，可以用表示，聯(lián)立公式-得：

在上面的公式推導(dǎo)過程中，我們采用了針孔相機(jī)模型來描述物和像之間的相似三角形的關(guān)系。但實(shí)際上相機(jī)鏡頭在加工和裝配過程中會(huì)存在誤差，導(dǎo)致相機(jī)圖像平面上實(shí)際所成的像與理想成像之間存在畸變。帶畸變的相機(jī)成像模型如所示。

圖5 相機(jī)畸變模型

其中，md[xd,yd]表示實(shí)際投影點(diǎn)的圖像坐標(biāo)系下的坐標(biāo)，帶有畸變，m[x,y]表示理想投影點(diǎn)的圖像坐標(biāo)系下的坐標(biāo)。鏡頭畸變模型可表示為：

σx和σy是非線性畸變值，它包括徑向畸變和切向畸變等。畸變模型可由公式-來表示：

聯(lián)立公式-可得：

其中，k1,k2,k3表示徑向畸變系數(shù)，p1,p2表示切向畸變系數(shù)，r=

至此，已建立從像素坐標(biāo)系到基坐標(biāo)系的坐標(biāo)變換關(guān)系。通過相機(jī)的畸變系數(shù)σ[k1,k2,k3,p1,p2]對(duì)光斑中心點(diǎn)的坐標(biāo)進(jìn)行矯正，通過相機(jī)的內(nèi)參K和外參，就可以根據(jù)公式求出光斑中心點(diǎn)在基坐標(biāo)系下的空間坐標(biāo)。

2.2.1 相機(jī)標(biāo)定

相機(jī)的畸變系數(shù)σ[k1,k2,k3,p1,p2]和內(nèi)參K是相機(jī)的固有參數(shù)，對(duì)相機(jī)進(jìn)行標(biāo)定就可以獲取到這些參數(shù)。本文采用了使用最廣泛的張正友0相機(jī)參數(shù)標(biāo)定算法。

準(zhǔn)備一個(gè)12*9 的黑白棋盤格，固定相機(jī)不動(dòng)，在保證棋盤格一直在相機(jī)視野的前提下，采集不同位姿下的棋盤格圖片。采集完成后，對(duì)相機(jī)進(jìn)行校準(zhǔn)。校準(zhǔn)結(jié)果如下：

2.2.2 白板位置標(biāo)定

白板固定在相機(jī)正前方，使得相機(jī)可以看到白板有效區(qū)域。白板坐標(biāo)系以[XF,YF,ZF]T表示，坐標(biāo)系原點(diǎn)在白板中心。將標(biāo)定板放在白板上，如所示，標(biāo)定板和白板之間的坐標(biāo)變換是一個(gè)平移變換。因此，在求解出標(biāo)定板相對(duì)于相機(jī)的坐標(biāo)變換之后，再加上一個(gè)平移變換，就可以得出白板相對(duì)于相機(jī)的坐標(biāo)變換。

圖6 白板位置標(biāo)定

求解相機(jī)和標(biāo)定板之間的坐標(biāo)變換是一個(gè)典型的PnP 問題，本文采用DLT 算法來求解該坐標(biāo)變換。棋盤格標(biāo)定板的每個(gè)方格邊長是已知的，假設(shè)標(biāo)定板在XGOGYG平面上，對(duì)每張拍攝的棋盤格圖片，可以通過角點(diǎn)檢測算法獲得圖片中棋盤格的所有角點(diǎn)。設(shè)角點(diǎn)Q 在標(biāo)定板坐標(biāo)系下齊次坐標(biāo)GQ=[qx,qy,qz,1]，其在像素平面上對(duì)應(yīng)的投影特征點(diǎn)。相機(jī)與標(biāo)定板之間的坐標(biāo)變換為：

由0 節(jié)坐標(biāo)變換可知：

用式第三行把ZC/K 消去，得到兩個(gè)約束：

每個(gè)特征點(diǎn)提供了關(guān)于n（n1,n2,n3）的兩個(gè)線性約束，對(duì)于有12 個(gè)未知數(shù)的，6 對(duì)匹配點(diǎn)即可求得。此時(shí)標(biāo)定板的原點(diǎn)在相機(jī)坐標(biāo)系下的坐標(biāo)為COG=[tx,ty,tz]。重復(fù)多次，即可用多個(gè)棋盤格的原點(diǎn)坐標(biāo)擬合出棋盤格所在的平面。用擬合出來的平面數(shù)據(jù)更新。用游標(biāo)卡尺測得標(biāo)定板的厚度為h=1.07mm，即可得到白板相對(duì)于相機(jī)的坐標(biāo)變換矩陣：

2.2.3機(jī)械臂手眼標(biāo)定

一個(gè)棋盤格被放置在場景中，并且棋盤格相對(duì)于機(jī)械臂基座位置不變，這意味著基坐標(biāo)系與棋盤格的關(guān)系是固定不變的。移動(dòng)機(jī)械臂末端，并保證相機(jī)始終能夠看到整個(gè)棋盤格，可以得到公式和。

圖7 手眼系統(tǒng)標(biāo)定坐標(biāo)系關(guān)系圖

2.3 激光光束定向

由可知，相機(jī)坐標(biāo)系下任意一點(diǎn)CM[mx,my,mz]的投影射線經(jīng)過原點(diǎn)OC[0,0,0]和點(diǎn)m[x,y]。點(diǎn)m在相機(jī)坐標(biāo)系下的坐標(biāo)Cm[x,y,f]。為了得到x和y，需要對(duì)之前獲取到的光斑中心點(diǎn)md[ud,vd]T用公式做畸變校正。本文使用OpenCV 中的undistortPoints（）函數(shù)來完成該校正。設(shè)從相機(jī)光學(xué)中心到空間點(diǎn)M的投影射線的方向向量為。則

圖8 線面交點(diǎn)

點(diǎn)CP 在投影射線上，故有：

同時(shí)點(diǎn)CP 在白板平面上，則有：

聯(lián)立公式和，消去CP，得：

聯(lián)立公式和即可算出激光點(diǎn)CP 的坐標(biāo)。并且通過公式即可計(jì)算出在基坐標(biāo)系下的激光點(diǎn)坐標(biāo)BP′。

其中i代表激光點(diǎn)的數(shù)量。

移動(dòng)機(jī)械臂到新的位置，獲取新的激光點(diǎn)，并執(zhí)行與本節(jié)所描述的相同的計(jì)算過程，以獲得新的激光點(diǎn)位置。當(dāng)i＞2 時(shí)，可以用最小二乘法來擬合光束的空間方向。

最后，采用雅克比逆運(yùn)動(dòng)學(xué)求解器計(jì)算要發(fā)送給機(jī)械臂運(yùn)動(dòng)控制器的有效關(guān)節(jié)值。

2.4 視覺伺服

視覺伺服利用視覺信息對(duì)機(jī)器人或者相機(jī)位姿進(jìn)行伺服控制,其目標(biāo)是控制機(jī)器人或者相機(jī)逐步到達(dá)期望的位姿。視覺伺服按照控制系統(tǒng)的構(gòu)成方式分為基于位置的視覺伺服（PBVS）控制、基于圖像的視覺伺服（IBVS）控制0。IBVS 相對(duì)于PBVS 來說，具有較少的計(jì)算量，當(dāng)存在圖像量化誤差、攝像機(jī)標(biāo)定誤差和圖像噪聲時(shí)，IBVS 可以更好地定位與跟蹤。

本文的視覺伺服控制采用IBVS 方法，目的是最小化圖像誤差，該誤差可以定義為：

其中，m（t）通常是特征點(diǎn)在圖像上的像素坐標(biāo)，a通常是相機(jī)的內(nèi)參。將提取的特征m（t）映射至目標(biāo)特征s*的空間中，并與s*進(jìn)行比較。當(dāng)e（t）為零時(shí)，系統(tǒng)達(dá)到穩(wěn)定的收斂位置0。

因?yàn)橄Ｍ３止獍唿c(diǎn)始終在圖像中心區(qū)域，所以s為光斑中心在圖像坐標(biāo)系下的坐標(biāo)，而s*為圖像中心的坐標(biāo)。

兩邊求導(dǎo)可得：

根據(jù)剛體運(yùn)動(dòng)學(xué)，有：

聯(lián)立公式,,，可得：

為圖像雅克比矩陣0，也稱交互矩陣。通過交互矩陣可以建立圖像變化與機(jī)械臂末端運(yùn)動(dòng)之間的關(guān)系。由上式可知，至少需要三組特征點(diǎn)s 的變化率才能求解得到對(duì)應(yīng)的相機(jī)速度CV，再根據(jù)坐標(biāo)變換矩陣即可算出機(jī)械臂末端的速度。本文僅需考慮將光斑控制在白板中心即可，因此只需要控制機(jī)械臂末端X、Y軸平移就可實(shí)現(xiàn)所需要求。等式可以簡化為：

圖10 光束定向方法驗(yàn)證平臺(tái)

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

3.1 光束定向方法驗(yàn)證平臺(tái)硬件設(shè)計(jì)

驗(yàn)證平臺(tái)包含硬件部分和軟件控制部分。硬件部分由新松(GCR5)6 自由度機(jī)械臂、鋁合金固定架、半透明玻璃白板、Basler(acA1920-48gc)相機(jī)以及Computar M0814-MP2 鏡頭組成，如圖9 所示。新松6 自由度機(jī)械臂末端固定著一個(gè)鋁合金材質(zhì)的固定架，來盡可能降低整個(gè)相機(jī)模塊的重量來減少機(jī)械臂的負(fù)載，以減少重力誤差。上方固定著旋轉(zhuǎn)偏振光發(fā)生器用于后續(xù)MSE 系統(tǒng)測試。下方固定著相機(jī)，相機(jī)前方固定著一塊白板，白板的有效面積為100×100mm，由玻璃制作，一面粘貼半透明紙，入射激光束可由正面反射。激光打在半透明的白板上，相機(jī)采集圖片，對(duì)圖像處理即可獲取光斑的中心點(diǎn)像素坐標(biāo)。通過坐標(biāo)變換即可獲取到光斑的空間坐標(biāo)。最后通過直線擬合就可以計(jì)算出光束方向。在計(jì)算出光束方向后，將白板的法向量與光束方向?qū)R，半透明的白板就會(huì)將光束反射回去。根據(jù)光反射原理，激光應(yīng)該被反射到發(fā)射點(diǎn)。距離機(jī)械臂基座L=1.8m 處（MSE 觀察鏡到觀察區(qū)域平均距離）放置一個(gè)激光源，將極坐標(biāo)紙貼在激光筆上，白板將激光反射打在測量板上，測量激光源中心與反射激光點(diǎn)中心之間的距離D，激光束對(duì)準(zhǔn)精度可描述為：

其中，θ表示激光束入射光線向量與反射光線向量之間的夾角。

3.2 光束定向方法驗(yàn)證平臺(tái)軟件設(shè)計(jì)

在硬件平臺(tái)基礎(chǔ)上基于RobWorkStudio0仿真軟件，采用Visual Studio 和Qt 混合編程開發(fā)了光束定向方法驗(yàn)證平臺(tái)軟件，以完成圖像的采集、光斑中心提取、坐標(biāo)變換、直線擬合、機(jī)械臂控制等功能。

是軟件操作界面，包括機(jī)械臂狀態(tài)區(qū)、相機(jī)控制區(qū)、機(jī)械臂控制區(qū)、操作按鈕區(qū)、以及日志等區(qū)域。紅色框選的是相機(jī)控制區(qū)，包含幾個(gè)相機(jī)常用控制按鈕，并且可以實(shí)時(shí)看到相機(jī)采集的圖像。藍(lán)色框選的是機(jī)械臂控制區(qū)，可以手動(dòng)控制機(jī)械臂移動(dòng)。左邊是機(jī)械臂狀態(tài)區(qū)可以看到機(jī)械臂當(dāng)前的工作狀態(tài)。綠色框選的是操作按鈕區(qū)，包括視覺伺服開啟關(guān)閉、仿真現(xiàn)實(shí)切換、圖像采集、圖像加載、圖像存儲(chǔ)、空間方向擬合、方向?qū)R等功能。最下面則是日志輸出。

圖11 軟件界面圖

3.3 光束定向方法驗(yàn)證

是工作流程圖，工作流程如下：軟件啟動(dòng)后加載機(jī)械臂模型。檢查機(jī)械臂和相機(jī)等硬件的連接狀態(tài)和初始的NBI 軸方向、激光束大致方向參數(shù)是否準(zhǔn)確。若機(jī)械臂和相機(jī)都在線，即可在線實(shí)時(shí)的光束定向。開啟相機(jī)，與真實(shí)機(jī)械臂建立通信，這時(shí)候相機(jī)采集的圖片將實(shí)時(shí)顯示在相機(jī)控制區(qū)，機(jī)械臂狀態(tài)也將與真實(shí)機(jī)械臂保持同步。打開視覺伺服功能后，機(jī)械臂將會(huì)自動(dòng)運(yùn)動(dòng)來保持光斑點(diǎn)在相機(jī)視野中心區(qū)域。點(diǎn)擊shoot 按鈕便采集了第一張圖片，經(jīng)過0 節(jié)獲取光斑中心點(diǎn)坐標(biāo)后，若滿足要求，則記錄下光斑的坐標(biāo)以及機(jī)械臂的位姿數(shù)據(jù)。然后手動(dòng)移動(dòng)機(jī)械臂到達(dá)新的位置，再采集新的圖片。當(dāng)滿足要求的光斑的數(shù)據(jù)達(dá)到指定的數(shù)量后，便可以進(jìn)行直線擬合計(jì)算出光束的空間方向。

最后再移動(dòng)機(jī)械臂使得白板法向量與光束方向重合，獲得測量板上反射點(diǎn)和光源之間的距離D，以及方位角。

圖12 光束定向方法工作流程圖

3.4 結(jié)果分析

實(shí)驗(yàn)分成三組，第一組取3 個(gè)光斑點(diǎn)來擬合直線，第二組取5個(gè)，第三組取7 個(gè)，每組重復(fù)24 次，記錄下反射點(diǎn)的極坐標(biāo)，如所示。每組最大、最小和平均偏離距離D以及根據(jù)公式計(jì)算出對(duì)應(yīng)的偏離角如所示。計(jì)算每組數(shù)據(jù)的協(xié)方差矩陣和協(xié)方差的特征值、特征向量，由于協(xié)方差的最大特征向量總是指向數(shù)據(jù)方差最大的方向，且特征向量對(duì)應(yīng)的特征值為數(shù)據(jù)在特征向量方向上的方差，因此使用最大特征值來描述數(shù)據(jù)的離散程度。計(jì)算結(jié)果如所示。根據(jù)上述計(jì)算結(jié)果，畫出95%置信橢圓。如 中3、5、7 點(diǎn)擬合結(jié)果圖左上角所示。

由可知，該方法平均誤差控制在0.3°以內(nèi)，最大誤差為0.32°低于0.5°，滿足MSE 系統(tǒng)的光束定向要求。由可知，隨著擬合的點(diǎn)數(shù)增加，最大特征值在降低，說明反射點(diǎn)越來越集中，擬合效果越好。5 點(diǎn)時(shí)最大特征值最小，擬合最好。

圖13 3 點(diǎn)擬合結(jié)果（左上）、5 點(diǎn)擬合結(jié)果（右上）、7 點(diǎn)擬合結(jié)果（左下）、全部擬合結(jié)果（右下）

表1 擬合結(jié)果

表2 協(xié)方差結(jié)果

綜上，實(shí)驗(yàn)表明，該方法可以很好的測量出光束的空間方向，平均誤差在0.28°，完全可以滿足MSE 測試的需要，且重復(fù)性測試結(jié)果均分布在較小范圍內(nèi)，說明該方法精度高，穩(wěn)定性好。