專利名稱：船用光纖陀螺捷聯系統初始姿態確定方法
技術領域：
本發明涉及的是一種測量方法，特別是涉及一種捷聯航姿系統的初始姿態確定技術。
背景技術：
捷聯慣導系統SINS是一種自主式導航系統，采用牛頓第二定律在初始信息的基礎上進行積分運算，可以連續、實時地提供位置、速度和姿態信息，具有隱蔽性好，不受干擾等優點，廣泛應用于軍事武器系統中。而初始信息的取得是整個系統的十分關鍵環節，如何快速準確獲得初始信息是慣性導航領域一項非常重要課題。
捷聯慣導系統初始姿態確定的過程包括粗對準和精對準階段；初始姿態確定要求包括精度性和快速性兩個方面，但是這兩個方面是十分難以協調的，如何在最短的時間內獲得最好的對準精度？傳統的精對準方法在精度上基本能滿足要求，但對準時間一般需要4～5分鐘，甚至更多，這對于那些要求快速性的艦船武器導航系統將是一個致命的弱點。
發明內容
本發明的目的在于提供一種既權衡時間，又權衡對準的精度，使在對準精度與傳統方法相當的前提下對準時間和收斂速度大大提高的船用光纖陀螺捷聯系統初始姿態確定方法。
本發明的目的是這樣實現的 (1)對SINS進行預熱準備； (2)SINS預熱準備完后，調整SINS進入初始對準的粗對準階段，采集船用陀螺儀輸出和加速度計輸出，通過粗對準結束后得到的粗略的初始捷聯姿態矩陣計算地理坐標系和真實地理坐標系之間的三個誤差失準角φxn、φyn和φzn，當三個誤差失準角小于10度時轉入精對準步驟、否則重新進行粗對準； (3)精對準，在粗對準的基礎上，繼續采集船用陀螺儀輸出和加速度計輸出，采用線性二次型LQ最優控制技術，得到精對準過程中所需要的最佳修正控制角速度，讓精對準過程快速收斂下來，精度還能與傳統方法相當。
所述的精對準包括 1)建立船用光纖陀螺捷聯系統的處于搖擺基座下狀態方程和量測方程； 2)進行船用光纖陀螺捷聯系統的抗干擾設計； 3)確定最優控制的線性二次型性能指標； 4)采用動態規劃的遞推最優控制增益； 5)離線算出最優控制增益矩陣L。
本發明的原理是首先建立船用光纖陀螺捷聯系統的空間狀態方程；對船用捷聯系統的干擾項進行抗干擾設計，得到去除干擾后的空間狀態方程；然后給出最優控制的線性二次型性能指標；最后采用動態規劃的遞推算出最優的控制增益。實際精對準過程中，將離線算出的最優控制增益加入到對準回路中即可。
本發明的精對準方法，采用現代最優控制理論中的線性二次型(LQ)最優控制系統，在二次型性能指標中，將精對準的最終對準精度、對準的時間和最優控制量的大小進行最優控制，使得二次型性能指標函數取得極小值。既權衡到了時間，又權衡到了對準的精度，該初始姿態求解方案的對準精度與傳統方法相當，但對準時間和收斂速度大大提高，相對于傳統的方案，該方法只花費3秒鐘左右就完成了整個精對準過程。
本發明的初始姿態求解新方案與現有技術相比的優點和自身的優勢 (1)在二次型性能指標中，將精對準的最終對準精度、對準的時間和最優控制量的大小進行最優控制，使得二次型性能指標函數取得極小值。既權衡到了時間，又權衡到了對準的精度，相對于其它傳統的對準方法，方案的對準時間短，收斂速度快，整個過程只需幾秒鐘； (2)針對船用捷聯系統中的干擾，通過進行差分變換，將階躍型干擾項從方程去除，達到抗干擾的效果； (3)反饋控制增益采用動態規劃的遞推法離線計算，由于系統能觀能控，反饋增益矩陣的系數都趨于常值(曲線從后往前看)，實際精對準時將增益矩陣的系數都取常值，并計算出最優控制量加入對準回路中就可完成用捷聯系統的精對準過程，這樣的做法減輕實際系統的計算量，不用在線計算控制增益也可以減少計算時間，提高整個系統的計算速度。
圖1是本發明的捷聯系統精對準的最優控制結構圖； 圖2是反饋控制矩陣L中L11、L21、L31的曲線圖； 圖3是反饋控制矩陣L中L12、L22、L32的曲線圖； 圖4是反饋控制矩陣L中L13、L23、L33的曲線圖； 圖5是反饋控制矩陣L中L14、L24、L34的曲線圖； 圖6是反饋控制矩陣L中L15、L25、L35的曲線圖； 圖7是反饋控制矩陣L中L16、L26、L36的曲線圖； 圖8是船用光纖陀螺捷聯系統初始姿態確定具體實施方式
； 圖9是搖擺基座下的初始精對準誤差失準角曲線。
具體實施例方式 下面結合附圖舉例對本發明做更詳細地描述 本發明的原理是首先建立船用光纖陀螺捷聯系統的空間狀態方程；對船用捷聯系統的干擾項進行抗干擾設計，得到去除干擾后的空間狀態方程；然后給出最優控制的線性二次型性能指標；最后采用動態規劃的遞推算出最優的控制增益。實際精對準過程中，將離線算出的最優控制增益加入到對準回路中即可。
1)建立船用光纖陀螺捷聯系統的處于搖擺基座下狀態方程和量測方程
其中ωxc、ωyc、ωzc為修正控制角速率，φxn、φyn、φzn為誤差角失準角；δωd、δad為干擾項。
為加速度在慣性系內的三周投影，
由于對準過程只需幾秒鐘，所以Cni中的t可以取常值(比如取t＝2)。
令狀態向量則上述捷聯系統初始對準的狀態方程和量測方程可寫成 其中
假設系統的采樣間隔為T秒，那么可得到上述狀態方程和量測方程的離散化形式 其中F＝eAT，F和G可以通過矩陣指數轉化為用矩陣級數求和法進行求解，v(k)、δad(k)不用實時求解，它可以通過下面的抗干擾設計而除去。
2)船用光纖陀螺捷聯系統的抗干擾設計 由離散化狀態方程式(3)可知，有一干擾項v(k)，δad(k)，其中v(k)為陀螺的常值漂移和隨機漂移等擾動，而δad(k)為加速度計常值偏置和隨機干擾等擾動。為了分析問題，我們完全可以認為v(k)、δad(k)為近似階躍型干擾。這樣我們可以通過下面的差分變換將v(k)、δad(k)進行消除。
定義各量的差分為 因為v(k)、δad(k)為近似階躍型干擾，顯然當k≥1時，有Δδad(k)＝0，Δv(k)＝0，對式(3)兩邊進行差分變換，得 Δx(k+1)＝FΔx(k)+GΔu(k) (5) Δy(k+1)＝CΔx(k+1) (6) 式(6)可進一步改寫為 y(k+1)-y(k)＝CΔx(k+1)(7) 將式(5)代入(7)式，可得 y(k+1)＝y(k)+CFΔx(k)+CGΔu(k)(8) 令結合式(5)、(8)，可寫出新的關于
的狀態方程為 其中 新的量測方程可寫為 3)給出最優控制的線性二次型性能指標 我們知道，進行捷聯慣導初始對準的實質上是讓誤差失準角快速收斂到零，通過分析，我們發現當三個誤差角收斂趨近于零時，系統的觀測量也將趨于零，這正好符合最優控制中的輸出調節器問題——即用不大的控制量，使輸出趨向零值附近。所以我們要進行最優控制的目的就是在給定的時間內讓y(t)的各分量趨近于零。輸出誤差向量e(t)為 e(t)＝0-y(t)＝-y(f)(11) 因此，控制任務就變為尋求一個控制u(t)，在有限的時間內，以使得系統的輸出誤差盡可能的小，而且對于u(t)也有要求，有時對u(t)有幅值要求，或者對于u(t)內各分量的權值有所要求。把這些性能要求用離散形式二次型性能指標來表示的話，可得 其中
為6×6半正定常數陣，
為6×6半正定常數陣，
為3×3正定常數陣。式中第一項是控制姿態的精度，第二項控制系統的收斂速度，第三項協調修正控制角速度的大小。
4)采用動態規劃的遞推最優控制增益 由式(9)，式(10)和式(12)的離散化二次型性能指標可以看出，這符合最優調節器問題用最優的控制量，在有限的時間內使輸出趨向于零值附近。
取采樣時間T＝0.01秒，取b1、b2、b2分別為常數(用于控制控制量的權值)，令為了便于分析，可取D＝I6×6，計算出
對(9)、(10)的線性離散化系統進行能控性和能觀性判定。
能控性判定所以能控； 能觀性判定所以能觀。
系統能控又能觀，則最優控制一定存在，且求解出來的控制系統是漸進穩定的。
設反饋控制矩陣則可得如下遞推公式 將代入式(11)中的可得 其中L＝[L1L2]，L1、L2是相應的3×3分塊矩陣，對式(12)兩邊作求和運算并整理可以得到 由上式看出，最優控制u(k)與初始條件x(0)、u(0)有關。實際上，x(0)和u(0)是難以預先知道的，因此通常只實現如下的控制規律 由上式(14)可見，u(k)由兩部分組成，一部分是狀態x(k)的比例調節，另一部分是輸出量的求和，可以看成是輸出量的積分調節。圖1表示了捷聯系統初始精對準的最優控制結構。
5)離線算出的最優控制增益矩陣L 反饋控制矩陣L中各項控制增益可以離線計算，所以可以事先用計算機計算出L中各項增益，然后再加入實際系統進行精對準最優控制，這樣可以達到減輕實際硬件系統的計算量，也可減少實際船用光纖陀螺捷聯系統的精對準時間。
圖2-7是按照狀態方程和線性二次型性能指標離線計算出來的反饋控制增益矩陣，實際精對準中不需要在計算。
結合圖8，本發明的具體實施方法如下 1.用PC機離線計算出LQ最優控制的控制增益矩陣的系數 按照上面介紹的方法，先得到船用光纖陀螺捷聯系統的狀態空間方程和二次型性能指標，進行抗干擾處理，采用動態規劃的遞推法計算出反饋增益系數。
2.船用光纖陀螺捷聯系統的準備 SINS開機后，進入準備狀態。
3.船用光纖陀螺捷聯系統的粗對準階段 SINS準備完畢，采集陀螺儀輸出和加速度輸出，采用二階調平+方位估算的粗對準方式對系統進行粗對準，獲得一個粗略的初始捷聯姿態矩陣，使真實地理系和計算地理系的失準角在10度以內(一般的粗對準方法都沒有任何問題)。
4.船用光纖陀螺捷聯系統的精對準階段 粗對準完畢后，進入系統的精對準過程。將用PC機離線計算出的反饋增益系數加入對準控制回路中，通過對準回路得到實時的最佳的修正控制角速度，以此修正角速度達到精對準的目的。
5.初始姿態確定完畢，對準過程結束。
對本發明的有益效果說明如下 假設有如下環境 (1)加速度計和陀螺的性能如下 ①陀螺的常值漂移為εx＝εy＝εz＝0.1o/h；陀螺的隨機漂移為0.01o/h； ②陀螺刻度系數誤差Kgx＝Kgy＝Kgz＝10-4； ③加速度計零位偏差x＝y＝z＝10-4g；加速度計隨機干擾10-5g； ④加速度計刻度系數誤差Kax＝Kay＝Kaz＝10-4； ⑤粗對準結束后姿態誤差角(4°5°5°)；⑥取采樣時間T＝0.01秒，各加權矩陣取 (2)假設船舶在海浪激勵下分別繞方位軸、縱搖軸和橫搖軸作搖擺運動。其模型為
由垂蕩、縱蕩和橫蕩引起的線加速度為
其中，ADx＝0.02m，ADy＝0.02m，ADz＝0.16m；ωDi＝2π/TDi，且TDx＝8s，TDy＝10s，TDz＝10s；Di為
上服從均勻分布的隨機相位； 載體初始位置北緯45.7796°，東經126.6705°； 通過計算機模擬仿真，如果初始時刻計算平臺坐標系和真實平臺坐標系間的三個誤差角小于10°的話，整個對準過程就能在幾秒鐘內完成。
搖擺基座下的初始精對準誤差失準角曲線如圖9所示。
圖中只用了3秒就已經達到了對準的精度，經過3秒后，北向水平誤差角收斂到0.05°左右，東向水平誤差失準角收斂到0.08°左右，方位誤差失準角收斂到0.r左右。
權利要求
1.一種船用光纖陀螺捷聯系統初始姿態確定方法，其特征是
(1)對SINS進行預熱準備；
(2)SINS預熱準備完后，調整SINS進入初始對準的粗對準階段，采集船用陀螺儀輸出和加速度計輸出，通過粗對準結束后得到的粗略的初始捷聯姿態矩陣計算地理坐標系和真實地理坐標系之間的三個誤差失準角φxn、φyn和φzn，當三個誤差失準角小于10度時轉入精對準步驟、否則重新進行粗對準；
(3)精對準，在粗對準的基礎上，繼續采集船用陀螺儀輸出和加速度計輸出，采用線性二次型LQ最優控制技術，得到精對準過程中所需要的最佳修正控制角速度，讓精對準過程快速收斂下來，精度還能與傳統方法相當。
2.根據權利要求1所述的船用光纖陀螺捷聯系統初始姿態確定方法，其特征是所述的精對準包括
1)建立船用光纖陀螺捷聯系統的處于搖擺基座下狀態方程和量測方程；
2)進行船用光纖陀螺捷聯系統的抗干擾設計；
3)確定最優控制的線性二次型性能指標；
4)采用動態規劃的遞推最優控制增益；
5)離線算出最優控制增益矩陣L。
全文摘要
本發明提供的是一種船用光纖陀螺捷聯系統初始姿態確定方法。包括(1)對SINS進行預熱準備；(2)SINS預熱準備完后，調整SINS進入初始對準的粗對準階段，采集船用陀螺儀輸出和加速度計輸出，通過粗對準結束后得到的粗略的初始捷聯姿態矩陣計算地理坐標系和真實地理坐標系之間的三個誤差失準角φxn、φyn和φzn，當三個誤差失準角小于10度時轉入精對準步驟、否則重新進行粗對準；(3)精對準，在粗對準的基礎上，繼續采集船用陀螺儀輸出和加速度計輸出，采用線性二次型LQ最優控制技術，得到精對準過程中所需要的最佳修正控制角速度。本發明既權衡到了時間，又權衡到了對準的精度，該初始姿態求解方案的對準精度與傳統方法相當，但對準時間和收斂速度大大提高。
文檔編號G01C21/10GK101187562SQ200710144849
公開日2008年5月28日 申請日期2007年12月18日 優先權日2007年12月18日
發明者博 徐, 偉 高, 周廣濤, 奔粵陽, 李兵軍, 磊 吳, 程建華, 陳世同, 強 于, 高洪濤 申請人:哈爾濱工程大學