用于机器人辅助关节置换手术的虚拟夹具控制方法与系统

1.本发明属于人机交互控制相关领域，更具体地，涉及一种用于机器人辅助关节置换手术的虚拟夹具控制方法与系统。


背景技术：

2.手术机器人在近20年间突飞猛进的发展。在骨科领域，基于虚拟夹具约束的机器人辅助手术系统被广泛应用，虚拟夹具提供引导、定位、禁止、力缩放等功能。临床证明虚拟夹具可以增加手术精度和质量，缩短病人康复时间，降低外科医生手术难度。
3.虚拟夹具是一种人机交互中实时约束机器人运动状态算法，又称为主动约束或运动约束。通常分为几何定义、状态评估、约束执行三个部分。虚拟夹具的约束形式可分为线引导型约束和区域禁止型约束。虚拟夹具的约束效果可分为力约束和位置约束，力约束在机器人违反虚拟夹具时只提供反向的力反馈，通常由直接力控制或基于力矩环的阻抗控制实现，位置约束保证虚拟夹具具有一定的约束位置精度，通常基于机器人的位置或速度伺服实现。力约束通过直接控制机器人关节力矩实现，与外界环境接触时表现为阻抗特性，具有较好的稳定性。位置约束的虚拟夹具在与外界环境接触时表现为导纳特性，容易发生碰撞反弹或失稳现象。
4.以髋关节置换手术为例，术中需要将髋臼窝打磨至与假体完美配合的大小和形状。通常以髋臼窝为目标点，构建一个圆锥形虚拟夹具，作用包括1.磨挫前，引导外科医生将与机器人固连的骨钻钻头(以下简称为机器人)精确地拖至手术位置(圆锥顶点)。2.磨挫时，约束机器人不能越过圆锥顶点，防止过度磨削。3.磨挫后，机器人可以很容易地拖回到锥内，进行下一步手术操作。由此可见，骨科手术场景既需要虚拟夹具有较高的位置约束能力来提高手术精度，也需要在复杂的磨挫交互力下保证稳定，难以通过单一的位置约束和力约束算法达到较佳的虚拟夹具约束效果。


技术实现要素：

5.针对现有技术的以上缺陷或改进需求，本发明提供了一种用于机器人辅助关节置换手术的虚拟夹具控制方法与系统，其可以满足骨科关节置换的手术场景中，对虚拟夹具约束的高位置精度和高环境交互稳定性的需求，提高手术质量的同时优化了医生的操作感，极大的提升了虚拟夹具约束效果，满足骨科手术中机器人引导、禁止和自由拖拽的需求。
6.为实现上述目的，按照本发明的一个方面，提供了一种用于机器人辅助关节置换手术的虚拟夹具控制方法，所述控制方法包括以下步骤：
7.(1)实时计算机器人相对每个虚拟夹具曲面的位置、速度及力，并计算距机器人位置最近的虚拟夹具曲面所处的切平面和机器人切向速度方向上的曲率；
8.(2)基于步骤(1)的计算结果划分机器人所处的区域和阻抗控制模式；
9.(3)计算机器人相对每个虚拟夹具约束面的阻抗控制参数变化和控制器加速度输
出；
10.(4)将多个虚拟夹具曲面的约束进行耦合，并基于二次规划算法确定最优的加速度输出，进而基于得到的最优的加速度输出来控制机器人运动。
11.进一步地，将最优的加速度输出传输给机器人，并基于机器人动力学模型计算关节力矩以作为指令传输给机器人，进而控制机器人运动以实现虚拟夹具约束；基于最优的加速度输出经过机器人动力学得到关节力矩输出值τc为：
[0012][0013]
其中j
t
(q)为机器人速度雅可比转置，m(q)是机器人惯性矩阵，是机器人向心力和科氏力矩阵，g(q)是机器人重力矩阵。
[0014]
进一步地，虚拟夹具将机器人的笛卡尔工作空间划分为五个区域，对应的在相图中把阻抗控制划分为五个模式，每个区域都对应着响应阻抗模式的控制目标和控制策略；所述的五个区域包括：自由区域、约束区域、禁止区域、缓冲区域和返回区域；对应的五个阻抗控制模式包括：自由模式、约束模式、禁止模式、缓冲模式和返回模式；通过对区域的划分，在不同的区域应用对应的阻抗控制模式，实现虚拟夹具约束目标。
[0015]
进一步地，自由区域中，机器人表现为顺从人类和外界环境，控制权掌握在人手中，机器人充当人与外部环境之间的力中继；
[0016]
约束区域，在虚拟夹具边界附近，且法向外力δf朝向虚拟夹具边界，表现为人试图拖动机器人突破虚拟夹具边界，此时沿虚拟夹具边界法向的控制权在机器人上，保持力动态平衡；
[0017]
禁止区域，在虚拟夹具边界内部，机器人需要快速返回虚拟夹具边界，同时应该保证人机交互的安全性；
[0018]
缓冲区域，建立在自由区域和约束区域之间，增加机器人接触虚拟夹具边界的空间和时间；
[0019]
返回区域，也建立在自由区域和约束区域之间，目的是使系统快速而渐进的地恢复人类的控制权。
[0020]
进一步地，采用的基本阻抗控制原理为：
[0021][0022][0023]
其中，δx,分别为机器人工具中心点tcp相对于虚拟夹具边界的位置和加速度。为阻抗控制加速度输出，h,d,k分别为虚拟惯性、速度阻尼和位置刚度项，为阻抗控制中力项的加速度输出，其中f
l
为力输入限值，其中f
l
为力输入限值，sat(δf,f
l
)是饱和函数，限制力输入δf不超过f
l
，，分别为阻尼项和刚度项的加速度输出，为加速度直接补偿量。
[0024]
进一步地，阻抗控制模式的切换条件为：
[0025]
当控制器处于自由模式，当时，控制器从自由模式切换至缓冲模式；
用来衡量机器人与虚拟夹具边界状态的变量，称为等效衰减速度，为设定的缓冲模式切换边界常数；
[0026]
当控制器处于缓冲模式，在时，控制器从缓冲模式切换至禁止模式；为机器人最大加速度限值；
[0027]
当控制器处于约束模式，当机器人所受的外力fe朝向虚拟夹具边界，人对机器人的操作意图是企图使机器人进入虚拟夹具内部，控制器就处于约束模式，控制机器人停靠在虚拟夹具边界上；当fe朝向自由区域时，人对机器人的操作意图企图使机器人离开虚拟夹具，控制器进入返回模式；
[0028]
当约束模式下控制器输出时，机器人的位置已经深入虚拟夹具，控制器切换至禁止模式；
[0029]
当控制器处于返回模式，表示机器人即使以最大的减速度减速，也一定会进入自由区域，此时控制器进入自由模式。
[0030]
进一步地，缓冲模式下通过调节阻尼和限制力的变化率来达到目标，具体为：
[0031]
确定机器人状态和控制器参数的中间变化量
[0032]
则阻尼和限制力变化率为：
[0033][0034][0035]
式中，kv为约束模式下的期望阻抗刚度。
[0036]
进一步地，由f
l
＝0得到约束模式和返回模式的模式切换线的斜率为：
[0037][0038]
阻尼和限制力变化公式为：
[0039][0040][0041]
式中，dc,kc为当前阻抗控制器的阻尼，刚度，df为自由模式下阻抗控制器的期望阻尼，为机器人最大速度限值；
[0042]
当机器人在虚拟夹具表面滑动时，为了使得机器人的运动方向始终沿着虚拟夹具曲面的切向，需要在法向方向补偿向心力，表现为补偿向心加速度：
[0043][0044]
其中ρ是距离机器人最近的虚拟夹具处的曲率半径，为机器人速度在虚拟夹具曲面切向投影。
[0045]
进一步地，单个虚拟夹具曲面计算得到的加速度将加速度域划分为可行域和禁止域，所有虚拟夹具曲面得到的可行域的交集组成组合可行域；在组合可行域中求距离最优目标最近的可行点，并表示为一个典型的二次规划问题：
[0046][0047][0048]
式中n表示朝向禁止区域的法向单位向量，表示单个虚拟夹具曲面约束控制器输出的加速度标量，n表示虚拟夹具曲面的数量。
[0049]
本发明还提供了一种如上所述的用于机器人辅助关节置换手术的虚拟夹具控制方法对机器人进行控制的用于机器人辅助关节置换手术的虚拟夹具控制系统：
[0050]
所述控制系统包括虚拟夹具定义模块、相对位置解算模块、多模态阻抗控制模块、多虚拟夹具曲面约束耦合模块及机器人关节力矩命令输出模块；
[0051]
虚拟夹具定义模块用于定义虚拟夹具的形状，把复杂的虚拟夹具解构为多个相交的虚拟夹具曲面；
[0052]
相对位置解算模块用于实时计算机器人相对每个虚拟夹具曲面的位置、速度及力，并计算距机器人位置最近的虚拟夹具曲面处的切平面和机器人切向速度方向上的曲率；
[0053]
多模态阻抗控制模块用于根据机器人相对虚拟夹具的状态划分机器人所处的区域和阻抗控制模式，并计算相对每个虚拟夹具约束面的阻抗控制参数变化和控制器加速度输出；
[0054]
多虚拟夹具曲面约束耦合模块用于耦合多个虚拟夹具面的约束，并通过二次规划算法寻找最优的加速度输出；
[0055]
机器人关节力矩命令输出模块用于基于机器人动力学模型及来自所述多虚拟夹具曲面约束耦合模块的最优的加速度输出计算机器人关节力矩，进而控制机器人运动以实现虚拟夹具约束。
[0056]
总体而言，通过本发明所构思的以上技术方案与现有技术相比，本发明提供的用于机器人辅助关节置换手术的虚拟夹具控制方法与系统主要具有以下有益效果：
[0057]
1.基于机器人动力学直接控制机器人关节力矩，在控制环内没有高位置增益，使机器人具有与高刚度环境稳定接触的能力，更适合骨科手术场景需求。
[0058]
2.采用五种阻抗控制模式实施虚拟夹具约束，自由区域与约束区域控制策略独立，经缓冲区域和返回区域解耦；自由区域控制权在人，可进一步应用其它柔顺拖动算法；约束区域控制权在机器人，可以实现定制化的约束效果，大大扩宽了此方法的适用范围。
[0059]
3.制定控制模式间自动切换的策略，使机器人在任何运动状态和受力条件下都能有较好的约束效果。
[0060]
4.在缓冲区域和返回区域内应用自适应变阻抗控制算法，实现高位置精度的虚拟夹具约束。
[0061]
5.采用二次规划算法耦合多个虚拟夹具曲面约束效果，对任意多的虚拟夹具曲面约束进行耦合，实现复杂虚拟夹具几何的高精度约束。
附图说明
[0062]
图1是本发明提供的一种用于机器人辅助关节置换手术的虚拟夹具控制方法的流程图；
[0063]
图2是本发明提供的一种用于机器人辅助关节置换手术的虚拟夹具控制方法涉及的圆锥形虚拟夹具场景展示图；
[0064]
图3是本发明提供的虚拟夹具在笛卡尔空间的区域划分示意图；
[0065]
图4是本发明提供的阻抗控制模式划分相图。
具体实施方式
[0066]
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。
[0067]
请参阅图1、图2、图3及图4，本发明提供了一种用于机器人辅助关节置换手术的虚拟夹具控制方法，所述控制方法在笛卡尔空间划分虚拟夹具作用区域，基于多模态自适应变阻抗控制，设计具有高位置约束精度的虚拟夹具控制，通过向心力补偿和多虚拟夹具曲面约束耦合算法实现复杂几何构型的虚拟夹具约束，基于机器人动力学解算关节控制力实施虚拟夹具约束。完成关节置换手术中在复杂虚拟夹具约束下的引导、禁止、自由拖拽等操作需求的控制。
[0068]
所述控制方法主要包括以下步骤：
[0069]
步骤一，确定虚拟夹具的形状，并把复杂几何构型的虚拟夹具解构为多个相交的虚拟夹具曲面。
[0070]
具体地，构虚拟夹具约束曲面，复杂的虚拟夹具可以解构为多个相交的约束曲面，并确定空间任一点距离虚拟夹具曲面最近的点和该点处的切平面的求解算法。
[0071]
本实施方式中，把圆锥形虚拟夹具解构为两个对侧相交的曲面，以解决圆锥形虚拟夹具在顶点附近的几何不连续。当机器人位置在圆锥顶点附近，机器人远离近侧虚拟夹具曲面时，近侧虚拟夹具曲面约束失效，对侧的虚拟夹具曲面约束生效，从而有效地将机器人限制在锥体内。
[0072]
步骤二，实时计算机器人相对每个虚拟夹具曲面的位置、速度及力，并计算距机器人位置最近的虚拟夹具曲面所处的切平面和机器人切向速度方向上的曲率。
[0073]
具体地，获取机器人的位置xr、速度和机器人所受的外力fe，计算虚拟夹具曲面上相对机器人tcp最近点的位置，依据空间投影原理得到机器人相对虚拟夹具曲面的法向位置、法向速度和法向外力δf。
[0074]
步骤三，基于步骤二的计算结果划分机器人所处的区域和阻抗控制模式。
[0075]
虚拟夹具将机器人的笛卡尔工作空间划分为五个区域，对应的在相图中把阻抗控制划分为五个模式。每个区域都对应着响应阻抗模式的控制目标和控制策略。
[0076]
所述的五个区域包括：自由区域、约束区域、禁止区域、缓冲区域和返回区域；对应的五个阻抗控制模式包括：自由模式、约束模式、禁止模式、缓冲模式和返回模式。通过对区域的划分，在不同的区域应用对应的阻抗控制模式，实现虚拟夹具约束目标。
[0077]
自由区域，机器人表现为顺从人类和外界环境，控制权掌握在人手中，机器人充当人与外部环境之间的力中继。机器人没有位置控制目标，所以阻抗控制器的刚度kf＝0。为了顺应人的运动，应该有低阻尼df一般取值范围为[10,50]kg/s，人机操作场景中，并且设置虚拟惯性h的取值为[0.5,5]kg。其中为对机器人最大速度和最大加速度的限制，用来保障人机交互的安全；的一般取值范围为[0.1,1]m/s，一般取值范围为[3,15]m/s2。
[0078]
约束区域，在虚拟夹具边界附近，且δf朝向虚拟夹具边界。表现为人试图拖动机器人突破虚拟夹具边界，此时沿虚拟夹具边界法向的控制权在机器人上，保持力动态平衡所以f
l
＝0。通过高刚度的kc，kc的一般取值为[1000,5000]n/m。自动快速收敛位置误差，配合过阻尼防止收敛误差时产生振荡。在虚拟夹具边界的切线方向上机器人可以自由移动，人掌握控制权，机器人仍然保持自由模式的低阻抗参数。
[0079]
禁止区域，在虚拟夹具边界内部，机器人需要快速返回虚拟夹具边界，同时应该保证人机交互的安全性。限制机器人朝向最近的虚拟夹具边界以最大加速度运动，目的是使机器人以最大的力加速返回虚拟夹具边界。
[0080]
缓冲区域，建立在自由区域和约束区域之间，增加机器人接触虚拟夹具边界的空间和时间。缓冲模式有明确的控制目标，缓冲模式的机器人初始的状态和控制参数是自由模式下的低阻抗，终止状态是约束模式的高阻抗。缓冲区域基于自适应变阻抗控制机器人运动，目标是机器人经过缓冲区的缓冲到虚拟夹具边界时，法线方向上的运动恰好能稳定停止，即δxn→
0，对于控制器，需要在这个过程中调节阻抗参数kf→
kc，df→
dc以及f
l
→
0。
[0081]
返回区域，也建立在自由区域和约束区域之间，目的是使系统快速而渐进的地恢复人类的控制权。控制参数从约束模式下的高阻抗快速调节至低阻抗kc→
0，dc→df
，同时调节
[0082]
步骤四，采用自适应阻抗控制算法计算机器人相对每个虚拟夹具约束面的阻抗控制参数变化和控制器加速度输出。
[0083]
采用的基本阻抗控制原理为：
[0084][0085][0086]
其中，δx,分别为机器人工具中心点tcp相对于虚拟夹具边界的位置和加速度。
为阻抗控制加速度输出，h,d,k为虚拟惯性、速度阻尼和位置刚度项，为阻抗控制中力项的加速度输出，其中f
l
为力输入限值，sat(δf,f
l
)是饱和函数，限制力输入δf不超过f
l
，分别为阻尼项和刚度项的加速度输出，为加速度直接补偿量。
[0087]
所述的五个控制模式间的切换条件可以合理的设置为：
[0088]
当控制器处于自由模式，当时，控制器从自由模式切换至缓冲模式。可自主设置来调节缓冲区域的厚度。
[0089]
当控制器处于缓冲模式，在时，控制器从缓冲模式切换至禁止模式。
[0090]
当控制器处于约束模式，当fe朝向虚拟夹具边界，人对机器人的操作意图是企图使机器人进入虚拟夹具内部，控制器就处于约束模式，控制机器人停靠在虚拟夹具边界上。当fe朝向自由区域时，人对机器人的操作意图企图使机器人离开虚拟夹具，控制器进入返回模式。当约束模式下控制器输出时，机器人的位置已经深入虚拟夹具，控制器切换至禁止模式。
[0091]
当控制器处于返回模式，表示机器人即使以最大的减速度减速，也一定会进入自由区域，此时控制器进入自由模式。
[0092]
缓冲模式下通过调节阻尼和限制力的变化率来达到目标。具体的实施方式为：
[0093]
确定可以衡量机器人与虚拟夹具边界状态的变量，称为等效衰减速度：
[0094]
确定机器人状态和控制器参数的中间变化量
[0095]
则阻尼和限制力变化率为：
[0096][0097][0098]
返回模式可行的具体的实施方法为：
[0099]
由f
l
＝0可以得到约束模式和返回模式的模式切换线的斜率为：
[0100][0101]
阻尼和限制力变化公式为：
[0102][0103]
[0104]
当机器人在虚拟夹具表面滑动时，为了使得机器人的运动方向始终沿着虚拟夹具曲面的切向，需要在法向方向补偿合适的向心力，在本实施方式中，表现为补偿向心加速度：
[0105][0106]
其中ρ是距离机器人最近的虚拟夹具处的曲率半径，为机器人速度在虚拟夹具曲面切向投影。
[0107]
步骤五，将多个虚拟夹具曲面的约束进行耦合，并基于二次规划算法确定最优的加速度输出。
[0108]
需要把多个虚拟夹具曲面拼接在一起，在本实施方式中，两个虚拟夹具曲面相交角度为圆锥顶角，可能是非正交的，所以不能简单地通过控制器正交方向解耦的维度扩展来实现三维空间的复杂虚拟夹具。因此，本实施方式提出采用二次规划算法耦合多个非正交虚拟夹具曲面的约束。
[0109]
具体地：首先，将最优目标定义为自由模式下控制器输出的加速度。单个虚拟夹具曲面计算得到的加速度将加速度域划分为可行域和禁止域。所有虚拟夹具曲面得到的可行域的交集组成组合可行域。在组合可行域中求距离最优目标最近的可行点，这可以表示为一个典型的二次规划问题：
[0110][0111][0112]
式中n表示朝向禁止区域的法向单位向量，表示单个虚拟夹具曲面约束控制器输出的加速度标量，n表示虚拟夹具曲面的数量，此实例中n＝2。此方法在确保多个虚拟夹具曲面的约束同时生效的同时尽可能地符合人体运动意图。
[0113]
阻抗输出项具有不同的物理意义。对于虚拟夹具约束任务，总是朝着能量衰减的方向或是可以补偿误差的方向运动。所以的可行域朝向原点侧，而的可行域背向原点侧。每项的最优值直接叠加后得到控制器加速度输出结果，如此保证二次优化结果的合理性。
[0114]
步骤六，将最优的加速度输出传输给机器人，并基于机器人动力学模型计算关节力矩命令，进而控制机器人运动以实现虚拟夹具约束。
[0115]
加速度值通常不能直接当作控制指令来控制机器人，经过机器人动力学得到关节力矩输出值τc为：
[0116][0117]
其中j
t
(q)为机器人速度雅可比转置，m(q)是机器人惯性矩阵，是机器人向心力和科氏力矩阵，g(q)是机器人重力矩阵，它们可以直接由基础机器人动力学解算得到。关节力矩值作为控制指令直接传递给机器人，由机器人底层控制器进行力矩伺服控制，实现虚拟夹具约束效果。
[0118]
本发明还提供了一种用于机器人辅助关节置换手术的虚拟夹具控制系统，所述控
制系统包括虚拟夹具定义模块、相对位置解算模块、多模态阻抗控制模块、多虚拟夹具曲面约束耦合模块及机器人关节力矩命令输出模块。
[0119]
虚拟夹具定义模块用于定义虚拟夹具的形状，把复杂的虚拟夹具解构为多个相交的虚拟夹具曲面。
[0120]
相对位置解算模块用于实时计算机器人相对每个虚拟夹具曲面的位置、速度及力，并计算距机器人位置最近的虚拟夹具曲面处的切平面和机器人切向速度方向上的曲率。
[0121]
多模态阻抗控制模块用于根据机器人相对虚拟夹具的状态划分机器人所处的区域和阻抗控制模式，应用自适应变阻抗控制算法计算相对每个虚拟夹具约束面的阻抗控制参数变化和控制器加速度输出。
[0122]
多虚拟夹具曲面约束耦合模块用于耦合多个虚拟夹具面的约束，并通过二次规划算法寻找最优的加速度输出。
[0123]
机器人关节力矩命令输出模块用于基于机器人动力学模型及来自所述多虚拟夹具曲面约束耦合模块的最优的加速度输出计算机器人关节力矩命令，进而控制机器人运动以实现虚拟夹具约束。
[0124]
本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。