本發(fā)明屬于二頻機(jī)抖激光陀螺技術(shù)領(lǐng)域，尤其是一種二頻機(jī)抖激光陀螺抖動(dòng)信號(hào)抵消裝置。

背景技術(shù)：

在快速跟蹤場(chǎng)合，要求控制系統(tǒng)實(shí)時(shí)輸出角速率信息。對(duì)于機(jī)械抖動(dòng)激光陀螺，其輸出信號(hào)既包含陀螺敏感到的載體角運(yùn)動(dòng)信息，也包含機(jī)械抖動(dòng)角運(yùn)動(dòng)。為了得到載體角運(yùn)動(dòng)信息，必須對(duì)陀螺輸出信號(hào)進(jìn)行解調(diào)。典型的解調(diào)方案主要有高速采樣低通濾波和抖動(dòng)抵消方法，高速采樣低通濾波通常通過FIR低通濾波器完成，設(shè)計(jì)得到的FIR濾波器階數(shù)較高，濾波延時(shí)較大，不適合快速跟蹤場(chǎng)合應(yīng)用，抖動(dòng)抵消具有零延時(shí)的特點(diǎn)，可以實(shí)時(shí)輸出角速率信息，僅適用于穩(wěn)定控制等快速跟蹤場(chǎng)合。

現(xiàn)有抖動(dòng)剝除裝置的抖動(dòng)參考信號(hào)通過抖動(dòng)拾取電路獲取，這種方法需要設(shè)有前端調(diào)理電路，還要外加A/D轉(zhuǎn)換芯片，增加了硬件的復(fù)雜度。通常采用壓電陶瓷等拾取抖動(dòng)信號(hào)，壓電陶瓷具有遲滯特性、蠕變特性和高頻振動(dòng)非線性特性，由壓電陶瓷拾取的信號(hào)往往不是真正的抖動(dòng)信號(hào)，表現(xiàn)為信號(hào)具有非線性,從頻譜上看其中含有較高的諧波成分。

現(xiàn)有自鎖相方法通過陀螺獲取抖動(dòng)信號(hào)，陀螺輸出采樣頻率通過固定的采樣時(shí)鐘完成，無(wú)法實(shí)現(xiàn)正、余弦抖動(dòng)參考信號(hào)嚴(yán)格相差90°，因此，抖動(dòng)剝除效果較差。

現(xiàn)有自鎖相方法通過帶通IIR濾波器得到抖動(dòng)信號(hào)，當(dāng)陀螺工作在輸出幅值較大的場(chǎng)合時(shí)，設(shè)計(jì)出的帶通濾波器階數(shù)較高，易導(dǎo)致IIR輸出發(fā)散；另外，由于IIR濾波器具有反饋環(huán)節(jié)，無(wú)法實(shí)現(xiàn)定點(diǎn)運(yùn)算時(shí)的全精度截位，截位不當(dāng)也會(huì)導(dǎo)致IIR輸出發(fā)散，造成濾波失敗。

現(xiàn)有的抖動(dòng)剝除裝置將陀螺輸出經(jīng)差分運(yùn)算后直接經(jīng)自適應(yīng)抖動(dòng)剝除裝置進(jìn)行抖動(dòng)剝除，LMS算法帶有反饋環(huán)節(jié)，無(wú)法實(shí)現(xiàn)定點(diǎn)運(yùn)算的全精度數(shù)據(jù)截位，為了提高系統(tǒng)的抖動(dòng)剝除效果，通常需要較多的字長(zhǎng)表示運(yùn)算數(shù)據(jù)，占用較多的硬件邏輯資源，同時(shí)截位不當(dāng)會(huì)導(dǎo)致LMS輸出發(fā)散，抖動(dòng)剝除失效。

現(xiàn)有的抖動(dòng)剝除裝置均采用兩路相位相差90°的正、余弦抖動(dòng)參考信號(hào)用于抖動(dòng)信號(hào)估計(jì)和LMS權(quán)系數(shù)更新，得到的抖動(dòng)剝除效果不夠理想。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明的目的在于克服現(xiàn)有設(shè)計(jì)的不足，提供一種二頻機(jī)抖激光陀螺抖動(dòng)信號(hào)抵消裝置，解決如下問題：1、現(xiàn)有LMS自適應(yīng)抖動(dòng)剝除技術(shù)將陀螺輸出信號(hào)經(jīng)過帶通濾波器得到抖動(dòng)參考信號(hào)，在陀螺輸出的角增量信號(hào)幅值較大場(chǎng)合，為了得到精確的抖動(dòng)參考信號(hào)N_d，設(shè)計(jì)出的帶通IIR濾波器階數(shù)較高，高階的IIR濾波器計(jì)算量大，且在FPGA內(nèi)實(shí)現(xiàn)時(shí)，截位處理不當(dāng)，濾波器的輸出發(fā)散，導(dǎo)致濾波裝置無(wú)法正常工作；2、現(xiàn)有LMS自適應(yīng)抖動(dòng)剝除技術(shù)抖動(dòng)估計(jì)量精度較差，抖動(dòng)剝除后的載體角運(yùn)動(dòng)信息精度較差，通常需要增加低階低通FIR濾波器的階數(shù)來(lái)提高陀螺輸出載體角運(yùn)動(dòng)信息的精度，該發(fā)明重新設(shè)計(jì)了抖動(dòng)估計(jì)算法，在保證陀螺精度的同時(shí)，進(jìn)一步降低了低通FIR濾波器的階數(shù)，減小了陀螺數(shù)據(jù)處理的延時(shí)，提高了系統(tǒng)的實(shí)時(shí)性；3、經(jīng)陀螺獲取抖動(dòng)參考信號(hào)，不需要外部抖動(dòng)拾取電路獲取抖動(dòng)參考信號(hào)，簡(jiǎn)化了抖動(dòng)抵消的硬件設(shè)計(jì)，在陀螺獲取抖動(dòng)參考信號(hào)時(shí)，設(shè)計(jì)了低階IIR高通濾波器得到抖動(dòng)參考信號(hào)，并設(shè)計(jì)了數(shù)字時(shí)鐘轉(zhuǎn)換器(DCC)實(shí)現(xiàn)數(shù)字信號(hào)到時(shí)鐘信號(hào)的轉(zhuǎn)化，對(duì)得到的時(shí)鐘信號(hào)在FPGA內(nèi)4M倍頻，得到與抖動(dòng)信號(hào)同步且同頻率的采樣時(shí)鐘信號(hào)對(duì)陀螺輸出信號(hào)采樣，得到相位嚴(yán)格相差90°的抖動(dòng)參考信號(hào)，從而提高抖動(dòng)抵消的效果。

本發(fā)明解決其技術(shù)問題是采取以下技術(shù)方案實(shí)現(xiàn)的：

一種二頻機(jī)抖激光陀螺抖動(dòng)信號(hào)抵消裝置，包括陀螺數(shù)據(jù)采集模塊、差分運(yùn)算模塊、低階FIR濾波器模塊、求和運(yùn)算模塊、高頻時(shí)鐘模塊、低階IIR濾波器模塊、IIR高通濾波器模塊、第一數(shù)字移相器模塊、抖動(dòng)估計(jì)模塊、數(shù)字時(shí)鐘轉(zhuǎn)換器模塊、第二數(shù)字移相器模塊、帶通濾波器模塊、數(shù)字時(shí)鐘管理模塊、第三數(shù)字移相器模塊和LMS權(quán)系數(shù)更新模塊；所述陀螺數(shù)據(jù)采集模塊、低階IIR濾波器模塊、數(shù)字時(shí)鐘轉(zhuǎn)換器模塊、數(shù)字時(shí)鐘管理模塊依次相連接，該低階IIR濾波器模塊的另一輸入端連接高頻時(shí)鐘模塊，所述陀螺數(shù)據(jù)采集模塊按照數(shù)字時(shí)鐘管理模塊輸出的采用頻率對(duì)抖動(dòng)信號(hào)進(jìn)行采樣，采樣數(shù)據(jù)經(jīng)差分運(yùn)算模塊輸出至低階FIR濾波器模塊，該低階FIR濾波器模塊輸出端分別連接IIR高通濾波器模塊及求和運(yùn)算模塊，該IIR高通濾波器模塊的輸出端分別連接第一數(shù)字移相器模塊、抖動(dòng)估計(jì)模塊、第二數(shù)字移相器模塊和LMS權(quán)系數(shù)更新模塊，該第一數(shù)字移相器模塊的輸出端分別連接抖動(dòng)估計(jì)模塊和LMS權(quán)系數(shù)更新模塊，該第二數(shù)字移相器模塊的輸出端分別連接抖動(dòng)估計(jì)模塊、第三數(shù)字移相器模塊和LMS權(quán)系數(shù)更新模塊，該第三數(shù)字移相器模塊的輸出端分別連接抖動(dòng)估計(jì)模塊和LMS權(quán)系數(shù)更新模塊，該抖動(dòng)估計(jì)模塊的輸出端連接求和運(yùn)算模塊的另一輸入端，該求和運(yùn)算模塊的輸出端連接帶通濾波器模塊的輸入端，該帶通濾波器模塊連接LMS權(quán)系數(shù)更新模塊，該LMS權(quán)系數(shù)更新模塊輸出LMS算法更新權(quán)系數(shù)至抖動(dòng)估計(jì)模塊。

所述數(shù)字時(shí)鐘轉(zhuǎn)換器模塊設(shè)置在FPGA中，其輸出的采樣時(shí)鐘頻率為4Mf_s，其中，M為大于等于1的整數(shù)，f_s為陀螺抖動(dòng)頻率。

所述差分運(yùn)算模塊輸出的脈沖增量ΔN(n)＝ΔN_b(n)+ΔN_d(n)，其中，ΔN_b(n)、ΔN_d(n)分別代表第k個(gè)采樣周期內(nèi)的陀螺基座角運(yùn)動(dòng)變化和機(jī)械抖動(dòng)角運(yùn)動(dòng)變化；所述低階FIR濾波器處理后的角運(yùn)動(dòng)脈沖增量ΔN_lpf(n)為：

ΔN_lpf(n)＝ΔN_b′(n)+ΔN_d′(n)：

其中，ΔN_b′(n)表示經(jīng)過低階FIR濾波器的陀螺基座角運(yùn)動(dòng)增量，ΔN_d′(n)表示經(jīng)過衰減的機(jī)械抖動(dòng)角運(yùn)動(dòng)增量信號(hào)，K為表示FIR濾波器的階數(shù)。

所述IIR高通濾波器濾除低階FIR濾波器輸出的陀螺基座角運(yùn)動(dòng)增量ΔN_b′(n)后得到第一抖動(dòng)參考信號(hào)ΔN_d1(n)：所述第一數(shù)字移相模塊對(duì)ΔN_d1(n)處理后得到第二路抖動(dòng)參考信號(hào)ΔN_d2(n)，ΔN_d2(n)＝ΔN_d1(n-1)；所述第二數(shù)字移相模塊對(duì)ΔN_d1(n)處理后得到與ΔN_d1(n)相位相差90°的第三路抖動(dòng)參考信號(hào)ΔN_d3(n)，該ΔN_d3(n)＝ΔN_d1(n-M)，M為大于等于1的整數(shù)；所述第三數(shù)字移相模塊對(duì)第三路抖動(dòng)參考信號(hào)ΔN_d3(n)處理后得到第四路抖動(dòng)參考信號(hào)ΔN_d4(n)，ΔN_d4(n)＝ΔN_d3(n-1)。

所述LMS權(quán)系數(shù)更新模塊根據(jù)四路抖動(dòng)參考信號(hào)ΔN_d1(n)、ΔN_d2(n)、ΔN_d3(n)、ΔN_d4(n)以及帶通濾波器輸出值ε(n)輸出LMS算法更新權(quán)系數(shù)：

h₁₁(n+1)＝h₁₁(n)+2×μ×ε(n)×ΔN_d2(n)

h₁₂(n+1)＝h₁₂(n)+2×μ×ε(n)×ΔN_d1(n)

h₂₁(n+1)＝h₂₁(n)+2×μ×ε(n)×ΔN_d4(n)

h₂₂(n+1)＝h₂₂(n)+2×μ×ε(n)×ΔN_d3(n)。

所述抖動(dòng)估計(jì)模塊根據(jù)四路抖動(dòng)參考信號(hào)ΔN_d1(n)、ΔN_d2(n)、ΔN_d3(n)、ΔN_d4(n)以及四個(gè)LMS權(quán)系數(shù)對(duì)ΔN_d′(n)進(jìn)行估計(jì)得到抖動(dòng)角運(yùn)動(dòng)增量的估計(jì)

$\mathrm{\Δ}{\hat{N}}_d=h_{11}\left(n\right)\×{\mathrm{\ΔN}}_{d2}\left(n\right)+h_{12}\left(n\right)\×{\mathrm{\ΔN}}_{d1}\left(n\right)+h_{21}\left(n\right)\×{\mathrm{\ΔN}}_{d4}\left(n\right)+h_{22}\left(n\right)\×{\mathrm{\ΔN}}_{d3}\left(n\right)$

其中h₁₁(n)、h₁₂(n)、h₂₁(n)、h₂₂(n)為第n個(gè)采樣周期的權(quán)系數(shù)。

本發(fā)明的優(yōu)點(diǎn)和積極效果是：

1、本發(fā)明通過陀螺儀獲取抖動(dòng)參考信號(hào),不再需要外部抖動(dòng)拾取電路獲取抖動(dòng)參考信號(hào)，簡(jiǎn)化了抖動(dòng)抵消的硬件設(shè)計(jì)，并且提高了獲取的抖動(dòng)信號(hào)的精確性，從而提高抖動(dòng)抵消的效果。

2、本發(fā)明采用陀螺獲取抖動(dòng)參考信號(hào)后，通過設(shè)計(jì)低階IIR高通濾波器和數(shù)字時(shí)鐘轉(zhuǎn)換器，獲取與抖動(dòng)信號(hào)同步且同頻率的時(shí)鐘信號(hào)，針對(duì)不同陀螺的抖動(dòng)頻率不同，可使采樣頻率為抖動(dòng)信號(hào)頻率的整數(shù)倍，抖動(dòng)抵消需要的相位相差90°的正、余弦抖動(dòng)參考對(duì)在FPGA內(nèi)移相得到，可改善抖動(dòng)抵消效果。

3、本發(fā)明采用的IIR濾波器是無(wú)限沖擊響應(yīng)濾波器，存在反饋環(huán)節(jié)，在FPGA內(nèi)定點(diǎn)運(yùn)算時(shí)無(wú)法保證全精度截位，可在FPGA內(nèi)實(shí)現(xiàn)全精度定點(diǎn)運(yùn)算的低階FIR濾波器對(duì)陀螺輸出的脈沖增量進(jìn)行預(yù)濾波，減小進(jìn)入高通IIR濾波器脈沖增量信號(hào)的幅值，從而減少數(shù)據(jù)在FPGA內(nèi)的表示位數(shù)，在相同數(shù)據(jù)位數(shù)的情況下提高有效位的位數(shù)，提高抖動(dòng)抵消精度。

4、本發(fā)明的抖動(dòng)參考信號(hào)經(jīng)高通IIR濾波器獲取，相比帶通IIR濾波器獲取抖動(dòng)參考信號(hào)的方式，可大大降低IIR濾波器的階數(shù)，從而解決高階IIR濾波器易發(fā)散，導(dǎo)致抖動(dòng)抵消失敗的問題，同時(shí)可得到與陀螺輸出中高頻干擾相關(guān)的高頻干擾信號(hào)，根據(jù)自適應(yīng)干擾抵消原理，在抖動(dòng)抵消時(shí)抵消部分高頻干擾，利于改善抖動(dòng)抵消效果。

5、本發(fā)明采用三個(gè)數(shù)字移相器模塊，可得到四路抖動(dòng)參考信號(hào)，通過四個(gè)LMS權(quán)系數(shù)對(duì)抖動(dòng)干擾進(jìn)行估計(jì)，可得到更加精確的抖動(dòng)估計(jì)信號(hào)，進(jìn)一步提高抖動(dòng)抵消效果。

6、本發(fā)明設(shè)計(jì)合理，其采用基于LMS算法的自適應(yīng)干擾抵消技術(shù)進(jìn)行激光陀螺抖動(dòng)信號(hào)抵消，抖動(dòng)抵消采樣頻率為抖動(dòng)頻率的整數(shù)倍，抵消用到的正、余弦參考信號(hào)對(duì)由一路采樣差分信號(hào)經(jīng)信號(hào)移相得到，系統(tǒng)輸入輸出響應(yīng)近似時(shí)不變，從而減小抖抵消除時(shí)間延時(shí)，可廣泛用于實(shí)時(shí)消除二頻機(jī)械抖動(dòng)激光陀螺儀輸出中的機(jī)械抖動(dòng)信號(hào)。

附圖說明

圖1為本發(fā)明的電路原理圖。

具體實(shí)施方式

以下結(jié)合附圖對(duì)本發(fā)明實(shí)施例做進(jìn)一步詳述：

一種二頻機(jī)抖激光陀螺抖動(dòng)信號(hào)抵消裝置，如圖1所示，包括陀螺數(shù)據(jù)采集模塊1、差分運(yùn)算模塊2、低階FIR濾波器模塊3、求和運(yùn)算模塊4、高頻時(shí)鐘模塊5、低階IIR濾波器模塊6、IIR高通濾波器模塊7、第一數(shù)字移相器模塊8、抖動(dòng)估計(jì)模塊9、數(shù)字時(shí)鐘轉(zhuǎn)換器模塊10、第二數(shù)字移相器模塊11、帶通濾波器模塊12、數(shù)字時(shí)鐘管理模塊13、第三數(shù)字移相器模塊14、LMS權(quán)系數(shù)更新模塊15。所述陀螺數(shù)據(jù)采集模塊1、低階IIR濾波器模塊6、數(shù)字時(shí)鐘轉(zhuǎn)換器模塊10、數(shù)字時(shí)鐘管理模塊13依次相連接，該低階IIR濾波器模塊6的另一輸入端連接高頻時(shí)鐘模塊5，所述陀螺數(shù)據(jù)采集模塊1按照數(shù)字時(shí)鐘管理模塊13輸出4M倍頻的采用頻率進(jìn)行采樣，采樣數(shù)據(jù)經(jīng)差分運(yùn)算模塊2輸出至低階FIR濾波器模塊3，該低階FIR濾波器模塊3輸出端分別連接IIR高通濾波器模塊7及求和運(yùn)算模塊4，該IIR高通濾波器模塊7的輸出端分別連接第一數(shù)字移相器模塊8、抖動(dòng)估計(jì)模塊9、第二數(shù)字移相器模塊11和LMS權(quán)系數(shù)更新模塊15，該第二數(shù)字移相器模塊11的輸出端分別連接抖動(dòng)估計(jì)模塊9、第三數(shù)字移相器模塊14和LMS權(quán)系數(shù)更新模塊15，該第一數(shù)字移相器模塊8的輸出端分別連接抖動(dòng)估計(jì)模塊9和LMS權(quán)系數(shù)更新模塊15，該第三數(shù)字移相器模塊14的輸出端分別連接抖動(dòng)估計(jì)模塊9和LMS權(quán)系數(shù)更新模塊15，抖動(dòng)估計(jì)模塊9的輸出端連接求和運(yùn)算模塊4的另一輸入端，求和運(yùn)算模塊4的輸出端連接帶通濾波器模塊12的輸入端，該帶通濾波器模塊12輸出端連接LMS權(quán)系數(shù)更新模塊15，該LMS權(quán)系數(shù)更新模塊15輸出LMS算法更新權(quán)系數(shù)至抖動(dòng)估計(jì)模塊9。

本發(fā)明的工作原理為：激光陀螺輸出計(jì)數(shù)脈沖經(jīng)采樣、差分后得到包含陀螺基座角運(yùn)動(dòng)和機(jī)械抖動(dòng)角運(yùn)動(dòng)的脈沖增量ΔN(n)；脈沖增量ΔN(n)經(jīng)低階FIR低通濾波器3預(yù)濾波后得到抖動(dòng)預(yù)衰減的陀螺基座角運(yùn)動(dòng)和衰減的機(jī)械抖動(dòng)角運(yùn)動(dòng)脈沖增量ΔN_lpf(n)；機(jī)械抖動(dòng)角運(yùn)動(dòng)脈沖增量ΔN_lpf(n)經(jīng)過高通IIR濾波器7后得到預(yù)衰減后的參考抖動(dòng)增量信號(hào)ΔN_d1(n)；預(yù)衰減后的參考抖動(dòng)增量信號(hào)ΔN_d1(n)經(jīng)數(shù)字移相器11后得到與ΔN_d1(n)相差90°的參考抖動(dòng)增量信號(hào)ΔN_d3(n)，預(yù)衰減后的參考抖動(dòng)增量信號(hào)ΔN_d1(n)經(jīng)數(shù)字移相器8后得到參考抖動(dòng)增量信號(hào)ΔN_d2(n)，參考抖動(dòng)增量信號(hào)ΔN_d3(n)經(jīng)數(shù)字移相器14后得到參考抖動(dòng)增量信號(hào)ΔN_d4(n)；抖動(dòng)估計(jì)模塊9根據(jù)四路抖動(dòng)參考信號(hào)ΔN_d1(n)、ΔN_d2(n)、ΔN_d3(n)、ΔN_d4(n)得到抖動(dòng)角運(yùn)動(dòng)增量的估計(jì)并與ΔN_lpf(n)相減得到慣性角運(yùn)動(dòng)增量Δs(n)；慣性角運(yùn)動(dòng)增量Δs(n)經(jīng)帶通濾波器12后通過LMS權(quán)系數(shù)更新模塊15并通過優(yōu)化的LMS算法更新權(quán)系數(shù)h₁₁(n)、h₁₂(n)、h₂₁(n)、h₂₂(n)。激光陀螺輸出計(jì)數(shù)脈沖信號(hào)N經(jīng)低階IIR高通濾波器6后得到抖動(dòng)信號(hào)N_d(n)，抖動(dòng)信號(hào)N_d(n)經(jīng)過數(shù)字時(shí)鐘轉(zhuǎn)換器10得到與抖動(dòng)信號(hào)同步且同頻率的調(diào)寬波信號(hào)f_s，調(diào)寬波信號(hào)f_s頻率信號(hào)經(jīng)過FPGA內(nèi)的數(shù)字時(shí)鐘管理器13進(jìn)行4M倍頻后得到與抖動(dòng)參考信號(hào)同步的采樣時(shí)鐘對(duì)陀螺輸出信號(hào)采樣。ΔN_d1(n)經(jīng)簡(jiǎn)單的M個(gè)采樣延遲后得到與ΔN_d1(n)相差90°的增量信號(hào)ΔN_d2(n)，這樣得到的抖動(dòng)參考信號(hào)ΔN_d1(n)、ΔN_d2(n)接近理想的正余弦信號(hào)，輸入ΔN_lpf(n)到抖動(dòng)抵消輸出Δs(n)的響應(yīng)近似為時(shí)不變，可以得到較理想的抖動(dòng)抵消效果。

下面對(duì)本發(fā)明中的一些關(guān)鍵點(diǎn)設(shè)計(jì)進(jìn)行說明：

(1)采樣時(shí)鐘設(shè)計(jì)

陀螺輸出經(jīng)過低階IIR高通濾波器6得到抖動(dòng)參考信號(hào)N_d(n)，抖動(dòng)參考信號(hào)N_d(n)經(jīng)數(shù)字時(shí)鐘轉(zhuǎn)換器模塊10可以得到與抖動(dòng)信號(hào)ΔN_d(n)同步且同頻率的時(shí)鐘信號(hào)f_s：

y₁(n)＝b_l10×(N(n)+b_l11×N(n-1)+b_l12×N(n-2))+a_l11×y₁(n-1)+a_l12×y₁(n-2)

N_d(n)＝y(tǒng)₁(n)+b_l21×y₁(n-1)+b_l22×y₁(n-2)+a_l21×N_d(n-1)+a_l22×N_d(n-2)

f_s通過FPGA內(nèi)的數(shù)字時(shí)鐘管理模塊13進(jìn)行4M倍頻后得到與抖動(dòng)參考信號(hào)同步的采樣時(shí)鐘，采樣時(shí)鐘頻率為4Mf_s(M為大于等于1的整數(shù)，f_s為陀螺抖動(dòng)頻率)。抖動(dòng)抵消裝置以4Mf_s的采樣頻率對(duì)陀螺輸出信號(hào)采樣。

(2)低通FIR預(yù)濾波器的設(shè)計(jì)

激光陀螺輸出計(jì)數(shù)脈沖由陀螺數(shù)據(jù)采集模塊1得到，其采樣值經(jīng)差分運(yùn)算模塊2得到脈沖增量：

ΔN(n)＝N(n)-N(n-1)＝ΔN_b(n)+ΔN_d(n)

其中，ΔN_b(n)、ΔN_d(n)分別代表第k個(gè)采樣周期內(nèi)的陀螺基座角運(yùn)動(dòng)變化和機(jī)械抖動(dòng)角運(yùn)動(dòng)變化。

陀螺角運(yùn)動(dòng)脈沖增量ΔN(n)經(jīng)低階FIR濾波器進(jìn)行預(yù)濾波，得到經(jīng)過一次抖動(dòng)衰減的角運(yùn)動(dòng)脈沖增量ΔN_lpf(n)，ΔN_lpf(n)作為抖動(dòng)抵消的期望信號(hào)：

ΔN_lpf(n)＝h₀×ΔN(n)+h₁×ΔN(n-1)+h₂×ΔN(n-2)+…+h_K-1×ΔN(n-K+1)+h_K×ΔN(n-K)＝ΔN_b′(n)+ΔN_d′(n)

其中，ΔN_b′(n)表示經(jīng)過FIR濾波器的陀螺基座角運(yùn)動(dòng)增量，ΔN_d′(n)表示經(jīng)過衰減的機(jī)械抖動(dòng)角運(yùn)動(dòng)增量信號(hào)，K為表示FIR濾波器的階數(shù)，根據(jù)系統(tǒng)的實(shí)際需求設(shè)計(jì)不同階數(shù)的FIR濾波器，K的優(yōu)選推薦值可選擇為K＝35。

(3)抖動(dòng)參考信號(hào)的獲取

抖動(dòng)參考信號(hào)ΔN_d1(n)由高通濾波器7給出。ΔN_lpf(n)經(jīng)高通濾波器7濾除陀螺基座角運(yùn)動(dòng)增量ΔN_b′(n)，得到一路比較理想的陀螺機(jī)械抖動(dòng)角運(yùn)動(dòng)增量ΔN_d1(n)：

order₁(n)＝b₁₀×(ΔN_lpf(n)+b₁₁×ΔN_lpf(n-1)+b₁₂×ΔN_lpf(n-2))+a₁₁×order₁(n-1)+a₁₂×order₁(n-2)

order₂(n)＝order₁(n)+b₂₁×order₁(n-1)+b₂₂×order₁(n-2)+a₂₁×order₂(n-1)+a₂₂×order₂(n-2)

order₃(n)＝order₂(n)+b₃₁×order₂(n-1)+b₃₂×order₂(n-2)+a₃₁×order₃(n-1)+a₃₂×order₃(n-2)

order₄(n)＝order₃(n)+b₄₁×order₃(n-1)+b₄₂×order₃(n-2)+a₄₁×order₄(n-1)+a₄₂×order₄(n-2)

ΔN_d1(n)＝order₄(n)+b₅₁×order₄(n-1)+b₅₂×order₄(n-2)+a₅₁×ΔN_d1(n-1)+a₅₂×ΔN_d1(n-2)

ΔN_d1(n)經(jīng)數(shù)字移相模塊8移相后得到第二路抖動(dòng)參考信號(hào)ΔN_d2(n)：

ΔN_d2(n)＝ΔN_d1(n-1)

ΔN_d1(n)經(jīng)數(shù)字移相模塊11移相后得到與ΔN_d1(n)相位相差90°的第三路抖動(dòng)參考信號(hào)ΔN_d3(n)：

ΔN_d3(n)＝ΔN_d1(n-M)

由于采樣頻率為4Mf_s，因此無(wú)論陀螺抖動(dòng)頻率f_s如何變化，ΔN_d1(n)、ΔN_d3(n)之間的相位始終相差90°，相應(yīng)地抖動(dòng)抵消輸入到抖動(dòng)抵消輸出的響應(yīng)近似時(shí)不變，可改善抖動(dòng)抵消效果。

ΔN_d3(n)經(jīng)數(shù)字移相模塊14移相后得到第四路抖動(dòng)參考信號(hào)ΔN_d4(n)：

ΔN_d4(n)＝ΔN_d3(n-1)

(4)抖動(dòng)估計(jì)算法的設(shè)計(jì)

抖動(dòng)估計(jì)模塊9通過四路抖動(dòng)參考信號(hào)以及四個(gè)LMS權(quán)系數(shù)對(duì)ΔN_d′(n)進(jìn)行估計(jì)得到

$\mathrm{\Δ}{\hat{N}}_d=h_{11}\left(n\right)\×{\mathrm{\ΔN}}_{d2}\left(n\right)+h_{12}\left(n\right)\×{\mathrm{\ΔN}}_{d1}\left(n\right)+h_{21}\left(n\right)\×{\mathrm{\ΔN}}_{d4}\left(n\right)+h_{22}\left(n\right)\×{\mathrm{\ΔN}}_{d3}\left(n\right)$

其中h₁₁(n)、h₁₂(n)、h₂₁(n)、h₂₂(n)為第n個(gè)采樣周期的權(quán)系數(shù)，它們由權(quán)系數(shù)更新算法模塊15確定。

(5)LMS的權(quán)系數(shù)更新算法設(shè)計(jì)

LMS的權(quán)系數(shù)更新算法通過四路抖動(dòng)參考信號(hào)ΔN_d1(n)、ΔN_d2(n)、ΔN_d3(n)、ΔN_d4(n)以及帶通濾波器輸出得到的ε(n)進(jìn)行更新，其具體的更新算法如下：

h₁₁(n+1)＝h₁₁(n)+2×μ×ε(n)×ΔN_d2(n)

h₁₂(n+1)＝h₁₂(n)+2×μ×ε(n)×ΔN_d1(n)

h₂₁(n+1)＝h₂₁(n)+2×μ×ε(n)×ΔN_d4(n)

h₂₂(n+1)＝h₂₂(n)+2×μ×ε(n)×ΔN_d3(n)

(6)陀螺基座角運(yùn)動(dòng)增量

增量ΔN_lpf(n)與抖動(dòng)估計(jì)經(jīng)求和運(yùn)算模塊4得到抖動(dòng)抵消輸出Δs(n)，Δs(n)為陀螺基座角運(yùn)動(dòng)增量，即濾波方案要得到的陀螺輸出有效信號(hào)，抖動(dòng)抵消輸出s(n)為陀螺基座角運(yùn)動(dòng)增量ΔN_b(n)的近似。抖動(dòng)抵消算法如下：

$\mathrm{\Δ}s\left(n\right)={\mathrm{\ΔN}}_{lpf}\left(n\right)-\mathrm{\Δ}{\hat{N}}_d\left(n\right)$

(7)帶通濾波器的設(shè)計(jì)

為防止低頻或恒定的陀螺基座角運(yùn)動(dòng)影響權(quán)系數(shù)更新和抖動(dòng)抵消效果，抖動(dòng)抵消輸出s(n)將經(jīng)帶通濾波器12消除陀螺基座角運(yùn)動(dòng)后再用于權(quán)系數(shù)更新。帶通濾波器算法如下：

ε(n)＝-Δs(n)+Δs(n-1)+Δs(n-2)-Δs(n-3)

8、完整的抖動(dòng)抵消算法

本發(fā)明完整的抖動(dòng)抵消算法為：

y₁(n)＝b_l10×(N(n)+b_l11×N(n-1)+b_l12×N(n-2))+a_l11×y₁(n-1)+a_l12×y₁(n-2)

N_d(n)＝y(tǒng)₁(n)+b_l21×y₁(n-1)+b_l22×y₁(n-2)+a_l21×N_d(n-1)+a_l22×N_d(n-2)

ΔN_lpf(n)＝h₀×ΔN(n)+h₁×ΔN(n-1)+h₂×ΔN(n-2)+…+h_K-1×ΔN(n-K+1)+h_K×ΔN(n-K)

order₁(n)＝b₁₀×(ΔN_lpf(n)+b₁₁×ΔN_lpf(n-1)+b₁₂×ΔN_lpf(n-2))+a₁₁×order₁(n-1)+a₁₂×order₁(n-2)

order₂(n)＝order₁(n)+b₂₁×order₁(n-1)+b₂₂×order₁(n-2)+a₂₁×order₂(n-1)+a₂₂×order₂(n-2)

order₃(n)＝order₂(n)+b₃₁×order₂(n-1)+b₃₂×order₂(n-2)+a₃₁×order₃(n-1)+a₃₂×order₃(n-2)

order₄(n)＝order₃(n)+b₄₁×order₃(n-1)+b₄₂×order₃(n-2)+a₄₁×order₄(n-1)+a₄₂×order₄(n-2)

ΔN_d1(n)＝order₄(n)+b₅₁×order₄(n-1)+b₅₂×order₄(n-2)+a₅₁×ΔN_d1(n-1)+a₅₂×ΔN_d1(n-2)

ΔN_d2(n)＝ΔN_d1(n-1)

ΔN_d3(n)＝ΔN_d1(n-M)

ΔN_d4(n)＝ΔN_d3(n-1)

$\mathrm{\Δ}{\hat{N}}_d=h_{11}\left(n\right)\×{\mathrm{\ΔN}}_{d2}\left(n\right)+h_{12}\left(n\right)\×{\mathrm{\ΔN}}_{d1}\left(n\right)+h_{21}\left(n\right)\×{\mathrm{\ΔN}}_{d4}\left(n\right)+h_{22}\left(n\right)\×{\mathrm{\ΔN}}_{d3}\left(n\right)$

$\mathrm{\Δ}s\left(n\right)={\mathrm{\ΔN}}_{lpf}\left(n\right)-\mathrm{\Δ}{\hat{N}}_d\left(n\right)$

ε(n)＝-Δs(n)+Δs(n-1)+Δs(n-2)-Δs(n-3)

h₁₁(n+1)＝h₁₁(n)+2×μ×ε(n)×ΔN_d2(n)

h₁₂(n+1)＝h₁₂(n)+2×μ×ε(n)×ΔN_d1(n)

h₂₁(n+1)＝h₂₁(n)+2×μ×ε(n)×ΔN_d4(n)

h₂₂(n+1)＝h₂₂(n)+2×μ×ε(n)×ΔN_d3(n)

需要強(qiáng)調(diào)的是，本發(fā)明所述的實(shí)施例是說明性的，而不是限定性的，因此本發(fā)明包括并不限于具體實(shí)施方式中所述的實(shí)施例，凡是由本領(lǐng)域技術(shù)人員根據(jù)本發(fā)明的技術(shù)方案得出的其他實(shí)施方式，同樣屬于本發(fā)明保護(hù)的范圍。