GNSS/INS组合导航的随机时延卡尔曼滤波*

潘新宇，赵英策，李建勋

(1． 上海交通大学自动化系，上海 200240；2． 中国航空工业沈阳飞机设计研究所，辽宁 沈阳 110035)

要实现无人驾驶，如何高精度地进行导航是不可避免的一个问题。常用的导航系统如全球卫星导航系统，惯性导航系统，激光雷达等，各自都具有一些优缺点。惯性导航系统(INS)具有短期精度高，连续工作的特点，但随着惯性传感器的误差的累积，长期导航的误差会无限增长。而全球导航卫星(GNSS)导航系统，可以提供良好的长期高精度导航结果，但GNSS信号的输出频率相比于惯性导航系统会低很多，同时当信号被遮挡以及干扰时，GNSS导航系统就会停止工作，没有输出的结果。如何将两个或者多个传感器的导航结果以合适的方式进行组合，以得到稳定、高精度的导航结果成为国内外众多学者的研究方向。

GNSS的信号获取是通过车载接收机与绕地卫星进行通信完成的，在通信过程中，信号不可避免地会通过电离层和对流层，加上卫星上存在时钟对准的问题，会出现接收机接收到的伪距和伪距率相比于真实的位置和速度具有随机的时间延迟。

面对测量信号具有随机延迟的情况，出现了很多对卡尔曼滤波算法进行改进的策略，主要分为抑制干扰与提前补偿两个角度。文献[4]中将随机延时视为量测方程的乘性不确定性，将当前噪声和滞后噪声结合成一个整体，将原系统转化为一个离散，具有自相关噪声的随机不确定系统，从抑制干扰的角度设计了鲁棒卡尔曼滤波器来提高滤波结果的稳定性和精度。文献[5]同样从抑制干扰角度，研究了基于性能准则的不确定时变状态时滞随机非线性耦合复杂网络的鲁棒滤波问题。从抑制干扰的角度出发的方法，在已知随机延迟的概率时，无法利用该信息。文献[6]中提出了一种利用数学期望的整数值函数来补偿随机延迟的新型滤波器结构，适当设计滤波器的增益，使误差协方差在跟踪意义上最小，并证明该算法经过线性化后的稳定性和收敛性。文献[6]中的方法从数学期望的整数值角度出发，取整过程中有截断误差，得到的滤波结果是有偏估计，并且最新接收的量测值不能直接用于更新最新的结果，输出具有非实时性。本文从平均状态时延补偿的角度，对卡尔曼滤波器进行改进，得到了一个实时无偏估计的滤波器，将测量值用于更新过去的多个状态。

1 GNSS与INS松组合系统模型

1.1 GNSS/INS系统的状态方程

INS的导航方程构成了系统的状态方程，组合导航系统中待估计的位置姿态为系统的状态，系统的速度和位置相对于地心地固(ECEF)坐标系而言，待估计的状态向量表示为式(1)

(1)

式中，上标T表示向量的转置，表示移动物体三个方向的姿态角，表示移动物体的三个方向速度，表示移动物体的三维位置，表示加速度计的零偏，表示陀螺仪的零偏，是一个15维的状态向量。组合导航系统的状态方程表达式，可以由式(2)表示：

=-1+

(2)

其中，∈×是系统的状态转移矩阵，由系统参数确定，∈是系统待估计的状态，为系统待估计的状态的维度，本文中取为15，为系统所处的时刻，对应到实际模型中即是加速度计和陀螺仪的所提供的加速度和角速度与实际的差值经过噪声驱动矩阵驱动后对系统状态的影响，这是由于陀螺仪和加速度的精度和漂移导致的，出于简单考虑，将驱动后的结果作为系统的过程噪声。将其建模成零均值的白噪声，应有～(0,)，为系统的过程噪声协方差矩阵。

1.2 GNSS/INS系统的量测方程

本文中选取的GNSS与INS组合方式是松耦合，相比于深耦合和紧耦合，松耦合具有结构简单，易于实现的特点。松耦合中GNSS与INS单独进行工作，GNSS将系统接收到的信号，单独进行解算，输出系统的位置和速度，再与INS系统的导航结果进行融合，并对INS系统中加速度计和陀螺仪的零偏进行反馈校正。

GNSS系统单独获得的位置速度信息可以由式(3)表示

=+

(3)

由于GNSS的信号在传输过程中，会有卫星时钟和地面时钟配准的问题，以及信号通过电离层和对流层会有较大的传输延迟，经常会出现GNSS的信号滞后于当前时刻的情况出现。为了获得针对量测具有随机延时的状态估计器，需要建立一个描述随机延迟性质的数学模型。当随机延时存在时，此时滤波器接收到的量测值可能不等于此时系统状态的观测值，而是之前某个时刻的测量。

本文的研究是在已知随机延迟的概率分布的条件下，重新设计一个迭代的滤波器，并给出该滤波器的具体的结构与参数的设置。

首先对随机延迟进行建模，设随机延迟的取值在集合={0,1,……,}中，其中是给定的最大可能的延迟时间。随机时延，过程噪声以及测量噪声三个随机量两两独立。

将随机延迟的概率可以表示为式(4)

Prob{=}=，∈

(4)

其中0≤≤1，∑∈=1。

此时滤波器所接收到的量测可以由式(5)描述：

=-

(5)

考虑以上两点，那么测量具有随机延迟的测量方程可以由式(6)表示

(6)

其中，

2 时延卡尔曼滤波的结构和原理

2.1 时延卡尔曼滤波的结构

常规的线性高斯的系统用式(7)、式(8)来表示：

=-1+

(7)

=+

(8)

其中，∈是系统待估计的状态，∈是系统的测量，和是协方差矩阵为和的零均值白噪声，是系统的状态转移矩阵。具有高斯白噪声的线性系统的滤波问题，传统的卡尔曼滤波已经给出了最优的无偏估计。

随机延时系统由状态方程(2)与观测方程(6)进行表示。本文针对该类系统设计的滤波器结构上与卡尔曼滤波器保持一致，分为先验预测估计以及后验更新校正两步。

时延卡尔曼滤波器的结构可由式(9)和(10)来表示，重点在于式(10)中的+1的确定：

(9)

(10)

其中

最新时刻的状态估计可由式(11)得到：

(11)

2.2 时延卡尔曼滤波器的原理

本文在已知上一时刻的估计值和误差的前提下，从最小化误差的协方差角度出发得到下一时刻的估计值和误差，以得到一个递归的滤波器。

(12)

(13)

其中

(14)

(15)

(16)

即可得到+1的表达式(17)。

(17)

那么算法就可以用式(17)～(21)进行描述：

首先是滤波器的先验预测值：

(18)

(19)

根据式(17)，便可以得到滤波器的后验估计：

(20)

(21)

值得注意的是，当随机延迟被设置为无延迟的时候，该滤波算法就会退化为基本的卡尔曼滤波。

3 仿真示例

3.1 仿真实现

本文的仿真中真实轨迹是由仿真平台Spirent SimGen产生的车载运动，包含以下运动：

初始速度为10 m/s，加速至20 m/s，减速到 10 m/s,90°转弯，加速到20 m/s,-30°转向；加速到30 m/s，30°转向，减速到5 m/s, -90°转弯，停车，共持续175 s。

系统中INS系统每0.01 s输出一次结果，而GNSS系统每0.1 s输出一次信号，分别对较低延迟和较高延迟分别进行仿真。仿真对比的结果由未考虑延时的卡尔曼滤波[6]中的算法，以及本文所提出的算法的结果对比。

3.2 仿真结果

当测量量具有较低延迟，延迟概率取为:

=07,=03

此时，三种滤波算法的北向的位移误差如图1所示。

图1 低时延下不同滤波算法的北向位置误差

北向速度误差如图2所示。

图2 低时延下不同滤波算法的北向速度误差

东向位置误差和速度误差如图3、图4所示。

图3 低时延下不同滤波算法的东向位置误差

图4 低时延下不同滤波算法的东向速度误差

进行100次蒙特卡洛仿真，低延迟下北东两个方向的速度位移的均方差(RMSE)如表1所示，其中算法1指的是[6]中的方法，算法2指的是本文提出的方法。

表1 低延迟RMSE比较

当测量量具有较高延迟，延迟概率取为：

=02,=04,=02,=02

此时，三种滤波算法的东向位置误差和速度误差如图5、图6所示。

图5 高时延下不同滤波算法的北向位置误差

图6 高时延下不同滤波算法的北向速度误差

东向位置误差和速度误差如图7、图8所示。

图7 高时延下不同滤波算法的东向位置误差

图8 高时延下不同滤波算法的东向速度误差

进行100次蒙特卡洛仿真，高延迟下北东两个方向的速度位移的均方差(RMSE)如表2所示，算法1和算法2同表1。

表2 高延迟RMSE平均比较

由图1～图8可知，本文提出的算法在载体发生速度变化的时候，与其余两种算法相比较，可以有效抑制由于量测时延所引起的滤波器输出的尖峰问题，系统的速度估计误差始终被维持在一个较小的区间中，位置曲线的震荡幅度也较为稳定。

从表1和表2的数据可以看出，本文提出的算法的滤波误差要明显小于其余两种，量测信号具有的随机延迟较大时，改进后的滤波器的输出误差减小更为明显。

4 结束语

针对测量具有随机延迟的滤波问题，本文首先建立一个描述随机延迟性质的数学模型，并将该数学模型用于松组合的组合导航系统，若忽略延迟，滤波器的输出会出现尖峰现象。本文设计了一个新的滤波器结构，从最小化滤波器后验误差的协方差矩阵的角度出发，给出了该滤波器的具体的结构与参数的设置。仿真结果表明，本文的滤波算法可以充分地利用随机时延信息，保证了INS/GNSS组合导航系统在随机延时下状态估计的精度与稳定性。