SVD分解在口腔手術(shù)機(jī)器人手眼標(biāo)定中的應(yīng)用

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

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

0 前言

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

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

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

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

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

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

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

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

(1)

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

(2)

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

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

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

(3)

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

第一步，分別計(jì)算兩組點(diǎn)集的重心。

(4)

第二步，將點(diǎn)集P、Q中每個(gè)元素減去重心，得到兩個(gè)新的點(diǎn)集X、Y。

(5)

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

(6)

(7)

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

S=XWYT

(8)

第五步，對S進(jìn)行SVD分解。

S=UΣVT

(9)

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

(10)

(11)

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

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

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

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

t=[t1,t2,t3]

(12)

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

(13)

(14)

(15)

(16)

(17)

且：

(18)

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

(19)

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

g=[gx,gy,gz]

(20)

則有：

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

(21)

式中：hix為第i個(gè)采集點(diǎn)的x值；hiy為第i個(gè)采集點(diǎn)的y值；hiz為第i個(gè)采集點(diǎn)的z值；d為采集點(diǎn)到圓心的距離。

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

(22)

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

Ag=B

(23)

式中：

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

則：

g=A+B

(24)

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

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

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

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

5 仿真

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

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

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

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

6 實(shí)驗(yàn)與分析

6.1 實(shí)驗(yàn)設(shè)備

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

圖2 實(shí)驗(yàn)平臺(tái)Fig.2 Experimental platform

6.2 實(shí)驗(yàn)方案

et=d1-d2

(25)

er=P(R1)-P(R2)

(26)

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

具體實(shí)驗(yàn)流程如下：

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

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

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

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

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

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

表2 實(shí)物實(shí)驗(yàn)整體誤差Tab.2 Overall error of physical experiments

7 結(jié)論

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