本發(fā)明涉及姿態(tài)估計(jì),特別涉及一種基于陀螺儀-碼盤的轉(zhuǎn)臺方位姿態(tài)估計(jì)方法和裝置。
背景技術(shù):
1、相關(guān)部門主要通過安裝在各關(guān)鍵要道的轉(zhuǎn)臺設(shè)備對目標(biāo)物進(jìn)行監(jiān)測、跟蹤、取證,但現(xiàn)有裝備單一傳感器的轉(zhuǎn)臺設(shè)備面對大風(fēng)、強(qiáng)降雨等復(fù)雜環(huán)境的適應(yīng)性較差,導(dǎo)致難以快速準(zhǔn)確估計(jì)轉(zhuǎn)臺方位姿態(tài)。針對這種轉(zhuǎn)臺方位姿態(tài)估計(jì),碼盤雖然沒有累計(jì)誤差,可以提供長期穩(wěn)定相對準(zhǔn)確的測量結(jié)果,但受限于采樣率,高頻響應(yīng)差,可能出現(xiàn)噪聲或延遲,無法及時(shí)反映轉(zhuǎn)臺方位姿態(tài)的快速變化。
2、因此,亟需提供一種新的轉(zhuǎn)臺方位姿態(tài)估計(jì)方法。
技術(shù)實(shí)現(xiàn)思路
1、為了解決傳統(tǒng)使用單一碼盤傳感器在復(fù)雜工況下難以快速準(zhǔn)確估計(jì)轉(zhuǎn)臺方位姿態(tài)的問題,本發(fā)明實(shí)施例提供了一種基于陀螺儀-碼盤的轉(zhuǎn)臺方位姿態(tài)估計(jì)方法和裝置。
2、一方面,提供了一種基于陀螺儀-碼盤的轉(zhuǎn)臺方位姿態(tài)估計(jì)方法,所述方法包括:
3、針對每一時(shí)刻的轉(zhuǎn)臺方位姿態(tài)估計(jì),均執(zhí)行:
4、獲取當(dāng)前時(shí)刻陀螺儀反饋的轉(zhuǎn)臺方位角速度測量值和碼盤反饋的轉(zhuǎn)臺方位角度測量值,以構(gòu)建當(dāng)前時(shí)刻的方位姿態(tài)測量矩陣;
5、基于當(dāng)前時(shí)刻的方位姿態(tài)測量矩陣、針對當(dāng)前時(shí)刻的方位姿態(tài)先驗(yàn)估計(jì)值以及上一時(shí)刻更新調(diào)整后的過程噪聲協(xié)方差矩陣和觀測噪聲協(xié)方差矩陣,利用卡爾曼濾波算法融合所述陀螺儀和所述碼盤反饋的測量值,得到當(dāng)前時(shí)刻的轉(zhuǎn)臺方位姿態(tài)估計(jì)值;其中,方位姿態(tài)包括方位角速度和方位角度;
6、基于當(dāng)前時(shí)刻的方位姿態(tài)測量矩陣和針對當(dāng)前時(shí)刻的方位姿態(tài)先驗(yàn)估計(jì)值計(jì)算殘差,以基于所述殘差自適應(yīng)更新調(diào)整所述過程噪聲協(xié)方差矩陣和所述觀測噪聲協(xié)方差矩陣,以迭代進(jìn)行下一時(shí)刻的轉(zhuǎn)臺方位姿態(tài)估計(jì)。
7、另一方面,提供了一種基于說明書任一方法實(shí)施例所述步驟的基于陀螺儀-碼盤的轉(zhuǎn)臺方位姿態(tài)估計(jì)裝置,裝置包括:
8、獲取單元,針對每一時(shí)刻的轉(zhuǎn)臺方位姿態(tài)估計(jì),均用于執(zhí)行:獲取當(dāng)前時(shí)刻陀螺儀反饋的轉(zhuǎn)臺方位角速度測量值和碼盤反饋的轉(zhuǎn)臺方位角度測量值,以構(gòu)建當(dāng)前時(shí)刻的方位姿態(tài)測量矩陣;
9、融合單元,用于基于當(dāng)前時(shí)刻的方位姿態(tài)測量矩陣、針對當(dāng)前時(shí)刻的方位姿態(tài)先驗(yàn)估計(jì)值以及上一時(shí)刻更新調(diào)整后的過程噪聲協(xié)方差矩陣和觀測噪聲協(xié)方差矩陣,利用卡爾曼濾波算法融合所述陀螺儀和所述碼盤反饋的測量值,得到當(dāng)前時(shí)刻的轉(zhuǎn)臺方位姿態(tài)估計(jì)值;其中,方位姿態(tài)包括方位角速度和方位角度;
10、更新單元,用于基于當(dāng)前時(shí)刻的方位姿態(tài)測量矩陣和針對當(dāng)前時(shí)刻的方位姿態(tài)先驗(yàn)估計(jì)值計(jì)算殘差,以基于所述殘差自適應(yīng)更新調(diào)整所述過程噪聲協(xié)方差矩陣和所述觀測噪聲協(xié)方差矩陣,以迭代進(jìn)行下一時(shí)刻的轉(zhuǎn)臺方位姿態(tài)估計(jì)。
11、另一方面,提供了一種計(jì)算機(jī)設(shè)備,所述計(jì)算機(jī)設(shè)備包括存儲器和處理器,所述存儲器用于存放計(jì)算機(jī)程序,所述處理器用于執(zhí)行所述存儲器上所存放的計(jì)算機(jī)程序,以實(shí)現(xiàn)上述所述方法的步驟。
12、另一方面,提供了一種計(jì)算機(jī)可讀存儲介質(zhì),所述存儲介質(zhì)內(nèi)存儲有計(jì)算機(jī)程序,所述計(jì)算機(jī)程序被處理器執(zhí)行時(shí)實(shí)現(xiàn)上述所述方法的步驟。
13、另一方面,提供了一種計(jì)算機(jī)程序產(chǎn)品,包括計(jì)算機(jī)程序,所述計(jì)算機(jī)程序被處理器執(zhí)行時(shí)實(shí)現(xiàn)上述所述方法的步驟。
14、本發(fā)明提供的技術(shù)方案至少可以帶來以下有益效果:
15、通過將陀螺儀和碼盤兩種傳感器的優(yōu)勢相結(jié)合,通過卡爾曼濾波算法融合陀螺儀的動態(tài)預(yù)測和碼盤的絕對觀測,能夠使系統(tǒng)達(dá)到高頻依賴陀螺儀、低頻依賴碼盤的效果,同時(shí)基于殘差實(shí)時(shí)調(diào)整協(xié)方差權(quán)重,用以平衡系統(tǒng)噪聲的不確定性,能夠大大提高復(fù)雜工況下轉(zhuǎn)臺方位姿態(tài)估計(jì)的準(zhǔn)確性和實(shí)時(shí)性。
1.一種基于陀螺儀-碼盤的轉(zhuǎn)臺方位姿態(tài)估計(jì)方法,其特征在于,包括:
2.如權(quán)利要求1所述的方法,其特征在于,當(dāng)前時(shí)刻的轉(zhuǎn)臺方位姿態(tài)估計(jì)值是通過如下方式計(jì)算的:
3.如權(quán)利要求2所述的方法,其特征在于,所述預(yù)測協(xié)方差矩陣通過如下方式計(jì)算:
4.如權(quán)利要求2所述的方法,其特征在于,當(dāng)前時(shí)刻的轉(zhuǎn)臺方位姿態(tài)估計(jì)值是通過如下公式確定的:
5.如權(quán)利要求2所述的方法,其特征在于,所述基于當(dāng)前時(shí)刻的方位姿態(tài)測量矩陣和針對當(dāng)前時(shí)刻的方位姿態(tài)先驗(yàn)估計(jì)值計(jì)算殘差,以基于所述殘差自適應(yīng)更新調(diào)整所述過程噪聲協(xié)方差矩陣和所述觀測噪聲協(xié)方差矩陣,包括:
6.如權(quán)利要求5所述的方法,其特征在于,所述基于所述殘差協(xié)方差和當(dāng)前時(shí)刻的卡爾曼增益矩陣,更新調(diào)整過程噪聲協(xié)方差矩陣,并對所述過程噪聲協(xié)方差矩陣進(jìn)行數(shù)值對稱化正定化處理,包括:
7.一種基于陀螺儀-碼盤的轉(zhuǎn)臺方位姿態(tài)估計(jì)裝置,用于實(shí)現(xiàn)上述權(quán)利要求1-6任一所述方法的步驟,其特征在于,包括:
8.一種計(jì)算機(jī)設(shè)備,其特征在于,所述計(jì)算機(jī)設(shè)備包括存儲器和處理器,所述存儲器用于存放計(jì)算機(jī)程序,所述處理器用于執(zhí)行所述存儲器上所存放的計(jì)算機(jī)程序,以實(shí)現(xiàn)上述權(quán)利要求1-6任一所述方法的步驟。
9.一種計(jì)算機(jī)可讀存儲介質(zhì),其特征在于,所述存儲介質(zhì)內(nèi)存儲有計(jì)算機(jī)程序,所述計(jì)算機(jī)程序被處理器執(zhí)行時(shí)實(shí)現(xiàn)權(quán)利要求1-6任一所述的方法的步驟。
10.一種計(jì)算機(jī)程序產(chǎn)品,其特征在于,包括計(jì)算機(jī)程序,所述計(jì)算機(jī)程序被處理器執(zhí)行時(shí)實(shí)現(xiàn)權(quán)利要求1-6任一所述的方法的步驟。