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

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


背景技术：

2.近年来，随着机器人技术的迅速发展，手术机器人在临床上的应用和优势 逐渐凸显，视觉信息引导下的机器人系统根据相机安装方式不同可分为两种：
3.1、光学定位系统安装于机械臂末端关节，即eye in hand模式(eih)。
4.2、光学定位系统与机械臂分离并位于距离机械臂2～3米的位置上，即eye tohand模式(eth)。
5.目前解决手眼标定的方法主要包括如下方式：
6.1、针对光学手术机器人导航系统的手眼标定，传统的标定方法在机器人辅 助的穿刺手术中，安装于机械臂末端的穿刺针需要被多次替换以实现多靶区进 针与定位，同时，需要在机器人末端重新安装特殊的标定装置来达到手眼标定 的目的。
7.2、根据坐标转换关系将刚性变换描述为：一个平移矩阵和一个旋转矩阵。 迄今为止，已经提出了许多用于手眼校准的封闭式解决方案。通过分别估计方 向分量和平移分量来求解机器人和相机之间的标定问题。
8.3、因为迭代法具有较高的效率和简单性。迭代法的基本思想是将手眼方程 的左右部分的差异或变化量最小化。利用对偶四元数和四元数乘法性质构造代 价函数，然后应用经典的优化方法最小化代价函数求解最低误差的标定方法。
9.然而，现有的技术存在以下缺点：
10.1、采用eih安装方式，手术过程中因为手术需要不断更换手术工具，导 致安装于末端的光学定位系统的与末端的相对位置关系发生改变，导致术前机 器人标定参数无法适用变更后的手术场景，进而影响机器人穿刺精度。同时， 光学定位系统会占据一定的手术空间，影响医生手术操作，增加手术成本。因 此，穿刺机器人应采用eth安装方式。而且，此安装方式有助于光学定位系 统监测更大范围的视场。
11.2、重复安装标定设备增加标定时间成本，导致标定过程复杂，额外的标定 设备也增加了系统标定成本。
12.3、标定过程中使用大量的封闭式求解计算，使用旋转矩阵结果求解平移矩 阵易叠加计算误差影响精度，标定需采集大量数据辅助标定运算，导致术前标 定时间过长，影响术前工作进度。同时，标定原理规定机器人基座坐标系和光 学定位系统坐标系相对位置保持不变，但在骨科、神经外科、内科等大型手术 过程中需要多人共同完成，如果因为操作失误改变坐标系的位姿都会导致术前 标定参数，需重新标定。
13.4、使用四元数和对偶四元数求解坐标系间的转换关系，公式推导复杂，实 现困难。
14.5、迭代方法实际应用中实现复杂，计算量大，对设备性能具有更高要求， 影响标
定效率。


技术实现要素：

15.本发明实施例所要解决的技术问题在于，提供一种基于光学定位系统的手 术机器人的手眼标定方法，以减少标定采集数据量，缩短标定时间，减小标定 误差。
16.为了解决上述技术问题，本发明实施例提出了一种基于光学定位系统的手 术机器人的手眼标定方法，所述手术机器人包括近红外光学定位系统、六自由 度机械臂及安装有四个反光标记球的手术工具，所述手术工具固定于六自由度 机械臂末端，所述标记球由pvc材料制成，表面涂有用于反射近红外光的反射 涂层；所述近红外光学定位仪安装有近红外摄像机，构成双目视觉系统；所述 方法包括以下步骤：
17.s1：采集标记球数据；
18.s2：根据机器人内部参数设计求解基座坐标系到末端坐标系的转换关系；
19.s3：根据采集数据求解基座坐标系和光学定位系统坐标系的坐标转换关系；
20.s4：结合三个位姿的坐标信息建立ax＝xb方程，根据李群算法求解手术工 具坐标系和末端坐标系的转换关系；
21.s5：设计多个闭环求解方法，建立基座、手术工具、光学定位系统坐标系 之间的相互转换关系，完成手眼标定。
22.本发明的有益效果为：本发明设计多闭环回路求解各坐标系间的转换关系， 完成手眼标定，本发明简化标定流程并自动实现标定，本发明控制机械臂沿基 座坐标系移动可准确求解基座到光学定位系统的坐标转换关系，通过李群算法 建立标定方程ax＝xb直接求解末端坐标系到手术工具坐标系的旋转平移矩阵， 减少因闭环回路累加计算误差；可避免标定前因采集大量数据增加标定时长， 提高了标定的效率；svd算法可实时动态求解手术工具和光学定位系统坐标转 换关系，并且在标定完成后可任意改变机械臂和定位仪的相对位置关系。
附图说明
23.图1是本发明实施例的基于光学定位系统的手术机器人的手眼标定方法的 流程图。
24.图2是本发明实施例的坐标系间转换关系表示图。
25.图3是本发明实施例的坐标系的具体含义表示图。
26.图4是本发明实施例的标定原理实物示意图。
27.图5是本发明实施例的标定原理示意图。
28.图6是本发明实施例的工具坐标系的构建示意图。
具体实施方式
29.需要说明的是，在不冲突的情况下，本技术中的实施例及实施例中的特征 可以相互结合，下面结合附图和具体实施例对本发明作进一步详细说明。
30.请参照图1～图6，本发明实施例的基于光学定位系统的手术机器人的手眼 标定方法采用光学定位系统和机械臂分离(eye-to-hand)模式。光学定位系统包 括近红外光学
定位系统、六自由度机械臂、安装有四个反光标记球的手术工具。 所述的手术工具固定于六自由度机械臂末端，其上的标记球由pvc材料制成， 表面涂有反射涂层，可以反射近红外光；所述近红外光学定位仪安装有两台近 红外摄像机，构成双目视觉系统，实现追踪标定工具上反光标记球的空间位姿。
31.本发明实施例的基于光学定位系统的手术机器人的手眼标定方法包括以下 步骤：s1，采集标记球数据，包括以下步骤：s11：在光学定位系统可监控的视 场范围内控制机械臂沿基座坐标系运动采集反光标记球的数据；s12：根据操作 人员经验选取末端初始位姿和两次变化较大的位姿，并记录保存标记球数据； s2：根据机器人内部参数设计求解基座坐标系到末端坐标系的转换关系；s3： 根据采集数据求解基座坐标系和光学定位系统坐标系的坐标转换关系；s4结合 三个位姿的坐标信息建立ax＝xb方程，使用李群算法求解手术工具坐标系和末 端坐标系的转换关系；s5设计多个闭环求解方法，建立基座、手术工具、光学 定位系统坐标系之间的相互转换关系。本发明不需要额外的硬件设备辅助定位， 操作简单，不需采集大量数据减少了标定时间，提高了手术机器人手眼标定的 自动化程度和标定效率。
32.为了简洁数学公式表示，不同的坐标系可用不同字母表示，小写字母表示 坐标系，其具体含义如附图3所示。s
x
代表x坐标系，而
x
p代表点在x坐标系下 的坐标点。r表示旋转矩阵，r
xy
表示从x坐标系到y坐标系的旋转矩阵，t代表 平移矩阵，并且t
xy
代表从x坐标系到y坐标系的平移矩阵。c代表转换矩阵，并 且c
xy
代表从x坐标系到y坐标系的转换矩阵。以转换矩阵为例表示各坐标系之间 的转换关系如附图2所示。
33.本发明实施例的基于光学定位系统的手术机器人的手眼标定方法包括数据 采集和相对位姿转换两大部分。
34.数据采集：
35.1)控制机械臂沿坐标系sb移动，采集运动过程中安装在手术工具上的反光 标记球在坐标系so下的位姿信息和坐标系sb下机械臂末端的位姿信息。
36.2)根据经验控制机械臂使末端前后相对位姿发生较大变化，同时采集运动 过程中反光标记球在光学定位系统坐标系下的位姿信息。
37.2、相对位姿转换：
38.(1)根据采集的位姿信息求解机械臂基座和末端、基座和光学定位系统的 坐标转换关系。
39.(2)结合采集数据和svd算法求解手术工具和光学定位系统坐标转换关 系。
40.(3)建立转换矩阵c
be
、转换矩阵c
et
、转换矩阵c
to
之间的位姿转换关系。
41.(4)建立标定方程ax＝xb，求解末端坐标系到手术工具坐标系的转换关 系。
42.(5)利用步骤(1)、(2)、(3)、(4)的计算结果，设计多闭环回路求解各 坐标系间的转换关系，完成手眼标定。该手眼标定方法控制机械臂沿基座坐标 系移动可准确求解计算基座到光学定位系统的坐标转换关系，通过李群算法建 立标定方程ax＝xb直接求解末端坐标系到手术工具坐标系的旋转平移矩阵，减 少因闭环回路累计计算误差；可避免标定前因采集大量数据减少标定时长，提 高标定效率；svd算法求解手术工具和光学定位系统坐标转换关系可实时动态 求解手术工具和光学定位系统坐标转换关系，并且在标定完成后可任意改变机 械臂和定位仪的相对位置关系。
43.具体地，数据采集：
44.1)标定机械臂和光学定位系统，需采集一组坐标系sb下机械臂末端的初始 位姿i＝(x,y,z,rx,ry,rz)，其中(x,y,z)表示机械臂末端位姿的位置分量、(rx,ry,rz)表 示机械臂末端位姿的旋转分量。控制机械臂末端分别沿坐标系sb的x,y,z轴移动， 在移动对应位姿后实时采集反光标记球在光学定位系统下的位置信息。标定过 程中必须保证安装在手术工具上的反光标记点始终处于光学定位系统的视野范 围内，确保光学定位系统可实时捕捉手术工具上标记点位置。机械臂的初始位 姿可由工程师根据经验选择合适位姿。沿sb的x,y,z坐标轴移动末端的目标偏移 位姿分别可表示为
x
mi、ymi、zmi，i∈[1,m]，m表示沿坐标系sb每个坐标轴移动 的次数，
x
mi表示沿sb的x轴偏移i次后的位姿，每次偏移dmm。末端偏移位姿 可由如下公式(1)表示：
[0045][0046]
保存末端位姿数据的同时，需记录安装在手术工具上反光标记球在so下的 三维坐标，所采集的标记点坐标可由如下公式(2)表示：
[0047][0048]
其中表示机械臂末端移动到位姿时标记球在坐标系so下的坐标值， 左上标k1∈{x,y,z}表示沿对应轴移动，右上标k2∈{a,b,c}表示标记点序号。
[0049]
2)根据经验设定机械臂末端在坐标系sb下的初始位姿bp0，两个偏移位姿bp1、 b
p2，其对应的在坐标系so下光学标记球的坐标值可由如下公式(3)表示：
[0050][0051]op
ik
表示机械臂末端移动到bpi位姿时标记球k在坐标系so下的坐标值，左上 标o表示坐标系so，右上标k∈{a,b,c}表示标记点序号，右下标i∈[0,2]表示末端 位姿序号，移动过程中必须确保反光球在光学定位系统视野范围内。
[0052]
具体地，相对位姿转换：
[0053]
a)计算c
be
[0054]
沿坐标轴移动只改变坐标系间的旋转矩阵，坐标系sb到坐标系se旋转矩阵 r
be
可通过四元数算法处理初始位姿的旋转分量(rx,ry,rz)计算得到。可由以下公式 (4)表示：
[0055]
[0056]
其中旋转矩阵是 正交矩阵，根据正交矩阵的性质可得：基座坐标系sb到末端坐标系se的 平移矩阵t
be
根据初始位姿的位置分量(x,y,z)求得。
[0057]
b)计算c
to
[0058]
如图6所示，手术工具有四个反光标记球，可随机选取其中三个标记球 pa,pb,pc所在位置建立手术工具坐标系s
t
。如附图6所示，选取pa作为的坐标系原 点，从pa点出发指向点pb的方向向量为即单位向量作为x轴的 单位向量。选取标记球平面法向量的单位向量作为z轴，z轴单位向量和y轴 的单位向量可根据以下公式(5)表示：
[0059][0060]
其中，v
ac
表示pa指向pc的方向向量。
[0061]
根据手术工具设计文件参数，坐标系s
t
下的标记球位置信息可由以下矩阵(6) 表示：
[0062][0063]
光学定位系统实时获取手术工具上光学标记球的位置信息，因此，光学标 记球在坐标系so下的位置信息可可由以下矩阵(7)表示：
[0064][0065]
矩阵op和矩阵
t
p的质心分别为ocp，
t
cp，其计算可由以下公式(8)表示：
[0066][0067]
建立转换关系：ocp＝r
to
·
t
cp t
to
[0068]
利用奇异值分解的方法解出r
to
和t
to
的值：
[0069]
[u,s,v
t
]＝svd(h)r
to
＝v
·ut
t
to
＝-r
to
·
t
cp ocp其中，h为协方差矩阵。c)计算r
ob
[0070]
标定过程中，基座和光学定位系统的相对位姿关系始终保持不变，坐标系sb变换到坐标系so的旋转矩阵可分解为不同坐标轴求解。令其 中r
ob
∈r3×3和令表示sb坐标系的x轴正方向的单位 向量。当机械臂末端沿基座坐标系的坐标轴平移时，so坐标系下，表示反光 标记球a的初始位姿到偏移位姿的方向向量对应的单位向量(选择工具上的任意 标记球即可，这里选择a)，其可根据以下公式(9)计算得到：
[0071][0072]
其中
x
p
ia
表示初始位姿沿坐标系sb的x轴移动i次后，反光标记球a在坐标 系so的位置信息，i∈[1,m]，表示初始位置信息。是从坐标系so转换到 坐标系sb下的结果。因此，相互位姿关系表示为：
[0073]
为减少机械臂移动过程中引入运动学误差，需采集多组沿对应坐标轴移动 数据并计算平均值，因此，r
bo
可由如下公式(10)计算求解：
[0074][0075]
d)c
et
、c
tp
[0076]
机械臂和光学定位系统安装分别固定底座和支架上，因此sb坐标系和so坐 标系的相对位姿在标定过程中始终保持不变。同时，手术工具固定安装于机械 臂末端，当机械臂末端位姿发生偏移时不会改变坐标系se和坐标系s
t
的相对位 姿关系，所以c
et
、c
bo
始终是定值。因此可确立方程：方程变形可得：令所以计算方 程ax＝xb，a＝xbx
t
，即可解得c
et
，其中a,b可步骤a)，b)求得。将方程以 矩阵形式表示如下(11)为：
[0077][0078]
上式展开可得：rar
x
＝r
x
rb，rar
x
ta＝r
x
tb t
x
，其中r∈so(3),t∈r3×1，可 采取两步法求解转换矩阵，即先求解旋转矩阵，再根据旋转矩阵建立两坐标系 转换关系方程求解平移矩阵。根据李群算法，并结合罗得里格斯公式，a＝xbx
t
方程左右两边同时取对数可得：
loga＝logxbx
t
，令loga＝[α],logb＝[β]，可化为 [α]＝x[β]x
t
＝[xβ]，[
·
]是反对称算子，中三组末端位姿表示可对应α1、 α2、β1、β2。
[0079]rx
＝mn-1
，其中m＝[α
1 α
2 α1×
α2]，n＝[β
1 β
2 β1×
β2]。
[0080]
根据附加图6，坐标系s
t
与坐标系s
p
间的转换关系是固定不变的，旋转矩阵 可用单位矩阵e表示，e∈r3×3。平移矩阵t
tp
可根据工具设计文件求得，t
tp
∈r3×1。
[0081]
e)计算c
op
、c
tb
[0082]
根据多闭环第三条回路，可有由如下公式(12)计算旋转矩阵
irop
，it
op
。
[0083][0084]
f)同理，根据多闭环第二条回路建立方程c
tb
＝c
te
·ceb
，即可求得c
tb
，根据 多闭环第一条回路即可求得任意坐标系间的转换关系.
[0085]
至此可构建坐标系sb，坐标系s
p
和坐标系so之间的转换关系，手眼标定完 成，机械臂可自主移动至光学定位系统视野范围内的任意位置。
[0086]
本发明减少了标定采集数据量，缩短了标定时间；本发明尽量避免或使用 较短闭环求解坐标系的转换关系，同时基于李群算法求解坐标系的转换关系， 减小标定误差；本发明可以使术中光学定位系统实现实时采集反光标记球数据， 更新标定参数。
[0087]
尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言， 可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变 化、修改、替换和变型，本发明的范围由所附权利要求及其等同范围限定。