基于圖優化的通信受限環境下協同導航方法

牛皓飛，蔡慶中，李健，楊功流

北京航空航天大學 儀器科學與光電工程學院，北京 100191

智能無人集群系統由無人機、無人車、機器人等多智能體通過彼此之間的信息交互構成，可以智能協同控制［1-2］，完成單個無人系統無法實現的復雜工作［3］，在軍事和民用領域具有廣闊應用前景。無人集群系統實現復雜的自主協同控制首要解決的一個問題便是節點間的時空統一問題，即系統中多智能體節點的相對與絕對定位與導航問題。

協同導航技術是一項在多智能體系統下提高個體定位與導航精度的關鍵技術［4-5］。在多智能體系統中，通過系統中各個子節點之間的導航狀態共享與相互觀測的方式來為子節點提供更豐富的觀測信息，各個節點相互協同，可實現在低成本條件下的導航定位精度提升［6-7］，現有協同導航方法多采用Kalman 濾波實現［8-13］。

然而，在復雜遮擋環境下，節點與基站之間的通信會受到影響，無法保證觀測信號的全覆蓋。這需要系統具備處理觀測信息時有時無、動態變化的能力，同時利用相鄰協同節點的相對觀測來對導航狀態進行約束，實現復雜遮擋環境下的多無人平臺協同導航。傳統基于Kalman 濾波的協同導航方法觀測方程狀態維數固定，要求觀測信息同時到達，無法解決遮擋環境下觀測信號不同步、不連續的問題。而圖優化算法將觀測與狀態約束編碼為圖模型中可增刪的因子和頂點，具有很強的靈活性，目前已經在即插即用的多源組合導航系統中體現了優勢［14-15］。因此，采用基于因子圖模型的圖優化算法在處理通信受限、觀測信息動態變化的情況具有理論優勢。

現有對圖優化算法的導航應用研究中，主要集中在單個載體的多傳感器融合領域［16-17］。目前對基于圖優化算法的多智能體協同導航的研究仍然較少，文獻［18］于2020 年提出了首個基于圖優化算法的多無人機分布式協同導航方案，利用UWB 模塊獲得相對距離觀測，與視覺慣性里程計的估計結果進行融合。文獻［19］在文獻［18］的基礎上，通過在環境中放置1 個靜態錨點，來糾正累計誤差。其在導航狀態估計過程中僅融合了視覺里程計與慣性信息，沒有利用到UWB 觀測信息對慣性器件進行校準，本質是一種松耦合，無法在通信受限情況保持精度。文獻［20］于2022 年提出了基于激光雷達的分布式多無人機協同導航方案，并提供了基于圖優化方法的精確估計結果，解決了文獻［18］等基于視覺類方案受到視場環境局限的問題［21-23］，且不依賴于錨點。該方法為每個節點配備了高精度激光雷達導致了較高的成本，且激光雷達里程計和慣性導航都存在誤差累積的問題，累計誤差需要依靠回環檢測來消除。綜上，在視覺組合導航的相關應用中，圖優化方法已取得較好效果。本文將重點關注其應用于慣性/里程計節點的協同導航，驗證其在通信受限環境的全局優化效果。

基于以上研究成果，本文提出基于圖優化算法的分布式協同導航方法，采用成本較低的UWB 測距信息提供相對導航約束，實現復雜環境下的高精度、高可靠協同導航。主要貢獻如下：①面向室內、叢林等復雜遮擋環境下無線電信號不穩定環境，采用圖優化算法解決傳統濾波方法無法適應觀測信息不同步不連續的問題；② 針對無線電信號拒止環境下節點通信短時中斷問題，通過建立慣性航位推算因子圖約束模型，在圖優化算法中對陀螺零偏等內參進行同步優化與實時補償，提高個體短時間自主導航的精度保持能力；③針對復雜環境下，協同導航節點拓撲結構動態變化、錨點觀測信號不足的問題，提出基于圖優化的分布式協同算法，通過節點間的相對距離觀測提高集群綜合導航精度。

1 問題建模

在叢林、室內等復雜通信遮擋環境下，無線電信號會受到遮擋等因素導致通信中斷。在此條件下智能體與錨點之間的非視距量測可由與附近協同節點之間的相對量測代替。一個典型的室內通信遮擋環境如圖1 所示，無人集群系統主要由智能體節點以及室外數量有限的定位錨點構成，節點1 處于室外通信較好環境下，可接收到2 個錨點的信息；節點2 位置較差僅能接收到1 個錨點；節點3 環境最惡劣，接收不到任何錨點信息。

圖1 系統結構圖Fig.1 Model of system structure

通過節點間的協同與相互觀測，可以有效為處于通信拒止環境下的節點提供豐富的觀測信息，從而提高節點導航精度。由于在實際情況中，對高度的測量一般由特定的傳感器獲得，為簡化運動學模型，不考慮高度項。根據不同傳感器的原理，分別建立對應于圖優化模型的約束形式，具體推導如下。

1.1 航位推算約束

對系統中單個智能體進行分析，由于智能體平臺通常配有速度傳感器，如無人車配備的里程計。因此利用航位推算在短時間內保持定位精度，導航定位的主要誤差來源于陀螺漂移，基于此建立航位推算模型，如圖2 所示。設陀螺零偏不隨時間變化，智能體在k?1 時刻與k時刻之間的狀態遞推方程為

圖2 航位推算因子圖模型Fig.2 Dead reckoning factor graph

式中：ts為采樣間隔；θk為智能體k時刻的航向角；θk?1為k?1 時刻的航向角；ωk?1為k?1 時刻天向陀螺儀輸出的角速率；xk為智能體k時刻的x軸位置坐標；xk?1為k?1 時刻的x軸位置坐標；vk?1為里程計k?1 時刻提供的速率；yk為k時刻的y軸位置坐標；yk?1為k?1 時刻的y軸位置坐標；bk為k時刻的天向陀螺零偏。

根據2 時刻之間的狀態增量構建殘差約束，設待估計狀態量為。以*代表該變量參與優化，其余作為已知量測值輸入，不參與優化。根據約束的不同可以分為時間約束與狀態約束，時間約束基于2 時刻之間狀態增量建立；狀態約束基于同一時刻下不同狀態量之間滿足的約束關系建立。

首先建立k?1～k時刻的時間約束，表示為

對殘差進行求偏導，對應雅可比矩陣為

k時刻的狀態約束可表示為

其中，天向陀螺零偏bk影響航向角的誤差，以當前時刻航向角估計值與量測值建立殘差：

根據式（1），展開θk：

設零偏為常值

則式（5）化簡為

航向狀態約束：

對應雅可比矩陣為

1.2 UWB 相對測距約束

基于UWB 提供的距離觀測信息，建立UWB相對測距約束，約束形式可表示為

式中：dk為k時刻載體通過UWB 相對測距獲得的與協同節點（或錨點）之間的距離觀測值。坐標（xc，yc)為協同節點（或錨點）坐標值。對殘差進行求偏導，對應雅可比矩陣為

基于上述建立的圖模型中因子節點具體殘差形式與雅可比矩陣，對因子圖模型的架構進行定義。對于系統中任一協同節點，狀態變量包括節點位置坐標、航向角、天向陀螺儀零偏。設為：Xk=[xk，yk，θk，bk]，對應因子圖中的頂點。觀測約束連接狀態變量，對應因子圖中的因子。根據不同傳感器的模型建立不同的約束方程，對應圖中與頂點相連的因子節點。建立系統中智能體導航定位的因子圖模型如圖3 所示。

圖3 智能體的因子圖模型Fig.3 Factor graph model of agent

本節對系統中的傳感器觀測建立了因子圖模型，為后續的非線性優化提供了框架。利用不同傳感器的特性建立不同的約束。其中航位推算模型分別建立了時間約束和狀態約束，利用IMU 和里程計無突變、隨時間發散緩慢的特點，建立時間約束對UWB 的隨機噪聲起到了平滑作用。同時，利用狀態變量之間滿足的約束關系對節點航向與陀螺零偏進行了估計。所估計出的零偏將在后續算法中進行實時補償，抑制導航誤差的發散，使節點在無法接收到信號的環境中也能夠保持一定的導航精度，該精度取決于零偏估計結果的精度。

2 算法設計

2.1 圖優化算法設計

基于1.2 節建立的系統因子圖模型，采用非線性優化算法對狀態進行估計。系統圖模型如圖4 所示。

圖4 系統因子圖模型Fig.4 Factor graph model of system

算法流程步驟如下：

步驟1設定起始時刻各個狀態變量的初值，添加先驗因子到圖模型中。

步驟2基于上一時刻優化結果進行航位推算，得到的下一時刻預測值作為優化初始值，添加到圖的頂點。將歷史時刻的優化結果，設為歷史時刻狀態的初值，添加到圖的頂點。

步驟3添加UWB 測距因子與航位推算因子。將協同節點發送的相對觀測信息作為UWB測距因子加入圖模型。

步驟4根據所推導的目標函數與雅可比矩陣，采用LM 算法對圖模型進行非線性優化，重復步驟2～步驟4。

設系統優化變量為所有時刻的狀態變量，表示為

優化的目標函數是圖模型所有殘差平方和，設航位推算因子的2 種約束殘差分別為

每個UWB 因子對應一組距離觀測，將k個時刻的所有觀測殘差的平方進行累加得到總UWB因子殘差，設k時刻載體共接收到m組UWB 觀測，表示為

則系統優化目標函數為

圖優化算法將各個觀測量編碼為因子圖模型中可任意增刪的因子，對應的數學表達式為式（16）中可隨時加減的殘差項。殘差平方和的形式具有即插即用、可動態增刪的特點，且圖優化是包含歷史時刻的全局優化，因此即使智能體節點在惡劣環境下僅能接收到一個錨點的觀測信息，也能夠與歷史時刻的觀測因子對狀態構成約束。因此本文基于圖優化的方法具備解決動態變化、復雜環境下錨點觀測不足的問題。將所推導出來的各殘差項的雅可比矩陣代入增量方程，推導出的雅可比矩陣用于指導圖優化的方向，由增量方程求出單次迭代的狀態最優估計值為

根據本文的系統與模型，構建信息矩陣的形式為

本文貢獻主要來源于對3 種約束因子的設計與數學推導，在算法實現過程中采用Levenberg-Marquardt 這一通用優化算法對目標函數進行迭代求解。

2.2 協同導航算法設計

本節對本文協同導航架構與協同導航算法進一步闡述。在本文的多智能體協同導航系統中，UWB 基站作為固定錨點信息源，具有最高精度的坐標位置信息供其他智能體節點參考。但由于本身位置固定不動導致其通信范圍有限，在協同定位系統中基站僅對智能體節點提供觀測信息，不更新基站自身的位置坐標。因此智能體節點接收到基站的觀測在圖模型中是一種一元邊。對此構建的信息傳播因子圖模型如圖5 所示。圖中，為t?1 時刻下的智能體節點1 的狀態變量，為t時刻下的智能體節點1 的狀態變量；為t時刻下的智能體節點1 與上一時刻構成的時間約束因子；為t時刻下的智能體節點1 與當前時刻狀態量構成的狀態約束因子；為t時刻下的智能體節點1 接收到的來自一個UWB 錨點的相對測距因子。

圖5 無人車與基站協同導航模型Fig.5 Model for cooperative navigation of unmanned vehicle and base station

在協同終端為室外錨點的情況下，相對測距因子的數學形式為

式中：(xanchor，yanchor)為所接收到的UWB 基站在局部坐標系下的坐標。坐標系以左側UWB 基站為原點，左側基站指向右側基站方向作為坐標系x軸，右側基站的位置坐標由兩基站之間所獲得的相對距離觀測信息取平均值得到。

在分布式協同導航系統中，智能體節點之間可以互相作為協同節點。以系統中任意2 個相互通信的智能體節點為例，將狀態按照時間軸展開，構造協同導航算法的因子圖模型如圖6 所示。如圖所示，為t?1 時刻下的智能體節點2 的狀態變量，為t時刻下的智能體節點2 的狀態變量；為智能體節點2 發送給智能體節點1 的自身位置信息與相對測距信息；為智能體節點1 發送給智能體節點2 的自身位置信息與相對測距信息；在t時刻智能體節點1、2 依靠航位推算分別進行自定位，將協同終端發送的位置信息與相對測距信息作為量測，將觀測編碼為因子加入自身因子圖模型中，每個節點分布式地執行局部圖優化算法。隨后將優化結果進行廣播發送給協同終端，協同終端利用此相對觀測信息作為相對測距因子再進行優化。經過多輪迭代后，終端節點的位置精度會逐步提高。

圖6 無人車之間協同導航模型Fig.6 Model for cooperative navigation between un‐manned vehicles

在協同終端為其他智能體節點的情況下，相對測距因子的數學形式為

式中：(xcoop，ycoop)來自協同節點在當前時刻下通過自身觀測信息進行位置校準后的定位結果。下面將分別設計典型場景對算法進行驗證。

3 仿真與實驗分析

為檢驗基于圖優化算法的協同導航系統定位性能，設計典型場景進行仿真與實驗分析研究。驗證算法在錨節點有限、觀測異步、動態增刪復雜條件下的協同導航定位精度；驗證算法處理異步、動態增刪信息的能力；驗證分布式協同導航算法對導航定位精度的提升。

3.1 仿真驗證

如圖7 所示，節點1 從室外進入到室內，室外可同時接收到2 個錨點的觀測信息，室內由于墻壁遮擋，UWB 信號時有時無，節點1 能間歇接收到2 個錨點其中的一個觀測。以3 輛無人車節點為例，節點1 靠近墻壁行進，部分UWB 觀測信號可透過窗戶到達無人車接收機，故節點1 接收信號條件較好，可作為高精度節點。節點2 距離錨點較遠，但軌跡靠近門，可接收到一個錨節點的觀測信息。節點3 的信號接收情況最差，既不靠近門也不靠近錨點，因此處于無錨點環境下。依靠節點之間的協同，節點2 可收到與節點1 之間的相對測距信息，節點3 可收到節點1 和節點2 的相對測距信息。期間設所接收到UWB 相對距離觀測誤差為0.1 m 白噪聲。陀螺儀零偏設為200（°）/h，角速率隨機游走；航位推算解算周期0.1 s，UWB 采樣間隔1 s。仿真時間300 s，仿真結果如圖8 所示。

圖7 仿真場景Fig.7 Simulation scenarios

圖8 仿真估計結果Fig.8 Simulation results

如圖8 所示，節點1 靠近錨點一側，保持水平直線運動。在0～50 s 期間可同時收到2 個錨點的觀測信息。50～100 s 無法接收到錨點觀測，100～300 s 通過窗戶可間歇性交替接收到2 個錨點中的其中一個觀測信息，處于觀測信息不同步的環境下。

如圖8（a）所示，錨點坐標分別位于坐標（0，0）m 與（400，0）m 處，3 個節點導航定位誤差均得到了有效校準。節點1 在觀測信息時有時無、不同步的情況下仍能保持較高導航定位精度，節點2 在僅接收到一個錨點觀測情況下，利用節點1 相對觀測信息能夠有效抑制誤差發散。節點3 在沒有接收到錨節點觀測的情況下，通過與節點1、2 的協同，最終導航定位精度高于航位推算精度，有效抑制了導航誤差發散。節點3 在全程沒有接收到錨節點觀測的條件下，依靠與節點1 和2 的協同，同樣實現了位置誤差校正。

如圖8（b）所示，從上到下每一行分別是每個節點的x、y軸定位誤差曲線，節點1 在觀測信息時有時無且不同步的情況下，仍能保持定位誤差<0.2 m。在100 s 時刻后，節點1 進入了室內，節點2 與節點3 接收到了來自節點1 的相對觀測信息，3 節點間的協同對各個節點的狀態產生了約束作用，因此3 個節點在該時刻的狀態量均有較小程度的跳變，其中節點2 的x軸誤差有效降低，y軸誤差受相對觀測影響有小幅度增大，但總定位誤差仍然降低。節點2 與節點3 由于不能收到2 個錨點的觀測，定位精度低于節點1，但通過與移動節點的協同，保持定位誤差不發散，相比于單純航位推算，導航定位精度獲得了較大的提升。

如圖8（c）所示，從上到下每一行分別是每個節點的航向角、陀螺儀零偏估計曲線。3 個節點的航向角均收斂至了真值附近。其中節點1 依靠2 個錨點有效估計出了陀螺儀零偏，估計結果1.040 0×10?3rad/s，真值為9.691 4×10?4rad/s，誤差<10%。節點1 在50～100 s 的無錨點期間，通過零偏的補償，航位推算沒有產生明顯的發散，導航精度得到了有效提高。節點2 和節點3由于無法獲得2 個錨點的觀測信息，零偏估計誤差較大，但也能夠收斂至真值附近上下波動。

節點1 在觀測信息時有時無、不同步的情況下仍能保持較高導航定位精度，驗證了所提出算法的即插即用性，使系統能夠處理觀測信息時有時無、信息異步的問題，具備比卡爾曼濾波更強的靈活性。在通過節點之間的協同獲得了高于單個節點航位推算的導航定位精度，驗證了協同導航算法的有效性。

節點4 在400～600 s 期間處于通信最惡劣的無錨點觀測環境下，僅在0～200 s 期間通過室外接收錨點觀測估計出的零偏進行補償，抑制導航誤差的發散。在長時間惡劣環境下進行導航，誤差呈線性發散，發散程度取決于零偏估計的精度，由于處于室外時間較短，零偏估計與真值之間存在一定誤差，但綜合定位精度高于航位推算方法，證明了本文方法的有效性。

3.2 綜合模擬實驗

實物系統如圖9 所示，包括3 輛同配置無人車作為系統節點以及4 個UWB 基站作為系統錨點。在實驗場地上沿路徑設置共25 組地標點，在每個地標點上靜態放置定位節點60 s，采集放置期間產生的4 個錨點網絡定位數據，對得到的定位結果取平均值抑制隨機誤差的影響，將得到的基準點坐標作為定位基準。

圖9 系統節點單元Fig.9 A platform in system

無人車節點僅采集室外2 個錨點的觀測數據，室內錨點僅用于提供基準。實驗過程中，同時控制3 輛無人車按照各自的路線行進，與此同時每個節點分別記錄行進過程中所產生的所有傳感器數據。將采集的數據經同步處理后進行離線解算。

實驗場景如圖10 所示，東西兩側各有一扇可進出的大門，經實測在門口附近可收到門外的一個錨點觀測信息，行進至室內中間路段，觀測迅速減少至無法接收到任何錨點觀測。在兩門之間以及東側門右側所有區域均無法收到任何一個室外錨點的觀測，為完全無錨點觀測的拒止環境，節點僅在室外以及正對門的區域能夠收到觀測信息。

圖10 實驗場景分布示意圖Fig.10 Scene map of the experiment

3 個無人車節點按照先后順序依次從室外開入室內，節點1 最早進入室內，并沿著圖10 中藍色軌跡行駛，經過東側大門后繼續向東側無錨點區域行進，到達距東側門60 m 的東側區域后掉頭返回至西側大門，為3 個節點中行進距離最遠的節點。節點2 在節點1 到達室內最東側即將掉頭時開始行進，節點3 滯后節點2 約20 s 從門口附近開入室內。在行進過程中3 輛無人車按照各自的路線行進，彼此之間通過UWB 模塊保持通信。節點1 在掉頭后，節點2 從西側大門進入室內，此時節點1 能夠接收到來自節點2 的相對觀測。在節點2 行進至東側大門附近時，節點3 從西側大門進入室內，此時節點2 可接收到來自節點3 的相對觀測，節點1 可同時接收到來自節點2、3 的相對觀測。

對各個節點的觀測變化情況進行分析，全程節點1 由在室外收到2 個錨點觀測，到進入室內后觀測減少至0，再到掉頭后與節點2、3 的協同，觀測數從0 增加至2 個，處于觀測信息動態變化的情況。節點2 進入室內后所收到的觀測信息逐漸由室外的2 個錨點減小到1 個，在進入室內后向東側形式觀測數減少至0，在無錨點環境中收到了掉頭后來自節點1 的相對觀測，最后到達東側門口接收到室外的1 個錨點觀測以及來自節點1 的相對觀測，觀測數恢復至2 個。節點3 最晚進入室內，僅行駛至西側門口附近為節點1 和節點2提供相對觀測信息。

按照上述條件采集各個傳感器產生的原始數據，進行離線解算，全程時長300 s。系統所采用的器件參數如下：UWB 測距模塊采用NOOPLOOP 的LinkTrackP-B 模塊，測距隨機誤差10 cm，頻率50 Hz；無人車航位推算解算頻率50 Hz，IMU 型號為MPU9250，RMS 噪聲0.1（°）/s。基于采集的實驗數據進行離線解算的結果如圖11 所示。

圖11 實驗結果Fig.11 Experiment result

由實驗結果可見，本文提出的圖優化算法可以提高各協同節點的定位精度，尤其對于通信環境最惡劣、導航時間最長的節點1 最為明顯，如圖11（a）和圖11（b）所示。全程無人車基于航位推算方法的定位均方根誤差為3.96 m，采用圖優化方法不對零偏進行補償的情況下定位均方根誤差為2.31 m，采用EKF 方法進行協同導航的定位均方根誤差為1.67 m，采用圖優化方法進行協同導航的定位均方根誤差為1.52 m。采用本文提出的方法節點1 在長時間處于拒止環境下綜合定位精度相比航位推算方法提高了61%；節點1大部分時間均處于無錨點環境，觀測信號不連續情況占比較少，精度相比EKF 提高了9%，節點2受觀測不同步的影響較大，定位精度相比EKF 方法提高了17%。節點3 處于可接收到2 個錨點觀測區域內的時間較長且運動時間最短，與基于濾波的方法定位精度差異不明顯；與航位推算方法相比，節點2 定位均方根誤差由0.69 m 降低到0.32 m，節點3 由0.46 m 降低到0.27 m。

4 結論

本文針對復雜環境下通信受限的問題，提出了基于圖優化算法的分布式協同導航方法。通過數學仿真與綜合模擬實驗，驗證了所提出方法的有效性，結論如下：

1）針對復雜遮擋環境下錨點觀測不同步不連續的問題，建立系統因子圖模型，將觀測與狀態約束編碼為圖模型中可增刪的因子和頂點，使得系統具備處理異步、動態增刪觀測信息的能力。數學仿真結果表明，該方法在錨點觀測不連續不同步情況下，相比于連續同步觀測的導航定位精度無明顯變化。證明了該方法在復雜環境條件下的靈活性和適應性。

2）針對無線電信號拒止環境下節點通信短時中斷問題，利用所建立的包含慣性航位推算的因子圖約束圖優化模型實現陀螺零偏的實時估計，在無錨點環境內通過對零偏的補償有效降低了航位推算誤差的累積。仿真結果表明，所建立的模型可精確估計出陀螺零偏，估計誤差<10%。綜合實驗結果表明，該方法使得短時通信拒止環境下長期處于無錨點環境下的單節點定位精度相比航位推算方法精度提高了61%。

3）針對復雜環境下，協同導航節點拓撲結構動態變化、錨點觀測信號不足的問題，利用節點間相對測距信息，設計并實現了分布式協同導航與定位算法。通過節點間的相互觀測約束，有效抑制了復雜拒止環境下節點的導航誤差發散，使集群在復雜通信受限環境的綜合定位精度提升1 倍。

后續將進一步研究無錨點環境下的多無人平臺協同導航問題，解決無錨點條件下分布式協同導航存在誤差耦合、發散問題。