CN113375666B - 一种传感器融合定位***及方法 - Google Patents

Abstract

本申请实施例中提供了一种传感器融合定位***及方法，属于自动驾驶技术领域，具体包括传感器、导航结果模块和至少两个导航模块，所述导航模块用于接收所述传感器信号，并输出车辆状态信息给所述导航结果模块；所述导航模块的输入端与所述传感器的输出端连接，所述导航模块的输出端与所述导航结果模块的输入端连接；且其中一个所述导航模块与其他所述导航模块运行不同的传感器融合算法，所述与其他所述导航模块运行不同的传感器融合算法的导航模块设为主导航模块，其他所述导航模块设为冗余导航模块。通过本申请的处理方案，为复杂环境中的车辆提供鲁棒的定位和导航结果，提高了多种环境下全局位姿的鲁棒性，提高了定位***的容错性和稳定性。

Description

一种传感器融合定位***及方法

技术领域

本申请涉及自动驾驶技术领域，尤其涉及一种传感器融合定位***及方法。

背景技术

自动驾驶领域近年来蓬勃发展，高精度的车辆定位技术是实现无人驾驶功能的重要基础，越来越多的研究机构在车辆定位这一领域展开深入研究,高精度的定位***不仅技术难度复杂而且要适应各种动态环境是极其困难的。

自动驾驶车辆定位的本质是状态估计问题，通常为以单一状态估计器为主的集中式设计，而基于单一状态估计的自动驾驶***缺乏容错机制，任何传感器的故障或者状态估计器自身解算的错误都可能会导致定位导航输出异常，为行驶中的自动驾驶车辆带来风险。此外，车辆定位所用的卫星信号的质量高低或者卫星信号的丢失，都会严重影响自动驾驶车辆的定位结果。

发明内容

有鉴于此，本申请实施例提供一种传感器融合定位***及方法，至少部分解决现有技术中存在的问题。

第一方面，本申请实施例提供一种传感器融合定位***，包括传感器、导航结果模块和至少两个导航模块，所述导航模块用于接收所述传感器信号，并输出车辆状态信息给所述导航结果模块；

所述导航模块的输入端与所述传感器的输出端连接，所述导航模块的输出端与所述导航结果模块的输入端连接；

且其中一个所述导航模块与其他所述导航模块运行不同的传感器融合算法，所述与其他所述导航模块运行不同的传感器融合算法的导航模块设为主导航模块，其他所述导航模块设为冗余导航模块。

根据本申请实施例的一种具体实现方式，所述***还包括故障诊断模块和输出选择模块，

所述故障诊断模块的输入端与各个所述导航模块的输出端连接，所述故障诊断模块的输出端与所述输出选择模块的输入端连接；

所述输出选择模块的输入端还与各个所述导航模块的输出端连接，所述输出选择模块的输出端与所述导航结果模块的输入端连接；

所述故障诊断模块用于监测所述导航模块是否出现故障，并输出指令到所述输出选择模块，所述输出选择模块根据所述指令对所述导航模块进行选择。

根据本申请实施例的一种具体实现方式，所述***还包括卫星完好性监测模块，用于监控卫星状态是否良好；所述卫星完好性监测模块的输入端与GNSS连接，所述卫星完好性监测模块的输出端分别与所述冗余导航模块的输入端和所述输出选择模块的输入端连接。

根据本申请实施例的一种具体实现方式，所述卫星完好性监测模块的输入端接收所述GNSS的卫星报文，所述卫星报文包括可视卫星数、高度角、信噪比、精度因子、测量值中误差、解状态和ratio值中的一种或几种。

根据本申请实施例的一种具体实现方式，所述传感器包括IMU、GNSS、RTK、激光雷达、RGB相机、深度相机、轮速计、气压计、地磁传感器、毫米波雷达和超声波雷达中的一种或几种。

根据本申请实施例的一种具体实现方式，所述主导航模块的输入端连接的所述传感器包括所述IMU和所述GNSS，所述冗余导航模块的输入端连接的所述传感器包括所述IMU和所述激光雷达。

根据本申请实施例的一种具体实现方式，所述传感器融合算法包括标准卡尔曼滤波、因子图、扩展卡尔曼滤波、无迹卡尔曼滤波、迭代卡尔曼滤波和粒子滤波中的一种或几种。

根据本申请实施例的一种具体实现方式，所述主导航模块运行的传感器融合算法为标准卡尔曼滤波，所述冗余导航模块运行的传感器融合算法为因子图。

第二方面，本申请实施例还提供一种传感器融合定位方法，所述方法包括：

主导航模块和/或冗余导航模块接收传感器数据；

所述主导航模块和冗余导航模块利用互为不同的传感器融合算法进行计算，得到车辆状态信息，通过导航结果模块进行输出；

所述主导航模块和冗余导航模块中的一个出现故障时，另一个进行接替，并进行所述车辆状态的输出。

根据本申请实施例的一种具体实现方式，判断所述主导航模块和所述冗余导航模块是否出现故障的方法为：

通过故障诊断模块监测所述导航模块是否出现故障，并输出指令到输出选择模块，所述输出选择模块根据所述指令对所述导航模块进行选择。

有益效果

本申请实施例中的传感器融合定位***及方法，通过设置两套并行的状态估计器对车辆状态实时解算，一套作为主导航模块，一套作为冗余导航模块，运行不同的传感器融合算法，互为冗余、互为备份、互相交叉验证，当有其中某一导航模块故障产生后，另一导航模块能够立即接替并持续输出，保证自动驾驶车辆***运行的连续性。

通过设置基于因子图实现的多传感器融合冗余模块，内部运行激光雷达里程计单元，可以在卫星信号较差、IMU积分发散的情况下提供较长时间的航位推算结果，进一步提高了***的稳定性，为复杂环境中的车辆提供鲁棒的定位和导航结果。

通过设置动态监控卫星状态并进行导航状态故障诊断的功能，可以实时根据监测结果和内部逻辑动态切换输出结果，保障车辆导航状态的连续性，同时丰富的导航状态标志位方便为其它上层模块如规划和控制单元随时采取应急预案提供可能。

附图说明

为了更清楚地说明本申请实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其它的附图。

图1为根据本发明一实施例的传感器融合定位***示意图；

图2为根据本发明一实施例的传感器融合定位***示意图；

图3为根据本发明一实施例的主导航模块的原理图；

图4为根据本发明一实施例的冗余导航模块的原理图；

图5为根据本发明一实施例的传感器融合定位***的帧结构及状态标志位。

具体实施方式

下面结合附图对本申请实施例进行详细描述。

以下通过特定的具体实例说明本申请的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本申请的其他优点与功效。显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。本申请还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本申请的精神下进行各种修饰或改变。需说明的是，在不冲突的情况下，以下实施例及实施例中的特征可以相互组合。基于本申请中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。

要说明的是，下文描述在所附权利要求书的范围内的实施例的各种方面。应显而易见，本文中所描述的方面可体现于广泛多种形式中，且本文中所描述的任何特定结构及/或功能仅为说明性的。基于本申请，所属领域的技术人员应了解，本文中所描述的一个方面可与任何其它方面独立地实施，且可以各种方式组合这些方面中的两者或两者以上。举例来说，可使用本文中所阐述的任何数目个方面来实施设备及/或实践方法。另外，可使用除了本文中所阐述的方面中的一或多者之外的其它结构及/或功能性实施此设备及/或实践此方法。

还需要说明的是，以下实施例中所提供的图示仅以示意方式说明本申请的基本构想，图式中仅显示与本申请中有关的组件而非按照实际实施时的组件数目、形状及尺寸绘制，其实际实施时各组件的型态、数量及比例可为一种随意的改变，且其组件布局型态也可能更为复杂。

另外，在以下描述中，提供具体细节是为了便于透彻理解实例。然而，所属领域的技术人员将理解，可在没有这些特定细节的情况下实践所述方面。

全球卫星导航***(Global Navigation Satellite System,GNSS)是车辆定位应用最普遍的技术，通常可以在户外环境中提供较准确的绝对位置信息。然而卫星定位的弊端是显而易见的，无法在卫星信号遮挡的隧道，城市峡谷，高架桥等区域提供定位信息。

车辆定位在实际应用中多采用以惯性导航+卫星导航为主的组合导航方案，通常使用卡尔曼滤波器做融合，惯性传感器单元(Inertial Measurement Unit，IMU)进行高频的航位推算和卫星定位的低频修正，在加上轮速信息的修正作用，在卫星有遮挡的区域也具备一定的定位能力；近年来，有研究者使用优化的框架代替传统滤波器，在IMU、GNSS、轮速的基础上融入更多的传感器类型，例如相机和激光雷达，从而使得定位信息更加鲁棒。

但是，通常车辆的定位***的多传感器融合采用的是集中式状态估计实现，通常为多个传感器测量值共享一个状态估计器，常见的传感器包括IMU、GNSS、轮速计、相机、激光雷达等，所有的传感器测量数据进入同一个状态估计器，由状态估计器实现信息融合输出车辆状态(包括位置、速度、姿态、加速度、角速度等)。IMU传感器测量的是角速度和加速度值，通常包含测量噪声和零点漂移，积分误差会随时间不断增加，需要状态估计器融合其它传感器(如GNSS)不断提供的观测量进行修正，最终输出修正后的车辆状态。

对于上述现有的车辆定位***研究发现，目前基于单一状态估计的自动驾驶***缺乏容错机制，任何传感器的故障或者状态估计器自身解算的错误都可能会导致定位导航输出异常，为行驶中的自动驾驶车辆带来风险。而且，卫星信号的质量高低对于定位结果的影响非常大，传统定位算法缺乏对卫星信号的动态监控机制；同时缺少基于冗余机制的故障诊断单元对***进行实时诊断。传统组合导航算法依靠IMU、轮速、GNSS等传感器融合的方案，在卫星信号丢失后误差量会迅速发散，导致定位输出结果很快超出自动驾驶车辆定位精度允许范围。

为了改善现有车辆定位***的缺陷，本申请实施例提供了一种传感器融合定位***，本***主要包括传感器、导航结果模块和至少两个导航模块，导航模块用于接收传感器信号，并输出车辆状态信息给导航结果模块；导航模块的输入端与传感器的输出端连接，导航模块的输出端与导航结果模块的输入端连接；且其中一个导航模块与其他导航模块运行不同的传感器融合算法，与其他导航模块运行不同的传感器融合算法的导航模块设为主导航模块，其他导航模块设为冗余导航模块。

传感器融合是将感觉数据或来自不同来源的数据组合在一起，以此得到的信息相比于单独使用某一来源的数据具有更小的不确定性，其中导航模块为运行状态估计器，由状态估计器实现传感器信息融合输出车辆状态，车辆状态主要包括位置、速度、姿态、加速度、角速度等。

在一个实施例中，传感器融合定位***中的导航模块为两个，其中一个为主导航模块，另一个为冗余导航模块，具体***结构参照附图1，主导航模块和冗余导航模块互为冗余、互为备份、互相交叉验证，当有其中某一导航模块故障产生后，另一导航模块能够立即接替并持续输出，保证自动驾驶车辆***运行的连续性。

进一步的，传感器融合定位***还包括故障诊断模块和输出选择模块，参照图2，图中包括一个主导航模块和一个冗余导航模块，需要解释的是，冗余导航模块可以为多个，并不局限于图2中所示的冗余导航模块，若其中某个冗余导航模块故障时，可利用其他冗余导航模块进行及时替换。故障诊断模块的输入端与各个导航模块的输出端连接，故障诊断模块的输出端与输出选择模块的输入端连接；输出选择模块的输入端还与各个导航模块的输出端连接，输出选择模块的输出端与导航结果模块的输入端连接。

故障诊断模块用于监测导航模块是否出现故障，包括主导航模块和冗余导航模块，并输出指令到输出选择模块，输出选择模块根据指令对导航模块进行选择。具体地说，故障诊断模块用于监测主导航模块和冗余导航模块的状态，通过一致性检查、平滑性检查来判断主导航模块和冗余导航模块是否出现故障，输出指令到输出选择模块中，所述指令为一致性检查和平滑性检查的结果。其中，一致性检查用于判断导航模块是否故障，平滑性检查用来判断导航模块数据平滑性的高低。输出选择模块综合来自故障诊断模块的消息，对导航结果的来源(来自主导航模块或冗余导航模块)进行选择，并打包组成导航结果消息发送给导航结果模块。

更进一步的，传感器融合定位***还包括卫星完好性监测模块，用于监控卫星状态是否良好；卫星完好性监测模块的输入端与GNSS连接，卫星完好性监测模块的输出端分别与冗余导航模块的输入端和输出选择模块的输入端连接，具体连接关系参照图2。其中，GNSS用于输入卫星报文，卫星完好性监测模块可以实时根据卫星报文中多种指标的数据，计算出当前卫星定位结果的置信度，为后续功能模块提供更加丰富的定位信息。

根据本申请实施例的一种具体实现方式，卫星完好性监测模块的输入端接收GNSS的卫星报文，卫星报文包括可视卫星数、高度角、信噪比、精度因子、测量值中误差、解状态和ratio值中的一种或几种。

在一个具体的实施例中，传感器包括IMU、GNSS、RTK(实时动态定位)、GPS、激光雷达、RGB相机、深度相机、轮速计、气压计、地磁传感器、毫米波雷达和超声波雷达中的一种或几种，传感器用于将车辆的状态信息以信号的形式传递给导航模块，并进行具体的传感器融合计算。

进一步的，传感器融合算法包括标准卡尔曼滤波、因子图、扩展卡尔曼滤波、无迹卡尔曼滤波、迭代卡尔曼滤波和粒子滤波中的一种或几种。

在一个实施例中，主导航模块的输入端连接的传感器包括IMU和GNSS，主导航模块运行的传感器融合算法为标准卡尔曼滤波；冗余导航模块的输入端连接的传感器包括IMU和激光雷达，冗余导航模块运行的传感器融合算法为因子图。本实施例中，因子图框架中融合了激光雷达里程计，在卫星信号受到影响时，缺少全局位姿信息约束情况下，可以极大减小车辆位置累计误差发散的速度，为***应急反应预留出极大的时间窗口。

更进一步的，冗余导航模块的输入端还连接有离线点云地图，离线点云地图是预先设置好的，激光雷达获取的实时点云与离线点云地图的匹配，为因子图融合提供除了卫星信号外的第二个全局位姿信息，提高了多种环境下全局位姿的鲁棒性。

在具体的工作过程中，主导航模块的输入端连接有IMU和GNSS，即其输入值为IMU的加速度和角速度以及GNSS的位置和速度，所用融合方法为卡尔曼滤波法，输出为车辆状态中的位置、速度和姿态。具体工作流程参照图3所示，IMU输入的加速度和角速度，经过捷联惯导状态递推方程(机械编排)进行状态时间更新和方差的时间更新，输出位置、速度等信息；同时卫星信号通过GNSS接收机转换为位置和速度观测量信息，通过卡尔曼滤波器量测方程与状态量(位置、速度和姿态)建立关系，并进行量测更新(计算出卡尔曼增益、更新状态和修正方差),最终输出更新后的组合导航输出(车辆状态)，同时计算出的修正量反馈回捷联惯导的状态递推方程来修正误差。

冗余导航模块的输入端连接有IMU、激光雷达和离线点云地图，即其输入值为IMU的加速度和角速度、激光雷达点云和离线点云地图，所用融合方法为因子图法，输出为车辆状态中的位置、速度和姿态。因子图使用图的理论来对物体进行状态估计，物体运动不同时刻的状态定义为节点，不同运动状态之间的运动方程和观测量定义为节点之间的因子，例如，t时刻和t+1时刻物体状态量(位置、速度、姿态)通过IMU的因子连接起来，同时两个时刻状态之间还有通过激光里程计计算得到的相对运动因子，除此以外还有GNSS和点云地图匹配产生的单边因子对相应时刻的节点产生全局约束。

具体的，冗余导航模块包括预处理模块，激光里程计模块，点云匹配模块，以及因子图优化模块，参照图4所示，冗余模块具体处理流程如下：预处理模块使用IMU预积分帮助去除激光雷达扫描的当前点云因为运动引起的畸变，即运动畸变补偿；去除畸变后的点云传送给激光里程计模块，在这里主要完成点云和更新后的局部地图之间的配准，即帧-局部地图匹配，得到递推的里程计；去除畸变的点云会同时传送给点云匹配模块，实时的点云与预先建立的离线点云地图(图中的全局点云地图)之间进行NDT(Normal DistributionTransform，正态分布变换)地图匹配以获得全局位姿；因子图优化模块首先将预积分结果、激光里程计结果、NDT匹配结果作为因子，按照时间戳添加到不同的状态量中(多观测量时间对齐)以更新因子图，最终以滑动窗口的形式对窗口内的状态量进行优化，获得最终更新结果。

卫星完好性监测模块的输入值为卫星报文中的可视卫星数、高度角、信噪比、精度因子、测量值中误差、解状态和ratio值，需要解释的是，卫星报文并不限定于本实施例中所列举的，可根据监测需求进行适当的更改，其输出为卫星信号置信度系数，系数值的范围为0-1，系数值越大，说明卫星信号越可靠。卫星完好性监测模块输出的结果，为因子图中是否融合GNSS数据提供参考，同时输出的结果也会传输到输出选择模块，为导航信息来源是否切换提供参考。

置信度系数＝(可视卫星数*归一化系数a*权值A+高度角*归一化系数b*权值B+信噪比*归一化系数c*权值C+精度因子*归一化系数d*权值D+测量值中误差*归一化系数e*权值E+解状态*ratio值*归一化系数g*权值G)*归一化系数K，其中，

归一化系数a-g由x＝(X-X_min)/(X_max-X_min)确定，其中X_max，X_min分别代表可视卫星数、高度角、信噪比、精度因子、测量值中误差和解状态各自状态的最大值和最小值，X代表当前测量值，x代表计算的归一化系数；权值A-G为大量测试得到经验值，通常为0-1之间；归一化系数K由所关心卫星报文的数量决定，例如上面有可视卫星数、高度角、信噪比、精度因子、测量值中误差、解状态、ratio值共7中指标，因此归一化系数K＝1/7。

故障诊断模块的输入值为主导航模块输出的位置、速度、姿态，冗余导航模块输出的位置、速度、姿态，输出值为导航模块的状态信息。

故障诊断模块的具体判断方法包括两个阶段，其中一个阶段为一致性检查，定义

为第k帧数据的一致性系数，表示n个历史数据(主导航输出值与冗余导航输出值之差)的累加二范数。其中p^M和p^A分别为主导航模块和冗余导航模块的速度，位置，姿态等车辆导航状态。如果满足位置一致性系数大于阈值DP，或速度一致性系数大于阈值DV,或姿态一致性系数大于阈值DA,则定义为一致性低，导航***故障；反之，一致性系数均小于预设阈值，则定义为一致性高，导航***非故障。其中位置，速度，姿态均为三维向量，阈值DP、DV、DA均为经验值。

另一个阶段为平滑性检查，定义

为第k帧的平滑性系数，表示n个前后帧结果之差的二范数的累加和。其中p可以为速度，位置，姿态等车辆导航状态。如果满足平滑性系数大于SP，或速度平滑性系数大于阈值SV,或姿态平滑性系数大于阈值SA,则定义为导航***平滑性低。反之，平滑性系数均小于等于预定阈值，则定义为导航***平滑性高。阈值SP、SV、SA均为经验值。

输出选择模块的输入值为：故障诊断模块输出标志位、卫星完好性监测模块、主导航模块输出、冗余导航模块输出，输出值为包含状态位的导航结果。

输出选择模块的具体选择方法为：

如果无故障产生，且主导航模块数据平滑性高，则输出主导航模块结果，并置位相应状态标志位；

如果无故障产生，且主导航模块数据平滑性低，冗余导航模块平滑性高，则输出冗余导航模块结果，并置位相应状态标志位；

如果无故障产生，且主导航模块和冗余导航模块平滑性均为低，则输出主导航模块结果，并置位相应状态标志位；

如果故障产生，则输出位置、速度、姿态置0，并置位相应状态标志位。

卫星信号置信度系数乘以10并取整归一化到1-10的整数，由消息状态1字节中高4位表示，具体的表示参照附图5。

输出选择模块综合卫星信号置信度系数以及故障诊断模块的输出，对输出结果进行选择性输出，大大提高了定位***的准确性。

第二方面，本申请实施例还提供一种多传感器融合定位方法，方法包括：

主导航模块和/或冗余导航模块接收传感器数据；

所述主导航模块和冗余导航模块利用互为不同的传感器融合算法进行计算，得到车辆状态信息，通过导航结果模块进行输出；

所述主导航模块和冗余导航模块中的一个出现故障时，另一个进行接替，并进行所述车辆状态的输出。

在上述实施例中，主导航模块接收的传感器包括IMU和GNSS，所用的传感器融合算法为卡尔曼滤波器法；冗余导航模块接收的传感器包括IMU、激光雷达，此外还连接有离线点云地图，所用的传感器融合算法为因子图法。需要解释的是，传感器的类型以及传感器融合方法并不限定于本实施例中所列举的，可以根据实际的定位需求进行更改。

进一步的，判断所述主导航模块和所述冗余导航模块是否出现故障的方法为：通过故障诊断模块监测所述导航模块是否出现故障，并输出指令到输出选择模块，所述输出选择模块根据所述指令对所述导航模块进行选择。

进一步的，判断所述主导航模块和所述冗余导航模块是否出现故障的方法还包括：利用卫星完好性监测模块对卫星报文进行计算，得到卫星信号置信度系数，为因子图中是否融合GNSS数据提供参考，同时也会传输到输出选择模块，为导航信息来源是否切换提供参考。

本发明提供的实施例，针对现有的自动驾驶定位***缺乏容错机制，以及卫星信号的质量高低对定位结果影响大的问题，发明了一种传感器融合定位***及方法，针对单一状态估计可靠性、容错性不高的问题，设计了两套并行运行状态估计器，一套作为主导航模块，一套作为冗余导航模块，两套状态估计器运行不同的传感器融合算法，从而保持各自的独立性；并设计了故障诊断单元来实时对两套状态估计器的状态进行诊断，当主状态估计器发生故障时候，冗余状态估计器如果正常运行则立即接替主状态估计器作为输出来源，保证保证自动驾驶车辆***运行的连续性。

基于因子图的冗余导航模块，包含了多个历史状态的位姿信息进行联合优化，并且为滑动窗口设计(维持固定长度的历史位姿)，在精度方面优于传统的滤波算法，并且具备运算实时性；另外因子图中扩展其它传感器观测量非常方便，方便***的传感器方案的更新；本发明中因子图框架中融合了激光雷达里程计，在卫星信号受影响时，缺少全局位姿信息约束情况下，可以极大减小车辆位置累计误差发散的速度，为***应急反应预留出极大的时间窗口；同时，激光雷达实时点云与离线点云地图的匹配，为因子图融合提供除了卫星信号外的第二个全局位姿信息，提高了多种环境下全局位姿的鲁棒性。

通过设置卫星完好性监测模块，可以实时根据卫星报文中多种指标的数据，计算出当前卫星定位结果的置信度系数，为后续功能模块提供更加丰富的定位信息。

以上所述，仅为本申请的具体实施方式，但本申请的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本申请揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本申请的保护范围之内。因此，本申请的保护范围应以权利要求的保护范围为准。

Claims (7)

1.一种传感器融合定位***，其特征在于，包括传感器、导航结果模块和至少两个导航模块，所述导航模块用于接收所述传感器信号，并输出车辆状态信息给所述导航结果模块；

所述导航模块的输入端与所述传感器的输出端连接，所述导航模块的输出端通过故障诊断模块、输出选择模块与所述导航结果模块的输入端连接；

且其中一个所述导航模块与其他所述导航模块运行不同的传感器融合算法，与其他所述导航模块运行不同的传感器融合算法的导航模块设为主导航模块，其他所述导航模块设为冗余导航模块；

所述故障诊断模块的输入端与各个所述导航模块的输出端连接，所述故障诊断模块的输出端与所述输出选择模块的输入端连接；所述输出选择模块的输入端还与各个所述导航模块的输出端连接，所述输出选择模块的输出端与所述导航结果模块的输入端连接；

所述故障诊断模块用于监测所述导航模块是否出现故障，并输出指令到所述输出选择模块；故障诊断模块的具体判断方法包括依次进行一致性检查和平滑性检查；

其中一致性检查流程为：定义

为第k帧数据的一致性系数，表示n个历史主导航输出值与对应冗余导航输出值之差的累加二范数；其中p ^M 和p ^A 分别为主导航模块和冗余导航模块的速度、位置、姿态的车辆导航状态；如果满足位置一致性系数大于阈值DP，或速度一致性系数大于阈值DV，或姿态一致性系数大于阈值DA，则定义为一致性低，判断为导航***故障；反之，一致性系数均小于预设阈值，则定义为一致性高，判断为导航***非故障；其中位置、速度、姿态均为三维向量，阈值DP、DV、DA均为经验值；

平滑性检查流程为：定义

为第k帧的平滑性系数，其中p为包括速度、位置、姿态的车辆导航状态；如果满足位置平滑性系数大于SP，或速度平滑性系数大于阈值SV，或姿态平滑性系数大于阈值SA，则判断为导航***平滑性低；反之，平滑性系数均小于等于预定阈值，则判断为导航***平滑性高，阈值SP、SV、SA均为经验值；

所述输出选择模块根据所述故障诊断模块的指令选择输出所述主导航模块或冗余导航模块的输出值。

2.根据权利要求1所述的传感器融合定位***，其特征在于，所述***还包括卫星完好性监测模块，用于监控卫星状态是否良好；所述卫星完好性监测模块的输入端与GNSS连接，所述卫星完好性监测模块的输出端分别与所述冗余导航模块的输入端和所述输出选择模块的输入端连接。

3.根据权利要求2所述的传感器融合定位***，其特征在于，所述卫星完好性监测模块的输入端接收所述GNSS的卫星报文，所述卫星报文包括可视卫星数、高度角、信噪比、精度因子、测量值中误差、解状态和ratio值中的一种或几种。

4.根据权利要求1所述的传感器融合定位***，其特征在于，所述主导航模块的输入端连接的所述传感器包括IMU和GNSS，所述冗余导航模块的输入端连接的所述传感器包括IMU和激光雷达。

5.根据权利要求1所述的传感器融合定位***，其特征在于，所述传感器融合算法包括标准卡尔曼滤波、因子图、扩展卡尔曼滤波、无迹卡尔曼滤波、迭代卡尔曼滤波和粒子滤波中的一种或几种。

6.根据权利要求5所述的传感器融合定位***，其特征在于，所述主导航模块运行的传感器融合算法为标准卡尔曼滤波，所述冗余导航模块运行的传感器融合算法为因子图。

7.一种传感器融合定位方法，该方法基于权利要求1-6任意一项所述的传感器融合定位***，其特征在于，所述方法包括：

主导航模块和/或冗余导航模块接收传感器数据；

所述主导航模块和冗余导航模块利用互为不同的传感器融合算法进行计算，得到包括车辆位置、速度、姿态的状态信息，

故障诊断模块根据主导航模块、冗余导航模块输出的包括位置、速度、姿态的状态信息依次进行一致性检查和平滑性检查，输出选择模块根据所述故障诊断模块的判断结果选择输出所述主导航模块或冗余导航模块的输出值，传递至导航结果模块，并由导航结果模块进行输出；

所述主导航模块和冗余导航模块中的一个出现故障时，另一个进行接替，并进行所述车辆状态的输出。

Images