周伟江,孙 龙

(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。基本容积点按照下列方式产生，记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 高阶模型与降维模型对比

从上面的仿真图看出：可观测性分析后，经过降维反馈校正后，平台误差角基本没有变化，速度误差也跟原来基本相同，位置误差和陀螺漂移要优于降维之前的高阶模型。总体上看保证甚至提高了导航的精度，而且收敛速度明显提高。从计算量上看，高阶模型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