因子图框架下无人艇主从式协同导航算法

张玉鹏，王子璇，刘剑威,2，蒲文浩，刘锡祥

因子图框架下无人艇主从式协同导航算法

张玉鹏1，王子璇1，刘剑威1,2，蒲文浩1，刘锡祥1

（1. 东南大学 仪器科学与工程学院/微惯性仪表与先进导航技术教育部重点实验室，南京 210096；2. 江苏自动化研究所，江苏 连云港 222061）

针对无人艇（USV）协同导航存在非线性量测和滞后信息的问题，提出一种基于因子图优化的无人艇主从式协同导航算法：对艇间测距信息进行因子建模，以捷联惯性导航系统（SINS）为核心构建从艇因子图框架；然后针对因子图优化算法计算量大难以实时融合的问题，借助滑动窗限制待优化节点数并利用增量平滑进行窗内变量更新；最后将滞后信息对应因子节点连接至因子图中历史时刻变量节点，实现对于滞后量测免补偿使用。仿真结果表明，所提方法在信息滞后小于1 s时可获得与无滞后情况下大致相当的精度；以时长15 s的滑窗为例，所提算法较传统滑动窗优化单步运行时间减少约91%，具有可实时实现的优点。半物理实验结果亦验证了所提方案的可靠性。

水面无人艇（USV）；协同导航；滑动窗优化；增量平滑；滞后信息处理

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 系统模型与算法原理

针对因子图模型，依据最大后验概率密度估计进行求负对数、马氏范数转换二范数等过程，将信息融合问题转换为非线性最小二乘问题，进行迭代求解[11]，获得最优估计，具体可表示为

2.2 因子节点模型描述

2.2.1 IMU因子节点

2.2.2 DVL因子节点

2.2.3 协同测距因子节点

协同测距信息对应因子节点可表示为

式中表示零矩阵。

2.3 主从协同导航的因子图描述及推理

图2 从艇因子图模型

2.3.1 滑动窗口优化

传统的因子图推理方法一次推理须对所有变量节点进行求解，随着时间增长，计算量不断增加，无法满足导航系统实时性需求[11]。在导航系统中最新观测值对于早期变量估计准确性提高不显著，早期估计对于当前状态估计准确性提高亦无明显帮助；因此可利用滑动窗口限定参与优化的节点数[12]，减少优化算法的计算量。通过对于窗口内节点进行批量优化可有效利用部分过去时刻的信息对于当前时刻进行估计，滑动窗口优化（sliding windows optimization，SWO）为优化算法实时处理信息提供了可能。算法流程如图3所示。

图3 滑动窗口优化

2.3.2 改进型滑动窗口优化

图4 基于增量平滑的改进滑动窗口优化

2.4 信息滞后处理方案

基于因子图优化的算法可在一次信息融合时刻完成对于历史导航状态量和当前导航状态量的更新，使得因子图算法在处理信息滞后具有得天独厚的优势。具体处理流程如图5所示。

图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 无人艇运动轨迹

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种方法的位置误差对比曲线

由图可知：在匀速直线运动阶段，因子图优化算法引入之前时刻信息用于校正当前时刻估计，精度略高且估计的曲线更平滑；当改变运动状态，如在0～200 s范围内从艇沿正北方向做加速运动以及由直线运动进入转向时，优化算法因较多利用过去时刻信息，导致稳定性不如基于卡尔曼滤波的方法。由图7可知0～200 s内，优化算法的北向定位误差明显大于滤波算法，因此致使整个阶段轨迹的北向定位均方根误差（root mean square error，RMSE）大于滤波方法。从艇定位的均方根误差统计如表1所示。

表1 从艇定位结果统计

比较2种基于滤波的方法可以看出：大部分时间UKF算法的定位精度高于EKF算法，可见UKF算法对于系统非线性的适应性更强；但当系统状态出现较大变化时，UKF算法中的西格玛（Sigma）采样点取值未必能够精确反映系统的特性，导致滤波精度反而低于EKF算法。比较2种基于因子图优化的算法，可发现改进型滑动窗优化算法精度略高，因其仅针对受影响的部分变量更新，相较于传统滑动窗优化减少了因对于滑窗内变量节点无差别更新引入的数值计算误差。

如图8所示，改进型算法运行时间远短于传统滑动窗优化算法，可发现单步融合时间约减少91%，为实时处理滞后信息创造了条件。

图8 因子图优化方法运行时间

为了分析因子图优化在处理滞后协同信息时的效果，以滑动窗长为15 s的改进型滑动窗口优化为融合工具，针对不同滞后时长，以领航艇信息无滞后的情况作为对照组，设计6组实验，如表2所示。

表2 实验设置

各组实验中从艇的仿真结果如图9所示。图中对照组为2艘领航艇协同信息均无滞后时的融合结果，因不补偿的实验组中误差发散明显，仅绘制了使用历史节点匹配滞后处理方案的实验对应的误差曲线。

图9 不同实验条件下的位置误差对比曲线

以无滞后信息时的结果作为对照，可发现从艇通过时间戳与历史节点进行匹配后，获得与无滞后信息情况下大致相当的定位精度，均方根误差如表3所示。滞后时长越长，对于艇定位精度影响越大；当2艘领航艇滞后信息时长不同时，若不处理则误差发散较为明显，而通过因子图优化可获得较高定位精度。

表3 各组实验定位均方根误差值 m

3.2 半物理实验

为进一步验证本文算法的有效性，在苏州元荡进行实验，获取3艘水面艇航行数据，并根据数据进行半物理仿真实验。

3.2.1 仿真条件设置

从艇完成惯导初始对准后，3艘同时机动，选取3艘艇运动阶段数据，时长均为470 s。实验过程中GNSS信号良好。艇间测距参考值由对应时刻位置参考值做差获得，添加量测噪声为0.2%测距值的高斯白噪声模拟协同信息，领航艇1和领航艇2协同信息广播频率分别为1和2 Hz。以窗长为15 s的改进滑动窗口优化为融合工具，依据表2设置进行实验，并以协同信息无滞后时的融合结果作为对照组。

图10 半物理实验所用艇外形和各艇轨迹

3.2.2 结果分析

按照各组实验设置进行实验获得的位置误差如表4所示。

USV协同导航实验误差曲线如图11所示，定位误差保持在较小范围内。根据表4的统计结果，亦可得到3.1节的结论。

表4 各组实验定位均方根误差值 m

图11 不同实验条件下的位置误差对比曲线

4 结束语

本文提出了基于因子图优化的无人艇主从协同算法，实现了对于艇群中相对测距信息融合。该方法将导航状态量作为变量节点，从艇将接收到的距离量测信息抽象为协同导航节点，依据非线性优化理论实现状态变量的更新。为提升算法实时性，利用滑动窗口限制参与优化的节点数量，利用增量平滑对滑窗内的节点进行增量更新。针对协同导航中因通信而导致的滞后情况，在因子图框架下可免补偿地使用滞后量测信息。仿真实验结果表明，所提算法具有可有效融合艇间信息，且可实时实现的特点，当滞后时长小于1 s时，可获得与无滞后大致相当的导航精度。半物理实验结果表明，本文方案亦具有较高的实际应用价值。本文仅对无人艇间距离信息进行融合，未来可针对诸如艇间方位角等其他信息的融合开展进一步研究。

[1] LIU W W. Robust multi-sensor data fusion for practical unmanned surface vehicles (USVs) navigation[D]. London：University College London, 2020.

[2] TANAKITKORN K. A review of unmanned surface vehicle development[J]. 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 swarms[EB/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 AUVs[J]. 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 practice[J]. Robotics, 2020, 9(3): 51.

[8] 高军强, 汤霞清, 张环, 等. 基于因子图算法的INS/GPS信息滞后处理方法[J]. 计算机应用, 2018, 38(11): 3342-3347.

[9] SÜNDERHAUF N, LANGE S, PROTZEL P. Incremental sensor fusion in factor graphs with unknown delays[EB/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 perception[M]. 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 driving[C]//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 tree[J]. The International Journal of Robotics Research, 2012, 31(2): 216-235.

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）

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.

unmanned surface vessel (USV); cooperative navigation; sliding window optimization; incremental smoothing; lag information processing

P228

A

2095-4999(2023)02-0131-08

张玉鹏, 王子璇, 刘剑威, 等. 因子图框架下无人艇主从式协同导航算法[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 optimization[J]. Journal of Navigation and Positioning, 2023, 11(2): 131-138.）DOI:10.16547/j.cnki.10-1096.20230215.

2022-04-14

张玉鹏（1997—），男，江苏省徐州人，硕士研究生，研究方向为协同导航。

刘锡祥（1976—），男，江苏省海安人，博士，教授，研究方向为惯性导航、组合导航与信息融合技术。