[發明專利]多傳感器手柄控制器混合追蹤方法及裝置

有效

• 申請號: 202010625647.9
• 申請日: 2020-07-01
• 公開（公告）號: CN111949123B
• 公開（公告）日: 2023-08-08
• 發明（設計）人: 吳濤;李漢振
• 申請（專利權）人: 青島小鳥看看科技有限公司
• 主分類號: G06F3/01
• 分類號: G06F3/01;G06F3/0346;G06F3/0487;G06T7/277;G06T7/73;G06T7/80
• 代理公司: 北京風雅頌專利代理有限公司 11403
• 代理人: 郭曼

1.一種多傳感器手柄控制器混合追蹤方法，其特征在于，包括：

獲取手柄控制器的追蹤數據，所述追蹤數據包括光學追蹤數據、電磁追蹤數據和慣性導航數據；

根據所述光學追蹤數據實時獲取手柄控制器上的光學圖案標記點相對于頭盔顯示器上的第一相機的6DoF數據和速度信息；

利用各個相機之間的標定參數，將第二相機、第三相機和第四相機的6DoF數據均轉換為以第一相機為原點的坐標系下的6DoF數據，記為Poseoptic；

通過頭盔顯示器上內置的電磁信號接收器和第一相機的標定參數，將手柄控制器的電磁6DoF數據轉換為以第一相機為原點的坐標系下的6DoF數據，記為PoseEM；

將數據Poseoptic和數據PoseEM融合為新的位置數據，記為PoseOEM；

利用新的位置數據創建擴展卡爾曼濾波迭代策略的狀態轉移模型和觀測模型，對所述光學追蹤數據、所述電磁追蹤數據和所述慣性導航數據進行擴展卡爾曼濾波融合；

根據擴展卡爾曼濾波迭代策略獲取手柄控制器在空間中的位置和姿態信息。

2.根據權利要求1所述的多傳感器手柄控制器混合追蹤方法，其特征在于，將數據Poseoptic和數據PoseEM融合為新的位置數據的步驟包括：

分別判斷數據Poseoptic和數據PoseEM在x軸、y軸和z軸上的角度相差是否小于第一設定閾值，位移相差是否小于第二設定閾值；

若角度相差小于第一設定閾值，且位移相差小于第二設定閾值，則分別取x軸方向、y軸方向和z軸方向的數據Poseoptic和數據PoseEM的平均值，作為數據PoseOEM；

若角度相差不小于第一設定閾值，和/或位移相差不小于第二設定閾值，則分別對x軸方向、y軸方向和z軸方向的數據Poseoptic和數據PoseEM做加權平均，得到數據PoseOEM。

3.根據權利要求1所述的多傳感器手柄控制器混合追蹤方法，其特征在于，擴展卡爾曼濾波迭代策略的狀態轉移模型如下式所示：

其中，和分別是第k時刻和k-1時刻的優化迭代后的x軸方向上的位移分量，和分別是第k時刻和k-1時刻的優化迭代后的y軸方向上的位移分量，和分別是第k時刻和k-1時刻的優化迭代后的z軸方向上的位移分量；

和分別是第k時刻和k-1時刻的光學追蹤數據中x軸方向上的速度位移分量，和分別是第k時刻和k-1時刻的光學追蹤數據中y軸方向上的速度位移分量，和分別是第k時刻和k-1時刻的光學追蹤數據中z軸方向上的速度位移分量；

分別表示第k時刻去掉重力方向之后的重力加速度傳感器在x軸、y軸和z軸的運動數據；

ΔT表示k時刻和k-1時刻的時間差。

4.根據權利要求3所述的多傳感器手柄控制器混合追蹤方法，其特征在于，擴展卡爾曼濾波迭代策略的觀測模型如下式所示：

其中，分別是按照擴展卡爾曼濾波迭代優化生成的x方向、y方向和z方向的位置數據；

k表示時刻；

分別是融合數據中的在k時刻的x，y，z三個軸方向上的位置移動數據。

5.根據權利要求1所述的多傳感器手柄控制器混合追蹤方法，其特征在于，所述方法還包括：設定如下式所示的過程噪聲方差矩陣：

其中，PNoiseCov表示過程噪聲方差矩陣，p_error表示位移噪聲誤差，v_error表示速度噪聲誤差。

6.根據權利要求1所述的多傳感器手柄控制器混合追蹤方法，其特征在于，所述方法還包括：設定如下式所示的測量噪聲方差矩陣：

其中，MNoiseCov表示測量噪聲方差矩陣，M_error表示測量噪聲誤差。