SINS/GPS组合导航的直接状态估计方法∗

代晓巍 赵书圆 张 梦

（1.中国人民解放军91550部队 大连 116023）（2.海军大连舰艇学院 大连 116018）

1 引言

组合导航系统将具有长时稳定性的GPS导航系统输出与具有短时精确性的捷联式惯性测量单元（IMU）输出通过滤波估计算法进行融合，能够得到相比原系统更为准确和稳定的结果。目前已经成为同时估计运动载体位置速度与姿态（INS状态）的标准方法。从状态估计的形式上看，GPS/SINS组合导航可以分为直接估计与间接估计两种形式［1～4］，即滤波器是通过估计误差间接地估计位速姿态状态，还是直接估计出所需要的信息。融合GPS数据与IMU数据最常用的方法即是基于间接估计的卡尔曼滤波方法，将姿态估计误差，速度估计误差和位置估计误差作为滤波模型的状态矢量，根据惯导力学编排，忽略二阶及以上小量，得到具有线性动态的误差状态方程，通过估计出的误差来修正当前的INS变量。得益于其融合模型为线性，具有低复杂性，低运算量和易于实际系统应用实现的优势。间接估计方法已经在导航领域有长时间的成熟应用。然而随着搭载平台计算技术的发展，研究更为精确和高效的方法是必要的［5～8］。

直接估计方法是基于标准的惯导力学编排，将姿态参数、速度矢量与位置矢量以及IMU偏差等变量直接作为滤波模型的状态矢量。因此，直接估计模型更能准确地描述载体的动态特性，对于大误差、恶劣环境和载体大机动的条件下具有优势，且相比间接估计的开环修正方式，直接估计的闭环修正结构也能够更好地避免和抑制误差累积传播。限制直接估计方式应用的主要原因是由于状态方程的非线性特征导致的计算复杂性、运算量与算法数值问题［9～12］。

本文提出一种基于直接估计方式和扩展卡尔曼滤波的松组合导航方法，首先通过惯导力学编排建立了直接估计状态模型与松组合的观测模型，然后在求解雅克比矩阵的基础上，采用扩展卡尔曼滤波求解。数值仿真结果表明，在对准误差较大等恶劣的条件下，直接估计方法相比间接估计方法具有更好的性能，但是同时其也存在计算形式复杂、数值处理精度要求高和运算量巨大的缺点。在精确对准和良好条件下，直接估计方式与间接估计方式的估计误差是相当的，因此前者的适用性会受到其大运算量的限制。

2 INS/GPS组合导航的滤波模型

2.1 惯性导航力学方程

对于直接估计方式，SINS的标准惯性导航力学方程即作为滤波状态模型。SINS体坐标系为FRD（前-右-下）坐标系，记为b系；当地导航坐标系为NED（北-东-地）坐标系，记为n系，ECEF系记为e系，ECI系记为i系，以四元数表示姿态的标准惯导力学方程为

2.2 间接估计的误差状态方程

假设位置、速度以及姿态误差都是小量，其二阶可忽略，那么根据式（1）～（3）可以得到姿态角、速度位置以及误差状态方程为

2.3 直接估计的状态空间模型

根据惯导力学方程导出直接估计的状态方程，状态方程主要包括姿态、速度、位置以及IMU测量偏差的演进方程。对于姿态演进方程，式（1）中，并考虑到测量偏差的影响，则可以将式（1）重写为如下的方程：

NED系平台相对地球的角速度为

地球转动角速度在NED系下表示为

陀螺仪测量值可以用如下模型来描述。

其中bg为建模为一阶随机游走的陀螺仪测量偏差，vg为陀螺仪测量白噪声，ug为随机游走过程的驱动噪声。

类似地，加速度计测量值也采用上式给出的模型来描述，即

其中ba为建模为一阶随机游走的加速度计偏差漂移，va为陀螺仪测量白噪声，ua是随机游走过程的驱动噪声。

综上可以归纳出直接模式状态量为

由式（2）～（3），式（5）～（6），式（9）～（10）构成函数矢量，这些函数构成的连续时间方程进行离散化，进一步可得到直接估计的状态方程为

其中f(·)是离散状态函数，wk-1为过程噪声矢量。

对于本文选择的惯导与GPS的松组合方式，直接以GPS给出的位置与速度量测作为观测值，则可以得到线性的观测模型为

其中εGPS为GPS的位置和速度测量误差，观测矩阵

需要指出的是，实际GPS的位置测量误差是在NED系下以米为单位给出的，而GPS测量位置的平面坐标是以曲线坐标系下以角度给出的（纬度和经度），因此要通过式（5）将NED系下的测量误差转换到纬度和经度单位。

3 数值结果与分析

为了直观地对比直接估计方法与经典方法对于姿态、速度以及位置等感兴趣的INS状态量的估计性能，本文基于文献中的GNSS模拟器生成的载体飞行数据进行仿真。飞行数据包括纬、经度及椭球高，北向、东向及地向速度，横滚角，俯仰角，偏航角。其中姿态角是载体相对于NED系的欧拉角。

为了直观描述上述载体运动情况，图1给出了飞行轨迹和姿态角随时间的变化情况。其中飞行速度为 200m/s，经历两个45°转弯后爬升至500m。加速度计和陀螺仪的读数首先利用上述运动数据经导航方程变换得到理想的惯性测量读数，然后在此之上再叠加典型惯性测量单元（IMU）的刻度、正交性、噪声、偏差以及漂移误差对理想读数进行变换得到仿真测量读数，具体加速度和陀螺仪传感器误差设置为加速度计零偏[900，-1300，800]μg ；测量噪声；刻度误差与轴耦合误差矩阵为

图1 载体飞行过程

加速度相关偏差：

通过上述针对根据模拟载体运动测量数据的处理结果与分析，如图2～4，在存在较大初始误差的情况下，经典的间接误差估计方法在位置估计上出现了大幅的累积误差。直接状态估计的滤波方法对于INS信息的估计结果要较大幅度的优于基于误差传播模型的卡尔曼滤波的经典方法，且能够在缺少准确初始条件的情况下收敛到真实值，因此相比间接模式估计具有较好的动态估计性能。

图2 跟踪跑动中的人的实例

图2 直接估计方法与经典方法的位置估计误差对比

图3 两种方法的速度估计误差对比

图4 两种方法的姿态估计误差对比

4 结语

通过惯性导航力学编排建立的直接形式状态估计模型与松组合观测模型。提出了一种基于扩展卡尔曼滤波求解的GPS/SINS组合导航方法，通过基于模拟飞行机动运动数据的数值仿真对提出模型和算法进行了验证，并与现有方法进行了对比。结果表明，直接估计方法相在恶劣初始误差条件下的具有较大的优势。在运算条件充分满足的条件下，可作为一种在初始对准阶段快速提供准确精确初值的滤波方法。下一步研究的重点是其在实际条件下的误差特性与高效计算的方法。