2.845

2023影响因子

(CJCR)

  • 中文核心
  • EI
  • 中国科技核心
  • Scopus
  • CSCD
  • 英国科学文摘

留言板

尊敬的读者、作者、审稿人, 关于本刊的投稿、审稿、编辑和出版的任何问题, 您可以本页添加留言。我们将尽快给您答复。谢谢您的支持!

姓名
邮箱
手机号码
标题
留言内容
验证码

面向RTK定位的整数约束型渐进高斯滤波方法

杨旭升 李唯诣 张文安

杨旭升, 李唯诣, 张文安. 面向RTK定位的整数约束型渐进高斯滤波方法. 自动化学报, 2025, 51(2): 366−375 doi: 10.16383/j.aas.c240384
引用本文: 杨旭升, 李唯诣, 张文安. 面向RTK定位的整数约束型渐进高斯滤波方法. 自动化学报, 2025, 51(2): 366−375 doi: 10.16383/j.aas.c240384
Yang Xu-Sheng, Li Wei-Yi, Zhang Wen-An. Integer-constrained progressive Gaussian filtering method for RTK positioning. Acta Automatica Sinica, 2025, 51(2): 366−375 doi: 10.16383/j.aas.c240384
Citation: Yang Xu-Sheng, Li Wei-Yi, Zhang Wen-An. Integer-constrained progressive Gaussian filtering method for RTK positioning. Acta Automatica Sinica, 2025, 51(2): 366−375 doi: 10.16383/j.aas.c240384

面向RTK定位的整数约束型渐进高斯滤波方法

doi: 10.16383/j.aas.c240384 cstr: 32138.14.j.aas.c240384
基金项目: 国家自然科学基金 (62473335), 浙江省自然科学基金 (LY23F030006), 中国博士后科学基金 (2024M752864) 资助
详细信息
    作者简介:

    杨旭升:浙江工业大学信息工程学院副教授. 主要研究方向为多源信息融合估计和目标定位. E-mail: xsyang@zjut.edu.cn

    李唯诣:浙江工业大学信息工程学院硕士研究生. 主要研究方向为RTK定位和信息融合估计. E-mail: lwy_2210@163.com

    张文安:浙江工业大学信息工程学院教授. 主要研究方向为多源信息融合估计和网络化系统. 本文通信作者. E-mail: wazhang@zjut.edu.cn

Integer-constrained Progressive Gaussian Filtering Method for RTK Positioning

Funds: Supported by National Natural Science Foundation of China (62473335), Natural Science Foundation of Zhejiang Province (LY23F030006), and China Postdoctoral Science Foundation (2024M752864)
More Information
    Author Bio:

    YANG Xu-Sheng Associate professor at the College of Information Engineering, Zhejiang University of Technology. His research interest covers multi-source information fusion estimation and target positioning

    LI Wei-Yi Master student at the College of Information Engineering, Zhejiang University of Technology. His research interest covers RTK positioning and information fusion estimation

    ZHANG Wen-An Professor at the College of Information Engineering, Zhejiang University of Technology. His research interest covers multi-source information fusion estimation and networked systems. Corresponding author of this paper

  • 摘要: 本文研究了卫星信号干扰下 RTK (Real-time kinematic)整周模糊度固定问题, 提出一种基于整数约束型渐进高斯滤波的 RTK 定位方法. 首先, 结合贝叶斯推理与同伦方法优势, 导出一种兼容整数、浮点状态的渐进高斯滤波框架. 其次, 构造从先验分布到后验分布的同伦路径, 以目标浮点状态与模糊度固定的迭代求解来提高信号干扰情形下的整周模糊度固定率. 特别地, 通过渐进地融合卫星双差信息来降低线性化误差, 进而提升对目标状态后验分布的逼近精度. 最后, 通过车载 RTK 实验及后处理分析, 验证了所提方法的有效性和优越性.
  • 作为一种 GNSS (Global navigation satellite system)高精度定位技术, RTK (Real-time kinematic)定位在测绘、农业和无人驾驶等领域具有巨大的发展潜力[13]. 通过差分处理来自基站和流动站的伪距和载波相位观测数据, RTK 技术能够有效地降低大气延迟误差、钟差等卫星系统误差, 从而提高定位精度[45]. 载波相位观测的整周模糊度固定是实现 GNSS 高精度定位的重要前提和基础. 然而, 由于多径效应、信号遮挡以及卫星几何分布等因素对观测质量和冗余性具有影响[67], 这将增加模糊度固定的难度同时降低定位准确度.

    针对 GNSS 模糊度固定问题, 以最小二乘模糊度降相关平差 (Least-square ambiguity decorrelation adjustment, LAMBDA)算法应用最为广泛[8]. 在模糊度残差平方和最小准则下, 该算法利用整数高斯变换将模糊度协方差矩阵进行去相关, 同时利用对角元素排序以构造更优的搜索空间, 这样极大地降低了模糊度搜索时间. 在此基础上, 文献[9] 通过引入选主元法、贪心算法等, 进一步提高了计算速度和去相关效能. 值得注意的是, 最小二乘法对动态变化适应性差且对噪声和异常值等较为敏感. 为此, 文献[10] 等提出利用卡尔曼滤波 (Kalman filter, KF)等方法解算双差量测以获取模糊度浮点解, 通过 LAMBDA 算法还原载波相位的整周特性, 从而得到整周模糊度固定解. 然而, 考虑 LAMBDA 算法对初值选取较为敏感, 同时, 受城市峡谷、高架桥等复杂环境影响, 信号遮挡和多径效应等因素使得卫星信号强度分布不均匀, 进而引起测量噪声增大. 若未能及时补偿该量测不确定性, 将严重影响滤波精度和稳定性[11], 使得模糊度浮点解难以收敛到模糊度固定解[12].

    另一方面, 对于 RTK 定位中模糊度浮点解的求解问题, 已有一系列非线性滤波方法被相继提出[1315]. 扩展卡尔曼滤波器 (Extended Kalman filter, EKF)凭借其较高的计算效率与实现的简便性, 已成为 GNSS 定位领域内广泛应用的滤波技术之一[16]. 然而, 受非视距等因素的影响, 复杂环境下 GNSS 信号的衰减问题将导致量测不确定性增加; 此外, 根据泰勒级数展开原理, 不难发现, 由忽略高阶项引起的截断误差将间接地随量测不确定性增大而增大. 为克服上述问题, 文献[17] 采用矢量跟踪环路 (Vector-tracking loop, VTL)模型, 结合迭代扩展卡尔曼滤波 (Iterative extended Kalman filter, IEKF), 提高了滤波性能. 文献[1819] 基于变分贝叶斯推理方法, 提出具有检测异常值功能的鲁棒扩展卡尔曼滤波 (Robust extended Kalman filter, REKF), 该方法虽有效提高了在多径效应影响下的滤波性能, 但同时也损失了整体的模糊度固定率. 文献[20] 基于载波噪声密度比和标准化残差对不同卫星量测进行重加权, 设计一种鲁棒滤波方法, 有效地减弱了多径效应和量测粗差对定位系统带来的影响. 不同于上述鲁棒方法, 文献[21] 基于贝叶斯框架提出一种渐进高斯滤波方法, 通过逐步地提取量测信息来补偿量测不确定性, 在一定程度上解决了量测信息缺失造成的估计误差增大问题. 文献[2223] 进一步提出一种自适应渐进高斯滤波方法, 通过收敛性分析给出渐进量测更新的截止条件, 有效避免了过估计问题. 在此基础上, 针对量测信息中包含的复杂噪声, 文献[24] 分别从空间、时间维度对量测进行相容性分析与分类处理并引入渐进量测更新与引导机制, 隐式地补偿量测不确定性. 然而, 上述方法虽然都在一定程度上提高了滤波器的鲁棒性, 但 RTK 定位系统中浮点状态估计和模糊度固定具有较强的耦合性, 使得当前滤波方法难以适用兼容整数、浮点状态的估计问题.

    针对以上问题, 本文提出一种面向 RTK 定位的整数约束型渐进高斯滤波方法. 首先, 基于贝叶斯推理导出整数约束下的渐进高斯滤波框架. 其次, 通过引入伪时间常数, 渐进地融合双差量测信息, 以减少线性化误差带来的不良影响. 特别地, 利用渐进量测更新和模糊度固定的迭代执行, 来提高在信号干扰下的模糊度固定率. 最后, 通过车载实验采集实时卫星数据并进行后处理分析, 验证了所提算法的有效性和优越性.

    考虑一类 GNSS 信号干扰下的 RTK 定位问题. 如图1所示, 在本文考虑的 RTK 定位过程中, 将由高斯滤波器从原始差分卫星观测中解算出模糊度浮点解, 并通过 LAMBDA 算法进一步获取固定解. 如图2所示, 当 GNSS 信号受环境遮挡、多径效应等影响时, 可用卫星数的减少以及伪距的测量误差都将引起量测噪声的显著增大, 导致传统一步量测更新难以获得精确的状态后验分布. 为此, 将构建先验分布到后验分布的同伦路径, 以多个渐进量测更新引导模糊度固定, 提升 RTK 定位的鲁棒性与准确性.

    图 1  基于高斯滤波的 RTK 定位框图
    Fig. 1  Diagram of RTK positioning based on Gaussian filter
    图 2  RTK 定位示意图
    Fig. 2  Schematic diagram of RTK positioning

    以第$ {i} $颗卫星和接收机$ {r} $为例, 伪距和载波相位的原始观测方程可表示如下:

    $$ P_r^i=\rho_r^i+c\times dt_r-c\times dt^i+T_r^i+I_r^i+\varepsilon_P $$ (1)
    $$ \Phi_r^i=\rho_r^i+c\times dt_r-c\times dt^i+T_r^i+I_r^i+\lambda a+\varepsilon_{\Phi} $$ (2)

    其中, $ {P} $和$ {{{\Phi}}} $分别表示伪距观测和载波相位观测, 其上下标分别表示卫星编号和接收机编号; $ \rho _{r}^{i}= \| {{r}^{i}}-{{r}_{r}} \| $表示星地间欧氏距离, $ {{r}^{i}} $和$ {{r}_{r}} $分别表示卫星和接收机的三维坐标向量; $ c $表示光速; $ d{{t}_{r}} $和$ d{{t}^{i}} $分别为接收机和卫星的钟差; $ T_{r}^{i} $表示星地间对流层延迟误差; $ I_{r}^{i} $表示星地间电离层延迟误差; $ \lambda $和$ a $分别表示载波相位观测的波长和模糊度; $ {{\varepsilon }_{P}} $和$ {{\varepsilon }_{{{\Phi}}}} $表示多径效应和接收机测量噪声等引起的误差项, 建模为高斯白噪声.

    对基站$ b $和接收机$ r $, 选取卫星$ i $作为参考卫星, 通过站间单差消除卫星钟差以及对流层、电离层延迟误差, 进一步利用星间双差以消除接收机钟差, 可得双差量测方程如下:

    $$ {P_{rb}^{ij}}={\rho _{rb}^{ij}+{{\varepsilon }_{P,\; {{\Delta }^{2}}}}} $$ (3)
    $$ {{{{\Phi}}} _{rb}^{ij}}={\rho _{rb}^{ij}+\lambda (a_{rb}^{i}-a_{rb}^{j})+{{\varepsilon }_{{{{\Phi}}},\; {{\Delta }^{2}}}}} $$ (4)

    其中, $ P_{rb}^{ij} $和$ {{{{\Phi}}} _{rb}^{ij}} $分别表示伪距和载波相位双差量测; $\rho_{rb}^{ij} $表示双差处理后的星地间欧氏距离; $ {a_{rb}^{i}} $和$ {a_{rb}^{j}} $表示单差模糊度; $ {{{\varepsilon }_{P,\; {{\Delta }^{2}}}}} $和$ {{{\varepsilon }_{{{{\Phi}}},\; {{\Delta }^{2}}}}} $表示经双差处理后的未建模误差. 假设$ {k} $时刻共观测到$ {m} $颗卫星, 以卫星$ {i} $为参考卫星, 系统模型描述如下:

    $$ \left\{ \begin{aligned} &{{x}_{k}}={{F}_{k|k-1}}{{x}_{k-1}}+{{w}_{k}} \\ &{{z}_{k}}=h({{x}_{k}})+{{v}_{k}} \end{aligned} \right. $$ (5)

    其中, $ {{x}_{k}}=[{{r}_{k}}\;\; {{a}_{k}} ]^{\text T} $为$ k $时刻的系统状态向量, $ {{r}_{k}} $为载体位置的行向量, $ {{a}_{k}} $为站间单差模糊度的行向量; 状态转移矩阵$ {{{F}_{k|k-1}}=I} $, $ I $为单位矩阵; 过程噪声$ {{w}_{k}} $服从均值为零, 协方差为$ {{{Q}_{k}}}={\text {diag}\{ {{Q}_{r}} \;\; {{Q}_{a}} \}} $的高斯分布. 其中, $ {{Q}_{r}} $表示位置参数噪声矩阵; $ {{Q}_{a}} $表示单差模糊度噪声矩阵.

    $$ \begin{array}{*{20}{l}} {{{z}_{k}}}={{{\left[ \begin{array}{*{35}{l}} P_{rb}^{i2} & \cdots & P_{rb}^{im} & {{\Phi}}_{rb}^{i2} & \cdots & {{\Phi}}_{rb}^{im} \\ \end{array} \right]}^{\text T}}} \end{array} $$ (6)

    $ {z_k} $为双差量测向量; $ h({{x}_{k}})=[ {{h}_{P}}({{x}_{k}})\;\;{{h}_{{{\Phi}} }}({{x}_{k}})]^{\text T} $为非线性量测函数, 且:

    $$ \left\{\begin{aligned} & {{h}_{P}}({{x}_{k}})=\left[ \begin{matrix} \rho _{rb}^{i2} & \cdots & \rho _{rb}^{im} \end{matrix} \right]^{\text T} \\ &{{h}_{{{\Phi}} }}({{x}_{k}})={{\left[ \begin{matrix} \rho _{rb}^{i2}+\lambda (a_{rb}^{i}-a_{rb}^{2}) \\ \vdots \\ \rho _{rb}^{im}+\lambda (a_{rb}^{i}-a_{rb}^{m}) \end{matrix} \right]}} \end{aligned}\right. $$ (7)

    $ {{v}_{k}} $表示量测噪声, 服从均值为零、协方差为$ {{R}_{k}} $的高斯分布, 且与过程噪声$ {{w}_{k}} $互不相关. 其中, $ {{{R}_{k}}}= {\text{diag}\{{{R}_{P}}\;\; {{R}_{{{\Phi}} }} \}} $, $ {{R}_{P}} $和$ {{R}_{{{\Phi}}}} $分别表示伪距量测和载波相位量测的噪声矩阵.

    注 1. 考虑到 GNSS 信号干扰情况下可能存在参考卫星变化的情况, 此时双差模糊度的参考卫星也将随之变化, 为简化计算, 选取单差模糊度为状态向量. 在执行 LAMBDA 算法前, 利用转换矩阵$ G $将状态向量中的单差模糊度转换为双差模糊度, 该矩阵将在第2节做详细介绍.

    考虑到双差量测数量不足引起的浮点解估计精度下降问题, 将导致模糊度解算的成功率下降. 尤其是, 模糊度参数的高度相关性将进一步增加 RTK 定位对卫星信号缺失及外部干扰的敏感度. 对此, 将建立先验分布−后验分布的渐进过程, 根据贝叶斯推理, 引入伪时间常数$ {\tau} $, 并给出整数约束型渐进高斯滤波框架如下:

    $$ \left\{\begin{aligned} & p(x_k|Z_{1:k-1})=\int_{ }^{ }p(x_k|x_{k-1})p(x_{k-1}|Z_{1:k-1})\mathrm{d}x_k \\ & p(x_k,\; \tau_i|Z_{1:k})=\frac{p(z_k,\; \Delta_i|x_k)p(x_k,\; \tau_{i-1}|Z_{1:k-1})}{\int_{ }^{ }p(z_k,\; \Delta_i|x_k)p(x_k,\; \tau_i|Z_{1:k-1})\mathrm{d}x_k} \\ & p(r_k,\; \tau_i|a_k,\; Z_{1:k})=\frac{p(x_k,\; \tau_i|Z_{1:k})}{p(a_k,\; \tau_i|Z_{1:k})}\end{aligned}\right. $$ (8)

    该框架包含时间更新、渐进量测更新和渐进模糊度更新过程. 式 (8) 中, $ p(\cdot) $表示概率密度函数; $ {{{Z}_{1: k}}}={\{{{z}_{1}},\; {{z}_{2}},\; \cdots,\; {{z}_{k}}\}} $为时间 $ k $ 处的量测集合; $ \tau_i\ (i\in[1,\; N]) $为伪时间常数且$ {{\tau }_{0}}={0} $, $ {{\tau }_{N}}\;= {\sum\nolimits_{i=1}^{N}{{{\Delta }_{i}}}}=1 $. $ {{\Delta }_{i}}=1/N $ 为第$ i $步渐进步长, $ N $为渐进总步数.

    注2. 在渐进高斯滤波中, 较大的$ N $值能够带来更精确的状态估计; 较小的$ N $值, 虽能够提高系统计算效率, 但会相应地损失部分滤波精度. 因此, 在实际应用中, $ N $的最佳取值通常需要基于实验来权衡和调整. 本文中$ N $的取值将在实验部分给出.

    $ {p({{z}_{k}},\; {{\Delta }_{i}}|{{x}_{k}})} $为渐进似然函数, 将其定义为:

    $$ \begin{array}{*{20}{l}} {p({{z}_{k}},\; {{\Delta }_{i}}|{{x}_{k}}):}={{{p}^{{{\Delta }_{i}}}}({{z}_{k}}|{{x}_{k}})} \end{array} $$ (9)

    进而, 可建立递推关系如下:

    $$ \begin{split} &{p({{x}_{k}},\; {{\tau }_{i}}|{{Z}_{1: k}})}=\\ &\quad{{{C}_{k}}\left( {{\tau }_{i-1}} \right){{p}^{{{\Delta }_{i}}}}({{z}_{k}}|{{x}_{k}})p({{x}_{k}},\; {{\tau }_{i-1}}|{{Z}_{1: k-1}})} \end{split} $$ (10)

    其中,

    $$ \begin{split} & C_k\left(\tau_{i-1}\right)= \\ & \quad\left(\int_{ }^{ }p(z_k,\; \Delta_i|x_k)p(x_k,\; \tau_{i-1}|Z_{1:k-1})\mathrm{d}x_k\right)^{-1}\end{split} $$ (11)

    伪时刻$ {{\tau }_{i}} $处的渐进后验概率密度函数为:

    $$ \begin{split} & p({{x}_{k}},\; {{\tau }_{i}}|{{Z}_{1: k}})= \\ & \quad\prod\limits_{j=1}^{i}{{{C}_{k}}({{\tau }_{j-1}})}\prod\limits_{j=1}^{i}{{{p}^{{{\Delta }_{j}}}}({{z}_{k}}|{{x}_{k}})p({{x}_{k}},\; {{\tau }_{0}}|{{Z}_{1: k-1}})}= \\ & \quad C_{k}^{i}{{[p({{z}_{k}}|{{x}_{k}})]}^{{{\tau }_{i}}}}p({{x}_{k}}|{{Z}_{1: k-1}})\\[-1pt] \end{split} $$ (12)

    其中, $ C_{k}^{i}=\prod\nolimits_{j=1}^{i}{{{C}_{k}}({{\tau }_{j-1}})} $. 由此建立了从先验分布到后验分布的渐进过程. 相应地, 伪时刻$ {{\tau }_{i}} $处系统位置更新过程表示如下:

    $$ \begin{split} & p({{r}_{k}},\; {{\tau }_{i}}|{{a}_{k}},\; {{Z}_{1: k}})=\frac{p({{x}_{k}},\; {{\tau }_{i}}|{{Z}_{1: k}})}{p({{a}_{k}},\; {{\tau }_{i}}|{{Z}_{1: k}})}= \\ &\quad \frac{{{C}_{k}}{{[p({{z}_{k}}|{{x}_{k}})]}^{{{\tau }_{i}}}}p({{x}_{k}}|{{Z}_{1: k-1}})}{p({{a}_{k}},\; {{\tau }_{i}}|{{Z}_{1: k}})} \end{split} $$ (13)

    其中, $ {{a}_{k}} $表示模糊度参数; $ p({{a}_{k}},\; {{\tau }_{i}}|{{Z}_{1: k}}) $表示其渐进归一化分布, 通过 LAMBDA 算法获取.

    注3. 如图3所示, 为实现模糊度固定对渐进量测更新过程的引导, 本文引入伪时间常数$ \tau $, 将系统量测更新分解为多个渐进量测更新, 利用整数模糊度作为参照来引导渐进量测更新的执行或截止, 实现伪时长的合理选取以补偿量测不确定性. 此外, 通过多个渐进后验估计解来迭代求解整数模糊度. 这样, 允许渐进量测更新与模糊度固定过程的迭代执行来逐步逼近真实后验估计. 这种迭代策略有效地提高了渐进后验估计和模糊度参数之间的信息反馈和利用, 从而提升整体定位系统的性能.

    图 3  整数约束型渐进高斯滤波框架
    Fig. 3  Integer-constrained progressive Gaussian filtering framework

    假设伪时刻$ {{\tau }_{i-1}} $处的渐进后验概率密度函数为$ p({{x}_{k}},\; {{\tau }_{i-1}}|{{Z}_{1: k}}) $且服从高斯分布, 则有:

    $$ \begin{array}{*{20}{l}} {p({{x}_{k}},\; {{\tau }_{i-1}}|{{Z}_{1: k}})}\sim{\text{N}({{x}_{k}};\hat{x}_{k|k}^{{{\tau }_{i-1}}},\; P_{k|k}^{{{\tau }_{i-1}}})} \end{array} $$ (14)

    式中$ \hat{x}_{k|k}^{{{\tau }_{i-1}}} $表示伪时刻$ {{\tau }_{i-1}} $的渐进后验估计; $ P_{k|k}^{{{\tau }_{i-1}}} $为其相应的协方差矩阵, 利用转换矩阵$ G $将状态向量中单差模糊度参数转换为双差模糊度参数, 可得:

    $$ \begin{array}{*{20}{l}} {x_{k|k,\; {{\Delta }^{2}}}^{{{\tau }_{i-1}}}=Gx_{k|k}^{{{\tau }_{i-1}}}={{\left[ \begin{matrix} r_{k|k}^{{{\tau }_{i-1}}} & a_{k|k,\; {{\Delta }^{2}}}^{{{\tau }_{i-1}}} \\ \end{matrix} \right]}^{\text T}}} \end{array} $$ (15)

    其中, $ x_{k|k,\; {{\Delta }^{2}}}^{{{\tau }_{i-1}}} $表示转换后的状态向量; $ r_{k|k}^{{{\tau }_{i-1}}} $表示位置向量; $ a_{k|k,\; {{\Delta }^{2}}}^{{{\tau }_{i-1}}} $表示双差模糊度向量; 矩阵$ G $为单差到双差的转换矩阵, 表示如下:

    $$ \begin{array}{*{20}{l}} {G=\left[ \begin{matrix} {{I}_{3\times 3}} & 0 \\ 0 & D \\ \end{matrix} \right],\; D=\left[ \begin{array}{*{35}{c}} 1 & -1 & 0 & \cdots & 0 \\ 1 & 0 & -1 & \cdots & 0 \\ \vdots & \vdots & \vdots & \ddots & \vdots \\ 1 & 0 & 0 & \cdots & -1 \\ \end{array} \right]} \end{array} $$ (16)

    则状态$ x_{k|k,\; {{\Delta }^{2}}}^{{{\tau }_{i-1}}} $对应的协方差矩阵表示为:

    $$ \begin{split} \Lambda _{k|k}^{{{\tau }_{i-1}}}=\;&GP_{k|k}^{{{\tau }_{i-1}}}{{G}^{\text T}}= \\ & \left[ \begin{matrix} P_{k|k}^{rr,\; {{\tau }_{i-1}}} & P_{k|k}^{ra,\; {{\tau }_{i-1}}}{{D}^{\text T}} \\ DP_{k|k}^{ra,\; {{\tau }_{i-1}}} & DP_{k|k}^{aa,\; {{\tau }_{i-1}}}{{D}^{\text T}} \end{matrix} \right]= \\ & \left[ \begin{matrix} P_{k|k}^{rr,\; {{\tau }_{i-1}}} & \Lambda _{k|k}^{ra,\; {{\tau }_{i-1}}} \\ \Lambda _{k|k}^{ra,\; {{\tau }_{i-1}}} & \Lambda _{k|k}^{aa,\; {{\tau }_{i-1}}} \end{matrix} \right] \end{split} $$ (17)

    假设浮点模糊度服从高斯分布, 那么将位置与模糊度的联合概率密度函数表示如下:

    $$ \begin{split} & p(r_k,\; a_k,\; \tau_{i-1}|Z_{1:k-1})\sim \\ & \quad\mathrm{N}\left(\begin{matrix}\left[\begin{matrix}r_k \\ a_k\end{matrix}\right]; & \left[\begin{matrix}r_{k|k}^{\tau_{i-1}} \\ a_{k|k,\; \Delta^2}^{\tau_{i-1}}\end{matrix}\right],\; \left[\begin{matrix}P_{k|k}^{rr,\; \tau_{i-1}} & \Lambda_{k|k}^{ra,\; \tau_{i-1}} \\ \Lambda_{k|k}^{ra,\; \tau_{i-1}} & \Lambda_{k|k}^{aa,\; \tau_{i-1}}\end{matrix}\right]\end{matrix}\right)\end{split} $$ (18)

    由此, 如图3所示, 利用渐进后验概率密度函数$ p({{x}_{k}},\; {{\tau }_{i-1}}|{{Z}_{1:k}}) $引导当前伪时刻的模糊度固定, 通过优化以下目标函数获取整数模糊度:

    $$ \begin{split} &\arg \min \left\| a_{k|k,\; {{\Delta }^{2}},\; AR}^{{{\tau }_{i-1}}}-a_{k|k,\; {{\Delta }^{2}}}^{{{\tau }_{i-1}}} \right\|_{\Lambda _{k|k}^{aa,\; {{\tau }_{i-1}}}}^{2},\;\\ &\qquad a_{k|k,\; {{\Delta }^{2}},\; AR}^{{{\tau }_{i-1}}}\in {{\mathbf{Z}}^{n}} \end{split} $$ (19)

    其中, $ a_{k|k,\; {{\Delta }^{2}},\; AR}^{{{\tau }_{i-1}}} $表示待求解的双差整数模糊度向量; $ {\mathbf{Z}}^{n} $表示整数向量空间. 该优化问题可通过 LAMBDA 算法求解, 即通过一系列高斯整数变换求取合适的幺模矩阵$ {{U}^{{{\tau }_{i-1}}}} $, 对协方差矩阵$ \Lambda _{k|k}^{aa,\; {{\tau }_{i-1}}} $作如下降相关处理:

    $$ \begin{align} \hat{z}_{k|k}^{{{\tau }_{i-1}}}={{({{U}^{{{\tau }_{i-1}}}})}^{\text T}}&\hat{a}_{k|k,\; {{\Delta }^{2}}}^{{{\tau }_{i-1}}} \end{align} $$ (20)
    $$ \begin{align} \Lambda _{k|k}^{zz,\; {{\tau }_{i-1}}}={{({{U}^{{{\tau }_{i-1}}}})}^{\text T}}&\Lambda _{k|k}^{aa,\; {{\tau }_{i-1}}}{{U}^{{{\tau }_{i-1}}}} \end{align} $$ (21)

    使搜索椭球按照有利于搜索的方向转换, 从而减少搜索空间内的整数节点数, 通过深度优先搜索方法获取一系列整数解, 并采取 Ratio 检验, 计算最优整数解与次优整数解的残差比值, 将其与预设阈值进行比较, 获取最优整数解向量[7, 25]. 因此, 待优化目标函数转化为:

    $$ \begin{array}{*{20}{l}} \arg \min \left\| z_{k|k}^{{{\tau }_{i-1}}}-\hat{z}_{k|k}^{{{\tau }_{i-1}}} \right\|_{\Lambda _{k|k}^{zz,\; {{\tau }_{i-1}}}}^{2} \end{array} $$ (22)

    由此, 对于每一伪时刻, 以中间后验分布为引导, 可获取不同的整数搜索空间$ \Lambda _{k|k}^{zz,\; {{\tau }_{i-1}}} $, 通过每一伪时刻的迭代更新, 逐步逼近最优整数模糊度. 获取最优解$ z_{k|k}^{{{\tau }_{i-1}}} $后, 由逆变换得整数模糊度$ a_{k,\; {{\Delta }^{2}},\; AR}^{{{\tau }_{i-1}}} $. 将 LAMBDA 算法记作下式:

    $$ a_{k|k,\; \Delta^2,\; AR}^{\tau_{i-1}}=\mathrm{LAMBDA}\left(\begin{matrix}a_{k|k,\; \Delta^2}^{\tau_{i-1}} & \Lambda_{k|k}^{aa,\; \tau_{i-1}} \end{matrix}\right) $$ (23)

    则相应的单差整数模糊度向量可表示为$ a_{k|k,\; AR}^{{{\tau }_{i-1}}} $. 通过图3所示迭代更新策略, 以整周模糊度$ a_{k|k,\; AR}^{{{\tau }_{i-1}}} $修正当前时刻的状态, 并引导下一时刻估计过程, 进而降低异常信息对系统状态的影响.

    考虑系统(5), 伪时刻$ {{\tau }_{i}} $处的渐进似然函数可由下式给出:

    $$ \begin{split} & {{p}^{{{\Delta }_{i}}}}({{z}_{k}}|{{x}_{k}})=\frac{1}{\sqrt{\left| 2\pi R_{k}^{{{\tau }_{i}}} \right|}}\; \times \\ &\quad \exp \left[ \frac{1}{2}{{({{z}_{k}}-h({{x}_{k}}))}^{\text T}}{{(R_{k}^{{{\tau }_{i}}})}^{-1}}({{z}_{k}}-h({{x}_{k}})) \right] \end{split} $$ (24)

    其中, $ R_{k}^{{{\tau }_{i}}}={{R}_{k}}/{{\Delta }_{i}} $. 假定先验概率密度函数都是服从高斯分布的, 则有:

    $$ \begin{array}{*{20}{l}} {p({{x}_{k}}|{{Z}_{1:k-1}})}\sim{\text{N}\left( {{x}_{k}};{{{\hat{x}}}_{k|k-1}},\; {{P}_{k|k-1}} \right)} \end{array} $$ (25)

    其中, $ k $时刻的状态预测及协方差矩阵由下式给出:

    $$ \begin{array}{*{20}{l}} {{{\hat{x}}_{k|k-1}}}={{{F}_{k|k-1}}{{x}_{k-1|k-1}}+{{w}_{k}}} \end{array} $$ (26)
    $$ \begin{array}{*{20}{l}} {{{P}_{k|k-1}}}={{{F}_{k|k-1}}{{x}_{k-1|k-1}}F_{k|k-1}^{\text T}+{{Q}_{k}}} \end{array} $$ (27)

    类似地, 假设系统状态与量测的联合渐进概率密度函数服从高斯分布, 即:

    $$ {p({{x}_{k}},\; {{z}_{k}},\; {{\tau }_{i}}|{{Z}_{1: k-1}})\sim \text{N}\left( \begin{matrix} \left[ \begin{matrix} {{x}_{k}} \\ {{z}_{k}} \\ \end{matrix} \right]; {{m}_{k}},\; {{\sum }_{k}} \\ \end{matrix} \right)} $$ (28)

    其中, 均值 $ {{m}_{k}}\;=\;[\hat{x}_{k|k,\; AR}^{{{\tau }_{i-1}}},\; \hat{z}_{k|k}^{{{\tau }_{i}}}] $, 协方差 $ {{\sum }_{k}}\;= $ $\left[\begin{matrix} P_{k|k,\; AR}^{{{\tau }_{i-1}}} & P_{k|k}^{xz,\; {{\tau }_{i}}} \\ {{(P_{k|k}^{xz,\; {{\tau }_{i}}})}^{\text T}} & P_{k|k}^{zz,\; {{\tau }_{i}}} \end{matrix} \right] $. 相应地, 可以推断后验概率密度函数服从高斯分布$ \text{N}({{x}_{k}};\hat{x}_{k|k}^{{{\tau }_{i}}},\; P_{k|k}^{{{\tau }_{i}}}) $, 考虑系统 (5), 将非线性量测$ {{z}_{k}} $在$ \hat{x}_{k|k}^{{{\tau }_{i}}} $ 处泰勒展开:

    $$ \begin{split}z_k=\; & h(\hat{x}_{k|k}^{\tau_i})+\nabla h(\hat{x}_{k|k}^{\tau_i})\tilde{x}_{k|k}^{\tau_i}\ + \\ & \frac{1}{2}\nabla^2h(\hat{x}_{k|k}^{\tau_i})(\tilde{x}_{k|k}^{\tau_i})^2+\cdots+v_k\end{split} $$ (29)

    其中$ \tilde{x}_{k|k}^{{{\tau }_{i}}} $表示估计误差, $ \tilde{x}_{k|k}^{{{\tau }_{i}}}={{x}_{k}}-\hat{x}_{k|k}^{{{\tau }_{i}}} $, $ \nabla $表示对非线性函数求导, 忽略高阶项可得:

    $$ \begin{array}{*{20}{l}} {\gamma _{k|k}^{{{\tau }_{i}}}}={\nabla h(\hat{x}_{k|k}^{{{\tau }_{i}}})\tilde{x}_{k|k}^{{{\tau }_{i}}}+{{v}_{k}}} \end{array} $$ (30)

    记新息$ \gamma _{k|k}^{{{\tau }_{i}}}={{z}_{k}}-h(\hat{x}_{k|k}^{{{\tau }_{i}}}) $, $ \nabla h(\hat{x}_{k|k}^{{{\tau }_{i}}})=H_{k}^{{{\tau }_{i}}} $. 由此, 将伪时刻$ {{\tau }_{i}} $处渐进似然重写为:

    $$ \begin{split}\;& {{p}^{{{\Delta }_{i}}}}({{z}_{k}}|{{x}_{k}})={{p}_{{{v}_{k}}}}(\gamma _{k|k}^{{{\tau }_{i}}}-{{H}_{k}^{{{\tau }_{i}}}}\tilde{x}_{k|k}^{{{\tau }_{i}}})= \\ &\qquad \frac{1}{\sqrt{\left| 2\pi R_{k}^{{{\tau }_{i}}} \right|}}\times \exp \Bigg[ -\frac{1}{2}{{(\gamma _{k|k}^{{{\tau }_{i}}}-{{H}_{k}^{{{\tau }_{i}}}}\tilde{x}_{k|k}^{{{\tau }_{i}}})}^{\text{T}}}\\ & \qquad{{(R_{k}^{{{\tau }_{i}}})}^{-1}}(\gamma _{k|k}^{{{\tau }_{i}}}-{{H}_{k}^{{{\tau }_{i}}}}\tilde{x}_{k|k}^{{{\tau }_{i}}}) \Bigg] \\[-1pt]\end{split} $$ (31)

    定理 1. 考虑系统 (5), 若伪时刻$ {{\tau }_{i}} $处渐进似然函数如式 (32) 所示, 假设$ p({{a}_{k,\; {{\Delta }^{2}}}},\; {{\tau }_{i}}|{{Z}_{1:k}}) $已由 LAMBDA 算法得出, 且服从高斯分布如下:

    $$ \begin{array}{*{20}{l}} {p({{a}_{k,\; {{\Delta }^{2}}}},\; {{\tau }_{i}}|{{Z}_{1:k}})}\sim{\text{N}({{a}_{k,\; {{\Delta }^{2}}}};\hat{a}_{k|k,\; {{\Delta }^{2}},\; AR}^{{{\tau }_{i}}},\; \Lambda _{k|k}^{aa,\; {{\tau }_{i}}})} \end{array} $$ (32)

    则系统位置更新过程由下式给出:

    $$ \begin{split} {r_{k|k,\; AR}^{{{\tau }_{i}}}}=\;&\hat{r}_{k|k}^{{{\tau }_{i}}}+\Lambda _{k|k}^{ra,\; {{\tau }_{i}}}{{(\Lambda _{k|k}^{aa,\; {{\tau }_{i}}})}^{-1}}\times\\ &(\hat{a}_{k|k,\; {{\Delta }^{2}},\; AR}^{{{\tau }_{i}}}-\hat{a}_{k|k,\; {{\Delta }^{2}}}^{{{\tau }_{i}}}) \end{split} $$ (33)
    $$ \begin{array}{*{20}{l}} {P_{k|k,\; AR}^{rr,\; {{\tau }_{i}}}}={P_{k|k}^{rr,\; {{\tau }_{i}}}-\Lambda _{k|k}^{ra,\; {{\tau }_{i}}}{{(\Lambda _{k|k}^{aa,\; {{\tau }_{i}}})}^{-1}}{{(\Lambda _{k|k}^{ra,\; {{\tau }_{i}}})}^{\text T}}} \end{array} $$ (34)

    其中, $ \hat{r}_{k|k}^{{{\tau }_{i}}} $和$ \hat{a}_{k|k,\; {{\Delta }^{2}}}^{{{\tau }_{i}}} $为伪时刻$ {{\tau }_{i}} $处的状态估计$ \hat{x}_{k|k}^{{{\tau }_{i}}} $的分量, 通过下式得出

    $$ {\hat{x}_{k|k}^{{{\tau }_{i}}}}={P_{k|k}^{{{\tau }_{i}}}\sum\limits_{j=1}^{i}{{{(H_{k}^{{{\tau }_{j}}})}^{\text T}}{{(R_{k}^{{{\tau }_{j}}})}^{-1}}\gamma _{k|k}^{{{\tau }_{j}}}}+{{\hat{x}}_{k|k-1}}} $$ (35)
    $$ {P_{k|k}^{{{\tau }_{i}}}}={{{\left[ \sum\limits_{j=1}^{i}{{{(H_{k}^{{{\tau }_{j}}})}^{\text T}}{{(R_{k}^{{{\tau }_{j}}})}^{-1}}H_{k}^{{{\tau }_{j}}}}+{{({{P}_{k|k-1}})}^{-1}} \right]}^{-1}}} $$ (36)

    证明见附录A.

    注4. 若模糊度成功固定, 迭代过程即可终止, 即满足$ {{[\hat{a}_{k|k,\; AR}^{{{\tau }_{i}}} ]}={\hat{a}_{k|k,\; AR}^{{{\tau }_{i}}}}} $则停止迭代, 并输出结果. 其中$ \left[\cdot \right] $表示取整过程. 然而, 若模糊度一直无法被固定, 迭代过程将会不断执行直至步长走完, 在此过程中不断变小的协方差所带来的系统线性化误差与粗差更易破坏系统的稳定性. 因此, 为防止系统过估计, 引入截止条件$ {\| \tilde{x}_{k|k}^{{{\tau }_{i}}}-\tilde{x}_{k|k}^{{{\tau }_{i-1}}} \|}<{\varepsilon } $和$ {{{(\gamma _{k|k}^{{{\tau }_{i-1}}})}^{\text T}}\gamma _{k|k}^{{{\tau }_{i-1}}}}<{{{(\gamma _{k|k}^{{{\tau }_{i}}})}^{\text T}}\gamma _{k|k}^{{{\tau }_{i}}}} $. $ {\varepsilon } $表示一个值很小的常数, 当该条件成立时, $ {\gamma _{k|k}^{{{\tau }_{i}}}} $趋近于 0 或渐进滤波增益趋近于 0. 截止条件$ {{{{(\gamma _{k|k}^{{{\tau }_{i-1}}})}^{\text T}}\gamma _{k|k}^{{{\tau }_{i-1}}}}<{{{(\gamma _{k|k}^{{{\tau }_{i}}})}^{\text T}}\gamma _{k|k}^{{{\tau }_{i}}}}} $的合理性已在文献 [22] 给出.

    最后, 给出本文所提算法的具体流程:

      算法1. 整数约束型渐进高斯滤波方法

    1: 初始化;

    2: while true do

    3:  利用式 (26) ~ (27) 进行时间更新;

    4:  for $i=1$ to $N$ do

    5:   利用式 (35) ~ (36) 进行渐进量测更新;

    6:   if ${\| \tilde{x}_{k|k}^{{{\tau }_{i}}}\;-\;\tilde{x}_{k|k}^{{{\tau }_{i-1}}} \|}\;<\;{\varepsilon}$ and ${{{(\gamma _{k|k}^{{{\tau }_{i-1}}})}^{\text T}}\gamma _{k|k}^{{{\tau }_{i-1}}}}\;< {{{(\gamma _{k|k}^{{{\tau }_{i}}})}^{\text T}}\gamma _{k|k}^{{{\tau }_{i}}}}$ then

    7:    break

    8:   end if

    9:   利用式 (23) 固定模糊度;

    10:   if $[\hat{a}_{k|k,\; AR}^{{{\tau }_{i}}} ]=\hat{a}_{k|k,\; AR}^{{{\tau }_{i}}}$ then

    11:    更新式 (33) ~ (34);

    12:    break

    13:   end if

    14:  end for

    15: end while

    为进一步验证所提出算法的有效性, 在开阔环境路段以及高架桥路段进行车辆测试, 并对实验结果进行分析. 为便于后文分析, 分别定义均方根误差 (Root mean square error, RMSE)和 Ratio 为:

    $$ {\mathrm{RMSE}}=\sqrt{\frac{1}{T}\sum\limits_{i=1}^{T}{\left\| {{{\hat{r}}}_{k|k}}-{{r}_{k}} \right\|}} $$ (37)
    $$ {\mathrm{Ratio}}=\frac{\left\| a_{k|k,\; {{\Delta }^{2}},\; AR}^{{{\tau }_{i}}}-a_{k|k,\; {{\Delta }^{2}}}^{{{\tau }_{i}}} \right\|_{\Lambda _{k|k}^{aa,\; {{\tau }_{i}}}}^{2}}{\left\| a_{k|k,\; {{\Delta }^{2}},\; AR}^{{{\tau }_{i}},\; \sec }-a_{k|k,\; {{\Delta }^{2}}}^{{{\tau }_{i}}} \right\|_{\Lambda _{k|k}^{aa,\; {{\tau }_{i}}}}^{2}} $$ (38)

    其中, $ T $表示采样时间; $ a_{k|k,\; \Delta^2,\; AR}^{\tau_i,\; \sec} $表示模糊度搜索过程中整周模糊度的次优解.

    此次测试于 2023 年 6 月 28 日在上海浦东新区进行, 采用原级科技 FSS-NAV619 导航设备, 采样频率设置为 1 Hz, 其中 RTK 模块为 UM982 板卡, 支持 BDS 和 GPS 全频段、GLONASS L1/L2 频段和Galileo E1/E5b 频段, 能够记录 RTK 原始观测数据. GNSS 接收机采样率为 1 Hz, 共采集 1320 s 实测数据, 卫星截止高度角设置为 15°, 信噪比掩模设置为 36 dB. 渐进步长经实验和调整后, 设置为 N = 10, Ratio 检验阈值设置为 3. 参考值利用武汉迈普时空组合导航系统数据处理软件的双频多 GNSS-RTK/INS 数据处理结果, 其水平和垂直精度分别为 0.02 m 和 0.03 m. 实验轨迹如图4所示, 实验环境包括卫星开阔观测环境, 同时包括高架桥、城市道路等复杂观测环境. 可见卫星数量如图5所示. 实验前半时段存在少部分卫星遮挡的情况, 后半时段信号遮挡情况频繁, 可视卫星数量变化范围相对较大, 最低可视卫星数量仅为 3 颗.

    图 4  车辆测试轨迹
    Fig. 4  Trajectory of the vehicle test
    图 5  可见卫星数量
    Fig. 5  Number of visible satellites

    为了验证所提方法对定位系统性能的改进, 将本文所提方法与扩展卡尔曼滤波 (EKF)、迭代 EKF (IEKF)、鲁棒 EKF (REKF) 以及迭代 REKF (IREKF) 等方法相比较, 并分别计算水平和垂直坐标方向的 RMSE 以及模糊度固定率记录于表1, 并将每个历元的平均解算时间记录于表2, 其中, 表1中的提升一栏表示所提出的算法相较于次优算法的提升幅度. 五种方法在每个历元的详细模糊度固定标志如图6所示. 由图7可知, 所采用的五种算法中, 本文所提算法对应的 Ratio 检验值相较于固定率次优的算法, 即 REKF, 有显著提高, 意味着模糊度固定的可靠性也更高. 在进入高架路段前, 所提算法相较于其他算法模糊度固定成功率有显著提升, 在进入高架路段后, 尽管固定率提升并不明显, 但定位误差有明显的降低. 由图8 可知, 在卫星可见数量降低的情况下, 受不准确预测值的影响, 定位结果都出现了一定程度的波动. 为评估卫星失锁后的收敛速度, 以图4中第一座天桥作为起点进行测试, 结果如图9所示, 在卫星失锁的情况下, 本文所提算法不仅具有快速收敛的特性, 且在误差跳变处也表现出良好的鲁棒性.

    表 1  RMSE和固定率对比
    Table 1  Comparison of RMSE and fixed rate
    方法EKFIREKFIEKFREKF所提方法提升
    RMSE-水平 (m)0.87181.02230.96000.90750.669623.19%
    RMSE-垂直 (m)0.29590.61740.51500.28710.206228.18%
    固定率 (%)50.330061.580058.480071.130090.380019.25%
    下载: 导出CSV 
    | 显示表格
    表 2  单个历元平均解算时间
    Table 2  The average calculation time of each epoch
    方法EKFIREKFIEKFREKF所提方法
    时间 (s)0.08210.15060.14900.08630.0899
    下载: 导出CSV 
    | 显示表格
    图 6  模糊度固定标志
    Fig. 6  Ambiguity fixed flag
    图 7  Ratio检验值对比
    Fig. 7  Comparison of ratio test value
    图 8  全程位置误差
    Fig. 8  Positioning error throughout the process
    图 9  卫星失锁后定位误差收敛速度
    Fig. 9  Positioning error convergence speed after satellite signal loss

    显然, 在卫星信号良好的情况下, 五种方法差距并不大. 但 EKF 方法受环境影响显著, 在 GNSS 信号缺失的情况下难以处理实时变化的量测噪声, 在高架路段估计精度显著下降. IREKF 和 REKF 虽然能够自适应地调节量测数据的权重来提高系统鲁棒性, 减小异常量测对系统的不利影响, 但在真实场景下, 经过调整后的量测噪声协方差仍会存在与实际不符的情况, 从而导致滤波器精度下降. IEKF 通过不断的线性化和更新步骤, 可以更好地处理非线性系统, 并提供更精确的状态估计结果, 但其对状态和协方差矩阵初值准确性要求较高, 不良的初始化会导致滤波结果难以收敛, 需要更多的迭代步骤才能获得准确的估计 (如图10), 且计算复杂度过高. 而本文所提出的算法, 最大化地利用了量测数据所携带的有效信息, 从而在提高滤波器估计精度的同时保证了系统的鲁棒性.

    图 10  开放环境下的位置误差
    Fig. 10  Positioning error in open environment

    针对 GNSS 信号干扰情况下的模糊度固定率下降问题, 提出一种面向 RTK 定位的整数约束型渐进高斯滤波方法. 该方法通过引入渐进高斯滤波, 迭代地执行渐进量测更新和模糊度固定. 这样, 利用渐进后验估计引导模糊度固定, 提高了定位结果的精度和鲁棒性. 实验结果表明, 相较于其他方法, 本文所提算法能够有效提高定位系统的固定率以及估计精度, 且在卫星失锁的情况下具有良好的收敛速度. 当然, 本文在卫星数据预处理和模型约束方面尚未深入展开. 在未来的研究中, 将考虑采用更优的选星策略, 并引入更多的约束方法, 对不同遮挡程度的卫星进行差异化处理, 进一步提升方法的自适应性, 并探索更具动态适应性的滤波策略.

    考虑式 (31), 结合先验概率密度函数, 相应的后验概率密度函数表示如下:

    $$ \begin{split} p({{x}_{k}},&\; {{\tau }_{i}}|{{Z}_{1: k}})= \\ & C_{k}^{i}\prod\limits_{j=1}^{i}{{{p}^{{{\Delta }_{j}}}}\left( {{z}_{k}}|{{x}_{k}} \right)}p({{x}_{k}}|{{Z}_{1: k-1}})= \\ & \prod\limits_{j=1}^{i}{{{\left( \sqrt{2\pi \left| \frac{{{R}_{k}}}{{{\Delta }_{j}}} \right|} \right)}^{-1}}}\times C_{k}^{i}p({{x}_{k}}|{{Z}_{1: k-1}}) \;\times \\ & \prod\limits_{j=1}^{i}{\exp \Bigg[ -\frac{1}{2}{{(\gamma _{k|k}^{{{\tau }_{j}}}-H_{k}^{{{\tau }_{j}}}\tilde{x}_{k|k}^{{{\tau }_{j}}})}^{\text T}} } \;\times \\ & {{\left( \frac{{{R}_{k}}}{{{\Delta }_{j}}} \right)}^{-1}} (\gamma _{k|k}^{{{\tau }_{j}}}-H_{k}^{{{\tau }_{j}}}\tilde{x}_{k|k}^{{{\tau }_{j}}}) \Bigg] \end{split} $$ (A1)

    整理上式可得:

    $$ \begin{split} p({{x}_{k}},&\; {{\tau }_{i}}|{{Z}_{1: k}})=C_{k}^{i}\prod\limits_{l=1}^{i}{{{\Delta }_{l}}}\frac{1}{\sqrt{\left| 2\pi {{R}_{k}} \right|}} \;\times \\ & \exp \Bigg[ -\frac{1}{2}\sum\limits_{j=1}^{i}{{{(\gamma _{k|k}^{{{\tau }_{j}}}-H_{k}^{{{\tau }_{j}}}\tilde{x}_{k|k}^{{{\tau }_{j}}})}^{\text T}}}{{(R_{k}^{{{\tau }_{j}}})}^{-1}} \\ & (\gamma _{k|k}^{{{\tau }_{j}}}-H_{k}^{{{\tau }_{j}}}\tilde{x}_{k|k}^{{{\tau }_{j}}}) \Bigg]p({{x}_{k}}|{{Z}_{1: k-1}}) \end{split} $$ (A2)

    其中, $ R_{k}^{{{\tau }_{j}}}={{R}_{k}}/{{\Delta }_{j}} $. 考虑式 (25), 可将先验概率密度函数表示为:

    $$ \begin{split} &p({{x}_{k}}|{{Z}_{1: k-1}})=\frac{1}{\sqrt{\left| 2\pi {{P}_{k|k-1}} \right|}}\; \times \\ &\quad\exp \left[ -\frac{1}{2}{{({{x}_{k}}-{{{\hat{x}}}_{k|k-1}})}^{\text T}}{{({{P}_{k|k-1}})}^{-1}}({{x}_{k}}-{{{\hat{x}}}_{k|k-1}}) \right]\\ \end{split} $$ (A3)

    令目标函数$ f({{x}_{k}}) $如下:

    $$ \begin{split} & f({{x}_{k}})=\\ &\quad\sum\limits_{j=1}^{i}{{{(\gamma _{k|k}^{{{\tau }_{j}}}-H_{k}^{{{\tau }_{j}}}\tilde{x}_{k|k}^{{{\tau }_{j}}})}^{\text T}}{{(R_{k}^{{{\tau }_{j}}})}^{-1}}(\gamma _{k|k}^{{{\tau }_{j}}}-H_{k}^{{{\tau }_{j}}}\tilde{x}_{k|k}^{{{\tau }_{j}}})}\; + \\ &\quad {{({{x}_{k}}-{{{\hat{x}}}_{k|k-1}})}^{\text T}}{{({{P}_{k|k-1}})}^{-1}}({{x}_{k}}-{{{\hat{x}}}_{k|k-1}}) \end{split} $$ (A4)

    则伪时刻$ {\tau_i} $处的系统最大后验估计可转化为:

    $$ \begin{array}{*{20}{l}} x_{k|k}^{{{\tau }_{i}}}=\arg \underset{{{x}_{k}}}{\mathop{\max }}\,\; p({{x}_{k}},\; {{\tau }_{i}}|{{Z}_{1: k}})=\arg \underset{{{x}_{k}}}{\mathop{\min }}\,\; f({{x}_{k}}) \end{array} $$ (A5)

    对$ f({{x}_{k}}) $求偏导:

    $$ \begin{split} \frac{\partial f({{x}_{k}})}{\partial {{x}_{k}}}=\;& -2\sum\limits_{j=1}^{i}{{{(H_{k}^{{{\tau }_{j}}})}^{\text T}}{{(R_{k}^{{{\tau }_{j}}})}^{-1}}\gamma _{k|k}^{{{\tau }_{j}}}}\; + \\ & 2\sum\limits_{s=1}^{m}{{{(H_{k}^{{{\tau }_{j}}})}^{\text T}}{{(R_{k}^{{{\tau }_{j}}})}^{-1}}H_{k}^{{{\tau }_{j}}}({{x}_{k}}-{{{\hat{x}}}_{k|k-1}})} \;+ \\ & 2{{({{P}_{k|k-1}})}^{-1}}({{x}_{k}}-{{{\hat{x}}}_{k|k-1}})=0 \end{split} $$ (A6)

    伪时刻$ {\tau_i} $处的渐进最大后验估计为:

    $$ \hat{x}_{k|k}^{{{\tau }_{i}}}=P_{k|k}^{{{\tau }_{i}}}\sum\limits_{j=1}^{i}{{{(H_{k}^{{{\tau }_{j}}})}^{\text T}}{{(R_{k}^{{{\tau }_{j}}})}^{-1}}\gamma _{k|k}^{{{\tau }_{j}}}}+{{\hat{x}}_{k|k-1}}$$ (A7)
    $$ P_{k|k}^{{{\tau }_{i}}}={{\left[ \sum\limits_{j=1}^{i}{{{(H_{k}^{{{\tau }_{j}}})}^{\text T}}{{(R_{k}^{{{\tau }_{j}}})}^{-1}}H_{k}^{{{\tau }_{j}}}}+{{({{P}_{k|k-1}})}^{-1}} \right]}^{-1}}$$ (A8)

    由转换矩阵$ G $, 可得出相应的双差模糊度向量及其协方差矩阵:

    $$ \begin{array}{*{20}{l}} \hat{x}_{k|k,\; {{\Delta }^{2}}}^{{{\tau }_{i}}}=Gx_{k|k}^{{{\tau }_{i}}}={{\left[ \begin{matrix} r_{k|k}^{{{\tau }_{i}}} & a_{k|k,\; {{\Delta }^{2}}}^{{{\tau }_{i}}} \end{matrix} \right]}^{\text T}} \end{array} $$ (A9)
    $$ \begin{array}{*{20}{l}} \Lambda _{k|k}^{{{\tau }_{i}}}=GP_{k|k}^{{{\tau }_{i}}}{{G}^{\text T}}=\left[ \begin{matrix} P_{k|k}^{rr,\; {{\tau }_{i}}} & \Lambda _{k|k}^{ra,\; {{\tau }_{i}}} \\ \Lambda _{k|k}^{ra,\; {{\tau }_{i}}} & \Lambda _{k|k}^{aa,\; {{\tau }_{i}}} \\ \end{matrix} \right] \end{array} $$ (A10)

    则相应的位置更新过程表示如下:

    $$ \begin{split} r_{k|k,\; AR}^{{{\tau }_{i}}}=\;&\hat{r}_{k|k}^{{{\tau }_{i}}}+\Lambda _{k|k}^{ra,\; {{\tau }_{i}}}{{(\Lambda _{k|k}^{aa,\; {{\tau }_{i}}})}^{-1}}\;\times\\ &(\hat{a}_{k|k,\; {{\Delta }^{2}},\; AR}^{{{\tau }_{i}}}-\hat{a}_{k|k,\; {{\Delta }^{2}}}^{{{\tau }_{i}}}) \end{split} $$ (A11)
    $$ \begin{array}{*{20}{l}} P_{k|k,\; AR}^{rr,\; {{\tau }_{i}}}=P_{k|k}^{rr,\; {{\tau }_{i}}}-\Lambda _{k|k}^{ra,\; {{\tau }_{i}}}{{(\Lambda _{k|k}^{aa,\; {{\tau }_{i}}})}^{-1}}{{(\Lambda _{k|k}^{ra,\; {{\tau }_{i}}})}^{\text T}} \end{array} $$ (A12)

  • 图  1  基于高斯滤波的 RTK 定位框图

    Fig.  1  Diagram of RTK positioning based on Gaussian filter

    图  2  RTK 定位示意图

    Fig.  2  Schematic diagram of RTK positioning

    图  3  整数约束型渐进高斯滤波框架

    Fig.  3  Integer-constrained progressive Gaussian filtering framework

    图  4  车辆测试轨迹

    Fig.  4  Trajectory of the vehicle test

    图  5  可见卫星数量

    Fig.  5  Number of visible satellites

    图  6  模糊度固定标志

    Fig.  6  Ambiguity fixed flag

    图  7  Ratio检验值对比

    Fig.  7  Comparison of ratio test value

    图  8  全程位置误差

    Fig.  8  Positioning error throughout the process

    图  9  卫星失锁后定位误差收敛速度

    Fig.  9  Positioning error convergence speed after satellite signal loss

    图  10  开放环境下的位置误差

    Fig.  10  Positioning error in open environment

    表  1  RMSE和固定率对比

    Table  1  Comparison of RMSE and fixed rate

    方法EKFIREKFIEKFREKF所提方法提升
    RMSE-水平 (m)0.87181.02230.96000.90750.669623.19%
    RMSE-垂直 (m)0.29590.61740.51500.28710.206228.18%
    固定率 (%)50.330061.580058.480071.130090.380019.25%
    下载: 导出CSV

    表  2  单个历元平均解算时间

    Table  2  The average calculation time of each epoch

    方法EKFIREKFIEKFREKF所提方法
    时间 (s)0.08210.15060.14900.08630.0899
    下载: 导出CSV
  • [1] Ji R, Jiang X, Chen X, Zhu H, Ge M, Neitzel F. Quality monitoring of real-time GNSS precise positioning service system. Geo-Spatial Information Science, 2023, 26(1): 1−15 doi: 10.1080/10095020.2022.2070554
    [2] Chen Q, Lin H, Kuang J, Luo Y, Niu X. Rapid initial heading alignment for MEMS land vehicular GNSS/INS navigation system. IEEE Sensors Journal, 2023, 23(7): 7656−7666 doi: 10.1109/JSEN.2023.3247587
    [3] 王婷娴, 贾克斌, 姚萌. 面向轻轨的高精度实时视觉定位方法. 自动化学报, 2021, 47(9): 2194−2204

    Wang Ting-Xian, Jia Ke-Bin, Yao Meng. Real-time visual localization method for light-rail with high accuracy. Acta Automatica Sinica, 2021, 47(9): 2194−2204
    [4] Medina D, Calatrava H, Castro-Arvizu J M, Closas P, Vila-Valls J. A collaborative RTK approach to precise positioning for vehicle swarms in urban scenarios. In: Proceedings of IEEE/ION Position, Location and Navigation Symposium (PLANS). Monterey, USA: IEEE, 2023. 254−259
    [5] Tao X, Liu W, Wang Y, Li L, Zhu F, Zhang X. Smartphone RTK positioning with multi-frequency and multi-constellation raw observations: GPS L1/L5, Galileo E1/E5a, BDS B1I/B1C/B2a. Journal of Geodesy, 2023, 97(5): Article No. 43 doi: 10.1007/s00190-023-01731-3
    [6] Gao Y, Jiang Y, Gao Y, Huang G, Yue Z. Solution separation-based integrity monitoring for RTK positioning with faulty ambiguity detection and protection level. GPS Solutions, 2023, 27(3): Article No. 140 doi: 10.1007/s10291-023-01472-y
    [7] 陈杰, 程兰, 甘明刚. 基于高斯和近似的扩展切片高斯混合滤波器及其在多径估计中的应用. 自动化学报, 2013, 39(1): 1−10 doi: 10.1016/S1874-1029(13)60001-4

    Chen Jie, Cheng Lan, Gan Ming-Gang. Extension of SGMF using Gaussian sum approximation for nonlinear/non-Gaussian model and its application in multipath estimation. Acta Automatica Sinica, 2013, 39(1): 1−10 doi: 10.1016/S1874-1029(13)60001-4
    [8] Teunissen P. The least-squares ambiguity decorrelation adjustment: A method for fast GPS integer ambiguity estimation. Journal of Geodesy, 1995, 70(1): 65−82
    [9] Chang X W, Yang X, Zhou T. MLAMBDA: A modified LAMBDA method for integer ambiguity determination. In: Proceedings of the 61st Annual Meeting of the Institute of Navigation. Cambridge, USA: 2005. 1086−1097
    [10] Takasu T, Yasuda A. Kalman-filter-based integer ambiguity resolution strategy for long-baseline RTK with ionosphere and troposphere estimation. In: Proceedings of the 23rd International Technical Meeting of the Satellite Division of the Institute of Navigation. Portland, USA: 2010. 161−171
    [11] Gao Y, Jiang Y, Liu B, Gao Y. Integrity monitoring of multi-constellation GNSS-based precise velocity determination in urban environments. Measurement, 2023, 222: Article No. 113676 doi: 10.1016/j.measurement.2023.113676
    [12] Giorgi G, Teunissen P. Carrier phase GNSS attitude determination with the multivariate constrained LAMBDA method. In: Proceedings of 2010 IEEE Aerospace Conference. Big Sky, USA: IEEE, 2010. 1−12
    [13] 张文安, 林安迪, 杨旭升, 俞立, 杨小牛. 融合深度学习的贝叶斯滤波综述. 自动化学报, 2024, 50(8): 1502−1516

    Zhang Wen-An, Lin An-Di, Yang Xu-Sheng, Yu Li, Yang Xiao-Niu. A survey on bayesian filtering with deep learning. Acta Automatica Sinica, 2024, 50(8): 1502−1516
    [14] Fang H, Tian N, Wang Y, Zhou M C, Haile M. Nonlinear Bayesian estimation: From Kalman filtering to a broader horizon. IEEE/CAA Journal of Automatica Sinica, 2018, 5(2): 401−417 doi: 10.1109/JAS.2017.7510808
    [15] 杨旭升, 王雪儿, 汪鹏君, 张文安. 基于渐进无迹卡尔曼滤波网络的人体肢体运动估计. 自动化学报, 2023, 49(8): 1723−1731

    Yang Xu-Sheng, Wang Xue-Er, Wang Peng-Jun, Zhang Wen-An. Estimation of human limb motion based on progressive unscented Kalman filter network. Acta Automatica Sinica, 2023, 49(8): 1723−1731
    [16] Katriniok A, Abel D. Adaptive EKF-based vehicle state estimation with online assessment of local observability. IEEE Transactions on Control Systems Technology, 2016, 24(4): 1368−1381 doi: 10.1109/TCST.2015.2488597
    [17] Chen X, Wang X, Xu Y. Performance enhancement for a GPS vector-tracking loop utilizing an adaptive iterated extended Kalman filter. Sensors, 2014, 14(12): 23630−23649 doi: 10.3390/s141223630
    [18] Li H, Medina D, Vilà-Valls J, Closas P. Robust Kalman filter for RTK positioning under signal-degraded scenarios. In: Proceedings of the 32nd International Technical Meeting of the Satellite Division of the Institute of Navigation. Miami, USA: 2019. 3717−3729
    [19] Medina D, Li H, Vilà-Valls J, Closas P. Robust filtering techniques for RTK positioning in harsh propagation environments. Sensors, 2021, 21(4): Article No. 1250 doi: 10.3390/s21041250
    [20] Yuan H, Zhang Z, He X, Wen Y, Zeng J. An extended robust estimation method considering the multipath effects in GNSS real-time kinematic positioning. IEEE Transactions on Instrumentation and Measurement, 2022, 71: 1−9
    [21] Huang Y, Zhang Y, Li N, Zhao L. Gaussian approximate filter with progressive measurement update. In: Proceedings of 54th IEEE Conference on Decision and Control (CDC). Osaka, Japan: IEEE, 2015. 4344−4349
    [22] 郑婷婷, 杨旭升, 张文安, 俞立. 一种高斯渐进滤波框架下的目标跟踪方法. 自动化学报, 2018, 44(12): 2250−2258

    Zheng Ting-Ting, Yang Xu-Sheng, Zhang Wen-An, Yu Li. A target tracking method in Gaussian progressive filtering framework. Acta Automatica Sinica, 2018, 44(12): 2250−2258
    [23] Yang X, Zhao C, Chen B. Progressive Gaussian approximation filter with adaptive measurement update. Measurement, 2019, 148: Article No. 106898 doi: 10.1016/j.measurement.2019.106898
    [24] 杨旭升, 吴江宇, 胡佛, 张文安. 基于渐进高斯滤波融合的多视角人体姿态估计. 自动化学报, 2024, 50(3): 607−616

    Yang Xu-Sheng, Wu Jiang-Yu, Hu Fo, Zhang Wen-An. Multi-view human pose estimation based on progressive Gaussian filtering fusion. Acta Automatica Sinica, 2024, 50(3): 607−616
    [25] Verhagen S, Teunissen P. The ratio test for future GNSS ambiguity resolution. GPS Solutions, 2013, 17: 535−548 doi: 10.1007/s10291-012-0299-z
  • 加载中
图(10) / 表(2)
计量
  • 文章访问数:  192
  • HTML全文浏览量:  61
  • PDF下载量:  44
  • 被引次数: 0
出版历程
  • 收稿日期:  2024-06-24
  • 录用日期:  2024-10-08
  • 网络出版日期:  2024-12-02
  • 刊出日期:  2025-02-17

目录

/

返回文章
返回