CNS/SINS组合导航系统的UKF算法研究

朱璐瑛 林雪原 王 萍 许家龙 陈祥光,3

1 烟台南山学院工学院，山东省龙口市大学路12号，265713 2 航天晨光股份有限公司，南京市天元中路188号，211100 3 北京理工大学化学与化工学院，北京市中关村南大街5号，100081

CNS抗干扰能力强，误差不随时间积累，精度高达角秒级[1]，在航天飞机、载人飞船及深空探测等领域得到广泛应用[2]；同时，SINS和CNS都是完全自主的导航系统，将二者组合也是目前的研究重点。组合导航系统的滤波方法主要分为线性和非线性，其中线性滤波方法以常规卡尔曼滤波为代表，而非线性滤波方法包括扩展卡尔曼滤波(EKF)、不敏卡尔曼滤波(UKF)及粒子滤波(PF)等。相对于扩展卡尔曼滤波和粒子滤波，不敏卡尔曼滤波因具有精度高及易于实现的特点而得到广泛应用[3]。

目前，基于UKF的GNSS/SINS组合导航系统得到深入研究，主要原因是其速度和位置的样本点、均值及方差较容易生成，且可通过线性叠加产生[4-6]；由于姿态角不是普通的矢量，故对基于UKF的CNS/SINS组合导航系统的研究相对繁琐[7-8]。本文将UKF方法运用于CNS/SINS组合导航系统中，建立其非线性状态方程及线性量测方程，并提出了一种基于UKF的CNS/SINS组合导航系统滤波算法。

1 基于UKF的组合导航滤波模型

1.1 标准UKF简介

考虑如下形式的n维离散时间非线性系统：

(1)

式中，f(·)、h(·)为非线性函数；k为离散时刻；Xk为k时刻的系统状态，假设其方差矩阵为Pk；uk为系统确定性输入项；Zk为量测值；Wk和Vk分别为系统噪声和量测噪声，其方差矩阵分别为Qk和Rk。

由式(1)可知，标准UKF算法包含初始化、样点计算、时间更新、量测更新及滤波更新等过程。因式(1)中状态方程及量测方程均为典型的非线性方程，故UKF中的时间更新及量测更新过程也是典型的非线性运算，算法复杂度高。同时，标准UKF算法将系统状态向量、系统噪声和量测噪声增广为系统状态，也进一步增大了滤波算法的运算量。

1.2 CNS/SINS组合导航系统滤波模型

结合式(1)的非线性系统模型，基于CNS/SINS组合导航系统的UKF算法，以导航参数直接作为被估计的状态，采用捷联惯性导航系统的机械编排方程作为状态方程，且状态方程无需进行线性化处理。选取N、E、U地理坐标系作为导航坐标系，惯导系统采取指北方位机械编排的形式，其状态方程可表示为：

(2)

依据SINS工作原理，展开式如下：

(3)

CNS的输出可转换为地理坐标系下的横滚角、俯仰角及航向角，故选取CNS输出作为观测量，则系统的量测方程可表示为：

(4)

对式(2)和式(4)进行离散化处理，可以得到离散化的状态方程及量测方程，且假设导航系统的先验状态、过程噪声方差和测量噪声方差相互独立。

1.3 CNS/SINS组合导航的简化UKF算法

基于式(2)和式(4)可以看出，CNS/SINS组合导航系统具有状态方程非线性而量测方程线性的特点，为此仅将系统状态向量和系统噪声增广为状态向量[9]，则简化的系统增广状态向量为：

(5)

式中，χa、χX和χW分别为Xa、X和W的样点，其维数分别定义为La、Lx和Lw，且La=Lx+Lw。

简化后的UKF具体计算步骤如下[10-11]。

步骤1)：初始化。

(6)

步骤2)：样点计算。

(7)

(8)

其中，λ=α2(n+κ)-La，α、β和κ为比例因子。

步骤3)：时间更新。

(9)

(10)

步骤4)：量测更新。

(11)

而

(12)

同理，有：

PXZ=Pk|k-1HT

(13)

步骤5)：滤波更新。

对比标准UKF算法，简化UKF算法的计算复杂度得到了降低。

2 组合导航系统中姿态融合的设计

与位置和速度不同，计算姿态与真实姿态之间的误差不能通过简单的线性减法来实现，必须借助于捷联姿态误差分析原理进行。在捷联式惯性导航系统中，用数字平台坐标系模拟导航坐标系，由于平台有误差，故数字平台坐标系(n′)和导航坐标系(n)之间存在着平台角误差，将这种平台角误差写成列向量，可表示为[12]：

(15)

式中，φx、φy和φz分别为x、y和z方向平台角误差。则2个坐标系之间的转换矩阵为：

(16)

对捷联式系统计算的姿态阵为：

(17)

即

(18)

2.1 系统状态向量协方差矩阵的设计

UKF算法中，位置、速度及IMU噪声的样点可以通过式(7)进行简单的线性叠加得到，并且位置和速度对应的协方差矩阵也可通过式(10)计算得到。根据捷联导航原理可知，为避免姿态计算过程中出现奇点(即方程退化的问题)，需要通过四元数乘法或方向余弦矩阵等算法来计算姿态角，是典型的非线性计算。为此，本文以方向余弦矩阵为工具，计算姿态角样点、均值和方差，具体过程如下。

(19)

2.2 量测值一步预测误差的确定

(20)

根据式(16)的定义，可以得到量测值一步预测误差为：

(21)

3 仿真结果及分析

3.1 仿真条件

设定飞行器初始经度、纬度和高度分别为118°、29°、50 m，初始航向角、俯仰角和横滚角分别为90°、0°和0°，飞行时间为3 600 s，捷联解算周期为0.02 s，滤波周期为1 s。滤波初始参数设置如下：三维位置误差均为5 m，三维速度误差均为0.1 m/s，三维姿态角误差均为0.5°；陀螺随机游走驱动噪声及陀螺白噪声均为1°/h，加速度计随机游走驱动噪声及陀螺白噪声均为10-4g；CNS测角误差为300″，其采样周期为1 s。系统仿真在MATLAB7.1环境下进行，仿真轨迹如图1所示。

图1 载体飞行轨迹Fig.1 Flight trajectory of carrier

3.2 仿真结果及分析

根据仿真初始条件的设定，对于相同导航传感器仿真的原始数据，本文分别进行了基于下述滤波模型的仿真实验：1)基于常规卡尔曼滤波[7-8]；2)基于简化UKF算法；3)基于EKF算法[13]。图2～4分别给出3组仿真实验结果的位置误差、速度误差及姿态误差对比曲线，可以看出，3种算法均可有效抑制位置及速度的发散，使姿态角处于收敛状态，且本文简化UKF算法的效果要优于常规卡尔曼滤波及EKF算法。

图2 位置误差曲线对比Fig.2 Position error curve

图3 速度误差曲线对比Fig.3 Velocity error curve

图4 姿态误差曲线对比Fig.4 Attitude error curve

为更加具体形象地说明3种滤波算法对姿态角的滤波效果，表1给出实验结果数据统计的对比分析。可以看出，简化UKF算法的姿态角滤波精度比EKF算法提高了约9%，比常规卡尔曼滤波算法提高了约14%。

表1 姿态均方误差统计对比

为研究本文算法对滤波器初始三维姿态角误差的敏感度，分别在初始三维姿态角误差为1°及300″的情况下，对三维姿态角的均方差(即平台角误差)进行仿真，时长为600 s。图5给出了平台角误差的协方差仿真曲线，可以看出，在2种滤波器初始三维姿态角误差的条件下，本文算法得到的平台角误差协方差曲线完全重合，即本文算法对滤波器初始三维姿态角误差的敏感度极低。为对滤波器初始阶段协方差曲线的细节有更完全的描述，仅取图5中1～4 s时间段进行对比，具体如图6所示。可以看出，在2种初始条件下，经过3次滤波后本文算法得到的平台角误差协方差曲线完全重合，进一步说明了本文算法对滤波器初始三维姿态角误差的鲁棒性极高。

图5 不同滤波器初始姿态误差角条件下的平台误差角协方差对比曲线Fig.5 Platform angle error covariance contrast curve under the condition of different filter initial attitude error

图6 图5中起始时间段内的平台角误差协方差对比曲线Fig.6 Platform angle error covariance contrast curve during the starting period of Fig.5

基于图2～4的仿真实验，对标准UKF算法及本文简化UKF算法的计算量进行对比分析。从表2可以看出，简化UKF算法的计算时间比标准UKF算法减少了18.5%，有效地降低了算法的复杂度。

表2 算法计算量分析

4 结 语

CNS/SINS组合导航系统采用姿态融合的方式，其状态方程具有强非线性、量测方程具有线性的特点。当采用UKF算法对系统进行滤波时，由于姿态角样点、样点均值和样点方差的生成不同于以往GNSS/SINS组合导航系统模式，本文首先对标准UKF算法进行简化，在生成姿态角样点的基础上，借助于平台角误差这一概念设计了姿态角样点均值及方差的生成过程；同样，借助于平台角误差，设计了量测更新过程中的量测一步预测误差生成过程。仿真结果表明，本文算法具有比常规卡尔曼滤波及EKF算法更高的导航精度，同时本文算法对滤波器初始三维姿态角误差具有更高的鲁棒性。