因子圖框架下無人艇主從式協同導航算法

張玉鵬，王子璇，劉劍威,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—），男，江蘇省海安人，博士，教授，研究方向為慣性導航、組合導航與信息融合技術。