基于里程计的克服卫星失锁时的gnss/ins车载组合定位定向算法
【专利摘要】本发明公开了基于里程计的克服卫星失锁时的GNSS/INS车载组合定位定向算法，将GNSS数据与惯性导航系统解算出位置和速度，二者求差后的值当做卡尔曼滤波器的量测观测值，反馈校正各状态量；使用里程计来校正IMU中各状态量，利用INS/ODO组合的方法，以IMU的输出量和里程计在滤波时刻期间的输出值求和作差来校正里程计的刻度因子，里程计坐标系和导航坐标系之间的安装误差角；GNSS卫星失锁时，里程计校正IMU中各误差项，尤其是对于提升IMU独立导航的位置精度有很大的帮助。这非常适合应用于在城市峡谷中，GNSS卫星信号常会受到遮挡导致的信号失锁现象，使得整个系统仍能保持高精度的定位结果。
【专利说明】
基于里程计的克服卫星失锁时的GNSS/INS车载组合定位定向 算法
技术领域
[0001] 本发明设及高精度的车载组合定位定向系统，具体是基于里程计的克服卫星失锁 时的GNSS^NS车载组合定位定向算法。
【背景技术】
[0002] 和GNSS/INS实时组合导航相比，测量领域的GNSS/INS组合定位定姿具有位置姿态 精度要求高、可事后处理等特点。GNSS(Global化vigation Satellite System,全球卫星 导航系统)工作于差分模式，利用载波相位观测量求解整周模糊度并获取厘米级位置信息， IMUQnedial measurement unit,惯性测量单元）提供姿态信息，在GNSS失锁期间提供载 体的连续位置信息。然而在城市峡谷等导航卫星信号接收条件较差的地区，GNSS存在大量 短时甚至长时间的失锁，此时GNSS/INS组合系统的精度也快速下降，不能满足MMS无控制测 量要求，因此有必要引入其它传感器辅助定位。里程计(Odometer, 0D0)是测量两厢正交脉 冲个数的传感器，乘W相应的刻度因子即可得到载体行驶距离的一种传感器，具有自主性 高、测量范围宽的特点，但是单独的里程计不具备导航定位功能，必须要和惯性导航系统同 时使用才能进行导航定位，其误差随行驶距离而线性累积，相对于INS( Inertial 化vigation System,即惯性导航系统）误差随时间呈二次增长关系，在地面车载等低速运 动情况下，里程计的误差要小的多，因此GNSS/0DCVINS的组合有利于提高在城市峡谷中的 定位定姿精度。
[0003] 另一方面，里程计刻度因子受溫度、胎压等影响，其初值并不精确，而P0S与载体之 间也存在一定的安装误差角，因此在MMS测量之前需对P0S系统进行误差标定。本文分析了 里程计刻度因子误差和P0S安装误差对测量结果的影响，利用里程计与INS里程增量之间约 束的关系，在GNSS连续观测和固定模糊度条件下，对误差进行校正；反之在GNSS失锁条件 下，利用校正过的里程计对INS导航误差进行估计。跑车实验验证了误差校正算法的有效 性;同时，补偿了安装误差角和刻度因子后，P0S的定位精度有显著提高。
【发明内容】
[0004] 本发明的目的在于提供基于里程计的克服卫星失锁时的GNSS/INS车载组合定位 定向算法，为了解决车载导航定位中，因卫星失锁导致组合系统转变成惯性导航系统单独 导航时各项导航参数误差随时间积分而发散运一技术难题。
[0005] 为实现上述目的，本发明提供如下技术方案：
[0006] 基于里程计的克服卫星失锁时的GNSS/INS车载组合定位定向算法，包括W下步 骤：
[0007] 步骤1，GNSS数据载波相位差分解算，得到厘米级的定位精度，即提供整个GNSS/ INS车载组合系统的位置基准，惯性导航系统利用GNSS提供的位置观测量和已知的地球自 转参数进行粗对准，完成后进行惯性导航系统的单独导航推算；
[0008] 步骤2，GNSS数据解算出位置和速度，惯性导航系统同时也算出位置和速度，二者 求差后的值当做卡尔曼滤波器的量测观测值，反馈校正各状态量；当GNSS/INS车载组合系 统构成的主滤波器已经收敛时，INS/0D0滤波器开启工作，即利用IMU来校正0D0中的误差， WIMU的输出量和里程计在滤波时刻期间的输出值求和作差来校正里程计的刻度因子、里 程计坐标系和导航坐标系之间的安装误差角；
[0009] 步骤3,若车载或者基站处的接收机观测数据无法进行差分解算，并且持续了不少 于5秒的时间，此时能计算出相应时间内里程计输出的脉冲个数，结合已经被IMU校正过后 的里程计刻度因子误差、安装误差角，转换成距离观测量，对GNSS/INS车载组合系统的状态 量进行校正。
[0010] 作为本发明进一步的方案:里程计安装在从动轮上，避免车辆打滑。
[0011] 作为本发明进一步的方案:里程计误差的校正使用的是惯性导航系统的位置和相 应的协方差矩阵。
[0012] 作为本发明进一步的方案:卡尔曼滤波器采用GNSS/INS与INS/0D0两级卡尔曼滤 波器级联结构;其中INS/0D0卡尔曼滤波器的时间更新和量测更新周期需要从里程计的分 辨率、车轮直径W及车速等多方面考虑，并且时间更新和量测更新周期可W不一致。
[0013] 作为本发明进一步的方案：里程计校正惯性导航系统状态量，仅当GNSS失锁一定 时间后才进行，一定时间是指不少于5秒。
[0014] 本发明的原理
[0015] 在GNSS/INS的组合定位测姿中，滤波的状态方程是非线性的，在经过线性化后各 状态量也就变成各状态的误差量，因而得采用扩展卡尔曼滤波的形式进行组合，其状态、量 测方程如下式：
[0016]
|| I
[0017] Z =册(t)+V (2)
[0018] 式中：X(t)是状态向量，其协方差矩阵为P阵，W(t)是系统噪声，其相应的协方差矩 阵为Q，G(t)是噪声驱动矩阵，F(t)是状态转移矩阵;Z是量测值，V的协方差矩阵是R阵。公式 和公式给出了 GNSS/INS滤波器的状态更新方程和量测更新方程。即是对本发明的步骤2中 GNSS/INS滤波器的具体说明。
[0019]导航参考坐标系选择地球固连坐标系ECEF，记为e系;载体坐标系XYZ轴分别为右、 前、上Ξ个方向，记为V系；IMU惯性传感器坐标系记为b系。里程计由于安装和测量原理的关 系，只能测量载体Y方向的运动;里程计在载体坐标系下的测量值可写为： 瓣]
Is)
[0021]其中η为里程计脉冲个数。里程计在载体系内的里程增量为：
[00。]
《排
[0023]其中k为里程计刻度因子。通过旋转矩阵，可将里程计在载体系内的里程增量换算 至地固系：
[0024]
[0025] 其中爲:为b系至e系旋转矩阵，嫣'为V系至b系旋转矩阵。在短时间内，认为蜀为常
值矩阵，令 馬/.:;:和竣<;分别为滤波周期前后的旋转矩阵。若IMU > 不存在安装误差，则變为单位阵，而更一般的情况下该矩阵为由航向安装误差角ay、俯仰安 装误差角ap和横滚安装误差角ar按顺序旋转构成的非单位阵。记安装误差角为a=[ap ar ay]T，在GNSS连续观测条件下忽略旋转矩阵够的误差，同时考虑式中的刻度因子误差，则实 际计算的里程计在e系中的里程增量可写为：
[0026]
[0027]其中化为里程计刻度因子误差，a X表示安装误差角向量的反对称矩阵。忽略式 (4)中的二阶小量，有
[002引
[0029] 其中公式右边第一项为理想里程增量，第二项和第Ξ项则为由里程计刻度因子误 差和P0S安装误差引起的里程增量误差。显然，由里程计刻度因子误差和安装误差角造成的 位置误差与单位时间内里程增量成正比关系，若未补偿则将分别引起烹ib、泉皆与 X《的里程误差。W1 %的刻度因子误差和1°航向安装误差角为例，若载体行驶1km，贝。 里程计将分别有10m和17.45m的误差，并最终进入GNSS/0DCVINS组合定位结果。
[0030] 另一方面，由GNSS^NS组合导航结果，有在地固系下的Ξ维里程增量：
[0031]
[0032] 在GNSS连续观测和固定整周模糊度的情况下，忽略式的位置误差。显然里程计理 想里程增量等于GNSS^NS理想里程增量，将式代入式，有：
[0033]
[0034] 根据式，WGNSS^NS组合结果作为观测，即可对里程计刻度因子误差和P0S安装误 差角进行估计。此上发明原理解释了
【发明内容】
的步骤2中INS/0D0滤波器，尤其是最后给出 的公式是INS/0D0滤波器的观测方程。
[0035] 在GNSS失锁条件下，可W使用校正过刻度因子的里程计和安装误差角辅助IMU，此 良阳DCVIMU组合导航。将式重写为
[0036]
[0037] 其中ε为旋转矩阵攝失准角，.超.和k均使用校正值。式中右边第一项为理想里程增 量，第二项为由失准角引起的里程误差。将式第一项用IMU里程增量代替，并写为误差的形 式：
[00；3 引
[0039] 其中與痴为^11在e系中的Ξ维位置误差。式即为0D0/IMU组合导航卡尔曼滤波器 量测方程，方程左边为观测量，右边写为矩阵的形式，有：
[0043] 0DCVIMU组合卡尔曼滤波器的状态方程与公式一致。
[0044] 运阐述了本
【发明内容】
的步骤3中提到的0D0/IMU滤波器内容。
[0045] 与现有技术相比，本发明的有益效果是：
[0046] 本发明基于里程计辅助的克服卫星失锁时的GNSS^NS车载组合定位定向算法，针 对里程计刻度因子和P0S安装误差角的校正，在不改变原GNSS^NS滤波器的基础上，设计了 GNSS/INS与INS/0D0两级卡尔曼滤波器级联结构，将INS导航误差与里程计刻度因子误差、 安装误差角依次与两个滤波器的系统状态对应，其包括GNSS结果正常时对于里程计刻度因 子误差校正，里程计坐标系和导航坐标之间安装误差角校正;GNSS卫星数据丢失时，里程计 校正IMU中各误差项，尤其是对于提升IM闲虫立导航的位置精度有很大的帮助。运非常适合 应用于在城市峡谷中，GNSS卫星信号常会受到遮挡导致的信号失锁现象，使得整个系统仍 能保持高精度的定位结果。
【附图说明】
[0047] 图1是GNSS^NS/ODO误差校正滤波器结构示意图；
[004引图2是0D0/INS组合导航滤波器结构示意图；
[0049] 图3是实验载体轨迹图；
[0050] 图4是实验安装误差角示意图；
[0051 ]图5是实验里程计刻度因子示意图；
[0化2]图6是实验0D0/INS组合平面误差图；
[0化3]图7是实验0D0/INS组合高程误差图。
【具体实施方式】
[0054]下面将结合本发明实施例，对本发明实施例中的技术方案进行清楚、完整地描述， 显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的 实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都 属于本发明保护的范围。
[0化5] 实施例1
[0056] 请参阅图1-图2,本发明实施例中，基于里程计的克服卫星失锁时的GNSS^NS车载 组合定位定向算法，其包括GNSS/INS滤波器收敛时对于里程计刻度因子误差校正，里程计 坐标系和导航坐标之间安装误差角校正;GNSS卫星数据丢失时，里程计校正IMU中各误差 项。
[0057] 具体操作步骤如下:GNSS/INS与INS/0D0两级卡尔曼滤波器级联结构
[0058] 利用式对刻度因子和安装误差角校正的前提是IMU被充分校正，此时安装误差角 和刻度因子与INS导航误差无关，因此本文中滤波器采用两级卡尔曼滤波级联设计，如图1 所示，第一级为GNSS/INS卡尔曼滤波器，第二级为INS/0D0卡尔曼滤波器。GNSS/INS卡尔曼 滤波器利用GNSS伪距和载波相位观测量，经双频相关法(DUFC0M)解算整周模糊度，由此获 得厘米级位置信息，与IMU进行松散组合，校正INS导航及传感器误差。在GNSS^NS卡尔曼滤 波器充分收敛后，启动INS/0D0卡尔曼滤波器，WGNSS^NS输出的高精度位置和姿态信息作 为INS/0D0的输入，其中姿态角用作将里程计增量转换至e系内，而前后滤波间隔内的INS里 程增量作为里程计的外部观测，估计IMU安装误差角和里程计刻度因子。
[0059] 由于里程计在b系中的里程增量与横滚安装误差角ar无关，因此滤波器状态量选 取里程计刻度因子误差化、航向安装误差角ay和俯仰安装误差角ap，均模拟为随机游走过 程，因此相应的有滤波器状态方程：
[0060]
[0061]其中Ksk、-^、和我，分别为Ξ个状态量的随机游走功率谱密度系数。式给出了滤波 器观测方程的一般形式，其中方程左边为观测量：
[0066]将式右边写为矩阵的形式，并忽略横滚安装误差角ar，则INS/0D0滤波器观测方程 为：
[0067]
[0068] 两级滤波器结构将INS误差与里程计等相关误差分别计算，有利于在原GNSS/INS 组合算法的基础上进行改进，工程实现相对简单;此外，相对于把INS导航误差和里程计刻 度因子、安装角误差归为一个滤波器状态的滤波器结构，级联滤波器更适合于在里程计更 新频率较GNSS频率高的情况，而通常情况也是如此;第Ξ，级联结构避免了过高的矩阵阶 数，系统更加稳定。对于卡尔曼滤波器的更新频率，由于事后处理不存在计算量上的考量， 因此更新频率可相对实时导航较高。GNSS/INS滤波器的时间更新W0.1 s间隔为宜，量测更 新可根据GNSS数据率选择1-10化。而INS/0D0滤波器的更新时间间隔不宜过长，否则不满足 短时间内旋转矩阵據为常值矩阵的条件，若时间过短，则更新周期内脉冲数目过低，引起较 大的截断误差，因此INS/0D0滤波器的时间更新和量测更新周期需要从里程计的分辨率、车 轮直径W及车速等多方面考虑，在车速过低的情况下，可W停止校正P0S安装误差角和里程 计刻度因子。
[0069] 为考察本发明对于GNSS失锁时系统输出量精度提升的效果，W具体数据说明。W 2014年6月20日在武汉某地的车载P0S系统和里程计组合采集的数据进行分析，实验全程中 GNSS观测条件良好（目的是把GNSS定位正常的结果作为基准，供其他方法的比较），速度在 18m/s，车辆首先静止约10分钟，完成IMU热启动后，按照预先规划好的线路行驶。
[0070] 首先计算出GNSS卫星未失锁时，GNSS/INS/0D0(里程计)组合的结果，作为比对的 基准，选择路线中一段曲线路段，删除掉数据长度为2分钟的GNSS定位结果，现若不使用0D0 数据辅助导航，即该时间段只能由IMU进行独立导航，若按照本发明提出的基于里程计的克 服卫星失锁时的GNSS^NS车载组合定位定向算法，和完整GNSS^NS组合的结果做对比。图3 实验载体轨迹图，图4和图5分别为实验II P0S安装误差角和里程计刻度因子校正过程，其 中俯仰安装误差角约为0.9°，航向安装误差角相对较大，约为1.8°，里程计刻度因子校正结 果为0.00209。使用校正获得的安装误差角和里程计刻度因子，考察在GNSS失锁条件按下 0DCVINS组合导航。人为在实验中删除一段2分钟GNSS数据，此时W里程计里程增量作为INS 的外部观测，对INS误差进行校正。处理按5种方式进行处理，INS表示INS独立导航;0DCVINS 1#表示使用里程计校正INS，里程计刻度因子使用初始值，安装误差角未校正;0D0/INS 2# 表示使用里程计校正INS，里程计刻度因子使用校正值，但安装误差角未校正;0D0/INS 3# 表示使用里程计校正INS，里程计刻度因子和安装误差角均使用校正值;0D0/INS 4#表示在 0D0/INS 3#的基础上，使用事后平滑卡尔曼滤波器做平滑滤波处理。图6和图7分别为实验 GNSS中断条件下的平面和高程误差，由图6和图7可知，在未补偿任何初始误差的条件下， 0D0/INS 1#组合导航的平面误差在开始阶段要大于INS独立导航结果，但INS误差累计速度 明显高于里程计，前者明显为二次关系，后者基本为线性关系，因此在GNSS中断的后半段 0D0/INS定位结果要小于INS独立导航结果;在补偿了里程计刻度因子条件下，0D0/INS 2# 定位误差能够一开始就小于纯INS，说明了里程计刻度因子校正结果的有效性;若里程计刻 度因子和安装误差角均使用校正值，则0D0/INS 3#的定位误差略小于0D0/INS 2#，运是由 于俯仰安装误差角和偏航安装误差角均比较小，对最终定位结果的影响有限；使用了卡尔 曼平滑滤波器后，ODO/INS 4#平面误差又相较ODO/INS 3#有大幅减小，比INS独立导航结果 提高了一个数量级，运是得益于每个历元均使用了前后历元数据作为观测，而实时或者单 向滤波只能使用时间上较前的数据。对于GNSS中断的高程误差，使用校正后的结果略有提 高，但是并不显著，而平滑后则有显著提高。
[0071] 对于本领域技术人员而言，显然本发明不限于上述示范性实施例的细节，而且在 不背离本发明的精神或基本特征的情况下，能够W其他的具体形式实现本发明。因此，无论 从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本发明的范围由所附权 利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有 变化囊括在本发明内。
[0072] 此外，应当理解，虽然本说明书按照实施方式加 W描述，但并非每个实施方式仅包 含一个独立的技术方案，说明书的运种叙述方式仅仅是为清楚起见，本领域技术人员应当 将说明书作为一个整体，各实施例中的技术方案也可W经适当组合，形成本领域技术人员 可W理解的其他实施方式。
【主权项】
1. 基于里程计的克服卫星失锁时的GNSS/INS车载组合定位定向算法，其特征在于，包 括以下步骤： 步骤1，GNSS数据载波相位差分解算，得到厘米级的定位精度，即提供整个GNSS/INS车 载组合系统的位置基准，惯性导航系统利用GNSS提供的位置观测量和已知的地球自转参数 进行粗对准，完成后进行惯性导航系统的单独导航推算； 步骤2，GNSS数据解算出位置和速度，惯性导航系统同时也算出位置和速度，二者求差 后的值当做卡尔曼滤波器的量测观测值，反馈校正各状态量；当GNSS/INS车载组合系统构 成的主滤波器已经收敛时，INS/ODO滤波器开启工作，即利用頂U来校正0D0中的误差，以頂U 的输出量和里程计在滤波时刻期间的输出值求和作差来校正里程计的刻度因子、里程计坐 标系和导航坐标系之间的安装误差角； 步骤3,若车载或者基站处的接收机观测数据无法进行差分解算，并且持续了不少于5 秒的时间，此时能计算出相应时间内里程计输出的脉冲个数，结合已经被MU校正过后的里 程计刻度因子误差、安装误差角，转换成距离观测量，对GNSS/INS车载组合系统的状态量进 行校正。2. 根据权利要求1所述的基于里程计的克服卫星失锁时的GNSS/INS车载组合定位定向 算法，其特征在于，里程计安装在从动轮上。3. 根据权利要求1所述的基于里程计的克服卫星失锁时的GNSS/INS车载组合定位定向 算法，其特征在于，里程计误差的校正使用的是惯性导航系统的位置和相应的协方差矩阵。4. 根据权利要求1所述的基于里程计的克服卫星失锁时的GNSS/INS车载组合定位定向 算法，其特征在于，卡尔曼滤波器采用GNSS/INS与INS/ODO两级卡尔曼滤波器级联结构。5. 根据权利要求1所述的基于里程计的克服卫星失锁时的GNSS/INS车载组合定位定向 算法，其特征在于，里程计校正惯性导航系统状态量，仅当GNSS失锁一定时间后才进行，一 定时间是指不少于5秒。
【文档编号】G01S19/48GK105824039SQ201610152739
【公开日】2016年8月3日
【申请日】2016年3月17日
【发明人】孙红星, 丁学文, 王晖
【申请人】孙红星