基于Newton迭代滚动时域估计的GPS/SINS紧组合导航技术

伍星华，战兴群，刘 铭

（上海交通大学航空航天学院,上海201812）

0 引言

捷联惯性导航系统（Strapdown Inertial Navigation System，SINS）中的陀螺和加速度计的误差会随时间累积，因此SINS的精度是随时间下降的。GPS具有全球性、全天候的特点，高精度和低成本是它的显著优点[1]。但是，GPS也存在一些不足之处，主要表现为：GPS接收机的工作受载体机动的影响较大，GPS接收机的数据更新频率低（一般为1Hz），还容易受到干扰和人为控制，在遮挡的情况下（如室内、水下、地下及城市峡谷环境）不能使用[2]。因此，GPS和SINS具有优势互补的特点，将两者组合可以构建一种比较理想的导航系统，是目前组合导航系统的主要形式之一。根据组合结构、信息交换及组合程度的不同，GPS/SINS的组合方式可以分为松组合、紧组合、超紧组合等模式。由于模型不复杂，导航精度较高，紧组合的方式被广泛使用。

传统的Kalman滤波算法常被用于GPS/SINS紧组合导航系统以实现两者之间的信息融合，然而即使是扩展Kalman滤波也未能充分考虑到紧组合导航系统的非线性特性。因此，引入了滚动时域估计以便更充分地利用系统的多时历信息。文献[3]提出 “有限记忆”估计以替代Kalman滤波，被认为是首次应用滚动时域估计。文献[4]将此算法引入对约束线性系统的优化估计问题。

一直以来，滚动时域估计（Moving Horizon Estimation，MHE）多被应用于控制领域的状态估计，这是一个耗时的过程。因此，对于响应速度有要求的控制系统，往往在未得到最优估计的情况下便被强制反馈到了输出反馈回路中。这样，就需要快速的MHE算法，在不牺牲稳定性的前提下削减计算的复杂度[5]。有关 MHE的文献[6]～文献[8]，均提出了使用快速技术进行在线优化。文献[9]记录了基于梯度与Newton法的初步结果。文献[10]证明了基于Newton法、MHE算法的系统方程在一般非线性假设中的局部稳定性。

1 GPS/SINS紧组合导航系统

在GPS/SINS紧组合中，状态方程由SINS误差状态和GPS误差状态组成。将由SINS信息、卫星星历推算出的接收机相对于卫星视线方向的伪距、伪距率与据GPS码环和载波环得到的伪距、伪距率相减，作为Kalman滤波器的量测信息，然后利用Kalman滤波器估计SINS的误差量及接收机钟差等，最后根据SINS的误差量对SINS进行输出校正或反馈校正。

1.1 状态方程

SINS以东北天地理坐标系为导航坐标系，其主要误差有位置误差、速度误差、失准角、加速度计常值偏置以及陀螺常值漂移等，SINS的系统状态方程为

式（1）中，FI（t）为系统的状态转移矩阵，GI（t）为系统的噪声矩阵，WI（t）为系统的过程噪声矢量，XI（t）为系统的误差状态矢量。XI（t）的具体表达式为

式（2）中，δx、δy、δz为地心地固坐标系3 方向上的位置误差，为地心地固坐标系3方向上的速度Δ误差，ΔφE、ΔφN、φU为东北天3方向上的失准角，x、y、z为载体坐标系3方向上的加速度计常值偏置，εx、εy、εz为载体坐标系3方向上的陀螺常值漂移。

GPS的状态参数取2个时间相关的误差，δtu为接收机的时钟偏差，δfu为接收机的时钟频漂，则GPS的状态方程为

式（3）中，β为误差相关时间，ωtu、ωfu为驱动噪声。

将式（3）改写为矩阵形式，即有

合并式（1）和式（4），可得紧组合导航系统的状态方程

1.2 量测方程

紧组合系统采用伪距、伪距率组合方式，其量测方程为

式（6）中，H（t）为量测矩阵，V（t）为接收机在地理坐标系下位置与速度的误差，分别为伪距、伪距率量测矩阵。

在地心地固坐标系中，设载体的真实位置为（x，y，z），SINS 测量得到的载体位置为 （xI，yI，zI），据卫星星历给出的卫星位置为（xG，yG，zG）。当选取4颗可见卫星时，伪距和伪距率量测方程为

因此，

其中，li、mi、ni为载体到卫星的向量的方向余弦，bclk为与时钟误差等效的距离误差，dclk为与时钟频率等效的距离率误差。

2 滚动时域估计

众所周知，GPS/SINS紧组合导航系统实质上是非线性系统。一般研究时常将其看作线性系统，然后使用Kalman滤波器求解其状态方程和量测方程，或是使用扩展Kalman滤波，而扩展Kalman滤波的中心思想便是将非线性系统线性化后再做Kalman处理。相比较而言，后者对导航系统状态的估计更加精确，但其在处理过程中仍然忽略了较多的非线性特性。

GPS/SINS紧组合导航系统的离散化非线性系统方程为

类似于滚动时域控制，滚动时域估计可被描述为基于当前和过去的测量输出在线重复求解一个约束优化的问题，即利用滚动时域估计综合信息批上多个时刻的信息估计动态系统的状态向量。其中心思想是，在每个当前时刻，信息批上总有最老的测量量被最新的测量量替换掉，然后通过最优化信息批的最小平方代价函数得到状态估计。

式（14）的滚动时域估计依赖于计算得到的当前时刻t状态矢量xt的估计，而对的估计则基于yt-N，…，yt，ut-N，…，ut等。其中，N为考虑的信息批的长度，被称作滚动时域窗口的宽度。此外，规定滚动窗口的起始时状态估计xt-N的预测值，有

从而得到最小平方代价函数

在当前时刻t，在以下条件下的最小平方代价函数可被最优化

在初始时刻，应当首先初始化状态估计值，设定初始时刻的先验估计值，然后根据图1所示的流程继续优化、传播和预测。

根据对滚动时域估计算法原理的介绍，理论上该算法可比扩展Kalman滤波获得更高的导航精度（此结论已通过实验仿真结果得到证明）。但是，在应用该算法前，还应考虑以下两个问题：

1）滚动时域估计需要考虑到多个历元时刻的状态信息，计算量相对更大，因此计算速度会有差距。为保证计算效率，本文控制了滚动时域窗口的数量，在仿真时分别取值N=3、5、7等，以比较不同窗口大小下的导航精度和计算效率。

图1 滚动时域估计的示意图Fig.1 Diagram of MHE process

2）应当认识到，滚动时域估计不能被应用于线性化紧组合导航系统中。这是由于在线性化系统中，滚动时域估计的工作原理与扩展Kalman滤波一致。

3 Newton迭代法

为使滚动时域估计值快速收敛，应采用梯度下降算法。梯度下降算法一般可分为梯度下降法、共轭梯度法和Newton法。

梯度下降算法形式简单，在一般情况下均可保证收敛，但是收敛速度较慢。

作为二阶迭代方法，Newton法需要对Hessen矩阵及其逆矩阵开展计算和存储，其收敛速度快，但收敛的稳定性不能保证。

共轭梯度法是介于梯度下降法与Newton法之间的一个方法，是一个一阶方法。它克服了梯度下降法收敛慢的缺点，又避免了存储和计算Newton法所需要的二阶导数信息。但是，其形式比前两者都要复杂，对解算效率会产生较大的损害。

结合上面的分析，如果能够保证Newton迭代方法稳定收敛，那么Newton法为最佳的方法。

在使用Newton法之前，首先应证明迭代算法估计误差的稳定性。

在x t-N+1附近进行T a y l o r展开，可得

已知α0≤1，于是可以证明Newton迭代法指数稳定。因此，本文采用Newton法作为迭代方法。

应用Newton迭代公式，有

综上，本文使用基于Newton法的多步迭代滚动时域估计，其步骤如下：

1）设定起始迭代参数为

2）当m从1迭代到M时，依次有

4）最终根据式（26），可解算出结果

4 验证实验与仿真分析

惯性数据与GPS数据采集设备主要包括：MEMS-IMU、高性能商用型GPS接收机、实验车、笔记本电脑等。MEMS是一款基于微机电的捷联惯性导航系统，其主要特点为体积小、质量小、集成化、谐振频率高、响应时间短，由陀螺和加速度计构成。其中，陀螺的敏感度为0.989（″）/脉冲，速度增量为27.5μg×s/脉冲 。图2为实验车主要测量装置示意图，由于实时动态（Real-time Kinematic，RTK）技术的定位精度可达厘米级，故可将RTK数据作为参考。

图2 实验车示意图Fig.2 Diagram of experiment vehicle

紧组合导航系统可分为硬件部分和软件部分。硬件部分由导航计算机、GPS接收机、捷联惯导系统组成；软件部分即导航程序，主要包括捷联惯导解算程序、中频数据处理程序、紧组合导航滤波器等，可完成GPS定位信息和SINS数据间的数据融合。

实验车在不平坦的路面上行驶，在起始时刻（t=0）时车辆开始启动。 汽车行驶于北纬 30°54′、东经121°40′，实验区域为一片开阔地带，车辆运动轨迹如图3所示。

图3 实验车运动轨迹图Fig.3 Motion trail diagram of experiment vehicle

最终，通过紧组合导航系统解算求出的位置滤波误差和速度滤波误差的对比图如图4、图5所示，位置误差均值比对和速度误差均值对比如表1、表2所示。

图4 滤波算法NMHE3、NMHE5和NMHE7位置误差比较Fig.4 Comparison of NMHE3，NMHE5 and NMHE7 filter algorithm in position error

图5 滤波算法NMHE3、NMHE5和NMHE7速度误差比较Fig.5 Comparison of NMHE3，NMHE5 and NMHE7 filter algorithm in velocity error

表1 滤波算法NMHE3、NMHE5和NMHE7位置误差均值对比Table 1 Comparison of NMHE3，NMHE5 and NMHE7 filter algorithm in mean value of position error

表2 滤波算法NMHE3、NMHE5和NMHE7速度误差均值对比Table 2 Comparison of NMHE3，NMHE5 and NMHE7 filter algorithm in mean value of velocity error

首先，为比较不同时域窗口下的MHE算法对载体运动状态估计的精度，分别取窗口数N=3、5、7，并分别记相应的算法为NMHE3、NMHE5、NMHE7。

图4显示NMHE算法均快速收敛，位置误差曲线光滑，波动较小；图5显示速度误差均在±1m/s范围内振荡。由表1和表2可知，NMHE5的位置误差均值比NMHE3的位置误差均值提升均超过25%，而NMHE7相较于NMHE5仅提升了不到10%；且NMHE5的水平面上速度误差均值比NMHE3的水平面上速度误差均值提高了约23%，而NMHE7相较于NMHE5提高了约14%。

所有计算都是在PC的Matlab上完成的，PC配置了2.7GHz的Intel Core i7 CPU及16GB的RAM。计算得到了计算时间、导航误差与滚动时域窗口N之间的折线图，如图6所示。

因此，NMHE5相比NMHE3获得了较大提升，而NMHE7相比NMHE5改善不是很大。随着时域窗口N值的增加，计算效率线性增大。故综合考虑，设置滚动时域窗口值N=5，下文中NMHE如无特殊说明即取N=5。

图6 导航误差和平均计算时间折线图Fig.6 Line chart of navigation error and average computing time

图7显示滤波结果不再随着时间积累而发散。采用扩展 Kalman滤波 (Extended Kalman Filter，EKF）时，东北天三方向的位置误差是收敛的，但是天向与北向位置误差的波动较大。而本文提出的Newton迭代MHE算法得到的位置误差振荡约为15s，可见它的位置误差不仅可快速收敛，而且曲线光滑，波动较小。由图8可知，采用EKF算法时，得到的东向速度误差在±0.4m/s范围内振荡，北向、天向速度误差在±0.2m/s范围内振荡，波动较大，而改进的自适应EKF算法所得到的速度误差在更小的范围内振荡，稳定性较好。

图7 NMHE算法与EKF算法位置误差比较Fig.7 Comparison between NMHE algorithm and EKF algorithm in position error

图8 NMHE算法与EKF算法速度误差比较Fig.8 Comparison between NMHE algorithm andEKF algorithm in velocity error

表3与表4分别对比了两种滤波算法在东北天3个方向的位置误差均值和速度误差均值。从数据中可以看出，与EKF算法相比，NMHE算法计算出的位置误差和速度误差较小，确保了GPS/SINS紧组合的导航精度。

表3 NMHE算法与EKF算法的位置误差均值对比Table 3 Comparison between NMHE algorithm and EKF algorithm in mean value of position error

表4 NMHE算法与EKF算法的速度误差均值对比Table 4 Comparison between NMHE algorithm and EKF algorithm in mean value of velocity error

最后，使用滚动时域估计算法仿真出实验车的运动轨迹，该轨迹接近于精确的RTK轨迹。截取其中一段典型的转弯路段轨迹进行对比，对比结果如图9所示。

图9 RTK轨迹与NMHE5仿真轨迹对比Fig.9 Comparison between RTK trajectory and NMHE5 simulation trajectory

利用RTK测量的高精度定位数据作为参考，模拟实验车的实际轨迹，与新算法NMHE5的仿真结果进行对比，滚动时域估计结果与真实轨迹接近，具备较高的定位精度。

5 结论

针对手机导航几乎完全依赖GPS的问题，提出了基于Newton迭代MHE算法的GPS/SINS紧组合导航技术。该技术对手机内置惯导系统的精度要求不高，又可有效提升导航精度，更可提升手机导航在复杂环境中的可用性。

相对于传统EKF算法，基于Newton迭代MHE算法对位置误差的估计精度提高了50%，对速度误差的估计精度提高了1倍以上。因此，在手机、车载导航等领域，MHE算法有着广泛的应用前景。

MHE算法的效率和精度互相矛盾，不可兼得，其效率主要取决于滚动时域窗口N的大小。在实际应用过程中，应针对不同场景设定相应的参数N，如对高动态载体设置较大的窗口，对低动态载体设置较小的窗口，可以采用自适应算法使得窗口大小随运动状态的变化而改变。