CN105806340B - 一种基于窗口平滑的自适应零速更新算法 - Google Patents

Abstract

本发明公开了一种基于窗口平滑的自适应零速更新算法，当GNSS数据没有故障，能得到连续保持高精度的位置信息、速度信息、姿态角信息；当GNSS数据故障后，零速修正的判定阈值是载体运动时一种隐藏的观测值，并判定：加速度计和陀螺仪的数据输出保持平稳，惯导导航输出的总体速度精度小于平面速度；采用一定时间长度为窗口宽度，当窗口内的所有数据满足以上两个条件时，则把窗口内的最后一个数据看成是ZUPT点；当进入零速时，闭环修正各状态量的误差。本发明能准确的判定载体的运动状态是否满足零速更新点的要求，采用闭环校正的方式修正各状态量，从而进而减小车载状态量的误差，提高定位定向精度。

Description

一种基于窗口平滑的自适应零速更新算法

技术领域

本发明涉及车载高精度组合定位定向技术，具体是一种基于窗口平滑的自适应零速更新算法。

背景技术

惯性测量单元IMU(Inertial measurement unit)是一能种测量载体三轴方向的角速度和加速度的量测装置。陀螺仪能测出相应的旋转角增量(或者角速度)，包含了载体自身的运动造成姿态角变化所带来的角速度以及地球自转角速度；加速度计的输出量是采样间隔时间上相应于比力的速度，包含由SINS(捷联惯导系统)的运动引起的加速度变化所带来的加速度以及地球重力场在内的三个分量。

当把惯性导航系统应用到车载导航定位时，载体的运动过程往往呈现出动态变化剧烈、运行环境复杂、时间长、各状态量之间非线性关系等特点，这些使得GNSS(GlobalNavigation Satellite System，全球卫星导航系统)卫星定位系统发生故障的可能性大大增加。如何仍能有效的提高车载导航系统的动态定位精度，改善由非线性的误差所引起的车载导航系统误差积累造成的影响。所以就必需要充分利用载体的运动状态特征，通过零速更新的方法能有效确定载体的动态特性。这其中就涉及到两大关键因素，一是导航解算过程中零速更新点的确定，通常采用判定加速度计的输出项，或者是载体的整体速度大小，设定相应的阈值，判定是否符合要求，但常常会出现ZUPT点判定的准确率不高，就会出现本身是静止状态却被判定为无效或者是本身不是静止状态却被判定为有效的ZUPT点，这样就会起不到改进甚至增大系统状态量的误差。二就是零速修正的方法，通常是常规的扩展卡尔曼滤波。

如辽宁工程技术大学的孙伟提出的基于零速修正的闭环卡尔曼滤波惯性定位方法(可参见中国专利CN201410372716)，该方案中利用零速更新来对惯性独立导航进行校正。但是仍存在以下不足：1)文中利用水平加速度计所敏感到的重力分量来确定载体水平姿态角，这里只能代表水平加速计水平姿态角，和惯导器件之间必然存在偏心角的问题，且该方法只能出俯仰角和横滚角，得不到航向角；2)零速判定条件单一，利用了加速度计总幅值和预设的阈值比较，这种单一的条件对于阈值的要求比较高，因而对零速的判定条件不够准确；3)零速更新卡尔曼滤波器设计思路过于简单，通过零速点来判定是否进行量测更新，未能就零速状态下系统所处本质的导航参数状态进行分析，滤波器过于简单。

发明内容

本发明的目的在于提供一种基于窗口平滑的自适应零速更新算法，该方法具体指如何判定载体的运动状态是否符合ZUPT点的要求，以及再ZUPT点后具体的修正方法，进而减小车载状态量的误差，提高定位定向精度。

为实现上述目的，本发明提供如下技术方案：

一种基于窗口平滑的自适应零速更新算法，包括以下步骤：

(1)当GNSS数据没有故障，能得到连续保持高精度的位置信息、速度信息、姿态角信息；

(2)当GNSS数据故障后，零速修正的判定阈值是载体运动时一种隐藏的观测值，需要对载体是否进入零速进行判定，对于载体进入零速判定的条件具体如下：条件一，当处于静止状态时，陀螺仪的输出量仅含有地球的自转角速度，加速度计的输出量仅包含地球的重力，这两个输出量均属于常量，即加速度计和陀螺仪的数据输出都保持平稳；条件二：受惯性原理决定，INS(Inertial Navigation System，即惯性导航系统)独立导航时其高程方向的速度精度比平面方向的速度精度低，即惯导导航输出的总体速度精度要小于平面速度；另一方面车辆的运动主要在平面方面，故仅比较载体的平面速度和阈值的关系；条件三：采用一定时间长度为窗口宽度，一定时间长度是不少于1s的时间长度，不是单个历元的原始数据，且利用的是航向角速度和天向加速度，当窗口内的所有数据满足以上两个条件时，则把窗口内的最后一个数据看成是ZUPT点；

(3)当判定载体进入零速时，其速度的真值为0，然后设定该GNSS数据的真值为V_GNSS＝0，以INS算出的速度和GNSS数据的真值做差即v_INS-v_GNSS做为零速更新的量测观测值，代入扩展滤波器，闭环修正各状态量的误差，但是位置状态量不做校准，保持量测更新前的位置。

作为本发明进一步的方案：ZUPT点的校正频率限制，对于零速量测更新的频率会发生闭环滤波的时间更新后进行。

作为本发明进一步的方案：闭环修正时使用的是卡尔曼滤波，对于已经判定出载体处于零速状态位置，保持载体的位置量不变，即位置量不进行校准，零速点上的位置保持一致。

作为本发明进一步的方案：所采用的零速修正的判定阈值不是一个人为设定的经验值，而是判断数据的整体特征是否符合随机游走特性。

与现有技术相比，本发明的有益效果是：

本发明能准确的判定载体的运动状态是否满足零速更新点的要求，采用闭环校正的方式修正各状态量，修正载体状态量的误差，提高定位定向精度。

附图说明

图1是速率型加速度计天向输出原始数据；

图2是基于窗口平滑的自适应零速更新算法流程图；

图3是零速更新纬度和经度平面图；

图4是零速更新对于大地高的效果图；

图5是零速更新对于天向速度效果图；

图6是零速更新对于航向角效果图。

具体实施方式

下面将结合本发明实施例，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

实施例1

本发明实施例中，一种基于窗口平滑的自适应零速更新算法，包括以下步骤：

(1)当GNSS数据没有故障，则用载波相位差分GNSS数据，通过DUFCOM模糊度搜索方法，得到载体的厘米级的定位精度，即其位置；惯导导航在地球固定坐标系中，由所测角速度和已知地球自转角速度进行四元数更新，得到由载体坐标系转到地球固定坐标系的旋转矩阵，再结合所测载体的加速度进行一次积分得到速度，已知的地球重力值进行二次积分得到位置；针对组合滤波中的状态量为非线性的特征，把两个位置量做差组成扩展卡尔曼滤波的量测观测值，提高载体的位置、速度、姿态观测量的精度，并且反馈校正各状态量的值。这里因在GNSS数据有效的情况下，所能提供的位置和速度观测量的精度一般很高，所以不再需要进行ZUPT修正(零速修正)。

(2)当GNSS数据故障后，无法得到无时间积累误差的位置和速度信息，只能通过惯导导航；这时惯导导航的精度会随时间的累积而带来积分误差，当进入零速时，使用零速修正各状态量的误差；

零速判定的条件具体如下：条件一，载体在进入零速之前会经过加速-匀速-减速的过程，当处于静止状态时，陀螺仪输出的数据不再包含由运动造成姿态角的变化带来的角速度，而仅仅含有地球的自转角速度，加速度计的输出量同样也不再包含由运动所带来的加速度，而是地球的重力，这两个量均属于常量，因而处于ZUPT期间的惯导的输出量基本保持不变，具有大致相同的运动特性。因为在静止状态时陀螺仪的输出量为地球自转角速度和随机噪声的叠加和，加速度计的输出量是常值的地球重力叠加入随机噪声后的结果。这是本发明中零速判定的条件一的依据，即加速度计和陀螺仪的数据输出都保持平稳。

条件二：受惯性原理决定，INS独立导航时其高程方向的速度精度一般比平面方向的精度低，即惯导输出的总体速度精度要小于平面速度；另一方面是车辆的运动主要在平面方面，故本发明中仅比较载体的平面速度和阈值的关系，这也更加符合客观实际；

条件三：采用一定时间长度为窗口宽度，当窗口内的所有数据满足以上两个条件时，则把窗口内的最后一个数据看成是ZUPT点；

(3)当判定载体进入零速时，速度的真值为0，然后设定V_GNSS＝0，以v_INS-v_GNSS做为零速更新的量测观测值，以闭环滤波的方式补偿各状态量的误差；

(4)对于零速量测更新的频率仅会发生系统时间更新后进行。

本发明的原理：陀螺仪是在惯导元件中承担着测量载体的角运动参数。其误差模型的一般形式为：

式中，d₀为陀螺的零偏，K_G是刻度因子误差，D_r是温度敏感系数，Δτ是温度变化量，D_h是磁场敏感系数，h是磁场矢量，β为安装误差角向量，ε_w是随机游走项。

加速度计在惯导元件中承担着测量载体的线运动参数。其误差模型常表示成：

δf^b＝b₀+F^bK_a+sgn(F^b)F¹ε₁+F²ε₂+F³ε₃+F_aa+F_cC+W_fW+ε_f (2)

其中，b₀是加速度计的零偏，K_a是刻度因子误差，ε₁是非对称刻度因子误差，ε₂ε₃分别是二次、三次非线性刻度因子误差，a为留个安装误差角，W是角速度敏感系数，ε_f是白噪声，sgn()是符号函数。

在惯导出厂前经仪器检校后，可以得到陀螺的常值零偏项、加速度计的常值零偏项，并且补偿改正到相应的观测量中。其余的元件误差则可认为是随机过程，用且用随机噪声来表示。从图1中可以看出惯导在静止和运动阶段的输出量存在明显的差异，且在车载条件中零速，其持续的时间一般不少于1s(以惯导的采样率时200HZ为例)，因而本发明采用判断窗口内数据是否满足随机过程的形式来表示载体的运动情况，避免了仅利用单个数据的随机性而使结果不准确。此外，车辆的运动中在横滚和俯仰上不会出现剧烈的变化，变化最大的一般是航向角速度，同样侧向加速度和天向加速度也不会出现剧烈的变化，变化最大的是一般是前进方向加速度，所以本发明的条件一中使用的是航向角速度、前进方向加速度这两个轴向的原始数据。

当准确判定载体是否为ZUPT点时，载体的速度真值是0，所以使用的量测方程是：

V_INS-0＝[0 I 0 0 0 0 0][δr δv ε d b k_d k_b]^T (3)

式中，δr是位置误差，δv是速度误差，ε是姿态角误差，d是陀螺零偏误差，b是加表零偏误差，k_d是陀螺的刻度因子误差，k_b是加表的刻度因子误差。和常规滤波修正不同的是，本发明中保持零速期间的位置量不变，即把位置量补偿回零速的第一个历元处。

基于窗口平滑的自适应零速更新算法，关键的两点：一是能准确的判定载体的运动状态是否满足零速更新点的要求，二是采用闭环校正的方式修正各状态量。

步骤1，若GNSS数据没有故障，则可用载波相位差分GNSS数据，利用DUFCOM模糊度搜索方式确整周模糊度，得到厘米级的位置信息，同时惯导在地固下利用角速度得到载体坐标系转到导航坐标系的旋转矩阵，对加速度一次积分速度，二次积分得到位置。把位置和速度差作为量测观测值，进行滤波，其形式为：

步骤2，若GNSS数据发生了故障，且不符合ZUPT，则把INS独立推算的结果输出。

步骤3，若GNSS数据发生了故障，按历元顺序使用本发明内容中零速状态的判定条件一和判定条件二。对于零速判定的条件二来说，受惯性原理决定，INS独立导航时其高程方向的速度精度一般比平面方向的精度低，即惯导输出的总体速度精度要小于平面速度。另一方面是车辆的运动主要在平面方面，故本发明中仅比较载体的平面速度和阈值的关系，这也更加符合客观实际。

本发明的零速判定条件三，采用一定时间长度为窗口宽度，避免仅使用单历元数据所造成的偶然性影响，当窗口内的所有数据满足以上两点时，则可把窗口内的最后一个数据看成是ZUPT点。

当判定载体进入零速时，速度的真值为0，故可设定V_GNSS＝0，以v_INS-v_GNSS做为零速更新的量测观测值，以闭环滤波的方式补偿各状态量。因已经判定出载体处于零速状态，故本发明中保持载体的位置量不变，即位置量不进行校准，零速点上的位置保持一致。

且符合上述的零速条件，则使用扩展卡尔曼滤波来校正各状态量中的误差。

为避免车辆长时间处于静止状态所造成的零速更新频繁的问题，造成系统不稳定，本发明对于零速量测更新的频率做了限制，即仅会发生系统时间更新后进行。

为考察本发明对于GNSS数据失锁时系统输出量精度提升的效果，以具体数据说明。以2014年12月19日在常州市某地的车载POS系统采集的数据进行分析，采用战术级IMU，实验全程中GNSS数据观测条件良好(目的是为了模拟仿真出GNSS数据失锁时输出量的真值，便于进行比对)，速度在15m/s，车辆首先静止约10分钟，完成IMU热启动后，按照预先规划好的线路行驶。

为了定量的对比本发明的优势，把人为的删除一段直线行驶路段的GNSS数据，时间长度约3分钟，期间车辆每隔25m左右会静止一段时间，使用本发明提出的ZUPT方法和GNSS原始数据组合结果做对比，不使用ZUPT方法和GNSS原始数据做组合结果做对比，对差值统计结果列在表1中。图3至图6定性的画出了三种结果之间的差异。图3给出本发明对于平面坐标精度提升的效果，使用本发明后可以明显看出解算出平面坐标较不使用ZUPT和GNSS数据完整时的重合度高很多，大大提高了卫星失锁时的位置精度；图4给出本发明对于椭球高精度提高的效果，3分钟时间内高程方向最大漂移了约0.75m，使用本发明后降至0.12m；图5给出本发明对于天向速度速度精度提升的效果，3分钟内天向速度最大漂移了约0.027m/s，使用本发明后降至0.006m/s。表1中所列的其余统计项，因差异值较小，故没有用画出。

表1GNSS数据失锁且未ZUPT、ZUPT和GNSS数据完整的差值统计量

对于本领域技术人员而言，显然本发明不限于上述示范性实施例的细节，而且在不背离本发明的精神或基本特征的情况下，能够以其他的具体形式实现本发明。因此，无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本发明的范围由所附权利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有变化囊括在本发明内。

此外，应当理解，虽然本说明书按照实施方式加以描述，但并非每个实施方式仅包含一个独立的技术方案，说明书的这种叙述方式仅仅是为清楚起见，本领域技术人员应当将说明书作为一个整体，各实施例中的技术方案也可以经适当组合，形成本领域技术人员可以理解的其他实施方式。

Claims (4)

1.一种基于窗口平滑的自适应零速更新算法，其特征在于，包括以下步骤：

(1)当GNSS数据没有故障，能得到连续保持高精度的位置信息、速度信息、姿态角信息；

(2)当GNSS数据故障后，零速修正的判定阈值是载体运动时一种隐藏的观测值，需要对载体是否进入零速进行判定，对于载体进入零速判定的条件具体如下：条件一，当处于静止状态时，陀螺仪的输出量仅含有地球的自转角速度，加速度计的输出量仅包含地球的重力，这两个输出量均属于常量，即加速度计和陀螺仪的数据输出都保持平稳；条件二：受惯性原理决定，INS独立导航时其高程方向的速度精度比平面方向的速度精度低，即惯导导航输出的总体速度精度要小于平面速度；另一方面车辆的运动主要在平面方面，故仅比较载体的平面速度和阈值的关系；条件三：采用一定时间长度为窗口宽度，一定时间长度是不少于1s的时间长度，不是单个历元的原始数据，且利用的是航向角速度和天向加速度，当窗口内的所有数据满足以上两个条件时，则把窗口内的最后一个数据看成是ZUPT点；

(3)当判定载体进入零速时，其速度的真值为0，然后设定该GNSS数据的真值为V_GNSS＝0，以INS算出的速度和GNSS数据的真值做差即v_INS-v_GNSS做为零速更新的量测观测值，代入扩展滤波器，闭环修正各状态量的误差，但是位置状态量不做校准，保持量测更新前的位置。

2.根据权利要求1所述的基于窗口平滑的自适应零速更新算法，其特征在于，ZUPT点的校正频率限制，对于零速量测更新的频率会发生闭环滤波的时间更新后进行。

3.根据权利要求1所述的基于窗口平滑的自适应零速更新算法，其特征在于，闭环修正时使用的是卡尔曼滤波，对于已经判定出载体处于零速状态位置，保持载体的位置量不变，即位置量不进行校准，零速点上的位置保持一致。

4.根据权利要求1所述的基于窗口平滑的自适应零速更新算法，其特征在于，所采用的零速修正的判定阈值不是一个人为设定的经验值，而是判断数据的整体特征是否符合随机游走特性。