曹家勇，吳世豪，馬千里

(上海應(yīng)用技術(shù)大學(xué)機械工程學(xué)院，上海 201418)

實驗研究表明，在對患者進行手術(shù)治療之前配合正骨治療，糾正軟組織錯位，能夠?qū)颊叩男g(shù)后恢復(fù)起到良好的促進作用，縮短患者住院時間[1]。但傳統(tǒng)中醫(yī)正骨復(fù)位主要依賴骨科醫(yī)生完成，給臨床醫(yī)生造成了巨大的工作負(fù)擔(dān)與安全隱患。

隨著機器人產(chǎn)業(yè)的快速發(fā)展，機器人輔助手術(shù)治療已逐步在臨床應(yīng)用，為實現(xiàn)臨床疾病微創(chuàng)、精準(zhǔn)、個性化治療提供了一個新的研究方向[2]，相比于傳統(tǒng)人工正骨，現(xiàn)代骨科醫(yī)用機器人可以提高手術(shù)操作的精確性、穩(wěn)定性、可重復(fù)性，從而輔助醫(yī)生完成一些以往無法實現(xiàn)的手術(shù)，促進患者的術(shù)后恢復(fù)。

郭悅等[3]結(jié)合六自由度并聯(lián)機器人模擬中醫(yī)正骨手法，針對簡單骨折的輔助復(fù)位可行性進行了驗證。北京航天航空大學(xué)研發(fā)的天璣骨科機器人是國內(nèi)首個獲得醫(yī)療器械注冊證的手術(shù)機器人產(chǎn)品[4]。這些應(yīng)用都極大地推進了醫(yī)療機器人的發(fā)展。

而伴隨著骨科機器人的發(fā)展，骨科機器人的智能化要求也在不斷提高，根據(jù)不同患者個體的實際情況制定相應(yīng)的手術(shù)方案是未來骨科機器人的研發(fā)趨勢之一[5]。而這需要骨科機器人具有較強的自主導(dǎo)航能力，目前骨科機器人主要通過光學(xué)視覺進行手術(shù)導(dǎo)航，但國內(nèi)外目前的相關(guān)研究主要集中在骨科外科手術(shù)等領(lǐng)域[6]。針對結(jié)合傳統(tǒng)中醫(yī)方法的正骨機器人視覺導(dǎo)航方案相對空白，而能夠正確抓握患者腳部是中醫(yī)正骨方法實施的重要前提。

因此，現(xiàn)基于正骨機器人在面對患者腿部受固定裝置約束狀態(tài)下，針對腳部目標(biāo)的術(shù)前定位和導(dǎo)航問題，提出一種基于立體視覺的腳部目標(biāo)點云位姿計算方法，通過固定在機器人末端的RGB-D相機獲取目標(biāo)點云數(shù)據(jù)，利用點云數(shù)據(jù)計算得到腳部目標(biāo)的位姿數(shù)據(jù)，并結(jié)合任務(wù)需要對數(shù)據(jù)進行修正。同時，基于此方法設(shè)計機器人引導(dǎo)系統(tǒng)，利用修正的腳部目標(biāo)點參數(shù)進行機器人的視覺引導(dǎo)實驗，驗證計算結(jié)果的準(zhǔn)確性和可靠性。

1 系統(tǒng)結(jié)構(gòu)

基于雙目視覺的機器人引導(dǎo)系統(tǒng)由Python編寫的上位機作為控制模塊和Intel RealSense D455深度相機作為視覺模塊組成。選用Universal Robot公司的UR5協(xié)作機器人，控制模塊通過URScript控制機器人，URScript是UniversalRobot公司在Python基礎(chǔ)上研發(fā)的語言，可在腳本層上控制UR機器人。

系統(tǒng)整體硬件結(jié)構(gòu)如圖1所示，深度相機固定在機器人末端，采集圖像并通過雙目立體匹配獲取得到深度圖并轉(zhuǎn)換為點云數(shù)據(jù)，基于點云數(shù)據(jù)獲取腳部目標(biāo)位姿參數(shù)經(jīng)過修正后發(fā)送至下位機(機器人控制器)，從而驅(qū)動機器人末端件以運動至貼合腳部目標(biāo)，完成正骨手術(shù)前的機器人定位和引導(dǎo)工作。

圖1 機器人視覺引導(dǎo)系統(tǒng)Fig.1 Robot vision guidance system

系統(tǒng)工作流程圖如圖2所示，對系統(tǒng)進行相機標(biāo)定和手眼標(biāo)定獲取標(biāo)定參數(shù)用于點云生成和目標(biāo)位姿計算，通過深度相機獲取深度圖并轉(zhuǎn)化為點云數(shù)據(jù)。通過點云數(shù)據(jù)處理在保留腳部點云特征的同時降低點云密度，處理后的點云數(shù)據(jù)的位姿信息經(jīng)過參數(shù)修正后轉(zhuǎn)換為機器人基坐標(biāo)系下的目標(biāo)位姿信息，利用URScript將目標(biāo)位姿信息轉(zhuǎn)為機器人控制指令，通過Socket通信發(fā)送至機器人控制器，完成機器人末端從起始點到腳部目標(biāo)的視覺引導(dǎo)。

圖2 機器人視覺引導(dǎo)系統(tǒng)流程圖Fig.2 Flowchart of robot visual guidance system

2 點云數(shù)據(jù)處理

使用Intel RealSense D455深度相機獲取深度信息計算得到點云數(shù)據(jù)，該深度相機尺寸小巧，內(nèi)置完整的深度處理模塊，同時提供成熟的接口便于程序開發(fā)調(diào)用，因此在機器人、物體識別和三維重建等領(lǐng)域都有很好的應(yīng)用[7]。結(jié)合深度信息和相機內(nèi)外參可以計算出像素點的三維世界坐標(biāo)，這樣便可通過深度圖獲取點云數(shù)據(jù)。

直接獲取的點云數(shù)據(jù)除了包含所需的腳部目標(biāo)點云外，還包含著大量的背景點云和噪聲數(shù)據(jù)。因此需要對原始點云數(shù)據(jù)進行處理，去除多余的點云，保留主體點云，點云處理流程如圖3所示。

圖3 點云處理流程圖Fig.3 Flowchart of point cloud processing

2.1 點云條件濾波

針對深度相機采集的原始點云數(shù)據(jù)中包含大量背景點云的問題，為了快速找到目標(biāo)區(qū)域，采用點云條件濾波方法[8]去除冗余的背景點云數(shù)據(jù)，突出目標(biāo)點云的信息。由于正骨手術(shù)中患者腿部往往固定在固定支架上，腳部目標(biāo)相對于機器人末斷的相機距離固定，因此可以利用條件濾波截取一定范圍內(nèi)的點云數(shù)據(jù)作為感興趣區(qū)域，在此區(qū)域內(nèi)進行目標(biāo)定位。空間坐標(biāo)軸各方向的范圍閾值設(shè)置可表示為

(1)

式(1)中：(Xmin,Xmax)、(Ymin,Ymax)、(Zmin,Zmax)對應(yīng)各方向范圍閾值，相關(guān)閾值可以根據(jù)實際條件計算獲得。

濾波前后感興趣區(qū)域的點云數(shù)量、形狀、位置均未發(fā)生變化[9]。經(jīng)過條件濾波處理后，點云數(shù)量得到了極大降低。

2.2 點云下采樣

為了提高后續(xù)計算速度，在保留腳部目標(biāo)特征信息的基礎(chǔ)上，利用體素濾波算法[10]根據(jù)點云數(shù)據(jù)創(chuàng)建一個最小三維體素柵格，根據(jù)點云分布情況計算出小立方體柵格邊長L，根據(jù)L的長度將三維體素柵格劃分為m×n×l個小柵格，劃分完成后將點云數(shù)據(jù)放入到對應(yīng)的小柵格。對于不包含點云的小柵格進行刪除，對于包含點云數(shù)據(jù)點的小柵格，將距離小柵格重心最近的點云數(shù)據(jù)點保留并刪除小柵格內(nèi)其余點云數(shù)據(jù)點。該方法簡單高效，容易實現(xiàn)，不需要建立復(fù)雜的拓?fù)浣Y(jié)構(gòu)可實現(xiàn)對點云數(shù)量進行整體簡化，從而達(dá)到快速點云計算的目的[11]。每個小立方體柵格中，三維柵格重心的計算公式為

(2)

式(2)中：n為小立方體柵格中點云個數(shù)，(xi,yi,zi)為小立方體柵格的第i個點坐標(biāo)。體素濾波前后的點云整體形狀、位置和大小均保持不變，但下采樣后點云在保留目標(biāo)特征的基礎(chǔ)上數(shù)量大幅降低。

2.3 統(tǒng)計離群點去除

為了去除因為深度相機采集產(chǎn)生的稀疏離群點、點云邊緣噪聲以及條件濾波留下的部分殘留離群點。采用統(tǒng)計離群點算法[12]對離群點進行去除。該算法通過對點云周圍的K近鄰點進行分析來判斷該點是否為離群點進行刪除。如果當(dāng)前點距離K近鄰點的平均距離與整個點云數(shù)據(jù)點之間的平均距離的差值大于一個標(biāo)準(zhǔn)差，則將該點視為離群點。鄰域平均距離的概率密度函數(shù)為

(3)

式(3)中：假設(shè)每一點的鄰域符合高斯分布，其形狀是由均值σ和標(biāo)準(zhǔn)差μ決定。li為任意點的鄰域平均距離。經(jīng)過統(tǒng)計離群點過濾后，腳部目標(biāo)邊緣的離群點得到有效濾除，得到相對規(guī)則的目標(biāo)點云用于后續(xù)的位姿計算。

3 點云位姿計算

3.1 基于PCA的目標(biāo)點云位姿估計

主成分分析法(principal component analysis，PCA)是一種以特征向量分析多元統(tǒng)計分布的方法，表征了數(shù)據(jù)內(nèi)部的主要分布方向，PCA處理可看作求一種線性投影，使數(shù)據(jù)最大方差的方向和新空間的軸對齊。PCA的工作就是從原始的空間中順序找出一組相互正交的坐標(biāo)軸建立PCA坐標(biāo)系，其中，第一坐標(biāo)軸與原始點云數(shù)據(jù)最大分布的方向重合，第二坐標(biāo)軸是與第一坐標(biāo)軸正交點云數(shù)據(jù)最大分布的方向，第三坐標(biāo)軸是與第一、第二坐標(biāo)軸正交點云數(shù)據(jù)最大分布的方向，在PCA中可視為三個相互垂直的主成分向量。因此PCA常用于點云配準(zhǔn)中且具有較好的效果[13]。

COV=(P-μ)T(P-μ)

(4)

(5)

式(5)中:世界坐標(biāo)系o的描述矩陣為三階單位矩陣。

解得的腳部目標(biāo)點云PCA坐標(biāo)系如圖4所示，此時，PCA坐標(biāo)系相對于相機坐標(biāo)系的姿態(tài)描述可以寫為

圖4 PCA坐標(biāo)系方向向量Fig.4 PCA coordinate system direction vector

(6)

由此，便可得到以相機坐標(biāo)系到腳部目標(biāo)點云PCA坐標(biāo)系的變換矩陣。

3.2 參數(shù)修正

由于正骨手術(shù)中患者腳部受傷的特殊性，在機器人引導(dǎo)過程中不能對患者造成二次損傷，因此需要對引導(dǎo)目標(biāo)點的初始參數(shù)進行修正，在機器人引導(dǎo)過程中碰撞檢測，保證機器人引導(dǎo)過程的安全性和可靠性。

OBB包圍盒是Gottschalk在1996年首次使用，如圖5所示，紅色包圍盒為軸向包圍盒AABB(axis aligned bounding box)，綠色包圍盒為方向包圍盒OBB(orientation bounding box)。由于OBB方向上的任意性，其比AABB和包圍球更加緊密地逼近物體，具有更高的檢測精度。

圖5 基于腳部目標(biāo)點云的AABB和OBBFig.5 AABB and OBB based on foot target point clouds

利用OBB包圍盒可以對目標(biāo)點的Z軸坐標(biāo)進行修正，避免在機器人移動過程中，末端件與腳部目標(biāo)發(fā)生碰撞，對患者造成二次傷害。

考慮到針對足踝的正骨手術(shù)需要機器人將患者腳部沿足踝轉(zhuǎn)動中心扭動，需要對引導(dǎo)目標(biāo)點設(shè)置在足踝轉(zhuǎn)動中心的軸線上。由于人體腳底踵心與足踝轉(zhuǎn)動中心基本處于同一位置，故選擇踵心作為引導(dǎo)目標(biāo)點。結(jié)合基于中國在1965年和1968年進行的兩次大型腳形測量的成年男、女及兒童腳型規(guī)律(表1)[14]，可以得到腳部踵心相對于腳部的位置范圍。

表1 全國成年男、女及兒童腳型規(guī)律Table 1 National foot patterns for males,females and children

綜合上述目標(biāo)點坐標(biāo)修正后，機器人引導(dǎo)的目標(biāo)點位置如圖6所示。

圖6 參數(shù)修正后目標(biāo)點位置Fig.6 Target point position after parameter correction

4 位姿轉(zhuǎn)換

為了根據(jù)獲得的腳部目標(biāo)位姿結(jié)果完成對正骨機器人的引導(dǎo)工作，需要建立完成的坐標(biāo)關(guān)系轉(zhuǎn)換，將相機坐標(biāo)系下的腳部目標(biāo)模型位姿轉(zhuǎn)換為機器人基坐標(biāo)系的位姿描述。在進行機器人目標(biāo)位姿轉(zhuǎn)換前，系統(tǒng)需要對整個機器人系統(tǒng)進行手眼標(biāo)定，對像素坐標(biāo)系和機器人末端坐標(biāo)系之間的關(guān)系進行求解，進而完成手眼標(biāo)定。

根據(jù)相機相對于機器人安裝位置的不同，可以分為眼在手外(eye-to-hand)和眼在手上(eye-in-hand)，眼在手外即攝像頭安裝在手臂之外的部分，與機器人的基座(世界坐標(biāo)系)相對固定，不隨機械臂的運動而運動；而眼在手上是攝像頭安裝在機械臂上，會隨著機械臂的運動而發(fā)生運動。本文引導(dǎo)系統(tǒng)采用eye-in-hand的模式，深度相機固定安裝于機器人末端下部，其手眼標(biāo)定原理如圖7所示。標(biāo)定時，相機固定安裝在機器人末端，標(biāo)定板相對于機器人基座固定。

圖7中，b表示機器人基坐標(biāo)系，e表示機器人末端坐標(biāo)系，c表示相機坐標(biāo)系，k表示標(biāo)定板坐標(biāo)系，A表示機器人基坐標(biāo)系b到機器人末端坐標(biāo)系e的轉(zhuǎn)換關(guān)系，B表示機器人末端坐標(biāo)系e到相機坐標(biāo)系c的轉(zhuǎn)換關(guān)系，C表示相機坐標(biāo)系c到標(biāo)定板坐標(biāo)系k的轉(zhuǎn)換關(guān)系，D表示機器人基坐標(biāo)系b到標(biāo)定板坐標(biāo)系k的轉(zhuǎn)換關(guān)系。此時則有系統(tǒng)坐標(biāo)系間關(guān)系為

D=ABC

(7)

式(7)中：相機坐標(biāo)系c到標(biāo)定板坐標(biāo)系k的轉(zhuǎn)換關(guān)系C可通過相機標(biāo)定求得。機器人基坐標(biāo)系b到機器人末端坐標(biāo)系e的轉(zhuǎn)換關(guān)系A(chǔ)可通過獲取機器人末端位姿得到。機器人的末端位姿描述描述通常有兩種方式：①基于關(guān)節(jié)坐標(biāo)值的顯示，以各軸相對于上一級的軸的旋轉(zhuǎn)角來顯示當(dāng)前位姿；②基于直角坐標(biāo)系的顯示，使用機器人末端的工具坐標(biāo)系和設(shè)定于基座的直角坐標(biāo)系來顯示當(dāng)前位姿[15]。

本系統(tǒng)選擇第二種表示方式，其中機器人末端中心位置采用(X，Y，Z)描述，在UR5機器人系統(tǒng)中，機器人末端相對于機器人基坐標(biāo)系的姿態(tài)描述采用旋轉(zhuǎn)矢量法表示，因此需要將旋轉(zhuǎn)矢量轉(zhuǎn)為旋轉(zhuǎn)矩陣，等效軸角坐標(biāo)系如圖8所示。

圖8 等效軸角坐標(biāo)系Fig.8 Equivalent axis angle coordinate system

(8)

因此，機器人基坐標(biāo)系b到機器人末端坐標(biāo)系e的轉(zhuǎn)換矩陣可表示為

RK(θ)=

(9)

式(9)中：S表示正弦函數(shù);C表示余弦函數(shù);Kθ=1-cosθ。

而機器人末端坐標(biāo)系e到相機坐標(biāo)系c的轉(zhuǎn)換關(guān)系B就是手眼標(biāo)定需要解決的問題，這也決定了手眼標(biāo)定的本質(zhì)就是數(shù)學(xué)問題。

如圖9所示，理論上，令機械臂運動兩次，得到兩個不同的機器人位姿，并保證標(biāo)定板在相機視野范圍內(nèi)，此時根據(jù)式(10)可得

圖9 兩個不同位置的標(biāo)定系統(tǒng)示意圖Fig.9 Schematic diagram of a calibration system at two different locations

D=A1BC1=A2BC2

(10)

經(jīng)過變換，式(10)可寫為

(11)

于是，手眼標(biāo)定又轉(zhuǎn)化為“AX=XB”的問題。關(guān)于“AX=XB”問題的求解目前存在許多方法，此處采用Tsai兩步標(biāo)定法求解[16]，Tsai兩步標(biāo)定法需要一個已知的標(biāo)定塊作為空間參照物，利用空間參照物與圖像特殊點的對應(yīng)關(guān)系標(biāo)定相機。

標(biāo)定過程如圖10所示，基于機器人操作系統(tǒng) (robot operating system,ROS)下利用開源手眼標(biāo)定功能包easy_handeye完成手眼標(biāo)定工作。由于X矩陣求解需要經(jīng)過機器人兩次不同的相對位置變換得到，位置變換過程中旋轉(zhuǎn)軸不平行，因此容易出現(xiàn)偶然誤差[17]。為了消除這種偶然誤差，在實際標(biāo)定過程中對機器人的位姿進行了17組變換，采集得到了相機在17個不同位置的圖像用于手眼標(biāo)定。

圖10 機器人手眼標(biāo)定Fig.10 Robot hand-eye calibration

經(jīng)過手眼標(biāo)定后獲得的相機機器人末端坐標(biāo)系e到相機坐標(biāo)系c的轉(zhuǎn)換關(guān)系如表2所示。

表2 手眼標(biāo)定參數(shù)Table 2 Hand-eye calibration parameters

完成手眼標(biāo)定后，可得到相機坐標(biāo)系c到標(biāo)定板坐標(biāo)系k的轉(zhuǎn)換關(guān)系C，其可描述為

(12)

類似的，在進行機器人視覺導(dǎo)航時(圖11)，則具有對應(yīng)的坐標(biāo)轉(zhuǎn)換關(guān)系。

圖11 視覺導(dǎo)航中的坐標(biāo)轉(zhuǎn)換Fig.11 Coordinate transformations in visual navigation

綜合上述各坐標(biāo)變換關(guān)系，有

(13)

將坐標(biāo)系旋轉(zhuǎn)變換用旋轉(zhuǎn)質(zhì)量表示，則有

(14)

故

(15)

綜上可得，發(fā)送給機器人控制器的腳部模型引導(dǎo)位姿為(Xd,Yd,Zd,Rx,Ry,Rz)。

5 實驗驗證

利用獲取的基于相機坐標(biāo)系下的目標(biāo)點云數(shù)據(jù)位姿信息，經(jīng)過相機標(biāo)定和手眼標(biāo)定轉(zhuǎn)換為基于機器人基坐標(biāo)系下的目標(biāo)位姿信息并作為目標(biāo)點發(fā)送給機器人控制器，完成機器人的視覺引導(dǎo)工作。

實驗針對本文方法的有效性和可靠性分別進行驗證。針對方法有效性，在相機視野下，對不同姿態(tài)的腳部目標(biāo)進行視覺定位與引導(dǎo)，并選取手動引導(dǎo)的姿態(tài)參數(shù)作為期望定位結(jié)果進行比較。定位效果如表3所示。

表3 不同姿態(tài)下的目標(biāo)定位結(jié)果Table 3 Arget positioning results in different postures

多次實驗結(jié)果表明，針對不同姿態(tài)的腳部目標(biāo)，系統(tǒng)可有效完成視覺定位和引導(dǎo)，經(jīng)由視覺引導(dǎo)的機器人末端件與腳部模型貼合度較高，且未與腳部模型發(fā)生碰撞造成腳部模型發(fā)生偏移。視覺引導(dǎo)機器人末端件與腳部模型的貼合效果如圖12所示。

圖12 視覺引導(dǎo)貼合效果Fig.12 The fit effect of visual guidance

為了更加直觀地觀察定位效果，對定位結(jié)果人為增加間隙，增加間隙后的機器人末端姿態(tài)與腳部模型姿態(tài)的符合程度由圖13可以看出，視覺定位結(jié)果與期望定位結(jié)果存在一定誤差，但定位效果符合定位期望。

圖13 定位姿態(tài)示意圖Fig.13 Schematic diagram of positioning posture

針對定位結(jié)果可靠性，通過對同一位姿目標(biāo)進行20組重復(fù)定位，實驗結(jié)果如圖14所示。

從圖14中可看出，20組重復(fù)定位結(jié)果分布較為集中，同時，對20組重復(fù)定位結(jié)果進行結(jié)果統(tǒng)計，從表4中可以看出，定位結(jié)果受外部環(huán)境影響的隨機波動較小，證明了方法的可靠性和穩(wěn)定性。

圖14 20組重復(fù)定位結(jié)果Fig.14 The results of 20 sets of duplicate positioning

表4 重復(fù)定位結(jié)果Table 4 The experimental result of duplicate positioning

綜上對實驗結(jié)果的分析可得，通過本文方法對腳部目標(biāo)的定位結(jié)果符合人體主觀判斷，結(jié)果一致性較高，相比與傳統(tǒng)方法更具靈活性和魯棒性，定位結(jié)果受外部條件的影響較小，基于定位結(jié)果的機器人引導(dǎo)工作具有較高的成功率。

經(jīng)誤差分析，定位結(jié)果的誤差主要由于相機標(biāo)定和手眼標(biāo)定過程中標(biāo)定板的制作誤差和相機的安裝誤差導(dǎo)致，該誤差可通過進一步提高標(biāo)定版的制作精度和相機與機器人的安裝精度來彌補，同時可以從視覺引導(dǎo)的工作流程上進行進一步的優(yōu)化。

6 結(jié)論

基于正骨機器人在面對患者腿部受固定裝置約束狀態(tài)下，針對腳部目標(biāo)的術(shù)前定位和導(dǎo)航問題，提出一種基于Intel Realsense D455深度相機的腳部目標(biāo)點云位姿計算方法，并對方法有效性和可靠性進行驗證。結(jié)合實驗相關(guān)結(jié)果可得以下結(jié)論：

(1)針對不同姿態(tài)下的腳部目標(biāo)，視覺定位結(jié)果具有較高的準(zhǔn)確度和一致性，根據(jù)視覺定位結(jié)果引導(dǎo)的機器人末端件與目標(biāo)腳部模型貼合效果良好，視覺引導(dǎo)效果符合預(yù)期判斷。

(2)本文方法的提出，實現(xiàn)了正骨手術(shù)中患者腳部的術(shù)前自動定位引導(dǎo)，解決了正骨機器人的術(shù)前導(dǎo)航問題，提高了正骨機器人的智能化。

(3)本文方法的應(yīng)用滿足正骨手術(shù)中機器人目標(biāo)定位的實際需求，為結(jié)合中醫(yī)正骨機器人進一步實際應(yīng)用提供了理論與實踐基礎(chǔ)。