因子图框架下无人艇主从式协同导航算法_张玉鹏.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，