7 t+ U# p, F+ y) \! Y U( L+ V
+ T8 G9 z7 ]: J0 S
# g/ x$ D5 M! o- J; |9 |- L 
( N9 s- _4 S" w0 k 本文内容来源于《测绘学报》2023年第8期（审图号GS京(2023)1524号） 基于抗差因子图的AUV多源信息融合定位方法黄紫如1,2 , 柴洪洲1 , 向民志1,3, 杜祯强1,3. _' M v" q- `+ k
1. 信息工程大学, 河南 郑州 450001;
6 k7 _9 E6 p0 l) W+ ~4 q& J9 U1 q 2. 32022部队, 广东 广州 510000; 3. 自然资源部海洋测绘重点实验室, 山东 青岛 266590
: t$ B9 }5 n( t$ ] 基金项目：国家自然科学基金(42074014)摘要：面向AUV搭载的传感器信息频率不一致与有效性易动态改变的情况, 因子图算法相较于扩展Kalman滤波算法表现出更好的稳定性、灵活性与扩展性。本文首先比较了FGO与EKF算法分别应用于AUV多传感器信息融合定位的性能, 再针对复杂水下环境中, 传感器的异常观测值影响FGO算法定位精度的问题, 提出了一种基于抗差因子图的AUV多源信息融合定位方法, 利用动态协方差缩放策略对粗差因子进行降权处理, 在海测数据的基础上模拟观测粗差进行算法验证。分别采用普通FGO算法、基于DCS的抗差FGO算法对受到粗差干扰后的数据进行解算。统计结果表明, 相较于不进行抗差处理, 该算法降低了14.6%的平面位置误差, 对异常观测具有良好的稳健性能。关键词：因子图 多源信息融合定位 自主水下潜航器 抗差因子图   引文格式：黄紫如, 柴洪洲, 向民志, 等. 基于抗差因子图的AUV多源信息融合定位方法[J]. 测绘学报，2023，52(8)：1278-1285. DOI: 10.11947/j.AGCS.2023.20210735 HUANG Ziru, CHAI Hongzhou, XIANG Minzhi, et al. AUV multi-source information fusion localization method based on robust factor graph[J]. Acta Geodaetica et Cartographica Sinica, 2023, 52(8): 1278-1285. DOI: 10.11947/j.AGCS.2023.20210735 阅读全文：http://xb.chinasmp.com/article/2023/1001-1595/20230805.htm引 言海洋中蕴藏着丰富的矿物、化学、生物和动力资源等。随着人口不断膨胀，陆地资源日渐稀缺，海洋勘探与开发成为各国持续关注的焦点[1-2]。AUV作为自主、灵活、隐蔽的重要作业载体，能够代替人类完成复杂、危险、苛刻的水下任务，为其提供实时精确的定位信息是保障AUV顺利作业的前提。目前，常用的AUV定位方案融合了捷联惯性导航系统(strap-down inertial navigation system, SINS)、多普勒测速仪(Doppler velocity log, DVL)与超短基线(ultra-short base line，USBL)等多种水下定位技术，利用传感器之间的互补特性进行多源信息融合，在水下定位中获得了广泛应用，包括单AUV的导航定位与多AUV协同定位场景[3-4]。以卡尔曼滤波为核心的融合算法应用广泛，具有计算效率高、灵活度高等优点，丹麦生产的MARIDAN150型AUV和挪威的HUGIN1000型AUV均使用卡尔曼滤波方法进行融合定位[5]。近年来，在SLAM定位领域发展的FGO方法开始应用于导航定位，因子图是一种表示变量联合概率分布的图模型，当接收到传感器信息时编码因子节点与变量节点进行全局优化，关联多源异步信息并采用固定频率解算[6]，具有“即插即用”的性质，展现出应用于AUV水下定位的潜力。文献[7]采用因子图算法融合AUV多传感器信息进行定位，利用仿真数据证明FGO与联邦卡尔曼滤波算法的精度相当，但灵活性与扩展性更好。文献[8]利用城市峡谷数据集，比较了两种算法应用于GNSS/INS松紧组合的定位效果，表明FGO算法解算精度更高，特别是在紧组合中展现出更好的性能[8]，在现有的工作中，仍缺少针对AUV实际环境FGO和EKF算法的性能比较。考虑到AUV进行水下作业时，观测条件较为苛刻，例如USBL发出的声波信号容易受到外界干扰，不可避免地出现异常测量值，直接进行融合将影响定位结果。目前常用的稳健性粗差处理方法包括抗差M估计和基于因子图的方法，M估计衍生出了Huber、IGG3和Cauchy等抗差估计方案。因子图类方法包括适用于混合高斯分布的最大混合(max-mixtures, MM)法[9]，文献[10]提出了可切换约束法(Switch Constraint, SC)，在图模型中引入切换变量约束异常因子，戴海发等将其应用于水面艇组合导航，取得了较好抗差效果[11]，但增加了额外的优化负担。文献[12]在SC基础上推导了动态协方差缩放(dynamic covariance scaling, DCS)方法，具有良好的应用前景。综上，本文从概率模型角度阐述了基于因子图与扩展卡尔曼滤波的多源信息融合定位算法原理。再利用天津海试的实测数据，将FGO与EKF算法应用于USBL/DVL/SINS多传感器融合定位，考察了在AUV机动较大、传感器长时间失效的情况下两种算法的性能并进行了详细分析与比较。针对AUV传感器异常观测的问题，提出了基于动态协方差缩放的抗差因子图算法，在海测数据上模拟信号干扰粗差，验证算法处理粗差的有效性。1 多源信息融合定位算法原理1.1 基于因子图的多源信息融合方法多源信息融合定位，本质上是在获取传感器测量值及其噪声的情况下求取变量最大后验概率(MAP)的问题。因子图模型代表随机变量的联合概率分布[13-14]，图中因素包括因子节点fi、变量节点xi、当且仅当fi与xi相关联时存在的连接边。变量节点即为定位所需的状态变量，tk时刻系统当前状态变量为 (1)式中，pk和vk分别表示k时刻导航系下，即基准站北东地坐标系下的3轴坐标与速度。rk表示偏航、俯仰、横滚姿态角。αk表示陀螺仪与加速度计观测值的零偏。AUV多源信息融合系统中的全部状态变量的集合可以表示为 (2)假设AUV搭载了SINS、USBL、DVL传感器, 系统在tk时刻接收到的测量值为zk=[zkSINSzkUSBLzkDVL]T，则所有历史测量值的集合为 (3)利用贝叶斯估计对全局函数进行因式分解，各时刻全体状态的联合后验概率密度为 (4)式中，P(X)表示所有变量的初始状态的先验信息；P(Xt|Xt-1, ztSINS)表示利用SINS递推的状态转移模型，P(zmUSBL|Xm)和P(znDVL|Xn)表示观测更新模型。状态变量的最大后验估计可表示为 (5)对于服从高斯噪声分布的测量，通常将每一个测量模型P(Xk|Zk)定义为一个因子P(Xk|Zk)∝fk(Xk)，用误差函数fk(Xk)=exp 来表示，其中‖·‖Σ2表示马氏距离的平方，Σ表示协方差矩阵。非线性最小二乘问题 (6)FGO优化时采用iSAM2增量平滑技术[15]，每次解算仅更新图中受影响的部分，提高了计算效率。AUV多源信息融合定位因子图框架如图 1所示，虚线部分表示tk时刻新添加到图中的边与状态变量。. `1 ~# i0 U7 P+ ^, B6 d
( Q" R; @0 }$ |
 图 1 多源信息融合定位因子Fig. 1 Multi-source information fusion location factor图选项 构建因子节点时，首先将AUV状态的先验信息进行建模为先验因子，该因子只与一个时刻的变量节点相关，为一元因子。假设先验信息服从均值为x0的高斯分布，则误差函数可表示为 (7)设tk时刻接收到DVL测速信息，DVL利用多普勒效应获得载体对地速度，其观测方程为 为DVL速度测量噪声， 表示速度状态变量与测量值之间的函数关系，与观测值作差，DVL误差函数可表示为 (8)设tk时刻接收到USBL测量信息，获得水面无人艇(unmanned surface vehicle, USV) 精确三维位置和USV上应答器对于AUV上基阵坐标系的相对三维位置信息，观测方程可列为zkUSBL=  为USBL位置测量噪声， 表示位置状态变量与测量值之间的函数关系，误差函数可表示为 (9)1.2 扩展Kalman滤波的图模型表示 为了与因子图方法进行对比，可以从贝叶斯估计的角度说明EKF多源信息融合算法的原理。EKF要求噪声服从高斯分布，状态量满足马尔可夫过程，即当前时刻的状态只与前一时刻有关，而与其他时刻相互独立。在状态更新时，对SINS测量值进行力学编排，假设上一时刻系统状态的最优估值为 ，其服从高斯分布。设线性化状态转移模型为Xk=FkXk－1+Gkwk。其中Fk为状态转移矩阵，Gk为系统噪声矩阵。wk为均值为零的高斯噪声序列，方差为Qk。预测过程可表示为 (10)再利用映射矩阵Hk将其从状态空间转换到观测空间，作为先验估计 (11)式中，系统状态的预测值满足均值为 =  ，方差阵为Pk=FkPk－1FkT+GkQkGkT的高斯分布。在时间更新时，观测值概率分布即为似然估计。设tk时刻传感器观测值服从均值为zk，协方差为Rk的高斯分布 (12)于是，更新过程可以表示为先验估计与似然估计的乘积，后验概率分布同样服从高斯分布 (13)式中，均值 ，协方差阵P′k=HkPkHkT-KkHkPkHkT，增益Kk=  。将其转换回状态空间后可得EKF算法公式，不断迭代可求得系统状态的后验估计 (14)可见，EKF模型的本质是一个被截断的因子图模型(图 2)，历史状态因子作为先验信息存储在图中，可以影响结果但不参与优化，仅有当前时刻变量节点、状态转移因子与观测因子进行优化。而FGO方法考虑了全体历史状态变量和测量值集合进行整体优化。2 o9 u0 E2 c8 b5 H, E6 g0 {* M
7 j+ M5 @8 T" b O Z# Z5 Y  图 2 EKF的因子Fig. 2 Factor graph representation of EKF图选项 2 多源信息融合定位抗差因子图算法构建FGO多源信息融合算法框架时，需要在各个传感器测量标称精度的基础上根据经验适当放大得到对应观测因子的噪声方差阵，代表信息融合系统对该观测的信任程度。若不降低粗差因子在融合中的权重，将导致定位精度下降甚至发散。因此，本文针对基本FGO定位算法进行稳健性改进。2.1 传感器异常观测值检测 在传统方法中，抗差估计在对所有的状态量进行优化后，计算当前测量值的残差，如果残差大于一定阈值，则判断其为粗差观测量进行处理，计算负载较大。考虑到AUV多源信息融合定位系统中，SINS自主性高，不易受到外界干扰，不妨假设为真实值。为避免重复优化、提高计算效率、便于程序实现，本文在对观测信息的检验过程中，利用观测值与SINS递推得到的状态预测估值之差作为判断参数，即新息向量 (15)如果当前时刻新息大于阈值，则认为当前观测异常，需要进行观测降权。图 3为异常观测处理流程。- |6 F1 }& y5 u4 i
+ r2 O/ S' e$ o' A: n  图 3 异常观测处理流程Fig. 3 Abnormal observation processing图选项 2.2 抗差因子图算法原理 在SLAM闭环检测中常用可切换约束来提升系统稳健性，基本思想是在因子图中引入开关变量，可以降低粗差观测因子对最优估计的影响，形成状态变量X和S的联合优化 (16)式中，S是估计的测量权重集，k表示各时刻测量值序号，i表示残差超过阈值的测量值序号；假设各个测量间相互独立，j为i时刻测量值与状态变量分量的序号，sij为变量si中的分量，Λi为观测因子的协方差阵，Ξi为切换因子的协方差阵，设两者皆为对角阵。ψ(sij)∈[0, 1]是一个比例函数，用于确定粗差观测因子的权重。可以解释为与约束相关的信息矩阵中的比例因子 (17)式中给出的关系直接显示了在可切换约束框架中实现的估计观测权重与先验测量协方差矩阵的缩放之间的关系。然而，对于每个多源信息融合定位图模型，优化器必须考虑额外的变量S。添加开关变量会扩大优化空间，增加每次迭代的计算成本，在这种情况下会增加问题的复杂性，因此可能会降低收敛速度。为了解决这个问题，文献[12]对SC方法进行了数学等价转换，本文在形式上进行了修改。首先不妨设ψ(sij)=sij，则联合函数中i时刻状态变量j分量的约束函数可表示为 (18)式中， 为当前观测的新息残差。Qij=Λij－1为观测因子的权，Pij=Ξij－1为切换因子的权。在联合函数收敛至全局最优解时，所有变量的偏导数为零，可得 (19)此时切换变量 (20)代入式(19)可得 (21)经分析，约束函数随观测误差增大将收敛至Pij，函数图形如图 4所示。因此 (22) (23)6 s# u: N7 m5 O$ b! _7 H' t
; X2 c( _, T0 p U6 q  图 4 约束函数图形Fig. 4 Constrained function graph图选项 式(23)表明了粗差约束的取值范围，基于此提出了DCS动态协方差缩放策略，该方法大大加快了收敛速度且便于实现。至此，对粗差因子进行约束时，可以令比例函数ψ(sij)由式(24)表示 (24)在传统M估计的Cauchy等价协方差方案中，等价权缩放公式为 (25)式中，m为阈值，若 大于m则观测被判定为粗差。由式(18)与式(19)可知，DCS算法与抗差M估计具有相似的形式和相同的原理，利用比例函数对粗差因子的协方差进行缩放，降低异常观测在系统中的可信度，更多地利用动力学模型信息，削弱对状态估值的影响，但DCS算法根据数据情况可以适当调整参数Φ的取值。3 试验与分析3.1 试验条件设置在天津塘沽海面试验中，搭建了装配有GNSS、SINS、DVL等多种传感器的数据采集平台，获取了一艘AUV在水面行驶的数据。图 5为试验期间AUV的航行轨迹。' c6 g% m- Q; J! X# n* K. N
8 r; C& X9 s: G. g: R
 图 5 AUV轨迹Fig. 5 The AUV trajectory图选项 由于缺乏USBL定位数据，本文假设在USV/AUV定位系统中，USBL传感器基阵安装在AUV上，应答器安装在水面USV载体上，为AUV提供位置观测，如图 6所示。
2 c; _9 X% T% p& m0 Y/ e4 H
8 U6 L2 r5 K4 f' a! y5 ~4 f+ p  图 6 模拟试验场景Fig. 6 The experimental scene图选项 提取AUV航行期间的光纤SINS与DVL观测数据，并将实测水面GNSS位置观测信息转换到载体系下模拟USBL应答器在基阵坐标系下的位置观测信息，更新频率为1 Hz，设置0.2%斜距的高斯白噪声。传感器测量参数见表 1。
4 v+ N+ ^5 K# G. k" ` 表 1 试验设备参数
) D) B, [+ G* I! F9 M7 k5 T, | Tab. 1 Experimental equipment parameters ( @8 I- P2 p. |0 M `% c
8 i1 Y5 q' t) I" l2 c
. }% F! t9 H9 T; ^1 @ 表选项
{. r/ x# Z0 {, e0 i0 f. _/ ~ 本文设计了两组试验，一是EKF与FGO算法的对比试验，分别采用EKF与FGO算法解算海测数据，综合对比两种算法性能，并模拟信号存在缺失的场景，验证FGO算法的定位精度与稳定性。二是通过人为设置USBL测量信息粗差，与不对粗差进行处理的普通FGO算法、M估计方法相比较，验证基于DCS抗差因子图算法的稳健性。本文在AUV的东向GNSS观测值上模拟粗差，假设AUV基阵脉冲信号每200 s遭遇一次敌方干扰，模拟粗差在仿真载体系USBL观测信息的分布如图 7所示。; O( a( J9 d1 v" {& B0 U) O" F8 F6 r
6 L( Q, e: O+ j5 i. L
 图 7 载体系下的模拟粗差Fig. 7 Gross error in body coordinate system图选项 3.2 试验结果分析 试验1在完成因子图算法与扩展卡尔曼滤波算法的实现与解算之后，表 2为两种算法的平面位置的均方根误差，可见定位精度相当。图 8为FGO与EKF的平面定位误差曲线，误差均处于-5~+5 m范围内。与航向角数值曲线对应分析，在AUV水下作业机动较大，观测误差增大的情况下，EKF定位误差曲线产生了数次突变，FGO定位结果更稳定，曲线较平稳。表 2 EKF、FGO定位误差统计结果Tab. 2 Statistical results of EKF and FGO positioning errors m7 _+ H3 Q6 r2 i& l4 S3 s
2 @. I' b4 `& c) s! l$ {
" D9 ]* c! D1 Q- o) X K. s) |, }; e9 U
表选项 . O( ~$ Z, G" ~7 u! B
" k1 P5 w) ~' X1 N1 k0 d6 O$ R
/ y( }, W6 K( S# V" \3 i U0 h
 图 8 EKF与FGO的平面误差曲线Fig. 8 Plane error curve of EKF and FGO图选项 在AUV航行期间设置了3段分别为200、300、400 s的USBL传感器信息缺失，图 9为EKF、FGO算法分别在无信息中断、有信息中断情况下的平面误差曲线。结果显示，在3段信息中断区间内，EKF算法精度明显降低，而FGO算法考虑了所有历史信息进行解算，能够有效抑制观测信息源缺失引起的发散。8 M2 ~+ w0 f( Y) D# b U
9 c# W" M& L+ r$ h; z! z6 ?6 _5 m  图 9 中断情况下平面误差曲线Fig. 9 Plane error curve in interrupt case图选项 此外，在图 10中截取某段误差曲线放大可见，在信息源重新可用时，EKF产生了系统性偏差，误差曲线相较于未中断时出现偏移，而FGO可以更贴近无中断时的精度水平。
7 |% [% M7 _' @/ I, Y" R
) _8 N% u. @ T' T) }  图 10 部分平面误差曲线Fig. 10 A part of plane error curve图选项 试验结果说明，基本EKF将非线性模型线性化后只进行了一次迭代，产生的截断误差较大[4]。FGO在优化中不断重新线性化，迭代逼近最优解，并且充分利用历史信息进行全局优化，提高了精度与稳定性。试验2在USBL测量数据上加上模拟粗差后进行算法的抗差性能验证。遍历观测文件，计算各个时刻新息向量是否超过阈值，若超过，则判断其为异常观测，利用2.2节中所提动态协方差策略进行降权处理。图 11为加入人工粗差但不进行处理与利用抗差FGO算法进行处理的误差曲线。可以看出，由于异常观测值的污染，FGO优化结果发生了偏移，且对前后一段时间内的定位结果造成了影响。利用DCS方法进行粗差处理，消除了异常偏移，有效平滑了误差曲线。4 L# y2 x& l4 _+ r* ~! ~! f) q
. O; ]4 E) P* e) N! e& M
 图 11 非抗差与抗差误差曲线Fig. 11 Plane error curve of robust FGO algorithm图选项 再利用M估计中的Cauchy方法进行抗差处理，与未加入人为粗差时的定位结果作差，抗差前后的偏离值如图 12所示，可见DCS算法削弱偏离值的效果总体上较好。
, T2 t" {+ j7 s: r# b1 _/ w
- a% d$ f9 p9 v' k6 n, H( g  图 12 DCS算法与M估计算法定位偏离值Fig. 12 Deviation value after using robust FGO图选项 由于模拟USBL数据本身不完全纯净，存在观测异常现象，本文对不加模拟粗差的数据集同样进行DCS方法解算。统计上述试验的平面定位误差见表 3。统计结果表明，未加入人为粗差时，使用抗差因子图算法精度有所提升。加入人为粗差后，使用抗差因子图进行处理后，相比普通因子图法精度提高了14.6%。表 3 抗差FGO定位误差统计结果Tab. 3 Statistical results of robust FGO positioning errors m " }* y0 W. ~& d( c5 _! a! j) g- |0 R
/ T v, L8 z' |2 s" C. ~$ \ 表选项
! Y% S% b5 _! O0 R6 H, ?' y) } `7 s 4 结论本文在天津塘沽海测数据的基础上，设置了传感器长时间失效情况，综合比较了使用EKF与FGO算法进行解算的定位性能。试验表明，本文所提出的方法与滤波方法定位精度相当，但FGO考虑了整个测量值与状态的集合进行优化，具有更好的稳定性；此外，FGO的优势还在于保留了再次对历史变量进行优化的接口，可以适应传感器信息发生滞后、异步的情况，将在另篇进行深入研究。针对水下环境易出现观测异常的问题，研究基于DCS的稳健性因子图算法，在USBL观测值上模拟干扰值进行算法验证，该方法能够削弱异常测量值导致的精度降低等影响，且性能略优于Cauchy估计方案，具有较好的抗差效果。 F- |! T; N8 J/ A/ g
作者简介第一作者简介：黄紫如(1997-), 女, 硕士, 助理工程师, 研究方向为多源信息融合定位技术。E-mail: ziruhuang97@163.com通信作者：柴洪洲, E-mail: chaihz1969@163.com8 f5 G. x0 w. x( Q1 V+ U& z( Y- [
初审：张艳玲
+ m$ q% x" N1 T/ [6 C# Z$ s( M7 z 复审：宋启凡 " p B8 c6 g! S; A
终审：金 君
' Q! X0 [7 E' P8 c- w6 ?3 G 资讯 % m+ r9 D7 I0 Q- t$ \
1 Z3 |" L2 \* U0 }+ l3 r" u( {
3 C( F2 t. [3 l- a5 t6 O
5 R+ R6 W4 j4 ^2 {* R$ t/ m& {4 x, O& G7 _7 \, I
$ W2 _6 V* A4 m
* v7 G8 D7 x8 D! d8 ~4 Q |
