一种改进gnss/ins实时紧组合导航实时性能的方法
【专利摘要】本发明公开了一种改进GNSS/INS实时紧组合导航实时性能的方法，包括(1)保存GNSS采样时刻k的预测误差协方差矩阵和机械编排解算的导航状态；(2)在时刻k时GNSS观测数据的接收时刻，采用Kalma滤波法进行组合更新解算，得时刻k的状态参数估计量以及状态参数协方差估计量(3)根据和估计组合更新解算完成时刻j的状态参数估计量以及状态参数协方差估计量(4)采用以及修正时刻j的惯导误差。本发明可降低GNSS数据延迟和组合解算耗时对实时导航输出的影响，可有效改善实时组合导航的实时性能，对组合导航算法在运算能力较低的处理器上的实现以及对实时性要求严格的应用场合有格外重要的意义。
【专利说明】
-种改进GNSS/1 NS实时紧组合导航实时性能的方法
技术领域
[0001] 本发明属于实时组合导航系统技术领域，尤其设及一种改进GNSS/INS实时紧组合 导航实时性能的方法。
【背景技术】
[0002] 多种手段组合导航是当前导航技术的发展趋势，基于全球卫星导航系统(Global 化vigation Satellite System,GNSS)和惯性导航系统（Ine;rtial 化vigation System, INS)的组合是目前最具有应用价值的组合模式之一，特别是基于GNSS原始观测量和INS数 据的紧组合。该组合模式下，即使在可见卫星不足W单独进行GNSS解算时，仍可有效利用有 限的GNSS观测数据和INS数据通过Kalman滤波进行紧组合解算，得到可靠的导航信息。因 此，GNSS/INS紧组合在工程中得到了越来越广泛的应用。
[0003] 然而，GNSS/INS紧组合实时应用中的GNSS观测数据时间延迟和组合解算时间延迟 (运里统称为时间延迟）问题，会分别导致当前时刻观测数据无法实时得到W及观测数据无 法及时处理的问题，直接影响GNSS/INS紧组合系统的实时性能。目前，时间延迟的解决策略 主要采用增广状态方法和基于新息重组理论建立最优滤波器。其中，增广状态方法会增加 状态维度，使得计算量增大;而新息重组的方法，需要建立多个观测方程，如果有多步延迟， 则不利于对Kalman滤波器的灵活拓展与应用。而且运些方法只能解决观测数据传输延迟， 对于更新计算延迟则无能为力。GNSS/INS实时紧组合法主要包括"机械编排、Kalman预测" W及"Kalman观测更新"两部分，前者一般可W在IMUQnedial Measurement化it)采样间 隔内完成计算;后者则比较耗时，普通嵌入式处理器中无法在足够短的时间内（例如下一个 惯导数据到来前)及时完成，再加上GNSS观测数据大都是从商业接收机板卡中获取，观测数 据传输会有一定的延迟，运就导致Kalman滤波的观测更新计算不能及时完成。运种情况下， 如果使用标准Kalman滤波算法，就需要将后续IM识见测数据缓存起来不处理，等待观测更新 完成才能进行下一步计算，运就会造成组合导航结果输出的阻塞和延迟，影响系统的实时 性能。开发GNSS/INS实时紧组合系统时，就需要一种方法解决在存在时间延迟的情况下，及 时完成导航解算。
【发明内容】
[0004] 针对现有GNSS/INS实时紧组合导航中GNSS观测数据传输延迟和更新计算耗时较 长所带来的时间延迟问题，本发明提出了一种改进GNSS/INS实时紧组合导航实时性能的方 法。
[0005] 本发明采用如下技术方案：
[0006] -种改进GNSS^NS实时紧组合导航实时性能的方法，包括：
[0007] 将GNSS/INS紧组合分解成机械编排、Kalman预测任务和Kalman观测更新任务，下 述步骤实施过程中保持机械编排、Kalman预测任务优先级高于Kalman观测更新任务：
[000引（1)将GNSS采样时刻记为时刻k，保存时刻k的预测误差协方差矩阵巧,W和机械编 排解算的导航状态；
[0009] (2)在时亥化时GNSS观测数据的接收时刻，基于时刻k的GNSS观测数据、私-1和根据 时刻k的机械编排解算的导航状态形成的状态向量Zk,采用Kalman滤波法进行组合更新解 算，得时刻k的状态参数估计量驾W及状态参数协方差估计量P/;
[0010] (3)将组合更新解算完成时刻记为时刻j，基于公J
估计 时刻j的状态参数估计量及状态参数协方差估计量Pm; 表示时刻k到时刻j的累 积状态转移矩阵;为时刻k+l到时刻j的累计状态噪声矩阵
[00川 （4)采用i;,山及Pm修正时刻j的惯导误差。
[0012] 和现有技术相比，本发明具有如下优点和有益效果：
[0013] 1、本发明针对GNSS/INS组合导航系统，基于Kalman滤波法，将滞后的状态参数估 计量W及状态参数协方差估计量转移到当前时刻，从而降低GNSS数据延迟和组合解算耗时 对实时导航输出的影响，可有效改善实时组合导航的实时性能。
[0014] 2、本发明对组合导航算法在运算能力较低的处理器上的实现W及对实时性要求 严格的应用场合有格外重要的意义。
[0015] 3、在DSP硬件平台上实现和验证本发明，车载实测结果表明：相对于标准组合导航 解算法，本发明能够在保障组合导航系统精度的前提下，最大限度地减小输出导航结果的 时间延迟，保障组合导航结果的实时性。
【附图说明】
[0016] 图1为GNSS^NS实时紧组合导航系统结构图；
[0017] 图2为时间延迟的处理时序示意图；
[001引图3为实施例中车载巧聯轨迹；
[0019] 图4为GNSS数据延迟的统计直方图，其中，横坐标表示GNSS数据延迟，单位为ms;
[0020] 图5为200Hz中断计算耗时的统计直方图，其中，横坐标表示200Hz中断计算耗时， 单位为ms;
[0021] 图6为组合更新计算耗时的统计直方图，其中，横坐标表示组合更新计算耗时，单 位为ms;
[0022] 图7为本发明导航解算延迟示意图；
[0023] 图8为实施例的导航误差曲线。
【具体实施方式】
[0024] GNSS/INS紧组合可分解成具有优先级别的两个任务"机械编排、Kalman预测"和 "Kalman观测更新"独立进行，其中，任务"机械编排、Kalman预测"的优先级高于任务 乂a Iman观测更亲开'。
[0025] 本发明具体步骤如下：
[00%] (I)将GNSS采样时刻记为时刻k，保存时刻k的预测误差协方差矩阵巧;W和机械编 排解算的导航状态。
[0027] (2)见图2,将时刻k时GNSS观测数据的实际接收时刻记为时刻ts，在时刻ts采用 Kalman滤波法进行组合更新解算(见式（12)~（13))，获得时刻k的状态参数估计量转W及 状态参数协方差估计量C。
[0028] 组合更新解算完成后，还需要将滞后计算出来的反映时刻k的状态参数估计量W 及状态参数协方差估计量转移到组合更新完成时刻j，W修正时刻j的惯导误差，即反馈修 正。本发明利用状态转移模型达到该目的，所述的状态转移即根据时刻k的状态参数估计时 亥Ijj的状态参数。
[0029] 根据k个时刻的观测向量zi、Z2……Zk对时刻j的状态参数Xj作线性最小方差估计
金从：
[0030] (1)
[0031] 式（1)中，zi、Z2……Zk分别表示时刻1、2、……k的观测向量，E(.)表示数学期望。
[0032] GNSS/INS紧组合导航系统中，状态参数X由INS误差状态和GNSS接收机时钟误差共 同组成，可W表示为：
[0033]
(2)
[0034] 式（2)中：Srn、Svn、4分别表示位置误差、速度误差、姿态角误差;bg、sg分别表示巧 螺的零偏和比例因子;ba、Sa分别表示加速度计的零偏和比例因子；表示接收机钟差误 差;SfR表示接收机钟漂误差。
[003引虹n、Svn、(6、bg、Sg、ba、Sa为INS误差状态，孙,，、SfR为GNSS接收机时钟误差。
[0036] 考虑如下线性离散系统：
[0037] Xk=巫 k/k-ixk-1+Gk/k-iwk-i (3)
[003引 Zk =化 xk+Uk (4)
[0039] 式(3)~(4)中：
[0040] a、祉-1分别表示时刻k、k-l的状态参数；
[0041] Ok/k-i为时刻k-巧Ij时刻k的状态转移矩阵，其值由状态参数的动力学模型决定；
[0042] Gk/k-i为时刻k-1到时刻k的系统噪声驱动矩阵；
[0043] Wk-I为时刻k-1的系统噪声；
[0044] Zk为时刻k的观测向量，其由INS推算的GNSS伪距、多普勒值与GNSS接收机观测的 伪距、多普勒观测值作差得到；
[0045] 化为时刻k的设计矩阵；
[0046] Uk为时刻k的观测噪声矩阵。
[0047] 根据式（3)，并且考虑到巫k+1/k-i=巫k+1/k巫k/k-i，从时刻k的状态参数Xk转移时刻占' 的状态参数Xi,如下：
[0048]
(5)
[0049] 上述Ok+1/k-i表不时刻k-1到时刻k+1的状态转移矩阵，Ok+1/k、巫j/k分别表不时刻k 到时刻k+1、时刻j的状态转移矩阵，康示时刻巧IJ时刻j的状态转移矩阵;Gi/1-康示时刻 i-1到时刻i的系统噪声驱动矩阵;Wi-I表示时刻i-1的系统噪声。
[(K)加]式(5)代入式（1)得到：
[0化1](6) =ZC千 1
[00对由式（3)可知，Wk-I只影响到祉，所Wwk-i与zi、Z2......Zk不相关，且E(Wk-I) =0，E( ?) 表示数学期望，因此有：
[0053]由式（3)可知，Wk-I 只影响到Xk,所 W Wi-I 与 Zi、Z2......Zk不相关，且E(Wi-I) = 0, i>k+ 1且i《j，因此有：
[0054]
(7)
[0化5]式(7)中，缉表示Xk的更新估计量。
[0化6] 的误差为：
(8)
[0化7]
[005引对于预测误差协方差矩阵Pm =巧i,,,兩J]，T表示矩阵的转置：
[0060] 式中
衰示Xk的误差;IVj为X在时刻j的预测误差协方差矩阵，K为X在时刻k的协方差
矩阵;Qi-I为Wi-I的协方差矩阵。GNSS/INS紧组合中Gi/i-i为正定矩阵，因此， 「 n (9)
[0化9]
[0061] 4
表示时刻k+l到时刻j的累计状态噪声矩阵。具体 实施时采用公式(10)累积状态噪声矩阵：
[0062]
(10)
[0063] 式（10)中，Qm、Qk分别表不wm、wk的协方差矩阵，机i、wk分别表不时刻m、k时的系统噪 声，m为大于k的时刻；表示时刻m到时刻m+1的转移矩阵;Mm,k+1表示时刻k+巧Ij时亥Ijm的 累计状态噪声矩阵，Mk+i,k+i表示时刻k+1的累计状态噪声矩阵。
[0064]综上，可获得状态转移公式，如下：
[00化]
(11)
[0066] 由式（11)可W看出，状态转移可W由累积状态转移矩阵及累计状态噪声矩阵 Mj,W得到，为时刻k到时刻j时间段内相邻时刻的状态转移矩阵的连乘，M川+1由式（10) 获得。把GNSS采样时刻看作时刻k，组合更新解算完成时刻看作时刻j，利用式（11)把时刻k 的状态参数估计量起W及状态参数协方差估计量Pr转移到时刻j，进而对时刻j的惯导误差 进行反馈修正。
[0067] 本发明组合更新解算采用标准Kalman滤波法，如下：
[006引 Cl 2)
[0069] (13)
[0070] 式中乂 W表示根据时刻l、2-'k-l的观测向量估计的时刻k的状态参数;Kk为卡尔 曼滤波增益矩阵;巧为时刻k的状态参数协方差估计量;巧为步骤(1)中保存的时刻k的预 测误差协方差矩阵；I表示单位矩阵;化为观测信息先验方差矩阵，按卫星高度角确定观测 值先验方差。
[0071] 在步骤（1)~(2)处理过程中保持优先进行INS机械编排和Kalman预测。运样，只要 能保证"惯性机械编排、Kalman预测"在IMU采样间隔内完成，并保证"Kalman观测更新"计算 在GNSS采样间隔内完成，就能够保证组合导航解算的实时性。
[0072] 实施例
[0073] 本实施例基于DSP(Digital Signal Processor)嵌入式平台实现并验证本发明方 法。
[0074] 本实施例采用基于TMS320C6747开发的GNSS/INS实时紧组合系统，IMU采样率为 200Hz，主要性能参数见表1 ,GNSS采样率为IHz，导航结果输出率为200Hz。
[00对表1 IMU主要性能参数
[0076]
[0077] GNSS/INS实时紧组合导航系统结构见图1，主要包括有2个处理器MCU (Microcontroller Unit)和DSP(Digi1:al Si即al Processor) ,MCU负责从巧螺仪及加速度 计采集原始数据并利用GNSS板卡提供的lPPS(Pulse Per Sond)把IMU数据标记上GPS时间 标志，通过串口向外部和DSP发送带有时间标志的IMU数据，DSP负责实时组合导航的计算。 为了实现多任务模式的解算，在DSP中进行了中断优先级和中断嵌套设计，DSP分为4k化的 中断、200化中断W及主函数S部分，4曲Z中断优先级高于200化中断，负责把GNSS、IMU原始 数据采集到原始数据环形缓存中W及把导航结果环形缓存中的数据通过串口发送到外部； 200化的中断从IMU原始数据环形缓存中获取IM识见测数据进行"机械编排、Kalman预测"W 及从主函数获取组合更新量进行反馈，再把导航结果存入导航结果环形缓存中；主函数负 责从GNSS原始数据环形缓存中获取GNSS观测数据进行"Kalman观测更新"，并将组合更新量 传入200化中断。
[0078] 对时间延迟的具体处理时序见图2 ,WGNSS采样时刻to的处理为例，具体步骤如 下：
[0079] 步骤l:200Hz中断进行"机械编排、Kalman预测"的同时，利用IMU数据的时间判断 当前时刻是否为GNSS采样时刻，若是GNSS采样时刻，保存当前时刻的预测误差协方差矩阵 W及机械编排解算的导航状态，并且开始累计计算状态转移矩阵W及状态噪声矩阵。
[0080] 步骤2:由于GNSS观测数据会滞后到来，主函数在该GNSS采样时刻的数据接收完成 之后，利用之前保存的预测误差协方差矩阵W及导航状态，再加上接收到的GNSS观测信息 形成Kalman观测更新方程进行乂alman观测更新"，得到GNSS采样时刻的组合更新量。
[0081] 步骤3:200Hz中断判断该GNSS采样时刻的组合更新是否完成，如果完成，则利用累 计到此刻的状态转移矩阵W及状态噪声矩阵，使用状态转移的方法将主函数传递过来的该 GNSS采样时刻的组合更新量变换到当前时刻，使用转移后的组合更新量进行反馈，W修正 当前时刻的惯导误差，输出最优的组合结果。最后清除累计的状态转移矩阵W及状态噪声 矩阵，为下一轮计算做准备。
[0082] 为验证GNSS^NS实时紧组合导航系统的性能，使用DSP片内计时器统计200化中断 内从IMU原始数据获取到输出导航结果W及主函数中组合更新的运行时间，并在导航结果 中记录输出，作为评价运行耗时的依据。
[0083] 测试验证
[0084] 图3为2015年9年29武汉市文化大道附近的实际车载测试轨迹，测试时长40多分 钟，平均速率为20km/h，最大速率达38km/h，测试环境为开阔天空。本次测试，使用化imble Net R9作为静止参考站，基线长度约16虹1，在测试车中搭载GNSS^NS实时紧组合导航系统， 同时采集GNSS原始观测数据、IMU原始观测数据W及实时组合导航解算结果。采用武汉迈普 时空导航公司开发的GINS Vl. 5软件提供的PPK(Post Processed Kinematic VINS松组合 平滑结果作为位置、速度和姿态的参考真值(基线小于20km的情况下，位置精度为0.05m，速 度精度为0.0 lm/s，航向精度为0.02~0.05deg，横滚俯仰精度为0.0 l~0.02deg)。
[0085] 1)实时性验证
[0086] 根据实时记录的计算耗时，绘制GNSS数据延迟、200化中断计算耗时W及组合更新 计算耗时的统计直方图，见图4~6。从图4可W看出，56ms、59msW及48ms左右获取的GNSS数 据有明显的延迟，时间延迟的差异可能是观测卫星数目不同W及导航星历消息数据更新引 起的传输数据大小不同导致的。从图5可W看出，200化中断函数的执行时间95%左右的都 大约是0.75ms，2 %左右的大约是Ims，少部分大约在2.8ms，没有大于3ms的部分，可W认为 中断函数可W在200化中断间隔中执行完毕。从图6可W看出，组合更新执行时间大部分在 45ms~70ms左右，没有大于90ms的部分，结合图4,可W看出GNSS延迟更新时间大约为 150ms，完全可W在一个GNSS采样间隔内完成组合更新计算。
[0087] 图7为本发明导航解算延迟示意图，从图7看出，本发明导航解算延迟都在5ms之 内，在IMU采样间隔内即可完成导航解算，延迟时间大致可W分为2部分，延迟较小的部分完 成的是"机械编排、Kalman预测"，延迟较大的部分要多处理组合更新量的状态转移W及反 馈。由此看来，本发明方法可有效减少时间延迟，确保系统的实时有效性能。
[0088] 2)实测精度验证
[0089] 将测试采集的实时导航结果与参考真值做比较，导航误差见图8和表2,从图8和表 2可W看出，北向、东向、垂向的位置误差Std分别为0.408m、0.363m、0.366m，北向、东向、垂 向的速度误差Std分别为0.034m/s、0.034m/s、0.014m/s，横滚、俯仰、航向的姿态误差Std分 别为0.017deg、0.021deg、0.184deg，导航误差均在伪距紧组合理论精度范围内，可W认为 运用本发明方法实现的GNSS^NS实时紧组合系统计算精度满足设计要求。
[0090] 表2实时导航误差统计
[00911
【主权项】
1. 一种改进GNSS/INS实时紧组合导航实时性能的方法，其特征是： 将GNSS/INS紧组合分解成机械编排、Kalman预测任务和Kalman观测更新任务，下述步 骤实施过程中保持机械编排、Kalman预测任务优先级高于Kalman观测更新任务： (1) 将GNSS采样时刻记为时刻k，保存时刻k的预测误差协方差矩阵1和机械编排解 算的导航状态； (2) 在时刻k时GNSS观测数据的接收时刻，基于时刻k的GNSS观测数据、和根据时刻 k的机械编排解算的导航状态形成的状态向量zk，采用Kalman滤波法进行组合更新解算，得 时刻k的状态参数估计量劣以及状态参数协方差估计量Pf ; (3) 将组合更新解算完成时刻记为时刻j，基于公式古计时刻 j的状态参数估计量％』以及状态参数协方差估计量巧；Φ i/k表示时刻k到时刻j的累积状 态转移矩阵;Mj,k+1为时刻k+Ι到时刻j的累计状态噪声矩阵(4) 采用以及修正时刻j的惯导误差。
【文档编号】G01C21/16GK105954783SQ201610262477
【公开日】2016年9月21日
【申请日】2016年4月26日
【发明人】常乐, 牛小骥, 章红平, 张全
【申请人】武汉大学