專利名稱：基于多普勒的光纖陀螺捷聯(lián)慣導系統(tǒng)初始姿態(tài)確定方法
技術領域：
本發(fā)明涉及一種捷聯(lián)慣性導航系統(tǒng)的初始對準技術，尤其涉及一種以船用光纖陀螺捷聯(lián)慣導系統(tǒng)和另外一種能獨立準確提供船體速度信息的導航系統(tǒng)為基礎的組合初始精對準方法。
背景技術：
初始對準誤差是慣性導航系統(tǒng)主要的誤差源之一，初始對準的誤差對系統(tǒng)誤差的影響不僅表現(xiàn)在姿態(tài)指標上，而且表現(xiàn)在速度和位置信息的獲取上。對準的精度直接影響著導航的精度。因此，在正常導航之前，必須首先完成初始對準過程。
捷聯(lián)慣導是現(xiàn)在導航技術中比較熱門的技術，由于它的低成本受到了越來越多的導航界人士的青睞，而且通過捷聯(lián)慣導系統(tǒng)與其它導航系統(tǒng)的組合，可以提高導航系統(tǒng)初始對準的精度。常用的組合有捷聯(lián)慣導SINS/多普勒DVL、捷聯(lián)慣導SINS/GPS、捷聯(lián)慣導SINS/天文導航CNS等。針對船用光纖陀螺捷聯(lián)慣性導航系統(tǒng)，以速度為外觀測量的初始對準多采用捷聯(lián)慣導/多普勒的組合方式。
對于捷聯(lián)慣性導航系統(tǒng)(Strapdown Navigation System)來說，初始對準的目的是精確的估計姿態(tài)失準角并予以補償，這個目的由于在實際中不可能理想的補償儀表誤差(尤其是陀螺誤差)而不可能完全精確地實現(xiàn)。實際中已廣泛應用的經(jīng)典控制理論對準方法常用于實現(xiàn)靜態(tài)下的初始對準，并能獲得較高的精度，但由于其固有缺陷不適用于動態(tài)對準；在動基座條件下應采用最優(yōu)控制方法，通常采用卡爾曼濾波方法。采用這種方法有很多優(yōu)點可以克服經(jīng)典控制理論中船體加速度對對準性能的不利影響，適合于多種運動情況下的初始對準；在估計船體姿態(tài)信息的同時可以估計慣性器件(光纖陀螺和加速度計)的誤差，實現(xiàn)初始對準過程中的測漂；初始對準過程中可以實現(xiàn)慣性器件誤差的測量和補償，進而實現(xiàn)對準精度的提高。但是，這種普通卡爾曼濾波方法也存在著缺陷當系統(tǒng)狀態(tài)變量較多的情況下，濾波估計算法的計算量很大，尤其是在增廣向量的情況下，每增加一個狀態(tài)變量都會使計算量大幅度增加，這種情況不利于初始對準快速性的要求；在估計慣性器件誤差(主要是光纖陀螺測漂)過程中，由于方位陀螺漂移的估計時間較長，要想準確測得必須大幅增加估計時間，此時的卡爾曼濾波器變得不穩(wěn)定甚至開始發(fā)散，導致水平測漂失敗，姿態(tài)角也開始發(fā)散。如何解決初始對準的快速性和如何準確估計陀螺漂移以提高對準精度的問題成為當前的首要任務。

發(fā)明內(nèi)容
本發(fā)明的目的在于提供一種能夠有效提高船用光纖陀螺捷聯(lián)慣導/多普勒組合初始對準快速性和精度的基于多普勒的光纖陀螺捷聯(lián)慣導系統(tǒng)初始姿態(tài)確定方法。
本發(fā)明的目的是這樣實現(xiàn)的 (1)首先對光纖陀螺捷聯(lián)慣性導航系統(tǒng)進行預熱，然后連續(xù)采集光纖陀螺儀和石英撓性加速度計輸出的數(shù)據(jù)； (2)對采集到的陀螺儀和加速度計的數(shù)據(jù)進行處理，采用二階調(diào)平和方位估計法完成捷聯(lián)慣導系統(tǒng)的粗對準，確定此時的縱搖角θ、橫搖角γ和航向角ψ姿態(tài)信息； (3)粗對準完畢后進入精對準階段，首先，繼續(xù)采集光纖陀螺和加速度計輸出的數(shù)據(jù)，并通過標準的四元數(shù)法進行導航解算，獲得船體的計算速度、姿態(tài)、位置等相關信息；同時，通過多普勒計程儀或者GPS測得船體的速度信息，并把這個速度近似看作船體的真實速度； (4)建立船用捷聯(lián)慣性導航系統(tǒng)的動基座誤差方程； (5)應用最優(yōu)控制濾波理論設計濾波器，并進行濾波估計； (6)從狀態(tài)估值

中提取船體姿態(tài)失準角信息α、β、γ，在組合精對準結(jié)束時用它來修正船體姿態(tài)，即縱搖角θ、橫搖角γ和航向角ψ，完成精確初始對準；同時，從偏差估計值

中獲得陀螺漂移的估計值，實現(xiàn)初始對準階段的測漂過程，并對陀螺漂移進行補償，進一步抑制器件誤差對船體導航信息的影響。
本發(fā)明還可以包括如下特征 1、所述的建立船用捷聯(lián)慣性導航系統(tǒng)的動基座誤差方程的誤差方程為
α、β、γ——計算地理坐標系與真實水平坐標系之間的姿態(tài)誤差角； δ——計算地理坐標系與真實地理坐標系之間的緯度誤差； δλ——計算地理坐標系與真實地理坐標系之間的經(jīng)度誤差； δVx、δVy——當?shù)氐乩碜鴺讼档妮S向速度計算值與真實值之間的誤差； Vx、Vy——船體的東北向速度； wie——地球自轉(zhuǎn)角速率； RE，RN——船體所在位置的地球曲率半徑； ——船體所在位置的緯度； g——當?shù)刂亓铀俣龋? εx、εy、εz——陀螺誤差在載體坐標系的投影， 這里簡記為陀螺零位漂移； x、y——加速度計在載體坐標系的投影，這里簡記為加速度計偏差。
2、所述的濾波器的設計與濾波估計過程包括 1)離散濾波器模型的建立； 系統(tǒng)的狀態(tài)方程和量測方程描述如下 xk＝Ak-1xk-1+Bk-1bk-1+ξk-1(2) bk+1＝bk(3) yk＝Hkxk+Ckbk+ηk(4) 其中， xk——第k個觀測時刻的n維狀態(tài)變量 yk——量測向量 bk——偏差向量 ξk——過程噪聲向量，且滿足 ηk——觀測噪聲向量，且滿足 Ak-1、Bk-1、Hk、Ck——時變的系數(shù)矩陣，Bk-1表明偏差向量bk進入動態(tài)方程的方式； 2)系統(tǒng)狀態(tài)的選擇，對于船用捷聯(lián)系統(tǒng)來說，當把偏差分離出來以后，狀態(tài)向量可以寫成以下形式 x＝[αβγ δδλδVx δVy]T(5) 偏差狀態(tài)寫成以下形式 b＝[εx εyεzxy]T(6) 觀測狀態(tài)變量選擇如下 y＝[δVx δVy]T(7) δVx、δVy——當?shù)氐乩碜鴺讼档妮S向速度計算值與真實值之間的誤差，在組合對準中可看成捷聯(lián)慣性導航系統(tǒng)解算速度與外測真實速度之差； 根據(jù)式(1)捷聯(lián)慣導系統(tǒng)誤差狀態(tài)方程，濾波模型參數(shù)設置如下


A21＝02×3(12)





C＝02×5(22) Tbn——從載體坐標系到導航坐標系的轉(zhuǎn)換矩陣； 3)濾波器的估計； 狀態(tài)估計值

由無偏狀態(tài)

和偏差b組成，即 整個濾波器的算法可以用以下方程表示 其中，

——x的增益矩陣；

——估計值

的協(xié)方差陣；

——

的預測值； 偏差估計表示如下 U(k)＝Ak-1V(k-1)+Bk(31) S(k)＝HkU(k)+Ck(33) 其中， Kb(k)——偏差的增益矩陣； M(k)，U(k)，V(k)，S(k)——為計算需要而引入的變量； 初始狀態(tài)設置為 U(0)＝0，M(0)＝Pb(0)； 解算時，首先根據(jù)式(23)～式(27)計算無偏狀態(tài)估計值

根據(jù)式(28)～式(33)計算偏差

的估計值，然后通過

來修正狀態(tài)估值得到
本發(fā)明的方法具有以下優(yōu)點(1)當系統(tǒng)狀態(tài)變量較多的情況下，由于偏差狀態(tài)從系統(tǒng)狀態(tài)向量中分離出來，降低了系統(tǒng)維數(shù)，從而大大減小了計算量，算短了估計時間，提高了初始對準的速度；(2)在估計慣性器件誤差(主要是光纖陀螺測漂)過程中，估計時間的增長不會導致濾波估計值的發(fā)散和濾波性能的下降，從而使得方位陀螺漂移也能通過這種方法進行估計，同時保證了初始對準的精度。
對本發(fā)明的有益效果說明如下 對載體在多種運動狀態(tài)進行了Matlab仿真，其中包括三軸搖擺狀態(tài)；勻速加搖擺運動；加速運動。
仿真條件如下 (1)設船體處于三軸搖擺運動狀態(tài)。模型為 yaw＝y(tǒng)awm sin(ωht+h)+K pitch＝pitchm sin(ωpt+p)(34) roll＝rollm sin(ωrt+r) 其中 yaw，pitch，roll分別表示航向角、縱搖角和橫搖角的搖擺角度變量；三軸運動幅度為yawm＝10°，pitchm＝8°，rollm＝6°；角頻率為wi＝2π/Ti，(i＝h，p，r)，周期Th＝6s，Tp＝10s，Tr＝5s。初始相位r，h，p都是30°，初始航向K設為90°。
初始真實姿態(tài)位0°、0°、90°； 初始失準角α＝1°，β＝1°，γ＝3°； 初始緯度＝45.7796°，初始經(jīng)度λ＝126.6705°； 陀螺零位漂移εx＝εy＝εz＝0.1°/h； 陀螺刻度因數(shù)誤差10-4； 陀螺儀白噪聲誤差0.005度/小時； 加速度計零位誤差x＝y＝1×10-4g； 加速度計刻度因數(shù)誤差為10-4； 加速度計白噪聲誤差5×10-5g 地球自轉(zhuǎn)角速率wie＝7.27220417rad/s； 載體初始位置北緯45.7796°，東經(jīng)126.6705°； 赤道半徑Re＝6378393.0m； 橢球度e＝3.367e-3； 由萬有引力可得的地球表面重力加速度g＝9.78049m/s2； 地球自轉(zhuǎn)角速度(弧度/秒)7.2921158e-5； 常數(shù)π＝3.1415926； 仿真分為兩個階段進行，先進行200秒的粗對準，然后進入精對準階段。
此時應用本發(fā)明方法分別進行3分鐘和15分鐘的仿真分析，并在精對準進行 600秒以后對船體姿態(tài)進行修正。3分鐘和15分鐘濾波估計的陀螺漂移曲線分別如圖1、圖2所示。整個對準階段船體姿態(tài)誤差輸出信息如圖3所示。
(2)設船體處于勻速加三軸搖擺運動狀態(tài)。船體的速度為10m/s，其余仿真條件與(1)相同。仿真3分鐘得到陀螺漂移的估計值曲線如圖4所示。
(3)設船體處于加速運動狀態(tài)。船體東向運動加速度1m/s，加速10s，其余仿真條件與(1)相同。精對準估計10分鐘所得陀螺漂移估計曲線如圖5所示，整個對準階段船體姿態(tài)誤差輸出信息如圖6所示。
仿真結(jié)果表明艦船在動基座下進行組合精對準時，采用本發(fā)明的方法可以在保證對準精度和快速性的要求下，實現(xiàn)對光纖陀螺零位漂移的準確估計。


圖1為三軸搖擺狀態(tài)下利用Matlab進行3分鐘精對準仿真得到的陀螺漂移的估計曲線圖； 圖2為三軸搖擺狀態(tài)下利用Matlab進行15分鐘精對準仿真得到的陀螺漂移的估計曲線圖； 圖3為三軸搖擺狀態(tài)下利用Matlab仿真得到整個初始對準中船體姿態(tài)誤差角的曲線圖； 圖4為勻速加三軸搖擺狀態(tài)下利用Matlab進行3分鐘精對準仿真得到的陀螺漂移的估計曲線圖； 圖5為加速狀態(tài)下利用Matlab進行10分鐘精對準仿真得到的陀螺漂移的估計曲線圖； 圖6為加速狀態(tài)下利用Matlab仿真得到的整個初始對準中船體姿態(tài)誤差角的曲線圖。
具體實施例方式 下面結(jié)合附圖舉例對本發(fā)明做更詳細地描述 實施實例一 (1)光纖陀螺捷聯(lián)慣性導航系統(tǒng)預熱后連續(xù)采集光纖陀螺儀和石英撓性加速度計輸出的數(shù)據(jù)，并對采集到的陀螺儀和加速度計的數(shù)據(jù)進行處理，采用二階調(diào)平和方位估計法完成捷聯(lián)慣導系統(tǒng)的粗對準，確定此時船體的姿態(tài)信息(縱搖角θ，橫搖角γ和航向角ψ)。
(2)粗對準完畢，進入精對準階段。首先，繼續(xù)采集光纖陀螺和加速度計輸出的數(shù)據(jù)，并通過標準的四元數(shù)法進行導航解算，獲得船體的計算速度、姿態(tài)、位置等相關信息；同時，通過多普勒計程儀或者GPS測得船體的速度信息，并把這個速度近似看作船體的真實速度。
(3)建立船用捷聯(lián)慣性導航系統(tǒng)的動基座誤差方程。不考慮垂直通道，通常的增廣狀態(tài)為7維誤差向量。誤差方程如下
α、β、γ——計算地理坐標系與真實水平坐標系之間的姿態(tài)誤差角； δ——計算地理坐標系與真實地理坐標系之間的緯度誤差； δλ——計算地理坐標系與真實地理坐標系之間的經(jīng)度誤差； δVx、δVy——當?shù)氐乩碜鴺讼档妮S向速度計算值與真實值之間的誤差； Vx、Vy——船體的東北向速度； wie——地球自轉(zhuǎn)角速率； RE，RN——船體所在位置的地球曲率半徑； ——船體所在位置的緯度； g——當?shù)刂亓铀俣龋? εx、εy、εz——陀螺誤差在載體坐標系的投影， 這里簡記為陀螺零位漂移； x、y——加速度計在載體坐標系的投影，這里簡記為加速度計偏差； (4)應用最優(yōu)控制濾波理論設計濾波器，并進行濾波估計。這里采用把偏差從系統(tǒng)狀態(tài)向量中分離出來的濾波方法，通過降低系統(tǒng)維數(shù)來提高計算的速度和精度。
1)離散濾波器模型的建立。
系統(tǒng)的狀態(tài)方程和量測方程描述如下 xk＝Ak-1xk-1+Bk-1bk-1+ξk-1(2) bk+1＝bk (3) yk＝Hkxk+Ckbk+ηk (4) 其中， xk——n維狀態(tài)變量(第k個觀測時刻). yk——量測向量 bk——偏差向量 ξk——過程噪聲向量，且滿足 ηk——觀測噪聲向量，且滿足 Ak-1、Bk-1、Hk、Ck——時變的系數(shù)矩陣，Bk-1表明偏差向量bk進入動態(tài)方程的方式。
2)系統(tǒng)狀態(tài)的選擇。對于船用捷聯(lián)系統(tǒng)來說，當把偏差(這里為慣性器件誤差)分離出來以后，狀態(tài)向量可以寫成以下形式 x＝[αβγδδλδVxδVy]T(5) 偏差狀態(tài)寫成以下形式 b＝[εxεyεzxy]T(6) 觀測狀態(tài)變量選擇如下 y＝[δVxδVy]T(7) δVx、δVy——當?shù)氐乩碜鴺讼档妮S向速度計算值與真實值之間的誤差，在組合對準中可看成捷聯(lián)慣性導航系統(tǒng)解算速度與外測真實速度之差； 根據(jù)式(1)捷聯(lián)慣導系統(tǒng)誤差狀態(tài)方程，濾波模型參數(shù)設置如下


A21＝02×3(12)





C＝02×5(22) Tbn——從載體坐標系到導航坐標系的轉(zhuǎn)換矩陣； 3)濾波器的估計算法。
狀態(tài)估計值

由無偏狀態(tài)

和偏差b組成，即 整個濾波器的算法可以用以下方程表示 其中，

——x的增益矩陣；

——估計值

的協(xié)方差陣；

——

的預測值； 偏差估計表示如下 U(k)＝Ak-1V(k-1)+Bk(31) S(k)＝HkU(k)+Ck(33) 其中， Kb(k)——偏差的增益矩陣； M(k)，U(k)，V(k)，S(k)——為計算需要而引入的變量； 初始狀態(tài)設置為 U(0)＝0，M(0)＝Pb(0)； 具體解算時，首先根據(jù)式(23)～式(27)計算無偏狀態(tài)估計值

根據(jù)式(28)～式(33)計算偏差

的估計值，然后通過

來修正狀態(tài)估值得到
(5)從狀態(tài)估值

中提取船體姿態(tài)失準角信息α、β、γ，在組合精對準結(jié)束時用它來修正船體姿態(tài)(縱搖角θ，橫搖角γ和航向角ψ)，實現(xiàn)精確初始對準。同時，從偏差估計值

中獲得陀螺漂移的估計值，實現(xiàn)初始對準階段的測漂過程，并對陀螺漂移進行補償，進一步抑制了器件誤差對船體導航信息的影響。
權利要求
1.一種基于多普勒的光纖陀螺捷聯(lián)慣導系統(tǒng)初始姿態(tài)確定方法，其特征是
(1)首先對光纖陀螺捷聯(lián)慣性導航系統(tǒng)進行預熱，然后連續(xù)采集光纖陀螺儀和石英撓性加速度計輸出的數(shù)據(jù)；
(2)對采集到的陀螺儀和加速度計的數(shù)據(jù)進行處理，采用二階調(diào)平和方位估計法完成捷聯(lián)慣導系統(tǒng)的粗對準，確定此時的縱搖角θ、橫搖角γ和航向角ψ姿態(tài)信息；
(3)粗對準完畢后進入精對準階段，首先，繼續(xù)采集光纖陀螺和加速度計輸出的數(shù)據(jù)，并通過標準的四元數(shù)法進行導航解算，獲得船體的計算速度、姿態(tài)、位置等相關信息；同時，通過多普勒計程儀或者GPS測得船體的速度信息，并把這個速度近似看作船體的真實速度；
(4)建立船用捷聯(lián)慣性導航系統(tǒng)的動基座誤差方程；
(5)應用最優(yōu)控制濾波理論設計濾波器，并進行濾波估計；
(6)從狀態(tài)估值
中提取船體姿態(tài)失準角信息α、β、γ，在組合精對準結(jié)束時用它來修正船體姿態(tài)，即縱搖角θ、橫搖角γ和航向角ψ，完成精確初始對準；同時，從偏差估計值
中獲得陀螺漂移的估計值，實現(xiàn)初始對準階段的測漂過程，并對陀螺漂移進行補償，進一步抑制器件誤差對船體導航信息的影響。
2.根據(jù)權利要求1所述的基于多普勒的光纖陀螺捷聯(lián)慣導系統(tǒng)初始姿態(tài)確定方法，其特征是所述的建立船用捷聯(lián)慣性導航系統(tǒng)的動基座誤差方程的誤差方程為
α、β、γ——計算地理坐標系與真實水平坐標系之間的姿態(tài)誤差角；
δ——計算地理坐標系與真實地理坐標系之間的緯度誤差；
δλ——計算地理坐標系與真實地理坐標系之間的經(jīng)度誤差；
δVx、δVy——當?shù)氐乩碜鴺讼档妮S向速度計算值與真實值之間的誤差；
Vx、Vy——船體的東北向速度；
wie——地球自轉(zhuǎn)角速率；
RE，RN——船體所在位置的地球曲率半徑；
——船體所在位置的緯度；
g——當?shù)刂亓铀俣龋?br> εx、εy、εz——陀螺誤差在載體坐標系的投影，這里簡記為陀螺零位漂移；
x、y——加速度計在載體坐標系的投影，這里簡記為加速度計偏差。
3.根據(jù)權利要求1或2所述的基于多普勒的光纖陀螺捷聯(lián)慣導系統(tǒng)初始姿態(tài)確定方法，其特征是所述的濾波器的設計與濾波估計過程包括
1)離散濾波器模型的建立；
系統(tǒng)的狀態(tài)方程和量測方程描述如下
xk＝Ak-1xk-1+Bk-1bk-1+ξk-1(2)
bk+1＝bk(3)
yk＝Hkxk+Ckbk+ηk(4)
其中，
xk——第k個觀測時刻的n維狀態(tài)變量
yk——量測向量
bk——偏差向量
ξk——過程噪聲向量，且滿足
ηk——觀測噪聲向量，且滿足
Ak-1、Bk-1、Hk、Ck——時變的系數(shù)矩陣，Bk-1表明偏差向量bk進入動態(tài)方程的方式；
2)系統(tǒng)狀態(tài)的選擇，對于船用捷聯(lián)系統(tǒng)來說，當把偏差分離出來以后，狀態(tài)向量可以寫成以下形式
x＝[α β γ δ δλ δVx δVy]T(5)
偏差狀態(tài)寫成以下形式
b＝[εx εy εz x y]T(6)
觀測狀態(tài)變量選擇如下
y＝[δVx δVy]T(7)
δVx、δVy——當?shù)氐乩碜鴺讼档妮S向速度計算值與真實值之間的誤差，在組合對準中可看成捷聯(lián)慣性導航系統(tǒng)解算速度與外測真實速度之差；
根據(jù)式(1)捷聯(lián)慣導系統(tǒng)誤差狀態(tài)方程，濾波模型參數(shù)設置如下
A21＝02×3(12)
C＝02×5(22)
Tbn——從載體坐標系到導航坐標系的轉(zhuǎn)換矩陣；
3)濾波器的估計
狀態(tài)估計值
由無偏狀態(tài)
和偏差b組成，即
整個濾波器的算法可以用以下方程表示
其中，
——x的增益矩陣；
——估計值
的協(xié)方差陣；
——
的預測值；
偏差估計表示如下
U(k)＝Ak-1V(k-1)+Bk (31)
S(k)＝HkU(k)+Ck (33)
其中，
Kb(k)——偏差的增益矩陣；
M(k)，U(k)，V(k)，S(k)——為計算需要而引入的變量；
初始狀態(tài)設置為
U(0)＝0，M(0)＝Pb(0)；
解算時，首先根據(jù)式(23)～式(27)計算無偏狀態(tài)估計值
根據(jù)式(28)～式(33)計算偏差
的估計值，然后通過
來修正狀態(tài)估值得到
全文摘要
本發(fā)明提供的是一種基于多普勒的光纖陀螺捷聯(lián)慣導系統(tǒng)初始姿態(tài)確定方法。預熱后連續(xù)采集光纖陀螺儀和石英撓性加速度計輸出的數(shù)據(jù)；對采集到的陀螺儀和加速度計的數(shù)據(jù)進行處理；完成捷聯(lián)慣導系統(tǒng)的粗對準；粗對準完畢后進入精對準階段；建立船用捷聯(lián)慣性導航系統(tǒng)的動基座誤差方程；應用最優(yōu)控制濾波理論設計濾波器，并進行濾波估計；提取船體姿態(tài)失準角信息，在組合精對準結(jié)束時用它來修正船體姿態(tài)，完成精確初始對準；同時，獲得陀螺漂移的估計值，實現(xiàn)初始對準階段的測漂過程，并對陀螺漂移進行補償，進一步抑制器件誤差對船體導航信息的影響。采用本發(fā)明的方法可以在保證對準精度和快速性的要求下，實現(xiàn)對光纖陀螺零位漂移的準確估計。
文檔編號G01C25/00GK101187567SQ20071014484
公開日2008年5月28日 申請日期2007年12月18日 優(yōu)先權日2007年12月18日
發(fā)明者郝燕玲, 周廣濤, 陳明輝, 偉 高, 博 徐, 高洪濤, 強 于, 陳世同, 磊 吳, 程建華 申請人:哈爾濱工程大學