張文露 王宏 雷超 王琳琳 沙洪

中國(guó)醫(yī)學(xué)科學(xué)院北京協(xié)和醫(yī)學(xué)院生物醫(yī)學(xué)工程研究所，天津300192

0 引 言

機(jī)器人技術(shù)不僅廣泛應(yīng)用于工業(yè)，而且逐步在醫(yī)療領(lǐng)域中嶄露頭角。目前，醫(yī)療機(jī)器人包括外科手術(shù)機(jī)器人、康復(fù)治療機(jī)器人、假體和殘障人士輔助機(jī)器人等[1]。其中，穿刺機(jī)器人是醫(yī)療外科機(jī)器人的分支之一，是用于微創(chuàng)外科手術(shù)的醫(yī)療機(jī)器人。機(jī)器人輔助穿刺手術(shù)系統(tǒng)主要由機(jī)械臂控制系統(tǒng)、成像系統(tǒng)、空間定位系統(tǒng)、工作站等組成[2]。機(jī)械臂在穿刺手術(shù)中的應(yīng)用可以減少對(duì)醫(yī)生經(jīng)驗(yàn)的依賴，提高手術(shù)的精準(zhǔn)度[3]。因?yàn)闄C(jī)械臂控制系統(tǒng)是穿刺機(jī)器人系統(tǒng)的重要組成部分，對(duì)其關(guān)鍵技術(shù)開展研究具有重要的理論與應(yīng)用意義。

奇異位置是機(jī)械臂重要的運(yùn)動(dòng)學(xué)特性點(diǎn)，當(dāng)處于奇異位置時(shí)，機(jī)械臂末端的運(yùn)動(dòng)會(huì)受到限制，導(dǎo)致機(jī)械臂運(yùn)動(dòng)失控或不能實(shí)現(xiàn)預(yù)定動(dòng)作。為了使機(jī)械臂可以更加高效、快速地完成指定操作任務(wù)，本研究中利用D-H（Denavit-Hartenberg）法對(duì)機(jī)械臂建模，進(jìn)行正、逆運(yùn)動(dòng)學(xué)分析和奇異位置分析，為后續(xù)的機(jī)械臂軌跡規(guī)劃和控制系統(tǒng)設(shè)計(jì)提供理論依據(jù)。此外，設(shè)計(jì)了簡(jiǎn)易的穿刺末端執(zhí)行器，編寫了安裝執(zhí)行器后穿刺針到達(dá)目標(biāo)靶點(diǎn)時(shí)機(jī)械臂末端位姿的計(jì)算程序及機(jī)械臂規(guī)避奇異點(diǎn)的程序，以實(shí)現(xiàn)對(duì)機(jī)械臂的自動(dòng)控制，為設(shè)計(jì)機(jī)械臂輔助穿刺手術(shù)系統(tǒng)奠定基礎(chǔ)。

1 儀器與方法

1.1 主要儀器

1.1.1 UR3機(jī)械臂

UR3是一種6自由度串聯(lián)式機(jī)械臂，包括腰部、肩部、肘部及3個(gè)自由度的腕關(guān)節(jié)，質(zhì)量為11 kg，最大負(fù)載為3 kg，工作半徑為500 mm。該型機(jī)械臂的靈活性高，其各個(gè)關(guān)節(jié)都能整周回轉(zhuǎn)，而且整機(jī)體積小，適合在穿刺手術(shù)場(chǎng)景中應(yīng)用。由于UR3機(jī)械臂的機(jī)構(gòu)特點(diǎn)滿足Pieper準(zhǔn)則[4]，其運(yùn)動(dòng)學(xué)逆解具有封閉解的優(yōu)勢(shì)，使運(yùn)動(dòng)控制更為容易。故本研究中，選擇UR3機(jī)械臂作為研究對(duì)象進(jìn)行建模。

1.1.2 末端穿刺裝置

利用Solid Works軟件設(shè)計(jì)了一個(gè)簡(jiǎn)易的機(jī)械臂末端穿刺套筒裝置，如圖1所示。該裝置可直接安裝到機(jī)械臂末端的法蘭盤上。安裝時(shí)，套筒方向與機(jī)械臂末端接口方向一致，通過控制機(jī)械臂到達(dá)穿刺位置，并且使套筒中心圓孔的方向與規(guī)劃路徑方向重疊，醫(yī)生只需將穿刺針沿著套筒穿入體內(nèi)即可。由于該套筒方向與機(jī)械臂末端的工具坐標(biāo)系（tool coordinate system,TC S）的方向一致，計(jì)算時(shí)不需要重新建立坐標(biāo)系，以工具坐標(biāo)系中心點(diǎn)（tool coordinate point，TCP）為坐標(biāo)原點(diǎn)，即可進(jìn)行各關(guān)節(jié)坐標(biāo)系的變換。

圖1 機(jī)械臂末端穿刺套筒

1.2 機(jī)械臂運(yùn)動(dòng)學(xué)分析

1.2.1 機(jī)械臂建模

D-H法是一種對(duì)機(jī)械臂連桿建模及關(guān)節(jié)建模的簡(jiǎn)單方法[5-6]。D-H法不需要考慮機(jī)器人的結(jié)構(gòu)順序和復(fù)雜程度，適用于任何機(jī)器人結(jié)構(gòu)。建立D-H連桿坐標(biāo)系時(shí)，首先要確定各個(gè)關(guān)節(jié)的z軸，一般將每個(gè)旋轉(zhuǎn)關(guān)節(jié)的軸線方向設(shè)為z軸，然后通過z軸確定x軸。x軸的確定分為三種情況，一是相鄰關(guān)節(jié)的軸線既不相交也不平行，此時(shí)存在一條距離最短的公垂線，將這條公垂線定義為x軸；二是相鄰關(guān)節(jié)的軸線平行，則選取與前一關(guān)節(jié)x軸平行的線作為此關(guān)節(jié)的x軸；三是相鄰關(guān)節(jié)軸線相交，則定義x軸使其垂直于相鄰關(guān)節(jié)軸線所在的平面。最后，在z軸和x軸都確定的條件下，通過右手定則確定y軸。通過D-H法建立UR3機(jī)械臂各關(guān)節(jié)之間的坐標(biāo)系，如圖2所示。坐標(biāo)系{0}固連于機(jī)械臂的基座，是一個(gè)固定不動(dòng)的坐標(biāo)系，在研究機(jī)械臂運(yùn)動(dòng)學(xué)問題時(shí)可以把該坐標(biāo)系作為參考坐標(biāo)系。為了減小機(jī)構(gòu)偏心對(duì)位姿矩陣精度的影響，將坐標(biāo)系{2}移動(dòng)至與坐標(biāo)系{1}重合[7]。

圖2 UR3機(jī)械臂連桿坐標(biāo)系

根據(jù)圖2所示的坐標(biāo)系，得到對(duì)應(yīng)的D-H連桿參數(shù)，見表1。其中，關(guān)節(jié)角θi為繞zi軸，從xi-1旋轉(zhuǎn)到xi的角度；連桿偏距di為沿zi軸，從xi-1移動(dòng)到xi的距離；連桿長(zhǎng)度ai為沿xi軸，從zi移動(dòng)到zi+1的距離；連桿轉(zhuǎn)角αi為繞xi軸，從zi旋轉(zhuǎn)到zi+1的角度。根據(jù)實(shí)際需求，UR3初始位置為θ1=-pi/2，θ2=-pi/2，θ3=-pi/2，θ4=0，θ5=pi/2，θ6=0。

表1 UR3機(jī)械臂D-H連桿參數(shù)

1.2.2 正運(yùn)動(dòng)學(xué)分析

機(jī)械臂正運(yùn)動(dòng)學(xué)分析是已知機(jī)械臂各個(gè)關(guān)節(jié)角度值，求解出對(duì)應(yīng)的機(jī)械臂末端位置和姿態(tài)[8]。由于機(jī)械臂由任意的連桿和關(guān)節(jié)以任意順序連接而成，首先為每個(gè)關(guān)節(jié)指定一個(gè)坐標(biāo)系，并求出相鄰兩關(guān)節(jié)的變化關(guān)系。對(duì)于UR3機(jī)械臂，先求出基座和第一個(gè)關(guān)節(jié)的變換關(guān)系，再求出第一個(gè)關(guān)節(jié)和第二個(gè)關(guān)節(jié)的變換關(guān)系，依次下去，直到求出最后一個(gè)關(guān)節(jié)的變化，把所有變換結(jié)合起來得到總變換關(guān)系，即機(jī)器人基座和末端執(zhí)行器的變換關(guān)系。通過矩陣運(yùn)算求導(dǎo)出坐標(biāo)系{i-1}到坐標(biāo)系{i}的變換矩陣：

化簡(jiǎn)整理得：

將D-H參數(shù)代入式（3）中，每個(gè)連桿的變換矩陣為：

式中：nx=s1s5c6+c1c5c6c234-c1s6s234；Ox=-s1s5s6-c1c5s6c234-c1c6s234；ax=s1c5-c1s5c234；px=d4s1+d5c1s234+d6s1c5-d6c1s5c234+a2c1c2+a3c1c23；ny=s1c5c6c234-c1s5c6-s1s6s234；Oy=-s1c6s234-s1c5s6c234+c1s5s6；ay=-c1c5-s1s5c234；py=-d4c1+d5s1s234-d6c1c5-d6s1s5c234+a2s1c2+a3s1c23；nz=s6c234+c5c6s234；Oz=c6c234-c5s6s234；az=-s5s234；pz=d1-d5c234+a2s2+a2s23-d6s5s234。

式（1）～式（5）中，px、py、pz為機(jī)械臂末端在三維笛卡爾空間坐標(biāo)下的位置，即為x、y、z。sθi表示sinθi，cθi表示cosθi（i=1，2，…，6）；sθ23表示sin（θ2+θ3），cθ23表示cos（θ2+θ3），sθ234表示sin（θ2+θ3+θ4），cθ234表示co s（θ2+θ3+θ4）。

1.2.3 逆運(yùn)動(dòng)學(xué)分析

逆運(yùn)動(dòng)學(xué)的求解過程是根據(jù)機(jī)械臂末端相對(duì)于基座的坐標(biāo)系（參考坐標(biāo)系）的位姿，求出關(guān)節(jié)角度θ1、θ2、θ3、θ4、θ5、θ6的過程[9-10]，是機(jī)械臂運(yùn)動(dòng)規(guī)劃和軌跡控制的基礎(chǔ)，也是運(yùn)動(dòng)學(xué)中最重要的部分[11]。目前，關(guān)于機(jī)械臂逆運(yùn)動(dòng)學(xué)求解的方法主要有解析法[12]、幾何法[13]、迭代法[14]等。本研究選擇解析法進(jìn)行求解[15]。

利用式（6）中左右兩側(cè)第三行第四列元素相等、第三行第三列元素相等、第三行第一列元素相等，聯(lián)立方程求出θ1、θ5、θ6。

同理，

利用式（7）第一行第四列元素相等、第二行第四列元素相等，聯(lián)立方程求出θ3和θ2；利用式（6）中第二行第二列元素、第一行第二列元素求解出θ2+θ3+θ4，從而求得θ4。由此，θ1～θ6均已求出。推導(dǎo)公式不一一列出。

UR3機(jī)械臂逆向運(yùn)動(dòng)學(xué)的結(jié)果一般會(huì)有多解，需要通過計(jì)算關(guān)節(jié)差的均方根（root mean square，RM S）來篩選最合適的解[16]。設(shè)當(dāng)前關(guān)節(jié)角度為qs=[θs1，θs2，θs3，θs4，θs5，θs6]，目標(biāo)關(guān)節(jié)角度為qe=[θe1，θe2，θe3,θe4，θe5，θe6]，則關(guān)節(jié)角度差的RMS為：

從所有的關(guān)節(jié)角度解中選取RMS值最小的，即為目標(biāo)關(guān)節(jié)角度最優(yōu)解q=[θ1，θ2，θ3，θ4，θ5，θ6]。

1.2.4 奇異位置分析

奇異位置是機(jī)械臂機(jī)構(gòu)學(xué)中重要的特征位置，當(dāng)機(jī)械臂運(yùn)動(dòng)到奇異位置附近時(shí)，雅克比矩陣趨于無窮大，從而需要無窮大的關(guān)節(jié)角速度和加速度，而這在現(xiàn)實(shí)世界中是不可能實(shí)現(xiàn)的。造成機(jī)械臂處于奇異位置的主要原因有兩個(gè)，一是由于機(jī)械臂超出自身工作范圍，機(jī)械臂末端無法到達(dá)；二是奇異位置與機(jī)械臂姿態(tài)有關(guān)，并不是一個(gè)固定的點(diǎn)，因此無法列出所有奇異點(diǎn)，但是可以將奇異點(diǎn)分為三類。

第一類奇異點(diǎn)為腕關(guān)節(jié)奇異位置，當(dāng)?shù)谒妮S和第六軸共線，造成系統(tǒng)將第四軸和第六軸瞬間旋轉(zhuǎn)180°；第二類為肩關(guān)節(jié)奇異位置，當(dāng)?shù)谝惠S與腕關(guān)節(jié)中心點(diǎn)（第五軸與第六軸交點(diǎn)）共線時(shí)，造成系統(tǒng)嘗試將第一軸與第四軸瞬間旋轉(zhuǎn)180°；第三類為肘關(guān)節(jié)奇異位置，當(dāng)腕關(guān)節(jié)中心點(diǎn)與第二軸、第三軸共面時(shí)，會(huì)造成肘關(guān)節(jié)卡住，無法移動(dòng)。

1.3 機(jī)械臂控制程序設(shè)計(jì)

為減少醫(yī)生規(guī)劃次數(shù)，提高效率。本研究中，設(shè)計(jì)了一種通過沿固定軸小角度旋轉(zhuǎn)提供多組位姿的方法，以規(guī)避奇異點(diǎn)。首先，根據(jù)規(guī)劃后的穿刺點(diǎn)在機(jī)械臂基座標(biāo)系下的末端位姿，計(jì)算出安裝套筒后機(jī)械臂的末端位姿。其次，根據(jù)該位姿套筒中心的方向矢量進(jìn)行小角度旋轉(zhuǎn)，得到多組位姿。然后，通過UR3機(jī)械臂腳本編程，進(jìn)行運(yùn)動(dòng)規(guī)劃，排除可能由奇異點(diǎn)導(dǎo)致運(yùn)動(dòng)規(guī)劃失敗的位姿。最后，提取運(yùn)動(dòng)規(guī)劃成功的位姿對(duì)應(yīng)的運(yùn)動(dòng)規(guī)劃關(guān)節(jié)值，并進(jìn)行最優(yōu)RMS值判定，選擇出有最佳響應(yīng)的最佳位姿。機(jī)械臂控制程序的界面如圖3所示；套筒旋轉(zhuǎn)示意圖如圖4所示；程序流程圖如5所示。

圖3 機(jī)械臂控制程序界面

圖4 套筒旋轉(zhuǎn)示意圖

圖5 程序流程圖

2 結(jié) 果

2.1 運(yùn)動(dòng)學(xué)仿真驗(yàn)證

在MATLAB軟件環(huán)境下，使用Robotics Toolbox 10.2軟件對(duì)UR3機(jī)械臂進(jìn)行仿真模型的創(chuàng)建，生成的機(jī)械臂模型如圖6所示，其與實(shí)際機(jī)械臂初始位置一致。

使用基于正運(yùn)動(dòng)學(xué)推導(dǎo)過程編寫的程序，隨機(jī)選出6組關(guān)節(jié)角進(jìn)行驗(yàn)證。首先，將關(guān)節(jié)角代入程序中得到x、y、z坐標(biāo)的計(jì)算值；然后，將6組關(guān)節(jié)角輸入到UR3機(jī)械臂的控制程序中得到x、y、z坐標(biāo)的實(shí)際值；最后，將計(jì)算值與實(shí)際值進(jìn)行比較。表2中的x、y、z坐標(biāo)值為TCP相對(duì)于機(jī)械臂基座標(biāo)系的實(shí)際位置。

表2 計(jì)算值與實(shí)際值對(duì)比表

6組關(guān)節(jié)角度分別為：

如表2所示，通過代入6組關(guān)節(jié)角度得到的x坐標(biāo)的計(jì)算值與實(shí)際值的平均誤差為0.68°，y坐標(biāo)的平均誤差為0.63°，z坐標(biāo)的平均誤差為0.43°。誤差可能來源于連桿的長(zhǎng)度、機(jī)械臂自身的機(jī)械誤差等，故可以證明所建立的運(yùn)動(dòng)學(xué)模型的正確性。

2.2 運(yùn)動(dòng)控制程序驗(yàn)證

采用3D打印的模擬穿刺套筒進(jìn)行實(shí)驗(yàn)驗(yàn)證。安裝時(shí)，套筒方向與機(jī)械臂末端接口的方向調(diào)為一致。驗(yàn)證時(shí)，選擇的穿刺針為9號(hào)腰椎穿刺針?？紤]到實(shí)際應(yīng)用中，初始位姿的規(guī)劃是根據(jù)醫(yī)學(xué)影像處理軟件（如3D Slice等）中建立的穿刺方向，并考慮利于醫(yī)師操作的基礎(chǔ)上進(jìn)行的，其結(jié)果是模擬姿態(tài)的組合。本驗(yàn)證實(shí)驗(yàn)僅驗(yàn)證初始規(guī)劃位姿出現(xiàn)運(yùn)動(dòng)失敗等情況下，規(guī)避程序的效果。在驗(yàn)證實(shí)驗(yàn)中，隨機(jī)模擬3組初始規(guī)劃穿刺靶點(diǎn)坐標(biāo)及姿態(tài)（不考慮初始位姿是否為奇異點(diǎn)），此時(shí)得到的3組靶點(diǎn)位姿是基于機(jī)械臂基座標(biāo)下TCP的位姿。將其輸入程序中，可算出機(jī)械臂安裝了套筒和穿刺針后，穿刺針針尖到達(dá)靶點(diǎn)時(shí)TCP的位姿。由于套筒坐標(biāo)系方向和TCP方向一致，因此計(jì)算前后TCP位姿的Rx，Ry，Rz相同（Rx，Ry，Rz為機(jī)械臂末端姿態(tài)參數(shù)）。模型沿計(jì)算后的方向順時(shí)針、逆時(shí)針各移動(dòng)9次，每次移動(dòng)5°，得到18組解，并通過機(jī)械臂腳本編程進(jìn)行運(yùn)動(dòng)規(guī)劃排除奇異點(diǎn)，剩余的解顯示在界面位姿集合對(duì)話框中，并根據(jù)RMS值選出最佳位姿。

相關(guān)參數(shù)和結(jié)果見表3。其中，位置1為初始規(guī)劃的TCP位姿對(duì)應(yīng)的位置（x1，y1，z1），位置2為計(jì)算后，穿刺針到達(dá)靶點(diǎn)時(shí)TCP的位姿對(duì)應(yīng)的位置（x2，y2，z2）（位置1與位置2需在機(jī)械臂工作范圍內(nèi)）。奇異點(diǎn)個(gè)數(shù)為每組位姿在進(jìn)行18次旋轉(zhuǎn)時(shí)對(duì)應(yīng)位姿為奇異點(diǎn)的個(gè)數(shù)，最優(yōu)RMS值為最佳位姿對(duì)應(yīng)的RMS值。

表3 相關(guān)參數(shù)和結(jié)果

將得到的最佳位姿傳輸?shù)綑C(jī)械臂的控制系統(tǒng)中，以控制機(jī)械臂移動(dòng)，機(jī)械臂移動(dòng)完成后將穿刺針沿套筒方向穿入，針尖可到達(dá)預(yù)定目標(biāo)點(diǎn)，如圖7所示。

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

3 討論與結(jié)論

本研究中，利用D-H法對(duì)機(jī)械臂進(jìn)行運(yùn)動(dòng)學(xué)分析，并在MATLAB軟件中根據(jù)推導(dǎo)的運(yùn)動(dòng)學(xué)公式編寫程序進(jìn)行仿真驗(yàn)證，為后續(xù)的機(jī)械臂軌跡規(guī)劃和控制系統(tǒng)設(shè)計(jì)提供了理論依據(jù)。設(shè)計(jì)了簡(jiǎn)易的穿刺末端執(zhí)行器，編寫了穿刺針到達(dá)目標(biāo)靶點(diǎn)時(shí)，機(jī)械臂末端位姿的計(jì)算程序。根據(jù)計(jì)算后的位姿方向，以機(jī)械臂沿其順時(shí)針、逆時(shí)針方向各移動(dòng)9次，每次移動(dòng)5°的方式規(guī)避奇異點(diǎn)，實(shí)現(xiàn)了對(duì)機(jī)械臂的自動(dòng)控制，為機(jī)械臂輔助穿刺手術(shù)系統(tǒng)的設(shè)計(jì)奠定了基礎(chǔ)。

在實(shí)際操作過程中，也發(fā)現(xiàn)了一些問題。首先，計(jì)算得到的任何位姿都應(yīng)該在機(jī)械臂的工作范圍內(nèi)，超出該范圍的位姿無論機(jī)械臂怎么旋轉(zhuǎn)，奇異點(diǎn)都無法規(guī)避。因此，對(duì)于機(jī)械臂的工作空間分析尤為必要。其次，由于對(duì)同一目標(biāo)點(diǎn)，程序可能會(huì)得到多個(gè)位姿解，根據(jù)臨床醫(yī)生的反饋，并不是每種位姿都適合進(jìn)行穿刺手術(shù)。雖然，本研究中基于RMS值選取了最優(yōu)位姿，但不能保證其完全滿足醫(yī)生需求，因此在初始位姿的規(guī)劃上應(yīng)增加約束條件?？梢姡瑢?duì)于適用于穿刺手術(shù)的最優(yōu)位姿的選取還有待進(jìn)一步研究。

利益沖突所有作者均聲明不存在利益沖突