手术器械模型控制稳定性以及反馈控制精度

董帅帅， 辛绍杰

(上海电机学院 电气学院, 上海 201306)

微创手术器械穿过在体表切开的小切口探入体内[1]，在狭小的空间里完成多种动作，微创手术具有手术质量高、术后并发症少等优点[2]。本文在一种手术机器人结构的基础上采用DH建模方法完成对手术机器人的建模，本文研究的手术器械共有7个自由度，其中，3个自由度由医生的手腕部分提供，剩下4个自由度由手术器械的驱动电动机提供，驱动电动机选用步进电动机，步进电动机具有误差不累加的优点[3-4]。对于给定型号的步进电动机，其运转性能主要受驱动电路控制策略影响[5-7]。在建模的基础上完成手术器械的动作仿真、姿态稳定、力反馈仿真和精度分析。

手术场景中医生的手部抖动影响手术质量[8]，本文使用加速度传感器进行机器人姿态保持的研究，探索如何在设计上能够缓解手抖造成的误差。将抖动当作一种不规律的外部扰动，通过传感器采样数据比对计算补偿误差，研究采样对精度的影响。

Okamura等[9]提出的经验穿刺力模型，将穿刺过程的受力分为3个不同阶段，实验表明，单位长度所受摩擦力随进针速度增加而增加，而针尖处的峰值几乎不随进针速度而改变。常用的组织建模方法有质点弹簧模型和有限元模型[10]，两种模型根据适用的情形有各自的特点[11]。本文研究的是穿刺过程中力的变化以及手术器械前端的运动控制，为了得到这样的数据，根据人体组织的特性[12]，将组织看成一群细胞质点的集群，密度不一的质点集群对外表现出不一样的力学特性，用生成随机点的方法模拟同一组织内部小范围的稀疏变化。本文将穿刺前端抽象成半径0.5 cm的球体，球体在1 cm3空间内沿着某条轨迹运动，以过程中框住的点数来模拟受力F的大小，根据恒功率条件下负载特性[13]关系通过一定的函数映射来模拟转速。最后由神经网络对穿刺过程的特征点进行分类，根据分类结果进行运动控制，验证控制的精度。

1 DH模型建立

1.1 模型的建立

本文设计的手术器械如图1(a)所示，该器械可以实现4个自由度的运动，前端的旋转和摇摆以及执行器械的旋转和夹取。仿真中，手术器械还需添加手腕部分的3个自由度，一共是7个自由度。模型的尺寸做了简化处理，并不与实际尺寸相对应。使用Matlab机器人工具箱建立的手术器械模型如图1(b)所示。

图1 手术器械模型及建模图

DH建模方法是常用的机器人建模方法，将两个机械臂抽象成两个连杆，两个连杆之间通过齐次变换矩阵相乘获得坐标系之间的相对位置和姿态。如果机械臂有多组，可以使用齐次变换矩阵连续相乘的方法，得到末端相对于基坐标系的姿态和位置。

根据DH建模方法，建模原则：① 将两个关节轴方向都定为第i-1个坐标系和第i个坐标系的z轴；② 将两个关节轴之间公垂线方向定为第i个坐标系的x轴，而在公垂线第i个关节轴上的交点作为原点。同理，第i-1个坐标系的x轴和原点由之前两个坐标系公垂线共同确定；③ 在已经确定坐标系z轴和x轴的情况下，坐标系的y轴通过右手定则来确定。

根据标准DH参数描述法，第i-1个连杆坐标系相比于第i个坐标系的4阶变换矩阵如下：

(1)

式中：c为cos，s为sin；ai-1为连杆长度，即关节轴i-1和关节i之间公垂线的长度；αi-1为连杆转角，即第i-1个关节和第i个关节轴之间的夹角；di为连杆偏距，即沿两个相邻连杆公共轴线方向的距离；θi为关节角，即两个相邻连杆绕公共轴线旋转的夹角。

(2)

由式(2)可计算任意时刻机器人顶端相对于基坐标系的空间位置。模型建立之后，a、α是定值不会改变，θi的改变决定机器人关节姿态的变化，di的变化决定滑动的距离。

为了体现机器人在实际使用时的情况，建模的时候共用到了7个连杆模拟手持时的7个自由度的运动，在Matlab中建立的机器人模型主要参数如表1所示。其中，L1～L7为连杆编号，模型的L1为滑动关节，L2到L7为旋转关节；P、R为连杆类型，P为滑动关节，R为旋转关节；offset为初始偏移角度；q1～q7为Matlab机器人模型参数。

表1 手术器械建模参数表

1.2 手术器械空间轨迹的仿真

手术器械模型在进行直线、曲线、圆形轨迹运动时，模型前端的主要参数变化不同，如图2～4所示。

图2 直线轨迹手术器械运动参数变化

图3 曲线轨迹手术器械运动参数变化

图4 圆形轨迹手术器械运动参数变化

由图2～4可知，虽然手术器械模型前端的运动轨迹以及参与运动的主要连杆L关节的角度变化，但运动轨迹数据变化平滑，不存在突变，表明该模型建立恰当。

2 稳定补偿研究

为了方便问题研究，本文先对研究的内容做简单定义，使研究具有前提条件。

(1) 稳定的含义是手术器械的某个参考点相对于组织是位置不变的或者速度是恒定的。

(2) 医生进行手术时轻微的手抖是非自愿行为，手抖主要集中在手腕部分，医生可以较好地控制上臂，或者采取外加辅助设备的方式帮助医生获得手臂手肘关节的相对稳定性。

(3) 不开启姿态稳定功能时，默认医生可以在自主意志下完成手术动作，医生可以很好地把握手术节奏控制误差等情况。

(4) 开启姿态稳定功能，默认医生具有自主意志可以完成幅度较大的动作，而对于精细动作，医生不可以凭借自主意志完成动作，会出现不可预测的手抖造成误差，此时精细动作的完成交给手术器械，在这里从逻辑上界定了误差与自主意志动作的识别，由医生选择是否开启姿态稳定功能。

以上述说明为基础，定义3个坐标系：① 手术部位坐标系O。固定坐标系不可动，对应到实际情况是进行手术的部位。② 手肘坐标系O1。医生手持机器人的上臂关节，将陀螺仪以恰当的方式安装在手肘部位。③ 手术器械坐标系O2。将陀螺仪以恰当的方式安装在手术器械上。

手术部位坐标系虚拟空间的组成如图5所示，O、O1、O2为坐标轴方向一致的3个坐标系，分别代表以标准姿态(遵循右手准则建立的空间坐标系且z轴垂直于地平面)放置于手术部位、手术器械前端以及医生手肘部分的陀螺仪建立的虚拟坐标系。在静止条件下，陀螺仪和加速度计数据不会发生变化；在手术场景下，坐标系O(手术部位)是稳定不动的，手肘坐标系O1与手腕坐标系O2是医生进行手术动作时的运动坐标系。根据6轴陀螺仪可以提供3个轴向上的加速度以及3个轴向上的陀螺仪数据可以计算出坐标系O1与坐标系O2相对于坐标系O的运动方向以及距离。根据前提

图5 手术部位坐标系虚拟空间图

假设，在开启姿态稳定功能之后，坐标系O1相对于坐标系O2的运动就是手术前端的误差。

手术器械在空间中的运动可以分解为3个轴向上的运动，同样，误差动作在空间中也可以分为3个轴向上的运动。设陀螺仪和加速度计提供的6个参数，ax、ay、az表示由加速度计得到的坐标系在x、y、z3个方向上的加速度，θx、θy、θz表示由陀螺仪得到的机器人姿态相对于初始坐标系x、y、z轴逆时针方向上偏移的角度。

根据以上条件，在开启姿态稳定功能后，由手抖造成的手术器械误差实际上是坐标系O1相对于坐标系O2的位移。设根据陀螺仪和加速度计得到坐标系O1的运动参数为Ax、Ay、Az，坐标系O2的运动参数为Bx、By、Bz，加速度计从0时刻开始采样，得到间隔为t的t0，t1，…，tn采样序列，获得O1坐标系x坐标轴上的加速度序列Ax0，Ax1，…，Axn，获得O1坐标系y坐标轴上的加速度序列Ay0，Ay1，…，Ayn，获得的O1坐标系z坐标轴上的加速度序列Az0，Az1，…，Azn。

计算可得O1坐标系在0～tn时间内x轴的位移为

(3)

同理可得，在0～tn时间内y轴的位移为

(4)

在0～tn时间内z轴的位移为

(5)

同理可得，O2坐标系在0～tn时间内x轴的位移为

(6)

在0～tn时间内y轴的位移为

(7)

在0～tn时间内z轴的位移为

(8)

由式(3)～式(8)可计算O1坐标系相对于O2坐标系的位移为

Δxa=Δx1-Δx2

Δya=Δy1-Δy2

Δza=Δz1-Δz2

这3个量即机器人前端相对于医生手肘部的抖动误差，以上不计入O1坐标系相对于O2坐标系发生角度偏移的情况，如果加入O1坐标系相对于O2坐标系偏移角度，即

Δθx=θx1-θx2

Δθy=θy1-θy2

Δθz=θz1-θz2

造成的x轴上总误差为

[(1-cos Δθz)+sinθy]

(9)

造成的y轴上总误差为

[(1-cos Δθx)+sinθz]

(10)

造成的z轴上总误差为

[(1-cos Δθy)+sinθx]

(11)

式中：θx1、θx2、θy1、θy2、θz1、θz2均为O1、O2坐标系相对于标准姿态的逆时针旋转角度。

由于手术动作具有明显的非线性加减速过程，反映到加速度的数据上就是在0附近波动变化，故在3个轴向上选取sinX函数作为加速度描述曲线，X可以随意选择取值范围，每个轴向上的X可以取值不同，但是要保证3个轴向上X的宽度一致，因为在相同采样频率下X宽度不一致会导致采样点个数不一致。sinX函数穿越0值的次数表示手术动作的次数。

图6给出了采样频率为80、90和100 Hz计算出来的抖动误差，可以看出采样频率不同，抖动误差结果也不同，随着采样频率的升高，抖动误差越来越小。由于抖动误差是随机生成的，故每次的抖动误差曲线形状有所不同，但是都处在μm量级。

图6 采样频率提高对抖动误差的影响

图7给出了固定采样频率100 Hz条件下，O1相对于O2坐标系加速度误差范围从10%增加到20%的过程中抖动误差的变化。可以看出，随着加速度误差范围的增加，抖动误差也会波动上升，但此时相应提高采样频率可以有效降低误差波动范围增大带来的影响。图8给出了极端抖动情形下采样频率对抖动误差影响，加速度误差范围增加到50%，采样频率从100 Hz增加到300 Hz后，仍能维持ε在μm量级。

图7 加速度误差范围增加对抖动误差的影响

图8 极端抖动情形下采样频率增加对ε的影响

对比图7和图8两种情况可以得出提高采样频率能很好降低抖动误差的结论，同时，在误差极端增大情况下提高采样频率仍然可以维持整体的误差范围，对应于手术器械的补偿机构在x、y、z轴向上输出-Δx、-Δy、-Δz的位移即可完成对手术器械动作误差的补偿。

现有常用加速度陀螺仪传感器件，例如MPU6050默认采样频率可以达到100 Hz，通过对传感器的设置还可以获得更高的采样频率，与本文的仿真参数一致，可知该方法具有可行性。

3 组织建模以及力反馈控制仿真

压力值增加的程度和电动机值减小的程度这两者之间变化的剧烈程度构成力反馈的标准，这就产生了对两个参数进行数据归类划分的需求，需要进行分类的情形，如表2所示。

表2 力反馈参数界定逻辑表

表2对应到实际物理情况，情形1与情形4是不存在的，因为不符合电动机的负载特性，故只需要研究情形2与情形3。

使用Matlab在1 cm3的空间中随机生成200 000组(x，y，z)坐标，其中100 000个点的坐标在0≤x≤1，0≤y≤1，0≤z≤1随机分布，100 000个点的坐标在0≤x≤0.5，0≤y≤1，0≤z≤1随机分布，如图9所示，这样在1 cm3中会形成两个点密度不同的区域代表两种组织。

图9 组织仿真

计算可得点密度较高的区域为1 570.8 mm-3，取名组织A，神经网络对应的输出为(0，1)；点密度较低的区域为523.6 mm-3，取名组织B，神经网络对应的输出为(1，0)。球体内部点的个数为电动机的负载受到的阻力，电动机转速是指机器人末端在进行动作时对应的电动机转速。根据恒功率负载下电动机负载与转速的关系P=F×v，取P=7 854 MW。图10(a)所示为器械末端针尖在组织A中沿着(0.25，0.1，0.5)到(0.25，0.9，0.5)直线轨迹上80个采样点的转速与采集到的点数据以及映射为压力传感器探测值电动机的转速值。图10(b)所示为器械末端针尖在组织B中沿着(0.75，0.1，0.5)到(0.75，0.9，0.5)直线轨迹上80个采样点采集到的点数据以及映射为压力传感器探测值电动机的转速值。

图10 组织A、B特征点数据

将图10得到的两组数据送入两输入两输出具有3层隐含网络的BP神经网络进行训练，训练的结果作为组织标准模型，将(0.1，0.5，0.5)～(0.9，0.5，0.5)轨迹上的特征点数据送入标准模型进行仿真，仿真结果如图11所示。图11(a)为特征点的神经网络分类结果，图11(b)为分类前对特征点的归一化处理结果。

图11 特征点分类结果

运动控制器的位移随着神经网络的输出进行变化的过程，在t=10 s时，神经网络控制器判断穿刺到不同的组织，控制器采用PID控制使手术器械末端回归穿刺初始点，末端位移变化如图12所示，可知控制器可以根据神经网络的反馈进行力反馈动作。

图12 手术器械末端的位移

采用步进电动机细分技术[14-15]，对于二相混合式步进电动机整步转动时，其机械结构决定的固有步距角为

(12)

式中：z为转子齿数；c为相数，决定于通电方式。

对步距角为1.8°的步进电动机进行64细分[16]，最小步进角度可以达到0.028 125°，手术器械的前端长度取30 mm，末端执行器由步进电动机直接驱动的情形下，步进电动机接收一个脉冲末端执行器动作的弧长为14.7 μm，可以实现比较精确的位移控制。仿真出现的结果有3个相对孤立的特征点，对这些特征点的划分不同，控制器最大会有3步的误差，此时手术器械末端最大的弧度误差为44.1 μm。

4 结 论

本文以Matlab机器人工具箱建立了机器人模型，验证了机器人执行较复杂的动作时参考点的轨迹特性，提出了一种基于加速度采样的抖动补偿方法，验证该方法对误差的补偿作用。使用随机生成的数据模拟组织，并在组织中按照某种轨迹生成特征数据，这些数据作为神经网络的学习基础，通过对神经网络控制器的仿真验证了力反馈的正确性与可行性。