一种高精度的手术机器人主从位姿配准方法

1.本发明属于导管建模与力触觉渲染技术领域，涉及一种高精度的手术机器人主从位姿配准方法。


背景技术：

2.手术机器人作为医生的辅助，使医生可以在远程主手端进行操作，防止x射线对医生的影响，使偏远地区或是战地手术等特殊情况下，病人可以得到专家医生的及时治疗。随着5g等通讯技术的发展，远程医疗技术得到了进一步提高，而当前医疗机器人力触觉反馈的缺乏成为制约其推广的重要问题。
3.血管介入手术机器人主手端虚拟现实通过建立空间三维坐标系，将从手端导管位姿真实地反映到虚拟端。柔性导管与血管为多点动态线接触，其弯曲扭转难以准确建模和配准。而基于物理模型计算的受力监测难以避免建模误差。影响建模控制效果。
4.当前血管介入机器人力触觉再现技术中基于位姿测量的模型配准难以准确模拟导管-血管碰撞受力的动态过程。导管与血管为动态多点线接触，导管弯曲和扭转变形的模型配准精度难以保证。
5.因此研究一种高精度的手术机器人主从位姿配准方法具有极其重要的意义。


技术实现要素：

6.本发明的目的是提高柔性导管与血管多点动态线接触模型配准的精度，提供一种高精度的手术机器人主从位姿配准方法。本发明基于帧差法图像识别进行导管前端柔软段位姿测量，实现模型初步配准；基于多点接触力测量，设计以力触觉信息误差最小化为目标的最近点迭代算法，实现精确的导管-血管多点动态线接触渲染动态模型配准。
7.为了达到上述目的，本发明采用的技术方案如下：
8.一种高精度的手术机器人主从位姿配准方法，先基于图像识别测量实现导管位姿(即位置和姿态)的初步配准，再基于多点接触力测量实现导管位姿的动态精确配准；
9.基于多点接触力测量实现导管位姿的动态精确配准的过程为：基于测量力，求解导管接触点形变位移量，通过主手和从手端导管相对距离，基于最近点迭代算法(icp)，即基于最小二乘法的最优匹配方法，找到主手端导管模型与从手端导管模型之间的旋转矩阵和平移向量，使得两匹配数据之间满足某种度量下的最优匹配。
10.作为优选的技术方案：
11.如上所述的一种高精度的手术机器人主从位姿配准方法，初步配准的过程为：在主手端以cosserat弹性杆理论建立导管模型，在从手端通过图像识测量导管前端柔软段的位姿变化，基于d-h坐标变换方法，求解主手端导管模型的坐标系与从手端导管模型的坐标系的转换矩阵，实现主手端导管模型与从手端导管模型的关联，通过旋转矩阵和平移向量实现导管位姿的初步配准。
12.如上所述的一种高精度的手术机器人主从位姿配准方法，初步配准阶段常用的做
法是运用icp算法，在导管进入主动脉血管选择目标分叉血管到达目标的过程中，由icp算法对基于单目相机输出的导管位置数据和虚拟三维图像坐标系下与该段导管路径对应的中心线进行匹配得到初始的旋转矩阵rk和平移向量tk，其实质就是寻找主从两个点集的准确对应点，以及不断迭代两个点集的旋转矩阵和平移向量；初步配准时，旋转矩阵和平移向量是通过以下流程获得的：
13.(i)基于术前三维虚拟图像坐标空间获取主手端虚拟三维图像坐标系下的中心线数据即主手端导管模型点集q，q＝{qi|qi∈r3,i＝1,2,...,n}(，qi表示主手端点集中的具体点，r3表示三维空间坐标，n表示点集里点的个数；通过位姿传感器实时获取从手端实际坐标系下的导管路径数据即从手端导管模型点集p，p＝{pi|pi∈r3,i＝1,2,...,n}(，pi表示从手端点集中的具体点，r3表示三维空间坐标，n表示点集里点的个数；
14.(ii)以p
ik
表示pi的第k次迭代，表示p
ik
对应的q中的最近点，即该点与q1的连线在直线q1qn上的投影线的长度与p中p
ik
与p1的连线在直线p1pn上的投影线的长度最接近；
15.(iii)计算旋转矩阵rk和平移向量tk，使得
16.(iv)计算p
k 1
为点集p在第k 1次迭代的数据，p
ik 1
为点集中具体点pi在第k 1次迭代的数据，为rk中第i个点的旋转矩阵，t
ik
为tk中第i个点的平移向量；
17.(v)计算主手端导管模型点集和从手端导管模型点集中每一个点的位置误差平均值
18.(vi)判断||d
k 1-dk||＜τ是否成立，dk和d
k 1
分别为第k和k 1次迭代的位置误差平均值，τ为迭代阈值，如果是，则输出最后一次迭代时的rk和tk，作为初步配准的旋转矩阵和平移向量；反之，则返回步骤(iii)。
19.如上所述的一种高精度的手术机器人主从位姿配准方法，通过位姿传感器实时获取从手端实际坐标系下的导管路径数据的具体步骤为：
20.(i1)取整个透明的模拟血管放置于单目相机的拍摄范围内；
21.(i2)用一张标有条纹a和条纹b的纸包裹导管前端；包裹前条纹a与条纹b连接成v形；包裹后条纹a所在的平面垂直于导管的中心轴，且条纹a位于条纹b的前端；
22.(i3)将导管放在模拟血管中接触运动，同时单目相机以每秒采集20帧图像的速度捕捉条纹a和条纹b，经滤波后，将图像转变为灰度图，通过帧差法检测并读取到条纹a和条纹b的中心点的像素坐标；
23.(i4)通过实际场景下血管的长宽与图像中血管的长宽，标定像素坐标与实际场景下坐标的比例关系，再通过比例关系，将条纹a和条纹b的中心点的像素坐标转化为实际坐标；
24.(i5)通过条纹a的中心点的实际坐标，转换可得到导管前端的平移自由度位移l，通过条纹a的中心点的实际坐标与条纹b的中心点的实际坐标的距离d获得旋转位移θ，
25.(i6)通过平移自由度位移l和旋转位移θ得到导管的运动坐标，即获取从手端实际
坐标系下的导管路径数据。
26.如上所述的一种高精度的手术机器人主从位姿配准方法，步骤(i1)中，单目相机的拍摄范围为640
×
480像素，分辨率为1920
×
1080，最大帧频为30fps。
27.如上所述的一种高精度的手术机器人主从位姿配准方法，和icp算法类似，精确配准也需要来获取转换矩阵，与初步配准不同的是，精确配准阶段，在导管动态运动过程中，考虑到导管接触受力对主从配准的有效性的影响，即将导管运动分为自由运动和接触运动，精确配准的目标函数中需要考虑主手端跟从手端接触力的误差；自由运动时，主从配准误差主要存在于从手端实际位姿和主手端虚拟位姿之间，系统的误差度量函数为从手端的实际位姿与主手端虚拟位姿之间的偏差除以从手端实际位姿的比值；精确配准时，旋转矩阵和平移向量是通过以下流程获得的：
28.(a)计算旋转矩阵ri和平移向量ti使得pi和qi分别是从手端导管路径位置和主手端血管中心线对应点位置，ri为r中第i个点的旋转矩阵，ti为t第i个点的平移向量，n为模型点集的点数，err(r,t)为主手端导管位姿和从手端导管位姿的误差，其中：
29.δxi＝fk/k；
30.k
n1
δz1 k
n2
δz2 l k
n(m-1)
δz
m-1
k
nm
δzm＝fk；
31.式中，δxi是第i个接触点的形变量；fk为弹簧质点受力，单位n；k为弹性系数，单位n/m；k
ni
为第i个质点法线方向上弹簧的弹性系数，单位n/m，i＝1
…
m；δzi为第i个质点在法向上的变形量，单位m，i＝1
…
m；
32.(b)计算其中，和t
ik
分别表示第k次迭代的ri和ti，p
k 1
为点集p在第k 1次迭代的数据，p
ik 1
为点集中具体点pi在第k 1次迭代的数据，为rk中第i个点的旋转矩阵，t
ik
为tk中第i个点的平移向量；
33.(c)计算主手端导管模型点集和从手端导管模型点集中每一个点的位置误差平均值
34.(d)判断||d
k 1-dk||＜τ是否成立，dk为和d
k 1
分别为第k和k 1次迭代的位置误差平均值，τ为迭代阈值，如果是，则输出最后一次迭代时的ri和ti，作为精确配准的旋转矩阵和平移向量；反之，则返回步骤(a)。
35.有益效果
36.本发明通过模型配准和位姿预测，实现了实时准确的血管介入机器人力触觉再现，提高了系统透明度，主要具有以下特色：
37.(1)基于多点接触力测量，设计以力触觉再现误差最小化为目标的模型配准方法，实现高精度血管介入手术机器人动态位姿配准；该发明能够丰富和发展动态建模理论，推动血管介入机器人力触觉反馈技术走向临床应用；
38.(2)以主手-从手端相对距离误差最小化为目标进行模型配准，实现准确动态的多点动态线接触建模的模型配准；本发明能够从力触觉渲染模型计算和接触力测量两方面进行安全性监测，提高手术机器人系统的透明度和安全性。
附图说明
39.图1为cosserat弹性杆模型；
40.图2为基于帧差法的导管位姿测量示意图；
41.图3为基于光纤和导电橡胶传感器的导丝碰撞和接触力测量示意图；其中，1-铜电极，2-导电橡胶，3-导管，4-光纤压力传感器；
42.图4为配准过程最近点选择策略示意图；
43.图5为基于质点弹簧模型的受力分析图；
44.图6为对应点相对距离示意图；
45.图7为传统icp方法与本发明精配准各路径点位置比较；其中，(a)为未配准的主从导管路径，(b)为传统icp方法配准后的主从导管路径，(c)为精配准后的主从导管路径；
46.图8为传统icp和本发明精配准的配准误差比较；其中，(a)为传统icp配准误差，(b)为本发明精配准误差；
47.图9为配准后的导管路径数据和真实路径对比。
具体实施方式
48.下面结合具体实施方式，进一步阐述本发明。应理解，这些实施例仅用于说明本发明而不用于限制本发明的范围。此外应理解，在阅读了本发明讲授的内容之后，本领域技术人员可以对本发明作各种改动或修改，这些等价形式同样落于本技术所附权利要求书所限定的范围。
49.一种高精度的手术机器人主从位姿配准方法，具体步骤如下：
50.(1)基于图像识别测量实现导管位姿的初步配准；
51.在主手端以cosserat弹性杆理论建立导管模型(如图1所示，n表示为导管模型的离散节点数，离散节点x0,x1,x2...xn将整条线分为多个线段e0,e1,e2...e
n-1
，其中e
i-1
＝x
i-x
i-1
)，在从手端通过图像识测量导管前端柔软段的位姿变化，基于d-h坐标变换方法，求解主手端导管模型的坐标系与从手端导管模型的坐标系的转换矩阵，实现主手端导管模型与从手端导管模型的关联，通过旋转矩阵和平移向量实现导管位姿的初步配准；
52.旋转矩阵和平移向量是通过以下流程获得的：
53.(i)基于术前三维虚拟图像坐标空间获取主手端虚拟三维图像坐标系下的中心线数据即主手端导管模型点集q，q＝{qi|qi∈r3,i＝1,2,...,n}(，qi表示主手端点集中的具体点，r3表示三维空间坐标，n表示点集里点的个数；通过位姿传感器实时获取从手端实际坐标系下的导管路径数据即从手端导管模型点集p，p＝{pi|pi∈r3,i＝1,2,...,n}(，pi表示从手端点集中的具体点，r3表示三维空间坐标，n表示点集里点的个数；
54.通过位姿传感器实时获取从手端实际坐标系下的导管路径数据的具体步骤为：
55.(i1)取整个透明的模拟血管放置于单目相机的拍摄范围内；单目相机的拍摄范围为640
×
480像素，分辨率为1920
×
1080，最大帧频为30fps；
56.(i2)如图2所示，用一张标有条纹a和条纹b的纸包裹导管前端；包裹前条纹a与条纹b连接成v形；包裹后条纹a所在的平面垂直于导管的中心轴，且条纹a位于条纹b的前端；
57.(i3)将导管放在模拟血管中接触运动，同时单目相机以每秒采集20帧图像的速度捕捉条纹a和条纹b，经滤波后，将图像转变为灰度图，通过帧差法检测并读取到条纹a和条
纹b的中心点的像素坐标；
58.(i4)通过实际场景下血管的长宽与图像中血管的长宽，标定像素坐标与实际场景下坐标的比例关系，再通过比例关系，将条纹a和条纹b的中心点的像素坐标转化为实际坐标；
59.(i5)通过条纹a的中心点的实际坐标，转换可得到导管前端的平移自由度位移l，通过条纹a的中心点的实际坐标与条纹b的中心点的实际坐标的距离d获得旋转位移θ，
60.(i6)通过平移自由度位移l和旋转位移θ得到导管的运动坐标，即获取从手端实际坐标系下的导管路径数据；
61.(ii)以p
ik
表示pi的第k次迭代，表示p
ik
对应的q中的最近点，即该点与q1的连线在直线q1qn上的投影线的长度与p中p
ik
与p1的连线在直线p1pn上的投影线的长度最接近；如图4所示；
62.(iii)计算旋转矩阵rk和平移向量tk，使得
63.(iv)计算p
k 1
为点集p在第k 1次迭代的数据，p
ik
1为点集中具体点pi在第k 1次迭代的数据，为rk中第i个点的旋转矩阵，t
ik
为tk中第i个点的平移向量；
64.(v)计算主手端导管模型点集和从手端导管模型点集中每一个点的位置误差平均值
65.(vi)判断||d
k 1-dk＜τ(τ取1mm)是否成立，dk和d
k 1
分别为第k和k 1次迭代的位置误差平均值，如果是，则输出最后一次迭代时的rk和tk，作为初步配准的旋转矩阵和平移向量；反之，则返回步骤(iii)；
66.(2)基于多点接触力测量实现导管位姿的动态精确配准；
67.基于最近点迭代算法(icp)，找到主手端导管模型与从手端导管模型之间的旋转矩阵和平移向量，使得两匹配数据之间满足某种度量下的最优匹配；
68.与初步配准相比，精确配准基于测量力，求解导管接触点形变位移量δxi，通过相对距离的概念，计算最佳转换矩阵，图6为导管路径点与血管中心线对应点的相对距离示意图，其中点a和点b是导管与血管壁的碰撞接触点，δx1和δx2，是接触受力时的形变量，d1和d2分别是点a，点b和其对应点间的距离，使用相对距离来衡量配准精度，可以减少受力形变造成的位置偏移对配准精度的影响；精确配准目标函数考虑受力形变，通过迭代使主手-从手对应点的相对距离最小，求解转换矩阵。
69.如图3所示，导管5前端的接触受力是利用安装在导管前端的光纤压力传感器6测量得到的，导管5侧壁的接触受力是利用包覆在导管侧壁的导电橡胶4的压阻效应测量得到的；“铜电极3”供电；光纤压力传感器的探头直径为0.25mm，精度为
±
2mmhg，分辨率为0.5mmhg，外径为1mm，长度为4m，表面包覆ptfe(聚四氟乙烯)，最高采样频率为250hz(即采样频率不超过250hz)；光纤压力传感器采用白光干涉传播，不会受到应用环境中的电磁信
号、射频信号，磁共振以及微波的其他信号的干扰，能够很好地保持其原有的精度、可重复性，其能够很容易进行测量，可以反复使用，并具有电磁兼容性，其能够满足普遍的血管介入要求：
①
足够柔软，对人体没有伤害，能够适应人体血管形状的复杂多变；
②
尺寸小，便于与导管进行集成；
③
具有较高的灵敏度；导电橡胶为内部均匀分散有导电颗粒的硅橡胶；在压力作用下导电橡胶中的导电颗粒发生接触，产生压阻效应；导电橡胶结构简单可靠，能够充分保证力测量的实时性和准确性，能够满足普遍的血管介入要求：
①
足够柔软，对人体没有伤害，能够适应人体血管形状的复杂多变；
②
尺寸小，便于与导管进行集成；
③
具有较高的灵敏度；
70.旋转矩阵和平移向量是通过以下流程获得的：
71.(a)计算旋转矩阵ri和平移向量ti使得pi和qi分别是从手端导管路径位置和主手端血管中心线对应点位置，ri为r中第i个点的旋转矩阵，ti为t第i个点的平移向量，n为模型点集的点数，err(r,t)为主手端导管位姿和从手端导管位姿的误差，其中δxi是第i个接触点的形变量，δxi是基于从手端受力测量计算得到的，考虑到血管壁以接触点为中心产生区域内变形，其变形行为与相邻弹簧有关，即需要求解相邻弹簧作用下总的形变位移，受力分析如图5所示；
72.fk＝k
n1
δz1 k
t1
δr1sinθ1＝f
n1
f
t1
sinθ1；
73.其中，f
t1
表示切向力，f
n1
表示法向力，k
n1
表示质点法线方向上弹簧的弹性系数，δr1表示切向上相邻质点间弹簧的变形量，δz1表示质点在法向上的变形量，θ1为相邻质点连线与水平方向的夹角；对于i个弹簧质点有：
74.f
n1
f
n2
l f
n(i-1)
f
ni
＝f
k-f
ti
sinθi；
75.设当i＝m，sinθi＝0，则
76.k
n1
δz1 k
n2
δz2 l k
n(m-1)
δz
m-1
k
nm
δzm＝fk；
77.其中，k
ni
为第i个质点法线方向上弹簧的弹性系数，单位n/m，i＝1
…
m；δzi为第i个质点在法向上的变形量，单位m，i＝1
…
m；每个接触点的区域内形变位移总量δxi的表达式如下：
78.δxi＝fk/k；
79.其中，fk为弹簧质点受力，单位n；k为弹性系数，单位n/m；
80.(b)计算其中，和t
ik
分别表示第k次迭代的ri和ti，p
k 1
为点集p在第k 1次迭代的数据，p
ik 1
为点集中具体点pi在第k 1次迭代的数据，为rk中第i个点的旋转矩阵，t
ik
为tk中第i个点的平移向量；
81.(c)计算主手端导管模型点集和从手端导管模型点集中每一个点的位置误差平均值
82.(d)判断||d
k 1-dk||＜τ是否成立，dk为和d
k 1
分别为第k和k 1次迭代的位置误差平均值，如果是，则输出最后一次迭代时的ri和ti，作为精确配准的旋转矩阵和平移向量；反之，则返回步骤(a)。
83.实验过程中，操作者在主手端握持omega.7手柄进行推拉/旋扭将位于从手端夹持在ur5机器人末端的导管插入血管内，驱动导管由主动脉血管经过分叉血管到达目标病灶。
同时对从手端单目相机以及力传感器采集到的导管前端位置数据，进行识别处理后获取导管路径，再和主手端导管路径进行配准处理。图7为使用传统icp的配准方法和本发明高精度位姿配准方法后各路径点位置数据，本发明高精度位姿配准方法配准后得到的主从导管路径位置准确性明显高于传统icp的配准方法。
84.计算导管路径点所对应的配准误差如图8所示，配准过程中，误差逐渐减小，初次迭代的误差最大，误差降低幅度也最大，随着迭代次数的增加，幅度最后趋于平稳。传统icp配准方法没有考虑受力的影响，最后的配准均方根误差达到6.72mm。而本发明的高精度位姿配准方法降低了配准误差。随着配准过程中迭代次数越来越多，获取的转换矩阵越来越精确，对应的配准误差越来越小，并最终稳定在1.02mm左右。
85.图9示出了真实路径和配准后路径，从图中可以看出，根据配准结果，主手端完成了导管位置的实时准确显示，本发明的精配准精度能够满足介入手术的需求。