CN105954783A - 一种改进gnss/ins实时紧组合导航实时性能的方法 - Google Patents

Abstract

本发明公开了一种改进GNSS/INS实时紧组合导航实时性能的方法，包括(1)保存GNSS采样时刻k的预测误差协方差矩阵和机械编排解算的导航状态；(2)在时刻k时GNSS观测数据的接收时刻，采用Kalma滤波法进行组合更新解算，得时刻k的状态参数估计量以及状态参数协方差估计量(3)根据和估计组合更新解算完成时刻j的状态参数估计量以及状态参数协方差估计量(4)采用以及修正时刻j的惯导误差。本发明可降低GNSS数据延迟和组合解算耗时对实时导航输出的影响，可有效改善实时组合导航的实时性能，对组合导航算法在运算能力较低的处理器上的实现以及对实时性要求严格的应用场合有格外重要的意义。

Description

一种改进GNSS/INS实时紧组合导航实时性能的方法

技术领域

本发明属于实时组合导航系统技术领域，尤其涉及一种改进GNSS/INS实时紧组合导航实时性能的方法。

背景技术

多种手段组合导航是当前导航技术的发展趋势，基于全球卫星导航系统(GlobalNavigation Satellite System，GNSS)和惯性导航系统(Inertial Navigation System，INS)的组合是目前最具有应用价值的组合模式之一，特别是基于GNSS原始观测量和INS数据的紧组合。该组合模式下，即使在可见卫星不足以单独进行GNSS解算时，仍可有效利用有限的GNSS观测数据和INS数据通过Kalman滤波进行紧组合解算，得到可靠的导航信息。因此，GNSS/INS紧组合在工程中得到了越来越广泛的应用。

然而，GNSS/INS紧组合实时应用中的GNSS观测数据时间延迟和组合解算时间延迟(这里统称为时间延迟)问题，会分别导致当前时刻观测数据无法实时得到以及观测数据无法及时处理的问题，直接影响GNSS/INS紧组合系统的实时性能。目前，时间延迟的解决策略主要采用增广状态方法和基于新息重组理论建立最优滤波器。其中，增广状态方法会增加状态维度，使得计算量增大；而新息重组的方法，需要建立多个观测方程，如果有多步延迟，则不利于对Kalman滤波器的灵活拓展与应用。而且这些方法只能解决观测数据传输延迟，对于更新计算延迟则无能为力。GNSS/INS实时紧组合法主要包括“机械编排、Kalman预测”以及“Kalman观测更新”两部分，前者一般可以在IMU(Inertial Measurement Unit)采样间隔内完成计算；后者则比较耗时，普通嵌入式处理器中无法在足够短的时间内(例如下一个惯导数据到来前)及时完成，再加上GNSS观测数据大都是从商业接收机板卡中获取，观测数据传输会有一定的延迟，这就导致Kalman滤波的观测更新计算不能及时完成。这种情况下，如果使用标准Kalman滤波算法，就需要将后续IMU观测数据缓存起来不处理，等待观测更新完成才能进行下一步计算，这就会造成组合导航结果输出的阻塞和延迟，影响系统的实时性能。开发GNSS/INS实时紧组合系统时，就需要一种方法解决在存在时间延迟的情况下，及时完成导航解算。

发明内容

针对现有GNSS/INS实时紧组合导航中GNSS观测数据传输延迟和更新计算耗时较长所带来的时间延迟问题，本发明提出了一种改进GNSS/INS实时紧组合导航实时性能的方法。

本发明采用如下技术方案：

一种改进GNSS/INS实时紧组合导航实时性能的方法，包括：

将GNSS/INS紧组合分解成机械编排、Kalman预测任务和Kalman观测更新任务，下述步骤实施过程中保持机械编排、Kalman预测任务优先级高于Kalman观测更新任务：

(1)将GNSS采样时刻记为时刻k，保存时刻k的预测误差协方差矩阵和机械编排解算的导航状态；

(2)在时刻k时GNSS观测数据的接收时刻，基于时刻k的GNSS观测数据、和根据时刻k的机械编排解算的导航状态形成的状态向量z_k，采用Kalman滤波法进行组合更新解算，得时刻k的状态参数估计量以及状态参数协方差估计量

(3)将组合更新解算完成时刻记为时刻j，基于公式估计时刻j的状态参数估计量以及状态参数协方差估计量Φ_j/k表示时刻k到时刻j的累积状态转移矩阵；M_j,k+1为时刻k+1到时刻j的累计状态噪声矩阵， $M_{j,k+1}=\underset{i=k+1}{\overset{j}{\Σ}}\left({\mathrm{\Φ}}_{j/i}{Q}_{i-1}{\mathrm{\Φ}}_{j/i}^T\right);$

(4)采用以及修正时刻j的惯导误差。

和现有技术相比，本发明具有如下优点和有益效果：

1、本发明针对GNSS/INS组合导航系统，基于Kalman滤波法，将滞后的状态参数估计量以及状态参数协方差估计量转移到当前时刻，从而降低GNSS数据延迟和组合解算耗时对实时导航输出的影响，可有效改善实时组合导航的实时性能。

2、本发明对组合导航算法在运算能力较低的处理器上的实现以及对实时性要求严格的应用场合有格外重要的意义。

3、在DSP硬件平台上实现和验证本发明，车载实测结果表明：相对于标准组合导航解算法，本发明能够在保障组合导航系统精度的前提下，最大限度地减小输出导航结果的时间延迟，保障组合导航结果的实时性。

附图说明

图1为GNSS/INS实时紧组合导航系统结构图；

图2为时间延迟的处理时序示意图；

图3为实施例中车载测试轨迹；

图4为GNSS数据延迟的统计直方图，其中，横坐标表示GNSS数据延迟，单位为ms；

图5为200Hz中断计算耗时的统计直方图，其中，横坐标表示200Hz中断计算耗时，单位为ms；

图6为组合更新计算耗时的统计直方图，其中，横坐标表示组合更新计算耗时，单位为ms；

图7为本发明导航解算延迟示意图；

图8为实施例的导航误差曲线。

具体实施方式

GNSS/INS紧组合可分解成具有优先级别的两个任务“机械编排、Kalman预测”和“Kalman观测更新”独立进行，其中，任务“机械编排、Kalman预测”的优先级高于任务“Kalman观测更新”。

本发明具体步骤如下：

(1)将GNSS采样时刻记为时刻k，保存时刻k的预测误差协方差矩阵和机械编排解算的导航状态。

(2)见图2，将时刻k时GNSS观测数据的实际接收时刻记为时刻ts，在时刻ts采用Kalman滤波法进行组合更新解算(见式(12)～(13))，获得时刻k的状态参数估计量以及状态参数协方差估计量

组合更新解算完成后，还需要将滞后计算出来的反映时刻k的状态参数估计量以及状态参数协方差估计量转移到组合更新完成时刻j，以修正时刻j的惯导误差，即反馈修正。本发明利用状态转移模型达到该目的，所述的状态转移即根据时刻k的状态参数估计时刻j的状态参数。

根据k个时刻的观测向量z₁、z₂……z_k对时刻j的状态参数x_j作线性最小方差估计

${\hat{x}}_{j,k}^{-}=\hat{E}\[x_j/z_1{z}_2\mathrm{...}{z}_k\]---\left(1\right)$

式(1)中，z₁、z₂……z_k分别表示时刻1、2、……k的观测向量，E(·)表示数学期望。

GNSS/INS紧组合导航系统中，状态参数x由INS误差状态和GNSS接收机时钟误差共同组成，可以表示为：

$x={\left[\begin{array}{ccccccccc}{\mathrm{\δr}}^n& {\mathrm{\δv}}^n& \mathrm{\ψ}& b_g& b_a& s_g& s_a& {\mathrm{\δV}}_{t_R}& {\mathrm{\δf}}_R\end{array}\right]}^T---\left(2\right)$

式(2)中：δrⁿ、δvⁿ、ψ分别表示位置误差、速度误差、姿态角误差；b_g、s_g分别表示陀螺的零偏和比例因子；b_a、s_a分别表示加速度计的零偏和比例因子；表示接收机钟差误差；δf_R表示接收机钟漂误差。

δrⁿ、δvⁿ、ψ、b_g、s_g、b_a、s_a为INS误差状态，δf_R为GNSS接收机时钟误差。

考虑如下线性离散系统：

x_k＝Φ_k/k-1x_k-1+G_k/k-1w_k-1 (3)

z_k＝H_kx_k+u_k (4)

式(3)～(4)中：

x_k、x_k-1分别表示时刻k、k-1的状态参数；

Φ_k/k-1为时刻k-1到时刻k的状态转移矩阵，其值由状态参数的动力学模型决定；

G_k/k-1为时刻k-1到时刻k的系统噪声驱动矩阵；

w_k-1为时刻k-1的系统噪声；

z_k为时刻k的观测向量，其由INS推算的GNSS伪距、多普勒值与GNSS接收机观测的伪距、多普勒观测值作差得到；

H_k为时刻k的设计矩阵；

u_k为时刻k的观测噪声矩阵。

根据式(3)，并且考虑到Φ_k+1/k-1＝Φ_k+1/kΦ_k/k-1，从时刻k的状态参数x_k转移时刻j的状态参数x_j，如下：

$x_j={\mathrm{\Φ}}_{j/k}{x}_k+\underset{i=k+1}{\overset{j}{\Σ}}\left({\mathrm{\Φ}}_{j/i}{G}_{i/i-1}{w}_{i-1}\right)---\left(5\right)$

上述Φ_k+1/k-1表示时刻k-1到时刻k+1的状态转移矩阵，Φ_k+1/k、Φ_j/k分别表示时刻k到时刻k+1、时刻j的状态转移矩阵，Φ_j/i表示时刻i到时刻j的状态转移矩阵；G_i/i-1表示时刻i-1到时刻i的系统噪声驱动矩阵；w_i-1表示时刻i-1的系统噪声。

式(5)代入式(1)得到：

$\begin{array}{c}{\hat{x}}_{j,k}^{-}=\hat{E}\[x_j/z_1{z}_2\mathrm{...}{z}_k\]=\hat{E}\[({\mathrm{\Φ}}_{j/k}{x}_k+\underset{i=k+1}{\overset{j}{\Σ}}({\mathrm{\Φ}}_{j/i}{G}_{i/i-1}{w}_{i-1}\left)\right)/z_1{z}_2\mathrm{...}{z}_k\]\\ =\hat{E}\[{\mathrm{\Φ}}_{j/k}{x}_k/z_1{z}_2\mathrm{...}{z}_k\]+\underset{i=k+1}{\overset{j}{\Σ}}({\mathrm{\Φ}}_{j/i}{G}_{i/i-1}\hat{E}\[w_{i-1}/z_1{z}_2\mathrm{...}{z}_k\])\end{array}---\left(6\right)$

由式(3)可知，w_k-1只影响到x_k，所以w_k-1与z₁、z₂……z_k不相关，且E(w_k-1)＝0，E(·)表示数学期望，因此有：

由式(3)可知，w_k-1只影响到x_k，所以w_i-1与z₁、z₂……z_k不相关，且E(w_i-1)＝0，i≥k+1且i≤j，因此有：

${\hat{x}}_{j,k}^{-}={\mathrm{\Φ}}_{j/k}\hat{E}\[x_k/z_1{z}_2\mathrm{...}{z}_k\]={\mathrm{\Φ}}_{j/k}{\hat{x}}_k^{+}---\left(7\right)$

式(7)中，表示x_k的更新估计量。

的误差为：

$\begin{array}{c}{\tilde{x}}_{j,k}=x_j-{\hat{x}}_{j,k}^{-}\\ ={\mathrm{\Φ}}_{j/k}{x}_k+\underset{i=k+1}{\overset{j}{\Σ}}\left({\mathrm{\Φ}}_{j/i}{G}_{i/i-1}{w}_{i-1}\right)-{\mathrm{\Φ}}_{j/k}{\hat{x}}_k^{+}\\ ={\mathrm{\Φ}}_{j/k}{\tilde{x}}_k+\underset{i=k+1}{\overset{j}{\Σ}}\left({\mathrm{\Φ}}_{j/i}{G}_{i/i-1}{w}_{i-1}\right)\end{array}---\left(8\right)$

对于预测误差协方差矩阵T表示矩阵的转置：

$\begin{array}{c}{P}_{j,k}^{-}=E\[{\tilde{x}}_{j,k}{\tilde{x}}_{j,k}^T\]\\ ={\mathrm{\Φ}}_{j/k}E\[{\tilde{x}}_k{\tilde{x}}_k^T\]{\mathrm{\Φ}}_{j/k}^T+\underset{i=k+1}{\overset{j}{\Σ}}({\mathrm{\Φ}}_{j/i}{G}_{i/i-1}E\[w_{i-1}{w}_{i-1}^T\]G_{i/i-1}^T{\mathrm{\Φ}}_{j/i}^T)\\ ={\mathrm{\Φ}}_{j/k}{P}_k^{+}{\mathrm{\Φ}}_{j/k}^T+\underset{i=k+1}{\overset{j}{\Σ}}\left({\mathrm{\Φ}}_{j/i}{G}_{i/i-1}{Q}_{i-1}{G}_{i/i-1}^T{\mathrm{\Φ}}_{j/i}^T\right)\end{array}---\left(9\right)$

式中，表示x_k的误差；为x在时刻j的预测误差协方差矩阵，为x在时刻k的协方差矩阵；Q_i-1为w_i-1的协方差矩阵。GNSS/INS紧组合中G_i/i-1为正定矩阵，因此，

令M_j,k+1表示时刻k+1到时刻j的累计状态噪声矩阵。具体实施时采用公式(10)累积状态噪声矩阵：

$\left\{\begin{array}{c}{M}_{m+1,k+1}=Q_m+{\mathrm{\Φ}}_{m+1/m}{M}_{m,k+1}{\mathrm{\Φ}}_{m+1/m}^T\\ M_{k+1,k+1}=Q_k\end{array}\right.---\left(10\right)$

式(10)中，Q_m、Q_k分别表示w_m、w_k的协方差矩阵，w_m、w_k分别表示时刻m、k时的系统噪声，m为大于k的时刻；Φ_m+1/m表示时刻m到时刻m+1的转移矩阵；M_m,k+1表示时刻k+1到时刻m的累计状态噪声矩阵，M_k+1,k+1表示时刻k+1的累计状态噪声矩阵。

综上，可获得状态转移公式，如下：

$\left\{\begin{array}{c}{\hat{x}}_{j,k}^{-}={\mathrm{\Φ}}_{j/k}{\hat{x}}_k^{+}\\ P_{j,k}^{-}={\mathrm{\Φ}}_{j/k}{P}_k^{+}{\mathrm{\Φ}}_{j,k}^T+M_{j,k+1}\end{array}\right.---\left(11\right)$

由式(11)可以看出，状态转移可以由累积状态转移矩阵Φ_j/k及累计状态噪声矩阵M_j,k+1得到，Φ_j/k为时刻k到时刻j时间段内相邻时刻的状态转移矩阵的连乘，M_j,k+1由式(10)获得。把GNSS采样时刻看作时刻k，组合更新解算完成时刻看作时刻j，利用式(11)把时刻k的状态参数估计量以及状态参数协方差估计量转移到时刻j，进而对时刻j的惯导误差进行反馈修正。

本发明组合更新解算采用标准Kalman滤波法，如下：

${\hat{x}}_k^{+}={\hat{x}}_{k,k-1}^{-}+K_k(z_k-H_k{\hat{x}}_{k,k-1}^{-})---\left(12\right)$

$P_k^{+}=(I-K_k{H}_k)P_{k,k-1}^{-}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T---\left(13\right)$

式中，表示根据时刻1、2…k-1的观测向量估计的时刻k的状态参数；K_k为卡尔曼滤波增益矩阵；为时刻k的状态参数协方差估计量；为步骤(1)中保存的时刻k的预测误差协方差矩阵；I表示单位矩阵；R_k为观测信息先验方差矩阵，按卫星高度角确定观测值先验方差。

在步骤(1)～(2)处理过程中保持优先进行INS机械编排和Kalman预测。这样，只要能保证“惯性机械编排、Kalman预测”在IMU采样间隔内完成，并保证“Kalman观测更新”计算在GNSS采样间隔内完成，就能够保证组合导航解算的实时性。

实施例

本实施例基于DSP(Digital Signal Processor)嵌入式平台实现并验证本发明方法。

本实施例采用基于TMS320C6747开发的GNSS/INS实时紧组合系统，IMU采样率为200Hz，主要性能参数见表1，GNSS采样率为1Hz，导航结果输出率为200Hz。

表1 IMU主要性能参数

GNSS/INS实时紧组合导航系统结构见图1，主要包括有2个处理器MCU(Microcontroller Unit)和DSP(Digital Signal Processor)，MCU负责从陀螺仪及加速度计采集原始数据并利用GNSS板卡提供的1PPS(Pulse Per Sond)把IMU数据标记上GPS时间标志，通过串口向外部和DSP发送带有时间标志的IMU数据，DSP负责实时组合导航的计算。为了实现多任务模式的解算，在DSP中进行了中断优先级和中断嵌套设计，DSP分为4kHz的中断、200Hz中断以及主函数三部分，4kHz中断优先级高于200Hz中断，负责把GNSS、IMU原始数据采集到原始数据环形缓存中以及把导航结果环形缓存中的数据通过串口发送到外部；200Hz的中断从IMU原始数据环形缓存中获取IMU观测数据进行“机械编排、Kalman预测”以及从主函数获取组合更新量进行反馈，再把导航结果存入导航结果环形缓存中；主函数负责从GNSS原始数据环形缓存中获取GNSS观测数据进行“Kalman观测更新”，并将组合更新量传入200Hz中断。

对时间延迟的具体处理时序见图2，以GNSS采样时刻t0的处理为例，具体步骤如下：

步骤1：200Hz中断进行“机械编排、Kalman预测”的同时，利用IMU数据的时间判断当前时刻是否为GNSS采样时刻，若是GNSS采样时刻，保存当前时刻的预测误差协方差矩阵以及机械编排解算的导航状态，并且开始累计计算状态转移矩阵以及状态噪声矩阵。

步骤2：由于GNSS观测数据会滞后到来，主函数在该GNSS采样时刻的数据接收完成之后，利用之前保存的预测误差协方差矩阵以及导航状态，再加上接收到的GNSS观测信息形成Kalman观测更新方程进行“Kalman观测更新”，得到GNSS采样时刻的组合更新量。

步骤3：200Hz中断判断该GNSS采样时刻的组合更新是否完成，如果完成，则利用累计到此刻的状态转移矩阵以及状态噪声矩阵，使用状态转移的方法将主函数传递过来的该GNSS采样时刻的组合更新量变换到当前时刻，使用转移后的组合更新量进行反馈，以修正当前时刻的惯导误差，输出最优的组合结果。最后清除累计的状态转移矩阵以及状态噪声矩阵，为下一轮计算做准备。

为验证GNSS/INS实时紧组合导航系统的性能，使用DSP片内计时器统计200Hz中断内从IMU原始数据获取到输出导航结果以及主函数中组合更新的运行时间，并在导航结果中记录输出，作为评价运行耗时的依据。

测试验证

图3为2015年9年29武汉市文化大道附近的实际车载测试轨迹，测试时长40多分钟，平均速率为20km/h，最大速率达38km/h，测试环境为开阔天空。本次测试，使用Trimble Net R9作为静止参考站，基线长度约16km，在测试车中搭载GNSS/INS实时紧组合导航系统，同时采集GNSS原始观测数据、IMU原始观测数据以及实时组合导航解算结果。采用武汉迈普时空导航公司开发的GINS v1.5软件提供的PPK(PostProcessed Kinematic)/INS松组合平滑结果作为位置、速度和姿态的参考真值(基线小于20km的情况下，位置精度为0.05m，速度精度为0.01m/s，航向精度为0.02～0.05deg，横滚俯仰精度为0.01～0.02deg)。

1)实时性验证

根据实时记录的计算耗时，绘制GNSS数据延迟、200Hz中断计算耗时以及组合更新计算耗时的统计直方图，见图4～6。从图4可以看出，56ms、59ms以及48ms左右获取的GNSS数据有明显的延迟，时间延迟的差异可能是观测卫星数目不同以及导航星历消息数据更新引起的传输数据大小不同导致的。从图5可以看出，200Hz中断函数的执行时间95％左右的都大约是0.75ms，2％左右的大约是1ms，少部分大约在2.8ms，没有大于3ms的部分，可以认为中断函数可以在200Hz中断间隔中执行完毕。从图6可以看出，组合更新执行时间大部分在45ms～70ms左右，没有大于90ms的部分，结合图4，可以看出GNSS延迟更新时间大约为150ms，完全可以在一个GNSS采样间隔内完成组合更新计算。

图7为本发明导航解算延迟示意图，从图7看出，本发明导航解算延迟都在5ms之内，在IMU采样间隔内即可完成导航解算，延迟时间大致可以分为2部分，延迟较小的部分完成的是“机械编排、Kalman预测”，延迟较大的部分要多处理组合更新量的状态转移以及反馈。由此看来，本发明方法可有效减少时间延迟，确保系统的实时有效性能。

2)实测精度验证

将测试采集的实时导航结果与参考真值做比较，导航误差见图8和表2，从图8和表2可以看出，北向、东向、垂向的位置误差Std分别为0.408m、0.363m、0.366m，北向、东向、垂向的速度误差Std分别为0.034m/s、0.034m/s、0.014m/s，横滚、俯仰、航向的姿态误差Std分别为0.017deg、0.021deg、0.184deg，导航误差均在伪距紧组合理论精度范围内，可以认为运用本发明方法实现的GNSS/INS实时紧组合系统计算精度满足设计要求。

表2 实时导航误差统计

Claims (1)

1.一种改进GNSS/INS实时紧组合导航实时性能的方法，其特征是：

将GNSS/INS紧组合分解成机械编排、Kalman预测任务和Kalman观测更新任务，下述步骤实施过程中保持机械编排、Kalman预测任务优先级高于Kalman观测更新任务：

(1)将GNSS采样时刻记为时刻k，保存时刻k的预测误差协方差矩阵和机械编排解算的导航状态；

(2)在时刻k时GNSS观测数据的接收时刻，基于时刻k的GNSS观测数据、和根据时刻k的机械编排解算的导航状态形成的状态向量z_k，采用Kalman滤波法进行组合更新解算，得时刻k的状态参数估计量以及状态参数协方差估计量

(3)将组合更新解算完成时刻记为时刻j，基于公式估计时刻j的状态参数估计量以及状态参数协方差估计量Φ_j/k表示时刻k到时刻j的累积状态转移矩阵；M_j,k+1为时刻k+1到时刻j的累计状态噪声矩阵，

(4)采用以及修正时刻j的惯导误差。