李恒，陳鑄，羅陽，伍劍波，方輝

(四川大學(xué)機械工程學(xué)院，四川成都 610041)

0 前言

隨著機器人技術(shù)的發(fā)展，機器人被越來越多地應(yīng)用到醫(yī)療行業(yè)。其中手術(shù)機器人是未來醫(yī)療機器人中的主要門類[1]。而手術(shù)導(dǎo)航系統(tǒng)的手眼標(biāo)定問題是手術(shù)機器人研究中十分重要的課題，手眼標(biāo)定的精度將直接影響手術(shù)機器人工作的精度。根據(jù)調(diào)研可知：進行口腔手術(shù)的機器人導(dǎo)航系統(tǒng)位置定位精度至少在0.5 mm以下，姿態(tài)定位精度至少在1°(即17 mrad)以下。

手眼標(biāo)定問題目前可分為兩類：一類是Eye-to-Hand(固定視點)，一類是Eye-in-Hand(非固定視點)。前者的傳感器與機器人分離，兩者分別固定，工作時傳感器保持靜止；后者的傳感器固定于機器人末端，工作時傳感器隨機器人末端移動[2-3]。相較于Eye-in-Hand系統(tǒng)，Eye-to-Hand系統(tǒng)的傳感器始終靜止，更容易實現(xiàn)高精度、高穩(wěn)定的導(dǎo)航，因此目前大多數(shù)口腔手術(shù)機器人均采用Eye-to-Hand系統(tǒng)。

手眼標(biāo)定問題的核心為求解傳感器坐標(biāo)系和機器人坐標(biāo)系之間的旋轉(zhuǎn)矩陣和平移向量。其相關(guān)算法國內(nèi)外均有大量的研究與成果，最早由SHIU和AHMAD[4]將該問題轉(zhuǎn)化為解齊次方程AX=XB的問題。圍繞求解AX=XB， TSAI和LENZ[5]提出了兩步法，先計算出手眼標(biāo)定中的旋轉(zhuǎn)矩陣，再利用旋轉(zhuǎn)矩陣求解平移向量。文獻(xiàn)[6-7]通過李群代數(shù)、對偶四元數(shù)相關(guān)數(shù)學(xué)工具，同時求解了旋轉(zhuǎn)矩陣和平移向量。前面所述算法都需要傳感器能夠同時精確測量標(biāo)志物的位置和姿態(tài)信息。

由雙目視覺姿態(tài)測量原理可知：其姿態(tài)定位精度低于位置定位精度，且其姿態(tài)精度難以控制(與標(biāo)志物形狀有關(guān))。當(dāng)雙目視覺相機測量的姿態(tài)數(shù)據(jù)誤差較大時，仍使用姿態(tài)數(shù)據(jù)進行手眼標(biāo)定，必然帶來較大的標(biāo)定誤差。在這種情況下，文獻(xiàn)[8]采用最近點迭代算法，但迭代時給出的初始值可能導(dǎo)致迭代進入局部最優(yōu)解，且迭代過程較長。文獻(xiàn)[9]通過剛體變化算法分別計算旋轉(zhuǎn)矩陣和平移向量，但需要人工操作視覺定位球的球心與機器人TCP重合，難以保證精度。文獻(xiàn)[3]通過簡單的3次純平移與1次純旋轉(zhuǎn)過程，計算旋轉(zhuǎn)矩陣和平移向量，但采集和用于計算的數(shù)據(jù)較少，該方法的精度有限。

本文作者闡述一種無需測量視覺標(biāo)志物姿態(tài)數(shù)據(jù)的口腔手術(shù)機器人手眼標(biāo)定算法，通過多次采集純平移與純旋轉(zhuǎn)過程中的數(shù)據(jù)，借助非奇異值分解的數(shù)學(xué)工具計算傳感器坐標(biāo)系與機器人基座坐標(biāo)系之間的旋轉(zhuǎn)矩陣與平移向量。

1 口腔手術(shù)機器人手眼標(biāo)定數(shù)學(xué)模型

在機器人的末端夾持手術(shù)所使用的工具并定義該工具的工作點為機器人的TCP，并將視覺識別標(biāo)志物與工具相對固定，雙目視覺在機器人的外部固定，如圖1所示。

圖1 手術(shù)機器人系統(tǒng)示意Fig.1 Surgical robot system

(1)

將式(1)分解后可得到如下齊次方程組：

(2)

2 基于SVD分解的剛體變換算法

文獻(xiàn)[10]提出了基于SVD分解的剛體變化算法計算兩組對應(yīng)點集之間的旋轉(zhuǎn)矩陣和平移向量。

設(shè)有兩組d維點集P={p1,p2，…，pn}和Q={q1，q2，…，qn}，其中p1和q1，p2和q2，…,pn和qn兩兩相互對應(yīng)。通過最小二乘法可得下列函數(shù)：

(3)

式中：R為點集P和點集Q對應(yīng)的旋轉(zhuǎn)矩陣；t為點集P和點集Q對應(yīng)的平移向量；ωi為兩點集每個對應(yīng)點的權(quán)重，常設(shè)為1，ωi>0。

第一步，分別計算兩組點集的重心。

(4)

第二步，將點集P、Q中每個元素減去重心，得到兩個新的點集X、Y。

(5)

第三步，將X、Y中的點集組成d×n的矩陣，分別為X、Y,將每對點集的權(quán)重組成對角矩陣W。

(6)

(7)

第四步，計算兩組新點集與權(quán)重矩陣的協(xié)方差矩陣。

S=XWYT

(8)

第五步，對S進行SVD分解。

S=UΣVT

(9)

第六步，計算旋轉(zhuǎn)矩陣R和平移向量t。

(10)

(11)

采用了上述方法進行手眼標(biāo)定時，由于雙目視覺獲取的是視覺標(biāo)志物的點集，機器人獲取的是TCP的點集，而視覺識別標(biāo)志物點集與TCP點集并不對應(yīng)，文獻(xiàn)[9]人工將視覺標(biāo)志球球心與機器人的TCP重合，但該方法操作難度大，因此最終標(biāo)定結(jié)果并不理想。

3 基于SVD分解的手眼標(biāo)定算法

觀察式(5)，將原始點集的每一個元素減去點集的重心后構(gòu)成新的點集，兩組新點集坐標(biāo)系之間的坐標(biāo)系原點重合。

而對于口腔手術(shù)機器人，視覺識別標(biāo)志物與手術(shù)工具工作點TCP之間的旋轉(zhuǎn)矩陣和平移向量隨結(jié)構(gòu)的固定而確定。在對應(yīng)點采集過程中如果保持手術(shù)工具只進行純平移運動，視覺識別標(biāo)志物的空間點坐標(biāo)與TCP空間點坐標(biāo)之間始終相差一個固定的平移向量t，設(shè)為

t=[t1,t2,t3]

(12)

則在平移過程中采集的TCP在機器人基座坐標(biāo)系下的坐標(biāo)點集為N={n1，n2，…，nn}，則根據(jù)式(12)可得：

(13)

(14)

(15)

(16)

(17)

且：

(18)

綜合式(17)和式(18)可得：

(19)

首先控制機器人以TCP為圓心進行旋轉(zhuǎn)，旋轉(zhuǎn)過程中采集一系列視覺識別標(biāo)志物在雙目視覺坐標(biāo)系下的坐標(biāo)點集H={h1，h2，…，hn}。設(shè)此時TCP在雙目視覺坐標(biāo)系下的坐標(biāo)為

g=[gx,gy,gz]

(20)

則有：

(hix-gx)2+(hiy-gy)2+(hiz-gz)2=d2

(21)

式中：hix為第i個采集點的x值；hiy為第i個采集點的y值；hiz為第i個采集點的z值；d為采集點到圓心的距離。

共有n個方程，將i≥2的方程減去i=1的方程，可得：

(22)

共有n-1個上述方程，轉(zhuǎn)化為矩陣形式，即為

Ag=B

(23)

式中：

由于A為(n-1)×3的矩陣，無法直接求逆，通過SVD分解計算A的偽逆A+。

則：

g=A+B

(24)

4 手術(shù)導(dǎo)航系統(tǒng)手眼標(biāo)定過程描述

文中所介紹的手眼標(biāo)定過程如下：

第一步，控制機器人進行若干次(至少兩次)純平移運動，每次運動完通過雙目視覺重復(fù)采集視覺識別標(biāo)志物的坐標(biāo)，計算平均值，同時通過機器人控制器重復(fù)采集TCP坐標(biāo)，計算平均值。從而獲得點集M與N。

第三步，控制機器人以TCP為球心，進行若干次(至少3次)純旋轉(zhuǎn)運動，每次運動完成后通過雙目視覺重復(fù)采集視覺識別標(biāo)志物的坐標(biāo)，計算平均值，從而獲得點集H。

5 仿真

設(shè)計驗證上述算法的仿真實驗。在該實驗中，設(shè)定雙目視覺相機在機器人坐標(biāo)系下的姿態(tài)以旋轉(zhuǎn)矢量法表示，其中Rx=0.3 rad，Ry=1.6 rad，Rz=0.5 rad，雙目視覺在機器人坐標(biāo)系下的坐標(biāo)x=600 mm、y=700 mm、z=1 000 mm，TCP在視覺識別標(biāo)志物坐標(biāo)系下的坐標(biāo)x′=200 mm、y′=0 mm、z′=0 mm，平移時視覺識別標(biāo)志物坐標(biāo)系與雙目視覺坐標(biāo)系完全平行。同時仿真中設(shè)定雙目視覺坐標(biāo)系下視覺識別標(biāo)定板在x、y、z各個方向的讀數(shù)誤差均值為0 mm、標(biāo)準(zhǔn)差0.12 mm的正態(tài)分布隨機誤差；機器人基座坐標(biāo)系下TCP的x、y、z各個方向上的讀數(shù)誤差為均值0、標(biāo)準(zhǔn)差0.1 mm的正態(tài)分布隨機誤差。

由于該算法中需要進行多次純平移運動和多次純旋轉(zhuǎn)運動，根據(jù)算法過程純平移采集點個數(shù)影響標(biāo)定誤差的旋轉(zhuǎn)部分和平移部分，而純旋轉(zhuǎn)采集點個數(shù)只對平移部分產(chǎn)生影響，仿真實驗時設(shè)定純旋轉(zhuǎn)次數(shù)為6次(共7個點)，對比不同純平移次數(shù)下計算的標(biāo)定誤差(手眼標(biāo)定算法計算旋轉(zhuǎn)矢量和平移向量與理論的旋轉(zhuǎn)矢量和平移向量之間的差值)。仿真實驗結(jié)果如表1所示。

表1 仿真實驗標(biāo)定誤差Tab.1 Calibration error of simulation experiments

由表1中可以看出：在上述仿真實驗條件下，標(biāo)定誤差的平移誤差隨著純平移采集點個數(shù)的增加并沒有明顯的提升；而旋轉(zhuǎn)誤差隨著采集點的增加而明顯減少。且在純平移采集點為8個、純旋轉(zhuǎn)采集點7個的情況下，仿真實驗表明平均平移誤差為0.101 mm，最大平移誤差為0.173 mm，平均旋轉(zhuǎn)誤差為0.07 mrad，最大旋轉(zhuǎn)誤差為0.14 mrad。上述實驗驗證了該手眼標(biāo)定算法的有效性，下一步將進行實物實驗。

6 實驗與分析

6.1 實驗設(shè)備

實驗平臺如圖2所示，所采用的視覺定位設(shè)備為廣州艾目易的AP-STD-200雙目視覺定位系統(tǒng),其位置定位精度為0.12 mm，姿態(tài)定位精度不詳。機器人設(shè)備為UR5協(xié)作機器人，具有6個自由度，位置定位精度為0.1 mm，姿態(tài)定位精度不詳。手眼標(biāo)定程序的開發(fā)環(huán)境為Visual Studio Community2022，采用C++作為編程語言。

圖2 實驗平臺Fig.2 Experimental platform

6.2 實驗方案

et=d1-d2

(25)

er=P(R1)-P(R2)

(26)

式中：et為整體誤差平移部分；d1為機器人控制器讀出的在機器人坐標(biāo)系下TCP坐標(biāo)值；d2為雙目視覺計算的TCP坐標(biāo)轉(zhuǎn)換到機器人坐標(biāo)系下的坐標(biāo)值；er為整體誤差的旋轉(zhuǎn)部分；R1為機器人控制器讀出的TCP在機器人坐標(biāo)系下的旋轉(zhuǎn)矩陣；R2為雙目視覺計算的TCP的旋轉(zhuǎn)矩陣轉(zhuǎn)換到機器人坐標(biāo)系下的旋轉(zhuǎn)矩陣；P(R)為旋轉(zhuǎn)矩陣R對應(yīng)的旋轉(zhuǎn)矢量。

具體實驗流程如下：

第1步，設(shè)置機器人TCP常用刀具工作點(實驗中設(shè)置其與視覺識別標(biāo)志物距離在200 mm左右)。

第4步，移動機器人，根據(jù)式(25)與式(26)計算60種不同位姿下的整體誤差，并記錄。

第5步，改變純平移的次數(shù)，依次為4、5、6、7、8次，重復(fù)第1步到第4步的操作。

按照上述實驗流程，獲取6組數(shù)據(jù)進行分析。

6.3 數(shù)據(jù)分析

按照第6.2節(jié)的實驗流程進行實驗，整體誤差的實驗結(jié)果如表2所示。

表2 實物實驗整體誤差Tab.2 Overall error of physical experiments

7 結(jié)論

文中論述了一種無需測量姿態(tài)數(shù)據(jù)的手眼標(biāo)定算法，進行了仿真實驗和實物實驗，驗證了該算法的有效性，對比了不同純平移采樣點下的標(biāo)定誤差。在雙目視覺相機位置定位精度0.1 mm、姿態(tài)定位精度不詳、機器人定位精度0.1 mm、姿態(tài)定位精度不詳?shù)臈l件下，標(biāo)定誤差的平移部分在0.411 mm以下，旋轉(zhuǎn)部分在4.0 mrad以下，能夠滿足大部分口腔手術(shù)機器人手眼標(biāo)定的精度要求。