SVD分解在口腔手术机器人手眼标定中的应用

李恒，陈铸，罗阳，伍剑波，方辉

(四川大学机械工程学院，四川成都 610041)

0 前言

随着机器人技术的发展，机器人被越来越多地应用到医疗行业。其中手术机器人是未来医疗机器人中的主要门类[1]。而手术导航系统的手眼标定问题是手术机器人研究中十分重要的课题，手眼标定的精度将直接影响手术机器人工作的精度。根据调研可知：进行口腔手术的机器人导航系统位置定位精度至少在0.5 mm以下，姿态定位精度至少在1°(即17 mrad)以下。

手眼标定问题目前可分为两类：一类是Eye-to-Hand(固定视点)，一类是Eye-in-Hand(非固定视点)。前者的传感器与机器人分离，两者分别固定，工作时传感器保持静止；后者的传感器固定于机器人末端，工作时传感器随机器人末端移动[2-3]。相较于Eye-in-Hand系统，Eye-to-Hand系统的传感器始终静止，更容易实现高精度、高稳定的导航，因此目前大多数口腔手术机器人均采用Eye-to-Hand系统。

手眼标定问题的核心为求解传感器坐标系和机器人坐标系之间的旋转矩阵和平移向量。其相关算法国内外均有大量的研究与成果，最早由SHIU和AHMAD[4]将该问题转化为解齐次方程AX=XB的问题。围绕求解AX=XB， TSAI和LENZ[5]提出了两步法，先计算出手眼标定中的旋转矩阵，再利用旋转矩阵求解平移向量。文献[6-7]通过李群代数、对偶四元数相关数学工具，同时求解了旋转矩阵和平移向量。前面所述算法都需要传感器能够同时精确测量标志物的位置和姿态信息。

由双目视觉姿态测量原理可知：其姿态定位精度低于位置定位精度，且其姿态精度难以控制(与标志物形状有关)。当双目视觉相机测量的姿态数据误差较大时，仍使用姿态数据进行手眼标定，必然带来较大的标定误差。在这种情况下，文献[8]采用最近点迭代算法，但迭代时给出的初始值可能导致迭代进入局部最优解，且迭代过程较长。文献[9]通过刚体变化算法分别计算旋转矩阵和平移向量，但需要人工操作视觉定位球的球心与机器人TCP重合，难以保证精度。文献[3]通过简单的3次纯平移与1次纯旋转过程，计算旋转矩阵和平移向量，但采集和用于计算的数据较少，该方法的精度有限。

本文作者阐述一种无需测量视觉标志物姿态数据的口腔手术机器人手眼标定算法，通过多次采集纯平移与纯旋转过程中的数据，借助非奇异值分解的数学工具计算传感器坐标系与机器人基座坐标系之间的旋转矩阵与平移向量。

1 口腔手术机器人手眼标定数学模型

在机器人的末端夹持手术所使用的工具并定义该工具的工作点为机器人的TCP，并将视觉识别标志物与工具相对固定，双目视觉在机器人的外部固定，如图1所示。

图1 手术机器人系统示意Fig.1 Surgical robot system

(1)

将式(1)分解后可得到如下齐次方程组：

(2)

2 基于SVD分解的刚体变换算法

文献[10]提出了基于SVD分解的刚体变化算法计算两组对应点集之间的旋转矩阵和平移向量。

设有两组d维点集P={p1,p2，…，pn}和Q={q1，q2，…，qn}，其中p1和q1，p2和q2，…,pn和qn两两相互对应。通过最小二乘法可得下列函数：

(3)

式中：R为点集P和点集Q对应的旋转矩阵；t为点集P和点集Q对应的平移向量；ωi为两点集每个对应点的权重，常设为1，ωi>0。

第一步，分别计算两组点集的重心。

(4)

第二步，将点集P、Q中每个元素减去重心，得到两个新的点集X、Y。

(5)

第三步，将X、Y中的点集组成d×n的矩阵，分别为X、Y,将每对点集的权重组成对角矩阵W。

(6)

(7)

第四步，计算两组新点集与权重矩阵的协方差矩阵。

S=XWYT

(8)

第五步，对S进行SVD分解。

S=UΣVT

(9)

第六步，计算旋转矩阵R和平移向量t。

(10)

(11)

采用了上述方法进行手眼标定时，由于双目视觉获取的是视觉标志物的点集，机器人获取的是TCP的点集，而视觉识别标志物点集与TCP点集并不对应，文献[9]人工将视觉标志球球心与机器人的TCP重合，但该方法操作难度大，因此最终标定结果并不理想。

3 基于SVD分解的手眼标定算法

观察式(5)，将原始点集的每一个元素减去点集的重心后构成新的点集，两组新点集坐标系之间的坐标系原点重合。

而对于口腔手术机器人，视觉识别标志物与手术工具工作点TCP之间的旋转矩阵和平移向量随结构的固定而确定。在对应点采集过程中如果保持手术工具只进行纯平移运动，视觉识别标志物的空间点坐标与TCP空间点坐标之间始终相差一个固定的平移向量t，设为

t=[t1,t2,t3]

(12)

则在平移过程中采集的TCP在机器人基座坐标系下的坐标点集为N={n1，n2，…，nn}，则根据式(12)可得：

(13)

(14)

(15)

(16)

(17)

且：

(18)

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

(19)

首先控制机器人以TCP为圆心进行旋转，旋转过程中采集一系列视觉识别标志物在双目视觉坐标系下的坐标点集H={h1，h2，…，hn}。设此时TCP在双目视觉坐标系下的坐标为

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个上述方程，转化为矩阵形式，即为

Ag=B

(23)

式中：

由于A为(n-1)×3的矩阵，无法直接求逆，通过SVD分解计算A的伪逆A+。

则：

g=A+B

(24)

4 手术导航系统手眼标定过程描述

文中所介绍的手眼标定过程如下：

第一步，控制机器人进行若干次(至少两次)纯平移运动，每次运动完通过双目视觉重复采集视觉识别标志物的坐标，计算平均值，同时通过机器人控制器重复采集TCP坐标，计算平均值。从而获得点集M与N。

第三步，控制机器人以TCP为球心，进行若干次(至少3次)纯旋转运动，每次运动完成后通过双目视觉重复采集视觉识别标志物的坐标，计算平均值，从而获得点集H。

5 仿真

设计验证上述算法的仿真实验。在该实验中，设定双目视觉相机在机器人坐标系下的姿态以旋转矢量法表示，其中Rx=0.3 rad，Ry=1.6 rad，Rz=0.5 rad，双目视觉在机器人坐标系下的坐标x=600 mm、y=700 mm、z=1 000 mm，TCP在视觉识别标志物坐标系下的坐标x′=200 mm、y′=0 mm、z′=0 mm，平移时视觉识别标志物坐标系与双目视觉坐标系完全平行。同时仿真中设定双目视觉坐标系下视觉识别标定板在x、y、z各个方向的读数误差均值为0 mm、标准差0.12 mm的正态分布随机误差；机器人基座坐标系下TCP的x、y、z各个方向上的读数误差为均值0、标准差0.1 mm的正态分布随机误差。

由于该算法中需要进行多次纯平移运动和多次纯旋转运动，根据算法过程纯平移采集点个数影响标定误差的旋转部分和平移部分，而纯旋转采集点个数只对平移部分产生影响，仿真实验时设定纯旋转次数为6次(共7个点)，对比不同纯平移次数下计算的标定误差(手眼标定算法计算旋转矢量和平移向量与理论的旋转矢量和平移向量之间的差值)。仿真实验结果如表1所示。

表1 仿真实验标定误差Tab.1 Calibration error of simulation experiments

由表1中可以看出：在上述仿真实验条件下，标定误差的平移误差随着纯平移采集点个数的增加并没有明显的提升；而旋转误差随着采集点的增加而明显减少。且在纯平移采集点为8个、纯旋转采集点7个的情况下，仿真实验表明平均平移误差为0.101 mm，最大平移误差为0.173 mm，平均旋转误差为0.07 mrad，最大旋转误差为0.14 mrad。上述实验验证了该手眼标定算法的有效性，下一步将进行实物实验。

6 实验与分析

6.1 实验设备

实验平台如图2所示，所采用的视觉定位设备为广州艾目易的AP-STD-200双目视觉定位系统,其位置定位精度为0.12 mm，姿态定位精度不详。机器人设备为UR5协作机器人，具有6个自由度，位置定位精度为0.1 mm，姿态定位精度不详。手眼标定程序的开发环境为Visual Studio Community2022，采用C++作为编程语言。

图2 实验平台Fig.2 Experimental platform

6.2 实验方案

et=d1-d2

(25)

er=P(R1)-P(R2)

(26)

式中：et为整体误差平移部分；d1为机器人控制器读出的在机器人坐标系下TCP坐标值；d2为双目视觉计算的TCP坐标转换到机器人坐标系下的坐标值；er为整体误差的旋转部分；R1为机器人控制器读出的TCP在机器人坐标系下的旋转矩阵；R2为双目视觉计算的TCP的旋转矩阵转换到机器人坐标系下的旋转矩阵；P(R)为旋转矩阵R对应的旋转矢量。

具体实验流程如下：

第1步，设置机器人TCP常用刀具工作点(实验中设置其与视觉识别标志物距离在200 mm左右)。

第4步，移动机器人，根据式(25)与式(26)计算60种不同位姿下的整体误差，并记录。

第5步，改变纯平移的次数，依次为4、5、6、7、8次，重复第1步到第4步的操作。

按照上述实验流程，获取6组数据进行分析。

6.3 数据分析

按照第6.2节的实验流程进行实验，整体误差的实验结果如表2所示。

表2 实物实验整体误差Tab.2 Overall error of physical experiments

7 结论

文中论述了一种无需测量姿态数据的手眼标定算法，进行了仿真实验和实物实验，验证了该算法的有效性，对比了不同纯平移采样点下的标定误差。在双目视觉相机位置定位精度0.1 mm、姿态定位精度不详、机器人定位精度0.1 mm、姿态定位精度不详的条件下，标定误差的平移部分在0.411 mm以下，旋转部分在4.0 mrad以下，能够满足大部分口腔手术机器人手眼标定的精度要求。