周偉江,孫 龍

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

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

周偉江1,孫 龍2

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

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

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

0 引言

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

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

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

1 SINS/CNS組合導航系統(tǒng)模型建立

1.1 狀態(tài)方程的建立

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

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

(1)

系統(tǒng)狀態(tài)方程為：

(2)

F為系統(tǒng)轉(zhuǎn)移矩陣：

(3)

其中:FN為9維基本導航參數(shù)系統(tǒng)陣，其非零元素為：

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為系統(tǒng)的噪聲傳遞矩陣：

(6)

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

1.2 量測方程的建立

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

(7)

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

(8)

2 狀態(tài)可觀測性分析

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

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

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

Q(j)=USVT

(9)

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

(10)

(11)

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

(12)

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

(13)

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

(14)

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

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

ηk=σi/σ0

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

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

表1 系統(tǒng)各狀態(tài)對應奇異值

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

根據(jù)可觀測度分析的結(jié)果，去掉狀態(tài)量中的可觀測度差的狀態(tài)得到新的狀態(tài)方程如下：

(15)

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

其中:

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

(16)

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

3 濾波算法

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

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

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

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

P=AAT=RTQTQR=RTR

(17)

記S=RT。

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

1)時間更新：

計算容積點:

(18)

(19)

計算預測狀態(tài)和預測協(xié)方差方根:

(20)

(21)

其中:

n為系統(tǒng)狀態(tài)維數(shù)，使用三階容積原則，容積點總數(shù)為2n。基本容積點按照下列方式產(chǎn)生，記n維單位向量，使用[1]表示對e的元素進行全排列和改變元素符號產(chǎn)生的點集，稱為完整全對稱點集，[1]i表示點集中[1]的第i列。例如假設(shè)系統(tǒng)狀態(tài)為三維則產(chǎn)生的點集按下列方式排列后為：

SQ,k-1=chol(Qk-1)即SQ,k-1是系統(tǒng)噪聲方差陣的喬列斯分解。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時刻的狀態(tài)估計:

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 仿真及分析

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

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

圖1 UKF與SCKF算法比較

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

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

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

5 結(jié)論

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

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

[2] 徐 帆. 慣性/天文/衛(wèi)星組合導航方法研究[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組合導航系統(tǒng)狀態(tài)的可觀測度分析方法[J]. 宇航學報2004,25(2):219-224.

[7] 吳海仙，俞文伯，房建成.高空長航時無人機SINS/CNS組合導航系統(tǒng)仿真研究[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-)，男，天津市人，碩士研究生，主要從事測控技術(shù)與儀器方向的研究。

1671-4598(2017)04-0143-04

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

V249.3

A