基于奇異值分解的組合導航可觀測度計算

張 丹，裴忠海，余薛浩

（上海航天控制技術研究所，上海201109）

0 引言

高精度導航是航海器能夠順利完成任務的前提保障，航海器既要提高精度，又要確保可靠性。捷聯慣性導航系統（Strapdown Inertial Navigation System，SINS）通常作為主導航系統，能夠實時輸出姿態、速度、位置信息，具有實時性好、短時精度高、輸出信息較為全面等優點，但其長時導航誤差大。多普勒計程儀（Doppler Velocity Log，DVL）是利用聲波在水下傳播的Doppler效應進行測速和計程的一種水聲導航儀，其具有較高的測速精度[1]。利用Kalman濾波技術可以實現SINS/DVL組合導航，不僅可提高導航系統在無GPS情況下的精度，同時也增強了導航信息的容錯性，具有較強的適應性和靈活性[2]。

對于SINS/DVL組合導航的Kalman濾波器[3]，由于初始協方差陣容易滿足正定性條件[4]，并且系統狀態轉移矩陣滿足滿秩條件，因而系統是完全可控的[5]，則Kalman濾波器的有效性主要取決于系統的可觀測性，可觀測度是真正反映Kalman濾波收斂精度和速度的重要指標。目前，國內外研究系統可觀測度的方法主要有以下幾種:1）經典可觀測矩陣法。該方法缺點明顯，只適用于定常系統，并且計算量巨大；2）Ky3obkob法。該方法僅能初步找出可觀測狀態的維度與觀測狀態間的線性關系；3）狀態方程解耦法。該方法只能定性分析可觀測性，具有主觀性[6-8]。因此，本文采用了一種可用于時變系統的分析方法——分段定常系統（Piece-wise Constant System， PWCS）。

對于時變系統，通常采用分段線性定常系統理論[9]計算系統的可觀測性矩陣，通過計算可觀測性矩陣的特征值或奇異值來定量分析系統狀態量對應的可觀測度[10-11]。但是，可觀測性矩陣的階數是隨時段數量的增加而不斷累積的。對于長時間的導航，實時觀測變得不太可能[12-13]。因此，需要對可觀測矩陣的計算進行改進。本文根據可觀測性矩陣的特性，提出了一種改進的簡化算法，可以在很大程度上減少系統可觀測性矩陣的計算工作量。最后，通過仿真驗證及湖面跑船實驗，驗證了該方法的可行性和合理性，并在此基礎上研究了不同航跡機動方式對系統狀態量可觀測度的影響[14-15]，為后續提高組合導航精度及其收斂速度提供了參考[16]。

1 SINS/DVL組合導航系統狀態方程與量測方程

本文研究了SINS/DVL組合導航系統的可觀可控性，系統主要由捷聯慣性系統、Doppler計程儀、Kalman濾波器等部分組成。組合導航系統利用DVL測量的速度信息對SINS累積誤差進行修正。相比GPS測量信息，DVL測量信息中僅有速度信息，組合導航系統對速度組合效果較好[17]，其具體結構如圖1所示。

圖1 SINS/DVL組合系統的基本工作原理Fig.1 Principle frame of SINS/DVL integrated navigation system

本文討論了利用DVL速度信息進行融合的松組合系統。系統采用北天東地理坐標系，選取慣性導航系統誤差及DVL相關誤差項作為狀態量的擴充。Kalman濾波器是針對如下線性系統建立的[18]

式（1）中，At為狀態轉移矩陣，Γt為狀態量噪聲轉移矩陣，Wt為隨機狀態量噪聲陣，Ht為觀測轉移矩陣，Vt為隨機觀測量噪聲陣，狀態量Xt為

式（2）中，δVe、 δVn分別為慣性導航系統的東向速度誤差、 北向速度誤差， Δφn、Δφu、 Δφe分別為北天東方位失準角，ΔL、Δλ分別為緯度誤差和經度誤差，εx、εy、εz為陀螺隨機漂移，ΔVd為DVL速度偏移誤差，δΔ為偏流角誤差，δc為刻度系數誤差。

r為焊點半徑,取0.175 mm[13],l為代表性胞元的寬度,取0.46 mm。各材料參數如表1所示。

在SINS/DVL組合導航系統中，將慣導解算的速度與DVL測量的速度之差作為量測值

2 基于奇異值分解的系統狀態可觀測度分析方法

2.1 可觀測性矩陣

在一個足夠小的時間區間Δti（i＝1，2，…，n）內，如果線性時變系統的系數矩陣變化量可以忽略不計，那么在該段時間區間內就可以把時變系統視為定常系統并進行處理，即可以使用PWCS可觀測性分析法。對于如下離散型PWCS，有

可觀測性矩陣可以表示為

若Q（q）列滿秩， 則系統完全可觀測，r＝rank（Q（q））則為系統的可觀測維度。

2.2 基于奇異值分解的可觀測度計算

基于奇異值分解的系統狀態可觀測度分析方法涉及外部測量值的使用，因而采用離散系統模型較為合理。對于上述離散系統，計算第j個時間段的可觀測性矩陣Q（j）， 并做奇異值分解[19]（SVD分解），即有

Z為整個時間段的觀測值，則初始狀態向量可以進一步表示為

定義第k個系統狀態量的可觀測度為:在系統初始狀態向量中使得該狀態取得最大絕對值時的奇異值與具有直接外部測量值的狀態量所對應的奇異值之比

式（9）中，k＝1， 2， …，n，σi為在狀態向量X0，i中取得最大絕對值的狀態對應的奇異值。為方便比較所有狀態量在同一時刻可觀測度的大小情況，本文提出了一種歸一化概念:定義某一時刻所有狀態量對應的奇異值中的絕對值最大者為σ0，這樣就將可觀測度ηk限定在了[0，1]之間，可以直觀地比較各個狀態量的可觀測度大小。

在深入理解SVD奇異值分解的基礎上，關于k的求取有如下研究結論:每一個奇異值σi都正好對應一個初始狀態向量X0，i。在本文的組合導航系統中，該列向量有13個元素，取得最大絕對值的數值對應的序號下標即為k的取值，表示第k個狀態量與該奇異值σi的關聯度最高，可以表征第k個狀態量的可觀測性能。通過歸一化計算，可以更直觀地看出關聯度的大小。

2.3 可觀測性矩陣的進一步簡化

由于目前的可觀測性矩陣需要進行累加計算，隨著可觀測性矩陣維數的劇增，對其進行奇異值分解的工作量也十分巨大。因此，本文提出了一種進一步簡化的可觀測性矩陣的計算方法。該方法可以大大降低運算量，提高實時觀測系統狀態的可行性。

設分段時間間隔為q，即濾波周期每運行q拍進行一次可觀測性矩陣運算，其示意圖如圖2所示。

圖2 可觀測性分段時間示意圖Fig.2 Schematic diagram of observability segmentation time

在j＝1的q拍時間段內，累加計算的可觀測性矩陣為

根據文獻[20]提供的推導和證明可知，連續型PWCS和離散型PWCS的可觀測性是等價的，離散的過程并沒有損失系統的可觀測度，即可以用式（11）中的替代Q（q）研究系統對應的離散系統的可觀測性。

本文在的上述簡化方法的基礎上進一步提出了可替代的簡化方法:對于在高動態狀態下的運動載體而言，選取的時間段越小，越為精確。可以認為在短時間段時間q內，Fq、Hq與F1、H1相差很小，其差異可以忽略不計。因此，用Q1替代q區間上的全觀測矩陣， 可以大大減少運算量，同時不會失去狀態量本身的觀測特性。

對于全時間段的可觀測性矩陣

最后，根據狀態矩陣的遞推關系，如果每個q+1時間段的狀態初始值設置為上一個q時間段的狀態末值，那么隨著狀態矩陣的遞推，可觀測性矩陣仍具有累積遺傳的性質。這樣，Q（q）的計算便無需累加，其階數也是恒定的，對其進行奇異值分解的運算量便大大減少了。

根據上述的簡化計算，實現高動態組合導航系統長時間導航的狀態可觀度計算可以在工程層面實現，最后為實現全時間段每個狀態量實時可觀測度的比較分析，根據本文提出的歸一化方法對各狀態的可觀測度進行了歸一化處理。

3 仿真驗證與分析

仿真軟件為Matlab2010a，設計了一條理論無機動彈道，包含900s初始對準。設定初始姿態角為[0°0° 0°]， 初始位置為[0m 0m 0m]， 初始速度為10m/s（船頭方向）， 陀螺常值漂移誤差為1（°）/h，隨機漂移誤差為1（°）/h，加速度計零偏誤差為1×10－3g， DVL 測速誤差為0.001m/s（均值）， DVL 常值誤差為0.0005m/s。采用上述組合導航系統，分析該系統在該單一航跡下的系統可觀測度，如圖3所示。在圖3中，橫坐標為仿真時間，縱坐標為歸一化處理后的各觀測參數的可觀測度（是量綱為1的比值參數，0表示不可觀測，1表示完全可觀測）。

圖3 各狀態量的可觀測度Fig.3 Observable degree of navigation states

仿真結果表明:Δφn、Δφe、δΔ的可觀測度較好，具有外部觀測信息的δVn、δVe具有一定的可觀測度，Δφu、ΔL、Δλ、ΔVd不可觀測或者幾乎不可觀測，εx、εy、εz的可觀測度不高。

4 基于SINS/DVL組合導航系統可觀測度計算的湖面實驗

4.1 硬件設備

實驗系統在硬件上由SINS、DVL和導航計算機構成:SINS輸出陀螺測量的角速度增量累加和信息，以及由加速度計測量的視速度增量累加和信息；DVL直接輸出本體速度信息，導航計算機完成數據采集、晃動基座初始對準和組合導航解算等功能。導航計算機通過1553B總線采集IMU中陀螺和加速度計的原始測量數據，數據更新頻率為200Hz。DVL速度信息在導航計算機中獲得提取后，與SINS的原始測量數據一起以100Hz的更新頻率被送入導航計算機。最后，根據接收到的數據完成濾波的信息融合運算，同時觀測各狀態量的可觀測度。

4.2 湖面跑船實驗

本次實驗在吉林松花湖上展開，將光纖慣組、DVL固聯在船體上，采用不同的兩組機動航跡進行解算和顯示，并計算系統的可觀測度。在所采用的光纖慣組中，光纖陀螺隨機誤差均方差為0.15（°）/h， 加速度計常值誤差為 0.11mg（3σ）， 隨機誤差均方差為0.11mg。在圖4、圖5中（除航行軌跡外），橫坐標為仿真時間，縱坐標為歸一化處理后的各觀測參數的可觀測度（是量綱為1的比值參數，0表示不可觀測，1表示完全可觀測）。

航跡1（來回，小角度機動）的關鍵參數的可觀測度如圖4所示。

圖4 航跡1的關鍵參數可觀測度Fig.4 Observable degree of navigation key parameters based on trajectory 1

根據可觀測度的計算結果分析，狀態量Δφu完全不可觀測；Δφn、Δφe可觀測度較高，受載體機動影響，可觀測度變化較大，但總體可觀測性好，可以獲得較高的濾波精度；δVn、δVe可觀測性較好， 但小于 Δφn、Δφe， 由于載體機動的不同，可觀測度也會有相應的變化，但總體可觀測，并且可以獲得一定的濾波精度；其余狀態量的可觀測度與仿真結果基本一致。

航跡2（畫圈，大角度機動）的關鍵參數可觀測 度如圖5所示。

圖5 航跡2的關鍵參數可觀測度Fig.5 Observable degree of navigation key parameters based on trajectory 2

在改變了跑船機動后，對比航跡1，總體結論一致: Δφn、 Δφe可觀測度較高； δVn、 δVe可觀測度較好，并且相差不多，該機動方式對δVn可觀測度有益；Δφu的可觀測度相較航跡1變化較大，可觀測度有較大提高（從幾乎不可觀測到在1250s～8700s之間有0.12的可觀測度）；其余狀態量的可觀測度與仿真結果基本一致。

對比兩種航跡下的位置誤差（采用亞米級RTK差分導航系統作為標準彈道），如表1所示。

表1 位置誤差對比Table 1 Comparison of position errors

由表1的位置精度對比可以看出，航跡2的畫圈機動對組合導航的精度提高比較明顯，可提高1個數量級，這與系統可觀測度的分析結果完全一致，可見其對組合導航系統的指導意義。

綜上所述，在實現遠程導航時，可以通過不同的航跡機動來實現系統性能的提高，如本文的畫圈、S機動、提速、轉彎等。對飛行器而言，也可以通過抬頭、加速拉升、轉彎、振翼等動作提高系統的可觀可控性，從而提高系統的濾波精度及收斂速度。

5 結論

本文提出了一種改進的SINS/DVL組合導航系統各狀態量的可觀測度計算方法，并通過數字仿真及湖面跑船實驗驗證了該方法可以實現濾波器狀態量可觀測度的實時計算。跑船實驗設置了不同的航跡機動方式，驗證了某些特定的機動方式可以提高系統的可觀測度，從而提高了系統的濾波精度和收斂速度。在DVL的導航輔助下，組合導航的定位精度可以提高1個數量級，為后續進一步探究組合導航性能提供了參考。