CN114343847A - 基于光学定位系统的手术机器人的手眼标定方法 - Google Patents

Abstract

本发明实施例公开了一种基于光学定位系统的手术机器人的手眼标定方法，包括以下步骤：S1：采集标记球数据；S2：根据机器人内部参数设计求解基座坐标系到末端坐标系的转换关系；S3：根据采集数据求解基座坐标系和光学定位系统坐标系的坐标转换关系；S4：结合三个位姿的坐标信息建立AX＝XB方程，根据李群算法求解手术工具坐标系和末端坐标系的转换关系；S5：设计多个闭环求解方法，建立基座、手术工具、光学定位系统坐标系之间的相互转换关系，完成手眼标定。本发明减少了标定采集数据量，缩短了标定时间，减小了标定误差。

Description

基于光学定位系统的手术机器人的手眼标定方法

技术领域

本发明涉及手术机器人技术领域，尤其涉及一种基于光学定位系统的手术 机器人的手眼标定方法。

背景技术

近年来，随着机器人技术的迅速发展，手术机器人在临床上的应用和优势 逐渐凸显，视觉信息引导下的机器人系统根据相机安装方式不同可分为两种：

1、光学定位系统安装于机械臂末端关节，即Eye in hand模式(EIH)。

2、光学定位系统与机械臂分离并位于距离机械臂2～3米的位置上，即Eye tohand模式(ETH)。

目前解决手眼标定的方法主要包括如下方式：

1、针对光学手术机器人导航系统的手眼标定，传统的标定方法在机器人辅 助的穿刺手术中，安装于机械臂末端的穿刺针需要被多次替换以实现多靶区进 针与定位，同时，需要在机器人末端重新安装特殊的标定装置来达到手眼标定 的目的。

2、根据坐标转换关系将刚性变换描述为：一个平移矩阵和一个旋转矩阵。 迄今为止，已经提出了许多用于手眼校准的封闭式解决方案。通过分别估计方 向分量和平移分量来求解机器人和相机之间的标定问题。

3、因为迭代法具有较高的效率和简单性。迭代法的基本思想是将手眼方程 的左右部分的差异或变化量最小化。利用对偶四元数和四元数乘法性质构造代 价函数，然后应用经典的优化方法最小化代价函数求解最低误差的标定方法。

然而，现有的技术存在以下缺点：

1、采用EIH安装方式，手术过程中因为手术需要不断更换手术工具，导 致安装于末端的光学定位系统的与末端的相对位置关系发生改变，导致术前机 器人标定参数无法适用变更后的手术场景，进而影响机器人穿刺精度。同时， 光学定位系统会占据一定的手术空间，影响医生手术操作，增加手术成本。因 此，穿刺机器人应采用ETH安装方式。而且，此安装方式有助于光学定位系 统监测更大范围的视场。

2、重复安装标定设备增加标定时间成本，导致标定过程复杂，额外的标定 设备也增加了系统标定成本。

3、标定过程中使用大量的封闭式求解计算，使用旋转矩阵结果求解平移矩 阵易叠加计算误差影响精度，标定需采集大量数据辅助标定运算，导致术前标 定时间过长，影响术前工作进度。同时，标定原理规定机器人基座坐标系和光 学定位系统坐标系相对位置保持不变，但在骨科、神经外科、内科等大型手术 过程中需要多人共同完成，如果因为操作失误改变坐标系的位姿都会导致术前 标定参数，需重新标定。

4、使用四元数和对偶四元数求解坐标系间的转换关系，公式推导复杂，实 现困难。

5、迭代方法实际应用中实现复杂，计算量大，对设备性能具有更高要求， 影响标定效率。

发明内容

本发明实施例所要解决的技术问题在于，提供一种基于光学定位系统的手 术机器人的手眼标定方法，以减少标定采集数据量，缩短标定时间，减小标定 误差。

为了解决上述技术问题，本发明实施例提出了一种基于光学定位系统的手 术机器人的手眼标定方法，所述手术机器人包括近红外光学定位系统、六自由 度机械臂及安装有四个反光标记球的手术工具，所述手术工具固定于六自由度 机械臂末端，所述标记球由PVC材料制成，表面涂有用于反射近红外光的反射 涂层；所述近红外光学定位仪安装有近红外摄像机，构成双目视觉系统；所述 方法包括以下步骤：

S1：采集标记球数据；

S2：根据机器人内部参数设计求解基座坐标系到末端坐标系的转换关系；

S3：根据采集数据求解基座坐标系和光学定位系统坐标系的坐标转换关系；

S4：结合三个位姿的坐标信息建立AX＝XB方程，根据李群算法求解手术工 具坐标系和末端坐标系的转换关系；

S5：设计多个闭环求解方法，建立基座、手术工具、光学定位系统坐标系 之间的相互转换关系，完成手眼标定。

本发明的有益效果为：本发明设计多闭环回路求解各坐标系间的转换关系， 完成手眼标定，本发明简化标定流程并自动实现标定，本发明控制机械臂沿基 座坐标系移动可准确求解基座到光学定位系统的坐标转换关系，通过李群算法 建立标定方程AX＝XB直接求解末端坐标系到手术工具坐标系的旋转平移矩阵， 减少因闭环回路累加计算误差；可避免标定前因采集大量数据增加标定时长， 提高了标定的效率；SVD算法可实时动态求解手术工具和光学定位系统坐标转 换关系，并且在标定完成后可任意改变机械臂和定位仪的相对位置关系。

附图说明

图1是本发明实施例的基于光学定位系统的手术机器人的手眼标定方法的 流程图。

图2是本发明实施例的坐标系间转换关系表示图。

图3是本发明实施例的坐标系的具体含义表示图。

图4是本发明实施例的标定原理实物示意图。

图5是本发明实施例的标定原理示意图。

图6是本发明实施例的工具坐标系的构建示意图。

具体实施方式

需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征 可以相互结合，下面结合附图和具体实施例对本发明作进一步详细说明。

请参照图1～图6，本发明实施例的基于光学定位系统的手术机器人的手眼 标定方法采用光学定位系统和机械臂分离(eye-to-hand)模式。光学定位系统包 括近红外光学定位系统、六自由度机械臂、安装有四个反光标记球的手术工具。 所述的手术工具固定于六自由度机械臂末端，其上的标记球由PVC材料制成， 表面涂有反射涂层，可以反射近红外光；所述近红外光学定位仪安装有两台近 红外摄像机，构成双目视觉系统，实现追踪标定工具上反光标记球的空间位姿。

本发明实施例的基于光学定位系统的手术机器人的手眼标定方法包括以下 步骤：S1，采集标记球数据，包括以下步骤：S11：在光学定位系统可监控的视 场范围内控制机械臂沿基座坐标系运动采集反光标记球的数据；S12：根据操作 人员经验选取末端初始位姿和两次变化较大的位姿，并记录保存标记球数据； S2：根据机器人内部参数设计求解基座坐标系到末端坐标系的转换关系；S3： 根据采集数据求解基座坐标系和光学定位系统坐标系的坐标转换关系；S4结合 三个位姿的坐标信息建立AX＝XB方程，使用李群算法求解手术工具坐标系和末 端坐标系的转换关系；S5设计多个闭环求解方法，建立基座、手术工具、光学 定位系统坐标系之间的相互转换关系。本发明不需要额外的硬件设备辅助定位， 操作简单，不需采集大量数据减少了标定时间，提高了手术机器人手眼标定的 自动化程度和标定效率。

为了简洁数学公式表示，不同的坐标系可用不同字母表示，小写字母表示 坐标系，其具体含义如附图3所示。S_x代表x坐标系，而^xP代表点在x坐标系下 的坐标点。R表示旋转矩阵，R_xy表示从x坐标系到y坐标系的旋转矩阵，T代表 平移矩阵，并且T_xy代表从x坐标系到y坐标系的平移矩阵。C代表转换矩阵，并 且C_xy代表从x坐标系到y坐标系的转换矩阵。以转换矩阵为例表示各坐标系之间 的转换关系如附图2所示。

本发明实施例的基于光学定位系统的手术机器人的手眼标定方法包括数据 采集和相对位姿转换两大部分。

数据采集：

1)控制机械臂沿坐标系S_b移动，采集运动过程中安装在手术工具上的反光 标记球在坐标系S_o下的位姿信息和坐标系S_b下机械臂末端的位姿信息。

2)根据经验控制机械臂使末端前后相对位姿发生较大变化，同时采集运动 过程中反光标记球在光学定位系统坐标系下的位姿信息。

2、相对位姿转换：

(1)根据采集的位姿信息求解机械臂基座和末端、基座和光学定位系统的 坐标转换关系。

(2)结合采集数据和SVD算法求解手术工具和光学定位系统坐标转换关 系。

(3)建立转换矩阵C_be、转换矩阵C_et、转换矩阵C_to之间的位姿转换关系。

(4)建立标定方程AX＝XB，求解末端坐标系到手术工具坐标系的转换关 系。

(5)利用步骤(1)、(2)、(3)、(4)的计算结果，设计多闭环回路求解各 坐标系间的转换关系，完成手眼标定。该手眼标定方法控制机械臂沿基座坐标 系移动可准确求解计算基座到光学定位系统的坐标转换关系，通过李群算法建 立标定方程AX＝XB直接求解末端坐标系到手术工具坐标系的旋转平移矩阵，减 少因闭环回路累计计算误差；可避免标定前因采集大量数据减少标定时长，提 高标定效率；SVD算法求解手术工具和光学定位系统坐标转换关系可实时动态 求解手术工具和光学定位系统坐标转换关系，并且在标定完成后可任意改变机 械臂和定位仪的相对位置关系。

具体地，数据采集：

1)标定机械臂和光学定位系统，需采集一组坐标系S_b下机械臂末端的初始 位姿I＝(x,y,z,rx,ry,rz)，其中(x,y,z)表示机械臂末端位姿的位置分量、(rx,ry,rz)表 示机械臂末端位姿的旋转分量。控制机械臂末端分别沿坐标系S_b的X,Y,Z轴移动， 在移动对应位姿后实时采集反光标记球在光学定位系统下的位置信息。标定过 程中必须保证安装在手术工具上的反光标记点始终处于光学定位系统的视野范 围内，确保光学定位系统可实时捕捉手术工具上标记点位置。机械臂的初始位 姿可由工程师根据经验选择合适位姿。沿S_b的X,Y,Z坐标轴移动末端的目标偏移 位姿分别可表示为^xM_i、^yM_i、^zM_i，i∈[1,m]，m表示沿坐标系S_b每个坐标轴移动 的次数，^xM_i表示沿S_b的X轴偏移i次后的位姿，每次偏移dmm。末端偏移位姿 可由如下公式(1)表示：

保存末端位姿数据的同时，需记录安装在手术工具上反光标记球在S_o下的 三维坐标，所采集的标记点坐标可由如下公式(2)表示：

其中

表示机械臂末端移动到

位姿时标记球在坐标系S_o下的坐标值， 左上标k₁∈{x,y,z}表示沿对应轴移动，右上标k₂∈{a,b,c}表示标记点序号。

2)根据经验设定机械臂末端在坐标系S_b下的初始位姿^bP₀，两个偏移位姿^bP₁、 ^bP₂，其对应的在坐标系S_o下光学标记球的坐标值可由如下公式(3)表示：

^oP_i ^k表示机械臂末端移动到^bP_i位姿时标记球k在坐标系S_o下的坐标值，左上 标o表示坐标系S_o，右上标k∈{a,b,c}表示标记点序号，右下标i∈[0,2]表示末端 位姿序号，移动过程中必须确保反光球在光学定位系统视野范围内。

具体地，相对位姿转换：

a)计算C_be

沿坐标轴移动只改变坐标系间的旋转矩阵，坐标系S_b到坐标系S_e旋转矩阵 R_be可通过四元数算法处理初始位姿的旋转分量(rx,ry,rz)计算得到。可由以下公式 (4)表示：

其中

旋转矩阵是 正交矩阵，根据正交矩阵的性质可得：

基座坐标系S_b到末端坐标系S_e的 平移矩阵T_be根据初始位姿的位置分量(x,y,z)求得。

b)计算C_to

如图6所示，手术工具有四个反光标记球，可随机选取其中三个标记球 P_a,P_b,P_c所在位置建立手术工具坐标系S_t。如附图6所示，选取P_a作为的坐标系原 点，从P_a点出发指向点P_b的方向向量为

即单位向量

作为X轴的 单位向量。选取标记球平面法向量的单位向量作为Z轴，Z轴单位向量

和Y轴 的单位向量

可根据以下公式(5)表示：

其中，v_ac表示P_a指向P_c的方向向量。

根据手术工具设计文件参数，坐标系S_t下的标记球位置信息可由以下矩阵(6) 表示：

光学定位系统实时获取手术工具上光学标记球的位置信息，因此，光学标 记球在坐标系S_o下的位置信息可可由以下矩阵(7)表示：

矩阵^oP和矩阵^tP的质心分别为^oCP，^tCP，其计算可由以下公式(8)表示：

建立转换关系：^oCP＝R_to·^tCP+T_to

利用奇异值分解的方法解出R_to和T_to的值：

[U,S,V^T]＝SVD(H)

R_to＝V·U^T

T_to＝-R_to·^tCP+^oCP

其中，H为协方差矩阵。

c)计算R_ob

标定过程中，基座和光学定位系统的相对位姿关系始终保持不变，坐标系S_b变换到坐标系S_o的旋转矩阵可分解为不同坐标轴求解。令

其 中R_ob∈R^3×3和

令

表示S_b坐标系的X轴正方向的单位 向量。当机械臂末端沿基座坐标系的坐标轴平移时，S_o坐标系下，

表示反光 标记球a的初始位姿到偏移位姿的方向向量对应的单位向量(选择工具上的任意 标记球即可，这里选择a)，其可根据以下公式(9)计算得到：

其中^xP_i ^a表示初始位姿沿坐标系S_b的X轴移动i次后，反光标记球a在坐标 系S_o的位置信息，i∈[1,m]，

表示初始位置信息。

是

从坐标系S_o转换到 坐标系S_b下的结果。因此，相互位姿关系表示为：

为减少机械臂移动过程中引入运动学误差，需采集多组沿对应坐标轴移动 数据并计算平均值，因此，R_bo可由如下公式(10)计算求解：

d)C_et、C_tp

机械臂和光学定位系统安装分别固定底座和支架上，因此S_b坐标系和S_o坐 标系的相对位姿在标定过程中始终保持不变。同时，手术工具固定安装于机械 臂末端，当机械臂末端位姿发生偏移时不会改变坐标系S_e和坐标系S_t的相对位 姿关系，所以C_et、C_bo始终是定值。因此可确立方程：

方程变形可得：

令

所以计算方 程AX＝XB，A＝XBX^T，即可解得C_et，其中A,B可步骤a)，b)求得。将方程以 矩阵形式表示如下(11)为：

上式展开可得：R_AR_X＝R_XR_B，R_AR_X+T_A＝R_XT_B+T_X，其中R∈SO(3),T∈R^3×1，可 采取两步法求解转换矩阵，即先求解旋转矩阵，再根据旋转矩阵建立两坐标系 转换关系方程求解平移矩阵。根据李群算法，并结合罗得里格斯公式，A＝XBX^T方程左右两边同时取对数可得：logA＝logXBX^T，令logA＝[α],logB＝[β]，可化为 [α]＝X[β]X^T＝[Xβ]，[·]是反对称算子，

中

三组末端位姿表示可对应α₁、 α₂、β₁、β₂。

R_X＝MN^-1，其中M＝[α₁ α₂ α₁×α₂]，N＝[β₁ β₂ β₁×β₂]。

根据附加图6，坐标系S_t与坐标系S_p间的转换关系是固定不变的，旋转矩阵 可用单位矩阵E表示，E∈R^3×3。平移矩阵T_tp可根据工具设计文件求得，T_tp∈R^3×1。

e)计算C_op、C_tb

根据多闭环第三条回路，可有由如下公式(12)计算旋转矩阵ⁱR_op，ⁱT_op。

f)同理，根据多闭环第二条回路建立方程C_tb＝C_te·C_eb，即可求得C_tb，根据 多闭环第一条回路即可求得任意坐标系间的转换关系.

至此可构建坐标系S_b，坐标系S_p和坐标系S_o之间的转换关系，手眼标定完 成，机械臂可自主移动至光学定位系统视野范围内的任意位置。

本发明减少了标定采集数据量，缩短了标定时间；本发明尽量避免或使用 较短闭环求解坐标系的转换关系，同时基于李群算法求解坐标系的转换关系， 减小标定误差；本发明可以使术中光学定位系统实现实时采集反光标记球数据， 更新标定参数。

尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言， 可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变 化、修改、替换和变型，本发明的范围由所附权利要求及其等同范围限定。

Claims (8)

1.一种基于光学定位系统的手术机器人的手眼标定方法，所述手术机器人包括近红外光学定位系统、六自由度机械臂及安装有四个反光标记球的手术工具，所述手术工具固定于六自由度机械臂末端，所述标记球由PVC材料制成，表面涂有用于反射近红外光的反射涂层；所述近红外光学定位仪安装有近红外摄像机，构成双目视觉系统；其特征在于，所述方法包括以下步骤：

S1：采集标记球数据；

S2：根据机器人内部参数设计求解基座坐标系到末端坐标系的转换关系；

S3：根据采集数据求解基座坐标系和光学定位系统坐标系的坐标转换关系；

S4：结合三个位姿的坐标信息建立AX＝XB方程，根据李群算法求解手术工具坐标系和末端坐标系的转换关系；

S5：设计多个闭环求解方法，建立基座、手术工具、光学定位系统坐标系之间的相互转换关系，完成手眼标定。

2.如权利要求1所述的基于光学定位系统的手术机器人的手眼标定方法，其特征在于，所述步骤S1包括以下子步骤：

S11：控制机械臂沿坐标系S_b移动，采集运动过程中安装在手术工具上的反光标记球在坐标系S_o下的位姿信息和坐标系S_b下机械臂末端的位姿信息；

S12：控制机械臂使末端前后相对位姿发生预设变化，同时采集运动过程中反光标记球在光学定位系统坐标系下的位姿信息。

3.如权利要求2所述的基于光学定位系统的手术机器人的手眼标定方法，其特征在于，标定机械臂和光学定位系统，需采集一组坐标系S_b下机械臂末端的初始位姿I＝(x,y,z,rx,ry,rz)，其中(x,y,z)表示机械臂末端位姿的位置分量、(rx,ry,rz)表示机械臂末端位姿的旋转分量，子步骤S11中，控制机械臂末端分别沿坐标系S_b的X,Y,Z轴移动，在移动对应位姿后实时采集反光标记球在光学定位系统下的位置信息，

沿S_b的X,Y,Z坐标轴移动末端的目标偏移位姿分别表示为^xM_i、^yM_i、^zM_i，i∈[1,m]，m表示沿坐标系S_b每个坐标轴移动的次数，^xM_i表示沿S_b的X轴偏移i次后的位姿，每次偏移dmm，末端偏移位姿由如下公式表示：

保存末端位姿数据的同时，需记录安装在手术工具上反光标记球在S_o下的三维坐标，所采集的标记点坐标由如下公式表示：

其中

表示机械臂末端移动到

位姿时标记球在坐标系S_o下的坐标值，左上标k₁∈{x,y,z}表示沿对应轴移动，右上标k₂∈{a,b,c}表示标记点序号；

子步骤S12中，设定机械臂末端在坐标系S_b下的初始位姿^bP₀，两个偏移位姿^bP₁、^bP₂，其对应的在坐标系S_o下光学标记球的坐标值由如下公式表示：

^oP_i ^k表示机械臂末端移动到^bP_i位姿时标记球k在坐标系S_o下的坐标值，左上标o表示坐标系S_o，右上标k∈{a,b,c}表示标记点序号，右下标i∈[0,2]表示末端位姿序号。

4.如权利要求1所述的基于光学定位系统的手术机器人的手眼标定方法，其特征在于，所述步骤S2中，根据以下方法计算基座坐标系到末端坐标系的转换矩阵C_be：

沿坐标轴移动只改变坐标系间的旋转矩阵，坐标系S_b到坐标系S_e旋转矩阵R_be通过四元数算法处理初始位姿的旋转分量(rx,ry,rz)计算得到，由以下公式表示：

其中

根据正交矩阵的性质得：

基座坐标系S_b到末端坐标系S_e的平移矩阵T_be根据初始位姿的位置分量(x,y,z)求得。

5.如权利要求4所述的基于光学定位系统的手术机器人的手眼标定方法，其特征在于，步骤S5中，采用以下方法计算手术工具坐标系到光学定位系统坐标系的转换矩阵C_to：

随机选取四个反光标记球中的三个标记球P_a,P_b,P_c所在位置建立手术工具坐标系S_t，选取P_a作为的坐标系原点，从P_a点出发指向点P_b的方向向量为

即单位向量

作为X轴的单位向量，选取标记球平面法向量的单位向量作为Z轴，Z轴单位向量

和Y轴的单位向量

根据以下公式表示：

其中，v_ac表示P_a指向P_c的方向向量；

根据手术工具设计文件参数，坐标系S_t下的标记球位置信息由以下矩阵(6)表示：

光学定位系统实时获取手术工具上光学标记球的位置信息，因此，光学标记球在坐标系S_o下的位置信息由以下矩阵表示：

矩阵^oP和矩阵^tP的质心分别为^oCP，^tCP，其计算由以下公式表示：

建立转换关系：^oCP＝R_to·^tCP+T_to；

利用奇异值分解的方法解出R_to和T_to的值：

[U,S,V^T]＝SVD(H)

T_to＝-R_to·^tCP+^oCP

其中，H为协方差矩阵。

6.如权利要求5所述的基于光学定位系统的手术机器人的手眼标定方法，其特征在于，采用以下方法计算R_ob：

标定过程中，基座和光学定位系统的相对位姿关系始终保持不变，坐标系S_b变换到坐标系S_o的旋转矩阵分解为不同坐标轴求解，令

其中R_ob∈R^3×3和

令

表示S_b坐标系的X轴正方向的单位向量；当机械臂末端沿基座坐标系的坐标轴平移时，S_o坐标系下，

表示反光标记球a的初始位姿到偏移位姿的方向向量对应的单位向量，其根据以下公式计算得到：

其中

表示初始位姿沿坐标系S_b的X轴移动i次后，反光标记球a在坐标系S_o的位置信息，i∈[1,m]，

表示初始位置信息；

是

从坐标系S_o转换到坐标系S_b下的结果；因此，相互位姿关系表示为：

为减少机械臂移动过程中引入运动学误差，需采集多组沿对应坐标轴移动数据并计算平均值，因此，R_bo由如下公式计算求解：

7.如权利要求6所述的基于光学定位系统的手术机器人的手眼标定方法，其特征在于，步骤S4和S5中，采用以下方法末端坐标系到工具坐标系的转换矩阵C_et和工具坐标系到针尖坐标系的转换矩阵C_tp：

确立方程：

方程变形得：

令X＝C_et,

所以计算方程AX＝XB，A＝XBX^T，再解得C_et，将方程以矩阵形式表示，如下式：

上式展开得：R_AR_X＝R_XR_B，R_AR_X+T_A＝R_XT_B+T_X，其中R∈SO(3),T∈R^3×1，采取两步法求解转换矩阵，即先求解旋转矩阵，再根据旋转矩阵建立两坐标系转换关系方程求解平移矩阵，根据李群算法，并结合罗得里格斯公式，A＝XBX^T方程左右两边同时取对数得：log A＝logXBX^T，令log A＝[α],log B＝[β]，简化为[α]＝X[β]X^T＝[Xβ]，[·]是反对称算子，

中

三组末端位姿表示对应α₁、α₂、β₁、β₂；

R_X＝MN^-1，其中M＝[α₁ α₂ α₁×α₂]，N＝[β₁ β₂ β₁×β₂]；

旋转矩阵R_tp用单位矩阵E表示，E∈R^3×3，平移矩阵T_tp根据工具设计文件求得，T_tp∈R^3×1。

8.如权利要求7所述的基于光学定位系统的手术机器人的手眼标定方法，其特征在于，步骤S5中，采用以下步骤计算定位系统坐标系到针尖坐标系的转换矩阵C_op、工具坐标系到基座坐标系的转换矩阵C_tb:

根据多闭环第三条回路，由如下公式计算旋转矩阵ⁱR_op和ⁱT_op：

再根据多闭环第二条回路建立方程C_tb＝C_te·C_eb，求得C_tb，根据多闭环第一条回路求得任意坐标系间的转换关系；

最后构建坐标系S_b、坐标系S_p和坐标系S_o之间的转换关系，手眼标定完成。

Images