[发明专利]双光球交互笔的刚体姿态确定方法、装置和计算机设备

权利要求书

1.一种双光球交互笔的刚体姿态确定方法，其特征在于，所述双光球交互笔的笔头设置有第一红外反光球，所述双光球交互笔的笔尾设置有第二红外反光球，所述双光球交互笔的笔体内装配有惯性传感器，所述方法包括：

初始时刻，在所述第一红外反光球和所述第二红外反光球的法向量上设置虚拟点，将所述虚拟点的位置设置为第一坐标位置点，其中，所述虚拟点的第一坐标位置点为(0、0、1)；

在所述双光球交互笔旋转时，根据初始时刻的所述第一坐标位置点、上一时刻所述双光球交互笔的刚体的旋转姿态和位置姿态计算得到当前时刻的所述虚拟点的第二坐标位置点；

根据当前时刻的所述第一红外反光球的坐标位置点、所述第二红外反光球的坐标位置点和所述第二坐标位置点，联合初始时刻所述第一红外反光球的坐标位置点、所述第二红外反光球的坐标位置点和所述第一坐标位置点，采用奇异值分解法求解得到当前时刻的所述双光球交互笔的刚体的旋转姿态和位置姿态。

2.根据权利要求1所述的方法，其特征在于，所述方法还包括：

初始时刻，将所述第一红外反光球和所述第二红外反光球在的平面作为坐标系的xoy平面，

确定所述第一红外反光球和所述第二红外反光球在所述xoy平面的中点位置，在所述中点位置设置z轴，所述法向量在所述z轴上；

根据所述xoy平面和所述z轴得到初始时刻的坐标系；

以所述坐标系为参考，确定初始时刻所述虚拟点的位置设置为第一坐标位置点；

以所述坐标系为参考，确定双光球交互笔旋转时各个时刻的双光球交互笔的刚体的旋转姿态和位置姿态。

3.根据权利要求2所述的方法，其特征在于，所述根据初始时刻的所述第一坐标位置点、上一时刻所述双光球交互笔的刚体的旋转姿态和位置姿态计算得到当前时刻的所述虚拟点的第二坐标位置点，包括：

获取所述初始时刻的所述第一坐标位置点的坐标向量与上一时刻所述双光球交互笔的刚体的旋转姿态的旋转矩阵进行乘积处理，得到第一向量；

将所述第一向量与上一时刻所述双光球交互笔的刚体的位置姿态的平移向量进行累加处理，得到第二向量；

根据所述第二向量确定当前时刻的所述虚拟点的第二坐标位置点。

4.根据权利要求3所述的方法，其特征在于，所述方法还包括：

以所述坐标系为参考，确定双光球交互笔旋转时各个时刻的双光球交互笔的刚体的旋转姿态的旋转矩阵以及位置姿态的平移向量。

5.根据权利要求1所述的方法，其特征在于，所述方法还包括：

初始时刻，采用奇异值分解法获得随机的旋转姿态以及随机的位置姿态，将所述随机的旋转姿态作为所述双光球交互笔的初始旋转姿态，将所述随机的位置姿态作为所述双光球交互笔的初始位置姿态。

6.根据权利要求1所述的方法，其特征在于，所述方法还包括：

初始时刻，读取所述双光球交互笔的所述双光球交互笔的刚体的实际旋转姿态和实际位置姿态，将所述实际旋转姿态作为所述双光球交互笔的初始旋转姿态，将所述实际位置姿态作为所述双光球交互笔的初始位置姿态。

7.一种双光球交互笔的刚体姿态确定装置，其特征在于，所述双光球交互笔的笔头设置有第一红外反光球，所述双光球交互笔的笔尾设置有第二红外反光球，所述双光球交互笔的笔体内装配有惯性传感器，所述装置包括：

设置模块，用于初始时刻，在所述第一红外反光球和所述第二红外反光球的法向量上设置虚拟点，将所述虚拟点的位置设置为第一坐标位置点，其中，所述虚拟点的第一坐标位置点为(0、0、1)；

计算模块，用于在所述双光球交互笔旋转时，根据初始时刻的所述第一坐标位置点、上一时刻所述双光球交互笔的刚体的旋转姿态和位置姿态计算得到当前时刻的所述虚拟点的第二坐标位置点；

求解模块，用于根据当前时刻的所述第一红外反光球的坐标位置点、所述第二红外反光球的第二坐标位置点和所述第二坐标位置点，联合初始时刻所述第一红外反光球的坐标位置点、所述第二红外反光球的坐标位置点和所述第一坐标位置点，采用奇异值分解法求解得到当前时刻的所述双光球交互笔的刚体的旋转姿态和位置姿态。