CN109839615A - 一种基于ukf算法的伪卫星室内定位方法 - Google Patents

Abstract

本发明公开了一种基于UKF算法的伪卫星室内定位方法，该方法包括以下步骤：(1)构建伪卫星双差伪距观测模型；(2)采用UKF算法对伪卫星进行室内定位。本发明构建伪卫星双差伪距观测模型，消除伪卫星伪距测量值中的接收机钟差、卫星钟差、电离层延迟、对流层延迟的影响。对于独立组网的伪卫星系统采用EKF算法会产生较大的线性化误差的问题，对观测模型的线性化误差进行分析，发现线性化误差的影响因素为伪卫星离接收机的距离和接收机位置误差，当收机坐标误差越大时和卫星距离接收机的距离越小时，线性化误差越大。在伪卫星室内定位中，采用UKF算法定位，可以避免线性化误差，当接收机坐标误差相对较大时，能够得到比EKF算法更快的收敛速度与更高的室内定位精度。

Description

一种基于UKF算法的伪卫星室内定位方法

技术领域

本发明涉及伪卫星系统定位技术领域，具体涉及一种基于UKF算法的伪卫星室内定位方法。

背景技术

卫星导航系统的可靠性和精度比较依靠于可见卫星的数量个几何分布情况。但是一旦处于室内，可见卫星的数量就会减少，因此会大大影响定位精度。针对可见卫星不足导致的定位精度不高的问题，可以利用伪卫星进行定位。伪卫星又被称为陆基发设计或陆基卫星，它的功能和原理与导航卫星类似，具有成本低、设置灵活等特点。与导航卫星不同的是，伪卫星是布设于地面的，而且设置灵活，所以可以解决可见卫星数量少和几何分布差的问题。

对于独立组网的伪卫星系统，在伪卫星定位解算时，伪距定位方程是非线性的，传统方法是利用扩展卡尔曼滤波(EKF)算法，对非线性函数进行一阶泰勒展开，然后略去二次以上项。但是因为伪卫星到用户接收机的距离比导航卫星到用户接收机的距离近很多，所以使用EKF算法造成的伪卫星伪距观测方程线性化误差会比较大。并且在卫星距离用户接收机的距离较小的时候，用户接收机坐标的误差对线性化误差影响也比较大。

发明内容

发明目的：为了克服现有技术的不足，本发明提供一种基于UKF算法的伪卫星室内定位方法，该方法可以解决非线性误差对室内定位的影响问题。

技术方案：本发明所述的基于UKF算法的伪卫星室内定位方法，该方法包括以下步骤：

(1)构建伪卫星双差伪距观测模型；

(2)采用UKF算法对伪卫星进行室内定位。

优选的，所述步骤(1)中，构建伪卫星双差伪距观测方程，包括：

(11)构建伪卫星伪距观测方程为：

其中，ρⁿ为接收机收到PRN号为n的伪卫星的伪距，r为接收机到所述伪卫星的实际距离，δt_u为接收机钟差，δtⁿ为所述伪卫星钟差，I为电离层延迟，T为对流层延迟，为测量噪声参量，所述接收机为用户接收机或基准接收机；

(12)根据所述伪卫星伪距观测方程，构造双差伪距观测方程：

双差伪距测量值定义为：

ρ^ij _uv＝(ρⁱ _u-ρⁱ _v)-(ρ^j _u-ρ^j _v)

双差几何距离定义为：

r^ij _uv＝(rⁱ _u-rⁱ _v)-(r^j _u-r^j _v)

其中，i和j是指不同的伪卫星，u和v分别是指用户接收机和基准接收机，为用户接收机到所述伪卫星i的实际距离，ρⁱ _u为用户接收机到所述伪卫星i的伪距。

优选的，所述步骤(2)中，采用UKF算法对伪卫星进行室内定位具体包括：

(21)根据伪卫星双差伪距观测模型设定状态方程和观测方程，所述状态方程为：

x_τ＝f(x_τ-1)+ω_τ

其中，x_τ和x_τ-1分别为τ和τ-1时刻的状态向量，即用户接收机坐标向量，ω_τ是状态噪声向量；

所述双差伪距测量值z_τ用观测方程h(x_τ)表示为：

z_τ＝h(x_τ)+v_τ

其中，v_τ是观测噪声向量；

(22)选定滤波初值：

其中，E()指数学期望，X₀指状态向量初值，指状态向量初值的均值，P₀指状态向量初值的协方差阵。

(23)计算sigma点：

其中，为上一个历元状态向量均值，为上一个历元第0个sigma状态向量值，P_k-1为上一个历元状态向量协方差阵，γ为比例参数，为上一个历元第i个sigma状态向量值，n是状态向量维数；λ＝a²(n+k)-n，其中，a是用于确定上一个历元状态向量均值周围sigma点的分布的正数；k为参数且k＝3-n；

(24)确定权值：

其中，和为第i个sigma点状态向量的权值，和为第i个sigma点状态向量的协方差阵权值，β为状态分布参数；

(25)时间更新：

其中，为上一个历元第i个sigma状态向量值，f()为两个历元间的状态传递函数，为第i个sigma预测状态向量值，为预测状态向量，P_k/k-1为预测状态向量的协方差矩阵，Q_k-1为上一个历元的状态向量噪声矩阵；

(26)测量更新：

其中，h()为观测方程，为预测观测向量，为预测向量和观测向量的协方差矩阵，R_k为观测噪声矩阵；

(27)滤波更新：

其中，为估计状态向量，为预测观测向量和状态向量的协方差阵，P_k为估计状态向量的协方差矩阵，K_k为增益矩阵。

优选的，所述步骤(1)还包括对线性化误差的影响因素进行分析，包括表示出伪卫星到用户接收机的几何距离：

可得出双差伪距方程的二阶残余项ε_dh有如下估计：

其中，分别为用户接收机坐标(x_u，y_u，z_u)的方差，为伪卫星i到用户接收机的几何距离，为伪卫星j到用户接收机的几何距离，u指用户接收机，i和j指不同的伪卫星。

优选的，所述线性化误差的影响因素为伪卫星与用户接收机的距离和用户接收机位置误差。

有益效果：本发明构建伪卫星双差伪距观测模型，消除伪卫星伪距测量值中的接收机钟差、卫星钟差、电离层延迟、对流层延迟的影响。对于独立组网的伪卫星系统采用EKF算法会产生较大的线性化误差的问题，对观测模型的线性化误差进行分析，发现线性化误差的影响因素为伪卫星离接收机的距离和接收机位置误差，当收机坐标误差越大时和卫星距离接收机的距离越小时，线性化误差越大。在伪卫星室内定位中，采用UKF算法定位，可以避免线性化误差，当接收机坐标误差相对较大时，能够得到比EKF算法更快的收敛速度与更高的室内定位精度。

附图说明

图1为本发明的方法流程示意图。

图2为本发明所得用户接收机坐标误差0.5m时定位结果图，横坐标Epoch为历元，纵坐标Error为定位误差。

图3为本发明所得用户接收机坐标误差1m时定位结果图，横坐标Epoch为历元，纵坐标Error为定位误差。

图4为本发明所得用户接收机坐标误差5m时定位结果图，横坐标Epoch为历元，纵坐标Error为定位误差。

图5为本发明所得用户接收机坐标误差10m时定位结果图，横坐标Epoch为历元，纵坐标Error为定位误差。

具体实施方式

实施例1

如图1，本发明提供一种基于UKF算法的伪卫星室内定位新方法，具体步骤如下：

步骤(1)，构建伪卫星双差伪距观测模型，伪卫星伪距观测方程为：

其中，ρⁿ为接收机收到PRN号为n的卫星的伪距，PRN号是指伪随机噪声码，用以区分不同卫星的C/A码。r为用户接收机到伪卫星的实际距离，δt_u为接收机钟差，δtⁿ为卫星钟差，I为电离层延迟，T为对流层延迟，为测量噪声参量。

为了消除伪卫星伪距测量值中的接收机钟差、卫星钟差、电离层延迟、对流层延迟，采根据所述伪卫星伪距观测方程，构造双差伪距观测方程：

双差伪距测量值定义为：

ρ^ij _uv＝(ρⁱ _u-ρⁱ _v)-(ρ^j _u-ρ^j _v) (3)

双差几何距离定义为：

r^ij _uv＝(rⁱ _u-rⁱ _v)-(r^j _u-r^j _v) (4)

式中，i和j是指不同的伪卫星，u和v分别是指用户接收机和基准接收机。

步骤(2)，对于独立组网的伪卫星系统采用EKF算法会产生较大的线性化误差的问题，对观测模型的线性化误差进行分析，发现线性化误差的影响因素：

用X表示状态向量，X₀为状态向量的近似估计值，δX＝X-X₀，A表示h(X)在X₀处的一阶偏导数，ε_h表示二阶残余量，则有：

h(X)＝h(X₀)+AδX+ε_h (5)

式中称为Hessian Matrix，由二阶偏导数构成，通常在EKF方法中，二阶残余量δ_h是直接舍去的，这一项就是线性化残余误差项。

假设u指用户接收机，i和j指伪卫星，则卫星到用户接收机的几何距离可表示为：

把双差伪距方程(1)进行泰勒展开，然后由公式(5)可得出双差伪距方程的二阶残余项ε_dh有如下估计：

式中，分别为用户接收机坐标(x_u，y_u，z_u)的方差，为伪卫星i到用户接收机的几何距离，为伪卫星j到用户接收机的几何距离。

由上述线性化误差的分析可以看出，因为伪卫星相对于全球导航卫星距离用户较近，如果不能保证每颗伪卫星离接收机的距离较为接近，那么其线性化误差就会比较大，不能够忽略。所以采用EKF算法会产生较大的线性化误差，线性化误差的影响因素为伪卫星离用户接收机的距离和用户接收机位置误差。

步骤(3)，对于独立组网的伪卫星系统，为了避免线性化误差，实现更准确的室内定位，采用UKF算法，具体步骤为：

设系统的状态方程和观测方程分别为：

x_τ＝f(x_τ-1)+ω_τ (9)

z_τ＝h(x_τ)+v_τ (10)

式中：x_τ和x_τ-1分别为τ和τ-1时刻的状态向量，即用户接收机坐标向量；z_τ为观测向量，即伪距双差值；ω_τ是状态噪声向量，对于室内静态定位噪声矩阵为零矩阵；v_τ是观测噪声向量；并且ω_τ和v_τ是互不相关的零均值白噪声序列，方差矩阵分别为Q_τ和R_τ。UKF算法的具体计算步骤如下：

(1)选定滤波初值：

式中，E()指数学期望，X₀指状态向量初值，指状态向量初值的均值，P₀指状态初值的协方差阵。

(2)计算sigma点：

其中，为上一个历元状态向量均值，为上一个历元第0个sigma状态向量值，P_k-1为上一个历元状态向量协方差阵，γ为比例参数，为上一个历元第i个sigma状态向量值，n是状态向量维数。

λ＝a₂(n+k)-n (17)

式中，a是很小的正数，通常为大于等于1/e^4，小于1，用于确定上一个历元状态向量均值周围sigma点的分布；κ＝3-n。

(3)确定权值：

式中，和W_i ^(m)为第i个sigma点状态向量的权值，和W_i ^(c)为第i个sigma点状态向量的协方差阵权值，β为状态分布参数。

(4)时间更新：

其中，为上一个历元第i个sigma状态向量值，f()为两个历元间的状态传递函数，为第i个sigma预测状态向量值，为预测状态向量，P_k/k-1为预测状态向量的协方差矩阵，Q_k-1为上一个历元的状态向量噪声矩阵。

(5)测量更新：

其中，h()为观测方程，为预测观测向量，为预测观测向量的协方差矩阵，R_k为观测噪声矩阵。

(6)滤波更新：

其中，为估计状态向量，为预测观测向量和状态向量的协方差阵，P_k为估计状态向量的协方差矩阵，K_k为增益矩阵。

从公式(8)可以看出，UKF方法直接使用了非线性系统的状态方程或观测方程进行计算，与EKF算法相比避免了线性化误差，从而实现更准确的室内定位。

为了验证卫星距离用户接收机的距离和用户接收机坐标的误差对线性化误差产生的影响，并且为了证明UKF算法在卫星距离接收机较近的情况下，定位效果优于EKF算法，实验采用自制的8颗伪卫星和2台U-BLOX接收机，实验场地在室内，8颗伪卫星。

分别采用了EKF和UKF进行定位解算，给定合适的初始历元的状态向量和状态向量的协方差矩阵，即X₀和P₀。对于UKF算法，设定适当的的权值参数。得到图2、图3、图4和图5为不同误差向量条件下的EFK算法和UKF算法的NEU三个方向上的误差图。从图2和图3可以发现，接收机误差比较小的时候，UKF算法和EKF算法定位误差比较接近，曲线几乎重合，分析原因应该是坐标偏差较小，线性化误差并不明显，所以UKF算法结果与EKF算法结果较为接近。

而从图4和图5可以发现，接收机误差比较大的时候，UKF算法的定位结果收敛速度比EKF算法快很多，这说明接收机坐标误差越大，线性化误差越大；而且UKF算法在初始阶段定位精度明显优于EKF算法，与导航卫星系统定位作对比，可以说明卫星距离接收机的距离越小，线性化误差越大。

表1用户接收机坐标误差0.5m时的均值和标准差对比

表2用户接收机坐标误差1m时的均值和标准差对比

表3用户接收机坐标误差5m时的均值和标准差对比

表4用户接收机坐标误差10m时的均值和标准差对比

表1、表2、表3和表4分别为不同接收机坐标误差时的定位结果均值和标准差对比，从表1和表2可以看出坐标偏差较小的时候，UKF和EKF定位结果较为接近。从表3可以看出，UKF算法和EKF算法的标准差比较接近，但UKF算法的N、E、U三个方向上的定位误差均略微小于EKF算法。这表明在坐标偏差为5米的时候，UKF算法的结果相比EKF算法波动情况接近，但定位精度已经有了一定的提高。从表4可以看出，UKF算法的N、E、U三个方向上的定位误差均值和标准差都大幅优于EKF算法，这表明了在坐标偏差为10米的时候，UKF算法在精度和波动情况上都比EKF算法好很多。并且从4个表格可以看出，当接收机误差越大的时候，EKF算法的定位误差相较于UKF算法越大，因为UKF算法的定位误差是不包括线性化误差的，所以可以认为EKF算法的线性化随着接收机误差增大而增大。

实验表明，对于伪卫星定位系统，如果接收机误差越大或者卫星距离接收机的距离越小，线性化误差越大，UKF算法定位效果与EKF算法相比越好。当接收机误差较大时，UKF算法的收敛速度和整体精度跟EKF算法相比有明显的提升。

本发明提出一种基于UKF算法的伪卫星室内定位新方法，能够分析得出线性化误差的影响因素，并且能够在伪卫星定位系统中接收机坐标误差相对较大时，使用UKF算法得到比EKF算法更快的收敛速度与更高的室内定位精度。

Claims (5)

1.一种基于UKF算法的伪卫星室内定位方法，其特征在于，该方法包括以下步骤：

(1)构建伪卫星双差伪距观测模型；

(2)采用UKF算法对伪卫星进行室内定位。

2.根据权利要求1所述的基于UKF算法的伪卫星室内定位方法，其特征在于，所述步骤(1)中，构建伪卫星双差伪距观测方程，包括：

(11)构建伪卫星伪距观测方程为：

其中，ρⁿ为接收机收到PRN号为n的伪卫星的伪距，r为接收机到所述伪卫星的实际距离，δt_u为接收机钟差，δtⁿ为所述伪卫星钟差，I为电离层延迟，T为对流层延迟，为测量噪声参量，所述接收机为用户接收机或基准接收机；

(12)根据所述伪卫星伪距观测方程，构造双差伪距观测方程：

双差伪距测量值定义为：

ρ^ij _uv＝(ρⁱ _u-ρⁱ _v)-(ρ^j _u-ρ^j _v)

双差几何距离定义为：

r^ij _uv＝(rⁱ _u-rⁱ _v)-(r^j _u-r^j _v)

其中，i和j是指不同的伪卫星，u和v分别是指用户接收机和基准接收机，为用户接收机到所述伪卫星i的实际距离，ρⁱ _u为用户接收机到所述伪卫星i的伪距。

3.根据权利要求2所述的基于UKF算法的伪卫星室内定位方法，其特征在于，所述步骤(2)中，采用UKF算法对伪卫星进行室内定位具体包括：

(21)根据伪卫星双差伪距观测模型设定状态方程和观测方程，所述状态方程为：

x_τ＝f(x_τ-1)+ω_τ

其中，x_τ和x_τ-1分别为τ和τ-1时刻的状态向量，即用户接收机坐标向量，ω_τ是状态噪声向量；

所述双差伪距测量值z_τ用观测方程h(x_τ)表示为：

z_τ＝h(x_τ)+v_τ

其中，v_τ是观测噪声向量；

(22)选定滤波初值：

其中，E()指数学期望，X₀指状态向量初值，指状态向量初值的均值，P₀指状态向量初值的协方差阵。

(23)计算sigma点：

其中，为上一个历元状态向量均值，为上一个历元第0个sigma状态向量值，P_k-1为上一个历元状态向量协方差阵，γ为比例参数，为上一个历元第i个sigma状态向量值，n是状态向量维数；λ＝a²(n+k)-n，其中，a是用于确定上一个历元状态向量均值周围sigma点的分布的正数；k为参数且k＝3-n；

(24)确定权值：

其中，和为第i个sigma点状态向量的权值，和为第i个sigma点状态向量的协方差阵权值，β为状态分布参数；

(25)时间更新：

其中，为上一个历元第i个sigma状态向量值，f()为两个历元间的状态传递函数，为第i个sigma预测状态向量值，为预测状态向量，P_k/k-1为预测状态向量的协方差矩阵，Q_k-1为上一个历元的状态向量噪声矩阵；

(26)测量更新：

其中，h()为观测方程，为预测观测向量，为预测向量和观测向量的协方差矩阵，R_k为观测噪声矩阵；

(27)滤波更新：

其中，为估计状态向量，为预测观测向量和状态向量的协方差阵，P_k为估计状态向量的协方差矩阵，K_k为增益矩阵。

4.根据权利要求2所述的基于UKF算法的伪卫星室内定位方法，其特征在于，所述步骤(1)还包括对线性化误差的影响因素进行分析，包括表示出伪卫星到用户接收机的几何距离：

可得出双差伪距方程的二阶残余项ε_dh有如下估计：

其中，分别为用户接收机坐标(x_u，y_u，z_u)的方差，为伪卫星i到用户接收机的几何距离，为伪卫星j到用户接收机的几何距离，u指用户接收机，i和j指不同的伪卫星。

5.根据权利要求4所述的基于UKF算法的伪卫星室内定位方法，其特征在于，所述线性化误差的影响因素为伪卫星与用户接收机的距离和用户接收机位置误差。