一种卫星/惯性/视觉组合导航系统完好性评估方法与流程

本发明公开了一种卫星/惯性/视觉组合导航系统完好性评估方法，涉及多传感器组合导航系统的完好性评估，属于计算、推算、计数的技术领域。

背景技术：

近年来，MEMS(Micro-Electro-Mechanical System，微机电系统)传感器、MSIS(Micro Solid-state Inertial Sensor，微小固态惯性传感器)、光纤陀螺和GNSS(Global Navigation Satellite System，全球导航卫星系统)接收机等新型低成本、小型、轻质量的导航传感器以及高速大容量的嵌入式微处理器和分布式模块化电子设备发展迅速，兴起了多传感器导航系统的研究浪潮。

为了提高多传感器导航系统的准确性和可靠性，国内外专家提出了各种软硬件冗余技术、完好性监测算法以及多传感器信息融合算法。大量研究发现，导航系统的故障容错能力主要由系统硬件冗余结构和完好性监测算法决定。目前，国内外对导航系统的完好性监测的研究方法主要分为快照法和连续法。

快照法利用单历元的测量信息来检测和隔离瞬时故障，通常用于变化较大的故障，典型的方法有最小二乘残差法、奇偶矢量法等，国内外广泛用于研究的关于GPS的RAIM算法属于快照法。杨永波等人提出全局最小二乘法，相较于惯用的最小二乘残差法，提高了RAIM算法的可靠性。针对最小二乘法在卫星故障检测和识别中存在的问题，王尔申等人将测量方程中协方差矩阵的对角线元素作为加权因子，进而得到基于加权最小二乘法的卫星故障检测算法。宋凯等人提出了基于Kalman滤波和奇偶矢量法的综合RAIM算法，与传统RAIM算法相比，提高了对微小伪距偏差的检测率,同时减少了对可见卫星数的要求。魏春玲等研究了基于神经网络的奇偶向量补偿法以解决斜置系统的故障诊断问题。然而，虽然快照法可以有效检测惯性传感器或GPS信号的阶跃故障，但很难检测惯性器件漂移等因素引起的软故障。

对于缓变误差的检测，通常使用SPRT法、基于动力学的模型法等基于历史累积信息的连续法。针对GPS接收机观测噪声分布的特点和基本粒子滤波存在的粒子退化与采样枯竭问题，王尔申等人给出了一种基于遗传算法进行再采样的粒子滤波与似然比方法有机结合的全球定位系统RAIM算法，该算法在非高斯测量噪声下成功地检测并隔离故障卫星,提高了故障检测的性能，但该算法比较耗时，还无法满足导航系统实时监测的要求。

尽管不少学者对多传感器导航系统做了大量的研究，但是大都采用滤波算法来进行完好性监测。目前，许多INS/GPS组合导航系统采用集中式滤波结构，并表现出较好的效果，但随着传感器数量的增多，计算量会激增，从而带来时间延时，不能满足用户的实时性需求，不利于故障的隔离。分布式滤波结构为多传感器导航系统提供了灵活可变的方案，但仍需解决各局部状态的动力学关系这一问题。

本申请旨在提出一种萤火虫算法和图优化相结合的方法，通过较少的计算量实现多传感器导航系统的全局优化。

技术实现要素：

本发明的发明目的是针对上述背景技术的不足，提供了一种卫星/惯性/视觉组合导航系统完好性评估方法，通过较少的计算量实现了多传感器组合导航系统的全局优化，解决了现有多传感器导航系统完好性评估方法不能满足实时性需求的技术问题。

本发明为实现上述发明目的采用如下技术方案：

一种卫星/惯性/视觉组合导航系统完好性评估方法，对当前时刻各传感器的测量值进行预处理得到当前时刻各传感器的观测向量，将当前时刻各传感器的观测向量表示为具有公共位姿节点的多条边，利用位姿节点和边之间的关系，图优化构建残差带权平方和函数，采用萤火虫算法优化位姿节点来求解残差带权平方和函数的最优解，对残差带权平方和函数的最优解标准化得到检验统计量并根据给定的误警率设置检测门限，在检验统计量超过检测门限时判定故障。

进一步的，一种卫星/惯性/视觉组合导航系统完好性评估方法中，当前时刻各传感器的观测向量包含各传感器采集的东北天坐标系下的位置信息及速度信息。

进一步的，一种卫星/惯性/视觉组合导航系统完好性评估方法中，通过位姿节点和位姿节点的位置对与位姿节点之间的边所代表的观测信息的影响构建残差带权平方和函数。

进一步的，一种卫星/惯性/视觉组合导航系统完好性评估方法中，采用萤火虫算法优化位姿节点来求解残差带权平方和函数的最优解的过程为：将图优化过程中的得到的一组位姿节点映射为萤火虫位置，以残差带权平方和函数取值最小为目标确定萤火虫群体的最大荧光亮度，根据萤火虫群体的最大荧光亮度决定萤火虫移动方向，再结合最佳位置萤火虫的最大吸引度随机移动最佳位置的萤火虫进而更新其空间位置，重新计算萤火虫群体的最大荧光亮度后进行下一次搜索直至满足搜索精度要求或达到最大搜索次数。

进一步的，一种卫星/惯性/视觉组合导航系统完好性评估方法中，对残差带权平方和函数的最优解标准化得到的检验统计量为：根据给定的误警率PFA设置检测门限T6的表达式为：其中，rk为k时刻的检验统计量，rk服从自由度为6的χ²分布，F(X)为残差带权平方和函数的最优解，为方差，为概率分布密度函数。

再进一步的，一种卫星/惯性/视觉组合导航系统完好性评估方法中，残差带权平方和函数为zij为位姿节点xi和位姿节点xj之间的边所表示的观测信息，e(xi,xj,zij)为表示xi和xj之间的关系与观测zij有多吻合的向量误差矩阵，fij为表示xi和xj之间的关系与观测zij有多吻合的向量误差函数，Ωij为e(xi,xj,zij)的权重，wij为fij的权重，X＝[x0,x1,x2,x3]，M＝{0,1,2,3}。

本发明采用上述技术方案，具有以下有益效果：本发明将萤火虫算法和图优化相结合，提出了一种多传感器组合导航系统的完好性评估方法，该方法首先将多传感器观测向量表示为具有公共位姿节点的多条边，利用位姿节点和边之间的关系，图优化构建残差带权平方和函数，再通过萤火虫算法优化位姿节点来进行全局优化，减少了系统计算量，避免内存消耗随时间推移急剧增加带来的时延问题，能够避免集中式结构组合导航系统不能满足实时性检测需求的缺陷，利用鲁棒性较强的萤火虫算法进行全局优化简单且易于实现，能够避免分布式滤波需要解决各局部状态动力学关系的问题。

附图说明

图1为本申请评估组合导航系统完好性的流程图。

图2为图优化的示意图。

具体实施方式

下面结合附图对发明的技术方案进行详细说明。

为满足不断提高的定位技术精度和可靠性需求，本发明公开了一种萤火虫算法和图优化相结合方法，用于评估卫星/惯性/视觉里程计组合导航系统的完好性(完好性包括两方面的含义：一是对于超过告警阀值的任何故障都要在给定的告警时间内发出警告；二是完好性风险概率必须在一定范围内,完好性风险概率是指导航位置信息超过了告警阀值这个事件被漏检的概率)。

视觉里程计主要依靠连接到移动物体上的视觉传感器，如单目相机，通过分析处理相关图像序列来确定机器人的运动，利用相邻图像间的相似性来估计物体的运动。将视觉里程计和传统的GPS/IMU导航系统相结合成为多传感器组合导航系统的一个发展趋势。

下面以包含GPS、IMU和视觉里程计多传感器组合导航系统为例阐述本发明的完好性评估方法，整个方法的流程如图1所示，具体包括如下6个步骤。

1)收集GPS、IMU和视觉里程计三者在k时刻的输出后对它们进行预处理，从而得到GPS、IMU和视觉里程计采集的东北天坐标系下的位置信息以及速度信息构建GPS、IMU和视觉里程计的观测量z10、z20、z30：

其中，为GPS在东向、北向、天向的位置信息，为GPS在东向、北向、天向的速度信息，为IMU在东向、北向、天向的位置信息，为IMU在东向、北向、天向的速度信息，为视觉里程计在东向、北向、天向的位置信息，为视觉里程计在东向、北向、天向的速度信息。

2)利用图优化展开分析，起点x0和顶点x1,x2,x3均为机器人位姿节点，对于安装在同一载体上的多传感器而言，起点x0为公共位姿节点，三组观测量信息分别对应于图优化中起点x0与三个顶点确定的三条边，如图2所示，由于它们是不同传感器观测的同一时刻同一状态的信息，理想情况下等同，顶点x1、x2、x3相互连接构成的边所代表的观测量为0，

于是，我们构建如下边的关系：

zi0＝xi-x0，i＝1,2,3，

xi＝xj，i,j取1,2,3且i≠j，

等价地，

fi0＝xi-x0-zi0＝0，i＝1,2,3，

fij＝xi-xj＝0，i,j取1,2,3且i≠j，

构建残差带权平方和函数F(X)来计算最优结果：

其中，zij为位姿节点xi和位姿节点xj之间的边所表示的观测信息，e(xi,xj,zij)为表示xi和xj之间的关系与观测zij有多吻合的向量误差矩阵，在本例中向量误差函数表示为fij，本式中，i、j可取值为0，故fij包括了fi0的情况，Ωij为e(xi,xj,zij)的权重，X＝[x0,x1,x2,x3]，M＝{0,1,2,3}，wij为误差fij对应的权重。

3)引入萤火虫算法求解图优化中的F(X)

a)初始化算法基本参数，随机生成n只萤火虫，设置最大吸引度β0，光强吸收系数γ，步长因子α，最大迭代次数MaxGeneration或搜索精度ε，以X(m)表示第m只萤火虫代表的位姿节点集合(m＝1,2,3,…,n)；

b)随机初始化萤火虫的位置，以F(X)为目标函数，计算目标函数的最小值进而确定萤火虫群体的最大荧光亮度I0：

其中，q>0且为常数；

c)计算群体中萤火虫的相对亮度I和吸引度β(r)，根据相对亮度决定萤火虫的移动方向：

其中，I0表示最亮萤火虫的亮度，即自身荧光亮度，与目标函数值相关，目标函数值越优，自身亮度越高；β0表示最大吸引度，即光源处的吸引度，γ表示光吸收系数，因为荧光会随着距离的增加和传播媒介的吸收逐渐减弱，所以设置光强吸收系数以体现此特性，可设置为常数；rab是萤火虫a与萤火虫b之间的距离,以欧几里得距离来表示；

d)更新萤火虫的空间位置，对处在最佳位置的萤火虫进行随机移动：

Xa(t+1)＝Xa(t)+β(Xb(t)-Xa(t))+α(rand-1/2)，

Xa(t)、Xa(t+1)为t时刻、t+1时刻萤火虫a的空间位置，Xb(t)为t时刻萤火虫b的空间位置，β(Xb(t)-Xa(t))表示t时刻萤火虫b对萤火虫a的吸引度，α为基于随机数(rand-1/2)的步长因子；

e)根据更新后萤火虫的位置，返回b)重新计算萤火虫的亮度；

f)当满足搜索精度或达到最大搜索次数时转下一步；否则，搜索次数增加1，转c)，进行下一次搜索；

g)输出全局极值点和最优个体值,对应上述图优化问题中的最优结果，即X如何取值时，F(X)取得最小值。

4)计算检验统计量rk和检测门限T6

将k时刻的F(X)标准化，可得检验统计量：

rk服从自由度为6的χ²分布，给定误警率PFA，根据概率分布密度函数可确定检测门限T6:

5)故障判定：当rk>T6时表示检测到故障，告警，需进行故障排除；当rk<T6时表示未检测到故障。

6)时间更新，返回第1步继续执行后面的步骤。