本發(fā)明涉及導(dǎo)航，尤其涉及基于力反饋模式的半球諧振慣導(dǎo)回溯濾波對(duì)準(zhǔn)方法及系統(tǒng)。

背景技術(shù)：

1、半球諧振慣導(dǎo)的核心部件是半球諧振陀螺，半球諧振陀螺屬于哥氏振動(dòng)陀螺，利用半球唇殼的徑向振動(dòng)駐波進(jìn)動(dòng)效應(yīng)感測基座旋轉(zhuǎn)，是高精度新型的純固態(tài)振動(dòng)陀螺，具備組成部件少、工作壽命長、啟動(dòng)時(shí)間短、物理特性穩(wěn)定等優(yōu)點(diǎn)，在慣導(dǎo)領(lǐng)域受到越來越多的關(guān)注和應(yīng)用，目前已成為國內(nèi)外慣性技術(shù)領(lǐng)域的研究熱點(diǎn)之一。

2、半球諧振陀螺誤差中包含對(duì)稱性漂移,對(duì)稱性漂移隨陀螺振型角呈周期性變化，會(huì)顯著影響系統(tǒng)對(duì)準(zhǔn)精度;現(xiàn)有技術(shù)中的半球諧振陀螺對(duì)稱性漂移標(biāo)定通常以陀螺的輸出作為量測，基于最小二乘法等參數(shù)辨識(shí)方法得到，這種標(biāo)定方式雖然簡單，但是需要耗費(fèi)較長時(shí)間。

技術(shù)實(shí)現(xiàn)思路

1、本發(fā)明所要解決的技術(shù)問題是提供基于力反饋模式的半球諧振慣導(dǎo)回溯濾波對(duì)準(zhǔn)方法及系統(tǒng)，在對(duì)準(zhǔn)過程中驅(qū)使半球諧振陀螺振型角勻速變化并遍歷完整周期，并通過回溯濾波在短時(shí)間內(nèi)辨識(shí)陀螺對(duì)稱性漂移并補(bǔ)償，提高了系統(tǒng)對(duì)準(zhǔn)收斂速度和精度。

2、本發(fā)明是通過以下技術(shù)方案予以實(shí)現(xiàn)：

3、一種基于力反饋模式的半球諧振慣導(dǎo)回溯濾波對(duì)準(zhǔn)方法，其包括如下步驟：

4、s1：初始化半球諧振慣導(dǎo)，并對(duì)變量進(jìn)行初始賦值，系統(tǒng)進(jìn)入力反饋模式，諧振陀螺振型角勻速單向變化，將半球諧振慣導(dǎo)的輸出傳輸給存儲(chǔ)器及自對(duì)準(zhǔn)模塊；

5、s2：自對(duì)準(zhǔn)模塊進(jìn)行自對(duì)準(zhǔn)，并將自對(duì)準(zhǔn)結(jié)果傳輸給回溯濾波對(duì)準(zhǔn)模塊，同時(shí)存儲(chǔ)器進(jìn)行降頻存儲(chǔ)后將降頻存儲(chǔ)數(shù)據(jù)傳輸給回溯濾波對(duì)準(zhǔn)模塊；

6、s3：回溯濾波對(duì)準(zhǔn)模塊通過多次回溯濾波對(duì)準(zhǔn)，并將回溯濾波對(duì)準(zhǔn)最終結(jié)果傳輸給反饋校正模塊；

7、s4：反饋校正模塊基于回溯濾波對(duì)準(zhǔn)最終結(jié)果進(jìn)行反饋校正后，系統(tǒng)進(jìn)入導(dǎo)航模式。

8、優(yōu)化的，步驟s1中諧振陀螺振型角的變化速率為3。

9、優(yōu)化的，步驟s1中所述變量包括半球諧振慣導(dǎo)的姿態(tài)、速度、位置及濾波參數(shù)，所述半球諧振慣導(dǎo)的輸出包括半球諧振慣導(dǎo)原始角速率、振型角控制信號(hào)、實(shí)際敏感到的外界角速率、角增量及比力增量。

10、優(yōu)化的，步驟s2中所述自對(duì)準(zhǔn)包括粗對(duì)準(zhǔn)階段及精對(duì)準(zhǔn)階段，所述粗對(duì)準(zhǔn)階段采用凝固系濾波方法進(jìn)行粗對(duì)準(zhǔn)，所述精對(duì)準(zhǔn)階段采用卡爾曼濾波方法進(jìn)行精對(duì)準(zhǔn)。

11、進(jìn)一步，步驟s2中存儲(chǔ)器采用如下方法進(jìn)行降頻存儲(chǔ)：

12、s21：系統(tǒng)的變量根據(jù)式（1）進(jìn)行更新：

13、（1）；

14、其中：表示初始載體坐標(biāo)系，表示載體坐標(biāo)系，表示初始載體坐標(biāo)系到第次解算載體坐標(biāo)系的四元數(shù)，表示初始載體坐標(biāo)系到第次解算載體坐標(biāo)系的四元數(shù)，表示第次解算載體坐標(biāo)系到第次解算載體坐標(biāo)系的四元數(shù)，表示比力增量在初始載體坐標(biāo)系內(nèi)分量的第次解算的積分，表示比力增量在初始載體坐標(biāo)系內(nèi)分量的第次解算的積分，表示第次解算由得到的轉(zhuǎn)換矩陣，表示載體坐標(biāo)系與初始載體坐標(biāo)系間的相對(duì)關(guān)系，表示第次解算的比力增量；

15、s22：每運(yùn)行設(shè)定時(shí)間后，將初始載體坐標(biāo)系到第次解算載體坐標(biāo)系的四元數(shù)、比力增量在初始載體坐標(biāo)系內(nèi)分量的第次解算的積分及當(dāng)前諧振陀螺振型角存入存儲(chǔ)器內(nèi)，然后將比力增量在初始載體坐標(biāo)系內(nèi)分量的第次解算的積分置零。

16、優(yōu)化的，步驟s22中所述的設(shè)定時(shí)間為1秒。

17、進(jìn)一步，步驟s3中在回溯濾波對(duì)準(zhǔn)時(shí)，慣導(dǎo)導(dǎo)航解算模式按時(shí)間分為正向?qū)Ш胶湍嫦驅(qū)Ш?，每一次狀態(tài)更新過程都分別進(jìn)行正向和逆向解算。

18、進(jìn)一步，步驟s3中回溯濾波對(duì)準(zhǔn)的方法如下：

19、s31：慣導(dǎo)導(dǎo)航解算模式按時(shí)間分為正向?qū)Ш胶湍嫦驅(qū)Ш剑驅(qū)Ш綍r(shí)，慣導(dǎo)的狀態(tài)更新方程為式（2），逆向?qū)Ш綍r(shí)，慣導(dǎo)的狀態(tài)更新方程為式（3）：

20、（2）；

21、（3）；

22、其中：表示載體坐標(biāo)系到地理坐標(biāo)系的轉(zhuǎn)換矩陣，表示載體坐標(biāo)系到地理坐標(biāo)系的轉(zhuǎn)換矩陣的導(dǎo)數(shù)，表示陀螺輸出的角速度在載體坐標(biāo)系中的分量，表示反對(duì)稱矩陣，表示地理坐標(biāo)系相對(duì)于慣性坐標(biāo)系的旋轉(zhuǎn)角速度在地理坐標(biāo)系中的分量，表示載體相對(duì)于地心地固坐標(biāo)系的速度，表示比力在載體坐標(biāo)系中的分量，表示地球自轉(zhuǎn)角速度在地理坐標(biāo)系中的分量，表示地理坐標(biāo)系相對(duì)于地心地固坐標(biāo)系的旋轉(zhuǎn)角速度在地理坐標(biāo)系中的分量，表示重力加速度,表示載體位置的導(dǎo)數(shù)，表示位置更新相對(duì)于速度的轉(zhuǎn)換矩陣，表示對(duì)陀螺輸出的角速度在載體坐標(biāo)系中的分量取反，表示對(duì)地理坐標(biāo)系相對(duì)于慣性坐標(biāo)系的旋轉(zhuǎn)角速度在地理坐標(biāo)系中的分量取反，表示載體相對(duì)于地心地固坐標(biāo)系的速度的導(dǎo)數(shù)，表示對(duì)比力在載體坐標(biāo)系中的分量取反，表示對(duì)地球自轉(zhuǎn)角速度在地理坐標(biāo)系中的分量取反，表示對(duì)地理坐標(biāo)系相對(duì)于地心地固坐標(biāo)系的旋轉(zhuǎn)角速度在地理坐標(biāo)系中的分量取反，表示對(duì)重力加速度取反，表示對(duì)位置更新相對(duì)于速度的轉(zhuǎn)換矩陣取反；

23、s32：根據(jù)離散式（4）進(jìn)行系統(tǒng)導(dǎo)航解算，并記錄慣導(dǎo)系統(tǒng)狀態(tài)誤差變量：

24、（4）；

25、其中：表示導(dǎo)航坐標(biāo)系到載體坐標(biāo)系的轉(zhuǎn)換矩陣，表示初始導(dǎo)航坐標(biāo)系到第秒時(shí)導(dǎo)航坐標(biāo)系旋轉(zhuǎn)矢量四元數(shù)，表示自對(duì)準(zhǔn)初始時(shí)刻的姿態(tài)四元數(shù)，表示初始載體坐標(biāo)系到第秒時(shí)載體坐標(biāo)系旋轉(zhuǎn)矢量四元數(shù)，表示第次解算的慣導(dǎo)速度，表示第次解算的慣導(dǎo)速度，表示單位矩陣，表示相鄰兩次解算的時(shí)間間隔，表示比力增量在第秒時(shí)載體坐標(biāo)系中的分量的積分，表示第次解算的載體位置，表示第次解算的載體位置；

26、s33：將慣導(dǎo)系統(tǒng)狀態(tài)誤差變量分別代入正向慣導(dǎo)系統(tǒng)狀態(tài)誤差方程式（5）及逆向慣導(dǎo)系統(tǒng)狀態(tài)誤差方程式（6）中，取速度位置為外部觀測量，得到正向慣導(dǎo)系統(tǒng)的卡爾曼濾波方程式（7）及逆向慣導(dǎo)系統(tǒng)的卡爾曼濾波方程式（8）：

27、

28、

29、

30、其中：表示系統(tǒng)誤差角，表示系統(tǒng)誤差角的導(dǎo)數(shù)，表示系統(tǒng)速度誤差，表示系統(tǒng)速度誤差的導(dǎo)數(shù)，表示常值陀螺漂移，表示陀螺對(duì)稱性漂移正弦系數(shù)，表示存入存儲(chǔ)器的振型角，表示陀螺對(duì)稱性漂移余弦系數(shù)，表示系統(tǒng)陀螺噪聲，表示慣導(dǎo)速度的導(dǎo)數(shù)，表示比力，表示常值加速度計(jì)零偏，表示系統(tǒng)加表噪聲，表示載體位置的導(dǎo)數(shù)，表示位置誤差相對(duì)于位置的轉(zhuǎn)換矩陣，表示載體位置，表示載體位置誤差，表示載體位置誤差的導(dǎo)數(shù)，表示常值加速度計(jì)零偏的導(dǎo)數(shù)，表示常值陀螺漂移的導(dǎo)數(shù)，表示陀螺對(duì)稱性漂移正弦系數(shù)的導(dǎo)數(shù)，表示陀螺對(duì)稱性漂移余弦系數(shù)的導(dǎo)數(shù)，表示矩陣轉(zhuǎn)置，表示慣導(dǎo)系統(tǒng)狀態(tài)誤差變量的導(dǎo)數(shù)，表示誤差狀態(tài)轉(zhuǎn)移矩陣，表示慣導(dǎo)系統(tǒng)狀態(tài)誤差變量，表示噪聲狀態(tài)轉(zhuǎn)移矩陣，表示系統(tǒng)噪聲，表示速度與位置的量測值，表示量測方程，表示量測噪聲，表示方向系數(shù)，正向時(shí)取1，逆向時(shí)取-1,表示地理坐標(biāo)系相對(duì)于慣性坐標(biāo)系的旋轉(zhuǎn)角速度在地理坐標(biāo)系中的分量的誤差，表示對(duì)地理坐標(biāo)系相對(duì)于慣性坐標(biāo)系的旋轉(zhuǎn)角速度在地理坐標(biāo)系中的分量取反的誤差，表示地球自轉(zhuǎn)角速度在地理坐標(biāo)系中的分量的誤差，表示對(duì)地球自轉(zhuǎn)角速度在地理坐標(biāo)系中的分量取反的誤差，表示地理坐標(biāo)系相對(duì)于地心地固坐標(biāo)系的旋轉(zhuǎn)角速度在地理坐標(biāo)系中的分量誤差，表示對(duì)地理坐標(biāo)系相對(duì)于地心地固坐標(biāo)系的旋轉(zhuǎn)角速度在地理坐標(biāo)系中的分量取反的誤差；

31、s34：系統(tǒng)完成一次正向、逆向回溯濾波后，記錄回溯濾波對(duì)準(zhǔn)模塊狀態(tài)變量，形成回溯濾波對(duì)準(zhǔn)模塊狀態(tài)變量的協(xié)方差陣，將回溯濾波對(duì)準(zhǔn)模塊狀態(tài)變量的協(xié)方差陣對(duì)角線上所有的元素逐個(gè)與設(shè)定的最小值比較，若小于設(shè)定的最小值則重置，若大于或等于設(shè)定的最小值則回溯濾波對(duì)準(zhǔn)結(jié)束。

32、進(jìn)一步，步驟s4中根據(jù)式（9）進(jìn)行反饋校正：

33、（9）；

34、其中：表示校正后的載體姿態(tài)四元數(shù)，表示由系統(tǒng)誤差角轉(zhuǎn)化得到的載體姿態(tài)誤差的四元數(shù)，表示校正后載體速度，表示校正前載體速度，表示校正后載體位置。

35、一種基于力反饋模式的半球諧振慣導(dǎo)回溯濾波對(duì)準(zhǔn)系統(tǒng)，用于執(zhí)行上述任意一項(xiàng)所述的一種基于力反饋模式的半球諧振慣導(dǎo)回溯濾波對(duì)準(zhǔn)方法，其包括存儲(chǔ)器、自對(duì)準(zhǔn)模塊、回溯濾波對(duì)準(zhǔn)模塊及反饋校正模塊；

36、所述存儲(chǔ)器用于接收半球諧振慣導(dǎo)的輸出數(shù)據(jù)并降頻存儲(chǔ)后傳輸給回溯濾波對(duì)準(zhǔn)模塊；

37、所述自對(duì)準(zhǔn)模塊用于接收半球諧振慣導(dǎo)的輸出數(shù)據(jù)并進(jìn)行自對(duì)準(zhǔn)后，將自對(duì)準(zhǔn)結(jié)果傳輸給回溯濾波對(duì)準(zhǔn)模塊；

38、所述回溯濾波對(duì)準(zhǔn)模塊接收自對(duì)準(zhǔn)模塊及存儲(chǔ)器數(shù)據(jù)，通過多次回溯濾波對(duì)準(zhǔn)后，將回溯濾波對(duì)準(zhǔn)最終結(jié)果傳輸給反饋校正模塊；

39、所述反饋校正模塊基于回溯濾波對(duì)準(zhǔn)最終結(jié)果進(jìn)行反饋校正。

40、發(fā)明的有益效果：

41、本發(fā)明提供的基于力反饋模式的半球諧振慣導(dǎo)回溯濾波對(duì)準(zhǔn)方法及系統(tǒng)，在系統(tǒng)狀態(tài)誤差方程中引入陀螺對(duì)稱性漂移系數(shù)項(xiàng)，并在對(duì)準(zhǔn)過程中采用力反饋模式讓對(duì)稱性漂移呈隨振型角的周期性變化，令陀螺對(duì)稱性漂移系數(shù)能在后續(xù)濾波對(duì)準(zhǔn)階段被辨識(shí)出來，從而提高了系統(tǒng)對(duì)準(zhǔn)精度；

42、另外通過降頻存儲(chǔ)半球諧振慣導(dǎo)的輸出數(shù)據(jù)，并在常規(guī)自對(duì)準(zhǔn)的基礎(chǔ)上進(jìn)行多次回溯濾波對(duì)準(zhǔn)，等效延長系統(tǒng)對(duì)準(zhǔn)時(shí)間，使得系統(tǒng)能夠在短時(shí)間內(nèi)狀態(tài)誤差收斂至穩(wěn)態(tài)值，提高了對(duì)準(zhǔn)精度和速度。