手術(shù)機器人的運動學(xué)標定方法與實驗

于凌濤， 于曉硯， 湯澤旭， 王嵐

(哈爾濱工程大學(xué) 機電工程學(xué)院，黑龍江 哈爾濱 150001)

機器人輔助微創(chuàng)外科手術(shù)是通過手術(shù)機器人輔助操作，讓醫(yī)生經(jīng)由患者體表較小切口實施手術(shù)的一種先進技術(shù)。與傳統(tǒng)外科手術(shù)相比，具有定位準確、創(chuàng)傷小、恢復(fù)時間短等優(yōu)點，但是也對手術(shù)機器人提出了更高的要求，特別是對機器人的末端定位精度提出了更高的要求[1]。目前，手術(shù)機器人多為串聯(lián)機械臂結(jié)構(gòu)，其末端的位置和姿態(tài)無法直接測量，常用機器人運動學(xué)間接獲得。但是在機器人的加工制造、生產(chǎn)裝配過程中，運動學(xué)實際參數(shù)與理論參數(shù)存在差異，從而導(dǎo)致末端定位精度低的問題。所以，為了提高手術(shù)機器人的末端定位精度，必須對機器人進行運動學(xué)參數(shù)標定。

機器人運動學(xué)參數(shù)標定分為4個步驟，即運動學(xué)建模、位置姿態(tài)測量、參數(shù)辨識以及誤差補償。學(xué)者針對位姿測量這一步驟開展了很多研究工作。比較常用的方法是基于運動學(xué)模型并利用高精度位姿測量設(shè)備，如激光跟蹤儀[2-3]、三坐標測量機[4-5]、伸縮式球桿儀[6]、Dynacal系統(tǒng)[7]等，得到末端位姿信息用于參數(shù)標定。這類方法定位精度高，參數(shù)辨識準確，但通常價格十分昂貴，且對工作環(huán)境有嚴格要求。除此之外，基于視覺傳感器以及結(jié)合其他傳感器[8-10]的運動學(xué)參數(shù)標定也有廣泛的應(yīng)用。這類方法相比于高精密儀器，應(yīng)用靈活、成本較低，但是相機參數(shù)誤差會導(dǎo)致定位精度較低的問題。

為了實現(xiàn)手術(shù)機器人末端位置較高精度的測量，本文設(shè)計一種基于拉線傳感器的三維位置檢測裝置，使用空間解析幾何法建立數(shù)學(xué)模型；搭建標定實驗平臺，辨識結(jié)構(gòu)參數(shù)誤差并修正數(shù)學(xué)模型；建立手術(shù)機器人的絕對誤差模型和距離誤差模型，辨識運動學(xué)參數(shù)誤差并進行實驗驗證。

1 位置檢測裝置數(shù)學(xué)模型

1.1 結(jié)構(gòu)及硬件設(shè)計

位置檢測裝置由傳感器安裝組件、過線座組件、零位組件及安裝底板4個部分組成，如圖1所示。上過線座為韌性樹脂，其余加工零件均為2A12鋁合金。為保證測量過程中線繩與倒角圓弧的切點不變，上過線座設(shè)計R10的內(nèi)凸圓弧倒角，采用立體光固化成型工藝，加工精度0.1 mm。

圖1 位置檢測裝置實驗樣機Fig.1 Position detection device experimental prototype

硬件部分由工控機、數(shù)據(jù)采集卡、拉線傳感器等組成，其中數(shù)據(jù)采集卡為研華PCI-1784U，4路獨立32位計數(shù)器，最大輸入頻率8 MHz；拉線傳感器為開思KS15MD-400-01-L，量程400 mm，線性誤差0.05%，分辨率0.01 mm/脈沖。

1.2 數(shù)學(xué)模型

裝置數(shù)學(xué)模型即建立拉線傳感器伸長量與檢測點位置坐標之間的關(guān)系。選定傳感器出線點(A′,B′,C′,D′)、線繩與過線座下切點(A,B,C,D)以及零位點(O)所在平面為基平面，在O點建立基礎(chǔ)坐標系X0Y0Z0，如圖2所示。

圖2 位置檢測裝置基礎(chǔ)坐標系Fig.2 Position detection device coordinate system

以第1路拉線傳感器為對象，建立數(shù)學(xué)模型。設(shè)第1路拉線傳感器的伸長量L1，線繩與過線座圓弧倒角上切點E(xe,ye,ze)，4路線繩延長線匯交點T(xt,yt,zt)，上過線座圓弧倒角圓心Oa，上過線座圓弧倒角半徑為r，面TAA′與水平面的夾角θ。根據(jù)幾何關(guān)系可得第1路拉線傳感器數(shù)學(xué)模型為：

(1)

根據(jù)上述方法，建立其他3路拉線傳感器數(shù)學(xué)模型為：

(2)

(3)

(4)

2 位置檢測裝置誤差分析

2.1 裝置精度測量實驗

位置檢測裝置精度測量實驗分為零位實驗和相對誤差實驗。實驗樣機由標定板、圓柱插頭和固定支架組成，如圖3所示。標定板有25個直徑3 mm通孔，各孔水平豎直方向間距為30 mm，孔距加工精度為±0.02 mm；圓柱插頭通過螺紋與測量頭相連接，頂部加工有直徑3 mm圓柱。零位實驗實驗結(jié)果如表1所示。

圖3 精度測量實驗樣機Fig.3 Precision test experimental prototype

表1 零位實驗結(jié)果Table 1 Return-to-zero experimental results

相對誤差實驗分為2組，一組是相同位置點，另一組是不同位置點，測量距離分別30、60、90和120 mm，實驗結(jié)果如圖4、圖5所示。

圖4 相同位置點實驗數(shù)據(jù)Fig.4 Relative error of the same point precision test

圖5 不同位置點實驗數(shù)據(jù)Fig.5 Relative error of the different points precision test

2.2 實驗結(jié)果分析

零位實驗，X、Y、Z誤差平均值分別為0.097、0.136、0.064 mm，方差分別為0.037 96、0.079 27、0.044 32，則裝置的零位重復(fù)性可以滿足使用要求。

相對誤差實驗，對實驗數(shù)據(jù)均值計算以及相對誤差計算，如表2所示。相同點平均相對誤差為1.404 2%，不同點平均相對誤差為1.322 4%，綜合測量距離平均相對誤差為1.363 3%。通過分析可知，裝置的精度距離使用要求仍有一定的差距，具體原因如下：由于裝置加工、裝配誤差，導(dǎo)致實際結(jié)構(gòu)參數(shù)與理論結(jié)構(gòu)參數(shù)存在偏差，從而造成裝置數(shù)學(xué)模型的不準確。因此，采用自標定對裝置測量精度做進一步的提高。

表2 精度測量實驗數(shù)據(jù)Table 2 Accuracy test experimental data

3 位置檢測裝置自標定

3.1 自標定誤差模型

根據(jù)位置檢測裝置數(shù)學(xué)模型，線繩與過線座上切點(E,F,G,H)，線繩與過線座下切點(A,B,C,D)以及零位點(O)位置坐標對測量點(T)的準確性產(chǎn)生影響，因此選擇上述坐標點作為自標定誤差模型參數(shù)。標定誤差模型即在各個所要標定參數(shù)后加該變量的誤差Δ，用二者和來表示變量的真實值。以式(1)中第3式為例，改寫標定誤差模型。其他3式建立方法相同，另外3路方程組也按照此方法建立誤差方程，聯(lián)立始末點誤差方程以及距離誤差方程，得到裝置自標定誤差模型：

(zt+Δzt)(ye+Δye)-(yt+Δyt)(ze+Δze)=0

3.2 自標定誤差參數(shù)辨識

利用精度測量實驗樣機完成40組不同距離實驗，排除其中相對誤差最高和最低的數(shù)據(jù)，隨機選擇10組數(shù)據(jù)用于求解自標定誤差模型，20組數(shù)據(jù)用于檢驗自標定結(jié)果。利用最小二乘法求解自標定誤差模型，獲得參數(shù)誤差，得到補償后參數(shù)如表3所示。

表3 結(jié)構(gòu)參數(shù)誤差及補償Table 3 Structural parameters error and compensation

根據(jù)補償后參數(shù)，得到修正數(shù)學(xué)模型，將20組數(shù)據(jù)代入修正模型中，得到修正模型平均相對誤差0.201 1%，原模型平均相對誤差1.348 9%，自標定后檢測裝置的精度大幅提高。

3.3 自標定后精度測量實驗

使用修正數(shù)學(xué)模型對裝置進行精度測量，30、60、90、120 mm距離各10次，相對誤差曲線如圖6所示。自標定后裝置平均相對誤差為0.530 7%，與原模型的平均相對誤差1.363 3%相比，裝置測量精度提升61.07%，滿足使用要求。

圖6 自標定后精度測量相對誤差Fig.6 Relative error of precision test after self-calibration

4 手術(shù)機器人運動學(xué)參數(shù)標定

4.1 運動學(xué)模型

本文以實驗室現(xiàn)有的微創(chuàng)腹腔手術(shù)機器人為研究對象，在其運動學(xué)基礎(chǔ)上建立絕對誤差模型和距離誤差模型，以位置檢測裝置作為標定設(shè)備，進行機器人運動學(xué)標定實驗。

手術(shù)機器人由被動關(guān)節(jié)和主動關(guān)節(jié)2個分組成，被動關(guān)節(jié)主要完成術(shù)前擺位，對精度要求不高，且運動范圍超出位置檢測裝置測量范圍，因此只標定直接影響手術(shù)精度的主動關(guān)節(jié)部分。運動學(xué)坐標系如圖7所示，坐標系B為基礎(chǔ)坐標系，坐標系0、1、2、3為4個被動關(guān)節(jié)坐標系，坐標系4、5、6為3個主動關(guān)節(jié)坐標系，坐標系7為機器人末端工具坐標系。

圖7 手術(shù)機器人運動學(xué)坐標系Fig.7 Surgical robot kinematics coordinate system

已知坐標系i對坐標系i-1相對變換矩陣i-1Ti，

運動學(xué)參數(shù)如表4所示，機器人末端工具坐標系o7相對于坐標系o4的正運動學(xué)模型:

表4 主動關(guān)節(jié)運動學(xué)參數(shù)Table 4 Kinematics parameters of active joints

(5)

本文根據(jù)機器人運動學(xué)模型建立絕對誤差模型和距離誤差模型，與機器人末端姿態(tài)無關(guān)，因此只使用式中的位置坐標px、py和pz。

4.2 絕對誤差模型

絕對誤差模型利用測量值與運動學(xué)模型理論值間的差值辨識機器人運動學(xué)參數(shù)誤差的模型。由于手術(shù)機器人主動關(guān)節(jié)有光柵、磁柵傳感器，可以準確獲得關(guān)節(jié)運動量，所以不對3個主動關(guān)節(jié)的運動量進行標定，需要標定的運動學(xué)參數(shù)為α5、α6、α7、a5、a6、a7、d5、d6和θ7。

設(shè)機器人在基礎(chǔ)坐標系的測量位置為M(xm,ym,zm)，理論位置為T(xt,yt,zt)，則得到絕對誤差模型，其中Δq表示需標定的參數(shù)誤差矩陣，Δp表示位置坐標誤差矩陣，J表示誤差雅克比矩陣：

Δp=J·Δq

0.868 90]T，建立標定辨識的運動學(xué)模型，取10組θ5、θ6、d7代入該模型求解位置坐標pc，同時代入含假設(shè)誤差的運動學(xué)模型得到位置坐標pa，結(jié)果如表5所示。

根據(jù)表5，x方向平均誤差0.078 mm，最大值0.111 mm，最小值0.022 mm；y方向平均誤差0.092 mm，最大值0.196 mm，最小值0.028 mm；z方向平均誤差0.049 mm，最大值0.052 mm，最小值0.048 mm，驗證得到絕對誤差模型的正確性。

表5 含假設(shè)誤差和標定辨識位置坐標Table 5 Hypothetical error and calibration identification position coordinate

4.3 距離誤差模型

距離誤差模型利用運動距離測量值與理論值間差值辨識機器人運動學(xué)參數(shù)誤差。設(shè)機器人末端測量運動距離為lm(i,i+1)，理論運動距離為lt(i,i+1)，機器人末端運動距離誤差Δl(i,i+1)。結(jié)合絕對誤差模型，得到手術(shù)機器人的距離誤差模型[11]：

Δl(i,i+1)=[bx,by,bz]·(Ji+1-Ji)·Δq

(6)

式中：

假設(shè)運動學(xué)參數(shù)誤差Δq′，建立含假設(shè)誤差的運動學(xué)模型。根據(jù)距離誤差模型，辨識得到運動學(xué)參數(shù)誤差Δqβ，Δqβ=[-0.0320.03902.583 22.103 5201.8550]T，建立標定辨識的運動學(xué)模型，取10組q5、q6、d7代入該模型求解運動距離Lc，同時代入含假設(shè)誤差的運動學(xué)模型得到運動距離La，二者誤差為ΔL，結(jié)果如表6所示。

根據(jù)表6，標定運動距離平均誤差0.083 3 mm，最大值0.227 6 mm，最小值0.018 3 mm，驗證得到距離誤差模型的正確性。

表6 含假設(shè)誤差和標定辨識運動距離Table 6 Hypothetical error and calibration identification moving distance

4.4 手術(shù)機器人標定實驗

進行標定實驗前，預(yù)估機器人末端在實驗設(shè)定關(guān)節(jié)變量情況下的大致工作空間，然后布置位置檢測裝置，使后者測量空間完全包含于前者工作空間，實驗平臺如圖8所示。

圖8 手術(shù)機器人標定實驗平臺Fig.8 Surgical robot calibration experiment platform

選擇距離誤差模型作為機器人標定的辨識模型，將實驗獲得20組運動距離代入距離誤差模型，使用最小二乘法辨識出各運動學(xué)參數(shù)誤差Δq，再將辨識的參數(shù)誤差補償?shù)皆\動學(xué)模型上，得到實際運動學(xué)參數(shù)，如表7所示。

表7 機器人運動學(xué)參數(shù)辨識結(jié)果Table 7 Robot kinematics parameter identification data

使用位置檢測裝置對手術(shù)機器人運動距離進行測量，并分別使用原模型和修正模型，計算運動距離相對誤差，如圖9所示。

由圖9可知，手術(shù)機器人標定后，運動距離平均相對誤差由5.157 9%降低至1.419 6%，精度提高了72%，表明位置檢測裝置可應(yīng)用于機器人運動學(xué)標定，并為裝置應(yīng)用其他領(lǐng)域提供保證和參考。但是標定后的模型仍有一定誤差，分析原因為各關(guān)節(jié)回程誤差和齒輪間隙使機器人存在運動不確定性。

圖9 機器人標定前后運動距離相對誤差Fig.9 Relative error of distance before and after calibration

5 結(jié)論

1)本文設(shè)計一種基于拉線傳感器的三維位置檢測裝置，并提出一種手術(shù)機器人運動學(xué)標定方法。該方法的優(yōu)勢在于：僅需自制位置檢測裝置和機器人的角度/位移傳感器即可實現(xiàn)運動學(xué)標定，無需其他高精度標定設(shè)備，成本低且易實現(xiàn)。

2)建立的自標定誤差模型對裝置的數(shù)學(xué)模型進行修正，使得裝置測量精度提升61.07%，滿足手術(shù)機器人標定實驗的使用要求。

3)使用3自由度微創(chuàng)腹腔手術(shù)機器人進行標定實驗，標定后的平均相對誤差下降了72%，實驗結(jié)果表明本文提出的標定方法與檢測裝置可以提高手術(shù)機器人的定位精度，具有明顯的實用價值。

在后續(xù)研究中，將使用激光跟蹤儀等高精度設(shè)備對檢測裝置的精度做進一步的提高。