一种SINS/SRS/CNS多源融合自主组合导航方法

技术领域

本发明属于资助组合导航技术领域，尤其涉及一种SINS/SRS/CNS多源融合自主组合导航方法。

背景技术

惯性导航系统(Inertial Navigation System,INS)可以独立给出载体的姿态、速度和位置等导航参数，抗外界干扰能力强，短期稳定性能好，相对导航精度较高，在航空、航天和航海等军事和民用领域得到了广泛应用。但惯性导航系统的缺点是导航误差随时间积累，纯惯性导航系统难以满足飞行器高精度、长航时导航的需求。

天文导航系统是用天文方法观测星辰日月等天体来确定航行体的位置，以引导航行体沿预定航线到达目的地的一种导航方法。它是一门古老而崭新的技术，在导航技术中占有重要的位置。它不需要地面设备支持，既不受人工或自然形成的电磁干扰，也不向外辐射电磁波，自主性强，隐蔽性好。其缺点是导航数据更新率低、导航性能受制于目标天体的数量、距离和空间环境等因素，且不能直接测速。

光谱红移导航是一种隐蔽性好、自主性强的新导航体制，它不向外发射任何电磁波，任何人用任何方法都打不掉用于航天器导航的太阳系天体(太阳、木星、地球等)，充分有效地利用这些天然资源实现航天器自主导航是人类的责任和荣耀。但单一的光谱红移自主导航方法，在航天器进行姿态机动过程中，由于短时间内无法获取足够的观测信息，或由于观测信息中断而无法获得测量信息，从而使导航精度变差，甚至结果发散。此外，由于天体遮挡等原因，在某些时段会出现测量信号暂时中断的情况，无法进行导航参数的解算。

综上，现有的导航方法在深空导航中的局限性主要是或需要地面站支持，不具备自主性，或需引入航天器轨道动力学，导致大量星上复杂计算。这些局限性使深空自主导航的实现更加困难，亟待在导航方法上有所突破。

发明内容

本发明的目的是提供一种SINS/SRS/CNS多源融合自主组合导航方法，以实现在深空导航中的全自主高精度多源融合导航。

本发明采用以下技术方案：一种SINS/SRS/CNS多源融合自主组合导航方法，包括以下步骤：

构建SINS/SRS导航子系统，确定SINS/SRS导航子系统的速度量测量和高度量测量；

结合速度量测量和高度量测量构建SINS/SRS导航子系统的量测方程并解算，得出第一估计值；

构建SINS/CNS导航子系统，确定SINS/CNS导航子系统的姿态角量测量；

基于姿态角量测量构建SINS/CNS导航子系统的量测方程并解算，得出第二估计值；

融合第一估计值和第二估计值，得到SINS/SRS/CNS导航系统的全局最优估计值，并利用全局最优估计值对SINS的误差进行实时校正。

进一步地，确定SINS/SRS导航子系统的速度量测量和高度量测量包括：

通过SINS获取载体的第一速度信息，通过SRS获取载体的第二速度信息；计算第一速度信息和第二速度信息的速度差信息，并将速度差信息作为SINS/SRS导航子系统的速度量测量；

通过SINS获取载体的第一高度信息，通过高度量测元件获取载体的第二高度信息；计算第一高度信息和第二高度信息的高度差信息，并将高度差信息作为SINS/SRS导航子系统的高度量测量。

进一步地，确定SINS/CNS导航子系统的姿态角量测量包括：

通过SINS获取载体的第一姿态角信息，通过CNS获取载体的第二姿态角信息；计算第一姿态角信息和第二姿态角信息的姿态角差信息，并将姿态角差信息作为SINS/CNS导航子系统的姿态角量测量。

进一步地，SINS/SRS导航子系统的量测方程为：

其中，Z

进一步地，SINS/CNS导航子系统的量测方程为：

Z

其中，Z

进一步地，SINS/SRS导航子系统的量测方程和SINS/CNS导航子系统的量测方程的解算方法均采用抗差自适应Unscented粒子滤波方法；

抗差自适应Unscented粒子滤波方法包括以下步骤：

步骤S1、采用粒子算法生成k＝0时刻的N个信息值xi

步骤S2、在k＝1,2,...M时刻，按照下面次序计算：

步骤S2.1、计算输入的量测量的等价权

步骤S2.2、采用UKF算法更新信息值

其中，

步骤S2.3、计算权值

其中，

步骤S2.4、计算估计式

将所得结果与阈值进行比较，当所得结果小于等于阈值，重复上述步骤S2.1-步骤S2.4，直至所得结果大于阈值，执行步骤S2.5；

步骤S2.5、计算状态量估计值

进一步地，计算输入的量测量的等价权

设等价权矩阵为

其中，k

自适应因子选取如下：

其中，c

进一步地，计算输入的量测量的等价权

设等价权矩阵为

自适应因子选取如下：

其中，c

本发明的有益效果是：本申请所设计的SINS/SRS/CNS自主组合导航系统，利用先进的信息融合技术，将SINS、SRS和CNS三者进行有机结合；该系统同时兼备了SINS、SRS和CNS各自的优点，克服了各单一导航系统的缺陷，在性能上实现了互补；SINS/SRS/CNS自主组合导航系统具有各单一导航系统所不具备的优良性能；组合导航系统的姿态精度达到1′以内，速度精度达到0.1m/s，定位精度3m以内，能满足飞行器对导航系统精度的需求，有效提高了导航系统的自主性、可靠性和精度。

附图说明

图1为本发明实施例中光谱红移自主导航基本原理图；

图2为本发明实施例中SINS/SRS导航子系统的原理图；

图3为本发明实施例中SINS/CNS导航子系统的原理图；

图4为本发明实施例中SINS/SRS/CNS导航系统图；

图5为本发明实施例中SINS/SRS/CNS导航原理图；

图6为本发明验证实施例SRS/SINS/CNS多原融合自主组合导航仿真实验系统方案；

图7为本发明验证实施例中载体飞行航迹仿真图；

图8为本发明验证实施例中航向角误差比较曲线图；

图9为本发明验证实施例中东向速度误差比较曲线图；

图10为本发明验证实施例中纬度误差比较曲线图；

图11为本发明验证实施例中星敏感器无量测时SINS/SRS导航子系统的东向速度误差曲线图；

图12为本发明验证实施例中姿态误差曲线图；

图13为本发明验证实施例中速度误差曲线图；

图14为本发明验证实施例中位置误差曲线图；

具体实施方式

下面结合附图和具体实施方式对本发明进行详细说明。

本发明基于太阳系天体光谱红移测速导航原理，利用太阳系自然天体的光谱红移信息、物体运动定律和自然天体的天文信息作为导航信息源，采用非线性滤波算法和多源信息融合技术，设计全自主高精度SINS/SRS/CNS多源融合智能导航新体制，建立SINS/SRS/CNS多源智能组合导航系统的原理、方案和数学模型；设计适合该组合导航系统解算的抗差自适应Unscented粒子滤波算法；分别建立SINS/SRS组合导航子系统仿真实验系统、SINS/CNS组合导航子系统的仿真实验系统，以及SINS/SRS/CNS多源融合自主组合导航仿真实验系统，对所设计的组合导航系统的原理、方案、模型和算法进行了实验验证和分析。

为了加深和拓展对光谱红移导航方法的理解，这里将从另一角度研究光谱红移导航原理和算法，虽然和传统的方法公式推导结果相同，但本申请的方法更直观、更容易理解。

航天器光谱红移自主导航的基本原理是：将太阳系天体的光信号作为导航信息源，结合太阳系天体星历信息及航天器惯性姿态信息，根据光谱红移效应测量获得航天器在惯性坐标系中的飞行速度，并通过积分获得航天器在惯性坐标系中的位置参数，其导航原理如图1所示。

假设航天器在运行过程中，人们可探测到天体发出的光信号，则根据多普勒原理(Doppler principle，DP)，航天器接收到的光谱频率不等于该天体发出的光谱频率，且频率的变化大小与航天器相对天体的运动状态变化有关。因此，通过测量光谱频率的红移，可间接获得航天器的相对运动速度。根据空间向量关系，若观测的不共线的天体数大于3个，则综合天体运行星历及通过惯性测量元件测量到的航天器姿态信息，可确定出航天器在惯性空间中的速度信息，通过积分可获得航天器的位置信息。

根据光谱红移特征频移公式，当航天器相对光源运动时，在航天器上接收到的光源频率f

公式(1)中，v为惯性坐标系中航天器相对光源运动的速度矢量，θ为惯性坐标系中光源——航天器波矢(即光源指向航天器)与v的夹角，c为真空中光速。

由公式(1)可推导出：

进一步，根据公式(2)可推导出下式成立：

公式(3)实际上是航天器相对天体运动的径向速率：

假设航天器在轨接收并测得各参考天体的光源频率为f

式(5)中，v

根据天体几何关系，v

式(6)中v

由此可建立对速度矢量、位置矢量的状态估计方程组为：

给定初值后，求解上述方程组式(7)，可得航天器在惯性坐标系中的速度矢量v

本发明实施例公开了一种SINS/SRS/CNS多源融合自主组合导航方法，包括以下步骤：

构建SINS/SRS导航子系统，确定SINS/SRS导航子系统的速度量测量和高度量测量；结合速度量测量和高度量测量构建SINS/SRS导航子系统的量测方程并解算，得出第一估计值；构建SINS/CNS导航子系统，确定SINS/CNS导航子系统的姿态角量测量；基于姿态角量测量构建SINS/CNS导航子系统的量测方程并解算，得出第二估计值；融合第一估计值和第二估计值，得到SINS/SRS/CNS导航系统的全局最优估计值；利用全局最优估计值对SINS的误差进行实时校正。

本申请所设计的SINS/SRS/CNS自主组合导航系统，利用先进的信息融合技术，将SINS、SRS和CNS三者进行有机结合。该系统同时兼备了SINS、SRS和CNS各自的优点，克服了各单一导航系统的缺陷，在性能上实现了互补。SINS/SRS/CNS自主组合导航系统具有各单一导航系统所不具备的优良性能。组合导航系统的姿态精度达到1′以内，速度精度达到0.1m/s，定位精度3m以内，能满足飞行器对导航系统精度的需求，有效提高了导航系统的自主性、可靠性和精度。

为了更好的设计SINS/SRS/CNS自主组合导航系统，选用东-北-天(E-N-U)地理坐标系作为导航坐标系，分别研究SINS/SRS组合导航子系统和SINS/CNS组合导航子系统的设计问题。

具体的，在本实施例中，确定SINS/SRS导航子系统的速度量测量和高度量测量包括：

通过SINS获取载体的第一速度信息，通过SRS获取载体的第二速度信息；计算第一速度信息和第二速度信息的速度差信息，并将速度差信息作为SINS/SRS导航子系统的速度量测量；

通过SINS获取载体的第一高度信息，通过高度量测元件获取载体的第二高度信息；计算第一高度信息和第二高度信息的高度差信息，并将高度差信息作为SINS/SRS导航子系统的高度量测量。

SINS/SRS导航子系统原理如图2所示，SINS/SRS导航子系统原理是将SINS输出的飞行器的速度、位置和姿态信息、SRS获得的速度信息和雷达高度计得到的高度信息送入组合导航滤波器，用SRS获得的高精度速度信息对SINS进行校正，克服SINS随时间累积的导航误差，并利用雷达高度计抑制SINS高度通道的发散，得到高精度的导航信息。

SINS/CNS组合导航子系统原理如图3所示，在SINS/CNS组合导航系统中，惯性器件输出载体角运动信息和线运动信息，对这些导航信息进行解算可以获得载体实时三维姿态、速度和位置信息。利用CNS可以得到载体的姿态信息，用来校正SINS随时间积累的姿态误差。

CNS的星敏感器固连在载体上，在安装误差已经被标定的条件下，认为星敏感器坐标系与载体坐标重合。在图3中，星敏感器观测到的天体高度角和方位角，通过计算可以获得天体的星光单位方向矢量，利用姿态解算算法可以计算出载体系相对于惯性系的姿态矩阵

具体的，确定SINS/CNS导航子系统的姿态角量测量包括：

通过SINS获取载体的第一姿态角信息，通过CNS获取载体的第二姿态角信息；计算第一姿态角信息和第二姿态角信息的姿态角差信息，并将姿态角差信息作为SINS/CNS导航子系统的姿态角量测量。

在整个导航系统中，如图4所示，根据光谱测量仪得到的光谱信号经过去除噪声和谱线分离，可计算获得光谱红移量的估计值。由光谱红移测速导航原理，结合天体星历信息和星敏感器测量的姿态信息，能够得到载体精确的速度信息，将该速度信息与SINS解算得到的速度信息之差作为量测量，将其作为观测量送到SINS/SRS子滤波器进行速度解算。

CNS以恒星为参照物，利用CCD星敏感器探测单元对天空直接捕获星图，并以数字方式存储。然后，经过数据处理单元对星图进行处理，并通过恒星质心提取、星图识别及坐标变换，将得到的恒星像点与星图库进行比较，可获得载体的姿态信息。将这些信息与来自SINS的相应信息进行比较，可得到CNS的测量偏差，将其作为观测量送到局部滤波器与其它观测值一起进行滤波解算。

信息融合中心(全局滤波器)将来自CNS的测量偏差与来自SINS的速度和方位偏差进行融合，可计算出导航误差估计，从而校正SINS并获得当前精确的导航解。这些导航解用来校正系统中的传感器误差，并为控制系统提供决策信息，从而使载机按预置航路精确飞行。

在导航系统中，由于CNS输出的姿态精度较高且误差不随时间积累，用CNS输出的姿态信息修正SINS的姿态误差，利用SRS输出的高精度的速度信息修正SINS的速度误差，因此，SINS/SRS/CNS组合系统具有很高的导航精度。为了减小组合导航系统的维数，假设SRS和CNS的量测误差为高斯白噪声，且不列为组合导航系统的状态量，只将SINS的系统误差考虑为SINS/SRS/CNS组合导航系统的状态量。

选取东北天(E-N-U)地理坐标系作为导航坐标系，根据惯性导航系统的误差方程，SINS/SRS/CNS组合导航系统的状态量x(t)选取为：

式(8)中，δv

根据惯导系统的误差模型，SINS/SRS/CNS自主组合导航系统的状态方程为

式(9)中，f(·)是非线性函数，X(t)是系统状态向量，W(t)是系统过程噪声，G(t)是噪声系数矩阵。

系统状态函数为

式(10)中，C

噪声系数阵G(t)为

系统噪声

为了克服SINS速度误差随时间积累的缺陷，利用SRS输出高精度的速度信息对SINS进行校正，利用雷达高度表提供的高度信息阻尼SINS高度通道的发散。

假设SINS和SRS输出的速度信息分别为V

用雷达高度表和SINS提供的高度信息之差作为SINS/SRS组合子系统的高度量测量

Z

式(13)中，h

综合式(12)和式(13)，可得SINS/SRS导航子系统的量测方程如下：

其中，Z

SINS与CNS均能输出载体的姿态角信息，在导航坐标系下，将SINS输出的姿态角与CNS输出的姿态角之差作为SINS/CNS组合导航的量测Z

Z

式(15)中，θ

SINS/CNS导航子系统的量测方程为：

Z

其中，Z

进而，可得SINS/SRS/CNS多源融合自主组合导航系统的量测方程为

由光谱测量仪得到的光谱信号经过去噪和谱线分离，可计算获得光谱红移量的估计值。根据光谱红移导航原理，结合天体星历信息和星敏感器测量的姿态信息，能够得到载体精确的速度信息，将该速度信息与SINS解算得到的速度信息作差作为量测量。将其作为观测量送到SINS/SRS局部子滤波器进行滤波计算。将CNS获得的飞行器的姿态信息与利用SINS获得的相应信息进行比较，得到CNS的测量偏差，将其作为观测量送到SINS/CNS局部子滤波器进行滤波计算。利用全局滤波器将来自SINS/CNS局部子滤波器经滤波计算得到的航天器的姿态误差、位置误差和速度误差信息与来自SINS/SRS局部子滤波器计算得到的相应误差信息，送入全局滤波器进行多源融合自主组合导航系统滤波计算，可计算出导航误差全局最优估计值，从而校正SRS并获得当前精确的导航解。这些导航解用来校正系统误差，获得航天器高精度的导航信息。

SINS/SRS/CNS多源融合自主导航系统原理如图5所示。先设计SINS/SRS导航子滤波器和SINS/CNS导航子滤波器。

在SINS/SRS组合导航子滤波计算中，SINS为主系统，SRS为辅助系统，利用SINS可得到航天器三维连续速度、位置和姿态信息。将SRS得到的高精度的速度信息与SINS得到的速度信息之差，作为SINS/SRS导航子系统的速度量测信息，结合SINS/SRS导航子系统的状态信息，通过抗差自适应Kalman滤波算法，解算得到SINS的误差最优估计值，用来修正SINS的速度误差；

在SINS/CNS组合导航子滤波计算中，将SINS和CNS输出的姿态角信息之差作为SINS/CNS导航子系统的姿态角量测信息，将SINS与雷达高度表输出的高度信息之差作为高度量测信息，用来修正SINS的姿态角误差。

最后，利用抗差自适应Kalman滤波算法，通过计算获得组合导航系统状态的两组局部最优估计值

由于惯导系统高度通道发散，且SRS和CNS都不能输出载体的高度信息，因此采用雷达高度表输出载体的高度信息对SINS高度通道进行校正，以抑制SINS高度通道发散。SINS/SRS/CNS多元融合自主组合导航系统最优估计融合算法为

考虑如下非线性系统

式(19)中，x

SINS/SRS导航子系统的量测方程和SINS/CNS导航子系统的量测方程的解算方法均采用抗差自适应Unscented粒子滤波方法；同时SINS/SRS/CNS多元融合自主组合导航系统最优估计融合算法也采用该方法。

抗差自适应Unscented粒子滤波方法包括以下步骤：

步骤S1、根据初始均值和均方差抽取N个粒子(即信息值)，在k＝0时刻，

步骤S2、在k＝1,2,...M时刻，按照下面次序计算：

步骤S2.1、计算输入的量测量的等价权

设等价权矩阵为

其中，k

在另一种实施例中，根据需要也可以采用另一种表达式

自适应因子选取如下：

其中，c

步骤S2.2、计算Sigma点，由UKF算法更新粒子

具体过程如下：

设新样本为

式(20)中，

上述2N+1个Sigma采样点集对应的权值集合为：

其中，上述集合w的第j列表示第j个Sigma点的权值，满足∑w

基于UKF算法，结合所述等价权

从而可以得到

其中，上式表明通过因子α

步骤S2.3、计算权值

其中，

步骤S2.4、计算估计式

将所得结果与既定阙值进行比较，判断粒子退化的严重程度，

步骤S2.5、计算非线性状态量估计值

上述步骤在选取重要性密度函数时，利用了两个重要的调节因子，即等价权和自适应因子对UT变换后所得粒子采样点更合理的分配有用信息，为重要性采样过程提供更好的采样分布函数。

采用抗差自适应Unscented粒子滤波(RAUPF)算法对SINS/SRS/CNS多源融合自主组合导航系统进行滤波计算。首先，对独立导航系统SINS的数学模型进行仿真，根据仿真结果优化调整误差模型，同时针对各项误差进行误差补偿，提高SINS性能；其次，模拟仿真光谱红移测量方法，为光谱红移导航提供测量数据来源，并在此基础上对光谱红移导航的原理、模型和算法进行仿真验证，根据仿真结果优化该导航系统的模型和算法，SRS/SINS/CNS多原融合自主组合导航仿真实验系统方案如图6所示。

选取东北天(E，N，U)地理坐标系为导航坐标系，假设飞行器初始位置为北纬34.246°，东经108.997°，高度为2.5km，经过平飞、转弯、爬升和俯冲等机动飞行到达终点。飞行时间为1000s，平均飞行速度为107.5m/s，飞行轨迹如图7所示。

仿真过程中，SINS初始对准误差为0，初始速度误差为0.1m/s，初始位置误差为10m，初始姿态误差为10″；仿真中采用的传感器参数如表1所示。

表1仿真中采用的传感器参数指标

利用RAUPF算法对建立的SINS/SRS/CNS自主组合导航系统进行滤波解算，得到该组合导航系统输出的导航参数，并与事先设定的参考信息作差，获得SINS/SRS/CNS组合导航误差，同时与SINS、SINS/SRS子系统和SINS/CNS子系统误差进行对比，以验证SINS/SRS/CNS自主组合导航系统性能。

分别对SINS、SINS/SRS导航子系统和SINS/CNS导航子系统进行仿真计算，并对仿真结果进行比较。

对SINS、SINS/SRS导航子系统和SINS/CNS导航子系统的航向角误差、东向速度误差和纬度误差进行比较，仿真结果如图8～图11所示。如表2给出了各子导航系统误差的数值统计结果。

表2仿真结果比较

由仿真结果可以看出：

(1)SINS姿态误差、速度误差和位置误差随时间累积甚至发散，因此需要利用其他导航方式进行误差校正。

(2)对于SINS/CNS子系统，星敏感器可以获得较高的姿态精度，因此主要用来校正SINS的姿态误差；对于SINS/SRS组合导航系统，SRS能够获得较高的速度精度，因此校正后的速度精度较高。但是，单一的SINS/CNS组合导航系统无法满足飞行器自主组合导航定位的需求，因此需要结合两者的优点，设计SINS/SRS/CNS自主组合导航系统，以精确确定飞行器的位置。

(3)由光谱红移导航原理可以看出，光谱红移导航在利用由星敏感器获得的飞行器的姿态参数进行速度解算时，若星敏感器故障或受到遮挡，需要临时使用SINS提供的姿态数据进行速度解算以确保光谱红移导航继续工作。由图11可知，采用这种方法估计的误差完全可以满足飞行器对导航精度的需求，这说明该自主组合导航系统的抗干扰能力较强。

对SINS/SRS/CNS自主组合导航系统进行仿真计算，仿真结果如图12～图14所示。表3给出了自主组合导航系统误差的数值仿真结果。

表3SINS/SRS/CNS组合导航系统仿真结果

从对上述仿真结果进的分析中可知：本发明所设计的SINS/SRS/CNS自主组合导航系统，利用先进的信息融合技术，将SINS、SRS和CNS三者进行有机结合。该系统同时兼备了SINS、SRS和CNS各自的优点，克服了各单一导航系统的缺陷，在性能上实现了互补。SINS/SRS/CNS自主组合导航系统具有各单一导航系统所不具备的优良性能。组合导航系统的姿态精度达到1′以内，速度精度达到0.1m/s，定位精度3m以内，能满足飞行器对导航系统精度的需求，有效提高了导航系统的自主性、可靠性和精度。

综上可知，SINS、SRS和CNS是三个自主性很强的导航子系统，各具有自己的优点，但也存在各自的缺点。本发明基于太阳系天体光谱红移测速导航原理，利用太阳系自然天体的光谱红移信息、物体运动定律和自然天体的天文信息作为导航信息源，采用智能化技术，设计出一种具有完全自主性的SINS/SRS/CNS多源融合自主组合导航新方案。首先，研究了光谱红移自主导航的基本原理和算法；其次，设计出了SINS/SRS/CNS多源融合自主组合导航系统，给出了适合该组合导航系统解算的抗差自适应Unscented粒子滤波算法；最后，分别建立了自主组合导航子系统仿真实验和SINS/SRS/CNS多源融合自主组合导航仿真实验。实验结果表明：本发明所设计的SINS/SRS/CNS自主组合导航系统，不但自主性强，可靠性高，而且能满足飞行器对导航系统精度的需求。