[发明专利]一种传感器融合定位系统及方法

权利要求书

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任意一项所述的传感器融合定位系统，其特征在于，所述方法包括：

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

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

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

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