機(jī)器人輔助腹腔鏡手術(shù)中的視覺測(cè)量方法研究

汪 祥，陳 柏，鞠 鋒

(南京航空航天大學(xué)機(jī)電學(xué)院，江蘇 南京 210016)

0 引言

在腹腔鏡手術(shù)中，醫(yī)生在患者腹部切開小口，在每個(gè)切口點(diǎn)經(jīng)套管插入各種外科器械和內(nèi)窺鏡。監(jiān)視器實(shí)時(shí)顯示內(nèi)窺鏡拍攝的圖像，醫(yī)生依據(jù)監(jiān)視器提供的視覺信息，操控手術(shù)器械以執(zhí)行手術(shù)任務(wù)。在此過程中，內(nèi)窺鏡通常只提供二維圖像信息，無法提供手術(shù)器械三維空間中的位姿信息。而在機(jī)器人輔助腹腔鏡手術(shù)中，快速而準(zhǔn)確地獲取手術(shù)器械三維位姿信息是機(jī)器人手術(shù)走向智能化的前提[1]。

目前，在國(guó)外，Krupa等[2-3]以激光光斑和LED燈作為標(biāo)記，解決了在人體器官附近控制手術(shù)器械動(dòng)作的問題；Speidel等[4]提出了一種基于不同的視覺特征來分割手術(shù)器械尖端的方法，并在跟蹤手術(shù)器械的過程中使用了多對(duì)象粒子濾波；Haase等[5]使用3-D ToF/RGB內(nèi)窺鏡跟蹤手術(shù)器械，實(shí)驗(yàn)中工具尖端的定位誤差在4 mm以內(nèi)；Khoiy等[6]研制了一種持鏡機(jī)器人的自主控制系統(tǒng)，能夠通過處理術(shù)中實(shí)時(shí)影像實(shí)現(xiàn)自動(dòng)追蹤無標(biāo)記的手術(shù)器械；Shin等[7]研究了一種使用單個(gè)相機(jī)配合標(biāo)記物的手術(shù)器械追蹤技術(shù)，此技術(shù)可以獲取腹腔鏡手術(shù)器械位置、抓取角度和橫滾角度，用于腹腔鏡手術(shù)模擬。

在國(guó)內(nèi)，潘晗倩[8]研究了在X射線圖像中動(dòng)態(tài)追蹤穿刺針并繪制其延長(zhǎng)線的方法，采用了霍夫直線檢測(cè)算法來檢測(cè)穿刺針的直線特征并據(jù)此動(dòng)態(tài)更新圖像中的參考線；劉大鵬等[9]設(shè)計(jì)了一套基于單目視覺和設(shè)置固定標(biāo)識(shí)的手術(shù)導(dǎo)航系統(tǒng)；胡天策等[10]利用透視投影成像的幾何關(guān)系以及內(nèi)窺鏡在術(shù)中移動(dòng)的位移差，提出一種基于內(nèi)窺鏡單目視覺測(cè)量距離和病灶大小的方法;趙子健[11]提出了一種腹腔鏡手術(shù)器械三維實(shí)時(shí)跟蹤方法，該方法以手術(shù)器械插入點(diǎn)和環(huán)形標(biāo)記的位置測(cè)量為基礎(chǔ)，并與器械的深度估計(jì)相結(jié)合;于凌濤等[12-13]建立了一種基于3D動(dòng)態(tài)視覺窗口引導(dǎo)的器械臂運(yùn)動(dòng)算法。

本研究參考了已有的手術(shù)器械位姿測(cè)量方法，針對(duì)腹腔鏡手術(shù)器械位姿測(cè)量，提出了一種快速的位姿測(cè)量算法并設(shè)計(jì)了手術(shù)器械位姿測(cè)量方案。

1 共線PnP問題的一般解法

本文采用常見的單目?jī)?nèi)窺鏡和中心透視投影相機(jī)為研究對(duì)象。針對(duì)在腹腔鏡手術(shù)場(chǎng)景下視覺測(cè)量手術(shù)器械位姿，解PnP問題是一個(gè)可行的方法，即“給定n個(gè)控制點(diǎn)的空間相對(duì)位置并給定由射影中心點(diǎn)到n個(gè)控制點(diǎn)的角度，求取射影中心點(diǎn)到各個(gè)控制點(diǎn)的距離”[14]。共線PnP問題如圖1所示。

圖1 共線PnP問題

空間中點(diǎn)P1,P2,…,Pi,…,Pn在同一直線上，已知任一點(diǎn)Pi與編號(hào)起點(diǎn)P1的距離為λi，其圖像坐標(biāo)為(ui，vi)，相機(jī)的內(nèi)參數(shù)矩陣為K。設(shè)編號(hào)起始點(diǎn)在相機(jī)坐標(biāo)系下的位置向量為t，直線的單位方向向量為r，根據(jù)相機(jī)內(nèi)參數(shù)模型可以得到：

(1)

式(1)可以改寫為

(2)

式(2)可以轉(zhuǎn)換為經(jīng)典的優(yōu)化問題：

min‖Arr+Att‖,rTr=1

(3)

當(dāng)n≥3時(shí)，式(3)有解，其解是由對(duì)稱矩陣E的奇異值分解得到的。

(4)

Ar和At定義為

2 手術(shù)器械位姿測(cè)量方案

應(yīng)用以上算法測(cè)量手術(shù)器械的位姿需要相機(jī)內(nèi)參數(shù)、標(biāo)記的幾何參數(shù)和標(biāo)記點(diǎn)在圖像上的投影。在得到標(biāo)記點(diǎn)的空間位置之后，仍需要根據(jù)標(biāo)記點(diǎn)和手術(shù)器械之間的幾何關(guān)系得到手術(shù)器械的三維表達(dá)。所以本方案由相機(jī)標(biāo)定及畸變校正、標(biāo)記設(shè)計(jì)、圖像處理、標(biāo)記點(diǎn)三維位置計(jì)算和手術(shù)器械的三維表達(dá)5個(gè)部分組成。

2.1 相機(jī)標(biāo)定及畸變校正

相機(jī)標(biāo)定就是利用世界坐標(biāo)系中的已知三維點(diǎn)坐標(biāo)和它在圖像中的像點(diǎn)坐標(biāo)之間的投影關(guān)系,計(jì)算相機(jī)的內(nèi)外參數(shù)的過程。本研究采用常用的Bouguet工具箱進(jìn)行相機(jī)的標(biāo)定工作，標(biāo)定過程見文獻(xiàn)[15]。

相機(jī)標(biāo)定結(jié)果如表1所示。其中,fx，fy是以像素為單位的焦距；(u0,v0)是主點(diǎn)坐標(biāo)。

表1 相機(jī)內(nèi)參數(shù)標(biāo)定結(jié)果

由表1的數(shù)據(jù)可以得到相機(jī)的內(nèi)參數(shù)矩陣為

(5)

相機(jī)的畸變系數(shù)如表2所示。在此次標(biāo)定中設(shè)置了k5不標(biāo)定，即默認(rèn)k5=0。

表2 相機(jī)畸變系數(shù)標(biāo)定結(jié)果

(6)

(7)

相機(jī)畸變校正過程中，真實(shí)圖像I和畸變圖像Id之間的關(guān)系為I(u,v)=Id(ud,vd)。首先根據(jù)式(8)可以得到真實(shí)圖像像素(u,v)對(duì)應(yīng)的歸一化坐標(biāo)(xn,yn)，再根據(jù)式(7)可以得到畸變后的歸一化坐標(biāo)(xd,yd)，根據(jù)式(9)得到對(duì)應(yīng)的畸變圖像像素(ud,vd)。此時(shí)得到的ud和vd可能并非整數(shù)，需要經(jīng)過插值處理才能得到像素值Id(ud,vd)，最后賦值給I(u,v)。循環(huán)以上過程，找遍真實(shí)圖像中所有的像素點(diǎn)，即可獲得真實(shí)圖像。

(8)

(9)

2.2 標(biāo)記設(shè)計(jì)

為方便在內(nèi)窺鏡圖像中檢測(cè)標(biāo)記，設(shè)置標(biāo)記顏色為人體中不常見的綠色，測(cè)得圓環(huán)直徑d=5 mm，λ1=0,λ2=10 mm,λ3=20 mm ，將標(biāo)記貼于手術(shù)器械上，如圖2所示。

圖2 帶有標(biāo)記的手術(shù)器械

2.3 圖像處理

體外仿真環(huán)境下相機(jī)拍攝并校正畸變的一副圖像I。將圖像的色彩通道分離：R=I(:,:,1)、G=I(:,:,2)和B=I(:,:,3)。根據(jù)式(10)得到二值圖像W，其中，T1、T2是可以調(diào)節(jié)的閾值，可以根據(jù)具體的狀態(tài)進(jìn)行調(diào)節(jié)，影響T1、T2的因素主要是光照和綠色標(biāo)記所用的顏料。處理圖3中的圖像時(shí)，取T1=20、T2=20。

圖3 內(nèi)窺鏡在體外實(shí)驗(yàn)中拍攝的圖像

(10)

處理二值圖像W的目的是獲取標(biāo)記區(qū)域的形心。首先分割出二值圖像中的八連通區(qū)域，再計(jì)算各個(gè)連通區(qū)域的面積，篩選出其中面積最大的3個(gè)區(qū)域即為3個(gè)綠色標(biāo)記的圖像區(qū)域，對(duì)區(qū)域中的圖像坐標(biāo)求平均，得到各自連通區(qū)域的形心。用最小二乘法將3個(gè)形心擬合成1條直線，該直線可視為手術(shù)器械直桿影像的中心線，如圖3所示。圖3中白色點(diǎn)為標(biāo)記區(qū)域的形心，經(jīng)過白色點(diǎn)的直線為直桿影像的中心線。

2.4 標(biāo)記點(diǎn)三維坐標(biāo)計(jì)算

在前文所述算法中，輸入所得到的3個(gè)標(biāo)記區(qū)域的形心坐標(biāo)，相機(jī)內(nèi)參數(shù)矩陣K，以及λ1、λ2、λ3，可以輸出標(biāo)記點(diǎn)所在直線的位置向量t和對(duì)應(yīng)的單位方向向量r。進(jìn)而求得P1、P2和P3在相機(jī)坐標(biāo)系下的位置向量為CP1、CP2、CP3。

2.5 手術(shù)器械的三維表達(dá)

本研究使用手術(shù)器械直桿中心線的方程h=M+ql來表達(dá)手術(shù)器械的位姿。其中，h是直桿中心線上任意一點(diǎn)的位置向量；M是直桿中心線上某固定點(diǎn)的位置向量；l是直桿中心線的單位方向向量；q是位置向量為M的點(diǎn)沿著l方向與位置向量為h的點(diǎn)的距離。

手術(shù)器械與不同坐標(biāo)系的示意圖如圖4所示。

圖4 手術(shù)器械位姿視覺測(cè)量

(11)

(12)

(13)

(14)

在算得Cl和CM之后，根據(jù)式(11)和式(12)可以求得Bl和BM。

對(duì)于手術(shù)器械直桿中心線上任意一點(diǎn)，幾何測(cè)量得到其沿著l方向與M點(diǎn)的距離q0，即可求得其在相機(jī)坐標(biāo)系下的位置向量Ch0=CM+q0Cl,以及其在機(jī)器人基座坐標(biāo)系下的位置向量Bh0=BM+q0Bl。

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

3.1 實(shí)驗(yàn)方案

本實(shí)驗(yàn)旨在體外模擬環(huán)境下對(duì)上述手術(shù)器械位置測(cè)量方法的可行性以及測(cè)量精度進(jìn)行測(cè)試。

實(shí)驗(yàn)裝置由PC(個(gè)人電腦)、UR5機(jī)器人、直桿腹腔鏡和腹腔鏡模擬訓(xùn)練模塊等組成，如圖5所示。直桿腹腔鏡的分辨率為1 920×1 080。PC通過USB和直桿鏡頭相連，通過網(wǎng)線和UR5機(jī)器人相連。PC上運(yùn)行MATLAB作為主控制器，通過Image Acquisition Toolbox獲取直桿鏡頭拍攝到的圖像，通過和UR5機(jī)器人的Socket通信,獲取UR5機(jī)器人末端法蘭坐標(biāo)系相對(duì)于基坐標(biāo)系的位置和姿態(tài)。

圖5 腹腔鏡手術(shù)機(jī)器人持鏡臂實(shí)驗(yàn)平臺(tái)

精度是多次重復(fù)測(cè)量某一量時(shí)，各測(cè)定值之間相一致的程度，表征測(cè)量的隨機(jī)誤差大小。為度量上述測(cè)量方案的精度，在保持手術(shù)夾鉗和機(jī)器人基坐標(biāo)系相對(duì)靜止的情況下，從不同的角度拍攝手術(shù)器械的圖像，并根據(jù)第2節(jié)所述方案計(jì)算Bl和BM。本研究根據(jù)式(15)計(jì)算M點(diǎn)位置測(cè)量的平均偏差，式中Xi為BM的各次測(cè)量結(jié)果。Bl在X、Y、Z3個(gè)方向的分量為lx、ly、lz。式(16)中的xi分別代入lx、ly、lz的各次測(cè)量結(jié)果，得到lx、ly、lz測(cè)量結(jié)果的標(biāo)準(zhǔn)差σx、σy、σz。式(15)和式(16)中,n是測(cè)量的總次數(shù)，i=1,2,…,n。

(15)

(16)

3.2 實(shí)驗(yàn)結(jié)果

實(shí)驗(yàn)中，從不同的角度分別拍攝了14幅圖像，即n=14，分別基于各個(gè)圖像和相應(yīng)的機(jī)器人的位姿,計(jì)算了M點(diǎn)在機(jī)器人基坐標(biāo)系下的坐標(biāo)BM和直桿中心線的方向向量在機(jī)器人基坐標(biāo)系下的表示Bl。

14次測(cè)量得到M點(diǎn)在機(jī)器人基坐標(biāo)系下的位置。計(jì)算每個(gè)M點(diǎn)測(cè)量位置與平均測(cè)量位置的距離如圖6所示。14個(gè)偏差距離的最大值為16.3 mm，最小值為1.2 mm,平均偏差為5.5 mm，測(cè)量位置與平均測(cè)量位置的距離的標(biāo)準(zhǔn)差為3.8 mm。

圖6 點(diǎn)M測(cè)量位置與平均測(cè)量位置的距離

直桿的單位方向向量Bl在機(jī)器人基坐標(biāo)系下的3個(gè)分量分別為lx、ly、lz，如圖7所示。lx的最大值為-0.12，最小值為-0.23，平均值為-0.18，標(biāo)準(zhǔn)差σx=0.027。ly的最大值為0.99，最小值為0.97，平均值為0.98，標(biāo)準(zhǔn)差σy=0.005。lz的最大值為0.03，最小值為-0.12，平均值為-0.05，標(biāo)準(zhǔn)差σz=0.043。

圖7 Bl在X、Y、Z方向的分量大小

4 誤差分析

鑒于測(cè)量的誤差較大，猜測(cè)是相機(jī)參數(shù)和機(jī)器人手眼參數(shù)標(biāo)定不夠準(zhǔn)確的原因。故而重新使用Bouguet工具箱對(duì)相機(jī)參數(shù)進(jìn)行了標(biāo)定，在標(biāo)定過程中選取了25幅更為清晰的圖像。得到了不確定度更小，并且考慮了相機(jī)畸變參數(shù)k5(即k5≠0)的相機(jī)參數(shù)。重新在30個(gè)持鏡位姿下測(cè)量了手術(shù)器械的位置和方向，得到手術(shù)器械的位置和方向的測(cè)量結(jié)果如圖8和圖9所示，測(cè)量結(jié)果的密集程度更高。在圖8中，測(cè)量點(diǎn)的平均偏差為1.3 mm,較于圖6中的結(jié)果精度更高。在圖9中，3個(gè)方向的分量的測(cè)量結(jié)果的標(biāo)準(zhǔn)差σx、σy、σz分別為0.003、0.001和0.011，較圖7中的測(cè)量結(jié)果波動(dòng)更小，測(cè)量精度更高。

圖8 相機(jī)重新標(biāo)定后點(diǎn)M測(cè)量位置與平均測(cè)量位置的距離

圖9 相機(jī)重新標(biāo)定后Bl在X、Y、Z 3個(gè)方向分量大小測(cè)量結(jié)果

由此可以看出測(cè)量精度的主要影響因素有相機(jī)內(nèi)參數(shù)的標(biāo)定精度以及相機(jī)畸變模型的準(zhǔn)確性。進(jìn)一步地，可以推測(cè)UR5機(jī)器人以及本文所介紹的視覺測(cè)量算法引起的誤差并不占主導(dǎo)。

此外，由于本文所做實(shí)驗(yàn)都是在光照條件良好、背景干擾少的條件下進(jìn)行的，在體內(nèi)的環(huán)境中還要考慮光照不足、血污和霧氣等惡劣條件引起的誤差。

5 結(jié)束語

本文提出了一種基于求解共線PnP問題的手術(shù)器械位姿視覺測(cè)量方法，包括相機(jī)標(biāo)定及畸變校正、標(biāo)記設(shè)計(jì)、圖像處理、標(biāo)記點(diǎn)三維坐標(biāo)計(jì)算和手術(shù)器械的三維表達(dá)，并對(duì)所提出的視覺測(cè)量方法進(jìn)行了精度測(cè)試實(shí)驗(yàn)，實(shí)現(xiàn)了提取腹腔鏡的二維圖像信息并將其轉(zhuǎn)換為手術(shù)器械在三維空間中的位姿信息。對(duì)測(cè)量的誤差進(jìn)行了分析，發(fā)現(xiàn)相機(jī)內(nèi)參數(shù)的標(biāo)定精度對(duì)測(cè)量精度影響較大。

測(cè)量結(jié)果表明，目前所提出的手術(shù)器械位姿檢測(cè)方法的精度尚不滿足指引機(jī)器人自主進(jìn)行準(zhǔn)確的手術(shù)操作如切除和縫合等的要求。但該方法仍然具有一定的應(yīng)用價(jià)值。一方面測(cè)量精度可以被提高以達(dá)到應(yīng)用要求，可以通過使用更高精度的相機(jī)、更準(zhǔn)確的參數(shù)標(biāo)定等方式提高測(cè)量的精度。另一方面，本研究在對(duì)手術(shù)器械的位姿精度要求不高的場(chǎng)合，如腹腔鏡手術(shù)過程中的視覺追蹤和手術(shù)器械的輔助定位等具有良好的應(yīng)用前景。