CN105891863A - 一种基于高度约束的扩展卡尔曼滤波定位方法 - Google Patents

Abstract

本发明公开了一种基于高度约束的扩展卡尔曼滤波定位方法，适用于GNSS导航定位***，先利用扩展卡尔曼滤波算法根据上一历元的状态估计值获取当前历元的状态估计值，再通过建立高度约束条件来约束当前历元的定位高度得到当前历元符合高度约束条件的最优状态估计值和对应的均方误差，最后再利用均方误差获取伪距和多普勒频移的测量残余对状态估计值进行进一步地校正得到当前历元最终的状态估计值，更准确的获得待定位目标在当前历元的位置信息，达到提高GNSS导航定位的精度。

Description

一种基于高度约束的扩展卡尔曼滤波定位方法

技术领域

本发明涉及卫星定位领域，尤其涉及一种基于高度约束的扩展卡尔曼滤波定位方法。

背景技术

卡尔曼滤波算法可以对线性高斯白噪声***的状态进行最优估计，在GNSS定位导航中被广泛使用。卡尔曼滤波算法首先利用状态方程预测接收机当前的位置、速度和钟差等状态，然后根据状态估计值以及卫星星历所提供的卫星位置和速度，预测出各卫星的伪距和多普勒频移值，最后通过处理测量残余得到校正后的最优估计值。卡尔曼滤波算法在每得到一个新的测量值后，就可以对***状态估计值更新一次，成为提高GNSS导航定位实时性的一种有效手段。

传统的卡尔曼滤波定位方法多采用传感器、气压计等辅助设备使用具有航向约束、速度约束的卡尔曼滤波算法来提高GNSS在高度方向上的定位精度。这些方法均采用外设或需要已知航向、实时速度等信息，增加了卫星定位的成本。

发明内容

发明目的：为了解决卫星定位在高度方向上的剧变问题，本发明提供一种基于高度约束的扩展卡尔曼滤波定位方法，在不借助外界辅助信息和外接传感器设备的基础上，能够提高GNSS导航定位的精度。

技术方案：为了实现上述目的，本发明中基于高度约束的扩展卡尔曼滤波定位方法，包括如下步骤：

(1)利用预测公式根据上一历元的最终状态估计值以及最终状态估计值的均方误差P_k-1估计待定位目标的在当前历元的先验状态估计值和该先验状态估计值的均方误差

(2)构建高度约束条件，利用最小均方误差原则获取在高度约束条件下，当前历元的最优状态估计值以及最优状态估计值的均方误差

(3)利用卡尔曼滤波的增益矩阵K_k对所述当前历元的最优状态估计值以及最优状态估计值的均方误差进行校正，得到当前历元的最终状态估计值和该最终状态估计值的均方误差阵P_k。

有益效果：本发明中基于高度约束的扩展卡尔曼滤波定位方法，从应用角度出发，充分考虑汽车行驶的各种场景，针对各场景均存在临界角度的特点提出爬坡模型无需外设传感器设备，便能够提高卫星定位精度、解决定位高度剧变现象。

附图说明

图1为本发明中建立的用户运动约束模型示意图；

图2为本发明中基于高度约束扩展卡尔曼滤波定位方法的流程图；

图3为本发明中基于高度约束扩展卡尔曼滤波定位方法HCEKF与传统的卡尔曼滤波EKF、高度气压计和卡尔曼滤波EKF融合算法与真实运动轨迹在海拔高度微变场景下的天向误差图；

图4为本发明中基于高度约束扩展卡尔曼滤波定位方法HCEKF与传统的卡尔曼滤波EKF、高度气压计和卡尔曼滤波EKF融合算法与真实运动轨迹在海拔高度陡变场景下的天向误差图；

具体实施方式

下面结合附图进一步解释本发明。

本发明中基于高度约束的扩展卡尔曼滤波定位方法，建立高度约束条件对传统的扩展卡尔曼滤波定位方法进行优化，本实施中先具体介绍所使用的爬坡模型。以汽车作为待定位目标，在实际场景中，如图1所示，汽车在行驶时与东北向平面的夹角均小于某一角度，称该角度为临界角θ，即汽车行驶的平面和东北向平面的实际角度均小于该临界角，本实施例中设置该临界角为5°。

根据三角函数关系，可以得到高度约束条件：

$\left|U_k-U_{k-1}\right|\≤\sqrt{{(N_k-N_{k-1})}^2+{(E_k-E_{k-1})}^2}\·tan\mathrm{\θ}---\left(1\right)$

式(1)中，E_k、N_k分别表示待定位目标在第k历元的东向、北向坐标，U_k表示待定位目标在第k历元的天向坐标，待定位目标的东北天坐标需满足上述高度约束条件。

在扩展卡尔曼滤波定位算法过程中，所得到的待定位目标位置为地心地固直角坐标系坐标，记为(X_k[0]，X_k[1]，X_k[2])，对应的东北天坐标记为(E_k,N_k,U_k)，其中E_k，N_k和U_k分别代表状态的东向、北向和天向坐标。

地心地固直角坐标(X_k[0]，X_k[1]，X_k[2])转化为东北天坐标(E_k,N_k,U_k)，首先需要转化到大地坐标(φ,λ,h)，其变换公式为：

$\mathrm{\λ}=arctan\left(\frac{X_k\[1\]}{X_k\[0\]}\right)---\left(2\right)$

$h=\frac{\sqrt{X_k{\[0\]}^2+X_k{\[1\]}^2}}{cos\mathrm{\φ}}-N---\left(3\right)$

$\mathrm{\φ}=arctan\[\frac{X_k\[2\]}{\sqrt{X_k{\[0\]}^2+X_k{\[1\]}^2}}(1-e^2\frac{N}{N+h})\]---\left(4\right)$

大地坐标系定义一个与地球几何最吻合的椭球体，称为基准椭球体。其中，N为基准椭球体的卯酉圆曲率半径，e为椭球偏心率。

因为h的计算式(3)中含有待求的φ，而φ的计算式(4)反过来又含有待求得h，所以一般借助迭代法来逐次逼近、求解φ和h的值。迭代的计算过程如下：首先假设φ的初始值为0，经过公式(2)、(3)和公式(4)依次计算出N，h和φ，然后再将刚得到的φ重新带入以上三式，再一次更新N，h和φ的值。一般经过3～4次迭代就可以结束计算。

进一步求解待定位目标在东北天向坐标系下的位置：

Δe＝Δλ·a·cosφ (5)

Δn＝Δφ·a (6)

Δu＝Δh (7)

其中，a为基准椭球体的长半径，(Δφ，Δλ，Δh)为待定位目标在第k历元和第k-1历元的大地坐标之差，(Δe，Δn，Δu)为待定位目标在第k历元和第k-1历元的东北天坐标之差。

基于上述高度约束条件，本发明中基于高度约束的扩展卡尔曼滤波定位方法，具体为：如图2所示，在卫星定位导航中，位于带定位目标上的接收机接收卫星信号用于定位，接收到的卫星信号经过数据处理得到卡尔曼定位所需要的卫星星历、伪距和多普勒频移测量值，卡尔曼定位算法一般是利用状态方程根据上一历元的状态估计值，来预测下一历元的状态估计值，本发明在这一过程中增加了高度约束条件，并利用高度约束条件来获取接收机得到的伪距和多普勒频移测量值和卡尔曼定位算法所得到的伪距和多普勒频移预测值之间的测量残余，对卡尔曼定位算法所得到的状态估计进行校正，综上所述，本发明中基于高度约束的扩展卡尔曼滤波定位方法，主要包括：预测过程、基于高度约束条件的最优状态值估计过程和校正过程。

预测过程：利用预测公式预测接收机当前的位置等状态。输入变量为上一历元的最终状态估计值最终状态估计值的均方误差P_k-1、过程噪声的协方差Q_k-1，输出变量为预测的当前历元的状态估计值和该状态估计值的均方误差阵

${\hat{x}}_k^{-}=A{\hat{x}}_{k-1}---\left(8\right)$

$P_k^{-}={\mathrm{AP}}_{k-1}{A}^T+Q_{k-1}---\left(9\right)$

基于高度约束条件的最优状态值估计过程：如上述公式(2)～(7)，将预测过程所得的状态估计向量和上一历元的最终状态估计值的位置坐标转化为待定位目标在第k历元和第k-1历元的东北天坐标之差(Δe，Δn，Δu)；因此公式(1)可化简为：

$\left|U_k-U_{k-1}\right|\≤\sqrt{{\mathrm{\Δn}}^2+{\mathrm{\Δe}}^2}\·tan\mathrm{\θ}---\left(10\right)$

扩展卡尔曼滤波遵从最小均方误差原则，可以从公式(1)中选择最优状态估计值。

${\hat{x}}_k^{~}={\mathrm{argmin}}_x{(x-{\hat{x}}_k^{-})}^{\′}{P}_k^{-}(x-{\hat{x}}_k^{-})---\left(11\right)$

$P_k^{~}={(x-{\hat{x}}_k^{-})}^{\′}{P}_k^{-}(x-{\hat{x}}_k^{-})---\left(12\right)$

公式(11)中代表卡尔曼滤波所预测的状态估计值，代表此状态估计值的均方误差。为求解公式(11)，将满足公式(10)的所有状态估计向量的天向坐标的范围区间分成足够多的等距离的子区间，截取区间的端点值组成一系列的离散的天向坐标，在这些离散点中选取的最小值作为最优状态估计值的均方误差，此时所对应的状态估计x记为最优状态估计值

校正过程：卫星接收信号经过接收机处理后得到卫星星历、伪距和多普勒频移的测量值，通过卫星星历得到的卫星位置与速度，卡尔曼滤波器预测出接收机对各颗卫星的伪距和多普勒频移值，而这些预测值和伪距及多普勒频移的测量值之间的差异形成测量残差，结合实际测量值校正所得到的约束条件下最优的状态估计值。利用状态估计值的均方误差阵P_k～的对角线元素最小原则推导出卡尔曼滤波的增益矩阵K_k，并利用实际测量值对最优状态估计值进行验证，从而使得它的均方误差值变小，可靠性增加，进一步校正得到最终的状态估计值和该状态估计值的均方误差阵P_k。

$K_k=P_k^{~}{C}^T{({{\mathrm{CP}}_k}^{-}{C}^T+R)}^{-1}---\left(13\right)$

${\hat{x}}_k={\hat{x}}_k^{~}+K_k(y_k-C{\hat{x}}_k^{~})---\left(14\right)$

$P_k=(I-K_{k}C)P_k^{~}---\left(15\right)$

其中，C表示观测量与***状态之间的关系矩阵，R为测量噪声向量的协方差矩阵，y_k为伪距及多普勒频移的实际观测量。

为了验证本发明中基于高度约束的扩展卡尔曼滤波定位方法相对于现有技术的优势，分别在海拔高度微变场景和海拔高度陡变场景下进行实际的路径测试，实验采用北斗星通C200接收机基带及处理模块得到卡尔曼滤波定位所需要的卫星星历、伪距及多普勒频移测量值；采用NovAtel公司的SPAN-CPT组合导航***进行实际路线的标定。本实施例中对比分析了传统的扩展卡尔曼滤波定位方法，记为EKF；高度气压计和卡尔曼滤波方法融合的方法，记为EKF+高度气压计；以及本发明中所提出的基于高度约束的扩展卡尔曼滤波定位方法，记为HCEKF。

从图3、4中可以看出，无论是在海拨高度微变还是陡变的场景下，本发明所提出的基于高度约束的扩展卡尔曼滤波定位方法HCEKF相对于传统的扩展卡尔曼滤波定位方法EKF、以及其余高度气压计融合方法的天向误差都比较稳定，在整个测试时间段内的起伏不是很大，因此本发明中基于高度约束的扩展卡尔曼滤波定位方法HCEKF能够有效解决卫星定位在高度方向上的剧变问题，提高定位精度。

以上所述仅是本发明的优选实施方式，应当指出：对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (5)

1.一种基于高度约束的扩展卡尔曼滤波定位方法，其特征在于，包括以下步骤：

(1)利用预测公式根据上一历元的最终状态估计值以及最终状态估计值的均方误差P_k-1估计待定位目标的在当前历元的先验状态估计值和该先验状态估计值的均方误差

(2)构建高度约束条件，利用最小均方误差原则获取在高度约束条件下，当前历元的最优状态估计值以及最优状态估计值的均方误差

(3)利用卡尔曼滤波的增益矩阵K_k对所述当前历元的最优状态估计值以及最优状态估计值的均方误差进行校正，得到当前历元的最终状态估计值和该最终状态估计值的均方误差阵P_k。

2.根据权利要求1所述的基于高度约束的扩展卡尔曼滤波定位方法，其特征在于，步骤(1)中所述预测公式为：

${\hat{x}}_k^{-}=A{\hat{x}}_{k-1}$

$P_k^{-}={\mathrm{AP}}_{k-1}{A}^T+Q_{k-1}$

式中，A为转移矩阵，Q_k-1为上一历元的过程噪声协方差。

3.根据权利要求1所述的基于高度约束的扩展卡尔曼滤波定位方法，其特征在于，所述高度约束条件为：

$|U_k-U_{k-1}|\≤\sqrt{{(N_k-N_{k-1})}^2+{(E_k-E_{k-1})}^2}\·tan\mathrm{\θ}$

E_k、N_k、U_k分别表示待定位目标在第k历元的东向、北向坐标和天向坐标，E_k-1、N_k-1、U_k-1分别表示待定位目标在第k-1历元的东向、北向坐标和天向坐标。

4.根据权利要求3所述的基于高度约束的扩展卡尔曼滤波定位方法，其特征在于，获取在高度约束条件下，当前历元的最优状态估计值以及最优状态估计值的均方误差P_k ^～包括以下步骤：

(31)将步骤(2)中所得的状态估计值和上一历元的最终状态估计值的位置坐标转化为待定位目标在第k历元和第k-1历元的东北天坐标之差(Δe，Δn，Δu)；

(32)将所述高度约束条件转化简化后的高度约束条件：

$|U_k-U_{k-1}|\≤\sqrt{{\mathrm{\Δn}}^2+{\mathrm{\Δe}}^2}\·tan\mathrm{\θ};$

(33)将满足所述简化后的高度约束条件的所有状态估计值的天向坐标的范围区间分成若干个等距离的子区间，截取区间的端点值组成一系列的离散的天向坐标，在这些离散点中获取状态估计值均方误差的最小值记为最优状态估计值的均方误差此时所对应的状态估计记为最优状态估计

5.根据权利要求4所述的基于高度约束的扩展卡尔曼滤波定位方法，其特征在于，在所述卡尔曼滤波定位方法中用于直接计算的是地心地固直角坐标系坐标X_k，其位置坐标记为(X_k[0]，X_k[1]，X_k[2])，设其对应的大地坐标为(φ,λ,h)，东北天坐标为(E_k,N_k,U_k)，则将地心地固直角坐标系坐标(X_k[0]，X_k[1]，X_k[2])转化为其对应的东北天坐标(E_k,N_k,U_k)包括以下步骤：

(1)将地心地固直角坐标系坐标(X_k[0]，X_k[1]，X_k[2])转化到大地坐标(φ,λ,h)，其变换公式为：

$\mathrm{\λ}=artan\left(\frac{X_k\[1\]}{X_k\[0\]}\right)$

$h=\frac{\sqrt{X_k{\[0\]}^2+X_k{\[1\]}^2}}{\cos \mathrm{\φ}}-N$

$\mathrm{\φ}=arctan\[\frac{X_k\[2\]}{\sqrt{X_k{\[0\]}^2+X_k{\[1\]}^2}}(1-e^2\frac{N}{N+h})\]$

(2)令φ的初始值为0，利用迭代算法计算N，h和φ的值；

(3)将待定位目标两个历元的大地坐标之差转化为东北天坐标之差：

Δe＝Δλ·a·cosφ

Δn＝Δφ·a

Δu＝Δh

式中，a为基准椭球体的长半径，(Δφ，Δλ，Δh)为在待定位目标第k历元和第k-1历元的大地坐标之差，(Δe，Δn，Δu)为待定位目标在第k历元和第k-1历元的东北天坐标之差。