基于機器人操作系統的機械臂輔助超聲掃描系統研究*

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

( 1.深圳大學醫學部生物醫學工程學院，深圳 518060;2.醫學超聲關鍵技術國家地方聯合工程實驗室，深圳 518060;3.廣東省生物醫學信息檢測與超聲成像重點實驗室，深圳 518060)

1 引 言

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

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

2 基于ROS的系統框架

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

圖1基于ROS的系統框架

Fig1SystemframeworkbasedonROS

3 KinectV2機器人視覺

目前，基于視覺圖像處理來進行場景的認知是機器人視覺導航的主要方法。微軟KinectV2傳感器利用RGB圖像結合深度數據，引入了三維空間信息，能夠使機器人更好地對環境進行認知、辨識，實現導航功能。

3.1 Kinect V2標定與配準

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

圖2RGB攝像頭內參標定

Fig2InternalparametercalibrationofRGBcamera

圖3IR深度攝像頭內參標定

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外參標定，即標定RGB和深度攝像頭的相對物理空間位置，見圖4，對同一棋盤放置位置，使用RGB和深度攝像頭同時進行角點捕捉，得到外參的標定結果見式(3)，R、T表示RGB和IR深度攝像頭之間的外參旋轉矩陣和平移矩陣。

T=[-0.05056 -0.00116 0.00073]T

(3)

圖4 (a).RGB角點捕捉；(b).IR角點標定

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

因RGB攝像頭和深度攝像頭位置不同且圖像的分辨率不同，如果RGB和深度圖像直接融合成彩色點云，兩組數據會出現較大的偏差，將兩個攝像頭進行內外參標定后，可以實現彩色圖和紅外深度圖之間的配準，配準前后效果對比見圖5。本系統使用配準后的點云數據，作為系統視覺導航的源數據。

圖5 (a).配準前彩色點云；(b).配準后彩色點云

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

3.2 點云圖像處理

本研究機械臂的運動控制是圍繞超聲掃描目標位置的點云信息進行的，而點云源數據中包含大量冗余信息。本系統利用點云庫(point cloud library，PCL)來進行前期的視覺點云圖像處理。

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

3.2.2路徑的獲取 我們需要從點云中提取出目標路徑點，讓機械臂持超聲探頭沿著路徑點運動。每個路徑點都包含空間坐標和方向兩部分，這決定探頭到達的位置和實施掃描的姿態。

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

圖6去除離群點前后

Fig6Beforeandaftercomparisonofoutlierremoval

圖7分割出目標物體

Fig7Segmentedtargetobject

圖8自定義路徑點

Fig8Customizedpathpoints

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

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

Fig9Aleast-squareplanefittingestimationinpk

(4)

(5)

(6)

4 UR5機械臂的建模與控制

MoveIt是ROS系統中運動控制的核心功能包，囊括運動規劃、操作控制、3D感知、運動學算法、碰撞檢測等，并且提供良好的界面交互。

4.1 機械臂模型建立

使用ROS中的MoveIt插件加載UR5機械臂的幾何和運動學模型，見圖10，通過TCP/IP通訊使得機械臂模型對應真實機械臂的實時姿態。

圖10MoveIt添加UR5模型(對應右下角實際狀態)

Fig10AddUR5modelinMoveit(Correspondstothelowerrightcorneroftheactualstateofrobotarm)

4.2 運動規劃

已知機械臂的初始位姿和目標位姿，那么需要通過一定的算法，找到一條到達目標位姿的較優路徑，這種算法就稱為運動規劃(Motion Planning)。

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

5 系統坐標系的轉換

圖11坐標系統

Fig11Coordinatesystem

圖12 (a).機械臂手眼標定；(b).手眼標定轉換關系

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

(7)

控制機械臂從位置(1)移動到位置(2)，則：

(8)

聯立，得到：

(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 仿體實驗結果及分析

本研究采用硅膠皮膚仿體驗證所提出的掃描系統的可行性。在KinectV2視野下，仿體分別放置在三個不同的位置，每個位置采集5點數據。記錄從視覺端鼠標選擇目標點到機械臂開始運動的時間間隔，即系統響應時間；圖13測量探頭到達的位置與目標實際位置的偏差，即系統誤差。

見表1，響應時間維持在3 s以下，證明本系統具有一定的實時性，符合醫療環境下的要求。系統誤差見表2，穩定在8 mm以下，這與KinectV2的測量誤差和手眼標定的誤差相關，目前這一精度基本滿足系統的要求，但我們仍需尋求方法來降低系統誤差。

圖13測量探頭與目標實際位置的偏差

Fig13Measurethedeviationoftheprobefromtheactualpositionofthetarget

表1 系統響應時間

7 結語

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

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