適合於光纖陀螺的載體姿態測量方法
2023-10-23 17:04:17 1
專利名稱：適合於光纖陀螺的載體姿態測量方法
技術領域：
本發明提供的是一種測量方法，具體地說是一種使用光纖陀螺測量載體相對 於導航坐標系姿態參數的方法。
(二)
背景技術：
在捷聯慣性導航系統中，慣性傳感器(陀螺和加速度計)直接固聯在載體上。 它們敏感載體相對於慣性坐標系的姿態變化及速度變化信息，這些信息經過導航 計算機的處理輸出導航參數，從而完成導航制導任務。由於採用慣性傳感器固聯 的方式，慣性傳感器會直接敏感載體的振動和環境的幹擾，對捷聯慣性導航系統 的測量精度帶來一定的影響。在捷聯慣性導航系統中，採用的是數學平臺，主要 依靠陀螺的輸出信息來構建數學平臺，以描述載體相對導航坐標系的姿態。而陀 螺的輸出為角增量或角速度，不能直接用於描述坐標系的相對關係。在常規的捷 聯慣性導航系統的姿態測量方法中，總是認為載體坐標系和導航坐標系間的轉換 是通過一系列的轉動來實現的。所以，為了測量載體的姿態，需要由相應的角速 度或角增量來求得描述坐標系關係的四元數或旋轉矢量。但是在高動態環境下， 在姿態更新周期內的每次陀螺採樣對應的載體坐標系的位置是不同的。另外，傳 統方法中是假設從載體坐標繫到參考坐標系間的轉動的次序是可以不予考慮的， 這是基於無限小轉動是矢量的原理來得到的。但實際的工程中，特別是在載體高 動態機動情形或惡劣振動環境下，載體的轉動常常是有限轉動，而有限轉動不是 矢量，其轉動次序不能交換。因而將姿態更新周期中的所有陀螺輸出的角增量求 和後再求解四元數的傳統姿態測量方法將帶來較大的誤差，即圓錐誤差。圓錐誤 差類似於陀螺漂移，給載體的姿態測量帶來負面影響。補償圓錐誤差，高質量地 測量載體的姿態就必須設計高性能的姿態測量方法。
在傳統的姿態測量方法中，利用陀螺輸出的角增量去估算出圓錐補償項，從
而估算出旋轉向量的增量。該基本思路是基於在傳統陀螺的輸出中，角增量的獲 取是很方便的。即要想得到旋轉向量的增量必須知道陀螺輸出的角增量。而在目 前廣泛使用的光纖陀螺捷聯慣性導航系統中，光纖陀螺的輸出信號為角速度信 息，為此，通常的做法是採用分段線性化，認為在每一個採樣周期內載體的角速 度是常量。這種假設在採樣周期足夠小且載體的角速度變化較慢的時候是成立的，但對於高動態的場合就必須另加考慮了。已有的研究成果表明，在光纖陀螺 輸出角速度的情形下，對於傳統的基於旋轉矢量的姿態測量方法來說，性能不如 常規的基於四元數的姿態測量方法，無法體現在應對圓錐效應的優越性。
發明內容
本發明的目的在於提供一種能夠解決在載體高動態環境或是高頻率振動環 境中，圓錐效應對於載體姿態測量精度產生影響的問題的適合於光纖陀螺的載體 姿態測量方法。
本發明的目的是這樣實現的-
步驟1、通過外部設備確定載體的初始位置參數與初始速度值；
步驟2、光纖陀螺捷聯慣性導航系統進行初始對準，確定載體相對導航坐標 系的初始姿態，得到姿態四元數的初始值；
步驟3、確定姿態更新周期// = ^-^)_1,所述姿態更新周期/Z等於AM咅的 光纖陀螺採樣周期& = 所述7V為大於零的整數；
步驟4、採集光纖陀螺輸出的載體相對於慣性坐標系的角速度計算旋轉矢量 的增量A^;
步驟5、通過旋轉矢量與四元數的關係，得到姿態更新周期/f內姿態更新四 元數g(//)，
formula see original document page 8其中p為旋轉矢量增量的模，formula see original document page 8
步驟6、由姿態四元數更新方程更新姿態四元數
其中姿態更新四元數《(//)由步驟5得到，0(、) 、 g(^)分別表示載體在G 、々M 時刻的姿態四元數；
步驟7、利用步驟6獲得的《,時刻姿態四元數^(^)=[《。&《2 ^:r計算 載體坐標系b系相對於導航坐標系n系的捷聯矩陣r，
formula see original document page 9步驟8、利用步驟7獲得的捷聯矩陣r求得載體相對導航坐標系的姿態角， 導航坐標系選取為遊動方位坐標系，則載體的格網航向角^、縱搖角e、橫搖角 y都可由捷聯矩陣r中的元素來表示，
formula see original document page 933
其中格網航向角^的定義域為(0。,360。)；縱搖角^的定義域為(-90°，90°);橫搖 角/的定義域為(-卯°,90°)，
由姿態角的定義域得到它們的真值， 格網航向角的真值為
formula see original document page 9縱搖角的真值為 橫搖角的真值為
formula see original document page 9本發明還可以包括
1、 在步驟1中所述的載體的初始位置由GPS裝置或者是外部的高精度組合
導航設備提供，所述載體的初始速度值由DVL都卜勒計程儀或者是外部的高精 度組合導航設備提供。
2、 在步驟2中所述的載體相對導航坐標系的初始姿態確定分為兩種情形
當載體處於靜基座時，採用基於古典控制理論的自對準方法；當載體處於動基座
時，採用基於卡爾曼濾波理論的組合對準方法。
3、在步驟4中所述的旋轉矢量增量/^的計算具體步驟如下 步驟41: 由光纖陀螺輸出的輸出計算A-中的慣性可測項，即角增量a;由
姿態更新周期和光纖陀螺採樣周期的關係= 7W2;每個姿態更新周期內己知
W + 1個光纖陀螺採樣角速度叫,^，L 則姿態更新周期i/內的角增量"為
formula see original document page 10其中formula see original document page 10步驟42:由光纖陀螺輸出的輸出計算A^中的非慣性可測項，即圓錐補償項 々；由姿態更新周期和光纖陀螺採樣周期的關係// = 7^;每個姿態更新周期// 內已知JV + 1個光纖陀螺採樣角速度叫,^,L ；
在姿態更新周期H內，圓錐補償項々由光纖陀螺採樣角速度叉乘項的線性 組合來計算
其中《^為優化係數，在典型圓錐環境中確定；
步驟43:將步驟41中得到的姿態更新周期/7內角增量cr，與步驟41中得 到的姿態更新周期//內圓錐補償項々相加，得到姿態更新周期7/內旋轉矢量增
設置A^=3，陀螺在/時刻採樣角速度w。，並且每個姿態更新周期//，在 /+2f//3、 ^//時刻採樣q、 ％、 得到姿態更新周期if內旋轉矢量增量A-為
formula see original document page 10
設置A^2，陀螺在,時刻採樣角速度叫，並且每個計算周期/f內，在/+///入200710144846.2 說明書第5/8頁
+//時刻採樣兩次《,、 ％;得到姿態更新周期//內旋轉矢量增量厶^為
formula see original document page 11
其中
formula see original document page 11
本發明與傳統方法相比較所的優點主要體現在
(1) 針對光纖陀螺輸出為角速度的情形，不同於傳統測量方法以角增量作 為輸入，發明方法直接以角速度輸入，以角速度的叉乘項來擬和圓錐補償項。避 免了使用光纖陀螺時，傳統方法對於划船補償項的測量偏差。
(2) 相對於提高捷聯慣性導航系統姿態計算精度的另一種途徑選用高性 能的光纖陀螺。發明的測量方法只需要作導航軟體設計，不需要增加捷聯慣性導 航系統的製造成本。
(四)
圖1為本發明的適合光纖陀螺捷聯慣性導航系統的姿態測量方法流程圖。 圖2為典型圓錐環境中，發明的姿態測量方法；傳統的姿態測量方法的實驗
曲線。典型圓錐環境定義為載體的兩個正交軸上存在同頻率，相位差90度的
角振動。光纖陀螺的採樣頻率為100Hz。
(五)
具體實施例方式
下面結合附圖舉例對本發明做更詳細地描述
結合圖1，本發明的適合於光纖陀螺捷聯慣性導航系統的姿態測量方法包括 如下步驟
步驟1、通過外部設備確定載體的初始位置參數與初始速度值。
步驟2、光纖陀螺捷聯慣性導航系統進行初始對準，確定載體相對導航坐標
系的初始姿態，得到姿態四元數的初始值。
步驟3、確定姿態更新周期/f-^-、—,，所述姿態更新周期i/等於W倍的
光纖陀螺採樣周期/2 所述iV為大於零的整數。
步驟4、採集光纖陀螺輸出的載體相對於慣性坐標系的角速度計算旋轉矢量 的增量A^。
步驟5、由旋轉矢量與四元數的關係，得到姿態更新周期Z/內姿態更新四元數剩。formula see original document page 122
(1)
其中p為旋轉矢量增量的模。
步驟6、由姿態四元數更新方程更新姿態四元數
抓) 剩 (2)
其中姿態更新四元數《(//)由步驟5求得。、 2d)分別表示載體在^ 、 ^
時刻的姿態四元數。
步驟7、利用步驟6獲得的^時刻姿態四元數2^J二[《。& &《3:^計算 載體坐標系b系相對於導航坐標系n系的捷聯矩陣r。
2(她+頓3) 《o212 +《22 + 2(秘-錢)
2(《《3—&《2)2(《2《3+《0仏) 《。2—《2—《22 +《32
(3)
步驟8、利用步驟7獲得的捷聯矩陣r求得載體相對導航坐標系的姿態角。
導航坐標系選取為遊動方位坐標系，則載體的格網航向角^、縱搖角^、橫搖角
^都可由捷聯矩陣r中的元素來表示。
formula see original document page 12
(4)
其中格網航向角^的定義域為(0°,360°);縱搖角^的定義域為(-卯。,9(0;橫搖 角y的定義域為(-90°，90°)。
由姿態角的定義域可以確定它們的真值。格網航向角的真值為formula see original document page 12縱搖角的真值為
formula see original document page 13
橫搖角的真值為
formula see original document page 13
在步驟1中所述的載體的初始位置由GPS裝置或者是外部的高精度組合導 航設備提供，所述載體的初始速度值由DVL都卜勒計程儀或者是外部的高精度 組合導航設備提供。
在步驟2中所述的載體相對導航坐標系的初始姿態確定分為兩種情形當載 體處於靜基座時，可以採用基於古典控制理論的自對準方法；當載體處於動基座 時，可以採用基於卡爾曼濾波理論的組合對準方法。
在步驟4中所述的旋轉矢量增量A^的計算具體步驟如下
步驟41:由光纖陀螺輸出的輸出計算A^中的慣性可測項，即角增量a。由 姿態更新周期和光纖陀螺採樣周期的關係= 。每個姿態更新周期內已知 W + l個光纖陀螺採樣角速度cv^,L c^。則姿態更新周期if內的角增量a為
formula see original document page 13(5)
其中
步驟42:由光纖陀螺輸出的輸出計算A-中的非慣性可測項，即圓錐補償項 々。由姿態更新周期和光纖陀螺採樣周期的關係// = ^//。每個姿態更新周期// 內已知W + l個光纖陀螺採樣角速度w。，^，L cv。
在姿態更新周期//內，圓錐補償項/ 可以由光纖陀螺採樣角速度叉乘項的 線性組合來計算
formula see original document page 13 (7) 其中/^_,為優化係數，可以在典型圓錐環境中確定。
步驟43:將步驟41中得到的姿態更新周期/Z內角增量a，與步驟41中得 到的姿態更新周期內圓錐補償項-相加，得到姿態更新周期內旋轉矢量增
A—a +々 (8) 設置W=3，陀螺在/時刻採樣角速度《。，並且每個姿態更新周期//，在/+///3、 r+2f// 、 /+//時刻採樣^、 w2、 《3。得到姿態更新周期//內旋轉矢量增量厶^為
formula see original document page 14
.叫x叫+—叫x叫+ 2240 Q3 56 13 2240
formula see original document page 14
(9)
其中
設置A^2，陀螺在Z時刻採樣角速度叫，並且每個計算周期/f內，在/+///2、 ,+//時刻採樣兩次^、 《2。得到姿態更新周期/Z內旋轉矢量增量A^為
其中
formula see original document page 14(10)
在姿態更新周期//內，姿態測量方法所使用的光纖陀螺採樣數越多，姿態 測量方法應對圓錐補償的效果就越好，光纖陀螺捷聯慣性導航系統輸出的姿態精 度就越高。
權利要求
1、一種適合於光纖陀螺的載體姿態測量方法，其特徵是步驟1、通過外部設備確定載體的初始位置參數與初始速度值；步驟2、光纖陀螺捷聯慣性導航系統進行初始對準，確定載體相對導航坐標系的初始姿態，得到姿態四元數的初始值；步驟3、確定姿態更新周期H＝tm-tm-1，所述姿態更新周期H等於N倍的光纖陀螺採樣周期h＝tl-tl-1，所述N為大於零的整數；步驟4、採集光纖陀螺輸出的載體相對於慣性坐標系的角速度計算旋轉矢量的增量Δφ；步驟5、通過旋轉矢量與四元數的關係，得到姿態更新周期H內姿態更新四元數q(H)，id="icf0001" left = "5" left = "5"/>步驟6、由姿態四元數更新方程更新姿態四元數其中姿態更新四元數q(H)由步驟5得到，Q(tm)、Q(tm-1)分別表示載體在tm、tm-1時刻的姿態四元數；步驟7、利用步驟6獲得的tm時刻姿態四元數Q(tm)＝[q0q1q2q3]T計算載體坐標系b系相對於導航坐標系n系的捷聯矩陣T，
2、 根據權利要求1所述的適合於光纖陀螺的載體姿態測量方法，其特徵是在步驟l中所述的載體的初始位置由GPS裝置或者是外部的高精度組合導航設 備提供，所述載體的初始速度值由DVL都卜勒計程儀或者是外部的高精度組合 導航設備提供。
3、 根據權利要求1或2所述的適合於光纖陀螺的載體姿態測量方法，其特徵是在步驟2中所述的載體相對導航坐標系的初始姿態確定分為兩種情形當載體處於靜基座時，採用基於古典控制理論的自對準方法；當載體處於動基 座時，採用基於卡爾曼濾波理論的組合對準方法。
4、 根據權利要求1或2所述的適合於光纖陀螺的載體姿態測量方法，其特徵是在步驟4中所述的旋轉矢量增量A^的計算具體步驟如下步驟41:由光纖陀螺輸出的輸出計算 中的慣性可測項，即角增量a; 由姿態更新周期和光纖陀螺採樣周期的關係// = 7^;每個姿態更新周期內 己知^ + 1個光纖陀螺採樣角速度^,0;1山fi^:則姿態更新周期i/內的角增量cr 為其中C>4^，-綠,) ；步驟42:由光纖陀螺輸出的輸出計算A^中的非慣性可測項，即圓錐補償 項"；由姿態更新周期和光纖陣螺採樣周期的關係// = 7^;每個姿態更新周期 Z/內已知iV + l個光纖陀螺採樣角速度叫,q，L在姿態更新周期內，圓錐補償項々由光纖陀螺採樣角速度叉乘項的線性 組合來計算AM其中i^—,為優化係數，在典型圓錐環境中確定；步驟43:將步驟41中得到的姿態更新周期/Z內角增量a，與步驟41中得 到的姿態更新周期內圓錐補償項"相加，得到姿態更新周期//內旋轉矢量增 量 ；設置A^3，陀螺在^時刻採樣角速度w。，並且每個姿態更新周期//，在r+W/J、 汁2/// 、 /+//時刻採樣^、 《2、 w3;得到姿態更新周期7/內旋轉矢量增量A^ 為formula see original document page 4其中formula see original document page 4設置iV=2，陀螺在/時刻採樣角速度cy。，並且每個計算周期//內，在/+//時刻採樣兩次^,、份2;得到姿態更新周期/f內旋轉矢量增量A-為formula see original document page 5 其中formula see original document page 56
5、根據權利要求3所述的適合於光纖陀螺的載體姿態測量方法，其特徵是: 在步驟4中所述的旋轉矢量增量A^的計算具體步驟如下步驟41:由光纖陀螺輸出的輸出計算A^中的慣性可測項，即角增量"； 由姿態更新周期和光纖陀螺採樣周期的關係/Z-iW/;每個姿態更新周期//內 已知7^ + 1個光纖陀螺採樣角速度0。,^,山6^;則姿態更新周期Z/內的角增量a 為formula see original document page 5其中formula see original document page 5步驟42:由光纖陀螺輸出的輸出計算A-中的非慣性可測項，即圓錐補償 項/ ;由姿態更新周期和光纖陀螺採樣周期的關係/Z-A^;每個姿態更新周期 a內已知7V + I個光纖陀螺採樣角速度叫，w,，L c^;在姿態更新周期//內，圓錐補償項-由光纖陀螺採樣角速度叉乘項的線性 組合來計算formula see original document page 5其中i^—,為優化係數，在典型圓錐環境中確定；步驟43:將步驟41中得到的姿態更新周期/Z內角增量"，與步驟41中得 到的姿態更新周期//內圓錐補償項-相加，得到姿態更新周期H內旋轉矢量增formula see original document page 5設置W=3,陀螺在/時刻採樣角速度0。，並且每個姿態更新周期//，在 /+2///3、 /+//時刻採樣6>1、 w2、 《3;得到姿態更新周期i/內旋轉矢量增量A^ 為其中formula see original document page 2設置7V=2，陀螺在Z時刻採樣角速度叫，並且每個計算周期W內，在 Z+i/時刻採樣兩次o;,、 w2;得到姿態更新周期i/內旋轉矢量增量A^為formula see original document page 2其中formula see original document page 2
全文摘要
本發明提供的是一種適合於光纖陀螺的載體姿態測量方法。包括通過外部設備確定載體的初始位置參數與初始速度值；光纖陀螺捷聯慣性導航系統進行初始對準，確定載體相對導航坐標系的初始姿態，得到姿態四元數的初始值；確定姿態更新周期H＝tm-tm-1；採集光纖陀螺輸出的載體相對於慣性坐標系的角速度計算旋轉矢量的增量Δφ；通過旋轉矢量與四元數的關係，得到姿態更新周期H內姿態更新四元數q(H)；由姿態四元數更新方程更新姿態四元數；計算載體坐標系b系相對於導航坐標系n系的捷聯矩陣T；求載體相對導航坐標系的姿態角等步驟。本發明解決了在載體高動態環境或是高頻率振動環境中，圓錐效應對於載體姿態測量精度產生影響的問題。
文檔編號G01C21/18GK101187561SQ20071014484
公開日2008年5月28日 申請日期2007年12月18日 優先權日2007年12月18日
發明者強 於, 磊 吳, 周廣濤, 奔粵陽, 楓 孫, 博 徐, 程建華, 陳世同, 偉 高, 高洪濤 申請人:哈爾濱工程大學