基于參數識別的SINS/DVL初始對準方法

徐 祥，史凡偉，徐大誠，富振鐸

（1.蘇州大學 電子信息學院，蘇州 215100；2.北京機械設備研究所，北京 100039）

捷聯慣性導航系統可以為艦船、導彈、水下航行器等設備提供全方位的導航信息。在捷聯慣導系統正常工作之前，通常需要進行初始對準過程，而初始對準過程對于捷聯慣導系統導航定位精度起著至關重要的作用[1]。

按照載體運動方式的不同，初始對準過程可以分為靜止基座初始對準及運動基座初始對準兩類[2]。其中，靜止基座初始對準不考慮載體進行線運動，通常采用基于慣性系原理的姿態確定方法[4-6]。但是，這種對準方式限制了載體的運動，無法在一些機動場合較強的條件下進行初始對準，這也限制了捷聯慣性導航系統的適用范圍，不符合捷聯慣導系統多用途、多功能的發展方向。因此，眾多學者研究了運動基座上的初始對準技術。文獻[7-8]介紹了一種基于 GPS速度輔助及DGPS速度輔助的運動基座初始對準技術，這為運動基座上的初始對準技術奠定了基礎。考慮到水下航行器在執行任務過程中無法獲取 GPS信息，文獻[9-10]研究了一種基于 DVL速度輔助的運動基座初始對準方法，利用速度輔助構造慣性系矢量觀測器，并通過最優基姿態確定算法完成初始對準過程。但是，該方法直接采用DVL速度構造觀測矢量，這將在觀測矢量中引入DVL量測噪聲，導致對準結果穩定性較差。

本文在深入研究DVL輔助SINS運動基座初始對準方法的基礎上，建立了觀測矢量參數模型，并設計自適應 Kalman濾波算法，估計未知參數，利用估計的參數對觀測矢量進行重構，實現了觀測矢量的降噪，提高了初始對準的穩定性。

1 粗對準坐標系定義

1）慣性系i：選取初始對準起始時刻地球系作為初始慣性系，并且不隨地球運動而改變；

2）導航系n：選取“東-北-天”為導航系；

3）初始導航系n0：初始對準起始時刻導航坐標系，并且相對慣性系不變；

4）地球系e：原點位于地心，ze軸從地心沿地球北極朝上，xe軸位于赤道平面，從地心指向載體所在子午線，ye軸與ze軸、xe軸構成右手坐標系；

5）初始地球系e0：初始對準起始時刻地球坐標系，并且相對慣性系不變；

6）載體系b：定義“右-前-上”為載體坐標系；

7）初始載體系b0：初始對準起始時刻載體坐標系，并且相對慣性系不變。

2 DVL輔助SINS初始對準

慣導系統比力方程可以表示為：

式中，fb表示比力，gn表示地球重力加速度在導航系的投影。

由方向余弦矩陣的鏈式法則可知：

將式(2)代入式(1)可得：

由坐標變換可知：

將式(4)代入式(3)，并對等式兩邊積分可得：

其中，

由于DVL采樣時間與SINS采樣時間不一致，此處假設DVL采樣時間為Δtd，SINS采樣時間為Δts，并且有 Δtd=SΔts（S∈ ?+） 。考慮到觀測矢量的計算方式，以DVL量測更新時刻計算離散化矢量。

求解觀測矢量：

式中，t=MΔtd，并以M進行簡化表示。式(7)中，為小量，可以忽略，且等號右側第二項積分可以計算為

式中：tk=kΔtd，并以k進行簡化表示；

由文獻[8]可知：

因此觀測矢量可離散化計算為：

同理，參考矢量可以計算為：

考慮到初始對準過程中，載體運動速度較慢，運動范圍較小，由文獻[10]可知，可以采用初始時刻的來替代

考慮DVL與SINS之間的安裝誤差角及比例因子誤差均得到標定并補償，此時DVL量測可以表示為：

將式(12)代入式(10)可以得到利用DVL量測計算的觀測矢量：

由式(13)可以看出，觀測矢量易受DVL量測噪聲的影響，并最終導致估計姿態呈現噪聲特性。為解決該問題，下面將通過設計參數識別算法，實現矢量的重構，減小DVL噪聲對初始對準結果的影響。

3 運動基座速度視運動建模

由觀測矢量的計算表達式可知：

因此，α可以計算為：

式中，Φ表示常值矩陣，Γ（t） 表示為：

由式(5)可知：

式(18)給出了不含噪聲情況下的觀測矢量的表示形式。但在實際情況中，計算的觀測矢量式(13)含有DVL量測噪聲及慣性傳感器量測噪聲，這會干擾對準的收斂速度及準確性。為此，本文將基于式(18)的參數模型，對未知參數矩陣進行估計，利用估計的參數矩陣進行觀測矢量重構，實現降噪過程，提高對準性能。

4 參數估計與矢量重構

由式(10)(13)和(18)可知：

假設觀測矢量三個通道之間不存在相互耦合，以z軸為例進行參數估計。參數模型為：

在實際情況中，觀測矢量噪聲中除 DVL量測噪聲外，還包含慣性傳感器的量測噪聲，這將導致觀測矢量噪聲統計不準確。為克服這個問題，本文采用自適應 Kalman濾波算法對未知參數進行估計，具體流程如下所示：

根據上述重構矢量，可以計算：

式中，

其中，[· ×]表示矢量對應的斜對稱矩陣。

由四元數最優基方法可知[8]，最小特征值對應的特征矢量即為當前時刻估計的最優姿態四元數由姿態四元數與方向余弦矩陣之間的轉換關系，結合方向余弦鏈式法則式(2)，即可得到當前時刻的方向余弦矩陣從而完成初始對準。

5 實驗與分析

5.1 仿真實驗

為驗證本文提出的算法，本節設計了仿真實驗，將本文設計的算法與傳統方法（文獻[10]）方法進行對比，從算法的收斂速度和對準精度兩個方面進行驗證分析。

仿真實驗過程中，傳感器誤差參數設定如表1所示。設定慣性傳感器數據輸出頻率為200 Hz，DVL數據輸出率為1 Hz，DVL量測噪聲設置為0.1 m/s。

表1 傳感器誤差設定Tab.1 Sensor errors setting

載體運動軌跡及狀態如圖1和圖2所示。運動過程持續300 s，運動過程中載體進行轉向機動。

圖3為觀測矢量與重構矢量效果圖。從圖中可以明顯的發現，采用參數識別之后，重構觀測矢量的噪聲明顯小于直接計算得到的觀測矢量噪聲，這也是本文算法具有抑制DVL量測噪聲的原因。

圖1 載體運動軌跡圖Fig.1 In-motion trajectory of the vehicle

圖2 載體運動狀態圖Fig.2 In-motion status of the vehicle

在上述仿真條件下，得到仿真結果如圖3～6所示。

圖3 觀測矢量與重構矢量Fig.3 Observation vector and reconstructed vector

圖4～6分別表示縱搖角、橫搖角和航向角的對準誤差曲線。從圖4和圖5可知，傳統方法對準誤差呈現波動特性，尤其在對準過程的起始階段。采用參數識別方法，誤差的波動特性明顯減小。兩種方法的對準誤差在300 s左右可以達到相同的對準精度，誤差角小于0.01°。圖6的航向角對準誤差表明，傳統方法易受DVL量測噪聲的影響，導致對準結果呈現波動特性，而采用參數識別之后對準誤差更加穩定。不同于水平姿態誤差，航向姿態角誤差易受觀測矢量噪聲影響，從而在整個對準過程中，傳統方法對準誤差的波動性大于參數識別方法，這可以從圖6的局部放大圖中發現。對航向誤差角進行200～300 s之間進行誤差統計可知，傳統方法的標準差為0.23°，而參數識別方法的標準差為0.1°左右，這也進一步驗證了參數識別方法的優勢。

圖4 縱搖誤差Fig.4 Errors of pitch

圖5 橫搖誤差Fig.5 Errors of roll

圖6 航向誤差Fig.6 Errors of yaw

5.2 跑車實驗

為驗證本文提出的算法，現設計跑車實驗進行驗證。實驗過程中采用光纖捷聯慣性導航系統，其陀螺儀和加速度計性能參數參見表2所示。

表2 傳感器特性Tab.2 Sensor errors

慣性傳感器采樣頻率為200 Hz。采用NovAtel GPS接收機輸出的BESTVEL Doppler 速度輸出指令模擬DVL載體系速度，其速度采樣頻率為1 Hz。在對準之前，系統工作在GPS與慣導組合導航模式下，并以GPS和慣導組合導航系統姿態作為參考姿態與對準結果進行標校。

載體運動軌跡如圖7所示。

圖8為觀測矢量與重構矢量效果圖，從圖中可以看出，采用參數識別方法可以減小DVL噪聲對觀測矢量的影響，從而提高對準結果的收斂速度。

圖7 載體運動軌跡圖Fig.7 In-motion trajectory of the vehicle

圖8 觀測矢量與重構矢量Fig.8 Observation vector and reconstructed vector

圖9～11分別為縱搖角、橫搖角和航向角對準誤差曲線圖。從圖中可以看出，采用參數識別算法之后，對準曲線收斂速度更快，這一結果在對準前100 s更為明顯。在對準精度方面，兩個水平姿態誤差角對準精度相似，均可以收斂在0.02°以內。而航向角誤差方面，采用參數識別算法具有更快的收斂速度，且對準穩定性更優。

圖9 縱搖誤差Fig.9 Errors of pitch

圖10 橫搖誤差Fig.10 Errors of roll

圖11 航向誤差Fig.11 Errors of yaw

6 結 論

針對傳統DVL速度輔助SINS行進間初始對準易受 DVL量測噪聲影響的問題，本文研究了一種基于參數識別方法的行進間初始對準方法。通過研究SINS/DVL初始對準觀測矢量，構建了參數識別模型，并對參數進行估計，從而重構觀測矢量，減小噪聲的影響。通過設計仿真與跑車實驗，驗證了本文設計的方法比傳統方法具有更高的穩定性。