因子图框架下无人艇主从式协同导航算法_张玉鹏.pdf

第 11 卷 第 2 期 导航定位学报 Vol.11，No.2 2023 年 4 月 Journal of Navigation and Positioning Apr.，2023 引文格式：张玉鹏,王子璇,刘剑威,等.因子图框架下无人艇主从式协同导航算法J.导航定位学报,2023,11(2):131-138.（ZHANG Yupeng,WANG Zixuan,LIU Jianwei,et al.Leader-follower cooperative navigation algorithm for unmanned surface vessels based on factor graph optimizationJ.Journal of Navigation and Positioning,2023,11(2):131-138.）DOI:10.16547/ki.10-1096.20230215.因子图框架下无人艇主从式协同导航算法 张玉鹏1，王子璇1，刘剑威1,2，蒲文浩1，刘锡祥1（1.东南大学 仪器科学与工程学院/微惯性仪表与先进导航技术教育部重点实验室，南京 210096；2.江苏自动化研究所，江苏 连云港 222061）摘要：针对无人艇（USV）协同导航存在非线性量测和滞后信息的问题，提出一种基于因子图优化的无人艇主从式协同导航算法：对艇间测距信息进行因子建模，以捷联惯性导航系统（SINS）为核心构建从艇因子图框架；然后针对因子图优化算法计算量大难以实时融合的问题，借助滑动窗限制待优化节点数并利用增量平滑进行窗内变量更新；最后将滞后信息对应因子节点连接至因子图中历史时刻变量节点，实现对于滞后量测免补偿使用。仿真结果表明，所提方法在信息滞后小于 1 s 时可获得与无滞后情况下大致相当的精度；以时长 15 s 的滑窗为例，所提算法较传统滑动窗优化单步运行时间减少约 91%，具有可实时实现的优点。半物理实验结果亦验证了所提方案的可靠性。关键词：水面无人艇（USV）；协同导航；滑动窗优化；增量平滑；滞后信息处理 中图分类号：P228 文献标志码：A 文章编号：2095-4999(2023)02-0131-08 Leader-follower cooperative navigation algorithm for unmanned surface vessels based on factor graph optimization ZHANG Yupeng1,WANG Zixuan1,LIU Jianwei1,2,PU Wenhao1,LIU Xixiang1（1.School of Instrument Science and Engineering,Southeast University/Key Laboratory of Micro-inertial Instrument and Advanced Navigation Technology,Ministry of Education,Nanjing 210096,China;2.Jiangsu Automation Research Institute,Lianyungang,Jiangsu 222061,China）Abstract：Aiming at the problem that nonlinear measurement and lag information affect unmanned surface vessel(USV)cooperative navigation,the paper proposed a USV cooperative navigation algorithm with leader-follower structure based on factor graph optimization:the factor modeling of ranging information between ships was carried out,and the factor graph of follower ship was constructed with strapdown inertial navigation system(SINS)as the main sensor;then in view of the fact that large amount of calculation in factor graph optimization algorithm leads to difficulty in real-time fusion,the number of nodes to be optimized was limited by means of the sliding window and the variables in the window were updated by incremental smoothing;finally,the factor node corresponding to the lag information was connected to the historical time variable node in the factor graph to realize the use of lag measurements without compensation.Simulational results showed that when the information lag is less than 1 s,the accuracy of the proposed method would be about the same as that without delay;in addition,taking the sliding window with a time of 15 s as an example,the proposed algorithm could reduce the single-step running time by about 91%compared with the traditional sliding window optimization,which indicates the advantage of real-time implementation.Moreover,the results of semi-physical simulational tests could also verify the reliability of the proposed scheme.Keywords:unmanned surface vessel(USV);cooperative navigation;sliding window optimization;incremental smoothing;lag information processing 收稿日期：2022-04-14 第一作者简介：张玉鹏（1997），男，江苏省徐州人，硕士研究生，研究方向为协同导航。通信作者简介：刘锡祥（1976），男，江苏省海安人，博士，教授，研究方向为惯性导航、组合导航与信息融合技术。132 导航定位学报 2023 年 4 月 0 引言 近年来，随着海洋和内河探索的发展，无人艇（unmanned surface vessel，USV）以其机动灵活、较高隐蔽性、可长时间作业等特性成为重要工具。同时诸如围补、打捞、驱离等作业需要多艘无人艇共同配合完成1，与单艘艇作业相比，无人艇集群可通过协作，高效地完成复杂任务，具有极大的实际应用价值。实时导航定位是保证无人艇集群高效执行各项任务的必要前提。对于单艇，传统导航方式以捷联惯导系统（strapdown inertial navigation system,SINS）为核心，利用多普勒测速仪（Doppler velocity log,DVL）、全球卫星导航系统（global navigation satellite system,GNSS）等辅助传感器进行修正2；除此之外，无人艇集群可利用艇间观测信息提升定位精度。文献3针对主从式 USV 协同导航定位，建立协同定位状态空间模型，利用扩展卡尔曼滤波进行信息融合。文献4针对协同导航设计一种分散式卡尔曼滤波算法，该算法在与传统滤波同等精度条件下所需通信次数减少。文献5提出一种基于因子图算法的多水下无人艇（autonomous underwater vehicle,AUV）协同导航方法，集中处理集群信息，定位精度优于传统滤波算法。文献6结合无人机自身导航信息和无人机间测距信息，提出一种基于因子图优化的分布式协同导航算法。但传统因子图优化算法计算量大，难以实时融合信息，多用于事后处理7。现有协同导航方案往往忽略因通信时延导致的量测信息滞后问题。针对滞后信息，基于滤波处理方法主要为外推和扩增，用滞后数据预测当前时刻量测，构建等效量测信息，此类方法会引入推估误差8；而借助因子图优化可追溯历史时刻变量节点的特点，可免补偿使用滞后信息9。文献8将此特性应用于 GNSS/INS 组合导航系统中，以降低 GNSS 信息滞后导致的定位误差。文献10利用此特点处理 AUV 信标信息延迟，与曲线拟合补偿方法进行了比较，发现基于因子图的滞后信息补偿能更为有效地补偿顺序、出序等多种滞后信息。无论文献8还是文献10均未对算法的实时性进行详细讨论。针对无人艇实时协同导航需求，本文首先介绍了一种基于因子图优化的主从式协同导航方案，以 SINS 为核心，将艇间的距离信息作为协同导航因子节点构建因子图模型；其次结合增量平滑和滑动窗优化的优点，提出一种改进型滑动窗优化算法，其具备实时求解模型的能力；然后针对协同信息存在的滞后问题引入滞后信息处理方案。1 无人艇协同导航方案 在无人艇集群执行复杂任务过程中，为了保证任务的有效实施，需要集群具有较高的定位精度。单艇利用高精度惯导并借助 DVL 等进行辅助，可获得较准确的导航信息6；但为集群中所有无人艇装配高精度导航设备成本较高。本方案中，为了保证集群可获得较高精度的导航信息，部分无人艇配备高精度惯性测量单元（inertial measurement unit,IMU）、DVL 和测距仪作为领航艇，其余无人艇仅配备精度相对较低的 IMU 和 DVL 等导航设备作为从艇。领航艇以预定时间间隔广播定位信息、位置协方差和与附近艇的相对距离，从艇接受带有时间戳的报文，通过对于位置协方差和时间戳判断协同信息的可靠性和实时性，建立因子图模型，利用优化算法实现对于相对定位信息的利用。双领航者协同方案如图 1 所示。图 1 无人艇群协同导航 2 基于因子图的信息融合方法 2.1 系统模型与算法原理 因子图模型由因子节点、变量节点构成，因子模型可表达随机变量的联合概率分布。构建融合协同定位信息的因子图框架时，将k时刻的导航状态量建模成变量节点kx，表示为 NEDNEDT kxyzxyzpppvvv=x（1）式中：、为姿态角；NEDppp、分别为北（N）向、东（E）向和地（D）向位置；NEDvvv、分别为北向、东向、地向速度；xyz、表示陀 第 2 期 张玉鹏，等.因子图框架下无人艇主从式协同导航算法 133 螺仪 3 个轴向漂移；xyz、表示加速度计 3 个轴向零偏。kX为初始 0 时刻至k时刻状态变量节点集合，表示为 Tkk=01Xxxx（2）kz为k时刻传感器的量测值，其中包括 IMU量测值IMUkz、DVL 量测DVLzk以及艇间距离量测COOPkz等。kZ为初始 0 时刻至k时刻系统接受到的所有量测值的集合，表示为 Tkk=01Zzzz（3）根据链式法则，状态变量和量测的联合概率密度kkP XZ（|）可利用因式分解的形式表示，并可转化成n个代价函数连乘积的形式11，其中每个代价函数对应因子图中的一个因子节点。kkP XZ（|）具体表示为 IMU-(|)()(|)(|)()jiiikkkiiiinjiimkmzPpppf=0111（4）zzXZXxx,uzXX 式中：()p0X表示待估计量的先验；-(|,)iiip1xxu表示 条 件 概率密 度，其中iu为 无 人艇输 入 量；(|)jiip zX表示观测更新模型；()mkfX表示代价函数。在高斯分布下，先验信息可用均值X和均方根误差表示，则()p0X可表示为 ()()exp()pf=-XXXX200012（5）在本系统中 IMU 输入可认为是iu，其受均方根误差为Q的高斯白噪声值影响5，此时条件概率密度函数可用代价函数表示为 IMU-(|,)()exp(g()iiikiiipf=-21112QxxuXx,ux （6）除了 IMU 以外的传感器j在i时刻的观测值jiz受高斯零均值噪声的干扰，用协方差矩阵R表示零均值高斯分布量测噪声，用()jh来表示传感器j的观测模型，则观测更新模型对应的代价函数表示为 (|)()exp()jjjjiikiipfh=-212RzXXXz（7）针对因子图模型，依据最大后验概率密度估计进行求负对数、马氏范数转换二范数等过程，将信息融合问题转换为非线性最小二乘问题，进行迭代求解11，获得最优估计，具体可表示为 IMU/,argmax(|)argmin log(|)argmin()(,)()kkkjiiikkkkkkiiiikjjiiiPPgh-=-=-+-+-XXXzz zXXZXZXXQxuxRXz 221 21 20122121 221（8）2.2 因子节点模型描述 构建因子图框架须为各个传感器的建立因子节点。下面介绍 USV 协同定位中常用的传感器因子模型，每个因子可表示成一个代价函数，代价函数可用误差函数()d 的形式表示。2.2.1 IMU 因子节点 IMU 因子节点输出的是相对前一时刻的载体坐标系的状态增量，该节点连接前后 2 个时刻的变量节点kx和k-1x，为二元因子节点。该节点可根据在某一时刻接受的 IMU 量测信息IMU,bbkf=z，其中bf为载体坐标系b下比力，b为载体坐标系下的角速度。结合前一时刻的估计状态k-1x来更新当前时刻的状态kx，其对应的噪声矩阵为Q。该因子节点可表示为 IMUIMUIMU,IMUIMU(,)d(,)exp(,)k kkkkkkkkkfhh-=-Qxxxzxxzx 111211（9）2 2.2.2 DVL因子节点 DVL 的速度量测方程为DVLDVLDVL()kkhn=+zx，其中DVL()khx为 DVL坐标系下速度的真实值，DVLn为测速噪声，对应的测速噪声矩阵为DVLR。DVL 量测只与当前时刻导航状态有关，即只与变量节点kx有关，因此 DVL 因子节点为一元因子节点，可表示为 DVLDVLDVLDVLDVLDVL()d()exp()kkkkkkfhh-=-Rxxzxz 2 1（10）2 2.2.3 协同测距因子节点 无人艇间的测距值可表示为COOP,COOPk,irn=+iz，其中k,ir为本艇k时刻与艇i之间的二维距离值，COOPn为测距噪声，对应的测距噪声矩阵为COOP,iR，观测函数COOP()khx为 COOPEE,NN,()()()kiihpppp=-+-x22 （11）134 导航定位学报 2023 年 4 月 式中：EP和NP分别为本艇当前时刻状态变量kx中东向和北向位置；E,iP和N,iP分别为艇i的东向和北向位置。协同测距信息对应因子节点可表示为 COOP,COOPCOOPCOOP,COOPCOOP,()d()exp()iikkkkikkfhh-=-Rxxzxz 21（12）2 线性化观测函数COOP()khx的雅可比矩阵为 COOPCOOP,1 3EENNCOOP,1 31 6()/()/0i,ik,iikpppp=-HO z z O O （13）式中 O 表示零矩阵。2.3 主从协同导航的因子图描述及推理 因无人艇群中主艇可利用高精度的设备获得较为准确的导航信息，本文重点讨论从艇的数据融合。根据无人艇集群的协同方案及上一小节定义的因子节点，对于从艇构建因子图模型。当接收到传感器观测信息时，添加对应节点，如图 2所示。先验因子为无人艇的初始导航状态信息，由捷联惯导的初始对准获得；IMU 因子连接前后2 个相邻时刻的变量节点，DVL 因子与对应变量节点相关联，当从艇获得领航艇信息时，添加协同因子。图中虚线表示k时刻添加的因子节点和变量节点。图 2 从艇因子图模型 2.3.1 滑动窗口优化 传统的因子图推理方法一次推理须对所有变量节点进行求解，随着时间增长，计算量不断增加，无法满足导航系统实时性需求11。在导航系统中最新观测值对于早期变量估计准确性提高不显著，早期估计对于当前状态估计准确性提高亦无明显帮助；因此可利用滑动窗口限定参与优化的节点数12，减少优化算法的计算量。通过对于窗口内节点进行批量优化可有效利用部分过去时刻的信息对于当前时刻进行估计，滑动窗口优化（sliding windows optimization，SWO）为优化算法实时处理信息提供了可能。算法流程如图 3 所示。图 3 滑动窗口优化 2.3.2 改进型滑动窗口优化 通过观察图 3，可发现k时刻添加的因子节点只与状态量kx与k-1x直接相连，仅须重新计算部分受影响的节点即可。基于文献13提出 iSAM2 算法，借助贝叶斯树对于受影响节点进行计算，在保留部分历史计算结果的基础上，实现增量更新，可改善优化算法的实时性。但随着状态量的增加，iSAM2 算法依然需要在部分时刻对于大量变量节点重新计算，同时亦需要存储较多时刻的状态信息。本文结合 iSAM2 增量平滑算法的优点对于滑动窗口因子图优化进行改进，在窗口内进行增量平滑而非批量优化，获得改进型动窗优化算法（improved sliding windows optimization，ISWO），算法流程如图 4 所示。与传统算法不同，该算法在添加新观测量时无须重新计算所有雅可比矩阵，仅对窗内受影响部分节点进行更新，可进一步提高算法实时性。图 4 基于增量平滑的改进滑动窗口优化 2.4 信息滞后处理方案 基于因子图优化的算法可在一次信息融合时刻完成对于历史导航状态量和当前导航状态量的更新，使得因子图算法在处理信息滞后具有得天独厚的优势。具体处理流程如图 5 所示。当从艇接收到的协同信息存在滞后时，如在k-1时刻未接收到COOPk-1z，因子图模型只添加IMU,kkf-21；在k时刻接收到COOPk-1z，此时根据时间戳判断滞后量，并与过去时刻状态关联，如图 2 中的因子COOPkf-1所示。通过因子图优化在k时刻利用更为精确的k-1x值进行导航状态量更新。第 2 期 张玉鹏，等.因子图框架下无人艇主从式协同导航算法 135 图 5 协同量测信息滞后处理流程 3 实验与结果分析 为了验证本文提出的基于因子图优化的无人艇主从式协同导航方法的性能，分别进行仿真验证和半实物实验验证。实验均在内存为 16 GB、主频为 2.6 GHz 的 Intel Core i7-10750H 笔记本电脑中进行。3.1 仿真实验 3.1.1 仿真条件设置 数值仿真包含 2 艘领航艇（领航艇 1 和领航艇 2）和 1 艘从艇的协同导航，领航艇利用高精度的 IMU 和 DVL 进行组合导航，从艇利用 IMU、DVL 和协同信息进行导航。3 艘艇的运动轨迹如图 6 所示。图 6 无人艇运动轨迹 各艇从静止出发，加速至 10 节左右开始匀速航行，仿真时长为 1 800 s。从艇 IMU 更新频率为 100 Hz，陀螺常值漂移误差为 0.05()/h，角度随机游走系数为 0.05.()/h0 5，加速度常值漂 移 误 差 为.-31 96 10 m/s2，速 度 随 机 游 走 为.-59 8 10.m/(sHz)20 5；DVL 更新频率为 1 Hz，量测噪声为幅值 0.1 m/s 的高斯白噪声；领航艇协同信息更新频率均为 1 Hz，量测噪声为 0.2%测距值的高斯白噪声。为了验证本文所提方法在融合测距信息时的有效性和实时性，分别利用扩展卡尔曼滤波（extended Kalman filter，EKF）、无迹卡尔曼滤波（unscented Kalman filter，UKF）和 2 种滑动窗口优化算法（窗口长均为 15 s）对于无滞后信息的情况进行数值仿真实验，并利用改进型滑动窗算法验证滞后处理方案在周期性协同量测信息存在滞后时的效果，信息融合周期为 0.5 s。3.1.2 结果分析 从艇的仿真结果如图 7 所示。图中 EKF 表示EKF 的融合结果，UKF 表示 UKF 的融合结果，SWO-15 s 表示窗口长为 15 s 的滑动窗优化算法的融合结果，ISWO-15 s 表示窗口长为 15 s 的改进的滑动窗口优化算法的融合结果，4 种算法的融合结果分别用点线、划线、浅色直线和深色直线表示。在整个仿真时间段内，几种方法都能保持相对较高的的导航精度，北向和东向定位误差大部分在 -5.0+5.0 m 以内。图 7 4 种方法的位置误差对比曲线 由图可知：在匀速直线运动阶段，因子图优化算法引入之前时刻信息用于校正当前时刻估计，精度略高且估计的曲线更平滑；当改变运动状态，如在 0200 s 范围内从艇沿正北方向做加速运动以及由直线运动进入转向时，优化算法因较多利用 136 导航定位学报 2023 年 4 月 过去时刻信息，导致稳定性不如基于卡尔曼滤波的方法。由图 7 可知 0200 s 内，优化算法的北向定位误差明显大于滤波算法，因此致使整个阶段轨迹的北向定位均方根误差（root mean square error，RMSE）大于滤波方法。从艇定位的均方根误差统计如表 1 所示。表 1 从艇定位结果统计 算法 北向位置的 RMSE/m 东向位置的 RMSE/m 平面位置的 RMSE/m 单步融合耗时/s EKF 1 1.8989 2.81 3.39 UKF 2.02 2.35 3.10 SWO 2.77 1.88 3.35 0.12 ISWO 2.35 1 1.5050 2 2.7878 0 0.0101 比较 2 种基于滤波的方法可以看出：大部分时间 UKF 算法的定位精度高于 EKF 算法，可见 UKF算法对于系统非线性的适应性更强；但当系统状态出现较大变化时，UKF 算法中的西格玛（Sigma）采样点取值未必能够精确反映系统的特性，导致滤波精度反而低于 EKF 算法。比较 2 种基于因子图优化的算法，可发现改进型滑动窗优化算法精度略高，因其仅针对受影响的部分变量更新，相较于传统滑动窗优化减少了因对于滑窗内变量节点无差别更新引入的数值计算误差。如图 8 所示，改进型算法运行时间远短于传统滑动窗优化算法，可发现单步融合时间约减少91%，为实时处理滞后信息创造了条件。图 8 因子图优化方法运行时间 为了分析因子图优化在处理滞后协同信息时的效果，以滑动窗长为 15 s 的改进型滑动窗口优化为融合工具，针对不同滞后时长，以领航艇信息无滞后的情况作为对照组，设计 6 组实验，如表 2所示。表 2 实验设置 编号 领航艇 1 信息滞后时长/s 领航艇 2 信息 滞后时长/s 滞后处理方案实验 1 0.5 0.5 未处理 实验 2 0.5 0.5 历史节点匹配 实验 3 1.0 1.0 未处理 实验 4 1.0 1.0 历史节点匹配 实验 5 0.5 1.0 未处理 实验 6 0.5 1.0 历史节点匹配 各组实验中从艇的仿真结果如图 9 所示。图中对照组为 2 艘领航艇协同信息均无滞后时的融合结果，因不补偿的实验组中误差发散明显，仅绘制了使用历史节点匹配滞后处理方案的实验对应的误差曲线。图 9 不同实验条件下的位置误差对比曲线 以无滞后信息时的结果作为对照，可发现从艇通过时间戳与历史节点进行匹配后，获得与无滞后信息情况下大致相当的定位精度，均方根误差如表 3 所示。滞后时长越长，对于艇定位精度影响 表 3 各组实验定位均方根误差值 m 位置误差统计 实验 1 的 融合结果 实验 2 的 融合结果 实验 3 的 融合结果 实验 4 的 融合结果 实验 5 的 融合结果 实验 6 的 融合结果 北向位置的 RMSE 1.98 1 1.5858 3.32 1 1.8080 15.74 1 1.6969 东向位置的 RMSE 2.40 1 1.4242 3.99 1 1.4343 15.82 1 1.4040 平面位置的 RMSE 3.11 2 2.1212 5.19 2 2.3131 22.32 2 2.2020 第 2 期 张玉鹏，等.因子图框架下无人艇主从式协同导航算法 137 越大；当 2 艘领航艇滞后信息时长不同时，若不处理则误差发散较为明显，而通过因子图优化可获得较高定位精度。3.2 半物理实验 为进一步验证本文算法的有效性，在苏州元荡进行实验，获取 3 艘水面艇航行数据，并根据数据进行半物理仿真实验。3.2.1 仿真条件设置 3 艘艇均配备 GNSS-RTK 接收机和导航计算机，各艇导航位置参考值由实时动态（Real-time kinematic，RTK）差分定位数据提供。从艇另配有IMU，输出频率为 100 Hz，陀螺零偏稳定性优于0.05()/h，随机游走优于 0.05.()/h0 5，加速度计常值零偏优于.-31 96 10 m/s2，随机游走优于.-59 8 10.m/(sHz)20 5。通过对领航艇位置参考值添加幅值 0.5 m 的白噪声来模拟领航艇的位置解算信息。半物理实验所用艇外形和各艇轨迹如图10 所示。从艇完成惯导初始对准后，3 艘同时机动，选取 3 艘艇运动阶段数据，时长均为 470 s。实验过程中 GNSS 信号良好。艇间测距参考值由对应时刻位置参考值做差获得，添加量测噪声为 0.2%测距值的高斯白噪声模拟协同信息，领航艇 1 和领航艇 2 协同信息广播频率分别为 1 和 2 Hz。以窗长为 15 s 的改进滑动窗口优化为融合工具，依据 图 10 半物理实验所用艇外形和各艇轨迹 表 2 设置进行实验，并以协同信息无滞后时的融合结果作为对照组。3.2.2 结果分析 按照各组实验设置进行实验获得的位置误差如表 4 所示。USV 协同导航实验误差曲线如图 11 所示，定位误差保持在较小范围内。根据表 4 的统计结果，亦可得到 3.1 节的结论。表 4 各组实验定位均方根误差值 m 位置误差统计 对照组的 融合结果 实验 1 的 融合结果 实验 2 的 融合结果 实验 3 的 融合结果 实验 4 的 融合结果 实验 5 的 融合结果 实验 6 的 融合结果 北向位置的 RMSE 1.93 1.95 1 1.4747 2.00 1 1.5858 22.13 1 1.7777 东向位置的 RMSE 2.76 3.57 3 3.1414 3.90 3 3.0505 23.52 3 3.6161 平面位置的 RMSE 3.30 4.07 3 3.4747 4.38 3 3.4444 32.29 4 4.0202 图 11 不同实验条件下的位置误差对比曲线 138 导航定位学报 2023 年 4 月 4 结束语 本文提出了基于因子图优化的无人艇主从协同算法，实现了对于艇群中相对测距信息融合。该方法将导航状态量作为变量节点，从艇将接收到的距离量测信息抽象为协同导航节点，依据非线性优化理论实现状态变量的更新。为提升算法实时性，利用滑动窗口限制参与优化的节点数量，利用增量平滑对滑窗内的节点进行增量更新。针对协同导航中因通信而导致的滞后情况，在因子图框架下可免补偿地使用滞后量测信息。仿真实验结果表明，所提算法具有可有效融合艇间信息，且可实时实现的特点，当滞后时长小于 1 s 时，可获得与无滞后大致相当的导航精度。半物理实验结果表明，本文方案亦具有较高的实际应用价值。本文仅对无人艇间距离信息进行融合，未来可针对诸如艇间方位角等其他信息的融合开展进一步研究。参考文献 1 LIU W W.Robust multi-sensor data fusion for practical unmanned surface vehicles(USVs)navigationD.London：University College London,2020.2 TANAKITKORN K.A review of unmanned surface vehicle developmentJ.Maritime Technology and Research,2019,1(1):2-8.3 徐博,肖永平,高伟,等.主从式无人水面艇协同定位滤波方法与实验验证J.兵工学报,2014,35(11):1836-1845.4 BURCHETT B T.Distributed Kalman filters for cooperative localization of munition swarmsEB/OL.2022-04-01.https:/apps.dtic.mil/sti/pdfs/AD1080927.pdf.5 FAN S,ZHANG Y,YU C,et al.An advanced cooperative positioning algorithm based on improved factor graph and sum-product theory for multiple AUVsJ.IEEE Access,2019,7(1):67006-67017.6 陈明星,熊智,刘建业,等.基于因子图的无人机集群分布式协同导航方法J.中国惯性技术学报,2020,28(4):456-461.7 GRISETTI G,GUADAGNION T,ALOISE I,et al.Least squares optimization:from theory to practiceJ.Robotics,2020,9(3):51.8 高军强,汤霞清,张环,等.基于因子图算法的 INS/GPS 信息滞后处理方法J.计算机应用,2018,38(11):3342-3347.9 SNDERHAUF N,LANGE S,PROTZEL P.Incremental sensor fusion in factor graphs with unknown delaysEB/OL.2022-04-01.https:/www.tu-chemnitz.de/etit/proaut/publications/ASTRA13-delays.pdf.10 黄紫如,柴洪洲,向民志,等.考虑信息滞后的 AUV 因子图多源信息融合定位算法J.中国惯性技术学报,2021,29(5):625-631.11 DELLAERT F,KAESS M.Factor graphs for robot perceptionM.Hanover:Now Publishers Inc.,2017:1-139.12 WILBERS D,MERFELS C,STACHNISS C.Localization with sliding window factor graphs on third-party maps for automated drivingC/The Institude of Electrical and Electronic Engineers(IEEE).2019 International Conference on Robotics and Automation(ICRA).Palms Springs:IEEE,2019:5951-5957.13 KAESS M,JOHANNSSON H,ROBERTS R,et al.iSAM2:incremental smoothing and mapping using the Bayes treeJ.The International Journal of Robotics Research,2012,31(2):216-235.