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

技术特征：

1.一种面向医疗机器人的肢体语言检测与跟踪系统设计方法，其特征在于，包括如下步骤：

(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显示模块；姿态解算是根据三轴加速度、三轴角速度到四元数的理论计算完成的，用四元数表示姿态矩阵：

$C_b^R=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]$

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

(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模型的第一个顶点都绑定在了不同的骨骼之上，不同骨骼对同一个顶点的影响程度使用权重表示，而当相应骨骼进行变换时，受其影响的顶点也会根据权重做出相应的响应。通过控制骨骼的运动，来实现使模型运动的目的。