[发明专利]一种多源融合导航系统的导航方法和装置

说明书

本发明公开了一种多源融合导航系统的导航方法和装置，包括：构造零速检测统计量；构造因子图；构造零速修正因子节点；因子图的计算。本发明利用输出的位姿变化量作为检测量进行广义似然比零速检测，检测到载体处于零速时，不增加新的因子节点，有效减轻了因子图的存储量和计算量；多源融合导航系统的测量值仍然对系统状态进行约束和更新，保证了零速时系统状态值估计的正确性。

技术领域

本发明涉及卫星导航技术领域，特别是涉及GNSS（Global Navigation SatelliteSystem，全球卫星导航系统）与INS（Inertial Navigation System，惯性导航系统）、激光雷达（LiDAR）多源融合导航系统组合模式下，利用测量信号进行导航的方法和装置，特别适用于城市中行驶车辆的自动驾驶辅助导航设备中。

背景技术

车载导航系统的定位技术是实现导航和自动驾驶关键技术之一。多源导航系统通过融合多种传感器信息可有效提高系统的整体性能和环境适应能力，能实现卫星导航信号受干扰、遮挡等各类复杂环境下的稳定可靠定位。在多源融合算法方面，因子图能够解决多源融合系统中测量值的异步传输、即插即用、多速率数据的融合问题，同时也能解决状态方程或观测方程非线性的问题，具有良好的扩展性和灵活性，是近年研究的热点。但是，随着观测量的不断增加，因子图中产生的变量节点也会逐渐增多，这给实时系统带来巨大的计算和存储负担，对处理器的处理能力和存储能力都是重大挑战。

发明内容

为解决现有技术中存在的问题，本发明在扩展的增量平滑与建图方法（Incremental Smoothing and Mapping 2 , iSAM2）iSAM2的基础上，结合GNSS、INS和LiDAR构成的多源融合导航系统，将因子图算法进行优化。其中，因子图的概念于2001年由Kschischang等人提出，它是概率图模型的一种表示方式，用来表示全局函数和局部函数关系的模型图，被广泛应用于人工智能、信号处理、数字通信等领域，能对各种复杂概率问题进行求解。本发明采用因子图建模，其主要目的是将复杂的概率问题进行因式化分解，即将复杂的全局函数分解为多个局部函数相乘的形式，当已知局部函数之间的相关性时，只需要分析每个局部函数就可以得到全局复杂概率问题的解。因子图中含有因子节点与变量节点，两种节点通过局部函数相连构成双向图。因子图通过各传感器的观测量建立当前系统状态和前一状态的联系，并构造代价函数，通过求解系统状态量后验概率的全局或者局部最大值，获得当前状态最优估计值。因子图能够解决多源融合系统中测量值的异步传输、即插即用、多速率数据的融合问题，同时也能解决状态方程或观测方程非线性的问题，具有良好的扩展性和灵活性。

一方面，本发明提出一种多源融合导航系统的导航方法，包括：步骤1：构造零速检测统计量，利用零速检测统计量判断载体是否处于静止状态；步骤2：构造因子图，因子图用于将多源融合导航系统的各传感器的观测量建立当前系统状态和前一状态的联系并用于优化计算；步骤3：基于步骤1的判断结果，构造零速修正因子节点，零速修正因子节点用于计算载体的状态量，以降低因子图的存储和简化计算；步骤4：基于构造的零速修正因子节点，计算因子图，得到系统导航解，导航解提供载体的三维位置、三维速度、三维姿态角信息、载体本地时钟误差、时钟误差漂移率信息。

进一步的，多源融合导航系统为全球卫星导航系统GNSS接收机、惯性导航系统INS和激光雷达LiDAR组合的导航系统，其中，INS包括测量惯性单元IMU。

进一步的，步骤1还包括：

利用LiDAR进行点云匹配后输出的相对位姿参数作为检测统计量，构造广义似然比公式，得到如下零速检测公式：

（1）