正骨機器人術前視覺導航方法與實驗驗證

曹家勇，吳世豪，馬千里

(上海應用技術大學機械工程學院，上海 201418)

實驗研究表明，在對患者進行手術治療之前配合正骨治療，糾正軟組織錯位，能夠對患者的術后恢復起到良好的促進作用，縮短患者住院時間[1]。但傳統中醫正骨復位主要依賴骨科醫生完成，給臨床醫生造成了巨大的工作負擔與安全隱患。

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

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

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

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

1 系統結構

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

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

圖1 機器人視覺引導系統Fig.1 Robot vision guidance system

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

圖2 機器人視覺引導系統流程圖Fig.2 Flowchart of robot visual guidance system

2 點云數據處理

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

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

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

2.1 點云條件濾波

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

(1)

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

濾波前后感興趣區域的點云數量、形狀、位置均未發生變化[9]。經過條件濾波處理后，點云數量得到了極大降低。

2.2 點云下采樣

為了提高后續計算速度，在保留腳部目標特征信息的基礎上，利用體素濾波算法[10]根據點云數據創建一個最小三維體素柵格，根據點云分布情況計算出小立方體柵格邊長L，根據L的長度將三維體素柵格劃分為m×n×l個小柵格，劃分完成后將點云數據放入到對應的小柵格。對于不包含點云的小柵格進行刪除，對于包含點云數據點的小柵格，將距離小柵格重心最近的點云數據點保留并刪除小柵格內其余點云數據點。該方法簡單高效，容易實現，不需要建立復雜的拓撲結構可實現對點云數量進行整體簡化，從而達到快速點云計算的目的[11]。每個小立方體柵格中，三維柵格重心的計算公式為

(2)

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

2.3 統計離群點去除

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

(3)

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

3 點云位姿計算

3.1 基于PCA的目標點云位姿估計

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

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

(4)

(5)

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

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

圖4 PCA坐標系方向向量Fig.4 PCA coordinate system direction vector

(6)

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

3.2 參數修正

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

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

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

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

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

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

綜合上述目標點坐標修正后，機器人引導的目標點位置如圖6所示。

圖6 參數修正后目標點位置Fig.6 Target point position after parameter correction

4 位姿轉換

為了根據獲得的腳部目標位姿結果完成對正骨機器人的引導工作，需要建立完成的坐標關系轉換，將相機坐標系下的腳部目標模型位姿轉換為機器人基坐標系的位姿描述。在進行機器人目標位姿轉換前，系統需要對整個機器人系統進行手眼標定，對像素坐標系和機器人末端坐標系之間的關系進行求解，進而完成手眼標定。

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

圖7中，b表示機器人基坐標系，e表示機器人末端坐標系，c表示相機坐標系，k表示標定板坐標系，A表示機器人基坐標系b到機器人末端坐標系e的轉換關系，B表示機器人末端坐標系e到相機坐標系c的轉換關系，C表示相機坐標系c到標定板坐標系k的轉換關系，D表示機器人基坐標系b到標定板坐標系k的轉換關系。此時則有系統坐標系間關系為

D=ABC

(7)

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

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

圖8 等效軸角坐標系Fig.8 Equivalent axis angle coordinate system

(8)

因此，機器人基坐標系b到機器人末端坐標系e的轉換矩陣可表示為

RK(θ)=

(9)

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

而機器人末端坐標系e到相機坐標系c的轉換關系B就是手眼標定需要解決的問題，這也決定了手眼標定的本質就是數學問題。

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

圖9 兩個不同位置的標定系統示意圖Fig.9 Schematic diagram of a calibration system at two different locations

D=A1BC1=A2BC2

(10)

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

(11)

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

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

圖10 機器人手眼標定Fig.10 Robot hand-eye calibration

經過手眼標定后獲得的相機機器人末端坐標系e到相機坐標系c的轉換關系如表2所示。

表2 手眼標定參數Table 2 Hand-eye calibration parameters

完成手眼標定后，可得到相機坐標系c到標定板坐標系k的轉換關系C，其可描述為

(12)

類似的，在進行機器人視覺導航時(圖11)，則具有對應的坐標轉換關系。

圖11 視覺導航中的坐標轉換Fig.11 Coordinate transformations in visual navigation

綜合上述各坐標變換關系，有

(13)

將坐標系旋轉變換用旋轉質量表示，則有

(14)

故

(15)

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

5 實驗驗證

利用獲取的基于相機坐標系下的目標點云數據位姿信息，經過相機標定和手眼標定轉換為基于機器人基坐標系下的目標位姿信息并作為目標點發送給機器人控制器，完成機器人的視覺引導工作。

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

表3 不同姿態下的目標定位結果Table 3 Arget positioning results in different postures

多次實驗結果表明，針對不同姿態的腳部目標，系統可有效完成視覺定位和引導，經由視覺引導的機器人末端件與腳部模型貼合度較高，且未與腳部模型發生碰撞造成腳部模型發生偏移。視覺引導機器人末端件與腳部模型的貼合效果如圖12所示。

圖12 視覺引導貼合效果Fig.12 The fit effect of visual guidance

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

圖13 定位姿態示意圖Fig.13 Schematic diagram of positioning posture

針對定位結果可靠性，通過對同一位姿目標進行20組重復定位，實驗結果如圖14所示。

從圖14中可看出，20組重復定位結果分布較為集中，同時，對20組重復定位結果進行結果統計，從表4中可以看出，定位結果受外部環境影響的隨機波動較小，證明了方法的可靠性和穩定性。

圖14 20組重復定位結果Fig.14 The results of 20 sets of duplicate positioning

表4 重復定位結果Table 4 The experimental result of duplicate positioning

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

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

6 結論

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

(1)針對不同姿態下的腳部目標，視覺定位結果具有較高的準確度和一致性，根據視覺定位結果引導的機器人末端件與目標腳部模型貼合效果良好，視覺引導效果符合預期判斷。

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

(3)本文方法的應用滿足正骨手術中機器人目標定位的實際需求，為結合中醫正骨機器人進一步實際應用提供了理論與實踐基礎。