一种GNSS/INS组合系统的导航方法和装置

一种gnss/ins组合系统的导航方法和装置
技术领域
1.本发明属于卫星导航技术领域，尤其涉及一种gnss(global navigationsatellite system，全球卫星导航系统)/ins(inertial navigation system， 惯性导航系统)组合系统的导航方法和系统。


背景技术：

2.gnss/ins紧组合模型通常采用gnss接收机输出的原始观测信息(如伪 距、伪距率、多普勒等)作为测量值，通过贝叶斯滤波器，和ins中惯性 测量单元(inertial measurement unit,imu)输出的测量值进行融合滤 波，得到系统最优的状态估计量。gnss/ins组合导航系统的紧组合模式相 比松组合模式，由于传感器之间的观测量不相关和gnss不需要独立进行定 位解算，因此，在可见卫星数量少于4颗时仍可以工作，且具备较高的导 航精度和系统可用性；而相比深组合模式，又具备计算量小、实时性强、 硬件实现成本低的优点。因此紧组合模式以其适中的计算量和良好的导航 性能，成为目前gnss/ins组合导航技术研究的热点。
3.然而在组合系统中，存在gnss接收机与imu的测量数据不同步，以及 gnss接收机测量值变差或不可用的问题，当使用传统的滤波方法时(如扩 展卡尔曼滤波、联邦卡尔曼滤波等)，会出现由观测时刻不一致引起的估计 误差，或者由滤波模型不准确而出现的结果发散等问题。
4.2001年，kschischang等人提出了因子图的概念，因子图是概率图模 型的一种表示方式，是一种用来表示全局函数和局部函数关系的模型图， 它被广泛应用于人工智能、信号处理、数字通信等领域，对各种复杂概率 问题进行求解。基于因子图建模的主要目的是将复杂的概率问题进行因式 化分解，即将复杂的全局函数分解为多个局部函数相乘的形式，当已知局 部函数之间的相关性时，只需要分析每个局部函数就可以得到全局复杂概 率问题的解。因子图模型中含有因子节点与变量节点，两种节点通过局部 函数相连构成双向图。在组合导航系统中，因子图通过各系统中各传感器 的观测量建立当前状态和前一状态的联系，并构造代价函数，通过求解系 统状态量的后验概率的全局或者局部最大值，获得当前最优状态的估计值。 因子图能够解决组合导航系统中各个传感器测量值的异步传输、即插即用 以及多速率数据的融合问题，同时也能解决状态方程或观测方程非线性的 问题，具有良好的扩展性和灵活性。然而，随着观测量的不断增加，因子 图中产生的因子节点也会逐渐增多，每次新增因子节点，都需要重新求解 已有各个因子节点中状态量的最优估计值，这带来了巨大的计算和存储负 担，将导致系统实时性不断下降。针对这个问题，有两种解决方法，第一 种方法是相同节点合并法，即通过检测算法将当前节点与过去节点进行比 较，把相同的节点合并为同一节点，从而控制图大小的增长；另一种方法 为节点移除法，将因子图中过去一些影响较小的节点进行移除，保证图的 稀疏性。上述两种方法虽然可以减缓图增大速度，但也存在不足，第一种 方法随着时间增加，检测代价会越来越高，且需要目标回到之前位置。而 第二种方式对节点移除过程比较繁琐，并且移除节点的选择本
身就是一个 问题。


技术实现要素：

5.为解决上述技术问题，本发明提出了一种gnss/ins组合系统的导航方 案，该方案涉及gnss与ins组合导航系统的紧组合模式下，利用测量信号 进行导航的方法和装置，特别适用于城市中行驶车辆的自动驾驶辅助导航 设备中。
6.该方案在扩展的增量平滑与建图方法(incremental smoothing andmapping 2,isam2)isam2的基础上，结合gnss/ins紧组合导航系统；同 时在传统的利用imu的三维加速度、姿态测量值的广义似然比零速检测的 基础上，增加了gnss接收机的载体速度测量值作为检测量；检测到载体处 于零速时，不增加新的因子节点，有效减轻了因子图的存储量和计算量； imu和gnss接收机的测量值仍然对系统状态进行约束和更新，保证了零速 时系统状态值估计的正确性。进一步，在因子图计算过程中，将imu测量 值进行预积分，预积分的时刻及时间间隔和gnss接收机输出的测量值时刻 及间隔一致，保证了两种传感器输出测量数据的同步。进一步，选择观测 质量最好的一颗卫星测量值作为零速修正因子，保证了零速修正的可靠性。
7.本发明第一方面公开了一种gnss/ins组合系统的导航方法。所述方法 包括：
8.步骤s1、基于全球卫星导航系统gnss输出的测量值和惯性导航系统 ins中的惯性测量单元imu输出的测量信息，构造所述gnss/ins组合系统 的零速检测统计量；
9.步骤s2、构造所述gnss/ins组合系统的因子图，所述因子图包括ins 节点和gnss节点，所述ins节点为所述因子图的过程模型因子节点，所述 gnss节点为所述因子图的测量模型因子节点，所述因子图表征所述 gnss/ins组合系统在各个测量时刻的系统状态量；
10.步骤s3、基于所述零速检测统计量确定载体的行进状态：
11.当所述载体为静止状态时，基于所述因子图构造零速校正因子节 点，并利用所述零速校正因子节点对所述载体进行定位，以获取所述载体 的位置和速度；
12.当所述载体为非静止状态时，基于所述因子图对所述载体进行定 位，以获取所述载体的位置和速度。
13.根据本发明第一方面的方法，在所述步骤s1中，利用所述gnss输出 的测量值计算载体速度，并基于所述载体速度和所述imu输出的测量信息 构造如以下公式的广义似然比，以得到所述零速检测统计量；所述imu输 出的测量信息为载体三维加速度和载体姿态信息；所述零速检测统计量的 表达式为：
[0014][0015]
其中，表示所述imu测量的第k个三个轴向的加速度，表示所述imu 测量的第k个三个轴向的角速度，表示由所述gnss测量的第k个载体 速度，表示所述imu的加速度测量方差，表示所述imu的角速度测量 方差，表示由所述gnss测量得到
的所述载体速度测量方差，n为检测 窗口长度，g为重力常量，其中为所述检测窗口长度n内的加 速度平均值，当所述零速检测统计量小于门限γ时，所述载体处于所述静止 状态。
[0016]
根据本发明第一方面的方法，在所述步骤s2中，构造所述因子图的所 述ins节点具体包括，基于所述gnss/ins组合系统在载体坐标系b下，在 测量时刻i，经过时间间隔t积分后得到的速度的变化量和姿态角的变 化量确定所述gnss/ins组合系统在地球地固坐标系e下，在测量时 刻i+1的速度估计值位置估计值和姿态角估计值以构造所 述ins节点在测量时刻i+1的状态量估计值
[0017]
根据本发明第一方面的方法，在所述步骤s2中，构造所述因子图的所 述gnss节点具体包括，基于所述gnss节点测量得到的在所述测量时刻i 对第j颗可见卫星的伪距ρ
i，j
、与所述伪距对应的伪距率第j颗可见卫 星在所述地球地固坐标系e下的位置和速度确定所述gnss节点 的在所述测量时刻i的状态量
[0018]
根据本发明第一方面的方法，在所述步骤s3中，当所述载体为所述静 止状态时，基于所述因子图构造所述零速校正因子节点包括，以所述载体 为所述静止状态为条件，约束所述ins节点的误差积累和所述gnss节点的 时钟漂移，以确定所述零速校正因子节点，所述零速校正因子节点包括ins 零速校正因子节点h
izupt_ins
和gnss零速校正因子节点h
izupt_gnss
。
[0019]
根据本发明第一方面的方法，在所述步骤s3中：
[0020]
当所述载体为所述静止状态时，利用所述零速校正因子节点，计算所 述gnss/ins组合系统的状态量的最大后验概率值，以对所述载体进行定位；
[0021]
当所述载体为非所述静止状态时，利用所述因子图中的所述ins节点 和所述gnss节点，计算所述gnss/ins组合系统的状态量的最大后验概率 值，以对所述载体进行定位。
[0022]
本发明第二方面公开了一种gnss/ins组合系统的导航装置。所述装置 包括：
[0023]
第一处理单元，被配置为，基于全球卫星导航系统gnss输出的测量值 和惯性导航系统ins中的惯性测量单元imu输出的测量信息，构造所述 gnss/ins组合系统的零速检测统计量；
[0024]
第二处理单元，被配置为，构造所述gnss/ins组合系统的因子图，所 述因子图包括ins节点和gnss节点，所述ins节点为所述因子图的过程模 型因子节点，所述gnss节点为所述因子图的测量模型因子节点，所述因子 图表征所述gnss/ins组合系统在各个测量时刻的系统状态量；
[0025]
第三处理单元，被配置为，基于所述零速检测统计量确定载体的行进 状态：
[0026]
当所述载体为静止状态时，基于所述因子图构造零速校正因子节 点，并利用所述零速校正因子节点对所述载体进行定位，以获取所述载体 的位置和速度；
[0027]
当所述载体为非静止状态时，基于所述因子图对所述载体进行定 位，以获取所述载体的位置和速度。
[0028]
根据本发明第二方面的装置，所述第一处理单元具体被配置为，利用 所述gnss输出的测量值计算载体速度，并基于所述载体速度和所述imu输 出的测量信息构造如以下公式的广义似然比，以得到所述零速检测统计量； 所述imu输出的测量信息为载体三维加速度和载体姿态信息；所述零速检 测统计量的表达式为：
[0029][0030]
其中，表示所述imu测量的第k个三个轴向的加速度，表示所述imu 测量的第k个三个轴向的角速度，表示由所述gnss测量的第k个载体 速度，表示所述imu的加速度测量方差，表示所述imu的角速度测量 方差，表示由所述gnss测量得到的所述载体速度测量方差，n为检测 窗口长度，g为重力常量，其中为所述检测窗口长度n内的加 速度平均值，当所述零速检测统计量小于门限γ时，所述载体处于所述静止 状态。
[0031]
根据本发明第二方面的装置，所述第二处理单元具体被配置为，构造 所述因子图的所述ins节点具体包括，基于所述gnss/ins组合系统在载体 坐标系b下，在测量时刻i，经过时间间隔t积分后得到的速度的变化量和姿态角的变化量确定所述gnss/ins组合系统在地球地固坐标系e 下，在测量时刻i+1的速度估计值位置估计值和姿态角估计值以构造所述ins节点在测量时刻i+1的状态量估计值
[0032]
根据本发明第二方面的装置，所述第二处理单元具体被配置为，构造 所述因子图的所述gnss节点具体包括，基于所述gnss节点测量得到的在 所述测量时刻i对第j颗可见卫星的伪距ρ
i，j
、与所述伪距对应的伪距率 第j颗可见卫星在所述地球地固坐标系e下的位置和速度确定所述gnss节点的在所述测量时刻i的状态量
[0033]
根据本发明第二方面的装置，所述第三处理单元具体被配置为，当所 述载体为所述静止状态时，基于所述因子图构造所述零速校正因子节点包 括，以所述载体为所述静止状态为条件，约束所述ins节点的误差积累和 所述gnss节点的时钟漂移，以确定所述零速校正因子节点，所述零速校正 因子节点包括ins零速校正因子节点h
izupt_ins
和gnss零速校正因子节点 h
izupt_gnss
。
[0034]
根据本发明第二方面的装置，所述第三处理单元具体被配置为，当所 述载体为所述静止状态时，利用所述零速校正因子节点，计算所述gnss/ins 组合系统的状态量的最大后验概率值，以对所述载体进行定位；
[0035]
当所述载体为非所述静止状态时，利用所述因子图中的所述ins节点 和所述gnss节点，计算所述gnss/ins组合系统的状态量的最大后验概率 值，以对所述载体进行定位。
[0036]
本发明第三方面公开了一种电子设备。所述电子设备包括存储器和处 理器，所述存储器存储有计算机程序，所述处理器执行所述计算机程序时， 实现本公开第一方面中任
一项所述的一种gnss/ins组合系统的导航方法中 的步骤。
[0037]
本发明第四方面公开了一种计算机可读存储介质。所述计算机可读存 储介质上存储有计算机程序，所述计算机程序被处理器执行时，实现本公 开第一方面中任一项所述的一种gnss/ins组合系统的导航方法中的步骤。
[0038]
本发明提供的技术方案与现有技术相比，优点在于：本发明在传统 的利用imu输出的三维加速度、姿态测量值作为广义似然比零速检测量 的基础上，增加了gnss接收机的载体速度测量值作为检测量，保证了检 测的稳定可靠及精确性。本发明在检测到载体处于零速时，不增加新的 因子节点，有效减轻了因子图的存储量和计算量；本发明在载体零速时， 将imu和gnss接收机的测量值对系统状态进行约束和更新，保证了系统 状态值估计的正确性。相比传统因子图方法，本发明在组合系统导航性 能不变的情况下，能有效降低因子图的存储量和计算量，特别适用于车 载gnss/ins紧组合系统在城市行驶中频繁启停的场景。
附图说明
[0039]
为了更清楚地说明本发明具体实施方式或现有技术中的技术方案下 面将对具体实施方式或现有技术描述中所需要使用的附图作简单地介 绍，显而易见地，下面描述中的附图是本发明的一些实施方式，对于本 领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这 些附图获得其他的附图。
[0040]
图1为根据本发明实施例的一种gnss/ins组合系统的导航方法的流 程图；
[0041]
图2a为根据本发明比较例的因子图；
[0042]
图2b为根据本发明实施例的因子图；
[0043]
图3为根据本发明实施例的车辆和传感器的搭建示意图；
[0044]
图4为根据本发明实施例的车辆行驶轨迹图；
[0045]
图5为根据本发明实施例的零速检测结果示意图；
[0046]
图6a为根据本发明实施例的计算时间结果比对的示意图；
[0047]
图6b为根据本发明实施例的轨迹比对的示意图；
[0048]
图6c为根据本发明实施例的定位精度比对的示意图；
[0049]
图7为根据本发明实施例的一种gnss/ins组合系统的导航系统的结 构图；
[0050]
图8为根据本发明实施例的一种电子设备的结构图。
具体实施方式
[0051]
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本 发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描 述，显然，所描述的实施例只是本发明一部分实施例，而不是全部的实施 例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动 前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0052]
本发明第一方面公开了一种gnss/ins组合系统的导航方法。图1为 根据本发明实施例的一种gnss/ins组合系统的导航方法的流程图；如图 1所示，所述方法包括：
[0053]
步骤s1、基于全球卫星导航系统gnss输出的测量值和惯性导航系统 ins中的惯性测量单元imu输出的测量信息，构造所述gnss/ins组合系 统的零速检测统计量；
[0054]
步骤s2、构造所述gnss/ins组合系统的因子图，所述因子图包括 ins节点和gnss节点，所述ins节点为所述因子图的过程模型因子节点， 所述gnss节点为所述因子图的测量模型因子节点，所述因子图表征所述 gnss/ins组合系统在各个测量时刻的系统状态量；
[0055]
步骤s3、基于所述零速检测统计量确定载体的行进状态：
[0056]
当所述载体为静止状态时，基于所述因子图构造零速校正因子 节点，并利用所述零速校正因子节点对所述载体进行定位，以获取所述 载体的位置和速度；
[0057]
当所述载体为非静止状态时，基于所述因子图对所述载体进行 定位，以获取所述载体的位置和速度。
[0058]
在步骤s1，基于全球卫星导航系统gnss输出的测量值和惯性导航系 统ins中的惯性测量单元imu输出的测量信息，构造所述gnss/ins组合 系统的零速检测统计量。
[0059]
在一些实施例中，在所述步骤s1中，利用所述gnss输出的测量值计 算载体速度，并基于所述载体速度和所述imu输出的测量信息构造如以下 公式的广义似然比，以得到所述零速检测统计量；所述imu输出的测量信 息为载体三维加速度和载体姿态信息；所述零速检测统计量的表达式为：
[0060][0061]
其中，表示所述imu测量的第k个三个轴向的加速度，表示所述imu 测量的第k个三个轴向的角速度，表示由所述gnss测量的第k个载 体速度，表示所述imu的加速度测量方差，表示所述imu的角速度 测量方差，表示由所述gnss测量得到的所述载体速度测量方差，n 为检测窗口长度，g为重力常量，其中为所述检测窗口长度 n内的加速度平均值，当所述零速检测统计量小于门限γ时，所述载体处 于所述静止状态。
[0062]
具体地，利用gnss输出的测量值计算载体速度值，再利用imu输出 的载体三维加速度和姿态信息，构造广义似然比公式，其中和分别表示imu测量的第k个三个轴向的加速度、角速度测量值和由gnss 测量的第k个载体速度，和分别为imu的加速度、角速度测 量方差以及由gnss接收机测量值得到的载体速度测量方差，n为检测窗 口长度，g为重力常量。其中为检测窗口长度n内的加速度 平均值，当检测量小于门限γ时认为载体处于静止状态。其中，||x||2＝x
t
x表 示x的二范数运算，表示二范数的平方根运算。
[0063]
在步骤s2，构造所述gnss/ins组合系统的因子图，所述因子图包括 ins节点和gnss节点，所述ins节点为所述因子图的过程模型因子节点， 所述gnss节点为所述因子图的测量模型因子节点，所述因子图表征所述 gnss/ins组合系统在各个测量时刻的系统状
fixed,地球地固坐标系)下测量时刻i时的位置、速度、姿态角， 由ins递推的原理可知这些为已知量；表示从测量时刻i到测量 时刻i+1，由b系变换到e系的旋转矩阵；ge表示重力矢量，在时间间隔 t内为一个常量。值得说明的是，测量时刻i到测量时刻i+1的时间间隔 是t。
[0076]
设过程模型因子节点(即ins节点)的状态量 上标“t”表示矩阵的转置运算， 则由前面的计算得到测量时刻i+1的ins节点的状态量的估计值 [0077]
在一些实施例中，在所述步骤s2中，构造所述因子图的所述gnss 节点具体包括，基于所述gnss节点测量得到的在所述测量时刻i对第j 颗可见卫星的伪距ρ
i，j
、与所述伪距对应的伪距率第j颗可见卫星在 所述地球地固坐标系e下的位置和速度确定所述gnss节点的 在所述测量时刻i的状态量
[0078]
具体地，已知测量时刻i，gnss接收机的测量值其 中ρi＝(ρ
i，1
，ρ
i，2
，
…
，ρ
i，j
…
，ρ
i，j
)
t
，ρ
i，j
为接收机在测量时刻i对第j颗可见卫星 的伪距测量值，的伪距测量值，为ρ
i，j
对应的伪距率。
[0079]
由导航电文可以得到测量时刻i第j颗卫星的在e系下的位置及 对应的速度则测量时刻i所有可见卫星的位置矢量和速度矢量为 和
[0080]
设gnss接收机在测量时刻i的状态量为设gnss接收机在测量时刻i的状态量为其中，δi为gnss接收机的本地时间和gnss系统时间的差，为δi的变化率。
[0081]
则gnss接收机的测量方程可以写为：
[0082][0083]
其中，c表示光速，表示接收机与卫星在视线方向上的位置大 小，表示接收机与卫星在视线方向上的速度大小。上式即表示估 计的伪距与伪距率。
[0084]
在步骤s3，基于所述零速检测统计量确定载体的行进状态：当所述 载体为静止状态时，基于所述因子图构造零速校正因子节点，并利用所 述零速校正因子节点对所述载体进行定位，以获取所述载体的位置和速 度；当所述载体为非静止状态时，基于所述因子图对所述载体进行定位， 以获取所述载体的位置和速度。
[0085]
在一些实施例中，当所述载体为所述静止状态时，基于所述因子图 构造所述零速校正因子节点包括，以所述载体为所述静止状态为条件， 约束所述ins节点的误差积累和所述gnss节点的时钟漂移，以确定所述 零速校正因子节点，所述零速校正因子节点包括ins零速校正因子节点 h
izupt_ins
和gnss零速校正因子节点h
izupt_gnss
。
[0086]
具体地，假设组合导航系统的载体为城市中行驶的车辆，当车辆处 于静止状态时，利用车辆速度为零的约束可以限制ins误差的积累和gnss 接收机的时钟漂移，此时不需要进行车辆位置和速度解算，极大的降低 了因子图中计算和存储的复杂度。当检测到车辆零速时，即车体坐标系 (m系)下三个方向的速度为零，即vm＝0。忽略ins安装角的偏差，认 为ins的载体坐标系(b系)与车体坐标系重合。但由于imu测量误差， 使得ins在测量时刻i由imu测量值实际计算得到的载体速度不为零， b系及e系下的速度矢量存在如下关系：
[0087][0088]
其中载体在i时刻e系下的速度可以由i-1时刻的速度计算得到，如 式所示。假设载体在导航坐标系(n系)下的姿态矢量为其 中αi、βi、γi分别为i时刻载体的航向角、横滚角、俯仰角。n系姿态矢量 到e系姿态矢量的转换关系为：
[0089][0090]
上式中，表示载体在i时刻由e系变换到n系的旋转矩阵。在时 间间隔δt内航向角的变化量δαi与姿态矢量各元素的关系为：
[0091][0092]
其中为载体在惯性系的角速度在δt内的变化量在b系下的投影。当载 体静止时，在航向角上的变化量为零，由上式可以约束imu输出的角速 度零偏值。可以得到ins节点的状态方程：
[0093][0094]
同时gnss接收机与卫星系统时的钟差和钟差变化率在载体静止时， 也可以用速度为零进行约束控制。选取一颗观测质量最好的卫星，在测 量时刻i得到该卫星的伪距率测量方程为：
[0095][0096]
其中为接收机与卫星之间的几何距离变化率，卫星的钟差变化率可由星 历计算得到，在测量方程中省略，其值归结到伪距率测量值中，ξi为测量 残差。为包括对流层电离层等在内的传播误差随时间的变化率，在短 时间内可以忽略。当用户静止时，接收机与卫星之间的几何距离变化率其实就是卫星的速度在卫星与接收机径向方向的大小。
可得gnss对应的 测量方程分别为：
[0097][0098]
当系统检测到车辆处于静止状态时，因子图中不增加新的因子节点， 对应矩阵的大小维持不变，只使用新测量的信息更新系统状态。图2a为 根据本发明比较例的因子图；图2b为根据本发明实施例的因子图。
[0099]
在一些实施例中，在所述步骤s3中：
[0100]
当所述载体为所述静止状态时，利用所述零速校正因子节点，计算 所述gnss/ins组合系统的状态量的最大后验概率值，以对所述载体进行 定位；
[0101]
当所述载体为非所述静止状态时，利用所述因子图中的所述ins节 点和所述gnss节点，计算所述gnss/ins组合系统的状态量的最大后验 概率值，以对所述载体进行定位。
[0102]
具体地，由上述推导得，测量时刻i+1，因子图系统状态量x
i+1
的最 大后验概率值x
i+1map
的表达式为：
[0103][0104]
其中，表示求矩阵的马氏距离的平方。
[0105]
当检测到载体处于静止时，利用零速修正因子节点进行计算状态量 的最大后验概率值x
i+1map
：
[0106][0107]
利用基于平滑和建图的标准c++的库gtsam(georgia tech smoothingand mapping)，采用第二版增量平滑与地图构建(incremental smoothingand mapping 2，isam2)算法对上述公式进行求解计算，可以得到系统 导航解。
[0108]
具体示例
[0109]
下面将通过gnss/ins紧组合实验对所提方法进行验证。图3为根据 本发明实施例的车辆和传感器的搭建示意图；如图3所示，gnss接收机 与imu分别采用诺瓦泰的propak6和iimu-fsas，其中gnss接收机输出 速率为1hz，imu输出速率为10hz。实验场景为城市道路，由于存在等待 红绿灯与礼让行人，实验期间存在多个零速时间段，图4为根据本发明 实施例的车辆行驶轨迹图；如图4中带箭头曲线所示，六角星标记为目 标起始点位置。同时propak6和iimu-fsas也组成诺瓦泰span紧组合导 航系统，以1hz的频率对外输出导航结果。本次实验在固定地点架设gnss 基准站，目的是使用rtk后处理数据的轨迹作为车辆行驶的参考轨迹， gnss基准站坐标精确已知，当目标在高架桥或建筑物遮挡区等卫星信号 差的地区时，参考轨迹使用组合导航系统输出的结果。
[0110]
零速检测实验分析
[0111]
采用广义似然比作为检测器对目标车辆进行零速检测，图5为根据 本发明实施例的零速检测结果示意图；如图5所示，其中检测结果曲线 在图中只有“0”和“15”两种取值，取值为“0”时代表检测到零速， 为“15”时表示目标处于运动状态。由于存在环境高频噪声以及静止时 汽车震动噪声，因此在检测器前加入低通滤波器以提高检测率，实验结 果表
明检测率达到90％时，误检率为1.7％。
[0112]
导航定位实验分析
[0113]
实验条件同上，车辆在城市中行驶过程中，有频繁启停的情况，利 用本文所提方法对gnss接收机和imu的测量值利用因子图模型进行计 算，实时得到车辆的位置、速度和姿态等信息。图6a为根据本发明实施 例的计算时间结果比对的示意图。行驶时间为300秒，其中车辆停止时 间为80秒，从图中可以看出，在刚开始的时间段内，由于启停的次数有 限，本文方法的计算时间与经典因子图的基本一致，随着时间推移，启 停次数增多后，本文方法计算时间增速较缓，相比于经典因子图在计算 代价方面优势越来越明显，实时性提升显著，而经典因子图的图增大的 速度与时间呈线性关系。对因子图中节点数量进行统计，在本次300秒 的实验时间内，主节点数量从经典方法的900个降至681个，降低24.3％。 当然，随着车辆停止的次数和时间增多，其节点数下降将更明显。
[0114]
另外，图6b为根据本发明实施例的轨迹比对的示意图；图6c为根 据本发明实施例的定位精度比对的示意图；可见，本文方法与经典因子 图方法，在车辆行驶轨迹上基本重合且与参考轨迹一致，在水平定位精 度上，本文方法在车辆零速区间不更新因子图的状态节点，只是利用imu 和gnss接收机输出的测量值来约束系统状态，总体性能水平与经典因子 图相当。
[0115]
本发明第二方面公开了一种gnss/ins组合系统的导航装置。图7为根 据本发明实施例的一种gnss/ins组合系统的导航系统的结构图；如图7 所示，所述装置700包括：
[0116]
第一处理单元701，被配置为，基于全球卫星导航系统gnss输出的测 量值和惯性导航系统ins中的惯性测量单元imu输出的测量信息，构造所 述gnss/ins组合系统的零速检测统计量；
[0117]
第二处理单元702，被配置为，构造所述gnss/ins组合系统的因子图， 所述因子图包括ins节点和gnss节点，所述ins节点为所述因子图的过程 模型因子节点，所述gnss节点为所述因子图的测量模型因子节点，所述因 子图表征所述gnss/ins组合系统在各个测量时刻的系统状态量；
[0118]
第三处理单元703，被配置为，基于所述零速检测统计量确定载体的行 进状态：
[0119]
当所述载体为静止状态时，基于所述因子图构造零速校正因子节 点，并利用所述零速校正因子节点对所述载体进行定位，以获取所述载体 的位置和速度；
[0120]
当所述载体为非静止状态时，基于所述因子图对所述载体进行定 位，以获取所述载体的位置和速度。
[0121]
根据本发明第二方面的装置，所述第一处理单元701具体被配置为， 利用所述gnss输出的测量值计算载体速度，并基于所述载体速度和所述imu 输出的测量信息构造如以下公式的广义似然比，以得到所述零速检测统计 量；所述imu输出的测量信息为载体三维加速度和载体姿态信息；所述零 速检测统计量的表达式为：
[0122][0123]
其中，表示所述imu测量的第k个三个轴向的加速度，表示所述imu 测量的
第k个三个轴向的角速度，表示由所述gnss测量的第k个载体 速度，表示所述imu的加速度测量方差，表示所述imu的角速度测量 方差，表示由所述gnss测量得到的所述载体速度测量方差，n为检测 窗口长度，g为重力常量，其中为所述检测窗口长度n内的加 速度平均值，当所述零速检测统计量小于门限γ时，所述载体处于所述静止 状态。
[0124]
根据本发明第二方面的装置，所述第二处理单元702具体被配置为， 构造所述因子图的所述ins节点具体包括，基于所述gnss/ins组合系统在 载体坐标系b下，在测量时刻i，经过时间间隔t积分后得到的速度的变化 量和姿态角的变化量确定所述gnss/ins组合系统在地球地固坐标 系e下，在测量时刻i+1的速度估计值位置估计值和姿态角估计 值以构造所述ins节点在测量时刻i+1的状态量估计值
[0125]
根据本发明第二方面的装置，所述第二处理单元702具体被配置为， 构造所述因子图的所述gnss节点具体包括，基于所述gnss节点测量得到 的在所述测量时刻i对第j颗可见卫星的伪距ρ
i，j
、与所述伪距对应的伪距 率第j颗可见卫星在所述地球地固坐标系e下的位置和速度确定所述gnss节点的在所述测量时刻i的状态量
[0126]
根据本发明第二方面的装置，所述第三处理单元703具体被配置为， 当所述载体为所述静止状态时，基于所述因子图构造所述零速校正因子节 点包括，以所述载体为所述静止状态为条件，约束所述ins节点的误差积 累和所述gnss节点的时钟漂移，以确定所述零速校正因子节点，所述零速 校正因子节点包括ins零速校正因子节点h
izupt_ins
和gnss零速校正因子节 点h
izupt_gnss
。
[0127]
根据本发明第二方面的装置，所述第三处理单元703具体被配置为， 当所述载体为所述静止状态时，利用所述零速校正因子节点，计算所述 gnss/ins组合系统的状态量的最大后验概率值，以对所述载体进行定位；
[0128]
当所述载体为非所述静止状态时，利用所述因子图中的所述ins节点 和所述gnss节点，计算所述gnss/ins组合系统的状态量的最大后验概率 值，以对所述载体进行定位。
[0129]
本发明第三方面公开了一种电子设备。所述电子设备包括存储器和处 理器，所述存储器存储有计算机程序，所述处理器执行所述计算机程序时， 实现本公开第一方面中任一项所述的一种gnss/ins组合系统的导航方法中 的步骤。
[0130]
图8为根据本发明实施例的一种电子设备的结构图，如图8所示，电 子设备包括通过系统总线连接的处理器、存储器、通信接口、显示屏和输 入装置。其中，该电子设备的处理器用于提供计算和控制能力。该电子设 备的存储器包括非易失性存储介质、内存储器。该非易失性存储介质存储 有操作系统和计算机程序。该内存储器为非易失性存储介质中的操作系统 和计算机程序的运行提供环境。该电子设备的通信接口用于与外部的终端 进行有线或无线方式的通信，无线方式可通过wifi、运营商网络、近场通 信(nfc)或其他技术实现。该电子设备的显示屏可以是液晶显示屏或者电 子墨水显示屏，该电子设备的输入装
置可以是显示屏上覆盖的触摸层，也 可以是电子设备外壳上设置的按键、轨迹球或触控板，还可以是外接的键 盘、触控板或鼠标等。
[0131]
本领域技术人员可以理解，图8中示出的结构，仅仅是与本公开的技 术方案相关的部分的结构图，并不构成对本技术方案所应用于其上的电子 设备的限定，具体的电子设备可以包括比图中所示更多或更少的部件，或 者组合某些部件，或者具有不同的部件布置。
[0132]
本发明第四方面公开了一种计算机可读存储介质。所述计算机可读存 储介质上存储有计算机程序，所述计算机程序被处理器执行时，实现本公 开第一方面中任一项所述的一种gnss/ins组合系统的导航方法中的步骤。
[0133]
本发明提供的技术方案与现有技术相比，优点在于：本发明在传统 的利用imu输出的三维加速度、姿态测量值作为广义似然比零速检测量 的基础上，增加了gnss接收机的载体速度测量值作为检测量，保证了检 测的稳定可靠及精确性。本发明在检测到载体处于零速时，不增加新的 因子节点，有效减轻了因子图的存储量和计算量；本发明在载体零速时， 将imu和gnss接收机的测量值对系统状态进行约束和更新，保证了系统 状态值估计的正确性。相比传统因子图方法，本发明在组合系统导航性 能不变的情况下，能有效降低因子图的存储量和计算量，特别适用于车 载gnss/ins紧组合系统在城市行驶中频繁启停的场景。
[0134]
请注意，以上实施例的各技术特征可以进行任意的组合，为使描述简 洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而， 只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。 以上所述实施例仅表达了本技术的几种实施方式，其描述较为具体和详细， 但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领 域的普通技术人员来说，在不脱离本技术构思的前提下，还可以做出若干 变形和改进，这些都属于本技术的保护范围。因此，本技术专利的保护范 围应以所附权利要求为准。