一种面向医疗机器人的肢体语言检测与跟踪系统设计方法与流程

本发明属于机器视觉与人工智能领域和肢体动作识别领域，尤其涉及一种面向医疗机器人的肢体语言检测与跟踪系统设计方法。

背景技术：

随着国家医疗卫生事业的蓬勃发展和自动化学科的不断进步，以自动化理论为核心的医疗辅助器械层出不穷。近几年来，一些医疗器具，如体温计、血糖计、灭菌灯、手术刀、高电位治疗仪等，都在不断进步，向着智能化、小型化和便携化发展，为医生和患者都提供了极大的便利。

在新兴的医疗器械中，用在手术中进行辅助的自动化机械占很大比例。由于多数外科手术时间长，强度大，精度要求高，对主刀医生的技术和体力都有非常高的要求，而利用自动控制理论制造的相关器械，在不影响精度的同时，可以长时间工作，能够比较完美的解决类似问题，减轻医生的负担，也为病患的康复添了一份保障。这是医学进步的一部分，同时也是自动化学科实际应用的经典案例。

针对需要用到内窥镜的外科手术，传统的方法常常需要用一只手来控制内窥镜的情况，这就限制了医生另一只操纵手术刀的手的动作幅度和精度，增加了医生体力的消耗，在一定程度上增加了相关手术的风险，减少了患者被成功治疗的机会。本发明基于人机交互技术和自动控制理论，通过用传感器采集医生的脚部的位姿信息，并识别某些特定的姿态变化及其按一定顺序的组合，完成对内窥镜的控制，在保证手术能够顺利完成的前提下，减轻了主刀医师的负担，实现了我们方便医生，造福患者的目标。

技术实现要素：

本发明的目的在于提供一种面向医疗机器人的肢体语言检测与跟踪系统设计方法。

本发明的目的是这样实现的：

(1)设计系统的整体结构；

整体系统分为两个大的功能模块，分别是下位机模块和上位机模块；

下位机模块由主控芯片、姿态传感器和通信元件组成；在上电以后，主控芯片对各个部件进行初始化，之后以固定的频率从姿态传感器中读取数据，在经过简单的平滑之后，通过通信模块，以既定的通信协议，向上位机进行发送；

上位机模块由通信模块，姿态解算模块，曲线显示模块，3D模型创建模块，骨骼创建模块，骨骼变换模块，3D显示及更新模块组成；启动时使用3D模型创建模块对3D模型的顶点、贴图、内骨骼拟合信息进行初始化，导入要用的渲染器，并创建好绘图的环境；之后通过通信模块与下位机进行通信，每当接收到下位机传来的姿态信息之后，将其通过既定接口送给姿态解算模块；姿态解算模块通过相关原理，将下位机传来的原始数据解算成所需的姿态信息，并进一步将数据传给曲线显示模块和骨骼变换模块；曲线显示模块将数据用实时数据的形式显示出来；骨骼变换模块根据姿态信息，深度优先遍历骨骼树，对骨骼所要做的姿态变换进行计算，之后将骨骼位姿信息传给3D更新模块；3D更新模块根据收到的骨骼位姿信息，最终将结果以3D模型的形式显示在电脑的屏幕上；

(2)设计系统的下位机模块；对姿态传感器、主控芯片、通信器件进行选型，并分别进行下位机硬件设计和下位机软件设计；

姿态传感器采用六轴跟踪传感器MPU-6050，主控芯片采用STM32F103C8T6单片机，通信模块使用无线串口进行通信；

其中MPU-6050固定在医生的脚上，采集姿态信息，单片机STM32F103C8T6负责MPU-6050的初始化工作，并从MPU-6050中读取数据，通过无线串口发送给上位机；使用STM32F103C8T6的SWD接口下载器的VCC和GND接口完成供电，MPU-6050和无线串口连接在STM32F103C8T6上，引入TPS76833供电；

系统下位机的软件设计包括STM32F103C8T6与MPU-6050的初始化，MPU-6050的数据的读取，通过无线串口将数据发送至上位机；

STM32F103C8T6的初始化主要完成四个步骤：

(2.1)初始化系统的SysTick定时器，使之启动之后将每1m秒触发一次中断；

(2.2)串口的初始化：启动GIPOA和USART1的设备时钟，将GPIOA9和GPIOA10设为复用推挽输出模式、浮空输入模式；配置USART1将其波特率设为9600，数据位为8位，1位停止位，无奇偶校验，无硬件控制，使用数据接收和数据发送；

(2.3)将C标准库中的printf()的输出重定向到USART1；

(2.4)I2C通信的初始化：启动GPIOB和I2C1的时钟，GPIOB6和GPIOB7设为复用漏输出模式I2C1的工作模式设为I2C，低电平数据变化时SCL线的Duty为0.33，开启消息应答，7位地址，设置自身地址为0xB；

MPU-6050的初始化是对内部的寄存器进行设置，其中寄存器PWR_MGMT_1设置为0x00，寄存器SAMPLE RATE DIVIDER设为0x07，寄存器CONFIG为0x06，寄存器ACCELEROMETER CONFIGURATION设为0x01，寄存器GYROSCOPE CONFIGURATION设为0x18；读取MPU-6050的WHO_AM_I寄存器，如果其值为0x68则正常启动，否则重新初始化；在正确配置MPU-6050并检查无误后，开始读取MPU-6050的数据；

读取MPU-6050的数据采用轮询的办法，每100ms读取一次加速度计和角速度计的数据，其中MPU-6050测量得到加速度数据和角速度数据各占用了六个连续的寄存器，温度传感器的测量值占用了两个寄存器；

通过无线串口将下位机数据发送至上位机是通过制定相关的通信协议实现的；

包括加速度的传输：第一个字节为0x55表示数据包的开头；第二个字节为0x51，表示这个数据包中存放的是加速度数据；第三个字节为AxL，表示加速度计X轴数据的低8位；第四个字节为AxH，表示速度计X轴数据的高8位；第五个字节为AyL，表示加速度计Y轴数据的低8位；第六个字节为AyH，表示加速度计Y轴数据的高8位；第七个字节为AzL，表示加速度计Z轴数据的低8位；第八个字节为AzH，表示加速度计Z轴数据的高8位；第九位为TL，为温度信息的低8位；第十位为TH，为温度信息的高8位；第十一位为，校验位，其值为前面所有字节之和对0x100的余数；角速度的传输：第一个字节为0x55表示数据包的开头；第二个字节为0x52，表示这个数据包中存放的是角速度数据；第三个字节为WxL，表示角速度计X轴数据的低8位；第四个字节为WxH，表示速度计X轴数据的高8位；第五个字节为WyL，表示角速度计Y轴数据的低8位；第六个字节为WyH，表示角速度计Y轴数据的高8位；第七个字节为WzL，表示角速度计Z轴数据的低8位；第八个字节为WzH，表示角速度计Z轴数据的高8位；第九位为TL，为温度信息的低8位；第十位为TH，为温度信息的高8位；第十一位为，校验位，其值为前面所有字节之和对0x100的余数；

(3)设计系统的上位机模块：接收下位机发送来的信息、对接收到的信息进行姿态解算、显示曲线、3D绘图；

串口通信模块借助于Qt5的SerialPort库来完成，负责与下位机通信，通过监听串口发送来的数据，按照既定通信协议对数据进行解析，将数据发送给姿态解算模块进行姿态解算；串口模块由两部分组成：第一部分SerialDialog类，负责UI中对串口的设置和人机交互，界面如图6所示；第二部分是SerialThread类，负责在另外一个线程中实时监听串口，并根据通信协议对其进行解析；串口数据解析采用的方法是建立一个类似数据结构中的队列的缓冲区，每次接收到数据就将其添加到缓冲区的尾部，之后从缓冲区的头部开始寻找通信协议的开头，找到开头之后，判断从此处到缓冲区的结尾的长度是否大于等于通信协议的长度，如果满足这个条件，就从缓冲区中从协议开头开始，取出通信协议规定长度的字节数组，并把缓冲区开头到取出数组结尾位置的数据删除，之后重复这个动作，直到缓冲区中没有通信协义开头，或其后的长度小于协议的长度，跳出，等待下一次接收到数据；

姿态解算模块是上位机程序中的灵魂，将从串口模块获得的数据进行姿态解算，并继续将数据传递给曲线显示模块和3D显示模块；姿态解算是根据三轴加速度、三轴角速度到四元数的理论计算完成的，用四元数表示姿态矩阵：

求解姿态即求解姿态矩阵，等价为四元数的更新，而四元数的更新可通过三轴角速度实现，具体步骤为：

(3.1)获取三轴角速度，并用gx、gy、gz分别代表角速度的三轴，采用积分的方法获得姿态，根据此前的四元数对重力方向进行估计；

(3.2)选用陀螺仪获取加速度计的值，并用ax、ay、az分别代表三轴加速度。在进行姿态解算之前，对其进行规范化；

(3.3)利用互补滤波算法融合角速度和加速度值并求取误差，将此误差经过比例和积分环节之后补偿到角速度的测量值当中；

(3.4)一阶龙格库塔法更新四元数；

上位机曲线显示模块使用的是Qt的第三方开源库QCustomPlot，曲线函数的初始化和数据更新函数位于主界面的类MainWindow中；系统的上位机程序主界面中显示三轴加速度曲线、三轴角速度曲线、姿态角曲线和四元数曲线四个曲线，曲线的绘制是与主界面UI的更新一同进行的；

3D显示模块负责3D模块的载入，骨骼变换的计算及应用，模型显示的更新；其中模型载入使用的是Assimp开源库导入3D模型的顶点、贴图和骨骼信息，使用OpenGL对导入的信息进行渲染；本系统在建模时使用Blender建模软件构建了33块骨骼，并将其变化信息存储在数据结构树中；3D模型更新的方法为：将3D模型的第一个顶点都绑定在了不同的骨骼之上，不同骨骼对同一个顶点的影响程度使用权重表示，而当相应骨骼进行变换时，受其影响的顶点也会根据权重做出相应的响应。通过控制骨骼的运动，来实现使模型运动的目的。

本发明的有益效果在于：

(1)采用医生脚部的姿态变化控制内窥镜的方位变化充分考虑到了手术时医生一手控制手术刀一手控制内窥镜对手术造成的影响，最大程度提高了手术的成功率，减轻了医生的手术负担。

(2)考虑直接通过脚部动作控制内窥镜会增加医生的不便，对手术造成一定的影响，设计上位机3D模型直接复现脚部动作和曲线变化反映姿态变化，进而控制内窥镜，进一步降低了手术的复杂度，提高了成功率。

附图说明

图1系统整体结构框图；

图2系统下位机硬件结构设计图；

图3系统下位机硬件实物图；

图4系统下位机软件流程图；

图5系统上位机总体结构；

图6串口相关窗口；

图7串口执行流程；

图8曲线显示界面；

图9 3D显示界面；

图10骨骼树。

具体实施方式

下面结合附图举例对本发明做更详细地描述：

本方法实现了人体姿态控制医疗机械进行手术，解决了在需要用到内窥镜的外科手术中医生需一手控制手术刀，一手控制内窥镜的问题。这样不仅可以减轻医生的手术负担，同时也提高了手术的成功率。

本发明的设计是通过以下方案实现的：

(1)设计系统的整体结构。

(2)设计系统的下位机模块。对姿态传感器、主控芯片、通信器件进行选型，并分别进行下位机硬件设计和下位机软件设计。

(3)设计系统的上位机模块，需要进行的工作有：接收下位机发送来的信息、对接收到的信息进行姿态解算、显示曲线、3D绘图。

采用医生脚部的姿态变化控制内窥镜的方位变化充分考虑到了手术时医生一手控制手术刀一手控制内窥镜对手术造成的影响，最大程度提高了手术的成功率，减轻了医生的手术负担。

考虑直接通过脚部动作控制内窥镜会增加医生的不便，对手术造成一定的影响，设计上位机3D模型直接复现脚部动作和曲线变化反映姿态变化，进而控制内窥镜，进一步降低了手术的复杂度，提高了成功率。

面向医疗机器人的肢体语言检测与跟踪系统设计，主要包括两大模块：下位机模块和上位机模块。其特点是：针对需要内窥镜的外科手术，采用医生脚部的姿态变化代替手动操作控制内窥镜的方位；为使系统不影响整个手术过程，脚部姿态的变化采用上位机3D模型和曲线变化显示。

本发明提出的一种面向医疗机器人的肢体语言检测与跟踪系统具体包括以下几个步骤。

步骤一：设计系统的整体结构。

系统的整体结构框图如图1所示。整体系统从功能上分为两个大的功能模块组成，分别是下位机模块和上位机模块。

下位机由主控芯片、姿态传感器和通信元件组成。主要的工作方式是在上电以后，主控芯片对各个部件进行初始化，之后以固定的频率从姿态传感器中读取数据，在经过简单的平滑之后，通过通信模块，以既定的通信协议，向上位机进行发送。

上位机从功能上分为通信模块，姿态解算模块，曲线显示模块，3D模型创建模块，骨骼创建模块，骨骼变换模块，3D显示及更新模块等组成。主要的工作方式是：启动时使用3D模型创建模块对3D模型的顶点、贴图、内骨骼拟合等信息进行初始化，导入要用的渲染器，并创建好绘图的环境；之后通过通信模块与下位机进行通信，每当接收到下位机传来的姿态信息之后，将其通过既定接口送给姿态解算模块；姿态解算模块通过相关原理，将下位机传来的原始数据解算成所需的姿态信息，并进一步将数据传给曲线显示模块和骨骼变换模块；曲线显示模块将数据用实时数据的形式显示出来；骨骼变换模块根据姿态信息，深度优先遍历骨骼树，对骨骼所要做的姿态变换进行计算，之后将骨骼位姿信息传给3D更新模块；3D更新模块根据收到的骨骼位姿信息，最终将结果以3D模型的形式显示在电脑的屏幕上。

步骤二：设计系统的下位机模块。对姿态传感器、主控芯片、通信器件进行选型，并分别进行下位机硬件设计和下位机软件设计。

考虑到实验需求和经济问题，姿态传感器采用低功耗的六轴跟踪传感器MPU-6050，主控芯片采用功能丰富且结构简单的STM32F103C8T6单片机，通信模块使用无线串口进行通信。

系统的下位机硬件结构设计图如图2所示，硬件实物图如图3所示。其中MPU-6050固定在医生的脚上，采集姿态信息，单片机STM32F103C8T6负责MPU-6050的初始化工作，并从MPU-6050中读取数据，在简单处理后，通过无线串口发送给上位机。系统分别采用两种不同的供电方式应用于开发过程中的调试和实际应用。在实际开发过程中由于系统硬件结构简单、消耗功率小以及电流基本没有太大波动，简单使用STM32F103C8T6的SWD接口下载器的VCC和GND接口即可完成供电，MPU-6050和无线串口连接在STM32F103C8T6上，不需要额外增加供电模块。这样的连接方式，简单方便，工作量基本为零。但在实际应用中，考虑到下位机要固定在医生的脚上，随医生的脚一同运动，不能使用有线的供电方式，因此引入TPS76833供电。

系统下位机的软件设计流程图如图4所示，主要包括STM32F103C8T6与MPU-6050的初始化，MPU-6050的数据的读取，通过无线串口将数据发送至上位机等步骤。

STM32F103C8T6的初始化主要完成四个步骤：

(1)初始化系统的SysTick定时器，使之启动之后将每1m秒触发一次中断。

(2)串口的初始化：启动GIPOA和USART1(即GPIOA9和GPIOA10复用为串口功能时对应的串口)的设备时钟，将GPIOA9和GPIOA10设为复用推挽输出模式、浮空输入模式；配置USART1将其波特率设为9600，数据位为8位，1位停止位，无奇偶校验，无硬件控制，使用数据接收和数据发送。

(3)将C标准库中的printf()的输出重定向到USART1。

(4)I2C通信的初始化：启动GPIOB和I2C1的时钟，GPIOB6和GPIOB7设为复用漏输出模式I2C1的工作模式设为I2C，低电平数据变化时SCL线的Duty为0.33，开启消息应答，7位地址，设置自身地址为0xB。

MPU-6050的初始化主要是对内部的寄存器进行设置，其中寄存器PWR_MGMT_1设置为0x00，寄存器SAMPLE RATE DIVIDER设为0x07，寄存器CONFIG为0x06，寄存器ACCELEROMETER CONFIGURATION设为0x01，寄存器GYROSCOPE CONFIGURATION设为0x18。接着读取MPU-6050的WHO_AM_I寄存器，如果其值为0x68则正常启动，否则说明出现了问题，需重新初始化。在正确配置MPU-6050并检查无误后，即可开始读取MPU-6050的数据。

读取MPU-6050的数据可以采用中断法或是轮询法，为实现方便，系统采用的是轮询的办法，每100ms读取一次加速度计和角速度计的数据，其中MPU-6050测量得到加速度数据和角速度数据各占用了六个连续的寄存器，温度传感器的测量值占用了两个寄存器。

通过无线串口将下位机数据发送至上位机是通过制定相关的通信协议实现的，具体描述为：(1)加速度的传输：第一个字节为0x55表示数据包的开头；第二个字节为0x51，表示这个数据包中存放的是加速度数据；第三个字节为AxL，表示加速度计X轴数据的低8位；第四个字节为AxH，表示速度计X轴数据的高8位；第五个字节为AyL，表示加速度计Y轴数据的低8位；第六个字节为AyH，表示加速度计Y轴数据的高8位；第七个字节为AzL，表示加速度计Z轴数据的低8位；第八个字节为AzH，表示加速度计Z轴数据的高8位；第九位为TL，为温度信息的低8位；第十位为TH，为温度信息的高8位；第十一位为，校验位，其值为前面所有字节之和对0x100的余数。(2)角速度的传输：第一个字节为0x55表示数据包的开头；第二个字节为0x52，表示这个数据包中存放的是角速度数据；第三个字节为WxL，表示角速度计X轴数据的低8位；第四个字节为WxH，表示速度计X轴数据的高8位；第五个字节为WyL，表示角速度计Y轴数据的低8位；第六个字节为WyH，表示角速度计Y轴数据的高8位；第七个字节为WzL，表示角速度计Z轴数据的低8位；第八个字节为WzH，表示角速度计Z轴数据的高8位；第九位为TL，为温度信息的低8位；第十位为TH，为温度信息的高8位；第十一位为，校验位，其值为前面所有字节之和对0x100的余数。

步骤三：设计系统的上位机模块，需要进行的工作有：接收下位机发送来的信息、对接收到的信息进行姿态解算、显示曲线、3D绘图。

上位机系统在整体系统的实现中占有至关重要的地位，本质上说，整个下位机系统相对于上位机而言，只是充当了一个传感器的角色。准确的对脚部姿态进行检测与跟踪主要是通过上位机的姿态解算模块、曲线绘制和3D模型实现的。因此，上位机的设计需具有模块耦合度低、性能好、方便易用的特点。

系统的上位机系统主要使用C++语言完成，使用Qt作为UI解决方案，OpenGL作为3D绘图支持，为方便编程，还使用了C++的STL标准库，以及QCustomPlot和Assimp等开源软件。为提高程序的运行效率，上位机多处使用了多线程技术。

上位机设计的总体结构如图5所示。

图中串口通信模块主要借助于Qt5的SerialPort库来完成，负责与下位机通信，通过监听串口发送来的数据，按照既定通信协议对数据进行解析，将数据发送给姿态解算模块进行姿态解算。根据需求，串口模块由两部分组成：第一部分SerialDialog类，负责UI中对串口的设置和人机交互，界面如图6所示；第二部分是SerialThread类，主要负责在另外一个线程中实时监听串口，并根据通信协议对其进行解析。串口数据解析采用的方法是建立一个类似数据结构中的队列的缓冲区，每次接收到数据就将其添加到缓冲区的尾部，之后从缓冲区的头部开始寻找通信协议的开头，找到开头之后，判断从此处到缓冲区的结尾的长度是否大于等于通信协议的长度，如果满足这个条件，就从缓冲区中从协议开头开始，取出通信协议规定长度的字节数组，并把缓冲区开头到取出数组结尾位置的数据删除，之后重复这个动作，直到缓冲区中没有通信协义开头，或其后的长度小于协议的长度，跳出，等待下一次接收到数据。整体的串口流程如图7所示。

姿态解算模块是上位机程序中的灵魂，将从串口模块获得的数据进行姿态解算，并继续将数据传递给曲线显示模块和3D显示模块。姿态解算是根据三轴加速度、三轴角速度到四元数的理论计算完成的，用四元数表示姿态矩阵如下所示：

求解姿态即求解姿态矩阵，又可以等价为四元数的更新，而四元数的更新可通过三轴角速度实现，具体步骤为：

(1)获取三轴角速度，并用gx、gy、gz分别代表角速度的三轴，采用积分的方法获得姿态，根据此前的四元数对重力方向进行估计；

(2)选用陀螺仪获取加速度计的值，并用ax、ay、az分别代表三轴加速度。在进行姿态解算之前，对其进行规范化；

(3)利用互补滤波算法融合角速度和加速度值并求取误差，将此误差经过比例和积分环节之后补偿到角速度的测量值当中；

(4)一阶龙格库塔法更新四元数。方程如下：

其中，

上位机曲线显示模块使用的是Qt的第三方开源库QCustomPlot，曲线函数的初始化和数据更新函数位于主界面的类MainWindow中。如图8所示，系统的上位机程序主界面中将会显示三轴加速度曲线、三轴角速度曲线、姿态角曲线和四元数曲线四个曲线，曲线的绘制是与主界面UI的更新一同进行的，因此并不需要另加线程。

3D显示模块上位机系统中最为复杂的一部分，主要负责3D模块的载入，骨骼变换的计算及应用，模型显示的更新。其中模型载入使用的是Assimp开源库导入3D模型的顶点、贴图和骨骼信息，使用OpenGL对导入的信息进行渲染，得到3D模型如图9。本系统在建模时使用Blender建模软件构建了33块骨骼，并将其变化信息存储在数据结构树中，如图10所示。3D模型更新的主要方法为：将3D模型的第一个顶点都绑定在了不同的骨骼之上，不同骨骼对同一个顶点的影响程度使用权重表示，而当相应骨骼进行变换时，受其影响的顶点也会根据权重做出相应的响应。这样，就可以通过控制骨骼的运动，来实现使模型运动的目的。本发明通过设计下位机完成了医生脚部姿态信息的采集，通过设计上位机实现了脚部姿态信息的准确检测与跟踪，并用3D模型实时复现医生脚部动作，为后续机械精确控制内窥镜做准备，也为生物医学的研究开启一个新篇章。