專利名稱：一種捷聯(lián)慣性導航中器件周期誤差快速精確補償方法
技術領域：
本發(fā)明屬于數(shù)據(jù)處理技術領域，特別涉及陀螺儀等慣性器件漂移數(shù)據(jù)中周期性誤差的補償方法，適用于各種過采樣數(shù)據(jù)的周期性誤差補償處理。

背景技術：
慣性導航系統(tǒng)可以分為器件級、部件級和系統(tǒng)級三個層次，器件級指慣性敏感器件，主要包括三個陀螺儀和三個加速度計，各慣性器件可以實時提供敏感到的角速度和加速度信息；部件級指將慣性器件按照特定方式組成的慣性測量單元，它對慣性器件進行高頻采樣，然后每多個數(shù)據(jù)做一次平滑，供給系統(tǒng)級；系統(tǒng)級指整個慣性導航系統(tǒng)，它在導航計算機中利用慣性測量單元提供的平滑后的數(shù)據(jù)進行導航解算，得到各個導航參數(shù)。
慣性導航系統(tǒng)工作時，器件級的輸出信號采樣頻率總是高于系統(tǒng)級導航解算要求頻率，而后在部件級進行數(shù)據(jù)平滑以降低采樣數(shù)據(jù)的誤差、提高數(shù)據(jù)精度。把這種高于正常應用要求頻率的信號采樣方式稱為過采樣，慣性導航系統(tǒng)中器件級的過采樣數(shù)據(jù)中常具有周期性誤差，某些器件的過采樣數(shù)據(jù)中周期性誤差還可能非常顯著，使平滑后數(shù)據(jù)依然含有顯著的周期性干擾，為提高器件和系統(tǒng)的精度，需要進行周期誤差的標定和補償。由此提出這樣一個問題在有些場合，含顯著周期性的過采樣數(shù)據(jù)需要經(jīng)過平滑方能應用，應用時需要實時補償其周期性。例如在導航、制導與控制領域中的慣性導航系統(tǒng)，陀螺儀與加速度計可以連續(xù)實時輸出角速度與加速度數(shù)據(jù)信息，其采樣頻率常在1000Hz左右，而在進行導航參數(shù)解算時，涉及到復雜的數(shù)據(jù)處理運算，故正常應用的解算頻率比較低，一般在50～100Hz。所以，陀螺儀與加速度計的原始采樣數(shù)據(jù)通常不直接應用于導航系統(tǒng)，而要先進行數(shù)據(jù)平滑，再應用到其它必須的在線數(shù)據(jù)預處理，如補償溫度、振動、加速度、角加速度引起的誤差，對噪聲進行濾波等，最后才提供給導航解算程序。而有些類型的陀螺儀與加速度計的輸出數(shù)據(jù)含有較強的周期性誤差項，比如石英系列MEMS陀螺儀的漂移數(shù)據(jù)，在高頻率采樣條件下，就可以發(fā)現(xiàn)很強的周期性誤差項和規(guī)律性，要想有效利用石英系列MEMS陀螺儀，必須對該周期性誤差項和規(guī)律性進行精確的在線補償。
對周期性誤差項的補償建立在對其精確標定的基礎上，現(xiàn)有的精確標定技術快速準確，精度高，不干擾有用信號，可以滿足要求，一般采用一段時間內(nèi)的數(shù)據(jù)，利用合適的數(shù)學方法，精確的尋求到周期性誤差項的頻率、振幅、初相位等參數(shù)。其中頻率的精確標定是關鍵，包括頻率初標定和頻率精標定兩步。頻率初標定是以PSD分析法確定出周期性誤差項的頻率。頻率精標定中將問題轉(zhuǎn)化為最優(yōu)化理論中的具有多約束非線性的最小范數(shù)局部優(yōu)化問題，構建模型，設計算法得出頻率。
慣性導航系統(tǒng)的實際工作過程中，就是在把器件級較高頻率的采樣數(shù)據(jù)平滑為符合系統(tǒng)級導航解算頻率要求的低頻數(shù)據(jù)的同時，又需要對器件的周期性誤差項進行補償。因此，在實際數(shù)據(jù)處理過程中，一般有兩種處理方法，一種是數(shù)據(jù)平滑之前在器件級進行周期性誤差項的標定和補償，簡稱為先補后平法；另一種是數(shù)據(jù)平滑之后在系統(tǒng)級進行周期性誤差項的標定和補償，簡稱為先平后補法。這兩種方法各有優(yōu)點先補后平法精度較高，先平后補法運算量小、實時性好。但同時這兩種補償方法又有各自的不足 (1)先補后平法的主要缺點實時測試頻率一般較高，所得的數(shù)據(jù)量大，對數(shù)據(jù)進行實時補償時，要求在極短時間里，完成巨大的運算量，實時性差。其數(shù)據(jù)采集量要求采集電路具備很高的性能，其算法運算量又要求系統(tǒng)運算處理器具備很強的能力。
(2)先平后補法的主要缺點數(shù)據(jù)平滑過程會影響顯著周期性誤差項的頻率穩(wěn)定性，造成補償精度下降；另外，數(shù)據(jù)平滑過程會增加低頻成分，使本來不顯著的周期性誤差變得顯著，從而增加了補償?shù)墓ぷ髁亢吞幚砥鞯倪\算量。


發(fā)明內(nèi)容
本發(fā)明的目的是克服現(xiàn)有用于周期性誤差項補償?shù)臄?shù)據(jù)處理方法的不足，提出一種兼有先補后平法和先平后補法的優(yōu)點，又克服了二者的缺點、比先補后平法精度更高、比先平后補法速度更快的周期性誤差項補償方法。
本發(fā)明的技術解決方案是根據(jù)器件級數(shù)據(jù)分析結果，以一套嚴格的可實時應用的誤差折算公式，實時折算出平滑后每個數(shù)據(jù)在導航系統(tǒng)級中所含的誤差量并將其補償，其中誤差折算公式如下求得取一段時間內(nèi)器件級的過采樣原始數(shù)據(jù)做分析，確定所測器件級原始數(shù)據(jù)中包含的顯著周期性誤差項的個數(shù)，并確定各個周期性誤差項的頻率、振幅、初相位等參數(shù)，然后根據(jù)推導的公式計算出誤差折算公式的各個系數(shù)，代入誤差折算公式，其補償步驟為 (1)設置解算周期Ts的值為采樣周期Tc的M倍，即Ts＝M·Tc，收集并處理初始對準期間器件級輸出的原始數(shù)據(jù)，確定出器件所含顯著周期性誤差的個數(shù)N和各顯著周期性誤差振幅Ai、頻率fi、初相位i，其中i＝1，…，N，進而計算出折算振幅Fi、折算角頻率ωi和折算初相位θi。轉(zhuǎn)入導航狀態(tài)，令ti＝1； (2)在部件級中，將慣性器件在第ti個解算周期中的M個原始數(shù)據(jù)進行一次平滑，得出第ti個解算周期的器件輸出數(shù)據(jù)平滑值xp(ti)，供給系統(tǒng)級； (3)確定系統(tǒng)中第ti個解算周期起點相對于初始對準起始點的時間值t； 由步驟(3)確定的時間值t，根據(jù)誤差折算公式折算出各個誤差pi(t)； (5)計算出總誤差值

并用來補償步驟(2)確定的第ti個平滑值xp(ti)，得出第ti個補償后的平滑值 (6)以補償后平滑值xb(ti)供第ti次導航解算用； (7)令ti＝ti+1，轉(zhuǎn)步驟(2)，直至導航過程結束。
本發(fā)明的原理是本發(fā)明參數(shù)計算過程如圖1所示，對誤差折算公式系數(shù)的計算過程，體現(xiàn)了有效標定器件級周期性誤差并在系統(tǒng)級補償?shù)倪^程，參照圖1將本發(fā)明原理作如下描述。
通常認為需要處理的原始數(shù)據(jù)序列包含的周期性誤差y(t)由N個不同周期的函數(shù)疊加而成，設其形式為
設置采樣周期Tc和解算周期Ts的值，假設每一個解算周期Ts包含M個采樣周期Tc，設第ti個解算周期在時刻t與時刻t+1之間，其間的原始輸出數(shù)據(jù)為x(tm)，其中，tm＝(ti-1)·Ts+(m-1)·Tc，而m＝1，2，…，M，每M個數(shù)據(jù)作一次平滑后，形成平滑數(shù)據(jù)序列xp(ti)，則有式 成立。設第ti個解算周期中第m個原始數(shù)據(jù)補償?shù)麸@著周期性誤差影響后剩下的值為xb(tm)，則 x(tm)＝xb(tm)+y(tm)(3) 將式(3)兩邊同時對第ti個解算周期中的M個原始數(shù)據(jù)取平均值得
式(4)中，左邊為式(2)表示的xp(ti)，其物理意義是每M個數(shù)據(jù)一次平滑所得的平滑值；右邊第一項的物理意義是M個原始數(shù)據(jù)補償?shù)舢斍爸芷谛哉`差項的平均值，也就是在當前時刻補償后的平滑數(shù)據(jù)，記為xb(ti)，將其輸出，即可供給后續(xù)應用。式(4)等號右邊第二項先求

再求

即先求原始數(shù)據(jù)中所有的顯著周期性誤差對時刻tm的影響，再求此時M個tm處的顯著周期性誤差影響的平均值，現(xiàn)將其換序，即對右邊第二項先求

再求

即先求原始數(shù)據(jù)中某一個顯著周期性誤差在時刻t對應的前M個時刻tm點處影響的平均值，再求所有的顯著周期性誤差在時刻t的影響之和，則式(4)化為
式(4)中括號內(nèi)部分的物理意義是原始數(shù)據(jù)中第i個顯著周期性誤差，對第ti個解算周期的平滑數(shù)據(jù)xp(ti)的影響，將其記為pi(t)，則
由于原始數(shù)據(jù)測試時的采樣頻率恒定，即其采樣間隔Tc恒定，由此可得 tm＝t+(m-1)·Tc (7) 將式(7)代入式(6)得
將式(8)中的余弦三角函數(shù)展開，并將常數(shù)提出來，將含有變量t的項置后得

(9) 將式(9)記為 pi(t)＝Ci·cos(2πfi·t)-Si·sin(2πfi·t) (10) 其中


(11) 式(11)中等號右邊的所有參數(shù)，均可離線標定。其中，M為每次平滑的數(shù)據(jù)個數(shù)，fi為第i個周期性誤差項的頻率，Ai為第i個振幅，i為第i個初相位角，可根據(jù)一段時間內(nèi)輸出的原始數(shù)據(jù)，分別精確標定出所有周期性誤差項的各個參數(shù)。因此在實時補償時，Ci和Si已經(jīng)被標定出，作為常數(shù)出現(xiàn)，大大簡化了補償運算量。而且令 ωi＝2πfi(12) 式(10)還可進一步化簡為 pi(t)＝Fi·cos(ωi·t+θi)(13) 其中 Fi，θi均可離線標定出，這樣使式(13)計算量比式(10)又減少了一倍。
最終，將式(13)代入式(5)得本發(fā)明的補償原理公式為 其中，F(xiàn)i，θi均離線標定出來，在實時補償時不占用機時，使得本發(fā)明每一次補償?shù)倪\算量大大減少。
本發(fā)明與現(xiàn)有技術相比的優(yōu)點在于 (1)本發(fā)明補償?shù)娘@著周期性誤差來源于器件級原始輸出數(shù)據(jù)，沒有受到平滑的破壞、成份單純、能量高、顯著周期性誤差個數(shù)少，具有先補后平法精度高的優(yōu)點，避免了先平后補法精度低的缺點。
(2)本發(fā)明只需對系統(tǒng)級內(nèi)的平滑數(shù)據(jù)實時補償，不至于使處理器疲于應付，補償頻率為采樣頻率的M分之一，M為每次平滑的原始數(shù)據(jù)個數(shù)，這大大降低了運算量，具有先平后補法運算量低的優(yōu)點，避免了先補后平法運算量大的缺點。



圖1為本發(fā)明參數(shù)計算過程示意圖； 圖2為本發(fā)明的流程圖； 圖3為本實施例選用陀螺的漂移原始數(shù)據(jù)圖； 圖4為本實施例選用陀螺的漂移功率譜分析圖； 圖5為本實施例選用陀螺漂移的先補后平法補償結果圖； 圖6a和圖6b為本實施例選用陀螺漂移的每10個數(shù)一次平滑后的特性示意圖，其中圖6a為平滑后陀螺漂移數(shù)據(jù)功率譜分析圖，圖6b為平滑后任取2秒鐘數(shù)據(jù)的陀螺漂移時域圖； 圖7為本實施例選用陀螺的先平后補法的補償結果示意圖； 圖8為本發(fā)明方法對實施例選用陀螺的漂移補償結果圖。

具體實施例方式 本發(fā)明實施例不失一般性，以某BEI GYROCHIP LCG50型石英MEMS陀螺儀輸出數(shù)據(jù)處理過程為例，補償流程如圖2所示。石英系列MEMS陀螺儀與其它陀螺儀一樣，可以連續(xù)輸出角速度信息，其采樣頻率較高，本實施例取典型采樣頻率1000Hz，而陀螺組成的慣導系統(tǒng)在進行導航參數(shù)解算時，涉及到復雜的數(shù)據(jù)處理運算，故解算頻率比較低，一般在100Hz左右。利用本發(fā)明對石英MEMS陀螺儀漂移周期性進行實時補償?shù)牟襟E是 (1)本實施例記錄120秒漂移測試數(shù)據(jù)，以驗證其補償效果。數(shù)據(jù)平滑采用10個數(shù)據(jù)作一次平滑，則本實施例中解算周期Ts＝0.01s，采樣周期Tc＝0.001s，每一個解算周期Ts包含M＝10個采樣周期Tc。收集并處理初始對準期間慣性器件輸出的原始數(shù)據(jù)，確定出器件所含顯著周期性誤差的個數(shù)N和各顯著周期性誤差振幅Ai、頻率fi、初相位i，其中i＝1，…，N。進而計算出折算振幅Fi、折算角頻率ωi和折算初相位θi。然后轉(zhuǎn)入導航狀態(tài)，令ti＝1。
需要確定的參數(shù)主要有各顯著周期性誤差振幅Ai、頻率fi、初相位i。本實施例中石英MEMS陀螺儀的原始輸出如圖3所示，通常將其看作一個由N個不同周期的函數(shù)疊加而成的時間序列y(t)，可設其函數(shù)形式如公式(1)所示。針對不同頻率，需分別精確估計其頻率fi、振幅Ai和初相位角i。本實施例利用傅立葉變換，求出石英MEMS陀螺漂移的頻域特性如圖4所示，其頻率隨能量大小排序如表1所示。
表1實施例中顯著周期性誤差頻率隨能量大小的排序 由圖4及表1分析可以確定顯著周期性誤差個數(shù)為3，并粗略確定3個顯著周期性誤差的頻率fi如表1前3項所示，然后采用頻率搜索算法，逐步精確定位各個顯著周期性誤差對應的頻率fi如表2中fi列所示再由最小二乘法擬合確定出振幅Ai和初相位角i。本實施例求得的各參數(shù)如表2所示。
表2本實施例3個顯著周期性誤差的參數(shù) 在本發(fā)明原理的分析過程中已知原始數(shù)據(jù)包含的顯著周期性誤差對平滑后當前時刻的數(shù)據(jù)造成的誤差可以記為式(10)，其系數(shù)由式(11)確定，式(11)中參數(shù)M取10為每次平滑的數(shù)據(jù)個數(shù)，其余參數(shù)已經(jīng)在步驟(1)中標定出，這樣就確定了Ci和Si的值。由于式(10)還可進一步化簡為式(13)，這樣Fi，θi的值均可由Ci和Si的值離線標定出，可以使式(13)計算量比式(10)又減少了一倍。求得的系數(shù)如表3所示。
表3本實施例3個誤差折算公式的系數(shù)值 (2)在后續(xù)的實時補償階段，將石英MEMS陀螺儀漂移的原始輸出數(shù)據(jù)在第ti個解算周期中的M個原始數(shù)據(jù)進行一次平滑，得出第ti個解算周期的器件輸出數(shù)據(jù)平滑值xp(ti)，將平滑值作為當前時刻陀螺儀的輸出數(shù)據(jù)。
設第ti個解算周期在時刻t與時刻t+1之間，將第ti個解算周期中的M個原始輸出數(shù)據(jù)計為x(tm)，其中，tm＝(ti-1)·0.01+(m-1)·0.001，而m＝1，2，…，M，每M個數(shù)據(jù)作一次平滑后，形成平滑數(shù)據(jù)序列xp(ti)，xp(ti)由式(2)表示。
(3)確定第ti個解算周期起點相對于初始對準起始點的時間值t。
由于每個周期性誤差項隨著時間的推移，其值的大小會不斷地發(fā)生變化，如果某時刻的時間值確定的不精確，就會影響確定周期性誤差項引起的誤差值。
(4)由步驟(3)確定的時間值t，根據(jù)誤差折算公式折算出各個誤差pi(t)。
由步驟(2)離線標定出的誤差折算公式的各個系數(shù)值，確定原始數(shù)據(jù)包含的顯著周期性誤差對平滑后當前時刻的數(shù)據(jù)造成的誤差大小，即確定將Fi，θi的值代入公式(13)求得的值。
(5)計算出總誤差值

并用來補償步驟(2)確定的第ti個平滑值xp(ti)，得出第ti個補償后的平滑值 將原始數(shù)據(jù)包含的顯著周期性誤差對平滑后當前時刻的數(shù)據(jù)造成的誤差，即算得的pi(t)＝Fi·cos(2πfi·t+θi)的值，代入本發(fā)明的補償原理公式(15)，其中，F(xiàn)i，θi均由步驟(4)離線標定出來。
(6)以補償后的平滑數(shù)據(jù)作為陀螺儀的輸出，供后續(xù)捷聯(lián)解算用。
(7)重復步驟(2)到步驟(6)，直到導航過程結束。
精度和實時性是評價實時補償方法優(yōu)劣的兩個重要指標。其中，精度的評價比較簡單，它是一個常見的指標，一般以陀螺漂移值的大小來表示。但運算量不易直接評價，而需要經(jīng)過折算，運算量是評價實時性的重要指標，它表征了補償運算能否跟上導航解算的速度，由于各種處理器的運算能力互有差異，以計算時間來表示運算量的大小就不合適了?？紤]到周期性實時補償中涉及到三角函數(shù)運算，而運算一次三角函數(shù)占用的機時與運算普通加減乘除占用的機時相比，要多一個數(shù)量級以上，所以將各個實時補償算法的運算量折合為余弦運算的個數(shù)，來評價各個補償方法的實時性能。以下對各補償方法進行評價。
本實施例所選用的BEI GYROCHIP LCG50型石英MEMS陀螺儀，未經(jīng)任何處理的原始輸出數(shù)據(jù)如圖3所示，原始數(shù)據(jù)每10個數(shù)一次平滑后的陀螺漂移數(shù)據(jù)功率譜分析圖如圖6a所示，平滑后任取2秒鐘數(shù)據(jù)的陀螺漂移時域圖如圖6b所示。分別采用先補后平法、先平后補法和本發(fā)明補償本實施例的陀螺儀，先補后平法的補償結果如圖5所示，先平后補法的補償結果如圖7所示，本發(fā)明的補償結果如圖8所示。根據(jù)漂移計算公式求出所涉及情況下的陀螺漂移值如表4所示， 表4陀螺漂移特性表 由表4可知，本發(fā)明補償精度最高，先平后補法精度最差，先補后平法精度介于前二者之間。將各個實時補償算法的運算量折合為余弦運算的個數(shù)，如表5所示 表5運算量特性表 由表3可知，本發(fā)明補償運算量最低，先補后平法運算量最大，先補后平法運算量介于前二者之間。
總之，相比之下，采用本發(fā)明方法實時補償精度最高，運算量最小，是石英系列MEMS陀螺儀漂移周期性誤差項實時補償方法的最佳選擇。
本發(fā)明可有效地精確標定和在線補償慣性器件輸出數(shù)據(jù)中包含的周期性誤差項，通過推導原始數(shù)據(jù)中顯著周期性誤差對平滑后數(shù)據(jù)造成誤差的誤差折算公式，使得本方法兼有先補后平法和先平后補法的優(yōu)點，又克服二者缺點，同時補償精度更高、補償運算速度更快，依據(jù)本發(fā)明的數(shù)學原理，可知本發(fā)明也適用于其它含有周期干擾項的信號周期性誤差項補償，是一種新穎高效的補償信號中周期性誤差項的信號處理方法。
權利要求
1、一種捷聯(lián)慣性導航中器件周期誤差快速精確補償方法，其特征在于包括以下步驟
(1)設置解算周期Ts的值為采樣周期Tc的M倍，即Ts＝M·Tc，收集并處理初始對準期間器件級輸出的原始數(shù)據(jù)，確定出器件所含顯著周期性誤差的個數(shù)N和各顯著周期性誤差振幅Ai、頻率fi、初相位i，其中i＝1，…，N，進而計算出折算振幅Fi、折算角頻率i和折算初相位θi，轉(zhuǎn)入導航狀態(tài)，令ti＝1；
(2)在部件級中，將慣性器件在第ti個解算周期中的M個原始數(shù)據(jù)進行一次平滑，得出第ti個解算周期的器件輸出數(shù)據(jù)平滑值xp(ti)，供給系統(tǒng)級；
(3)確定系統(tǒng)中第ti個解算周期起點相對于初始對準起始點的時間值t；
(4)由步驟(3)確定的時間值t，根據(jù)誤差折算公式折算出各個誤差pi(t)；
(5)計算出總誤差值并用來補償步驟(2)確定的第ti個平滑值xp(ti)，得出第ti個補償后的平滑值
(6)以補償后平滑值xb(ti)供第ti次導航解算用；
(7)令ti＝ti+1，轉(zhuǎn)步驟(2)，直至導航過程結束。
2、根據(jù)權利要求1所述的高精度的慣性器件周期性誤差快速補償方法，其特征在于所述的步驟(1)中M為自然數(shù)，取值為8≤M≤50。
3、根據(jù)權利要求1所述的高精度的慣性器件周期性誤差快速補償方法，其特征在于所述的步驟(1)中的N為整數(shù)，取值為2≤N≤5。
4、根據(jù)權利要求1所述的高精度的慣性器件周期性誤差快速補償方法，其特征在于所述的步驟(1)中誤差折算公式的折算振幅Fi、折算角頻率ωi和折算初相位θi的計算方法為
ωi＝2πfi，
式中
5、根據(jù)權利要求1所述的一種捷聯(lián)慣性導航中器件周期誤差快速精確補償方法，其特征在于所述的步驟(4)中誤差折算公式為
pi(t)＝Fi·cos(ωi·t+θi)。
全文摘要
一種捷聯(lián)慣性導航中器件周期誤差快速精確補償方法，根據(jù)器件級數(shù)據(jù)分析結果，利用誤差折算公式，實時折算出導航系統(tǒng)級的解算數(shù)據(jù)所含誤差量并將其補償。其中誤差折算公式如下求得取一段時間內(nèi)器件級的過采樣原始數(shù)據(jù)做分析，確定所測器件級原始數(shù)據(jù)中包含的顯著周期性誤差項的個數(shù)及其特征參數(shù)，根據(jù)推導的公式計算出誤差折算公式的各個系數(shù)，代入誤差折算公式。本發(fā)明充分利用了器件級原始數(shù)據(jù)包含的周期性誤差項信息，又具備系統(tǒng)級數(shù)據(jù)處理在運算量上的優(yōu)勢，補償精度高，運算量小。本發(fā)明適用于各種過采樣數(shù)據(jù)的周期性誤差項補償處理，尤其適用于慣性導航系統(tǒng)中陀螺儀與加速度計輸出數(shù)據(jù)的周期性誤差項補償處理。
文檔編號G01C21/10GK101038171SQ20071006554
公開日2007年9月19日 申請日期2007年4月16日 優(yōu)先權日2007年4月16日
發(fā)明者房建成, 張海鵬, 陳曉光, 孫科, 張霄, 于歌 申請人:北京航空航天大學