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

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

（1.合肥工業大學智能互聯系統安徽省實驗室 安徽 230601；2.中國科學院等離子體物理研究所 安徽 230031）

1 引言

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

圖1 EAST 托卡馬克上的MSE 診斷系統布局圖[4]

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

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

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

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

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

2.1 光斑中心點坐標獲取

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

圖3 獲取光斑中心點像素坐標

2.2 坐標變換

空間點投影到圖像上經歷四個坐標系的變換：世界坐標系、相機坐標系、圖像坐標系、像素坐標系，如所示。

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

圖4 四坐標系關系圖

像素坐標系與圖像坐標系之間屬于平面坐標變換，它們之間的轉換關系為：

其中，[uo,vo]是圖像坐標系原點在像素坐標系中的坐標，dx和dy分別是每個像素在圖像平面X 軸和Y 軸方向上的物理尺寸。

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

其中f為焦距（圖像平面與相機坐標系原點的距離）。故像素坐標系到相機坐標系之間的變換可以寫成：

為相機的內參，用K表示。

相機坐標系與基坐標系的變換為空間坐標變換，公式為：

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

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

圖5 相機畸變模型

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

σx和σy是非線性畸變值，它包括徑向畸變和切向畸變等?；兡Ｐ涂捎晒?來表示：

聯立公式-可得：

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

至此，已建立從像素坐標系到基坐標系的坐標變換關系。通過相機的畸變系數σ[k1,k2,k3,p1,p2]對光斑中心點的坐標進行矯正，通過相機的內參K和外參，就可以根據公式求出光斑中心點在基坐標系下的空間坐標。

2.2.1 相機標定

相機的畸變系數σ[k1,k2,k3,p1,p2]和內參K是相機的固有參數，對相機進行標定就可以獲取到這些參數。本文采用了使用最廣泛的張正友0相機參數標定算法。

準備一個12*9 的黑白棋盤格，固定相機不動，在保證棋盤格一直在相機視野的前提下，采集不同位姿下的棋盤格圖片。采集完成后，對相機進行校準。校準結果如下：

2.2.2 白板位置標定

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

圖6 白板位置標定

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

由0 節坐標變換可知：

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

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

2.2.3機械臂手眼標定

一個棋盤格被放置在場景中，并且棋盤格相對于機械臂基座位置不變，這意味著基坐標系與棋盤格的關系是固定不變的。移動機械臂末端，并保證相機始終能夠看到整個棋盤格，可以得到公式和。

圖7 手眼系統標定坐標系關系圖

2.3 激光光束定向

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

圖8 線面交點

點CP 在投影射線上，故有：

同時點CP 在白板平面上，則有：

聯立公式和，消去CP，得：

聯立公式和即可算出激光點CP 的坐標。并且通過公式即可計算出在基坐標系下的激光點坐標BP′。

其中i代表激光點的數量。

移動機械臂到新的位置，獲取新的激光點，并執行與本節所描述的相同的計算過程，以獲得新的激光點位置。當i＞2 時，可以用最小二乘法來擬合光束的空間方向。

最后，采用雅克比逆運動學求解器計算要發送給機械臂運動控制器的有效關節值。

2.4 視覺伺服

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

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

其中，m（t）通常是特征點在圖像上的像素坐標，a通常是相機的內參。將提取的特征m（t）映射至目標特征s*的空間中，并與s*進行比較。當e（t）為零時，系統達到穩定的收斂位置0。

因為希望保持光斑點始終在圖像中心區域，所以s為光斑中心在圖像坐標系下的坐標，而s*為圖像中心的坐標。

兩邊求導可得：

根據剛體運動學，有：

聯立公式,,，可得：

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

圖10 光束定向方法驗證平臺

3 實驗

3.1 光束定向方法驗證平臺硬件設計

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

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

3.2 光束定向方法驗證平臺軟件設計

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

是軟件操作界面，包括機械臂狀態區、相機控制區、機械臂控制區、操作按鈕區、以及日志等區域。紅色框選的是相機控制區，包含幾個相機常用控制按鈕，并且可以實時看到相機采集的圖像。藍色框選的是機械臂控制區，可以手動控制機械臂移動。左邊是機械臂狀態區可以看到機械臂當前的工作狀態。綠色框選的是操作按鈕區，包括視覺伺服開啟關閉、仿真現實切換、圖像采集、圖像加載、圖像存儲、空間方向擬合、方向對齊等功能。最下面則是日志輸出。

圖11 軟件界面圖

3.3 光束定向方法驗證

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

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

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

3.4 結果分析

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

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

圖13 3 點擬合結果（左上）、5 點擬合結果（右上）、7 點擬合結果（左下）、全部擬合結果（右下）

表1 擬合結果

表2 協方差結果

綜上，實驗表明，該方法可以很好的測量出光束的空間方向，平均誤差在0.28°，完全可以滿足MSE 測試的需要，且重復性測試結果均分布在較小范圍內，說明該方法精度高，穩定性好。