張玉鵬，王子璇，劉劍威,2，蒲文浩，劉錫祥

因子圖框架下無(wú)人艇主從式協(xié)同導(dǎo)航算法

張玉鵬1，王子璇1，劉劍威1,2，蒲文浩1，劉錫祥1

（1. 東南大學(xué) 儀器科學(xué)與工程學(xué)院/微慣性儀表與先進(jìn)導(dǎo)航技術(shù)教育部重點(diǎn)實(shí)驗(yàn)室，南京 210096；2. 江蘇自動(dòng)化研究所，江蘇 連云港 222061）

針對(duì)無(wú)人艇（USV）協(xié)同導(dǎo)航存在非線性量測(cè)和滯后信息的問(wèn)題，提出一種基于因子圖優(yōu)化的無(wú)人艇主從式協(xié)同導(dǎo)航算法：對(duì)艇間測(cè)距信息進(jìn)行因子建模，以捷聯(lián)慣性導(dǎo)航系統(tǒng)（SINS）為核心構(gòu)建從艇因子圖框架；然后針對(duì)因子圖優(yōu)化算法計(jì)算量大難以實(shí)時(shí)融合的問(wèn)題，借助滑動(dòng)窗限制待優(yōu)化節(jié)點(diǎn)數(shù)并利用增量平滑進(jìn)行窗內(nèi)變量更新；最后將滯后信息對(duì)應(yīng)因子節(jié)點(diǎn)連接至因子圖中歷史時(shí)刻變量節(jié)點(diǎn)，實(shí)現(xiàn)對(duì)于滯后量測(cè)免補(bǔ)償使用。仿真結(jié)果表明，所提方法在信息滯后小于1 s時(shí)可獲得與無(wú)滯后情況下大致相當(dāng)?shù)木龋灰詴r(shí)長(zhǎng)15 s的滑窗為例，所提算法較傳統(tǒng)滑動(dòng)窗優(yōu)化單步運(yùn)行時(shí)間減少約91%，具有可實(shí)時(shí)實(shí)現(xiàn)的優(yōu)點(diǎn)。半物理實(shí)驗(yàn)結(jié)果亦驗(yàn)證了所提方案的可靠性。

水面無(wú)人艇（USV）；協(xié)同導(dǎo)航；滑動(dòng)窗優(yōu)化；增量平滑；滯后信息處理

0 引言

近年來(lái)，隨著海洋和內(nèi)河探索的發(fā)展，無(wú)人艇（unmanned surface vessel，USV）以其機(jī)動(dòng)靈活、較高隱蔽性、可長(zhǎng)時(shí)間作業(yè)等特性成為重要工具。同時(shí)諸如圍補(bǔ)、打撈、驅(qū)離等作業(yè)需要多艘無(wú)人艇共同配合完成[1]，與單艘艇作業(yè)相比，無(wú)人艇集群可通過(guò)協(xié)作，高效地完成復(fù)雜任務(wù)，具有極大的實(shí)際應(yīng)用價(jià)值。

實(shí)時(shí)導(dǎo)航定位是保證無(wú)人艇集群高效執(zhí)行各項(xiàng)任務(wù)的必要前提。對(duì)于單艇，傳統(tǒng)導(dǎo)航方式以捷聯(lián)慣導(dǎo)系統(tǒng)（strapdown inertial navigation system, SINS）為核心，利用多普勒測(cè)速儀（Doppler velocity log, DVL）、全球衛(wèi)星導(dǎo)航系統(tǒng)（global navigation satellite system, GNSS）等輔助傳感器進(jìn)行修正[2]；除此之外，無(wú)人艇集群可利用艇間觀測(cè)信息提升定位精度。文獻(xiàn)[3]針對(duì)主從式USV協(xié)同導(dǎo)航定位，建立協(xié)同定位狀態(tài)空間模型，利用擴(kuò)展卡爾曼濾波進(jìn)行信息融合。文獻(xiàn)[4]針對(duì)協(xié)同導(dǎo)航設(shè)計(jì)一種分散式卡爾曼濾波算法，該算法在與傳統(tǒng)濾波同等精度條件下所需通信次數(shù)減少。文獻(xiàn)[5]提出一種基于因子圖算法的多水下無(wú)人艇（autonomous underwater vehicle, AUV）協(xié)同導(dǎo)航方法，集中處理集群信息，定位精度優(yōu)于傳統(tǒng)濾波算法。文獻(xiàn)[6]結(jié)合無(wú)人機(jī)自身導(dǎo)航信息和無(wú)人機(jī)間測(cè)距信息，提出一種基于因子圖優(yōu)化的分布式協(xié)同導(dǎo)航算法。但傳統(tǒng)因子圖優(yōu)化算法計(jì)算量大，難以實(shí)時(shí)融合信息，多用于事后處理[7]。

現(xiàn)有協(xié)同導(dǎo)航方案往往忽略因通信時(shí)延導(dǎo)致的量測(cè)信息滯后問(wèn)題。針對(duì)滯后信息，基于濾波處理方法主要為外推和擴(kuò)增，用滯后數(shù)據(jù)預(yù)測(cè)當(dāng)前時(shí)刻量測(cè)，構(gòu)建等效量測(cè)信息，此類方法會(huì)引入推估誤差[8]；而借助因子圖優(yōu)化可追溯歷史時(shí)刻變量節(jié)點(diǎn)的特點(diǎn)，可免補(bǔ)償使用滯后信息[9]。文獻(xiàn)[8]將此特性應(yīng)用于GNSS/INS 組合導(dǎo)航系統(tǒng)中，以降低GNSS信息滯后導(dǎo)致的定位誤差。文獻(xiàn)[10]利用此特點(diǎn)處理AUV信標(biāo)信息延遲，與曲線擬合補(bǔ)償方法進(jìn)行了比較，發(fā)現(xiàn)基于因子圖的滯后信息補(bǔ)償能更為有效地補(bǔ)償順序、出序等多種滯后信息。無(wú)論文獻(xiàn)[8]還是文獻(xiàn)[10]均未對(duì)算法的實(shí)時(shí)性進(jìn)行詳細(xì)討論。

針對(duì)無(wú)人艇實(shí)時(shí)協(xié)同導(dǎo)航需求，本文首先介紹了一種基于因子圖優(yōu)化的主從式協(xié)同導(dǎo)航方案，以SINS為核心，將艇間的距離信息作為協(xié)同導(dǎo)航因子節(jié)點(diǎn)構(gòu)建因子圖模型；其次結(jié)合增量平滑和滑動(dòng)窗優(yōu)化的優(yōu)點(diǎn)，提出一種改進(jìn)型滑動(dòng)窗優(yōu)化算法，其具備實(shí)時(shí)求解模型的能力；然后針對(duì)協(xié)同信息存在的滯后問(wèn)題引入滯后信息處理方案。

1 無(wú)人艇協(xié)同導(dǎo)航方案

在無(wú)人艇集群執(zhí)行復(fù)雜任務(wù)過(guò)程中，為了保證任務(wù)的有效實(shí)施，需要集群具有較高的定位精度。單艇利用高精度慣導(dǎo)并借助DVL等進(jìn)行輔助，可獲得較準(zhǔn)確的導(dǎo)航信息[6]；但為集群中所有無(wú)人艇裝配高精度導(dǎo)航設(shè)備成本較高。本方案中，為了保證集群可獲得較高精度的導(dǎo)航信息，部分無(wú)人艇配備高精度慣性測(cè)量單元（inertial measurement unit,IMU）、DVL和測(cè)距儀作為領(lǐng)航艇，其余無(wú)人艇僅配備精度相對(duì)較低的IMU和DVL等導(dǎo)航設(shè)備作為從艇。領(lǐng)航艇以預(yù)定時(shí)間間隔廣播定位信息、位置協(xié)方差和與附近艇的相對(duì)距離，從艇接受帶有時(shí)間戳的報(bào)文，通過(guò)對(duì)于位置協(xié)方差和時(shí)間戳判斷協(xié)同信息的可靠性和實(shí)時(shí)性，建立因子圖模型，利用優(yōu)化算法實(shí)現(xiàn)對(duì)于相對(duì)定位信息的利用。雙領(lǐng)航者協(xié)同方案如圖1所示。

圖1 無(wú)人艇群協(xié)同導(dǎo)航

2 基于因子圖的信息融合方法

2.1 系統(tǒng)模型與算法原理

針對(duì)因子圖模型，依據(jù)最大后驗(yàn)概率密度估計(jì)進(jìn)行求負(fù)對(duì)數(shù)、馬氏范數(shù)轉(zhuǎn)換二范數(shù)等過(guò)程，將信息融合問(wèn)題轉(zhuǎn)換為非線性最小二乘問(wèn)題，進(jìn)行迭代求解[11]，獲得最優(yōu)估計(jì)，具體可表示為

2.2 因子節(jié)點(diǎn)模型描述

2.2.1 IMU因子節(jié)點(diǎn)

2.2.2 DVL因子節(jié)點(diǎn)

2.2.3 協(xié)同測(cè)距因子節(jié)點(diǎn)

協(xié)同測(cè)距信息對(duì)應(yīng)因子節(jié)點(diǎn)可表示為

式中表示零矩陣。

2.3 主從協(xié)同導(dǎo)航的因子圖描述及推理

圖2 從艇因子圖模型

2.3.1 滑動(dòng)窗口優(yōu)化

傳統(tǒng)的因子圖推理方法一次推理須對(duì)所有變量節(jié)點(diǎn)進(jìn)行求解，隨著時(shí)間增長(zhǎng)，計(jì)算量不斷增加，無(wú)法滿足導(dǎo)航系統(tǒng)實(shí)時(shí)性需求[11]。在導(dǎo)航系統(tǒng)中最新觀測(cè)值對(duì)于早期變量估計(jì)準(zhǔn)確性提高不顯著，早期估計(jì)對(duì)于當(dāng)前狀態(tài)估計(jì)準(zhǔn)確性提高亦無(wú)明顯幫助；因此可利用滑動(dòng)窗口限定參與優(yōu)化的節(jié)點(diǎn)數(shù)[12]，減少優(yōu)化算法的計(jì)算量。通過(guò)對(duì)于窗口內(nèi)節(jié)點(diǎn)進(jìn)行批量?jī)?yōu)化可有效利用部分過(guò)去時(shí)刻的信息對(duì)于當(dāng)前時(shí)刻進(jìn)行估計(jì)，滑動(dòng)窗口優(yōu)化（sliding windows optimization，SWO）為優(yōu)化算法實(shí)時(shí)處理信息提供了可能。算法流程如圖3所示。

圖3 滑動(dòng)窗口優(yōu)化

2.3.2 改進(jìn)型滑動(dòng)窗口優(yōu)化

圖4 基于增量平滑的改進(jìn)滑動(dòng)窗口優(yōu)化

2.4 信息滯后處理方案

基于因子圖優(yōu)化的算法可在一次信息融合時(shí)刻完成對(duì)于歷史導(dǎo)航狀態(tài)量和當(dāng)前導(dǎo)航狀態(tài)量的更新，使得因子圖算法在處理信息滯后具有得天獨(dú)厚的優(yōu)勢(shì)。具體處理流程如圖5所示。

圖5 協(xié)同量測(cè)信息滯后處理流程

3 實(shí)驗(yàn)與結(jié)果分析

為了驗(yàn)證本文提出的基于因子圖優(yōu)化的無(wú)人艇主從式協(xié)同導(dǎo)航方法的性能，分別進(jìn)行仿真驗(yàn)證和半實(shí)物實(shí)驗(yàn)驗(yàn)證。實(shí)驗(yàn)均在內(nèi)存為16 GB、主頻為2.6 GHz的Intel Core i7-10750H筆記本電腦中進(jìn)行。

3.1 仿真實(shí)驗(yàn)

3.1.1 仿真條件設(shè)置

數(shù)值仿真包含2艘領(lǐng)航艇（領(lǐng)航艇1和領(lǐng)航艇2）和1艘從艇的協(xié)同導(dǎo)航，領(lǐng)航艇利用高精度的IMU和DVL進(jìn)行組合導(dǎo)航，從艇利用IMU、DVL和協(xié)同信息進(jìn)行導(dǎo)航。3艘艇的運(yùn)動(dòng)軌跡如圖6所示。

圖6 無(wú)人艇運(yùn)動(dòng)軌跡

3.1.2 結(jié)果分析

從艇的仿真結(jié)果如圖7所示。圖中EKF表示EKF的融合結(jié)果，UKF表示UKF的融合結(jié)果，SWO-15 s表示窗口長(zhǎng)為15 s的滑動(dòng)窗優(yōu)化算法的融合結(jié)果，ISWO-15 s表示窗口長(zhǎng)為15 s的改進(jìn)的滑動(dòng)窗口優(yōu)化算法的融合結(jié)果，4種算法的融合結(jié)果分別用點(diǎn)線、劃線、淺色直線和深色直線表示。在整個(gè)仿真時(shí)間段內(nèi)，幾種方法都能保持相對(duì)較高的的導(dǎo)航精度，北向和東向定位誤差大部分在 -5.0～+5.0 m以內(nèi)。

圖7 4種方法的位置誤差對(duì)比曲線

由圖可知：在勻速直線運(yùn)動(dòng)階段，因子圖優(yōu)化算法引入之前時(shí)刻信息用于校正當(dāng)前時(shí)刻估計(jì)，精度略高且估計(jì)的曲線更平滑；當(dāng)改變運(yùn)動(dòng)狀態(tài)，如在0～200 s范圍內(nèi)從艇沿正北方向做加速運(yùn)動(dòng)以及由直線運(yùn)動(dòng)進(jìn)入轉(zhuǎn)向時(shí)，優(yōu)化算法因較多利用過(guò)去時(shí)刻信息，導(dǎo)致穩(wěn)定性不如基于卡爾曼濾波的方法。由圖7可知0～200 s內(nèi)，優(yōu)化算法的北向定位誤差明顯大于濾波算法，因此致使整個(gè)階段軌跡的北向定位均方根誤差（root mean square error，RMSE）大于濾波方法。從艇定位的均方根誤差統(tǒng)計(jì)如表1所示。

表1 從艇定位結(jié)果統(tǒng)計(jì)

比較2種基于濾波的方法可以看出：大部分時(shí)間UKF算法的定位精度高于EKF算法，可見(jiàn)UKF算法對(duì)于系統(tǒng)非線性的適應(yīng)性更強(qiáng)；但當(dāng)系統(tǒng)狀態(tài)出現(xiàn)較大變化時(shí)，UKF算法中的西格瑪（Sigma）采樣點(diǎn)取值未必能夠精確反映系統(tǒng)的特性，導(dǎo)致濾波精度反而低于EKF算法。比較2種基于因子圖優(yōu)化的算法，可發(fā)現(xiàn)改進(jìn)型滑動(dòng)窗優(yōu)化算法精度略高，因其僅針對(duì)受影響的部分變量更新，相較于傳統(tǒng)滑動(dòng)窗優(yōu)化減少了因?qū)τ诨皟?nèi)變量節(jié)點(diǎn)無(wú)差別更新引入的數(shù)值計(jì)算誤差。

如圖8所示，改進(jìn)型算法運(yùn)行時(shí)間遠(yuǎn)短于傳統(tǒng)滑動(dòng)窗優(yōu)化算法，可發(fā)現(xiàn)單步融合時(shí)間約減少91%，為實(shí)時(shí)處理滯后信息創(chuàng)造了條件。

圖8 因子圖優(yōu)化方法運(yùn)行時(shí)間

為了分析因子圖優(yōu)化在處理滯后協(xié)同信息時(shí)的效果，以滑動(dòng)窗長(zhǎng)為15 s的改進(jìn)型滑動(dòng)窗口優(yōu)化為融合工具，針對(duì)不同滯后時(shí)長(zhǎng)，以領(lǐng)航艇信息無(wú)滯后的情況作為對(duì)照組，設(shè)計(jì)6組實(shí)驗(yàn)，如表2所示。

表2 實(shí)驗(yàn)設(shè)置

各組實(shí)驗(yàn)中從艇的仿真結(jié)果如圖9所示。圖中對(duì)照組為2艘領(lǐng)航艇協(xié)同信息均無(wú)滯后時(shí)的融合結(jié)果，因不補(bǔ)償?shù)膶?shí)驗(yàn)組中誤差發(fā)散明顯，僅繪制了使用歷史節(jié)點(diǎn)匹配滯后處理方案的實(shí)驗(yàn)對(duì)應(yīng)的誤差曲線。

圖9 不同實(shí)驗(yàn)條件下的位置誤差對(duì)比曲線

以無(wú)滯后信息時(shí)的結(jié)果作為對(duì)照，可發(fā)現(xiàn)從艇通過(guò)時(shí)間戳與歷史節(jié)點(diǎn)進(jìn)行匹配后，獲得與無(wú)滯后信息情況下大致相當(dāng)?shù)亩ㄎ痪龋礁`差如表3所示。滯后時(shí)長(zhǎng)越長(zhǎng)，對(duì)于艇定位精度影響越大；當(dāng)2艘領(lǐng)航艇滯后信息時(shí)長(zhǎng)不同時(shí)，若不處理則誤差發(fā)散較為明顯，而通過(guò)因子圖優(yōu)化可獲得較高定位精度。

表3 各組實(shí)驗(yàn)定位均方根誤差值 m

3.2 半物理實(shí)驗(yàn)

為進(jìn)一步驗(yàn)證本文算法的有效性，在蘇州元蕩進(jìn)行實(shí)驗(yàn)，獲取3艘水面艇航行數(shù)據(jù)，并根據(jù)數(shù)據(jù)進(jìn)行半物理仿真實(shí)驗(yàn)。

3.2.1 仿真條件設(shè)置

從艇完成慣導(dǎo)初始對(duì)準(zhǔn)后，3艘同時(shí)機(jī)動(dòng)，選取3艘艇運(yùn)動(dòng)階段數(shù)據(jù)，時(shí)長(zhǎng)均為470 s。實(shí)驗(yàn)過(guò)程中GNSS信號(hào)良好。艇間測(cè)距參考值由對(duì)應(yīng)時(shí)刻位置參考值做差獲得，添加量測(cè)噪聲為0.2%測(cè)距值的高斯白噪聲模擬協(xié)同信息，領(lǐng)航艇1和領(lǐng)航艇2協(xié)同信息廣播頻率分別為1和2 Hz。以窗長(zhǎng)為15 s的改進(jìn)滑動(dòng)窗口優(yōu)化為融合工具，依據(jù)表2設(shè)置進(jìn)行實(shí)驗(yàn)，并以協(xié)同信息無(wú)滯后時(shí)的融合結(jié)果作為對(duì)照組。

圖10 半物理實(shí)驗(yàn)所用艇外形和各艇軌跡

3.2.2 結(jié)果分析

按照各組實(shí)驗(yàn)設(shè)置進(jìn)行實(shí)驗(yàn)獲得的位置誤差如表4所示。

USV協(xié)同導(dǎo)航實(shí)驗(yàn)誤差曲線如圖11所示，定位誤差保持在較小范圍內(nèi)。根據(jù)表4的統(tǒng)計(jì)結(jié)果，亦可得到3.1節(jié)的結(jié)論。

表4 各組實(shí)驗(yàn)定位均方根誤差值 m

圖11 不同實(shí)驗(yàn)條件下的位置誤差對(duì)比曲線

4 結(jié)束語(yǔ)

本文提出了基于因子圖優(yōu)化的無(wú)人艇主從協(xié)同算法，實(shí)現(xiàn)了對(duì)于艇群中相對(duì)測(cè)距信息融合。該方法將導(dǎo)航狀態(tài)量作為變量節(jié)點(diǎn)，從艇將接收到的距離量測(cè)信息抽象為協(xié)同導(dǎo)航節(jié)點(diǎn)，依據(jù)非線性優(yōu)化理論實(shí)現(xiàn)狀態(tài)變量的更新。為提升算法實(shí)時(shí)性，利用滑動(dòng)窗口限制參與優(yōu)化的節(jié)點(diǎn)數(shù)量，利用增量平滑對(duì)滑窗內(nèi)的節(jié)點(diǎn)進(jìn)行增量更新。針對(duì)協(xié)同導(dǎo)航中因通信而導(dǎo)致的滯后情況，在因子圖框架下可免補(bǔ)償?shù)厥褂脺罅繙y(cè)信息。仿真實(shí)驗(yàn)結(jié)果表明，所提算法具有可有效融合艇間信息，且可實(shí)時(shí)實(shí)現(xiàn)的特點(diǎn)，當(dāng)滯后時(shí)長(zhǎng)小于1 s時(shí)，可獲得與無(wú)滯后大致相當(dāng)?shù)膶?dǎo)航精度。半物理實(shí)驗(yàn)結(jié)果表明，本文方案亦具有較高的實(shí)際應(yīng)用價(jià)值。本文僅對(duì)無(wú)人艇間距離信息進(jìn)行融合，未來(lái)可針對(duì)諸如艇間方位角等其他信息的融合開(kāi)展進(jìn)一步研究。

[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] 徐博, 肖永平, 高偉, 等. 主從式無(wú)人水面艇協(xié)同定位濾波方法與實(shí)驗(yàn)驗(yàn)證[J]. 兵工學(xué)報(bào), 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] 陳明星, 熊智, 劉建業(yè), 等. 基于因子圖的無(wú)人機(jī)集群分布式協(xié)同導(dǎo)航方法[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 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] 高軍強(qiáng), 湯霞清, 張環(huán), 等. 基于因子圖算法的INS/GPS信息滯后處理方法[J]. 計(jì)算機(jī)應(yīng)用, 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]. 中國(guó)慣性技術(shù)學(xué)報(bào), 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

張玉鵬, 王子璇, 劉劍威, 等. 因子圖框架下無(wú)人艇主從式協(xié)同導(dǎo)航算法[J]. 導(dǎo)航定位學(xué)報(bào), 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—），男，江蘇省徐州人，碩士研究生，研究方向?yàn)閰f(xié)同導(dǎo)航。

劉錫祥（1976—），男，江蘇省海安人，博士，教授，研究方向?yàn)閼T性導(dǎo)航、組合導(dǎo)航與信息融合技術(shù)。