基于STFT的微创手术机器人运动控制系统设计

昆，晓峰，郝

(山西大学 物理电子工程学院，太原 030006)

0 引言

传统的微创手术[1]在实际应用环境中仍然存在一些问题，以外科医生的经验来看，由于手部疲劳而引起的抖动会对手术造成不良影响，而医用成像射线则对医生有害。但是最重要的问题是，由于医生的手直接操作手术端工具，采用了类似于杠杆原理的方法，会导致手的动作方向与实际操作方向相反，并不符合正常的生理习惯，造成了安全隐患，容易引起手术事故。为此，相关学者对微创手术机器人运动控制系统做出了研究。文献[2]提出基于FPGA控制器的机器人运动控制系统设计。采用集中计算和分布式控制结构，利用FPGA控制器作为从站控制4台无刷直流伺服电机，实现位置采集和速度反馈。该系统的实时性较好，但运行稳定性有待提升。

文献[3]提出基于主从控制的微创手术机器人运动控制系统设计。构建运动模型，提出内窥镜坐标系下的主从运动控制算法，引入主从式位姿配准功能，实现运动控制。该方法的控制精度较好，但实时性较差。

针对以上问题，设计了基于STFT的微创手术机器人运动控制系统。系统采用主从臂控制方式，提高了微创手术效率，并能有效地消除医生抖动。

1 系统总体架构

从本质上说，运动控制器是一种具有数据接收和调度、算法和控制策略执行功能的智能设备，是实现机器人各种控制算法的核心场所。其先进程度对机器人是否能按照预定的路线正确操作有着重要的影响。另外，它对机器人整个操作过程的稳定性也有着不容忽视的影响，这也是设计中不可低估的指标之一。该系统的主要功能是接收主机的指令或数据，分析指令，对数据进行运动学或动态计算，从而获得各关节的活动量。整理后的数据送至联合控制单元，控制电机严格按要求运转。

运动控制系统原理图充分考虑了操作者在操作过程中对触觉和力度信息的采集和传递、机械手工作空间环境信息的传递以及控制指令的传递。

如图1所示，单片机在接收到主机运动量等数据后，驱动机械手，各关节信息采集单元分别采集关节的位置、速度、加速度、力度等信息，经单片机过滤后通过蓝牙模块传送到上位机。

图1 运动控制系统原理

2 系统硬件结构设计

通信链路设计中，主控机通过蓝牙模块与上位机进行通信，副控机通过 CAN总线与主控机进行通信。图2中显示了系统硬件结构。

图2 系统硬件结构

通信链路设计中，主控机通过蓝牙模块与上位机进行通信，副控机通过 CAN总线与主控机进行通信，图2中显示了系统硬件结构。通过深入调查，决定采用“金软介绍”蓝牙模块[4]。嵌入模块提供 SCI通讯接口，可与 PC机、 MCU进行灵活的通讯[5]。由于蓝牙模块采用 SCI通信方法和硬件流程控制，而 SCI接口核心处理器不具备这样的功能，所以用普通 I/O口模拟 SCI硬件流程控制方法实现单片机间的正常通信是非常必要的[6-7]。另外，MC9S12XDP512单片机集成了丰富的易罐资源，共有5个易罐可供选择。具体开发过程中，根据 CAN模块的使用情况，只需增加 CAN收发器8X250就可以完成两台或两台以上的通讯[8-9]。

2.1 时钟基准电路

时钟电路是关键，该电路表面上看起来简单，但经常由于线路板或元件的选择而导致时钟电路不振动或不稳定，产生高频噪声，从而使整个系统处于潜在的安全状态[10]。该系统采用16 MHz晶体与处理器外管脚 EXTAL和 XTAL端口直接相连，实现简单、振荡稳定。设计时，将时钟基准电路设计为并联谐振电路。在这种情况下，单片机PE7端口与高电平连接，晶体振荡器的选择范围为4～16 MHz[11-12]。在PE7单片机的端口连接在 RB反馈电阻为2 MΩ～0 Ω的较低层时，晶体振荡器的选择范围可扩展为2～40 MHz。此外，当选用16 MHz的外部晶体振荡器时，软件还可以设置内部锁相环以提高母线时钟频率。因为单片机的功率可达3.3 V和5 V，而内部 CPU需要2.5 V的电压才能通过调压器进行转换，因此在设计供电电路时，选用LM2576作为电压转换芯片。采用宽带输入电压(7～40 V)方式，可调节输出电压为1.23～37 V，电流输出容量为3 A，能满足单片机供电要求，扩大了电压输入范围。在系统调试阶段，时钟参考电路尤其重要，因为当程序被干扰，出现意外情况时，只需手动按下复位键，就可以很容易地重新启动，使系统回到正常状态。时钟基准电路设计的目的是在程序失控时，便于调试。

2.2 运动控制器

运动控制器功能与人脑相似，它负责感知外部环境信息，按照内在逻辑进行计划与协调。所以运动控制器是整个控制系统的核心。如图3所示，运动控制器通常由以下部分组成。

图3 运动控制器

总线接口用来连接上位机，接口用来与外部信号交互，反馈输入用来接收电机执行机构的位置信号。通常情况下，控制器需要与伺服电机和步进电机的控制接口相兼容，因此需要预留模数转换(DAC)接口、数字脉宽调制(PWM)和脉冲输出接口。FPGA采用 DSP或 MCU作为控制核心，外围输入输出接口采用FPGA并行处理，运动控制器关键技术是总线技术、 FPGA技术和控制方法。

图4中显示了机器人的主从式操作臂。

图4 机器人主从操作臂

1)机器人主操作臂：

机器人主操作臂采用了冗余自由度设计，增加了第二个肘关节，使其适应不同病人。机械臂4个自由度均为活动关节，无源关节具有精确的位置传感器，主动关节有两个位置传感器，保证了其可靠性。主操手分为左、右两部分，分别对应于医生的左、右手；通过冗余控制，机器人操作臂可灵活地移动手臂；该仪器是由高精度马达作为整个操作的执行机构，实现精细控制，在手术过程中能够顺利完成切口与缝线是整个手术的关键。

2)机器人从操作臂：

机器人从操作臂直接进入病人体内，操作小手。小手的切割与缝合在手术过程中很重要，小手由4个自由度组成：腕部俯仰，腕部摆动，工具端(如剪刀、电动刀、夹子等)摆动，以及工具端上的开合。

微型仪器因体积小，采用异质设计，一端为电机，四端由线驱动器驱动。小手部是微创手术的执行机构，要求电机控制精度高。把收集到的机器人手臂各个关节的信息传送给计算机模块，从而得到真正运动学模型，从而获得手术端信息并将其传递到从控制器，通过逆运动学计算获得各关节的运动信息，最终由运动控制器根据运动信息完成对机械手各关节的马达控制。

3 系统功能设计

由于控制系统会受到外界环境影响，损失大量有用信号，因此，需使用基于STFT短时傅里叶变换方法来抑制外界干扰，并基于此设计运动控制程序。

3.1 基于STFT信息频域和时域转换干扰抑制方法

设置接收信号在频域上有N个样点M(0≤M≤N)样本点受到干扰，此时信号信噪比损失定义为：

LSNR=N/(N-M)

(1)

由公式(1)可知，接收信号的频域带宽越宽，信噪比损失就越大。因为此时考虑了 LFM干扰，所以可以根据 LFM干扰占用的频带宽度在一定的时间窗口内对外界环境进行抑制。针对这一问题，提出了基于短时傅立叶变换信息频域和时域变换的干扰抑制方法。

STFT短时间傅里叶变换是与傅里叶变换相联系的数学变换，用来确定时变信号局部区域的正弦波的频率和相位。STFT的基本概念如下：FFT是频域分析的基本工具，为实现时间局部化，在傅里叶分析的基本变换函数之前，先将一个有时间限制的函数乘上g(t)，之后将该组合函数用作分析函数，由此限制时频。

时频双限制变换式为：

(1)

公式(1)中，e-jwt通过时频双限可以对信号进行局部化分析，为其频限函数；g表示紧支集函数；f表示分析的信号。

当时间窗τ变化时，g在时间轴上也发生改变，信号进入被分析状态。时频双限制变换式表示分析信号在时间窗τ下，变化频率为ω的信号组成成分，等价于去除信号在分析时间窗τ附近的一个分析片断，此时的STFT短时间傅里叶变换直接是将分析信号在该时间窗下进行局部谱分析，由此确定信号频域和时域的信息，消除干扰，避免医生操作发生抖动现象。

3.2 运动控制程序设计

选择LM629运动控制专用集成芯片作为运动控制环节的核心，这种芯片外围结构简单，内建PID控制器，大大降低了电机运动控制的难度，因此运动控制程序的设计是与芯片相关联的。设计软件时，首先要检查芯片的“忙碌”状态。若状态标志的位置是“1”，LM629运动控制专用集成芯片将拒绝所有其它操作，直到LM629完全接收该信息，然后重新设置“busy”状态。所以必须先检查状态位的逻辑状态，然后再读写数据或命令，以判断芯片目前是否在“忙碌”。此外，芯片的每一个参数必须在正常运行前初始化。状态字的初始化形式为“OOH”。若晶片复位成功，则状态字在1.5 ms内变为84 H或 CAH。另外，硬件需要重置，以确保芯片正常工作。通过RSTI命令检查芯片复位是否成功，以确定是否成功。若重置成功，状态字节从'84 H'或' CAH'变为'80 H'或' COH'。另外，还需要再次执行上面的操作，检查重新设置是否成功。运动控制流程的设计见图5。

图5 运动控制流程

由于内建 PID控制器LM629凝固增量积分分离 PID算法，实时控制过程中，当达到积分模块设定的上限时，积分就会停止，造成控制量的饱和区过长。因此，运动控制的效果大大降低。所以，当软件设置 PID参数时，要加入一套判别器。如果控制器超出了执行器的最大限制，就停止对不断增加的积分项的累积计算，并根据偏差的正负来判断偏差是否会增加或减少系统的超调。

若增加，则只允许模块进行积分降阶运算，此方法可以有效地防止控制变量由于积累而长期处于饱和状态，通过LFIL指令实现对模块中 PID算法参数的动态修改。前两个字在LFIL指令之后用来决定微分采样时间和具体 PID参数，每个参数按照积分上限的顺序被传递。此外，根据微创手术机器人的具体情况，即在机器人运动过程中，各关节的姿态及活动量实时变化。所以，在改变关节姿态时，程序应立即进行 PID参数计算，并实时计算出各种姿态下的最佳 PID控制参数。另外，由于参数计算的滞后，机器人手臂末端的运动也会偏离预定轨迹，从而产生较大的偏差。对位置、速度数据进行同步参数修改和采集，可以将参数的计算传递给CPU，通过 Xgate进行实时数据采集，这样，两个进程就可以同步了。

4 实验分析

为了验证基于STFT的微创手术机器人运动控制系统设计合理性，进行实验验证分析。

4.1 实验平台

实验平台设置如图6所示。

图6 实验平台

通过人工控制发送预定义位置资料，在收到数据后，上位机将解算值发送给控制器，控制器根据收到的各关节运动量对PID参数进行初始化。通过蓝牙模块，主机和控制单元之间建立正常的通讯关系，手部控制器重复200次相同的操作，发出相同的预定义轨迹，记录下机器人端的实际位置。实验过程中，该机械手运行平稳，无明显振动，并有光滑的末端轨迹，与预定的轨迹基本一致。

4.2 操作条件设置

第一个关节角初始设置为90°，第二个关节角初始设置为45°，第三个关节角初始长度为10 cm，以2 cm /s速度在腹腔镜控制臂上沿水平线扫描病变。微创手术机器人如图7所示。

图7 微创手术机器人

主从式操作臂由1:1比例控制，主从式操作臂的初始位置为零。

4.3 主从臂控制验证分析

分别采用基于杠杆原理控制系统和基于STFT控制系统跟踪机器人主从臂运动轨迹，结果如图8所示。

由图8(a)可知，期望主臂运动轨迹的终点坐标为(10 mm，-35 mm，45 mm)，整体呈现下降后又上升的趋势，使用基于STFT控制系统与期望主臂运动轨迹的终点坐标一致，而使用基于杠杆原理控制系统与期望主臂运动轨迹终点不一致，其终点坐标为(5 mm，-30 mm，65 mm)。

由图8(b)可终点知，期望从臂运动轨迹的终点坐标为(18 mm，-45 mm，25 mm)，使用基于STFT控制系统与期望从臂运动轨迹的终点坐标一致，而使用基于杠杆原理控制系统与期望从臂运动轨迹终点不一致，其终点坐标为(8 mm，-32 mm，60 mm)。

通过上述验证结果可知，使用基于STFT控制系统控制效果更好。

5 结束语

根据微创手术机器人的实际特点，分析了运动控制系统的具体要求，对运动控制系统的相关技术进行了研究，提出了系统的总体设计方案。通过对CAN总线网络分布式控制的研究，确定了控制系统总体结构。利用蓝牙无线通讯技术，实现了运动控制系统与主机的通讯。这样可以减少医生与病人之间的直接接触，避免相互间的交叉感染。模块化设计，便于系统调试和升级，并充分考虑控制系统可靠性。实验结果表明，该系统能满足运动控制器的实时稳定控制要求。

图8 两种系统机器人主从臂运动轨迹跟踪结果对比分析