[發明專利]雙光球交互筆的剛體姿態確定方法、裝置和計算機設備

在審

• 申請號: 202110520992.0
• 申請日: 2021-01-28
• 公開（公告）號: CN113375662A
• 公開（公告）日: 2021-09-10
• 發明（設計）人: 洪智慧;許秋子
• 申請（專利權）人: 深圳市瑞立視多媒體科技有限公司
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C21/00
• 代理公司: 深圳市鼎泰正和知識產權代理事務所(普通合伙) 44555
• 代理人: 繆太清

1.一種雙光球交互筆的剛體姿態確定方法，其特征在于，所述雙光球交互筆的筆頭設置有第一紅外反光球，所述雙光球交互筆的筆尾設置有第二紅外反光球，所述雙光球交互筆的筆體內裝配有慣性傳感器，所述方法包括：

初始時刻，在所述第一紅外反光球和所述第二紅外反光球的法向量上設置虛擬點，將所述虛擬點的位置設置為第一坐標位置點；

在所述雙光球交互筆旋轉時，根據初始時刻的所述第一坐標位置點、上一時刻所述雙光球交互筆的剛體的旋轉姿態和位置姿態計算得到當前時刻的所述虛擬點的第二坐標位置點；其中，所述計算公式為：P_Ck＝P_C，0*R_k-1+T_k-1，P_C，o為所述虛擬點的初始時刻的第一坐標位置點，R_k-1為上一時刻所述雙光球交互筆的剛體的旋轉姿態，T_k-1為上一時刻所述雙光球交互筆的剛體的位置姿態，后續每個時刻，均根據所述公式變換得到所述虛擬點在當前時刻的第二坐標位置點P_Ck；

根據當前時刻的所述第一紅外反光球的坐標位置點、所述第二紅外反光球的坐標位置點和所述第二坐標位置點，聯合初始時刻所述第一紅外反光球的坐標位置點、所述第二紅外反光球的坐標位置點和所述第一坐標位置點，采用奇異值分解法求解得到當前時刻的所述雙光球交互筆的剛體的旋轉姿態和位置姿態。

2.根據權利要求1所述的方法，其特征在于，所述方法還包括：

初始時刻，將所述第一紅外反光球和所述第二紅外反光球在的平面作為坐標系的xoy平面，

確定所述第一紅外反光球和所述第二紅外反光球在所述xoy平面的中點位置，在所述中點位置設置z軸，所述法向量在所述z軸上；

根據所述xoy平面和所述z軸得到初始時刻的坐標系；

以所述坐標系為參考，確定初始時刻所述虛擬點的位置設置為第一坐標位置點；

以所述坐標系為參考，確定雙光球交互筆旋轉時各個時刻的雙光球交互筆的剛體的旋轉姿態和位置姿態。

3.根據權利要求2所述的方法，其特征在于，所述根據初始時刻的所述第一坐標位置點、上一時刻所述雙光球交互筆的剛體的旋轉姿態和位置姿態計算得到當前時刻的所述虛擬點的第二坐標位置點，包括：

獲取所述初始時刻的所述第一坐標位置點的坐標向量與上一時刻所述雙光球交互筆的剛體的旋轉姿態的旋轉矩陣進行乘積處理，得到第一向量；

將所述第一向量與上一時刻所述雙光球交互筆的剛體的位置姿態的平移向量進行累加處理，得到第二向量；

根據所述第二向量確定當前時刻的所述虛擬點的第二坐標位置點。

4.根據權利要求3所述的方法，其特征在于，所述方法還包括：

以所述坐標系為參考，確定雙光球交互筆旋轉時各個時刻的雙光球交互筆的剛體的旋轉姿態的旋轉矩陣以及位置姿態的平移向量。

5.根據權利要求1所述的方法，其特征在于，所述方法還包括：

初始時刻，采用奇異值分解法獲得隨機的旋轉姿態以及隨機的位置姿態，將所述隨機的旋轉姿態作為所述雙光球交互筆的初始旋轉姿態，將所述隨機的位置姿態作為所述雙光球交互筆的初始位置姿態。

6.根據權利要求1所述的方法，其特征在于，所述方法還包括：

初始時刻，讀取所述雙光球交互筆的所述雙光球交互筆的剛體的實際旋轉姿態和實際位置姿態，將所述實際旋轉姿態作為所述雙光球交互筆的初始旋轉姿態，將所述實際位置姿態作為所述雙光球交互筆的初始位置姿態。

7.根據權利要求1所述的方法，其特征在于，初始時刻，所述虛擬點的第一坐標位置點為(0、0、1)。