王 林，吳文啟，魏 國，潘獻飛，高春峰

（1.國防科學技術大學 機電工程與自動化學院，長沙 410073；2.國防科學技術大學 光電科學與工程學院，長沙 410073）

雙航海慣導聯(lián)合旋轉調制協(xié)同定位與誤差參數(shù)估計

王 林1，吳文啟1，魏 國2，潘獻飛1，高春峰2

（1.國防科學技術大學 機電工程與自動化學院，長沙 410073；2.國防科學技術大學 光電科學與工程學院，長沙 410073）

單軸/雙軸旋轉調制航海慣導備份配置滿足了艦艇對于定位精度、可靠性、成本的綜合要求，但系統(tǒng)間缺少信息融合。針對此問題，以單軸旋轉慣導的姿態(tài)誤差、速度誤差、位置誤差與雙軸旋轉慣導對應誤差的差值以及兩套慣導的陀螺常值漂移、水平加速度計常值零偏為系統(tǒng)狀態(tài)，并以二者間扣除桿臂效應后的速度及位置的差值為觀測量，通過聯(lián)合旋轉調制，改變兩套系統(tǒng) IMU的相對姿態(tài)關系。分段常值可觀測性分析表明，所有系統(tǒng)狀態(tài)完全可觀。建立了定位誤差預測方程，對單軸旋轉慣導方位陀螺漂移造成的定位誤差進行預測補償。實驗結果表明，對單軸旋轉慣導方位陀螺漂移造成的定位誤差預測補償后，其定位誤差減小了30%，不僅滿足了高可靠性的要求，而且提高了故障情況下的導航精度。

單軸旋轉慣導；雙軸旋轉慣導；協(xié)同定位；誤差參數(shù)估計

為滿足各類艦艇對長航時、高精度導航定位的需求，國內外對激光陀螺單軸、雙軸(或三軸)旋轉慣導進行了深入研究[1-7]。影響高精度激光陀螺慣導精度的誤差因素分為確定性誤差和隨機性誤差(主要是陀螺角度隨機游走)[1-2,8]。

單軸旋轉慣導無法調制方位陀螺漂移的影響，其會造成與時間成正比的定位誤差[2,4]；雙軸旋轉慣導可調制所有陀螺漂移及加速度計零偏的影響，決定其定位精度的主要因素是陀螺的角度隨機游走[1,3,5]，其造成的定位誤差正比于時間的均方根，相較于單軸旋轉慣導而言其定位精度理論上更高，但其結構相對復雜、成本相對更高、可靠性低于單軸旋轉慣導[2]。綜合考慮定位精度、成本等方面的因素，對可靠性要求很高的艦艇而言，單軸、雙軸旋轉慣導備份配置是一種合理的方案[2]。通過系統(tǒng)間的信息融合，對單軸旋轉慣導方位陀螺漂移造成的確定性誤差進行估計和補償以提高其定位精度，既能保證單軸/雙軸旋轉慣導備份配置正常情況下的定位精度，又能保證某一設備故障情況下的定位精度，特別是在雙軸旋轉慣導故障情況下。

文獻[9]合理選取系統(tǒng)狀態(tài)，以兩套旋轉慣導間的導航參數(shù)差值為觀測量，利用卡爾曼濾波，估計兩套系統(tǒng)的陀螺常值漂移、加速度計常值零偏。以此為基礎，本文通過合理地設計聯(lián)合旋轉調制策略，建立單軸旋轉慣導和雙軸旋轉慣導的聯(lián)合誤差狀態(tài)方程，以系統(tǒng)間扣除桿臂效應后的速度、位置的差值為觀測量，實現(xiàn)兩套系統(tǒng)陀螺常值漂移、加速度計常值零偏的估計，并對單軸旋轉慣導的方位陀螺漂移造成的確定性的長期定位誤差進行預測補償，使得單軸/雙軸備份配置的協(xié)同定位精度在各種工作條件下都能得到保證。

本文以兩套旋轉慣導的導航參數(shù)輸出量的差值為觀測量進行濾波器設計，無需改動現(xiàn)有導航系統(tǒng)的導航算法內部架構；對兩套系統(tǒng)的相對安裝關系沒有要求，具有實施簡單的優(yōu)點；實現(xiàn)了艦艇搭載的多套旋轉慣導系統(tǒng)間導航信息的融合利用。

1 單軸/雙軸旋轉慣導聯(lián)合狀態(tài)方程

以單軸旋轉慣導、雙軸旋轉慣導的姿態(tài)誤差、速度誤差、位置誤差的差值為系統(tǒng)狀態(tài)，同時增加兩套系統(tǒng)各自三個陀螺常值漂移、兩個水平方向的加速度計常值零偏作為系統(tǒng)狀態(tài)，共計17個誤差狀態(tài)：

考慮到船上兩套慣導系統(tǒng)安裝位置很近，位置、速度、比力的差異很小，則狀態(tài)方程為

其中：

系統(tǒng)噪聲為

系統(tǒng)噪聲驅動矩陣為

其中：單軸旋轉慣導體坐標系b1定義為右-前-上；雙軸旋轉慣導體坐標系b2定義同上；導航坐標系n為當?shù)厮降乩碜鴺讼?東-北-天)；分別為單軸/雙軸旋轉慣導姿態(tài)誤差差值的東向、北向、天向分量，分別為單軸/雙軸旋轉慣導的東向、北向速度誤差差值，分別為單軸/雙軸旋轉慣導的緯度、經(jīng)度誤差差值，εx1、εy1、εz1為單軸旋轉慣導體坐標系下相應坐標軸的陀螺常值漂移，εx2、εy2、εz2為雙軸旋轉慣導體坐標系下相應坐標軸的陀螺常值漂移，▽x1、▽y1、▽x2、▽y2為兩套系統(tǒng)各自體坐標系下相應水平坐標軸的加速度計常值零偏；w(t)各元素為兩套系統(tǒng)各自對應的陀螺、加速度計噪聲；高度通道相關的速度和位置誤差及加速度計零偏未考慮。

2 觀測方程

扣除桿臂造成的相對速度誤差，以單軸/雙軸旋轉慣導間的速度之差、位置之差為相應的觀測量：

觀測方程為

3 聯(lián)合旋轉調制策略編排及可觀性分析

根據(jù)式(1)～(4)，將單軸/雙軸旋轉慣導姿態(tài)誤差差值微分方程寫為矢量形式，有：

從式(11)(12)可看出，要實現(xiàn)單軸/雙軸旋轉慣導各自的陀螺常值漂移從的估計值中分離，關鍵在于：1）兩套慣導系統(tǒng)之間的相對姿態(tài)存在變化；2）兩套系統(tǒng)各自姿態(tài)均需變化。

式中：b1(0)為聯(lián)合旋轉調制開始時刻單軸旋轉慣導的載體系，此時轉動機構歸零；b2(0)為聯(lián)合旋轉調制開始時刻雙軸旋轉慣導的載體系，此時轉動機構歸零；單/雙軸旋轉慣導旋轉機構歸零時，二者載體系相對水平姿態(tài)角0γ、0θ滿足小角假設，相對航向角為0ψ；為t時刻單軸旋轉慣導相對其初始零位繞方位軸轉動的角度；ψ2(t)、γ2(t)為t時刻雙軸旋轉慣導相對其初始零位分別繞方位軸、橫滾軸轉動的角度。

將式(14)代入式(13)，有：

單軸旋轉慣導相對其轉動機構零位進行 4位置轉停，停轉角度分別為 0°、90°、180°、270°；雙軸旋轉慣導分別繞其轉動機構橫滾軸、方位軸進行16次序轉停[5]，兩個軸向的停轉角度均為0°、180°。若定義b1(t)、b2(t)坐標系間相對航向角、相對橫滾角取值定義域分別為則由式(15)可得b1(t)、b2(t)坐標系間相對姿態(tài)角取值如表1所示，此處忽略了與0γ、0θ有關的小量的影響。

表1 相對姿態(tài)角Tab.1 Relativ e attitude angle

由表1可以看出，b1(t)、b2(t)坐標系間相對姿態(tài)角取值可達 4×2=8種情況，包含了方位軸相對姿態(tài)角和橫滾軸相對姿態(tài)角兩個軸向的變化。

采用分段線性定常系統(tǒng)可觀性分析方法[10]，根據(jù)式(2)～(6)(10)，求取系統(tǒng)的總可觀性矩陣

若Q(r)滿秩，則該系統(tǒng)完全可觀。結果表明，通過聯(lián)合旋轉調制策略編排，當單/雙軸旋轉慣導間的相對姿態(tài)矩陣既包含了方位軸相對姿態(tài)角的變化，也包含了橫滾軸相對姿態(tài)角γ2(t)的變化時，滿秩，系統(tǒng)狀態(tài)完全可觀。聯(lián)合旋轉調制策略使得單/雙軸旋轉慣導之間的相對姿態(tài)存在變化，實現(xiàn)了各自陀螺常值漂移的分離估計；加速度計常值零偏的分離估計類似。

4 定位誤差預測

圖1所示為一種單軸/雙軸備份配置示意圖。通過單軸/雙軸旋轉慣導系統(tǒng)間的信息融合對各自慣性器件的常值誤差進行估計，雖然單軸旋轉慣導水平方向的陀螺漂移和加速度計零偏、雙軸旋轉慣導所有方向的陀螺漂移和加速度計零偏對各自定位的影響均被調制，但可用于監(jiān)控慣導系統(tǒng)中慣性器件的工作狀態(tài)以進行故障診斷。正常情況下對單軸旋轉慣導方位陀螺漂移進行估計，同時對其造成的確定性的長期定位誤差進行預測補償，補償方式為輸出校正。補償后的單軸旋轉慣導定位誤差主要為陀螺角度隨機游走造成的隨機性誤差。單軸旋轉慣導的可靠性通常高于雙軸旋轉慣導[2]，即使雙軸旋轉慣導出現(xiàn)故障，單軸旋轉慣導輸出的補償過確定性誤差的定位結果仍能保證較高的定位精度，這樣的配置既保證了可靠性，又能保證故障情況下的定位精度，同時節(jié)約了成本。

圖1 雙航海慣導配置Fig.1 Dual MAINS configuration

下面給出單軸旋轉慣導的定位誤差預測模型。

誤差狀態(tài)為

陀螺漂移、加速度計的零偏輸入為

誤差狀態(tài)方程為

其中：

各分塊矩陣同式(3)～(6)，離散化式(19)得到誤差狀態(tài)預測方程：

5 仿真分析與實驗驗證

聯(lián)合旋轉調制策略設置：單軸旋轉慣導繞方位軸周期性地進行4位置轉停，雙軸旋轉慣導繞橫滾軸、方位軸周期性地進行16次序轉停[5]，緯度、經(jīng)度分別為28.222°N、112.993°E。通過理想噪聲條件下半實物仿真實驗考察陀螺常值漂移、加速度計常值零偏的估計精度；通過含趨勢項噪聲條件下的仿真實驗考察單軸旋轉慣導定位誤差的預測補償情況，在此基礎上通過實際實驗進行了進一步驗證。

5.1 半實物仿真分析一

考察陀螺常值漂移、加速度計常值零偏的估計精度。陀螺、加速度計噪聲數(shù)據(jù)為兩套90型高精度激光陀螺航海慣導長時間（120 h）靜態(tài)測試數(shù)據(jù)按每5 min求均值，進而減去此均值所得，所得數(shù)據(jù)作為不包含趨勢項（主要為環(huán)境溫度敏感性誤差）的理想噪聲數(shù)據(jù)，該噪聲數(shù)據(jù)可以反映實際慣性器件的噪聲水平。陀螺角度隨機游走均優(yōu)于加速度計隨機噪聲的功率譜密度方根值均優(yōu)于

陀螺常值漂移、加速度計常值零偏的設定值均假定為單/雙軸旋轉慣導長時間對準結束時各漂移、零偏項估計補償后的殘余部分。

單軸旋轉慣導誤差參數(shù)：1）εz1的取值為 0.0003(°)/h，εx1、εy1的取值分別為 0.003 (°)/h、-0.002 (°)/h，方位陀螺及水平陀螺常值漂移值的設定以定位精度優(yōu)于1 nm / 72 h的單軸旋轉慣導誤差分配方案中對激光陀螺精度的要求為基準[4]；2）▽x1、▽y1的取值分別為（忽略高度通道）。

雙軸旋轉慣導誤差參數(shù)：1）εx2、εy2、εz2的取值分別為 0.004 (°)/h、-0.005 (°)/h、0.003 (°)/h；2）▽x2、的取值分別為（忽略高度通道）。

圖2、圖3分別給出了單/雙軸旋轉慣導聯(lián)合狀態(tài)卡爾曼濾波估計得到的陀螺常值漂移、加速度計常值零偏在前16 h的估計值。單軸旋轉慣導水平陀螺常值漂移估計誤差及雙軸旋轉慣導三個陀螺的常值漂移估計誤差均優(yōu)1.0×10-3(°)/h，單軸旋轉慣導方位陀螺常值漂移的估計誤差優(yōu)于5.0×10-5(°)/h，所有水平加速度計常值零偏的估計誤差在10-6g量級。

從上面的估計誤差值可以看出，與單軸旋轉慣導的方位陀螺常值漂移估計誤差相比，其水平陀螺常值漂移及雙軸旋轉慣導的陀螺常值漂移估計誤差相對較大，主要原因在于：經(jīng)旋轉調制后，單軸旋轉慣導的水平陀螺常值漂移及雙軸旋轉慣導的陀螺常值漂移均被調制掉，其對定位誤差的影響不明顯，而單軸旋轉慣導的方位陀螺常值漂移會造成與導航時間成正比的確定性誤差，其影響作用持續(xù)存在。

圖2 陀螺漂移估計Fig.2 Gyro drift estimation

圖3 加速度計零偏估計Fig.3 Accelerometers bias estimation

5.2 半實物仿真分析二

單軸/雙軸備份配置情況下，對單軸旋轉慣導方位陀螺漂移造成的確定性定位誤差進行預測補償，不僅能夠滿足高可靠性的要求，同時可以實現(xiàn)系統(tǒng)故障情況(特別是雙軸旋轉慣導故障)下定位精度的保證。

實際航海慣導系統(tǒng)工作時，陀螺漂移、加速度計零偏雖經(jīng)過溫度補償，但仍有可能存在未補償?shù)内厔蓓?。與仿真分析一中的陀螺、加速度計噪聲數(shù)據(jù)產(chǎn)生方式不同，此處陀螺、加速度計噪聲數(shù)據(jù)為三套 90型高精度激光陀螺航海慣導長時間(120 h)靜態(tài)測試數(shù)據(jù)整體求均值，進而減去此均值所得。因為是整個測試時間段內求均值進而減去此均值，得到的噪聲數(shù)據(jù)包含了相關趨勢項。將其中一套系統(tǒng)的測試噪聲數(shù)據(jù)作為單軸旋轉慣導的噪聲數(shù)據(jù)，另外兩套系統(tǒng)的噪聲數(shù)據(jù)作為雙軸旋轉慣導的噪聲數(shù)據(jù)。

單軸旋轉慣導分別與兩套雙軸旋轉慣導進行聯(lián)合狀態(tài)卡爾曼濾波，并根據(jù)單軸旋轉慣導的定位誤差預測模型對其方位陀螺漂移造成的確定性的定位誤差進行預測補償。兩次實驗均假定雙軸旋轉慣導在第36 h故障，故障時刻后由補償過確定性定位誤差的單軸旋轉慣導提供導航定位結果。

圖4～5為單軸旋轉慣導與第一套雙軸旋轉慣導的實驗結果示意圖。圖4給出了按照定位誤差預測模型計算得到的單軸旋轉慣導的緯度誤差、經(jīng)度誤差預測值，從圖中可以看出，該預測模型表現(xiàn)良好，特別是對于單軸旋轉慣導方位陀螺漂移造成的正比于時間的定位誤差具有較好的預測表現(xiàn)。圖5給出了單軸旋轉慣導補償確定性誤差前后的徑向位置誤差對比，補償過確定性定位誤差之后，誤差減小了30%以上。作為對比，圖中綠色虛線給出了雙軸旋轉慣導不存在假定故障時的徑向位置誤差，將其與補償過確定性定位誤差后的單軸旋轉慣導定位誤差曲線對比可以發(fā)現(xiàn)兩者精度相當。因此，通過兩套系統(tǒng)間的信息融合，提高了故障情況下的導航定位精度。

圖6～7為單軸旋轉慣導與第二套雙軸旋轉慣導的實驗結果示意圖。圖6給出了按照定位誤差預測模型計算得到的單軸旋轉慣導的緯度誤差、經(jīng)度誤差預測值，從圖中可以看出，該預測模型表現(xiàn)良好。圖7給出了單軸旋轉慣導補償確定性誤差前后的徑向位置誤差對比。補償過確定性定位誤差之后，定位精度提升明顯，并優(yōu)于此套雙軸旋轉慣導的定位精度。出現(xiàn)這種情況的原因在于：單軸旋轉慣導補償過方位陀螺漂移造成的確定性定位誤差之后，剩余部分主要是陀螺角度隨機游走造成的隨機性誤差，若其陀螺角度隨機游走小于雙軸旋轉慣導的陀螺角度隨機游走，其定位精度將優(yōu)于雙軸旋轉慣導的定位精度。

因此，通過兩套系統(tǒng)間的信息融合，對單軸旋轉慣導的定位誤差進行預測、補償，既滿足單軸/雙軸旋轉慣導備份配置可靠性的要求，又能提高故障情況下的導航定位精度。

圖4 單軸旋轉慣導位置誤差預測Fig.4 Position error prediction of one-indexing MAINS

圖5 位置誤差對比Fig.5 Position error comparison

圖6 單軸旋轉慣導位置誤差預測Fig.6 Position error prediction of one-indexing MAINS

圖7 位置誤差對比Fig.7 Position error comparison

5.3 實驗驗證

利用兩套 90型高精度激光陀螺航海慣導分別進行單軸旋轉調制、雙軸旋轉調制實驗，考察單/雙軸旋轉慣導備份配置情況下系統(tǒng)間信息融合的有效性，實驗時長為144 h（6天）。

假定雙軸旋轉慣導在第24 h發(fā)生故障，故障時刻之后由單軸旋轉慣導輸出導航定位結果，并對單軸旋轉慣導方位陀螺漂移造成的與導航時間成正比的確定性誤差進行輸出校正補償。圖8給出了單軸旋轉慣導緯度誤差及經(jīng)度誤差的實際值、預測值（均為量化值），從圖中可以看出，定位誤差預測效果較好，對其經(jīng)度誤差發(fā)散項的預測基本反映了其變化趨勢。

補償單軸旋轉慣導定位誤差預測值后，剩余部分主要為陀螺角度隨機游走造成的隨機性誤差。圖9給出了補償確定性誤差前后單軸旋轉慣導的定位誤差曲線，從圖中可以看出，在144 h導航時間內，補償前單軸旋轉慣導的最大定位誤差可達0.8（量化值），單軸旋轉慣導的定位誤差漂移顯著，補償后單軸旋轉慣導的最大定位誤差優(yōu)于0.5（量化值），其定位誤差漂移趨勢項得到抑制，定位誤差減小了30%，與無故障情況下的雙軸旋轉慣導定位精度相當（綠色虛線給出了不存在假定故障時的雙軸旋轉慣導的定位誤差曲線）。因此，在單軸/雙軸旋轉慣導備份配置情況下，通過補償單軸旋轉慣導的長期確定性誤差，既滿足了單軸/雙軸旋轉慣導備份配置可靠性的要求，又能提高故障情況下的導航定位精度。

圖8 單軸旋轉慣導位置誤差預測Fig.8 Position error prediction of one-indexing MAINS

圖9 位置誤差對比Fig.9 Position error comparison

6 結 論

本文建立了單軸/雙軸旋轉慣導的聯(lián)合狀態(tài)方程，在對系統(tǒng)狀態(tài)可觀性分析基礎上，通過合理地設計聯(lián)合旋轉調制策略，以扣除桿臂效應后單軸/雙軸旋轉慣導系統(tǒng)間的速度、位置差為觀測量，通過卡爾曼濾波實現(xiàn)了兩套系統(tǒng)陀螺常值漂移、加速度計常值零偏的分離估計。估計得到的陀螺漂移、加速度計零偏可用于慣導系統(tǒng)慣性器件工作狀態(tài)的監(jiān)控。建立了定位誤差預測方程，并對單軸旋轉慣導方位陀螺漂移造成的確定性的長期定位誤差進行預測補償，當雙軸旋轉慣導出現(xiàn)故障時，仍能保證高精度的導航定位。

（References）：

[1]Levinson E, Majure R.Accuracy enhancement techniques applied to the marine ring laser inertial navigator (MARLIN)[J].Navigation, 1987, 36(9): 64-86.

[2]Tucker T, Levinson E.The AN/WSN-7B marine gyrocompass/navigator[C]//Proceedings of the 2000 National Technical Meeting of The Institute of Navigation.Anaheim, CA, 2000: 348-357.

[3]魏國.二頻機抖激光陀螺雙軸旋轉慣性導航系統(tǒng)[D].長沙: 國防科學技術大學, 2013: 6-19, 43.Wei G.Research on some key technologies for doubleaxis rotation inertial navigation system with mechanically dithered ring laser gyroscope[D].Changsha: National University of Defense Technology, 2013: 6-19, 43.

[4]于旭東.二頻機抖激光陀螺單軸旋轉慣性導航系統(tǒng)若干關鍵技術研究[D].長沙: 國防科學技術大學, 2013: 15, 43.Yu X D.Research on some key technologies for single-axis rotation inertial navigation system with mechanically dithered ring laser gyroscope[D].Changsha:National University of Defense Technology, 2013: 15, 43.

[5]Yuan B L, Liao D, Han S L.Error compensation of an optical gyro INS by multi-axis rotation[J].Measurement Science and Technology, 2012, 23: 025102.

[6]Zheng Z C, Han S L, Zheng K F.An eight-position self-calibration method for a dual-axis rotational inertial navigation system[J].Sensors and Actuators A: Physical,2015, 232: 39-48.

[7]Gao P Y, Li K, Wang L, et al.A self-calibration method for tri-axis rotational inertial navigation system[J].Measurement Science and Technology, 2016, 27: 115009.

[8]Unsal D, Demirbas K.Estimation of deterministic and stochastic IMU error parameters[C]//Proceedings of the Position, Location and Navigation Symposium.Myrtle Beach, USA, 2012: 862-868.

[9]吳文啟, 王林, 潘獻飛, 等.一種雙慣導聯(lián)合旋轉調制導航與在線相對性能評估方法: 中國, 201510390333.4[P].2015-07-06.

[10]Bar-Itzhack I Y, Goshen-Meskin D.Observability analysis of piece-wise constant systems-part I: theory[J].IEEE Transactions on Aerospace and Electronic Systems, 1992,28: 1056-1067.

[11]Wei G, Li G, Wu Y, et al.Application of least squares support vector machine in system-level temperature compensation of ring laser gyroscope[J].Measurement,2011, 44: 1898-1903.

Dual marine INS joint rotation and modulation for cooperative positioning and error parameter estimation

WANG Lin1, WU Wen-qi1, WEI Guo2, PAN Xian-fei1, GAO Chun-feng2
(1.College of Mechatronics Engineering and Automation, National University of Defense Technology,Changsha 410073, China; 2.College of Optoelectronic Science and Engineering, National University of Defense Technology, Changsha 410073, China)

The dual marine inertial navigation system (MAINS) backup configuration, including a oneindexing MAINS and a two-indexing MAINS, can meet the demands of position accuracy, reliability and cost.However, it lacks the information fusion between systems.Using the velocity and position differences between the systems as observations, a Kalman filter is designed with the attitude error, the velocity error,and the position error differences between the two systems as well as the gyro drifts and horizontal accelerometer biases being system states.A joint rotation and modulation method is proposed, whereby the relative attitude between the two IMUs can be changed to make all the states completely observable according to PWCS observability analysis.A position error prediction equation is formulated to estimate and compensate the positioning error caused by z-axis gyro drift of one-axis indexing MAINS, whereby the position accuracy provided by the one-indexing MAINS can be guaranteed.The experiment results show that the one-indexing MAINS position error is reduced by 30% after error compensation, which can satisfy not only the high reliability but also the position accuracy in case of failure.

one-axis indexing MAINS; two-axis indexing MAINS; cooperative positioning; error parameter estimation

U666.1

A

1005-6734(2017)05-0599-07

10.13695/j.cnki.12-1222/o3.2017.05.008

2017-05-26；

2017-09-10

預研基金重點項目（9140A09031815KG01)）；專利申請?zhí)枺?01510390333.4）

王林（1987—），男，博士研究生，從事慣性導航系統(tǒng)研究。E-mail: wanglinshanda@163.com

聯(lián) 系 人：吳文啟（1967—），男，教授，博士生導師。E-mail: wenqiwu_lit@sina.com