基于機(jī)器人操作系統(tǒng)的機(jī)械臂輔助超聲掃描系統(tǒng)研究*

張娟，李銳，程威，秦浩，吳詩堯，劉立，2，3，倪東，2，3，陳思平，2，3，錢建庭，2，3△

( 1.深圳大學(xué)醫(yī)學(xué)部生物醫(yī)學(xué)工程學(xué)院，深圳 518060;2.醫(yī)學(xué)超聲關(guān)鍵技術(shù)國家地方聯(lián)合工程實(shí)驗(yàn)室，深圳 518060;3.廣東省生物醫(yī)學(xué)信息檢測與超聲成像重點(diǎn)實(shí)驗(yàn)室，深圳 518060)

1 引 言

超聲檢查是臨床最為普及的常規(guī)檢查方法之一，這就導(dǎo)致超聲科室工作繁重，超聲醫(yī)師長期以非生理性的姿態(tài)工作，身體呈高負(fù)荷狀態(tài)[1-2]。由此引發(fā)的職業(yè)損傷[3]，已嚴(yán)重影響超聲醫(yī)師的工作和家庭生活，亟待尋求一種新的超聲診療方式來緩解這一情況。

1999年，法國Pierrot教授團(tuán)隊(duì)第一次挑戰(zhàn)性地利用工業(yè)機(jī)器人(PA-10)突破了超聲人工掃描的局限性[4]，之后類似機(jī)器人輔助超聲診療的各種系統(tǒng)設(shè)計(jì)和臨床應(yīng)用都逐步開展起來[5]。2002年，不列顛哥倫比亞大學(xué)的Abolmaesumi團(tuán)隊(duì)將視覺伺服用于機(jī)器人輔助超聲診斷的系統(tǒng)中，同時(shí)也證明了基于互聯(lián)網(wǎng)的機(jī)器人輔助遠(yuǎn)程超聲系統(tǒng)的可行性[6]。機(jī)器人輔助超聲穿刺是超聲機(jī)器人的主流方向之一，2004年，Hong團(tuán)隊(duì)將超聲引導(dǎo)的穿刺機(jī)器人用于實(shí)時(shí)經(jīng)皮膽囊造口術(shù)，并且能夠跟隨患者因非自主性呼吸運(yùn)動(dòng)而引起的器官移位，實(shí)時(shí)調(diào)節(jié)穿刺針的路徑[7]。近年來，約翰·霍普金斯大學(xué)Nassir Navab團(tuán)隊(duì)提出了一種超聲機(jī)器人運(yùn)動(dòng)軌跡規(guī)劃算法，根據(jù)RGB-D相機(jī)數(shù)據(jù)事先分割出感興趣區(qū)域，參數(shù)化生成單個(gè)或多個(gè)平行掃描軌跡并使之覆蓋整個(gè)區(qū)域，自動(dòng)進(jìn)行機(jī)器人超聲采集，該方法已在動(dòng)物內(nèi)臟上實(shí)現(xiàn)[8]。

2 基于ROS的系統(tǒng)框架

目前，超聲機(jī)器人的研究因囊括多個(gè)硬件平臺(tái)，都是較為分散地控制，大多的研究并沒有去探究各模塊如何整合。本研究基于ROS集成的平臺(tái)，提出一種依據(jù)視覺導(dǎo)航，機(jī)械臂實(shí)施自動(dòng)化超聲掃描并獲取實(shí)時(shí)超聲圖像的整體化系統(tǒng)，圖1為系統(tǒng)框架。

圖1基于ROS的系統(tǒng)框架

Fig1SystemframeworkbasedonROS

3 KinectV2機(jī)器人視覺

目前，基于視覺圖像處理來進(jìn)行場景的認(rèn)知是機(jī)器人視覺導(dǎo)航的主要方法。微軟KinectV2傳感器利用RGB圖像結(jié)合深度數(shù)據(jù)，引入了三維空間信息，能夠使機(jī)器人更好地對環(huán)境進(jìn)行認(rèn)知、辨識，實(shí)現(xiàn)導(dǎo)航功能。

3.1 Kinect V2標(biāo)定與配準(zhǔn)

相機(jī)標(biāo)定是KinectV2應(yīng)用于本系統(tǒng)之前必要的一項(xiàng)工作，獲得圖像與物理空間的映射關(guān)系。本研究使用經(jīng)典的張正友棋盤標(biāo)定法[9]，內(nèi)參標(biāo)定過程見圖2、圖3，分別使用RGB和深度攝像頭對20個(gè)不同的棋盤位置進(jìn)行角點(diǎn)捕捉，內(nèi)參標(biāo)定結(jié)果見式(1)，MRGB、DistRGB表示RGB攝像頭的內(nèi)參矩陣和畸變系數(shù)，MIR、DistIR表示深度攝像頭的內(nèi)參矩陣和畸變系數(shù)。

圖2RGB攝像頭內(nèi)參標(biāo)定

Fig2InternalparametercalibrationofRGBcamera

圖3IR深度攝像頭內(nèi)參標(biāo)定

Fig3Internalparametercalibrationofdepthcamera

MRGB=

DistRGB=[0.011，0.063，-0.001，0.005，-0.152]

(1)

DistIR=[0.078，-0.275，-0.002，0.001，0.113]

(2)

KinectV2外參標(biāo)定，即標(biāo)定RGB和深度攝像頭的相對物理空間位置，見圖4，對同一棋盤放置位置，使用RGB和深度攝像頭同時(shí)進(jìn)行角點(diǎn)捕捉，得到外參的標(biāo)定結(jié)果見式(3)，R、T表示RGB和IR深度攝像頭之間的外參旋轉(zhuǎn)矩陣和平移矩陣。

T=[-0.05056 -0.00116 0.00073]T

(3)

圖4 (a).RGB角點(diǎn)捕捉；(b).IR角點(diǎn)標(biāo)定

Fig4 (a).CornercaptureofRGB；(b).CornercaptureofIR

因RGB攝像頭和深度攝像頭位置不同且圖像的分辨率不同，如果RGB和深度圖像直接融合成彩色點(diǎn)云，兩組數(shù)據(jù)會(huì)出現(xiàn)較大的偏差，將兩個(gè)攝像頭進(jìn)行內(nèi)外參標(biāo)定后，可以實(shí)現(xiàn)彩色圖和紅外深度圖之間的配準(zhǔn)，配準(zhǔn)前后效果對比見圖5。本系統(tǒng)使用配準(zhǔn)后的點(diǎn)云數(shù)據(jù)，作為系統(tǒng)視覺導(dǎo)航的源數(shù)據(jù)。

圖5 (a).配準(zhǔn)前彩色點(diǎn)云；(b).配準(zhǔn)后彩色點(diǎn)云

Fig5 (a).Pre-registrationcolorpointcloud；(b).Registratedcolorpointcloud

3.2 點(diǎn)云圖像處理

本研究機(jī)械臂的運(yùn)動(dòng)控制是圍繞超聲掃描目標(biāo)位置的點(diǎn)云信息進(jìn)行的，而點(diǎn)云源數(shù)據(jù)中包含大量冗余信息。本系統(tǒng)利用點(diǎn)云庫(point cloud library，PCL)來進(jìn)行前期的視覺點(diǎn)云圖像處理。

3.2.1去噪 由于硬件設(shè)備的局限，獲得的深度圖像有較多的噪聲信息，因此點(diǎn)云數(shù)據(jù)中含有大量的稀疏離群點(diǎn)，這會(huì)影響點(diǎn)云的局部特征估計(jì)(例如法向量)。本研究移除稀疏離群點(diǎn)的方法是基于點(diǎn)到臨近點(diǎn)的距離分布來計(jì)算的，對每個(gè)點(diǎn)，計(jì)算它到其所有臨近點(diǎn)的平均距離，將得到的結(jié)果假定為一個(gè)高斯分布，其形狀由均值和標(biāo)準(zhǔn)差決定，平均距離在定義范圍之外的點(diǎn)，即為離群點(diǎn)并從數(shù)據(jù)集中去除掉[10]。

3.2.2路徑的獲取 我們需要從點(diǎn)云中提取出目標(biāo)路徑點(diǎn)，讓機(jī)械臂持超聲探頭沿著路徑點(diǎn)運(yùn)動(dòng)。每個(gè)路徑點(diǎn)都包含空間坐標(biāo)和方向兩部分，這決定探頭到達(dá)的位置和實(shí)施掃描的姿態(tài)。

首先需分割出目標(biāo)物體，然后才能在其上進(jìn)行路徑的提取。本研究使用半自動(dòng)的分割方法，在點(diǎn)云可視化窗口中，鼠標(biāo)選擇目標(biāo)物體上一點(diǎn)，以此為種子點(diǎn)，通過區(qū)域增長，將目標(biāo)區(qū)域分割出來，再在其上自定義一個(gè)方向提取一條路徑點(diǎn)，見圖7、圖8。

圖6去除離群點(diǎn)前后

Fig6Beforeandaftercomparisonofoutlierremoval

圖7分割出目標(biāo)物體

Fig7Segmentedtargetobject

圖8自定義路徑點(diǎn)

Fig8Customizedpathpoints

探頭的姿態(tài)取決于路徑點(diǎn)的方向，即物體表面法向量，決定著超聲聲束的入射角度與目標(biāo)物體表面的夾角。我們獲取的點(diǎn)云數(shù)據(jù)集在真實(shí)物體的表面表現(xiàn)為一組定點(diǎn)樣本，本研究的法向量計(jì)算方法是通過對表面點(diǎn)云切向平面的法線估計(jì)來表示此點(diǎn)的法線，見圖9，因此這就轉(zhuǎn)換為最小二乘法平面擬合估計(jì)問題[11-12]。

圖9pk最小二乘法擬合平面

Fig9Aleast-squareplanefittingestimationinpk

(4)

(5)

(6)

4 UR5機(jī)械臂的建模與控制

MoveIt是ROS系統(tǒng)中運(yùn)動(dòng)控制的核心功能包，囊括運(yùn)動(dòng)規(guī)劃、操作控制、3D感知、運(yùn)動(dòng)學(xué)算法、碰撞檢測等，并且提供良好的界面交互。

4.1 機(jī)械臂模型建立

使用ROS中的MoveIt插件加載UR5機(jī)械臂的幾何和運(yùn)動(dòng)學(xué)模型，見圖10，通過TCP/IP通訊使得機(jī)械臂模型對應(yīng)真實(shí)機(jī)械臂的實(shí)時(shí)姿態(tài)。

圖10MoveIt添加UR5模型(對應(yīng)右下角實(shí)際狀態(tài))

Fig10AddUR5modelinMoveit(Correspondstothelowerrightcorneroftheactualstateofrobotarm)

4.2 運(yùn)動(dòng)規(guī)劃

已知機(jī)械臂的初始位姿和目標(biāo)位姿，那么需要通過一定的算法，找到一條到達(dá)目標(biāo)位姿的較優(yōu)路徑，這種算法就稱為運(yùn)動(dòng)規(guī)劃(Motion Planning)。

運(yùn)動(dòng)學(xué)算法是機(jī)械臂控制算法的核心，本研究使用逆向運(yùn)動(dòng)學(xué)(inverse kinematics，IK)算法，即已知機(jī)械臂工具末端在基座坐標(biāo)系下的位置和姿態(tài)，求解對應(yīng)的6個(gè)關(guān)節(jié)的角度。這一過程存在多解情況[14]，而我們只需要一個(gè)唯一確定的解。求解唯一性的過程有兩個(gè)方向，一個(gè)是找到最佳解，使用最優(yōu)規(guī)劃；另一個(gè)是快速找到一個(gè)有效解，使用采樣規(guī)劃。我們使用MoveIt默認(rèn)的開源運(yùn)動(dòng)規(guī)劃庫，其中包含有最優(yōu)規(guī)劃，但總體來說還是一個(gè)采樣規(guī)劃算法庫，利于本系統(tǒng)的快速應(yīng)答。

5 系統(tǒng)坐標(biāo)系的轉(zhuǎn)換

圖11坐標(biāo)系統(tǒng)

Fig11Coordinatesystem

圖12 (a).機(jī)械臂手眼標(biāo)定；(b).手眼標(biāo)定轉(zhuǎn)換關(guān)系

Fig12 (a).Hand-eyecalibrationofrobotarm；(b).Transformationinhand-eyecalibration

(7)

控制機(jī)械臂從位置(1)移動(dòng)到位置(2)，則：

(8)

聯(lián)立，得到：

(9)

(10)

qKB=[qwqxqyqz]

qw=0.01207

qx=-0.00133

qy=0.83050

qz=-0.55689

tKB=[0.13039 1.22668 0.49889]T

(11)

6 仿體實(shí)驗(yàn)結(jié)果及分析

本研究采用硅膠皮膚仿體驗(yàn)證所提出的掃描系統(tǒng)的可行性。在KinectV2視野下，仿體分別放置在三個(gè)不同的位置，每個(gè)位置采集5點(diǎn)數(shù)據(jù)。記錄從視覺端鼠標(biāo)選擇目標(biāo)點(diǎn)到機(jī)械臂開始運(yùn)動(dòng)的時(shí)間間隔，即系統(tǒng)響應(yīng)時(shí)間；圖13測量探頭到達(dá)的位置與目標(biāo)實(shí)際位置的偏差，即系統(tǒng)誤差。

見表1，響應(yīng)時(shí)間維持在3 s以下，證明本系統(tǒng)具有一定的實(shí)時(shí)性，符合醫(yī)療環(huán)境下的要求。系統(tǒng)誤差見表2，穩(wěn)定在8 mm以下，這與KinectV2的測量誤差和手眼標(biāo)定的誤差相關(guān)，目前這一精度基本滿足系統(tǒng)的要求，但我們?nèi)孕鑼で蠓椒▉斫档拖到y(tǒng)誤差。

圖13測量探頭與目標(biāo)實(shí)際位置的偏差

Fig13Measurethedeviationoftheprobefromtheactualpositionofthetarget

表1 系統(tǒng)響應(yīng)時(shí)間

7 結(jié)語

本文提出一種基于ROS的機(jī)械臂輔助醫(yī)學(xué)超聲掃描系統(tǒng)，采用Kinect V2作為視覺導(dǎo)航，通過對掃描路徑的提取，坐標(biāo)系統(tǒng)轉(zhuǎn)換，得到目標(biāo)表面的運(yùn)動(dòng)軌跡規(guī)劃，引導(dǎo)機(jī)械臂持超聲探頭實(shí)施掃描動(dòng)作，采集實(shí)時(shí)超聲影像。通過仿體實(shí)驗(yàn)驗(yàn)證了本系統(tǒng)的實(shí)時(shí)性與精度，結(jié)果表明，在Kinect視覺導(dǎo)航下，機(jī)械臂能夠夾持超聲探頭實(shí)現(xiàn)自主超聲掃描，并且有較高的效率和穩(wěn)定性。從一定程度上來說，本系統(tǒng)有替代超聲醫(yī)生實(shí)施重復(fù)性、機(jī)械性掃描的可行性，使得超聲醫(yī)生有更充裕的時(shí)間去實(shí)施高階的診斷和治療工作，優(yōu)化醫(yī)療資源。

當(dāng)然，本系統(tǒng)仍有許多需要改進(jìn)的地方，機(jī)械臂的動(dòng)作需盡可能地模仿超聲醫(yī)生的掃查手法，進(jìn)一步量化探頭運(yùn)動(dòng)路徑和探頭掃描切角，智能化地針對某一個(gè)待測部位設(shè)計(jì)一系列連貫動(dòng)作；系統(tǒng)還需添加力反饋傳感器模塊，量化探頭與目標(biāo)表面接觸的壓力和角度，解決系統(tǒng)安全性問題。