5.1.9 Realization and Control of Robotic Injection Prototype With Instantaneous Remote Center of Motion Mechanism
Key Words : Impedance control, linear parameter-varying (LPV) system, remote center of motion (RCM) mechanism, Robotic needle insertion.
Authors : Lin Liu , Wei Chen , Zhi Chen, Weiguo Zhou , Ruofeng Wei , Member, IEEE, and Yunhui Liu , Fellow, IEEE
Source : IEEE TRANSACTIONS ON BIOMEDICAL ENGINEERING, VOL. 71, NO. 2, FEBRUARY 2024
摘要
虽然已经对瞬时远心运动（RCM）机制进行了研究，但尚未研究一般闭环控制方法。 因此，本文填补了这一空白，并利用该机制的优势开发了一种新型注射系统。 方法：注射原型包括瞬时RCM机制、插入单元和注射单元。在螺杆传动的时间变化轴向刚度和欠驱动情况下，研究了RCM系统。 为了安全交互，插入系统中设计了顺应性控制。 通过有界参数变化率研究了所有独立系统的稳定性。 然后将注射原型和机器人末端执行器结合起来进行注射。结果：所研究的RCM原型可以实现较大的工作空间，并通过多个框架和与先前研究的比较验证了其控制效果。通过顺应性控制的插入可以实现准确的深度调节和手动操作针的零阻抗控制。 借助三维重建和手眼标定，机械手可以将注射原型引导到适当的姿态进行面部模型的注射。结论：注射原型设计成功。 通过模拟和实验验证了整个控制系统的有效性。原型机可以执行特定的机器人注射任务意义：本文提供了开发瞬时RCM系统、基于螺旋传动的手术工具和小针注射的替代方案。
背景
现代针插入程序需要准确和有效的手动操作。医生为越来越多的患者进行越来越多的手术。 繁重的工作负荷导致医生疲劳和注意力不集中。 为了减轻负担，已经开发了机器人辅助自动化技术。机器人设备可以通过定位和操作插入设备轻松完成插入任务。然而，远心运动中心（RCM）机制，即一种可以为末端执行器上安装的手术工具产生旋转运动的机械装置，在机器人针插入系统的设计中也受到了相当大的关注。 这些RCM系统已被证明对减少创伤和提高机械可靠性和工作空间非常有用。
单自由度(DOF)的RCM机制具有紧凑且稳定的结构。 这种机制可以轻松升级为多自由度的RCM机器人，并与操纵器结合使用。 已经开发出了各种类型的单自由度RCM机制。 圆形RCM机制是基于棱柱关节构建的。 该机制的工作空间较小，需要高生产和装配精度才能实现托架在圆弧齿上的运动。 基于连杆机构的RCM原型可以实现相对较大的工作空间。 然而，由于多个旋转关节的缺乏刚性，会影响末端执行器带载情况下系统的精度。 为了满足上述问题，可以采用所谓的瞬时RCM机制。 这种机制利用两个线性驱动系统之间的速度比来实现RCM。比值的大小决定了工作空间。 仅通过调整比例就很容易实现大范围的运动。 此外，传统的螺旋传动系统可以用于这种RCM机制。 通过这种传统的螺旋传动系统，可以保证负载能力和稳定运动。
高机动性机器人臂和电子注射器（用作执行器）的组合将是支持上述操作空间要求的便捷方法。 然而，全面的机器人注射系统可以包括额外的单元，这将增加末端执行器的长度。 由于这个限制，末端执行器可能无法每次都达到所需的注射姿势。 因此，所提出的RCM原型专门设计用于实现大范围的倾斜运动，具有小尺寸和轻量化，并且可以轻松与可替换的机器人臂配合使用。在机器人靠近人体时，通过紧凑倾斜的RCM机制的额外运动可以有效地将机器人与人体保持距离，减少恐惧感。 广泛的倾斜运动范围特别有利于实现正确的注射角度，从而防止注射产品进入浅筋膜层的无控制分布。 此外，同时，RCM机制的机械约束可以在移动注射单元时提供安全性。
此外，先前的瞬时RCM系统采用开环控制方案来控制线性驱动器的托架（即从执行器获取反馈）。在这种情况下，托架的运动是未知的，并且由于准静态误差（例如形状误差或热膨胀）而偏离期望位置。 为了提高精度，可以在托架的初始位置安装额外的传感器（例如接近开关）。然而，在一个手术过程中，RCM机制的托架可能需要多次移动，比如在我们的情况下，面部的左侧、右侧和前部区域。 此外，还可能需要调整治疗其他区域，比如颈部的皮下脂肪带。 因此，开环控制方法的主要缺点是时间和能量消耗较大。
Methodology
A. 注射原型的设计
注射原型由三个功能模块组成，即瞬时RCM机制、注射单元和插入单元。
- 瞬时RCM机制：瞬时RCM机制的设计使用的组件安装在图1(a)的RCM机制底座上。该机制涉及两个丝杠传动系统。丝杠的旋转运动的动力源是一台直流电机（DCX16L，Maxon Motor AG，瑞士）配备行星齿轮头(GPX19, 瑞士Maxon Motor AG公司)，称这个电机单元为RCM电机. 皮带传动设计用于将电机功率传输到丝杠上。两个滑轮分别安装在丝杠的输入轴和谐波减速器(CSF-5-50-1 U, 日本谐波传动系统公司)的输入轴上。谐波减速器的输出轴通过联轴器与另一个丝杠相连。这种实现可以实现两个托架之间位移的固定比例.如图1(b)所示，轴承安装在托架上. 当托架移动时[参见图1(a)中的红色箭头]，轴承的旋转驱动滑块在导轨上移动[参见图1(b)中的红色箭头].。
图1. 机器人注射原型的组成部分和工作原理。（a）原型的概述。（b）剖面视图中的原型。（c）工作原理图。
提出的硬件如图1©.RCM机制的工作原理根据此图，可以推导出以下几何关系：
其中 u1和 u2是小车的位移， h (=0.053[m]) 是两个小车之间的距离， yrt是RCM点与小车之间的理论距离。根据上述硬件设置，可以得到一个固定的 yrt，因为 u1 =γ1u2，其中 γ1(= 50)是谐波减速器的齿轮比。然后，可以将(1)重写为:
此外，如图1©所示，小车的运动会产生一个倾斜角度：
线性运动可以通过（3）实现，并且可以通过磁编码器（Orbis，Renishaw Plc.，英国）安装在导螺杆的输出轴上来测量小车的位置信息[图1(a)]。 然后，将测量到的运动添加到期望的针尖位置上可以轻松实现补偿。
2）插入单元和注射单元：注射和插入单元中使用的电机单元和编码器与RCM机制相同。针尖插入装置设计用于插入单元的底座上[图1(a)中用绿色标出]。 在针尖和导螺杆小车之间（由插入电机驱动）安装了一个力传感器[M3701 A，Sunrise Instruments (SRI)，中国]。 小车的位置由磁编码器测量。 这两个传感器可以实现闭环力/阻抗控制的针尖插入系统。 基于这个系统，该原型不仅可以执行自动插入任务，还可以涉及"物理人机交互"（pHRI）的应用案例，即手动提取和权衡插入深度的柔顺性和准确性。 此外，如图1(b)所示，插入单元的安装类型类似于悬臂梁结构。 为了增强刚度，在插入单元的小车和底座之间添加了V型导向机构。在注射单元[图1(a)]中，使用两个夹具和一个快速夹紧器来定位和锁定注射器（带有标记的部件：柱塞、筒体、卢尔锁适配器）。 柱塞的夹具安装在导丝的滑块上（由注射电机驱动）。 可以通过调节滑块来控制柱塞的位置。 我们还安装了一个磁性编码器来测量滑块的位置。 这种实现确保了对注射体积的闭环控制。该原型配备了一个30号针头（1毫升注射器）。 这种类型的针头可以减少并发症，因此在面部美容和恢复的临床实践中是可接受的。 在我们的应用中，使用带有卢尔锁适配器的管道连接针头和注射器以输送药物。 带有针头的卢尔锁适配器通过螺丝固定在适配套上，按下快速夹紧器即可拆卸注射器。 通过这种设计，方便处理注射器和针头，例如更换或灭菌。
B. 原型的建模
RCM机制和插入单元是独立激活的不同系统。图2描述了这两个系统的动态结构模型。 两个系统都包含导螺杆传动。 因此，它们可以被建模为轴向-扭转动态系统，因为螺杆部件表现出扭转柔度。 然而，导螺杆的建模过程很复杂。 为了简化进一步研究我们原型的控制，采用了对角质量矩阵和相对简单的视觉阻尼矩阵（由Galerkin过程产生）。
图2. 原型的动力学结构模型。(a)瞬时RCM机构。(b)插入单元。
如图2(a)所示，RCM电机和小车通过两个平行安装的螺杆连接。 该系统的模型可以描述为：
其中，m1和m2是托架的质量，br是托架的粘性摩擦，x1和x2是托架的线性位置，σ1和σ2是螺杆组件的轴向刚度，F1和F2是螺杆的弹性力，x3是电机的线性位置，mm和bm是RCM电机的等效质量和粘性摩擦，γ2是电机的齿轮比，F3是电机力。
将为插入单元设计闭环阻抗控制方案。 为了将力反馈项纳入控制模型中，力传感器可以被视为弹性元件。 结合导丝传动模型，插入单元的动态结构模型如图2(b)所示。 因此，插入单元的模型如下
其中，m3是小车的质量，m4是针头单元的质量，σ3是螺杆部件的轴向刚度，σs是力传感器的轴向刚度，x4、x5和x6分别是电机、小车和针头的线性位置，F4是电机力，F5是螺杆的弹性力，F6是控制力，Fext是环境的外力（针-组织相互作用）
表I. RCM机制和插入单元模型的参数值
C. 控制开发
1）瞬时RCM机制的位置反馈控制框架
讨论了瞬时RCM机制控制的两种情况。整个控制框架如图3所示。内环由比例积分（PI）控制器组成，用于调节RCM电机的速度。该设计的目的是减少来自齿轮箱的不必要干扰。PI控制器的增益是根据电机驱动器（Gold Solo Twitter，Elmo Motion Control Ltd.，以色列）的速度控制器在电机侧进行调整的，然后转换为传动系统中的等效值。将PI控制定律代入(4)-(8)式的系统模型中，可以得到速度源RCM机制的以下LPV状态空间表示：
在图3的第一种情况中，即单环反馈框架中，执行器主动调节一个小车的位置（其中 y = x 1 ， C = [ 1 , 0 1 × 6 ] y = x_1，C = [1, 0_{1×6}] y=x1，C=[1,01×6]），同时，第二个小车在没有反馈控制的情况下移动。 这意味着第二个小车系统中的不确定性可能无法被执行器有效地衰减。 第二种情况涉及对第二个小车的反馈控制。
在这种情况下，设计了双环反馈框架来跟踪小车的位置指令。 这样的控制框架，即所谓的单输入双输出（SITO）控制配置，由于系统的对齐，存在一些权衡。 在下面给出了轨迹跟踪的模拟结果，以澄清哪种情况适合控制RCM机制。对于单环反馈框架，设计了比例-微分（PD）控制器（即 K 1 K_1 K1）。
双回路反馈框架中的控制器设计简单但对齐（考虑两个小车之间的速度比（=50））。 这两个控制器的传递函数分别为 K1(s) = 200/(s+ 1)和 K2(s) = 4/(s+ 1)。 设计这样的控制器的目的是为了轻松实现时域性能分析。 值得注意的是，当这两个控制器增益在适当范围内变化时，跟踪性能不会显著受到影响。 仿真结果如图4所示。 双回路反馈框架可以保持正确的稳态值，但会产生较大的超调和振动。 为了安全起见，应实现RCM机制的稳定运动。 因此，本文在实践中实现了单回路反馈框架。此外，比较结果是合理的，因为两个控制系统具有类似的响应速度。
图3. 瞬时RCM机制位置控制的框图。 符号 x 1 d x_{1d} x1d和 x 2 d x_{2d} x2d是小车的期望位置， K 1 K_1 K1和 K 2 K_2 K2是控制器， x 3 d x_{3d} x3d是期望电机速度， k m p （ = 140 N s / m ) k_{mp}（= 140 Ns/m) kmp（=140Ns/m)是比例增益， k m i （ = 3510 N / m ) k_{mi}（= 3510 N/m) kmi（=3510N/m)是积分增益。
图4. 单环和双环反馈控制的仿真比较。 在仿真中，步进输入的振幅为 x 1 d = 0.01 m x_{1d}=0.01m x1d=0.01m和 x 2 d = 0.0002 m x_{2d}=0.0002m x2d=0.0002m。
- 插入单元的级联阻抗控制框架：针的插入可以包括在安全人机交互范围内。因此，我们对基于螺旋驱动的插入单元施加级联阻抗控制。在这个控制框架中（图5），最内层环路是速度控制环路，与RCM机制中使用的环路相同（图3）。
图5. 插入单元的阻抗控制框图。
中间和外层环路分别是力环路和位置环路。 在这两个环路中，控制规律如下：
其中， x 5 d x_{5d} x5d是小车的期望位置， F 6 d F_{6d} F6d是期望插入力， x ˙ 4 d x˙ _{4d} x˙4d是插入电机的期望速度， k f k_f kf是力控制器的比例增益， k s k_s ks和 k d k_d kd分别是阻抗控制器的比例增益（虚拟刚度）和导数增益（虚拟阻尼）。
上述控制法则可以适应软交互的情况，例如使用小针头进行插入。 内部力环确保力轨迹的准确跟踪，从而可以保持阻抗控制的准确性。 与柔性执行器不同，刚性执行器的力控制器应该经过精心设计。 因此，我们提出使用比例增益 k f k_f kf。 这个增益的值经过迭代调整，直到达到适当的响应速度和正确的稳态值为止。 通过调整（17）中的阻抗控制器增益，可以实现可编程的柔顺性。 在本情况下，使用高增益来施加所需值的插入深度，并且如果人工操作员手动拉出针头单元（执行器可以通过力/阻抗控制进行可逆驱动），将使用零阻抗控制（ k s = 0 k_s = 0 ks=0和 k d = 0 k_d = 0 kd=0）。 值得注意的是，小插入力和大传感器刚度实现了微小的弹性偏转[见(13)]。 因此，在不考虑针头偏转的情况下，可以假设托架位置代表插入深度。
3）稳定性测试：将控制定律（15）代入系统（14）可以得到闭环RCM机制系统的表示。将未受力的闭环
系统表示为x˙ = Acl(σ1, σ2)x，其中系统矩阵
在我们的原型中，小车位置 x2在一个很小的范围内以低速运行（由于谐波传动的高速比和小螺杆导程）。 因此，我们假设刚度σ2以恒定值σ¯2运行。 然后，之前的LPV系统简化为仅涉及时变刚度 σ1。 如果存在一个可微的正定矩阵值函数 P，使得以下线性矩阵不等式（LMI）成立[22]，则未强迫系统 x˙ = Acl（σ1）x是鲁棒稳定的：
其中，P(σ1) = P0 + P1σ1被设计为仿射函数σ1的函数（其中P0和P1是对称矩阵）。 系统矩阵（18）可以写成Acl(σ1) = Acl0 + Acl1σ1的形式（其中Acl0和Acl1是已知常数矩阵）。 然后，(18)式中的项Acl(σ1)TP(σ1)和P(σ1)Acl(σ1)涉及二次项 σ 1 2 σ_1^2 σ12。 为了使用多面体表示来描述当前函数（这对于轻松实现凸优化技术很有用），σ1和 σ 1 2 σ_1^2 σ12可以被视为不同的参数。 在下面的推导中，这里使用 σ 1 2 = w σ_1^2=w σ12=w。
方程（19）可以重写为
结合系统(9)-(13)，图3中的PI速度控制器以及控制定律(16)和(17)，可以得到阻抗控制插入系统 x ˙ i n s = A c l i n s ( σ 3 ) x i n s x˙{ins}=A{clins}(σ3)x_{ins} x˙ins=Aclins(σ3)xins的闭环状态空间表示
矩阵(21)与 Acl(σ1)具有类似的形式。 我们可以再次使用上述稳定性测试过程。 在这里，稳定性测试的目的是确定在预定义力控制器增益 kf下的阻抗控制器增益范围。
测试结果表明，闭环系统可以实现具有虚拟刚度和虚拟阻尼的稳定性。
实验
A. 注射原型的实验
注射原型的实时控制是通过嵌入了Beckhoff TwinCAT 3软件系统的工业计算机（C6030，Beckhoff Automation GmbH & Co. KG，德国）实现的。 以太网用于控制自动化技术（EtherCAT）被用作计算机和硬件之间的通信接口，即编码器（其输出与Beckhoff EL6022和EtherCAT终端连接）、力传感器和电机驱动器。 控制器被编程为MATLAB/Simulink模型和TwinCAT PLC。控制的采样频率设置为1 kHz。图6显示了注射原型的照片和控制实施的设置。
图6. 系统设置的概述。
1）瞬时RCM机制的测试：RCM机制系统的控制基于单环反馈框架。在这个控制框架的测试中，我们对期望的小车位置施加了一个方波信号x1d。测量到的位置x1和x2用于评估RCM机制的准确性。实验结果如图7(a)和(b)所示。 在理想情况下，x1−γ1x2=0。 然而，由于不确定性和额外的传动部件（谐波驱动器），无法实现这种理想的同步运动。如图7©所示，最大位置差小于0.2毫米。这个结果显示了良好的同步性。此外，当小车移回零点时，位置差返回到零。 这表明所提出的RCM机制系统具有良好的重复定位精度。
2）关于瞬时RCM机制控制的三种案例研究：本节的第一个控制测试研究了具有两个小车之间较小速度比的RCM系统的操作。在测试之前，原型上安装了一个较大直径的滑轮[图8（a）]。这导致较大直径滑轮的速度比较较小直径滑轮慢。 在我们的情况下，滑轮之间的速度比（由直径比确定）为1.46。因此，理论速度比为34.2。在之前的实验中使用了单环反馈框架来测试当前的RCM系统。 实验结果如图8(b)所示。 可以看到，两个小车可以被满意地控制。 测得的速度比的平均值为33.8，接近上述理论值。值得注意的是，较小的速度比可以使RCM点接近尖端[见(2)]。 从结果可以推断出，所提出的控制方法适用于其他瞬时RCM原型。小车位置x1被用作控制输出，而x2（通过谐波驱动传输的小车）是测量输出。 与此相反，本节的第二个实验以x2作为控制输出，x1作为测量输出。 值得注意的是，PD位置控制器的增益被重新设计。 实验结果如图9所示。 在图9(b)和7(b)中可以观察到x2的位置波动，这证实了波动不是由于所提出的控制框架引起的。 从图9的实验结果可以看出，两个托架的位置可以被满意地调节，因为RCM机制的精度可以得到保持。
图7. RCM机制位置方波响应实验。(a) 托架的位置跟踪。(b) 通过谐波传动传输的托架位置。（c）两个托架之间的位置差异。(d) 两个托架之间的速度比。(e) 理论值和实验值之间的位置差异。
图8.（a）具有较大直径滑轮的RCM原型的照片。 （b）小车的位置。
图9. 实施单环反馈框架控制传输谐波驱动的小车的实验。（a)小车的位置。(b) 使用谐波驱动传输的小车的位置跟踪。
图10. 开环控制实验. (a) 托架的位置. (b) 两个托架之间的位置差异。
图11. 插入单元系统的力和阻抗控制环实验. (a) 力步响应. (b) 测量的刚度。
图12. 阻抗控制插入过程的实验。（a）使用低虚拟刚度进行插入深度跟踪。 （b）使用高虚拟刚度进行插入深度跟踪。
图13. (a) 相机和投影仪的放置照片。(b) 结构光系统的示意图。 © 重建棋盘的分析。
B. 机器人注射
在本节中，原型机安装在机器人臂的末端执行器上，移动到适当的位置和插入方向，然后插入单元对硅胶模特的脸部进行阻抗控制插入。 这个应用程序模拟了自主机器人面部注射。为了指导原型的定位，我们建立了一个机器人相机系统，主要包括结构光系统的设计，面部模型的三维重建和手眼校准。
- 结构光系统：为了重建3D物体，实现了一个结构光系统。系统的硬件包括数字光处理(DLP)投影仪(DLP LightCrafter 4500，912×1140分辨率，美国德州仪器公司)和互补金属氧化物半导体相机(BFS-U3-32S4C-C，2048×1536分辨率，每秒118帧，美国Teledyne FLIR LLC) [见图13(a)]。重建过程以C++代码的形式在Visual Studio 2015中编程。软件框架涉及照片采集(投影仪触发相机进行同步)，编码(一系列正负灰度编码模式)，解码，校准(相机和投影仪以及立体校准[32]，[33])和重建。使用[34]的算法计算真实物体的3D坐标。在这一步骤中，使用从校准中估计的相机和投影仪的内部和外部参数以及相机和投影仪之间的相应像素点(从解码中获得)。实现面部模型的3D重建的系统设置如图13(b)所示。
值得注意的是，白光源仅在面部模型中实现。 对于与人体测试对象的实验，我们将使用近红外光来减少由白光引起的眼部不适。 基于近红外光和白光的三维重建具有相似的原理和准确性。 因此，所提出的系统可以轻松应用于近红外光的应用。
在结构光系统的准确性评估中，平板物体（即棋盘）被用作重建的目标。 准确性分析是通过eomagic Wrap 2017软件进行的[图13©]。 在这里，准确性通过点云与拟合平面之间的距离差异（平坦度）来描述。 结果显示标准差为0.4毫米。
图14。 机器人注射系统的实验流程。(a) 用于手眼标定的棋盘姿势的照片。 (b) 人脸模型的三维重建结果。 © 机器人注射实验的照片。
- 机器人注射实验：提出的相机硬件固定在机器人臂（LBR-iiwa 7 R800，德国KUKA AG）外部。机器人臂使用KUKA Sunrise.OS中的Java进行编程。为了实现机器人注射的引导，我们确定了机器人基座坐标系和相机坐标系之间的转换矩阵。工作原理如下。针插入点的位置 c p p ∈ R 3 ^cp_p ∈ R^3 cpp∈R3相对于相机坐标可以通过以下关系描述相对于机器人基座坐标 b p p ∈ R 3 ^bp_p ∈ R^3 bpp∈R3：
其中，齐次变换矩阵 b H c ∈ S E ( 3 ) ^bH_c ∈SE(3) bHc∈SE(3)由手眼标定过程确定。这个过程涉及计算和实验。 在实验之前，将棋盘安装在机器人臂的末端执行器上。
下一个任务是根据所需的插入点计算机器人臂的末端执行器姿态。 计算通过以下公式进行：
其中，齐次变换矩阵 p H t ∈ S E ( 3 ) ^pH_t ∈ SE(3) pHt∈SE(3)表示针尖姿态相对于插入点的变换， e H t ∈ S E ( 3 ) ^eH_t ∈SE(3) eHt∈SE(3)表示针尖姿态相对于末端执行器的变换。
表格II 机器人面部注射的实验参数
图15. 面部区域的注射角度。
我们根据图14(b)中重建的点云选择插入点 然后，根据(22)，可以确定向量 bpp 同一图中的标记区域（用黑色突出显示），即颧骨和鱼尾纹，通常用于面部注射我们将插入方向定义为医生操作的方式此外，为了成功插入，插入点需要在针轴上 这两个条件确定了矩阵 p H t ^pH_t pHt末端执行器的坐标系标记在机械臂的末端法兰处。组装注射原型和末端执行器后，可以确定矩阵 e H t ^eH_t eHt，因为机械尺寸是已知的 当RCM原型的小车移动时，针尖的姿态会发生变化 因此，矩阵 e H t ( α ) ^eH_t(α) eHt(α)取决于倾斜角α的值 值得注意的是，原型的长度会影响机械臂末端执行器的初始定位的简便性 因此，需要为α设置一个相对较大的值，并且针尖与插入点 p H t ^pH_t pHt之间的初始距离相对较小这种方法可以增加初始定位的简便性，即增加注射的成功率。
图16. 注射中有无倾斜运动的实验比较。 （a）倾斜角度的值大约为零。（b）提供了倾斜角度。
Conclusion
本文基于瞬时RCM机制的注射原型已成功设计。 同时，RCM原型和插入单元的控制可以确保提议的性能和稳定性。 所提出的控制方案和模型可用于其他原型。 所提出的结构光系统已实现了适当的3D重建精度。 原型和机器人臂的协调可以在面部模型的子区域内进行插入。 未来的研究将专注于图像引导方法，以提高针头姿态的控制精度。
Reference
[1] Liu, L., Chen, W., Chen, Z., Zhou, W., Wei, R., & Liu, Y. (2023). Realization and Control of Robotic Injection Prototype with Instantaneous Remote Center of Motion Mechanism. IEEE Transactions on Biomedical Engineering.