[發(fā)明專利]剛體姿態(tài)追蹤方法及其裝置、設備、存儲介質(zhì)

在審

• 申請?zhí)枺?/td>: 202010793587.1
• 申請日: 2020-08-07
• 公開（公告）號: CN112084458A
• 公開（公告）日: 2020-12-15
• 發(fā)明（設計）人: 黃少光;許秋子
• 申請（專利權(quán)）人: 深圳市瑞立視多媒體科技有限公司
• 主分類號: G06F17/15
• 分類號: G06F17/15;G06F17/16;G01C9/00;G01C21/16
• 代理公司: 深圳市鼎泰正和知識產(chǎn)權(quán)代理事務所(普通合伙) 44555
• 代理人: 繆太清

1.一種剛體姿態(tài)追蹤方法，其特征在于，包括：

根據(jù)預設的函數(shù)模型分別建立卡爾曼預測四元數(shù)模型f_kQ、卡爾曼預測位移模型f_kP、濾波預測四元數(shù)模型f_kQ、濾波預測位移模型f_εP，其中，所述函數(shù)模型包括速度和加速度狀態(tài)變量；

獲取剛體初始幀的姿態(tài)四元數(shù)和位移，得到初始化后的模型f_kQ、f_kP、f_εQ、f_εP；

根據(jù)所述初始化后的模型f_kQ、f_kP、f_εQ、f_εP分別預測剛體當前幀的姿態(tài)四元數(shù)和位移，得到姿態(tài)四元數(shù)預測值和位移預測值；

將所述姿態(tài)四元數(shù)預測值和位移預測值以及獲取的剛體當前幀姿態(tài)四元數(shù)和位移真實值引入二范數(shù)函數(shù)中，計算得到剛體當前幀的姿態(tài)信息。

2.如權(quán)利要求1所述的剛體姿態(tài)追蹤方法，其特征在于，所述根據(jù)預設的函數(shù)模型分別建立卡爾曼預測四元數(shù)模型f_kQ、卡爾曼預測位移模型f_kP、濾波預測四元數(shù)模型f_kQ、濾波預測位移模型f_εP，其中，所述函數(shù)模型包括速度和加速度狀態(tài)變量，包括：

將速度和加速度加入卡爾曼濾波的狀態(tài)維度因子，結(jié)合濾波器原理，得到所述預設的函數(shù)模型；

根據(jù)所述預設的函數(shù)模型分別建立卡爾曼預測四元數(shù)模型f_kQ、卡爾曼預測位移模型f_kP、濾波預測四元數(shù)模型f_εQ、濾波預測位移模型f_εP。

3.如權(quán)利要求2所述的剛體姿態(tài)追蹤方法，其特征在于，所述將速度和加速度加入卡爾曼濾波的狀態(tài)維度因子包括：

選用kalman濾波模型，得到輸出剛體姿態(tài)四元數(shù)和位移預測值的kalman濾波公式：

其中v為速度，a為加速度，Q為協(xié)方差矩陣，F(xiàn)為狀態(tài)轉(zhuǎn)移矩陣。

4.如權(quán)利要求2所述的剛體姿態(tài)追蹤方法，其特征在于，所述濾波器原理包括：

令剛體姿態(tài)四元數(shù)和位移的預測值在T_i時刻為X_i，濾波后的預測值為則：

其中，α∈[0，1]代表平滑因子。

5.如權(quán)利要求1所述的剛體姿態(tài)追蹤方法，其特征在于，所述卡爾曼預測四元數(shù)模型f_kQ和卡爾曼預測位移模型f_kP均包括：狀態(tài)參數(shù)維度和測量變量，所述f_kQ的狀態(tài)參數(shù)維度、測量變量個數(shù)分別為12、4，所述f_kP的狀態(tài)參數(shù)維度、測量變量個數(shù)分別為9、3。

6.如權(quán)利要求1-5任一項所述的剛體姿態(tài)追蹤方法，其特征在于，所述將所述姿態(tài)四元數(shù)預測值和位移預測值以及獲取的剛體當前幀姿態(tài)四元數(shù)和位移真實值引入二范數(shù)函數(shù)中，計算得到剛體當前幀的姿態(tài)信息包括：

將所述姿態(tài)四元數(shù)預測值Qk_frame、Qε_frame和位移預測值Pk_frame、Pε_frame，以及獲取的剛體當前幀姿態(tài)四元數(shù)真實值Qt_frame和位移真實值Pt_frame引入二范數(shù)函數(shù)中，以進行比較，求得剛體當前幀的姿態(tài)四元數(shù)Q_frame和位移P_frame，所述二范數(shù)函數(shù)為：

distQ＝||Qk_frame-Qt_frame||

distP＝||Pk_frame-Pt_frame||

其中ThQ、ThP分別為一固定閾值。

7.如權(quán)利要求6所述的剛體姿態(tài)追蹤方法，其特征在于，所述ThQ、ThP根據(jù)調(diào)試結(jié)果得出，其取值范圍為0-0.01。