基于鲁棒马氏距离统计量的多源融合抗差估计方法.pdf

第 45 卷 第 2 期2024 年 2 月仪器仪表学报Chinese Journal of Scientific InstrumentVol.45 No.2Feb.2024DOI:10.19650/ki.cjsi.J2312152收稿日期:2023-11-13 Received Date:2023-11-13基金项目:国家自然科学基金(62203111)、国家重点研发计划(2021YFB3900804)、江苏省自然基金(BK20231434)项目资助基于鲁棒马氏距离统计量的多源融合抗差估计方法姜颖颖1,2,潘树国1,2,孟 骞1,2,高 旺1,2(1.东南大学仪器科学与工程学院 南京 210096;2.东南大学微惯性仪表与先进导航技术教育部重点实验室 南京 210096)摘 要:为了有效抵御复杂多变城市环境下的全球卫星导航系统(GNSS)信号干扰、增强多源融合定位可靠性,提出一种基于鲁棒马氏距离的多源融合抗差估计方法。该方法在分析观测值故障传播特点以及典型方差膨胀抗差估计模型基础上,基于相邻新息序列构造鲁棒马氏距离检验统计量。历史新息的引入能够提高系统观测冗余,同时不同观测量间的新息交互增强了异常检验统计量的鲁棒性。根据鲁棒马氏距离的统计特性,给出抗差关键门限取值规则并分别结合两种典型加权策略自适应调节观测值噪声矩阵。利用典型城市峡谷环境下惯性导航系统(INS)/GNSS/激光雷达(LiDAR)/VINS 多源融合车载数据进行相关实验,与现有方法相较,所提方法能够将三维均方根定位误差最低限制在 3.37 m。通过对比不同组显著性水平下的定位结果,进一步说明所提方法在城市峡谷环境下定位的优越性。关键词:多源融合;城市环境;马氏距离;自适应权因子;可靠性中图分类号:TH762 TN967.2 文献标识码:A 国家标准学科分类代码:420.10Robust Mahalanobis distance statistic-based multi-sensor integration robust estimation methodJiang Yingying1,2,Pan Shuguo1,2,Meng Qian1,2,Gao Wang1,2(1.School of Instrument Science and Engineering,Southeast University,Nanjing 210096,China;2.Key Laboratory of Micro-Inertial Instruments and Advanced Navigation Technology,Ministry of Education,Southeast University,Nanjing 210096,China)Abstract:To effectively overcome the interferences of GNSS signals and enhance the reliability of multi-sensor integration positioning in complex urban environments,a robust Mahalanobis distance statistic-based multi-sensor integration robust estimation method is proposed.With the basis of faulty measurements evaluation and typical model of variance inflation robust estimation,the robust Mahalanobis distance statistic is constructed based on the adjacent innovation sequences.The introduction of past innovation contributes to the observation redundancy.Meanwhile,the robustness of anomaly detection statistics can be improved by interacting between innovations from different measurements.According to the statistical property of this robust distance,the critical thresholds are ensured and then the measurement noise covariance can be adjusted adaptively with two traditional weighted strategies.Some experiments have been implemented on the INS/GNSS/LiDAR/VINS vehicle positioning system in an urban canyon environment.It shows that compared with existing methods,the 3D positioning error root-mean-square of proposed method is limited within 3.37 m.The superiority of our method is further validated by analyzing the positioning results with different significances.Keywords:multi-sensor integration;urban environment;Mahalanobis distance;adaptive weight factor;reliability0 引 言全 球 卫 星 导 航 系 统(global navigation satellite systems,GNSS)作为主要的定位信息源,在开阔环境下能够提供厘米级高精度定位服务。然而,在城市环境中,可见星数明显减少,同时 GNSS 信号经常受到多路径和非视距(non-line-of-sight,NLOS)等干扰的严重影响1-2,导致定位结果精度下降、可靠性降低。多传感器辅助GNSS 进行定位已经成为当前解决上述困境的重要手段。第 2 期姜颖颖 等:基于鲁棒马氏距离统计量的多源融合抗差估计方法253 常用的辅助导航传感器包括惯性导航系统(inertial navigation system,INS),视 觉 相 机,激 光 雷 达(light imaging detection and ranging,LiDAR),里程计等。其中,INS 具有短时高精度的显著优势但解算误差会随时间累积;视觉、LiDAR 等传感器观测信息丰富但质量受环境、天气等因素影响严重。多源融合导航在量测机理、场景适应性等方面具有显著的互补性,传感器间相互融合有利于提高定位性能、弥补单一定位手段的不足。为了削弱 GNSS 干扰信号对定位结果的不良影响,大量的研究工作都集中在对观测源的质量控制上,其核心思想主要是通过对发生故障或偏差的异常观测值进行抑制以达到对其质量进行把控的目的。其中最具代表性的方法之一是基于一致性检验的故障检测和排除(fault detection and exclusion,FDE)。该方法主要基于假设检验理论构造检验统计量并与给定门限比较以定位故障源。但基于 FDE 思想的质量控制方法将超出关键门限的观测量“硬性”规定为故障,处于门限下的故障仍可能对系统的定位结果可靠性产生一定威胁3。而抗差估计方法本质上是一种调节思想4。抗差估计质量控制方法以充足可靠的观测冗余作为关键前提条件,其抗差效率一方面取决于等价权函数的选择,另一方面也受抗差系数等关键参数取值的影响5。通过构造观测异常检验量对异常值赋以合理的权重,抗差估计能够同时应对多个异常观测。凭借这一鲜明优势,抗差估计被广泛应用到诸多克服 GNSS 信号干扰场景中6。文献7提出一种基于新息卡方检测8的卫星/惯性松组合抗差卡尔曼滤波(robust Kalman filtering,RKF)方法,提升了系统定位性能;文献9利用 RKF 增强 GNSS/INS 组合导航系统对缓变故障的检测灵敏性;文献10设计基于抗差估计的故障检测算法,提高了组合导航系统对突变、缓变及多故障的容错性能;文献11基于平滑有界层和抗差估计思想克服突变、缓变 GNSS 信号干扰对组合导航系统的定位影响。然而,上述研究都具有限定传感器组合类型、限定故障发生类型等特点,难以在异质异构传感器间直接拓展。实际上,抗差估计中观测异常检验量的构造是系统定位故障源的主要手段12。正态分布统计量和学生化分布统计量是目前两种常用的异常检验量13-14。前者结构简单、使用普遍,但易受状态估计值影响;后者顾及到多余观测信息,但观测冗余受限时,多故障出现很可能使其性能下降。城市环境场景频繁切换往往导致多故障并发的可能性增加且异常观测特性无法提前得知。尽管辅助传感器输出的冗余观测量能够帮助系统对异常 GNSS观测值进行控制,但上述统计量的抗差效率将受到一定程度限制。如何借鉴成熟的抗差估计思想在复杂多变的城市环境中构建合理观测异常检验量、自适应分配各传感器观测量权重、执行有效的多源融合抗差滤波流程有待进一步研究。为了提高多源融合系统定位性能,结合多传感器融合架构模型特点以及城市应用场景特殊性,提出一种基于鲁 棒 马 氏 距 离 统 计 量(robust Mahalanobis distance statistic,RMDS)的多源融合抗差估计方法。该方法在充分分析观测值故障传播特点以及典型方差膨胀抗差估计模型基础上,基于相邻新息序列构造鲁棒马氏距离检验统计量。历史新息的引入提高了系统观测冗余,同时不同观测量间的新息交互增强了异常检验统计量的鲁棒性。根据鲁棒马氏距离的统计特性,给出抗差关键门限取值规则并分别结合两种典型加权策略自适应调节观测值噪声矩阵,从而削弱城市环境下的多路径、NLOS 等干扰对多源融合定位精度和可靠性的不良影响。最后,利用典型城市峡谷环境下 INS/GNSS/LiDAR/VINS(visual inertial navigation system)多源融合车载数据进行相关实验,实验结果证明了该方法的有效性和优越性。1 多源融合定位模型及卡尔曼滤波 首先介绍对多传感器系统的松组合模型的建立过程,然后阐述经典状态估计方法 卡尔曼滤波的详细步骤。1.1 多源融合定位模型建立 松组合方式下各传感器的融合保持在位置域,即用各传感器的绝对/相对位置输出量作为滤波器的输入以估计 INS 的解算误差。由于这种融合方式并不涉及传感器的原始观测值,因此其定位模型结构简单、易于实现。在多源融合导航系统中,松组合的融合方式最为广泛。在地心地固坐标系下对 INS/GNSS/LiDAR/VINS 多源融合系统进行松组合。系统在历元 k 的状态向量为:xk=vrbabgT(1)式中:、v、r 分别表示姿态误差、速度误差和位置误差向量;ba、bg是加速度计和陀螺偏差向量。每个误差向量又分别包括 x、y、z 三个坐标分量。多源融合导航系统的离散化状态空间模型如下:xk=k/k-1xk-1+wkzk=Hkxk+vk(2)式中:xk是15维状态向量;k/k-1表示从历元(k-1)到k 的 15 15 状态转移矩阵,具体形式可以参考文献13;zk为 9 1 维观测向量;Hk表示 9 15 维观测矩阵;wk是 15 1 过程噪声向量;vk为 9 1 维观测噪声向量;噪声向量 wk和 vk相互独立,且均为高斯白噪声,即满足式(3)。E(wk)=0,E(vk)=0E(wTkwk)=Qk,E(vTkvk)=RkE(wTkwj)=E(vTkvj)=0,k j(3)254 仪 器 仪 表 学 报第 4 5 卷式中:E()表示数学期望。多传感器松组合模型的观测矩阵为:Hk=036-I3036036-I3036036-I3036(4)观测传感器 GNSS/LiDAR/VINS 组成的观测向量如下:zk=rGNSSkrLiDARkrVINSkT(5)式中:rGNSSk和 rLiDARk分别表示历元 k 对应的 GNSS 和LiDAR 输出位置与 INS 估计位置的差值;rVINSk是 VINS输出相对位置与 INS 估计相对位置的差值。1.2 卡尔曼滤波 标准卡尔曼滤波包括预测和更新两个核心步骤。第1 步利用前一历元后验状态估计对当前历元的状态进行预测;第 2 步通过结合输入观测对预测的状态进行校正。1)利用历元(k-1)的后验状态估计和误差协方差计算预测状态及其误差协方差:x-k=k/k-1xk-1P-k=k/k-1Pk-1Tk/k-1+Qk-1(6)式中:xk-1是历元(k-1)的后验状态估计,Pk-1=E(xk-1-xk-1)(xk-1-xk-1)T 为相应的误差协方差矩阵;x-k是历元 k 的预测状态,其误差协方差矩阵为 P-k=E(xk-x-k)(xk-x-k)T。2)对预测状态及其误差协方差进行量测更新,得历元 k 的后验状态估计及其误差协方差:Kk=P-kHTkS-1kk=zk-Hkx-kxk=x-k+KkkPk=(I15-KkHk)P-k(7)式中:Kk是卡尔曼增益;Sk是新息向量 k的协方差矩阵,Sk=E(Tkk)=HkP-kHTk+Rk。在标准卡尔曼滤波中,正常观测值对应的新息向量应服从多维零均值高斯分布,即 k N N(0,Sk)。因此,新息是组合导航系统进行故障检测以及抗差滤波等关键容错算法设计的重要元素。2 观测值故障传播过程及方差膨胀抗差滤波方法 在上述滤波模型框架下,首先分析观测值故障在状态估计中的传播过程;接着基于极大似然估计理论推导方差膨胀抗差滤波算法。2.1 观测值故障模型及其传播过程 假设历元 k 的故障向量为 fk,则异常情形下的观测向量为:z(fk)k=zk+fk(8)其中,故障向量的维数与观测向量相等,其元素由有限个零和非零元素组成,其中非零表示对应观测量发生故障。对于城市环境而言,多故障并发的概率极大提高。由于闭环反馈的存在,式(8)又可以表示为:(fk)k=k+fk(9)式中:fkk为历元 k 受故障 fk影响的新息向量。由式(9)可知,该历元下的新息能够如实跟踪故障幅值。根据卡尔曼滤波更新过程,此时故障影响下的后验状态估计x(fk)k为:x(fk)k=x-k+Kk(fk)k=xk+Kkfk(10)由于卡尔曼滤波是一种递推型滤波器,受故障向量fk影响下历元(k+1)的预测状态、新息向量和后验状态如下。1)预测状态:x-(fk)k+1=k+1/kx(fk)k=x-k+1+k+1/kKkfk(11)2)新息向量:(fk)k+1=zk+1-Hk+1x-(fk)k+1(12)(fk)k+1=(zk+1-Hk+1x-k+1)-Hk+1k+1/kKkfk(13)由式(12)、(13)可推导出受故障影响的新息与无故障情况下新息向量间的关系为:(fk)k+1=k+1-Hk+1k+1/kKkfk(14)3)后验状态:x(fk)k+1=x-(fk)k+1+Kk+1(fk)k+1=xk+1+(I15-Kk+1Hk+1)k+1/kKkfk(15)式中:x(fk)k+1表示历元(k+1)受故障fk影响下的后验状态估计。结合式(9)、(10)、(14)、(15)可知,随着载体运行时间的增加,对于前一历元发生的故障,若未能及时检测,则会传递到下一个历元,且下一个历元的新息无法如实跟踪到故障幅值。同时,如果无法有效消除观测值故障,卡尔曼滤波的递推机制则会进一步影响后续历元的状态估计,严重时可能导致滤波发散。2.2 方差膨胀抗差滤波 当多源定位中的传感器观测值受到污染时,基于传统卡尔曼滤波的定位结果会受到异常值的严重影响,导致滤波器失真甚至不收敛。抗差估计的引入可以在一定程度上抑制异常值的不利影响。根据极大似然估计理论,抗差估计的目标函数可以表示为15:xk=argmin(xk-x-k2(P-k)-1+zk-Hkxk2(Rk)-1)(16)第 2 期姜颖颖 等:基于鲁棒马氏距离统计量的多源融合抗差估计方法255 式(16)可以等价转换为:xk=argmin15i=1(ex,k,i)+9j=1(ez,k,j)()(17)其中,()是代价函数,其输入为标准化残差,分别为:ex,k=(P-k)-12(xk-x-k)ez,k=(Rk)-12(zk-Hkxk)(18)由式(19)可知,其属于最值问题。基于微积分最值理论,上述问题解为:(P-k)-T2x,kex,k-HTk(Rk)-T2z,kez,k=0(19)式中:矩阵 为权重对角阵。x,k=diag(ex,k,i)z,k=diag(ez,k,j)(20)式中:()是等价权函数,代表性的函数包括两段式Huber 函数以及三段式 IGG 函数等16。当仅考虑系统存在观测值故障时,预测状态权重矩阵 x,k=I15。将式(18)代入式(19)并重新整理,此时可得:(P-k+Hk(Rk)-1HTk)(xk-x-k)=HTk(Rk)-1(zk-Hkx-k)(21)式中:Rk是观测噪声膨胀矩阵。方差膨胀抗差滤波进一步表示为:Kk=P-kHTk(HkP-kHTk+Rk)-1Rk=RT2k-1z,kR12k(22)式中:Kk为方差膨胀卡尔曼增益矩阵;z,k为膨胀矩阵。基于 Huber 和 IGG 两种加权策略,膨胀矩阵对角元素计算表达式分别如下:H(ez)=1,ez k0k0ez,ez k0(23)G(ez)=1,ez k0k0ez,k0 k1(24)式中:k0和 k1为抗差系数,分别用于界定观测值是否可信或者是否淘汰的门限。在实际应用中,k0和 k1的取值往往依赖经验。由上述推导过程可知,如何设计合理的权因子自适应跟踪到观测值状态变化成为整个抗差滤波流程的关键所在。基于标准化残差的方差膨胀法受状态估计值的影响较大。在多源融合背景下,尽管辅助传感器提供的冗余观测量在一定程度能够帮助系统增强定位估计鲁棒性。但随着城市环境下多故障并发干扰出现可能性的急剧增加,上述传统方法的抗差性能在多源融合模型上受限。为此,本文在结合城市复杂性以及模型特点基础上,提出一种基于鲁棒马氏距离统计量的多源融合抗差估计方法。3 基于鲁棒马氏距离统计量的多源融合抗差估计方法 在卡尔曼滤波模型基础上,首先分析相邻新息序列的统计规律,在此基础上提出基于鲁棒马氏距离统计量的多源融合抗差估计方法。3.1 相邻新息序列的统计特性 由式(9)可知,新息向量能够反映出观测异常的幅值。但由于多传感器间异构异质,在定位精度上存在一定差异。因此首先通过标准化处理对其进行消除。在卡尔曼滤波中,相邻历元 k-1k 的标准新息序列 k可以表示为:k=k-1 k(25)式中:k是历元k下相邻历元标准新息构成的9 2维序列矩阵;表示标准新息,可以通过新息向量 及其协方差 S 计算获得。标准新息向量中各分量计算表达式如下:i=iSii(26)式中:i表示新息向量的第 i 个元素;Sii则为其对应的方差(i=1,2,9)。在卡尔曼滤波中,正常观测值对应的标准化新息服从标准正态分布。即正常情况下,标准新息向量 k-1和k满足:k-1 N N(0,I9)k N N(0,I9)(27)在上述统计规则基础上,利用各序列分量构造检验样本 si为:si=k-1 ki(28)由于不同历元间的新息统计独立,因此各检验样本服从相同的二维标准高斯分布,其概率密度分布函数表达式为:f(x1,x2)=12e-x21+x222(29)式中:x1和 x2表示函数自变量。基于此统计规则,本文提出在各检验样本间构造鲁棒马氏距离统计量并通过等价权函数自适应各观测量权重,从而对各观测值进行合理赋权。3.2 基于鲁棒马氏距离统计量的多源融合抗差估计方法 鲁棒马氏距离统计量主要用于衡量每个数据点与大256 仪 器 仪 表 学 报第 4 5 卷部分数据样本中心的距离。针对样本容量为 p 的样本序列 =s1,s2,sp,其中每个样本点的维数为 q且均来自同一多元高斯分布主体,坐标中位数为:M=medi(si1),medi(si2),medi(siq)(30)式中:medi(sij)表示所有样本点第 j 维的中位数,i=1,2,p,j=1,2,q。因此,M 的维度与各样本维数相同。一旦样本序列给定,坐标中位数 M 也就随之确定。样本点 si的 RMDSi为17:RMDSi=maxu=1sTiu-medj(sTju)1.482 6medksTku-medj(sTju)(31)式中:u 表示与以坐标中位数 M 为起点并经过每个样本点的同方向单位向量,ui=(si-M)/si-M,因此单位向量 u 的个数与样本容量相等。由表达式可以看出,统计量的计算依赖于样本中位数以及样本绝对偏差。由上述过程可知,样本点 si对应的统计量 RMDSi是样本点在每个单位向量 u 上进行标准投影的最大值,即它是样本点到样本中心最坏的一维投影值。由于各样本点均来自高维高斯分布主体,因此 RMDSi服从一定自由度的卡方分布,其中自由度等于每个样本中非零元素个数之和。在此统计前提下,通过将计算得到的统计值与给定显著性水平的门限作比较,即可判定样本点的质量,进一步采取相应的容错措施。基于上述统计背景,考虑在新息序列样本上进行相应过程。历史新息的引入能够提高系统观测冗余,同时不同观测量间的新息交互增强权因子的鲁棒性。结合 RMDSi的统计分布,在给定显著性水平下,关键门限可定义为:Th(1-)2=F-1(1-)2=x:F(x 2)=1-(32)式中:(1-)是显著性水平;Th为门限值;F(x 2)表示自由度为 2 的卡方累积分布函数。F(x 2)=x-12(1)e-y2dy(33)式中:(1)为伽马函数在自变量为 1 处的取值。由关键门限计算表达式可知,在给定自由度下,显著性水平越小,关键门限越大,所提算法的容忍度越高;相反地,一旦自由度确定,本文方法将随着显著性水平的增大而变得越严格。根据上述分析过程,基于鲁棒马氏距离统计量的膨胀矩阵 z,k为:z,k=diag(RMDSz,k,i)(34)等价权函数中相应抗差系数分别为:k0=Th(1-0)2k1=Th(1-1)2(35)式中:(1-0)和(1-1)分别为界定可疑区段和有害区段的显著性水平。基于鲁棒马氏距离统计量的多源融合抗差估计方法原理图如图 1 所示。1)利用 IMU 的比力及角速度观测值,基于捷联算法构建状态方程并根据 INS 推算位置以及 GNSS、LiDAR 和 VINS 3 种观测传感器输出的位置量测构建观测方程,如式(1)(5)。2)执行卡尔曼滤波状态预测过程,得预测状态及其误差协方差矩阵。计算新息及其协方差矩阵并对相邻新息序列进行标准化。3)基于式(30)(31)计算各观测值对应鲁棒马氏距离统计值。在给定显著性水平下,利用式(32)、(33)计算各统计值对应关键门限(抗差系数)。4)将统计值逐个与对应关键门限作比较,基于等价权函数计算对应观测值的权重、构建方差膨胀矩阵式(34)。5)结合状态预测,基于式(22)进行抗差状态更新。输出可靠的多源融合定位解并将所得状态作为下一历元的输入。图 1 所提方法原理示意图Fig.1 The illustration of the proposed method4 实验验证 为了验证所提方法的有效性和优越性,利用城市峡谷环境下的多源车载数据进行实验。首先介绍测试数据来源以及算法中涉及的参数取值;然后分析比较两种等价权函数(Huber 和 IGG)下本文所提方法与标准的方差膨胀抗差滤波以及文献14改进抗差滤波定位性能;最后,顾及显著性水平取值差异对抗差性能的影响,设置不同组显著性水平进行对比实验,通过比较定位误差统计值进一步证明本文方法的优越性。4.1 实验数据与参数设置 1)实验数据实验采用的数据来自文献18。测试数据是在由GNSS、IMU、相机和激光雷达 4 种导航传感器搭载的车载 第 2 期姜颖颖 等:基于鲁棒马氏距离统计量的多源融合抗差估计方法257 平台上进行采集,整个过程持续时间为 456 s。实验测试的参考轨迹由高精度 Novatel SPAN-CPT 导航系统提供,该组合导航设备采用高精度惯性器件以及先进的滤波技术,能够实现厘米级定位精度。表 1 为各传感器的定位信息。在城市峡谷环境下,实时动态 RTK 中的模糊度很难固定,因此实验中 GNSS 的定位模式采用伪距单点定位(single point positioning,SPP)。测试数据的单一传感器水平定位结果如图 2 所示,其中,VINS 位置解算主要基于开源算法 VINS-MONO 处理相机和 INS 数据提供,而激光 雷 达 则 使 用 正 态 分 布 变 换(normal distributions transform,NDT)进行解算。由图 2 可知,GNSS 的定位误差受周围环境影响的程度最大。当卫星信号质量较差时,可能达到数十米的误差。激光雷达的定位精度则最高。表 1 各传感器数据采集及定位信息Table 1 Positioning details of the sensor description名称采样频率/Hz定位方式GNSS1SPP(BDS+GPS)IMU100捷联算法相机10VINS-MONO激光雷达10NDT 2)参数设置系统噪声协方差中陀螺噪声和陀螺随机游走方差分别取值为 2.510-5和 4.010-11 rad2/s3;加计相应参数 图 2 单一传感器水平定位和动态参考轨迹Fig.2 Single sensor positioning trajectory and reference trajectory取值分别为 6.410-5和 1.010-5 m2/s5。另外,在城市峡谷环境下,各传感器观测值噪声 vk的标准差分别为10、0.3 和 0.5 m。现有抗差系数的选择往往依赖经验。为了比较不同抗差滤波算法性能,在实验中,各系数的选择均由等价权因子统计特性以及显著性水平确定。如表 2 所示,分别给出不同显著性水平下 3 种方法的抗差系数取值。相应的抗差系数可以根据标准正态分布、t 分布以及卡方分布的逆累积分布函数在给定显 著 性 水 平 下 计 算。在MATLAB 中可以分别利用 norminv、tinv 以及 chi2inv 函数实现。将标准的方差膨胀抗差滤波和文献14改进抗差滤波记分别为方法 1 和方法 2。表 2 不同显著性水平下抗差系数取值Table 2 Robust coefficients with different significances0显著性水平k0方法 1方法 2所提方法1显著性水平k1方法 1方法 2所提方法0.0110.011.6451.8952.14610-7110-75.32722.0405.6780.0510.051.9602.3642.448510-71510-75.02617.4475.3870.110.12.5763.5003.03510-6110-64.89215.7705.2574.2 实验结果对比分析 按照抗差滤波流程,在 Huber 和 IGG 两种等价权函数下,分别对传感器观测值在 3 种方法下的权重进行对比;而后在此基础上给出包括基于传统 EKF 定位解算结果在内的 4 种方案定位误差以证明所提算法的先进性。其中,0和 1分别取值为 0.05、10-6。1)权重比较Huber 等价权函数下 GNSS 在各坐标轴上观测值对应的加权情况如图 3 所示。由于 LiDAR 和 VINS 输出的观测值权重均为 1,因此仅研究 GNSS 各坐标轴观测值权重变化。根据 Huber 函数的加权规则,权重越小说明对应观测量检验值越大,观测值越不可靠。由图 3 可知,方法 1 及所提算法的权重均在 0 和 1 之间剧烈波动,而方法 2 的权重则相对平稳,特别是 z 轴(图 3(c)。同时,方法 1 与所提方法的权重变化在大部分历元基本保持相同的趋势,不同的是对于同一故障观测值,所提算法对其进行降权的幅度较大,这说明本文方法对污染观测的敏感程度更高。相反地,方法 2 虽然对异常观测值有所降权,但始终处于间歇加权状态。这主要是因为随着幅值较大的多故障观测量频繁出现,其相应权因子鲁棒性降258 仪 器 仪 表 学 报第 4 5 卷低。经统计,超出 1/2 的运行时间 GNSS 3 个坐标轴上的数据均受到城市环境下多路径等干扰影响而处于异常状态。Huber 权函数下的权重结果说明相较于其他两种方法,本文所提算法对故障观测值更为敏感,更适合用于城市环境下多故障频发定位场景。图 3 Huber 权函数下 3 种方法 GNSS 观测值权重对比Fig.3 Three kinds of weights comparison of GNSS measurements with Huber function为验证本文方法的优越性,进一步给出 IGG 权函数下 GNSS 三轴观测值的不同加权情况,如图 4 所示。与Huber 函数不同,IGG 函数不仅包括等权段、降权段,还包括零权段,即对严重污染的观测值直接给予零权处理。与图 3 对比可知,IGG 权函数下 3 种权重的变化规律与Huber 下的结果相近。可以看出,图 3 中权重较小的历元,在 IGG 加权策略下均直接零权。这说明相较于 IGG函数,尽管 Huber 函数能够对严重污染的异常观测给予相当大程度的降权,但仍不能完全削弱其有害影响。零权处理对于控制较大幅值观测故障对状态估计的影响具有重要意义,特别是在城市环境中。2)定位误差比较两类加权规则下 4 种方法在 ENU 坐标系下的定位误差如图 5、6 所示。由于传统卡尔曼滤波方法不具备鲁棒性,故障观测值的出现导致相应的定位结果产生较大偏差,部分历元甚至达到十几米。方法 1 和本文算法均可以显著提高定位结果的精度。而方法 2 精度提高范围有限。在这 3 种抗差滤波定位算法中,所提方法的定位 图 4 IGG 权函数下 3 种方法 GNSS 观测值权重对比Fig.4 Three kinds of weights comparison of GNSS measurements with IGG function性能提升效果最好。4 种方案的三维(3D)定位性能的均方根误差(root mean square,RMS)、平均值以及累积分布函数(cumulative distribution function,CDF)如表3、4 所示,本文方法的抗差性能最好,在两种加权规则下可以将 3D定位误差 RMS 分别限制在 3.59 和 3.37 m。相较于 EKF,定位精度分别提高:59.59%、62.35%。同时,均值、不同CDF 百分比统计数字也证明了所提方法相较于其他两种抗差滤波的优越性。另外,IGG 加权策略下的定位性能优于 Huber 方法,这主要因为城市峡谷环境下 GNSS 观测值频繁受到周围环境的严重干扰而出现幅值较大的故障。第 2 期姜颖颖 等:基于鲁棒马氏距离统计量的多源融合抗差估计方法259 图 5 Huber 权函数下 4 种方法定位误差对比Fig.5 Four kinds of positioning errors comparison with Huber function图 6 IGG 权函数下 4 种方法定位误差对比Fig.6 Four kinds of positioning errors comparison with IGG function表 3 三维定位误差统计值对比(Huber)Table 3 Comparison of 3D positioning error statistics(Huber)m3D 误差EKF方法 1方法 2所提方法RMS8.953.997.763.59平均值7.203.786.293.4350%CDF5.483.904.853.2570%CDF8.574.796.843.8490%CDF13.575.3311.855.06表 4 三维定位误差统计值对比(IGG)Table 4 Comparison of 3D positioning error statistics(IGG)m3D 误差EKF方法 1方法 2所提方法RMS8.953.477.763.37平均值7.203.286.293.1650%CDF5.483.254.852.9570%CDF8.573.876.843.5190%CDF13.574.9011.854.864.3 不同显著性水平下定位抗差性能比较 抗差估计效率一方面取决于等价权函数的选择,另一方面也受抗差系数等关键参数取值的影响。为了充分说明所提方法的优越性,分别统计了表 2 中各组显著性水平下 3 种抗差方法的 3D 定位误差 RMS,各算法对比实验结果如图 7 所示。其中图 7(a)为 Huber 权函数下统计结果,图 7(b)(d)的均方根误差均来自 IGG 权函数。可以明显看出,无论是哪种加权策略,何种显著性水平组合,均处于坐标区域最底端,这说明相同显著性水平情况下,本文算法的抗差性能最优。由图 7(a)可知,Huber 等价权函数下,0越大,抗差滤波的效果越显著。当 0=0.1 时,3 种方法的均方根误差分别为 3.8、7.14、3.53 m。相较于前两种方法,本文算法分别降低 0.27 和 3.61 m。特别地,3 种抗差定位误差RMS 随显著性水平减小而下降的幅度分别为 0.59、1.46、0.46 m。而对于 IGG 加权策略,由图 7(b)(d)可知,尽管 1取值差异带来的定位性能变化相较来说并不显著,但方法 1 和所提方法的定位性能都有所改善。为了便于比较这两种方法在 IGG 加权策略下随显著性水平取值不同而导致的差异,9 组 3D 定位误差 RMS 统计结果如图 8 所示。其中,和分别表示 IGG 等价权函数下方法 1 和本文所提方法的 3D 定位误差 RMS 统计数字。可以明显看出,小于四边形面积。受显著性水平取值变化影响,方法1 的定位误差 RMS 波动范围是 260 仪 器 仪 表 学 报第 4 5 卷图 7 不同显著性水平下各方法 3D 定位误差 RMS 对比Fig.7 3D RMS positioning errors comparison with different significances3.41 m,3.68 m,所提方法为 3.37 m,3.41 m。对比结果说明相较于方法 1,本文方法不仅能够表现出最优的定位性能同时对抗差系数取值变化导致抗差效果差异波动较小,包容性较强。这种特点对于等价权函数中关键门限的确定具备一定的优势,特别是三段式加权函数。标准差统计值数据进一步证明该结论。图 8 3D 定位误差 RMS 统计数字对比图(方法 1 vs 所提方法,IGG 权函数)Fig.8 3D RMS positioning errors statistics comparison(method 1 vs proposed method,IGG function)5 结 论 城市环境下多路径、NLOS 等干扰对多源融合定位精度和可靠性造成不良影响。为此,本文提出了基于鲁棒马氏距离的多源融合抗差估计算法。检验统计量中历史新息的引入提高了系统观测冗余,且不同观测量间的新息交互增强了异常检验统计量的鲁棒性。另外,通过分析异常检验量的统计特性确定了关键门限取值规则。基于典型城市峡谷环境下 INS/GNSS/LiDAR/VINS 多源融合车载数据进行了相关实验,定位结果证明了该方法的有效性和优越性。相较于现有方法,本文方法对多故障更鲁棒,更适合用于城市环境中。未来