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