一种基于高度约束的扩展卡尔曼滤波定位方法
【专利摘要】本发明公开了一种基于高度约束的扩展卡尔曼滤波定位方法，适用于GNSS导航定位系统，先利用扩展卡尔曼滤波算法根据上一历元的状态估计值获取当前历元的状态估计值，再通过建立高度约束条件来约束当前历元的定位高度得到当前历元符合高度约束条件的最优状态估计值和对应的均方误差，最后再利用均方误差获取伪距和多普勒频移的测量残余对状态估计值进行进一步地校正得到当前历元最终的状态估计值，更准确的获得待定位目标在当前历元的位置信息，达到提高GNSS导航定位的精度。
【专利说明】
一种基于高度约束的扩展卡尔曼滤波定位方法
技术领域
[0001] 本发明涉及卫星定位领域，尤其涉及一种基于高度约束的扩展卡尔曼滤波定位方 法。
【背景技术】
[0002] 卡尔曼滤波算法可以对线性高斯白噪声系统的状态进行最优估计，在GNSS定位导 航中被广泛使用。卡尔曼滤波算法首先利用状态方程预测接收机当前的位置、速度和钟差 等状态，然后根据状态估计值以及卫星星历所提供的卫星位置和速度，预测出各卫星的伪 距和多普勒频移值，最后通过处理测量残余得到校正后的最优估计值。卡尔曼滤波算法在 每得到一个新的测量值后，就可以对系统状态估计值更新一次，成为提高GNSS导航定位实 时性的一种有效手段。
[0003] 传统的卡尔曼滤波定位方法多采用传感器、气压计等辅助设备使用具有航向约 束、速度约束的卡尔曼滤波算法来提高GNSS在高度方向上的定位精度。这些方法均采用外 设或需要已知航向、实时速度等信息，增加了卫星定位的成本。
【发明内容】
[0004] 发明目的：为了解决卫星定位在高度方向上的剧变问题，本发明提供一种基于高 度约束的扩展卡尔曼滤波定位方法，在不借助外界辅助信息和外接传感器设备的基础上， 能够提高GNSS导航定位的精度。
[0005] 技术方案:为了实现上述目的，本发明中基于高度约束的扩展卡尔曼滤波定位方 法，包括如下步骤：
[0006] (1)利用预测公式根据上一历元的最终状态估计值％-1以及最终状态估计值的均 方误差Pk-l估计待定位目标的在当前历兀的先验状态估计值％和该先验状态估计值的均方 误差P".
[0007] (2)构建高度约束条件，利用最小均方误差原则获取在高度约束条件下，当前历元 的最优状态估计值4以及最优状态估计值的均方误差&?.
[0008] (3)利用卡尔曼滤波的增益矩阵Kk对所述当前历元的最优状态估计值耘以及最优 状态估计值的均方误差斤进行校正，得到当前历元的最终状态估计值％和该最终状态估计 值的均方误差阵Pk。
[0009] 有益效果:本发明中基于高度约束的扩展卡尔曼滤波定位方法，从应用角度出发， 充分考虑汽车行驶的各种场景，针对各场景均存在临界角度的特点提出爬坡模型无需外设 传感器设备，便能够提高卫星定位精度、解决定位高度剧变现象。
【附图说明】
[0010] 图1为本发明中建立的用户运动约束模型示意图；
[0011] 图2为本发明中基于高度约束扩展卡尔曼滤波定位方法的流程图；
[0012] 图3为本发明中基于高度约束扩展卡尔曼滤波定位方法HCEKF与传统的卡尔曼滤 波EKF、高度气压计和卡尔曼滤波EKF融合算法与真实运动轨迹在海拔高度微变场景下的天 向误差图；
[0013] 图4为本发明中基于高度约束扩展卡尔曼滤波定位方法HCEKF与传统的卡尔曼滤 波EKF、高度气压计和卡尔曼滤波EKF融合算法与真实运动轨迹在海拔高度陡变场景下的天 向误差图；
【具体实施方式】
[0014]下面结合附图进一步解释本发明。
[0015] 本发明中基于高度约束的扩展卡尔曼滤波定位方法，建立高度约束条件对传统的 扩展卡尔曼滤波定位方法进行优化，本实施中先具体介绍所使用的爬坡模型。以汽车作为 待定位目标，在实际场景中，如图1所示，汽车在行驶时与东北向平面的夹角均小于某一角 度，称该角度为临界角9,即汽车行驶的平面和东北向平面的实际角度均小于该临界角，本 实施例中设置该临界角为5°。
[0016] 根据三角函数关系，可以得到高度约束条件：
[0017] \Uk - U^l < - ￡fc-i)2 ? tan0 (1)
[0018]式⑴中，Ek、Nk分别表示待定位目标在第k历元的东向、北向坐标，Uk表示待定位目 标在第k历元的天向坐标，待定位目标的东北天坐标需满足上述高度约束条件。
[0019] 在扩展卡尔曼滤波定位算法过程中，所得到的待定位目标位置为地心地固直角坐 标系坐标，记为他[0]上[1]41<[2])，对应的东北天坐标记为收，他，1]1〇，其中￡1<眞和1]1<分 别代表状态的东向、北向和天向坐标。
[0020] 地心地固直角坐标(Xk[0]，Xk[l]，Xk[2])转化为东北天坐标^^^，队八首先需要 转化到大地坐标（巾，A，h)，其变换公式为：
[0024] 大地坐标系定义一个与地球几何最吻合的椭球体，称为基准椭球体。其中，N为基 准椭球体的卯酉圆曲率半径，e为椭球偏心率。
[0025] 因为h的计算式(3)中含有待求的巾，而巾的计算式(4)反过来又含有待求得h，所 以一般借助迭代法来逐次逼近、求解巾和h的值。迭代的计算过程如下：首先假设巾的初始 值为0,经过公式(2)、（3)和公式(4)依次计算出N，h和巾，然后再将刚得到的巾重新带入以 上三式，再一次更新N，h和巾的值。一般经过3~4次迭代就可以结束计算。
[0026] 进一步求解待定位目标在东北天向坐标系下的位置：
[0027] a e= A 入? a ? cos (5)
[0028] A n= A (}) ? a (6)
[0029] Au= Ah (7)
[0030] 其中，a为基准椭球体的长半径，（A (})，A A，Ah)为待定位目标在第k历元和第k_l 历元的大地坐标之差，（A e，An，A u)为待定位目标在第k历元和第k-1历元的东北天坐标 之差。
[0031] 基于上述高度约束条件，本发明中基于高度约束的扩展卡尔曼滤波定位方法，具 体为:如图2所示，在卫星定位导航中，位于带定位目标上的接收机接收卫星信号用于定位， 接收到的卫星信号经过数据处理得到卡尔曼定位所需要的卫星星历、伪距和多普勒频移测 量值，卡尔曼定位算法一般是利用状态方程根据上一历元的状态估计值，来预测下一历元 的状态估计值，本发明在这一过程中增加了高度约束条件，并利用高度约束条件来获取接 收机得到的伪距和多普勒频移测量值和卡尔曼定位算法所得到的伪距和多普勒频移预测 值之间的测量残余，对卡尔曼定位算法所得到的状态估计进行校正，综上所述，本发明中基 于高度约束的扩展卡尔曼滤波定位方法，主要包括:预测过程、基于高度约束条件的最优状 态值估计过程和校正过程。
[0032] 预测过程:利用预测公式预测接收机当前的位置等状态。输入变量为上一历元的 最终状态估计值％-i、最终状态估计值的均方误差Ph、过程噪声的协方差Qh，输出变量为 预测的当前历元的状态估计值％和该状态估计值的均方误差阵
[0033] (8)
[0034] = AP,^ + Qk_± (9)
[0035] 基于高度约束条件的最优状态值估计过程:如上述公式(2)~（7)，将预测过程所 得的状态估计向量和上一历元的最终状态估计值的位置坐标转化为待定位目标在第k历元 和第k-1历元的东北天坐标之差（A e，An，A u);因此公式（1)可化简为：
[0036] \Uk - Uk-\ \ ^ V^a2 + Ae2 tan6 (10)
[0037] 扩展卡尔曼滤波遵从最小均方误差原则，可以从公式(1)中选择最优状态估计值。
[0038] % = argmin^.^ - xk ) Pr(x - xk ) (11)
[0039] PI = (x-x^P^(x-x-) (12)
[0040] 公式（11)中$代表卡尔曼滤波所预测的状态估计值，代表此状态估计值的均 方误差。为求解公式（11)，将满足公式（10)的所有状态估计向量的天向坐标的范围区间分 成足够多的等距离的子区间，截取区间的端点值组成一系列的离散的天向坐标，在这些离 散点中选取4的最小值作为最优状态估计值的均方误差，此时所对应的状态估计x记为最 优状态估计值
[0041] 校正过程:卫星接收信号经过接收机处理后得到卫星星历、伪距和多普勒频移的 测量值，通过卫星星历得到的卫星位置与速度，卡尔曼滤波器预测出接收机对各颗卫星的 伪距和多普勒频移值，而这些预测值和伪距及多普勒频移的测量值之间的差异形成测量残 差，结合实际测量值校正所得到的约束条件下最优的状态估计值。利用状态估计值的均方 误差阵 Pk~的对角线元素最小原则推导出卡尔曼滤波的增益矩阵Kk，并利用实际测量值对 最优状态估计值C进行验证，从而使得它的均方误差值变小，可靠性增加，进一步校正得到 最终的状态估计值私和该状态估计值的均方误差阵Pk。
[0042] Kk = P；：CT{CPkCT + R)1 (13)
[0043] xfe - % +Mk(yk - Cxp, (14)
[0044] Pk = (l-KkQP【 (15)
[0045] 其中，C表不观测量与系统状态之间的关系矩阵，R为测量噪声向量的协方差矩阵， yk为伪距及多普勒频移的实际观测量。
[0046] 为了验证本发明中基于高度约束的扩展卡尔曼滤波定位方法相对于现有技术的 优势，分别在海拔高度微变场景和海拔高度陡变场景下进行实际的路径测试，实验采用北 斗星通C200接收机基带及处理模块得到卡尔曼滤波定位所需要的卫星星历、伪距及多普勒 频移测量值;采用NovAtel公司的SPAN-CPT组合导航系统进行实际路线的标定。本实施例中 对比分析了传统的扩展卡尔曼滤波定位方法，记为EKF;高度气压计和卡尔曼滤波方法融合 的方法，记为EKF+高度气压计；以及本发明中所提出的基于高度约束的扩展卡尔曼滤波定 位方法，记为HCEKF。
[0047] 从图3、4中可以看出，无论是在海拨高度微变还是陡变的场景下，本发明所提出的 基于高度约束的扩展卡尔曼滤波定位方法HCEKF相对于传统的扩展卡尔曼滤波定位方法 EKF、以及其余高度气压计融合方法的天向误差都比较稳定，在整个测试时间段内的起伏不 是很大，因此本发明中基于高度约束的扩展卡尔曼滤波定位方法HCEKF能够有效解决卫星 定位在高度方向上的剧变问题，提高定位精度。
[0048]以上所述仅是本发明的优选实施方式，应当指出：对于本技术领域的普通技术人 员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应 视为本发明的保护范围。
【主权项】
1. 一种基于高度约束的扩展卡尔曼滤波定位方法，其特征在于，包括以下步骤： (1) 利用预测公式根据上一历元的最终状态估计值以及最终状态估计值的均方误 差Pk-I估计待定位目标的在当前历兀的先验状态估计值知和该先验状态估计值的均方误差 Pk^ (2) 构建高度约束条件，利用最小均方误差原则获取在高度约束条件下，当前历元的最 优状态估计值却以及最优状态估计值的均方误差 (3) 利用卡尔曼滤波的增益矩阵Kk对所述当前历元的最优状态估计值私·以及最优状态 估计值的均方误差K:进行校正，得到当前历元的最终状态估计值4和该最终状态估计值的 均方误差阵Pk。2. 根据权利要求1所述的基于高度约束的扩展卡尔曼滤波定位方法，其特征在于，步骤 (1)中所述预测公式为：式中，A为转移矩阵，Qk-i为上一历元的过程噪声协方差。3. 根据权利要求1所述的基于高度约束的扩展卡尔曼滤波定位方法，其特征在于，所述 高度约束条件为：Ek、Nk、Uk分别表示待定位目标在第k历元的东向、北向坐标和天向坐标，Ek-i、Nk-i、Uk-1分 别表示待定位目标在第k-1历元的东向、北向坐标和天向坐标。4. 根据权利要求3所述的基于高度约束的扩展卡尔曼滤波定位方法，其特征在于，获取 在高度约束条件下，当前历元的最优状态估计值％~以及最优状态估计值的均方误差ΡΓ包 括以下步骤： (31) 将步骤(2)中所得的状态估计值％"和上一历元的最终状态估计值氣-i的位置坐标 转化为待定位目标在第k历元和第k-Ι历元的东北天坐标之差（Δ e，Δη，Au); (32) 将所述高度约束笋Ikk P夂I- (33) 将满足所述简化后的高度约束条件的所有状态估计值的天向坐标的范围区间分 成若干个等距离的子区间，截取区间的端点值组成一系列的离散的天向坐标，在这些离散 点中获取状态估计值均方误差的最小值记为最优状态估计值的均方误差4~，此时所对应的 状态估计记为最优状态估计箱。5. 根据权利要求4所述的基于高度约束的扩展卡尔曼滤波定位方法，其特征在于，在所 述卡尔曼滤波定位方法中用于直接计算的是地心地固直角坐标系坐标Xk，其位置坐标记为 (Xk[0]，Xk[l]，Xk[2])，设其对应的大地坐标为（Φ，A，h)，东北天坐标为(Ek，Nk，Uk)，则将地 心地固直角坐标系坐标〇1<[0]41<[1]41<[2])转化为其对应的东北天坐标江1 {，池，1]1〇包括以 下步骤： (1)将地心地固直角坐标系坐标(Xk[ O ]，Xk[ I ]，Xk[ 2 ])转化到大地坐标（Φ，λ，h)，其变 换公式为：(2) 令Φ的初始值为0,利用迭代算法计算N，h和Φ的值； (3) 将待定位目标两个历元的大地坐标之差转化为东北天坐标之差： Ae=AA*a· cos Φ Δ η= Δ φ · a Λ u= Λ h 式中，a为基准椭球体的长半径，（Δ φ，Δλ，Ah)为在待定位目标第k历元和第k-l历元 的大地坐标之差，（Ae，Δη，Au)为待定位目标在第k历元和第k-Ι历元的东北天坐标之差。
【文档编号】G01S19/42GK105891863SQ201610432153
【公开日】2016年8月24日
【申请日】2016年6月16日
【发明人】庄园, 马金凤, 戚隆宁, 刘新宁, 杨军
【申请人】东南大学