[发明专利]一种基于惯性测量装置的力反馈设备手柄的姿态测量方法

权利要求书

1.一种基于惯性测量装置的力反馈设备手柄的姿态测量方法，其特征在于，包括以下六个步骤：

步骤(1)、将惯性测量装置和力反馈设备手柄固连：首先将惯性测量装置和力反馈设备手柄固连，实现两者的同步运动；然后调整惯性测量装置的连接方式，使得惯性测量装置的坐标系的x/y/z轴与力反馈设备的坐标系的x/y/z轴朝向相同；惯性测量装置测量得到东北天坐标系下的欧拉角，用惯性测量装置测得的欧拉角表示力反馈设备手柄的姿态时的坐标系旋转顺序定义为ZYX，表示力反馈设备手柄先绕Z轴转，再绕Y轴转，再绕X轴转；

步骤(2)、上位机获取姿态信息：惯性测量装置连接到上位机，通信方式采用串行通信；

步骤(3)、基于多线程的姿态处理：力觉渲染操作启动后，上位机开启力觉渲染计算线程进行反馈力的计算；确定惯性测量装置连接正常后，上位机开启姿态信息处理线程，对惯性测量装置传输的欧拉角进行姿态处理，以满足力觉渲染计算的精度要求和频率要求；上位机建立不同线程间的通信，确保处理完成的姿态信息可以发送至力觉渲染计算线程，进行反馈力的计算；

步骤(4)、基于姿态匹配的姿态校准：当获取惯性测量装置测得的欧拉角后，对姿态信息进行校准；设定力反馈设备手柄的初始校准状态，通过测量得到该状态下力反馈设备手柄的方位角，求出标准初始姿态矩阵；同时测得该状态下惯性测量装置传入的欧拉角，求出实际初始姿态矩阵；通过两个矩阵间的矩阵运算，求得姿态校准矩阵，对后续传入的欧拉角进行左乘处理，将惯性测量装置的欧拉角变换到力反馈设备的坐标系；

步骤(5)、基于姿态信息插值的姿态频率校准：针对校准后的姿态信息，在下次姿态更新前，根据欧拉角姿态插值的原理，通过前两次的姿态信息预测下次循环的姿态信息，提高姿态测量的频率，满足力觉渲染姿态信息预设的刷新频率的要求；

步骤(6)、构建虚拟力觉交互场景，屏蔽掉力反馈设备自身的姿态信息；通过步骤(1)-(5)对惯性测量装置测得的姿态信息进行优化，得到力反馈设备手柄的姿态角；将姿态角发送到力觉渲染计算线程，配合力反馈设备测得的力反馈设备手柄的位置信息进行力觉计算，最后通过力反馈设备向用户输出反馈力。

2.根据权利要求1所述的基于惯性测量装置的力反馈设备手柄的姿态测量方法，其特征在于：所述步骤(2)中，上位机收到惯性测量装置发送的数据后，先将高位字节转换为一个有符号的short类型的数据，然后再移位，获得惯性测量装置测得的欧拉角。

3.根据权利要求2所述的基于惯性测量装置的力反馈设备手柄的姿态测量方法，其特征在于：所述步骤(2)中，惯性测量装置测得的数据按照16进制的方式向上位机发送，每个数据分低字节和高字节依次传送，二者组合成一个有符号的short类型；上位机收到数据后，计算欧拉角的公式如下所示：

angle_x＝((wxH＜＜8)|wxL)/32768*2000

angle_y＝((wyH＜＜8)|wyL)/32768*2000

angle_z＝((wzH＜＜8)|wzL)/32768*2000

其中angle_x俯仰、angle_y偏航和angle_z横滚为惯性测量装置的欧拉角，wxH、wyH、wzH、wxL、wyL和wzL为惯性测量装置向上位机发送的角度数据，每个数据分低字节和高字节依次传送，二者组合成一个有符号的short类型的数据。

4.根据权利要求1所述的基于惯性测量装置的力反馈设备手柄的姿态测量方法，其特征在于：所述步骤(3)中，上位机在力觉计算线程外开启新的姿态处理线程对姿态信息进行处理，同时保持姿态处理线程和力觉计算线程间的通信；在不阻塞力觉计算的同时，确保处理后的姿态信息能够用于反馈力的计算。