Global Navigation Satellite System/Strapdown Inertial Navigation System Integrated Navigation Algorithm in Complex Urban Environment

1.   引言

全球卫星导航系统 (Global Navigation Satellite System, GNSS) 以其全天时、全球覆盖、高精度的定位性能，成为当今主流的导航手段，然而GNSS落地信号功率低(约–160 dBw)且易受各种干扰，导致其可用性差，特别是城市复杂环境下，存在多径干扰、卫星信号受遮挡或卫星不可视等情况，导致GNSS定位误差急剧增大。捷联惯性导航系统 (Strapdown Inertial Navigation System, SINS) 以其短时精度高和全自主工作的特点，不受外界信号影响，恰好弥补了GNSS的缺点。将上述两种导航设备组合在一起的GNSS/SINS组合导航系统，取二者之优点，已成为组合导航技术发展的主流^[1–3]。

在SINS性能固定的情况下，影响GNSS/SINS组合导航系统性能的主要因素就是GNSS信号测量精度和稳定性，在GNSS卫星不可视或信号频繁短暂失锁时，由于组合导航系统的滤波模型无法实时感知而使用了测量性能变差的信号，导致组合滤波效果变差或结果不可用。解决此类问题有两种思路，一种是增加其他辅助传感器，如使用3D激光雷达作为GNSS定位的辅助手段，提高系统定位精度^[4]；使用全向鱼眼相机，实时获取在城市峡谷中的四周环境景象，并与此时卫星的位置匹配计算，剔除不可视卫星，得到更好的滤波性能^[5]；使用3D地图辅助GNSS解决定位环境信号起伏或受遮挡下组合导航系统定位的问题^[6,7]；使用超宽带 (Ultra Wide Band, UWB) 辅助GNSS，实现在可见卫星数少于4颗时仍可得到较高的定位精度^[8,9]等，以上通过增加传感器的方法，在解决问题的同时无疑也增加了系统成本和计算复杂度。另一种思路是使用改进的信号处理方法，如采用联邦卡尔曼滤波、动静态滤波、基于神经网络的滤波方法等，使得滤波模型能与实际情况匹配。其中，联邦卡尔曼滤波由于各个局部滤波器使用了相似或相同的状态方程，主滤波器与局部滤波器的传感器输出量相互不独立，融合结果不具有最优性^[10,11]，而动静态滤波^[12,13]等方法由于实现复杂，特别是神经网络还需要大量训练数据样本^[14]，也增加了系统实现的复杂度和难度。

因子图的概念是在2001年由文献[15]提出，它是概率图模型的一种表示方式，是用来表示全局函数和局部函数关系的模型图，被广泛应用于人工智能、信号处理、数字通信等领域，能对各种复杂概率问题进行求解^[16,17]。基于因子图建模的主要目的是将复杂的概率问题进行因式化分解，即将复杂的全局函数分解为多个局部函数相乘的形式，当已知局部函数之间的相关性时，只需要分析每个局部函数就可以得到全局复杂概率问题的解^[18]。因子图能够解决组合导航系统对多种导航源测量值的异步传输、即插即用、多速率数据的融合问题，同时也能解决状态方程或观测方程非线性的问题，具有良好的扩展性和灵活性。

为解决GNSS信号受多径干扰、遮挡或拒止时，组合导航系统性能下降的问题，本文基于因子图模型，提出一种GNSS/SINS紧组合系统导航方法，组合系统架构如图1所示，首先，利用GNSS接收机内部的相干积分结果构造信号误差鉴别函数，能实时估计GNSS信号受多径干扰、短暂失锁、拒止情况下的测量性能，以此构造联合权值矩阵，实时修正GNSS伪距、伪距率测量值的协方差矩阵；其次，在GNSS信号拒止时，利用载体运动约束条件构建零速修正因子，对系统状态进行修正，保证了GNSS/SINS的定位结果不迅速发散。数据仿真和跑车实验表明，本文方法能有效解决GNSS/SINS组合导航系统在城市峡谷等复杂环境下的连续可靠导航定位的问题，特别适用于车辆在城市行驶中频繁启停的场景。

图  1  系统整体框架图

下载: 全尺寸图片 幻灯片

2.   GNSS/SINS因子图模型

在GNSS/SINS组合导航系统中，定义任意一个时刻 ${t_i}$的系统状态量为 ${{\boldsymbol{x}}_i}$，测量值为 ${{\boldsymbol{z}}_i}$。设 ${{\boldsymbol{X}}_k} = \left\{ {{{\boldsymbol{x}}_1},{{\boldsymbol{x}}_2}, \cdots ,{{\boldsymbol{x}}_k}} \right\}$表示截至当前时刻 ${t_k}$系统所有的状态量， ${{\boldsymbol{Z}}_k} = \left\{ {{{\boldsymbol{z}}_1},{{\boldsymbol{z}}_2}, \cdots ,{{\boldsymbol{z}}_k}} \right\}$ 表示截至当前时刻 ${t_k}$系统所有的测量值。GNSS/SINS组合的目标就是求联合概率分布函数 $p\left( {{{\boldsymbol{X}}_k}|{{\boldsymbol{Z}}_k}} \right)$的最大后验概率估计，利用因子图算法，状态量 ${{\boldsymbol{X}}_k}$的最大后验概率可以归结为最大化所有因子节点乘积

其中， ${f_i}$表示因子图中 ${t_i}$时刻的因子节点， ${\boldsymbol{X}}_k^i$表示与因子节点 ${f_i}$相连接的变量节点的集合，且 ${\boldsymbol{X}}_k^i \subseteq {{\boldsymbol{X}}_k}$。在测量误差高斯噪声分布的情况下，最大后验概率可以转换成如式(2)的最小二乘问题

其中， ${\boldsymbol{z}}_i^{{\mathrm{IMU}}}$, ${\boldsymbol{z}}_i^{{\mathrm{GNSS}}}$分别为 ${t_i}$时刻的惯性测量单元 (Inertial Measurement Unit, IMU)测量值和GNSS接收机测量值， ${\boldsymbol{h}}^{{\mathrm{SINS}}}\left( \cdot \right)$和 ${\boldsymbol{h}}^{{\mathrm{GNSS}}}\left( \cdot \right)$分别为SINS和GNSS对应的非线性测量方程， ${{\boldsymbol{\varSigma}} _i}$, ${{\boldsymbol{\varLambda}} _i}$分别为测量方程对应的噪声协方差矩阵， $\left\| {\boldsymbol{x}} \right\|_{\boldsymbol{\varSigma}} ^2 \doteq {{\boldsymbol{x}}^{{\mathrm{T}}}}{{\boldsymbol{\varSigma}} ^{ - 1}}{\boldsymbol{x}}$表示马氏距离的平方。本文为了表达简略，在式(2)中省略了SINS的测量偏差因子节点，计算时将测量偏差量因子节点作为与SINS相关的状态量一并计算。

2.1   GNSS因子节点

假设GNSS接收到的信号包括1路直达信号和M路多径反射信号，则信号经过接收机基带信号处理下变频后得到式(3)

其中，A为直达信号的幅度， $D\left( t \right)$, $c\left( t \right)$分别为直达信号的数据符号位和伪码， ${\tau _0}$, ${f_{\text{d}}}$, ${\theta _0}$分别为直达信号的传输延迟、多普勒频率和载波相位， ${\alpha _m}$, ${\tau _m}$, ${\theta _m}$分别为第m路多径信号的幅度衰减系数、相对直达信号的伪码延时、相对直达信号的载波相位变化， $n\left( t \right)$为噪声项。

上述信号与本地复制的伪码和载波估计信号进行 ${T_c}$时间长度的相关积分(假设积分过程 $D\left( t \right) = 1$)，忽略积分后的噪声项，结果为

其中， $\varepsilon = {\tau _0} - \tilde \tau$为本地伪码延迟估计误差， ${\theta _e} = {\theta _0} - \tilde \theta$为载波相位估计误差， ${R_s}\left( \varepsilon \right)$为直达信号的自相关函数，为表述简洁，式(4)假设已准确估计直达信号载波频率且已使用直达信号的幅度A进行了归一化。

以标量跟踪接收机为例，通常使用码延迟锁定环路(Delay Lock Loop, DLL)对接收信号的伪码相位进行跟踪，并利用鉴相器输出结果来估计伪码相位误差。以相关器间隔为d的相干早迟码鉴相器 $D\left( \varepsilon \right)$为例

其中， ${{\mathrm{Re}}} \left( x \right)$表示对x取实部操作。当DLL进入稳定跟踪阶段后，鉴相器输出结果 $D\left( \varepsilon \right) \approx 0$。对于没有多径干扰的信号，自相关函数关于0点左右对称，鉴相器输出为0表明此时误差 $\varepsilon$为0，可实现伪码正确跟踪；当存在多径干扰时，自相关函数将不具有关于0点左右对称特性，鉴相器输出结果为0点就不再对应正确的伪码相位值，由此将导致伪距测量值的误差增大。另外，当信号进入稳定跟踪阶段，锁相环路能准确估计出伪码和载波相位时，如式(4)所示的相干积分结果中，信号能量将集中在实部，虚部接近于0。

根据以上的分析，可构造信号误差鉴别函数

其中， ${{\mathrm{Im}}} \left( x \right)$表示对x取虚部操作， ${a_{\text{E}}}$和 ${a_{\text{L}}}$分别为伪码跟踪环路中超前支路斜率和滞后支路斜率，其表达式分别为

当信号中没有多径干扰且环路实现伪码相位稳定跟踪时，超前支路和滞后支路斜率大小相等，方向相反；同时，环路在实现信号的载波相位稳定跟踪时，相干积分值的实部远大于虚部，因此w将趋于0附近。相反，若信号中存在多径干扰或环路没有实现伪码稳定跟踪，DLL中的超前和滞后支路斜率将不具备对称规律；同时，环路在没有实现载波相位稳定跟踪时，相干积分结果中信号能量也不集中在实部，因此w将不趋于0。当多径干扰影响越严重，信号伪码和载波跟踪误差越大时w也越大。

当可观测到n颗卫星时，将各卫星对应的误差鉴别函数进行归一化，对于第i颗可观测卫星，可以得到归一化的误差鉴别函数值 ${\bar w_i}$，利用 ${\bar w_i}$建立权值矩阵 ${\boldsymbol{W}}$, ${\boldsymbol{W}}$是对角矩阵 ${\boldsymbol{W}} = {\mathrm{diag}}\left( \bar w_1^{ - 1}, \bar w_2^{ - 1}, \cdots ,\bar w_n^{ - 1} \right)$，它表征了GNSS接收机内部各颗卫星信号的测量性能。将权值矩阵对式(2)中的GNSS测量噪声协方差矩阵 ${\varLambda _i}$进行加权，可有效降低GNSS信号异常对系统状态解算的影响。

假设GNSS接收机在 ${t_i}$时刻的状态量为 ${\boldsymbol{x}}_i^{{\mathrm{GNSS}}} \in {\mathbb{R}^8} = {\left[ {{\boldsymbol{p}}{{_i^{\mathrm{e}}}^{{\mathrm{T}}}},{\boldsymbol{v}}{{_i^{\mathrm{e}}}^{{\mathrm{T}}}},\delta _i^{},\dot \delta _i^{}} \right]^{{\mathrm{T}}}}$，其中载体位置矢量 ${\boldsymbol{p}}_i^{\mathrm{e}} = \left( {x_i}, {y_i}, {z_i} \right)^{{\mathrm{T}}}$为地心地固坐标系(简称E系)下的3维位置，速度矢量 ${\boldsymbol{v}}_i^{\mathrm{e}} = {\left( {{v_{xi}},{v_{yi}},{v_{zi}}} \right)^{{\mathrm{T}}}}$为E系下的3维速度， ${\delta _i}$与 $\dot \delta _i^{}$分别为接收机与GNSS系统时的钟差和钟差变化率。在测量时刻 ${t_i}$，GNSS接收机的测量值为 ${\boldsymbol{z}}_i^{{\mathrm{GNSS}}} = {\left[ {{{\boldsymbol{\rho}} _i}^{{\mathrm{T}}},\dot {\boldsymbol{\rho}} {{_i}^{{\mathrm{T}}}}} \right]^{{\mathrm{T}}}}$，其中 ${{\boldsymbol{\rho}} _i} = {\left( {\rho _{i,1}^{},{\rho _{i,2}}, \cdots ,{\rho _{i,n}}} \right)^{{\mathrm{T}}}}$, ${{\boldsymbol{\rho}} _{i,j}}$为接收机在 ${t_i}$时刻对第 $j$颗可见卫星的伪距测量值(假设有n颗可见卫星)， ${\dot {\boldsymbol{\rho}} _i} = {\left( {\dot \rho _{i,1}^{},{{\dot \rho }_{i,2}}, \cdots ,{{\dot \rho }_{i,n}}} \right)^{{\mathrm{T}}}}$为 ${{\boldsymbol{\rho}} _i}$对应的伪距率。由导航电文可以计算得 ${t_i}$时刻所有可见卫星的位置矢量和速度矢量为 ${\boldsymbol{p}}_i^{{\mathrm{GNSS}}} = \left\{ {{\boldsymbol{p}}_{i,1}^{{\mathrm{GNSS}}},{\boldsymbol{p}}_{i,2}^{{\mathrm{GNSS}}}, \cdots ,{\boldsymbol{p}}_{i,n}^{{\mathrm{GNSS}}}} \right\}$和 ${\boldsymbol{v}}_i^{{\mathrm{GNSS}}} = \left\{ {\boldsymbol{v}}_{i,1}^{{\mathrm{GNSS}}},{\boldsymbol{v}}_{i,2}^{{\mathrm{GNSS}}}, \cdots ,{\boldsymbol{v}}_{i,n}^{{\mathrm{GNSS}}} \right\}$，其中 ${\boldsymbol{p}}_{i,j}^{{\mathrm{GNSS}}}$和 ${\boldsymbol{v}}_{i,j}^{{\mathrm{GNSS}}}$分别为 ${t_i}$时刻第 $j$颗可见卫星在E系下的位置和速度。可以得到关于GNSS接收机的测量方程为

其中，c为光速， $\left\| {\boldsymbol{x}} \right\|$表示x的2范数。上述方程假设GNSS信号传输过程中的星钟、电离层、对流层延迟、地球自转等误差均已经消除。

2.2   SINS因子节点

由于SINS导航解算的连续性和稳定性，通常用SINS节点作为载体运动姿态的节点。对于任意 ${t_i}$时刻，载体运动姿态状态量定义为 ${\boldsymbol{x}}_i^{{\mathrm{SINS}}} \in {\mathbb{R}^{15}} = {\left[ {{\boldsymbol{p}}_i^{{\mathrm{e}}{\mathrm{T}}},{{\boldsymbol{v}}_i^{{\mathrm{e}}{\mathrm{T}}}}, {{\boldsymbol{\varPhi}}_i^{{\mathrm{eT}}}}} \right]^{{\mathrm{T}}}}$，偏差状态量定义为 ${\boldsymbol{\alpha}} _i = {\left[ {{{\text{∇}} ^{{\mathrm{b}}{\mathrm{T}}}_i},{{\boldsymbol{\varepsilon}} ^{{\mathrm{b}}{\mathrm{T}}}_i}} \right]^{\text{T}}}$。其中姿态矢量 ${\boldsymbol{\varPhi}} _i^e = {\left( {{\phi _{xi}},{\phi _{yi}},{\phi _{zi}}} \right)^{{\mathrm{T}}}}$为E系下载体的3维姿态角， ${\text{∇}} ^b_i$与 ${\boldsymbol{\varepsilon}} ^b_i$为IMU在载体坐标系(简称B系)下3轴的加速度计测量偏差和陀螺仪测量偏差。

IMU的输出频率一般远高于GNSS接收机，且IMU输出测量值为B系下的比力测量值 ${\boldsymbol{f}}_i^{\mathrm{b}}$和角速度测量值 ${\boldsymbol{\omega}} _i^{\mathrm{b}}$，即 ${\boldsymbol{z}}_i^{{\mathrm{IMU}}} = {\left[ {{\boldsymbol{f}}{{_i^{\mathrm{b}}}^{{\mathrm{T}}}},{\boldsymbol{\omega}} {{_i^{\mathrm{b}}}^{{\mathrm{T}}}}} \right]^{{\mathrm{T}}}}$。而SINS节点的状态量在E系下，为了减小坐标系的频繁转换带来的巨大计算量，通常采取预积分的方法将高频率的IMU输出测量值压缩成低频率再进行因子节点计算。假设在 $\left[ {{t_i},{t_{i + 1}}} \right]$时间段内对IMU输出测量值进行预积分，得到速度、位置、姿态的变化量为

其中， $\Delta {\boldsymbol{v}}{_{i \to {i + 1}}^{{{\text{b}}_i}}}$, $\Delta {\boldsymbol{p}}{_{i \to{i + 1} }^{{{\text{b}}_i}}}$分别为B系 ${t_{i + 1}}$时刻相对于 ${t_i}$时刻的速度、位置变化量， $\Delta {\boldsymbol{q}}{_{i \to {i + 1}}^{{{\text{b}}_i}}}$为B系 ${t_{i + 1}}$时刻相对于 ${t_i}$时刻的旋转四元数， ${\boldsymbol{C}}_{{\text{b}}_t}^{{\text{b}}_i}$表示B系下t时刻到 ${t_i}$时刻的旋转矩阵。由以上预积分求出的变化量，假设在时间间隔 $\Delta t$( $\Delta t = {t_{i + 1}} - {t_i}$)内载体为小角度旋转，且加速度计测量精度较差，无法敏感捕捉到地球自转角速度，可得 ${t_{i + 1}}$时刻载体在E系下的速度、位置、4元数的估计值为

其中， ${\boldsymbol{p}}_i^{\mathrm{e }}$, ${\boldsymbol{v}}_i^{\mathrm{e}}$, ${\boldsymbol{q}}_{{{\text{b}}_i}}^{\mathrm{e}}$分别为载体在E系 ${t_i}$时刻的位置、速度值和4元数，由SINS递推的原理知这些为已知量。 ${\boldsymbol{C}}_{{\mathrm{b}}_i}^{\mathrm{e }}$表示在 ${t_i}$时刻由B系变换到E系的旋转矩阵， ${{\boldsymbol{g}}^{\mathrm{e}}}$表示 ${t_i}$时刻的重力矢量在E系的投影，在时间间隔 $\Delta t$内认为是一个常量，“ $\otimes$” 表示4元数的乘法运算，由 ${t_{i + 1}}$时刻4元数的值可以得到该时刻对应的姿态角 ${\boldsymbol{\varPhi}} _i^{\mathrm{e}}$。

同时由陀螺仪、加速度计的测量模型可知其误差为1阶马尔科夫模型，加速度计测量偏差、陀螺仪测量偏差的估计可以用式(12)表示

由式(10)—式(12)可以得到关于运动姿态的测量方程和偏差的测量方程

2.3   零速检测与校正

大多数GNSS/SINS组合导航系统中的SINS均为低精度的微机电系统(Micro Electro Mechanical Systems, MEMS)传感器，在GNSS良好可视情况下，利用GNSS测量值就可实时准确校正SINS，使之始终工作于较高定位精度上，但是当GNSS信号拒止时，无法实现对SINS的精确校正，此时SINS定位、测速结果将迅速发散。

零速检测是利用载体处于静止状态时，IMU输出的比力大小已知且为重力加速度，以及不考虑地球自转时IMU输出的角速度为零等条件，进行载体运动状态的探测。当检测到载体静止时，利用SINS的姿态变化和加速度输出值作为误差观测量，以此对其他各项误差进行校正以减小累积误差。

将零速检测理解为一个2元信号的统计检测问题，定义

当 ${\mathrm{H}}0$为真时，判断载体处于静止状态的概率为虚警概率 ${P_{{\mathrm{FA}}}}$，当 ${\mathrm{H}}1$为真时，判断载体处于静止状态的概率为检测概率 ${P_{\mathrm{D}}}$。使用Neyman-Pearson准则，在给定虚警概率 ${P_{{\mathrm{FA}}}} = \alpha$时，检测概率 ${P_{\mathrm{D}}}$最大，可以得到检测函数为

$L(z_n^{})$为检测统计信号 $z_n^{}$的似然比， $\gamma$为检测统计门限值，当似然比 $L(z_n^{})$大于门限值时，认为载体处于静止状态，否则处于非静止状态。统计信号 $z_n^{}$取IMU在一个时间段内的加速度、角速度测量值。假设各个测量值服从高斯分布，构造对数形式的检测统计量

则当 ${\mathrm{H}}1$成立时，检测函数的对数表达形式为

其中， ${\boldsymbol{y}}_k^a$表示IMU测量的第k个3轴加速度矢量， ${\boldsymbol{y}}_k^\omega$表示IMU测量的第k个3轴角速度矢量， $\sigma _a^2$和 $\sigma _\omega ^2$分别为加速度和角速度测量方差，N为检测窗口长度，g为重力常量， $\bar {\boldsymbol{y}}_n^a = \left( {\displaystyle\sum\nolimits_{k = n}^{n + N - 1}{\boldsymbol{y}}_k^a} \right)/N$为检测窗口长度内的加速度平均值， ${\left\| {\bar {\boldsymbol{y}}_n^a} \right\|^2} = \bar {\boldsymbol{y}}{_n^{a{\text{T}}}}\bar {\boldsymbol{y}}_n^a$表示 $\bar {\boldsymbol{y}}_n^a$的2范数的平方，门限值为 $\gamma ' = - 2\left( {\ln\gamma } \right)/N$，当检测量小于门限 ${\gamma '}$时，载体处于静止状态。

在GNSS信号拒止时，利用车辆速度为0的约束条件可以限制SINS误差的积累。由于车辆运动状态不变，在因子图中不增加新的变量节点，可极大地降低因子图中计算和存储的复杂度。当IMU检测到0速时，车体坐标系(简称M系)下3个方向的速度为0，即 ${{\boldsymbol{v}}^{\mathrm{m}}} = {{\bf{0}}}$。忽略INS安装角的偏差，认为INS的B系与车体的M系重合。车辆静止时由于IMU测量误差，使得测量时刻 ${t_i}$由IMU测量值实际计算得到的载体速度 ${\boldsymbol{v}}_i^{\mathrm{e}}$不为0，B系及E系下的速度矢量存在关系为

其中， ${\boldsymbol{C}}_{\mathrm{b}}^{\mathrm{e}}$表示B系到E系的旋转矩阵，载体在 ${t_i}$时刻B系下的速度可以由 ${t_{i - 1}}$时刻的速度计算得到，如式(11)所示。假设此时载体在导航坐标系(简称N系)下的姿态矢量为 ${\boldsymbol{\varPhi}} _i^{\mathrm{n}} = {\left( {{\alpha _i},{\beta _i},{\gamma _i}} \right)^{{\mathrm{T}}}}$，其中 ${\alpha _i}$, ${\beta _i}$, ${\gamma _i}$分别为 ${t_i}$时刻载体的航向角、横滚角、俯仰角。N系姿态矢量到E系姿态矢量的转换关系为

其中， ${\boldsymbol{C}}_{{\mathrm{e}}i}^{{\mathrm{n}}i}$表示载体在 ${t_i}$时刻由E系变换到N系的旋转矩阵。在时间间隔 $\Delta t$内航向角的变化量 $\Delta {\alpha _i}$与姿态矢量各元素的关系为

其中， $\delta {\boldsymbol{\omega}} _{i{\mathrm{b}}}^{\mathrm{b}}$为载体在惯性系的角速度在 $\Delta t$内的变化量在B系下的投影。当载体静止时，在航向角上的变化量为0，由式(21)可以约束IMU输出的角速度零偏值。

由式(19)—式(21)可以得到SINS部分的零速更新因子节点的方程 ${h_i}^{{\mathrm{ZUPT}}\_{\mathrm{SINS}}}\left( {{\boldsymbol{x}}_{i - 1}^{{\mathrm{SINS}}},{\boldsymbol{z}}_i^{{\mathrm{IMU}}}} \right)$。

另外，在只能观测到1颗GNSS卫星的情况下，也可以用载体速度为零的约束对GNSS接收机钟差变化率进行控制。假设在测量时刻 ${t_i}$得到该卫星的伪距率测量方程为

其中， ${\dot p_i}$为接收机与卫星之间的几何距离变化率，卫星的钟差变化率可由星历计算得到，在测量方程中省略，可归结到伪距率测量值中， ${\xi _i}$为伪距率测量残差。 ${\varepsilon _{\dot \rho ,i}}$为包括对流层电离层等在内的传播误差随时间的变化率，在短时间内可以忽略。当用户静止时，接收机与卫星之间的几何距离变化率 ${\dot p_i}$其实就是卫星的速度在卫星与接收机径向方向的大小。可得GNSS对应的测量方程为

以上就得到零速更新时SINS与GNSS对应因子节点的表达式。当检测到载体处于静止状态时，使用上述方法对系统状态进行约束，无需增加新的因子节点，因子图的大小维持不变。

如图2所示为经典因子图方法与本文方法在图结构上的比较，图2(a)为经典因子图方法，图2(b)为本文所提方法。对应因子图从左至右划分为运动区间、零速区间、运动区间，相比图2(a)，图2(b)在零速区间中只进行状态约束，没有增加新的因子节点。

图  2  经典因子图方法与本文方法结构对比

下载: 全尺寸图片 幻灯片

3.   实验验证

实验分为仿真实验和实际跑车实验，仿真实验主要验证GNSS信号在遮挡、多径干扰、拒止、可见卫星不足等情况下的系统定位性能；实际跑车实验主要验证在车辆频繁启停的实际道路运行时系统定位性能和系统内存消耗情况。本文基于乔治亚理工学院开源的C++库GTSAM进行导航解算，使用iSAM2算法进行计算，iSAM2利用贝叶斯树表示最大后验概率密度，在保留稀疏性的基础上计算效率更高，其涉及的关键步骤包括变量顺序使用、解更新、非线性观测值处理等。

3.1   仿真实验

实验数据采用信号模拟器生成GNSS基带导航信号，经过GNSS软件接收机解算最终得到伪距和伪距率测量值及其权值矩阵。GNSS测量值输出速率为1 Hz, IMU测量值输出速率为100 Hz, GNSS与IMU具体的仿真参数如表1所示。

表  1  仿真传感器参数设置

相关参数	数值
陀螺仪零偏	[–180, 260, –160] °/h
角度随机游走	2.909×10–4 rad/ $\sqrt {\mathrm{s}}$
加速度计零偏	[9000, –13000, 8000] μG
速度随机游走	1000 ug/ $\sqrt {\rm{Hz}}$
初始位置误差	10 m
初始速度误差	0.1 m/s
可观测卫星数	8
卫星载噪比	40～45 dBHz

下载: 导出CSV 

| 显示表格

仿真的数据为一个车辆匀速运动的轨迹，坐标系统采用E系，车辆首先从坐标原点出发向西运动，运动持续时间为600 s，整个轨迹共进行了4次45°和1次90°转弯，可见卫星数量为8颗，车辆轨迹和卫星星空图分别如图3(a)和图3(b)所示。

图  3  仿真轨迹和星空图

下载: 全尺寸图片 幻灯片

为模拟复杂城市环境下多星受树荫遮挡、多径干扰以及建筑物遮挡等情况，80～95 s时段，将高仰角的4颗可见卫星(C7, C8, C11, C16)对应的基带导航信号噪声分量增加3～5 dB不等，模拟受树荫遮挡情况，使接收机对这些卫星信号锁定效果变差；200～235 s时段，在低仰角的4颗卫星(C1, C2, C6, C12)基带信号中加入2～3路多径干扰信号，模拟卫星信号受多径干扰情况；在400～450 s时间段，将方位角300°～330°范围内的5颗卫星(C1, C2, C6, C11, C12)设为不可见，模拟被高层建筑遮挡造成可见卫星不足4颗的场景，具体场景设置如表2所示。实验结果如图4所示，其中分别比较了卡尔曼滤波、经典因子图、本文方法共3种方法定位结果，首先，经典因子图方法由于使用了所有历史观测数据，定位误差比卡尔曼更小；另外，当GNSS数据出现异常时，本文方法由于能自适应地调节测量噪声协方差矩阵，而经典因子图使用了固定的测量噪声协方差矩阵，因此本文方法在各方向上的定位结果皆优于卡尔曼滤波算法与经典因子图方法。图5为3种方法速度误差实验结果，与经典因子图方法和卡尔曼滤波相比，本文方法速度估计误差更小。3种方法的定位误差和测速误差如表3所示，由表3可见，在上述实验场景中，本文方法在定位误差方面比卡尔曼滤波和经典因子图方法分别提高了75.24%和63.50%，在测速方面，本文方法较上述两种方法也提高了71.70%和42.26%。

表  2  仿真场景参数设置

序号	时间段(s)	可见卫星	多径干扰影响卫星	多径干扰数量	受遮挡卫星	受遮挡卫星载噪比(dBHz)	备注
1	1～80	C1/C2/C6/C7/C8/ C11/C12/C16	无	无	无	无	正常行驶阶段
2	80～95	C1/C2/C6/C12	无	无	C7/C8/C11/C16	38(C7/C8) 40(C11/C16)	部分卫星受遮挡阶段
3	95～200	C1/C2/C6/C7/ C8/ C11/C12/C16	无	无	无	无	正常行驶阶段
4	200～235	C7/C8/C11/C16	C1/C2/C6/C12	3(C1/C2) 2(C6/C12)	无	无	多径干扰阶段
5	235～400	C1/C2/C6/C7/C8/ C11/C12/C16	无	无	无	无	正常行驶阶段
6	400～450	C7/C8/C16	无	无	C1/C2/C6/C11/C12	不可见	小于4颗卫星可视阶段
7	450～600	C1/C2/C6/C7/C8/ C11/C12/C16	无	无	无	无	正常行驶阶段

下载: 导出CSV 

| 显示表格

图  4  本文方法与经典方法在定位误差的结果对比图

下载: 全尺寸图片 幻灯片

图  5  本文方法与经典方法在测速误差的结果对比图

下载: 全尺寸图片 幻灯片

表  3  3种方法的定位与测速性能对比(速度(m/s)/位置(m))

	卡尔曼滤波	经典因子图	本文方法
X轴方向	0.7189/2.0286	0.4187/1.4252	0.2215/0.5237
Y轴方向	0.6709/1.7552	0.3217/0.7746	0.2270/0.6821
Z轴方向	1.2893/6.6258	0.5941/4.5711	0.3317/1.5473

下载: 导出CSV 

| 显示表格

3.2   实际跑车实验

实验硬件平台中，GNSS/SINS组合导航设备使用华测CGI-410高精度MEMS组合导航接收机，采集CGI-410输出的原始GNSS和IMU测量数据用于后处理解算。同时采用NovAtel ProPak6+SPAN OEM6紧组合导航系统作为系统测量基准，CGI-410和NovAtel ProPak6共用一个接收天线，组合导航系统内部已进行初始化及杆臂校正，具体如图6所示，表4是上述使用导航设备的部分性能参数。跑车实验中车辆行驶在城市道路上，期间经过林荫道、高架桥底和城市峡谷道路。采集行驶时间为718 s的数据。由于模拟常规汽车等待红绿灯与礼让行人的情况，实验期间车辆存在多个零速时间段。

图  6  实验车辆和传感器搭建图

下载: 全尺寸图片 幻灯片

表  4  部分系统参数

设备名称	指标种类	相应数值
NovAtel ProPak6+SPAN紧组合系统	观测卫星系统	BDS/GPS/Galileo/GLONASS
	GNSS接收频点	B1/B2/L1/L2/L2C/E1/E5
	RTK定位精度	1 cm±1×10–6
	位置输出频率	10 Hz
	测速精度(RMS)	0.03 m/s
	授时精度(RMS)	20 ns
	陀螺仪测量范围	±1000 °/s
	陀螺仪零偏	<1 °/h
	角度随机游走	0.1 °/ $\sqrt{\rm h}$
	加速度计零偏	9.8×10–3 m/s2
	加速度计比例因子	4×10–4
CGI-410 GNSS/SINS组合导航接收机	IMU输出频率	100 Hz
	陀螺仪零偏	200 °/h
	加速度计零偏	9.8×10–3 m/s2
	角度随机游走	0.2 °/ $\sqrt {\rm h}$
	速度随机游走	0.2 m/(s· $\sqrt {\rm h}$)
	GNSS测量值输出频率	1 Hz

下载: 导出CSV 

| 显示表格

本文采用广义似然比检测方法作为检测器对目标车辆在过程中进行零速检测，检测结果如图7所示，检测结果曲线在图中取“0”代表检测到零速，取“1”表达车辆处于运动状态。由于存在环境高频噪声以及静止时汽车震动噪声，因此在检测器前加入低通滤波器以提高检测率，实验结果表明检测率达到93%时，误检率为0.7%。图8为本文方法与经典因子图在跑车实验中的定位性能对比，图9为本文方法与传统方法3维测试误差对比。由图9可见，在零速时间段内，当GNSS信号误差增大或大多数卫星信号不可见时，本文方法使用零速校正对系统状态进行约束，能有效降低由GNSS信号不可见时带来的组合导航性能迅速下降问题。

图  7  零速检测结果

下载: 全尺寸图片 幻灯片

图  8  本文方法与传统方法3维定位误差

下载: 全尺寸图片 幻灯片

图  9  本文方法与传统方法3维测速误差

下载: 全尺寸图片 幻灯片

实验中计算平台使用的处理器为Intel Core i7-13620H，主频为2.6 GHz，具有10核心/16线程。在因子图计算资源消耗方面，由于本方法在车辆零速期间只对状态进行修正而不增加因子节点，在存储的节点数和因子图边缘化等优化计算时间方面具有一定优势，图10是本文方法与经典因子图方法在车辆行驶过程中组合导航计算所存储的节点数和优化时间的对比图，整个实验路程行驶时间为718 s，其中车辆一共停止了5段，停止时间一共为200 s，从图10可以看出，在刚开始的时间段内，由于启停的次数有限，本文方法的节点数与经典因子图的基本一致，随着时间推移，启停次数增多后，本文方法相比于经典因子图优势越来越明显，在实验时间内，主节点数量从经典方法的718个降至518个，降低27.86%，优化所用总时长由43.89 s降至29.83 s，降低32.04%。其中，总时长是指在整个车辆行驶时间内所有定位点因子图算法的计算时间总和。节点数量与测量值是成对存在的，因此，节点数量的降低将带来存储量上的有效降低。当然，随着车辆停止的次数和时间增多，其节点数下降将更明显。另外，增量平滑的求解过程中涉及图维护，相比经典因子图，本文方法由于存储节点数的减少，当图具备一定规模后，在维护消耗时间方面具备一定优势。

图  10  本文方法与传统方法在节点数和优化所用时间上的对比

下载: 全尺寸图片 幻灯片

4.   结论

本文研究了复杂城市环境下GNSS/SINS组合导航方法，利用因子图对组合系统的状态量进行全局估计，从提高GNSS测量模型准确度和提高GNSS不可视情况下的组合导航系统定位性能两方面入手，分别对GNSS的信号测量误差进行建模，误差模型能准确反映信号受多径干扰和锁定。工作内容如下：

(1)利用GNSS接收机中的相干积分结果构造信号误差鉴别函数，实时估计信号在多径干扰、短暂失锁、拒止情况下的测量误差。本文方法相比经典因子图方法，可提高组合导航系统的定位精度63.50%和测速精度42.26 %；

(2)在GNSS信号拒止时，利用载体运动约束条件构建零速修正因子，对组合导航系统状态量进行修正，能有效提高组合导航系统的定位性能；同时随着载体零速情况的增加，本文方法存储的因子节点数量和优化时间，相比经典因子图均有大幅的下降。

实验表明，本文方法具备良好的导航性能、较低的存储量和较高的计算效率，在工程实践中具有较高的应用价值。