基于可觀測性分析的SINS/CNS降維設計

周偉江,孫 龍

(1.中國人民解放軍92493部隊，遼寧 葫蘆島 121500; 2.中國電子科技集團公司第38研究所，合肥 230000)

基于可觀測性分析的SINS/CNS降維設計

周偉江1,孫 龍2

(1.中國人民解放軍92493部隊，遼寧 葫蘆島 121500; 2.中國電子科技集團公司第38研究所，合肥 230000)

針對長航時無人機的長航時特性和組合導航系統固有的非線性特性，一種SINS/CNS組合導航系統的非線性模型被提出，該模型能夠更加趨近于真實模型;模型建立之后詳細分析了其可觀測性，并根據可觀測性分析的結果對該模型進行了降維設計，只對可觀測性好的狀態進行狀態反饋;在濾波算法的選擇中，精度更高的SCKF算法被應用，仿真結果表明，SCKF濾波算法精度更高，降維設計之后組合導航系統即能夠保證導航精度又能夠大大提高實時性。

SINS/CNS；可觀測性；降維模型；濾波算法

0 引言

基于最優估計的天文/慣導組合系統(SINS/CNS)能夠彌補雙方的缺點，大幅度的提高定位精度，因此受到各國的特別重視，被廣泛的應用于空間飛行器和長航時的飛行平臺上。而決定組合導航系統定位精度的主要因素：一是模型建立的準確性，二是濾波的精度[1]。通常SINS/CNS組合導航系統使用的模型都是線性模型，然而非線性是實際導航系統固有的特性。使用線性模型就勢必會存在模型誤差，隨著時間的積累模型誤差帶來的導航誤差會逐漸增大，尤其是在長航時的任務中[2]。現在的濾波方法基本上都是基于卡爾曼濾波的基礎上發展起來的，而系統狀態的可觀測性對卡爾曼濾波的精度有著重要的影響，因此模型建立之后進行系統的可觀測性分析是必不可少的環節。文獻[3]指出，不可觀測或者可觀測性差的狀態變量對系統的影響極小，這些狀態的忽略不會影響到動態系統的特性。實時性是系統的重要指標，經過可觀測性分析之后，可以進行降維設計，在濾波時只對可觀測性好的狀態進行反饋，從而大大提高系統實時性。

目前針對線性系統的可觀測性分析方法有很多，大部分都是依據分段線性定常系統可觀測性分析理論(PWCS，piece-wise constant system)[4-5]。但是針對非線性模型的可觀測性分析方法卻沒有統一的定義。一般采用將非線性系統線性化為線性系統，將時變系統看作分段線性定常系統的做法來進行可觀測性和可觀測度的分析。文獻[6]結合擴展卡爾曼濾波特點對狀態和量測方程取偏導數得到可觀測性矩陣，然后使用可觀測性秩條件和可觀測性矩陣的條件數分別作為可觀測性和可觀測度的度量標準。這種方法類似于泰勒展開，但是只取了一階項，線性化不夠精確，而且只分析了整個系統的可觀測性和可觀測度，沒有能夠做出針對單個狀態變量的分析。

文獻[7]給出了一種能夠針對每個狀態變量進行可觀測分析的方法，本文利用這種可觀測性分析方法對系統模型進行可觀測性分析，然后根據分析結果進行降維設計從而大大降低計算量，然后采用SCKF濾波算法進行系統狀態估計，通過仿真結果證明了降維設計和濾波算法的正確性。

1 SINS/CNS組合導航系統模型建立

1.1 狀態方程的建立

本文研究的載體是長航時無人機，無人機的長航時特點要求系統模型應該足夠精確，而非線性是系統的固有特性，因此建立系統的非線性模型是必要的。SINS/CNS組合導航系統狀態方程由數學平臺失準角Φ=[ΦeΦnΦu]、速度誤差方程δv=[δveδvnδvu]、位置誤差方程δp=[δLδλδh]、陀螺漂移ε=[εxεyεz]、加速度計偏置▽=[xyz]構成如下所示：

X=[Φδvδpε▽]T

(1)

系統狀態方程為：

(2)

F為系統轉移矩陣：

(3)

其中:FN為9維基本導航參數系統陣，其非零元素為：

F(1,2)=ωiesinL+Ve·tanL/(Rn+h)F(1,3)=-(ωiecosL+Ve/(Rn+h))

F(1,5)=-1/(Rm+h)F(2,1)=-ωiesinL-Ve·tanL/(Rn+h)

F(2,3)=-Vn/(Rm+h)F(2,4)=1/(Rn+h)

F(2,7)=-ωiesinLF(3,1)=ωiecosL+Ve/(Rn+h)

F(3,2)=Vn/(Rm+h)F(3,4)=tanL/(Rn+h)

F(3,7)=(ωiecosL+Ve·sec2L/(Rn+h)F(4,2)=-fUF(4,3)=fN

F(4,4)=Vn·tanL/(Rm+h)-Vu/(Rm+h)

F(4,5)=2ωiesinL+Ve·tanL/(Rn+h)F(4,6)=-(2ωiecosL+Ve/(Rn+h))

F(4,7)=2ωiecosL·Vn+VeVn·sec2L/(Rn+h)+2ωiesinL·Vu

F(5,1)=fUF(5,3)=-fE

F(5,4)=-(2ωiesinL+Ve·tanL/(Rn+h))F(5,5)=-Vu/(Rm+h)F(5,6)=Vn/(Rm+h)F(5,7)=-(2ωiecosL+Ve·sec2L/(Rn+h))Ve

F(6,1)=-fNF(6,2)=fE

F(6,4)=2(ωiecosL+Ve/(Rn+h))F(6,5)=2Vn/(Rm+h)F(6,7)=-2Ve·ωiesinLF(7,5)=1/(Rm+h)F(8,4)=secL/(Rn+h)F(8,7)=secLtanL·Ve/(Rn+h)

F(9,6)=1

FS分FM別為：

(4)

W=[WεxWεyWεzWWW00×9]T

(5)

G為系統的噪聲傳遞矩陣：

(6)

式中,Wεx,Wεy,Wεz和W,W,W分別為陀螺儀和加速度計的隨機噪聲為從載體坐標系到地理坐標系的轉換矩陣。由載體坐標系轉動三次得到，三次轉動順序為：先繞z軸轉動，再繞x1軸轉動θ，最后繞y2軸轉動γ角，得到為：

1.2 量測方程的建立

在SINS/CNS組合導航系統全捷聯模式下，SINS通過捷聯解算得到載體的三軸姿態信息為俯仰角θ0、φ0航向角和橫滾角γ0，而利用星敏感器獲取的姿態信息也可以得到載體的三軸姿態信息，即俯仰角θ、航向角φ和橫滾角γ。將兩者相減得到三軸姿態誤差角δα為：

(7)

由于SINS的姿態角誤差方程中采用的是數學平臺失準角，因此需要將上式的姿態誤差角轉換成數學平臺失準角才能作為濾波器的量測量。轉換關系推導如下：

(8)

2 狀態可觀測性分析

一般地，在進行濾波時都是將所有的狀態進行反饋，在這種情況下如果系統不完全可觀測，或可觀測度較差，則利用濾波器不能對狀態進行準確的估計，甚至經過長時間的迭代估計之后使得效果更差。因此新模型建立之后首先需要分析系統狀態的可觀測度，只對部分可觀測性較好的狀態進行反饋。本文采用文獻[7]的可觀測性分析方法，分析系統的可觀測性，然后只對可觀測性好的狀態進行狀態反饋，不可觀測或可觀測性較低的狀態不反饋，從而達到降維，提高計算速度的目的。

由于組合導航系統是時變系統，定常系統可觀測性分析方法不適用。分段線性定常系統可觀測性分析方法(PWCS)是專門用于判斷時變系統可觀測性分析的一種方法。它采用條帶化可觀測性矩陣(SOM，stripedobservabilitymatrix)代替總的可觀測性矩陣(TOM，totalobservabilitymatrix)，從而使問題得到簡化。但是隨著時間段的增加，可觀測性矩陣的維數仍然很高，使得奇異值分解工作量相當大。文獻[6]提出了一種可觀測度的定義，即系統狀態在不同時段對應的奇異值與在全過程中最大的奇異值之比。其思想如下所示：

設某時間段動態系統的可觀測性矩陣為Q(j),對Q(j)陣進行奇異值分解得：

Q(j)=USVT

(9)

Q(j)=[Q1Q2Q3...Qj]T

(10)

(11)

式中,H為離散后的量測矩陣，F為離散后的狀態轉移陣，U=[u1u2…um·n·j]，V=[v1v2……vn]都是正交矩陣，j是代表某個時間段，在這個時間段內可認為量測陣和狀態轉移陣都是常值矩陣；

(12)

其中：Λ=diag(σ1,σ2,......,σr)σ1>σ2>…>σr>0是矩陣Q的奇異值。則由:

(13)

若將正交矩陣U、V分別用各自的列構成的向量表示，則上式可以進一步寫成：

(14)

從而可以計算出每個奇異值對應的初始狀態向量。從數值上看，較大奇異值可以獲得較好的狀態估計。

具有直接外部測量值的系統狀態總是可觀測的，例如本設計中的數學平臺失準角誤差。這些系統狀態對應的奇異值為σ0。文獻中定義系統狀態的可觀測度為：在系統初始狀態向量中使該狀態取得最大絕對值時的奇異值與具有外測量值的狀態所對應的奇異值之比。即：

ηk=σi/σ0

式中，k=1,2....n；σi為狀態向量X0,i中取得最大絕對值的狀態所對應的奇異值。

這種方法要不可避免地引入量測值，增加了計算的復雜性。實際上，從系統理論分析，可觀測度只與系統的結構有關而與觀測值無關。因此在不需要量測量的前提下得到系統的可觀測情況是研究的重點，文獻[7]根據式(13)分析uiviT矩陣，觀察它的各列元素的大小，判斷出每個奇異值對應的初始狀態向量，本系統一共有6個狀態的奇異值較大，其它的都與它們相差幾個量級，如表1所示。

表1 系統各狀態對應奇異值

根據觀測度的定義，狀態變量中數學平臺失準角是量測必定是可觀測的，與上表結果相符合，而根據上表又可以得到第10，11，12狀態量即陀螺常值漂移可觀測性也比較好。其它的幾個狀態量可觀測性較差。根據實驗統計得到：在其它時間段內，雖然數值的大小稍有變化，但是可觀測性總體的分布跟上表基本一樣。所以可得出結論在這15維狀態變量中只有數學平臺失準角和陀螺常值漂移的可觀測性比較好。

根據可觀測度分析的結果，去掉狀態量中的可觀測度差的狀態得到新的狀態方程如下：

(15)

X=[ΦeΦnΦuεxεyεz]T

其中:

W=[WεxWεyWεz0 0 0]T

(16)

量測矩陣不變，可以發現狀態變量由原來的15維，變成現在的6維。計算量勢必大大減小，實時性得到顯著的提案高。

3 濾波算法

當建立的模型為非線性模型時，傳統的卡爾曼濾波器無法使用，在其基礎上產生了很多的非線性濾波器。其中最經典應用最廣泛的是擴展卡爾曼(EKF，extended kalman filter)濾波器，但是它只能處理弱非線性的情況。后來又出現了無跡卡爾曼濾波(UKF，unscented kalman filter)[8]，UKF的狀態估計可以精確到三階，而EKF只精確到二階，但當系統狀態維數較高時，UKF 會出現維數災難。同時，由于UKF算法在迭代過程需要計算矩陣開方，若矩陣不滿足正定性，UKF算法將被無法繼續執行。近年來出現的粒子濾波[9](PF，particle filter)依據蒙特卡羅思想，隨機產生大量粒子近似計算后驗概率密度，但隨著迭代次數的增加，會產生粒子退化和貧化現象，而且其計算量大，達不到實時性的要求。

最近文獻[10]提出了一種非線性濾波的新方法容積法卡爾曼濾波(CKF)。相比于上述非線性濾波方法，CKF能在高維情況下以較高的精度和較好的實時性進行非線性逼近。但是在實際計算中由于計算機的字長限制，可能使誤差的協方差陣失去正定性從而使得CKF無法繼續下去。

求容積法卡爾曼濾波(square-root cubature kalman filter, SCKF)通過引入 QR分解來回避直接對矩陣開方，從而提高了濾波的穩定性[11]。CKF中狀態方差陣可寫為：P=AAT，考慮QR分解：AT=QR

其中:Q為正交陣，R為上三角陣，則有：

P=AAT=RTQTQR=RTR

(17)

記S=RT。

QR分解避免了直接對矩陣求平方根的操作，即使在矩陣非正定的情況下，濾波算法仍可繼續進行。SCKF就是在CKF濾波時間更新和量測更新兩個環節中首先更新誤差協方差陣的方根。其算法如下：

1)時間更新：

計算容積點:

(18)

(19)

計算預測狀態和預測協方差方根:

(20)

(21)

其中:

n為系統狀態維數，使用三階容積原則，容積點總數為2n?；救莘e點按照下列方式產生，記n維單位向量，使用[1]表示對e的元素進行全排列和改變元素符號產生的點集，稱為完整全對稱點集，[1]i表示點集中[1]的第i列。例如假設系統狀態為三維則產生的點集按下列方式排列后為：

SQ,k-1=chol(Qk-1)即SQ,k-1是系統噪聲方差陣的喬列斯分解。Tria代表QR分解，并將分解得到的RT的值賦給S。

2)量測更新：

計算預測容積點，并通過量測方程傳播:

(24)

Szz,k/k-1=Tria([rkSR,k])

(25)

Pxz,k/k-1=Χk/k-1γk/k-1T

(26)

最后計算得到k時刻的狀態估計:

Wk=(Pxz,k/k-1/Szz,k/k-1T)/Szz,k/k-1

(27)

(28)

Sk=Tria([Χk/k-1-Wkrk/k-1WkSR,k])

4 仿真及分析

仿真條件：捷聯慣導系統陀螺常值漂移為0.1°/h；隨機漂移為0.05°/h；加速度常值偏置為20μg；隨機偏置為10μg；天文導航系統觀測誤差10″。

設載體的初始位置為：東經116°，北緯39°；初始速度北向100m/s，濾波周期為0.1s。SINS/CNS組合導航系統在濾波中采用反饋校正方式。限于篇幅關系此處給出的仿真圖以東向為例，UKF與SCKF仿真比較如下圖所示。

圖1 UKF與SCKF算法比較

從仿真圖可以看出，平臺誤差角UKF與SCKF的估計基本相同，速度、位置、陀螺漂移后者都要由于前者。下圖4是高階模型與經過可觀測度分析之后的降維低階模型比較的仿真圖：

圖2 高階模型與降維模型對比

從上面的仿真圖看出：可觀測性分析后，經過降維反饋校正后，平臺誤差角基本沒有變化，速度誤差也跟原來基本相同，位置誤差和陀螺漂移要優于降維之前的高階模型?？傮w上看保證甚至提高了導航的精度，而且收斂速度明顯提高。從計算量上看，高階模型15維時計算量跟(153+3×152)成正比，降階之后為6維，計算量跟(63+3×62)成正比，實時性顯著提高。

5 結論

UKF濾波算法與SCKF相比，后者綜合性能要優于前者。而且SCKF采用QR分解來回避對協方差陣的開方，避免了舍入誤差的影響，因此穩定性也要比UKF好。對SINS/CNS分析可觀測性后，進行降維處理，在精度保證的前提下提高了計算速度。說明不可觀測或可觀性不好的狀態不能很好地被濾波器估計，對其進行狀態反饋會降低系統的精度。

[1] 全 偉, 劉百奇, 宮曉琳, 等. 慣性/天文/衛星組合導航技術[M]. 北京：國防工業出版社，2011.

[2] 徐 帆. 慣性/天文/衛星組合導航方法研究[D]. 北京：北京航空航天大學, 2009.

[3]DavisonEJ.AMethodforSimplifyinglineardynamicSystem[J].IEEETrans.AutomaticControl, 1967, 12: 119-121.

[4]Goshen-MeskinD,Bar-ItzhackIY.Observabilityanalysisofpiece-wiseconstantsystemPartI:theroy[J].IEEETransactionsonAerospaceandElectronicsSystems, 1992, 28(4): 1056-1067.

[5]Goshen-MeskinD，Bar-ItzhackIY．Observabilityanalysisofpiece-wiseconstantsystems(partII) :applicationtoinertialnavigationin-flightalignmen[J]．IEEETransactionsonAerospaceandElectronicSystems，1992，28(4): 1068-1075.

[6] 帥 平，陳定昌，江 涌.GPSSINS組合導航系統狀態的可觀測度分析方法[J]. 宇航學報2004,25(2):219-224.

[7] 吳海仙，俞文伯，房建成.高空長航時無人機SINS/CNS組合導航系統仿真研究[J].航空學報，2006,27(2).

[8]JulierSJ,UhlmannJK.Unscentedfilteringandnonlinearestimation[J].Proc.ofTheIEEE, 2004, 92(3): 401-422.

[9]DjuricPM,KotechaJH,ZhangJ,etal.Particlefiltering[J].SignalProcessingMagazine,IEEE. 2003,20(5):19-38.

[10]IenkaranArasatatnam,HaykinS.CubatureKalmanFilters[J].IEEETransactionsonAutomaticControl,2009,54(6).

[11] 管兵蕾, 湯顯峰, 葛全波. 平方根求容積卡爾曼濾波的組合導航算法[M]. 中國航海, 2011, 34(4):1-4.

Reduced-dimension Model of SINS/CNS Based on Observability Analysis

Zhou Weijiang1，Sun Long2

(1.92493 Troops, Huludao 121500, China; 2.38th Research Institutie, China Electronics Technology Group
Corporation, Hefei 230000, China)

According to the trait of long-flight-time and the nonlinear of unmanned aerial vehicleand integrated navigation system, a nonlinear model of SINS/CNS(Strap-down Inertial Navigation System/Celestial Navigation System) has been founded, which is more similar to the real model. After that, the observability analysis has been used to design the reduced -dimension model. Then the SCKF(Square-Root Cubature Kalman Filter) is used to estimate the states of the system. The simulation results show that, the SCKF is better than the others , the navigation precious and real-time is promoted after using the reduced-dimension model.

SINS/CNS；observability；reduced-dimension model；filter algorithm

2016-07-15；

2016-08-24。

周偉江(1983-)，男，天津市人，碩士研究生，主要從事測控技術與儀器方向的研究。

1671-4598(2017)04-0143-04

10.16526/j.cnki.11-4762/tp.2017.04.040

V249.3

A