基于圖優(yōu)化的通信受限環(huán)境下協(xié)同導航方法

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

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

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

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

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

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

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

1 問題建模

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

圖1 系統(tǒng)結(jié)構(gòu)圖Fig.1 Model of system structure

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

1.1 航位推算約束

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

圖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時刻的天向陀螺零偏。

根據(jù)2 時刻之間的狀態(tài)增量構(gòu)建殘差約束，設(shè)待估計狀態(tài)量為。以*代表該變量參與優(yōu)化，其余作為已知量測值輸入，不參與優(yōu)化。根據(jù)約束的不同可以分為時間約束與狀態(tài)約束，時間約束基于2 時刻之間狀態(tài)增量建立；狀態(tài)約束基于同一時刻下不同狀態(tài)量之間滿足的約束關(guān)系建立。

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

對殘差進行求偏導，對應(yīng)雅可比矩陣為

k時刻的狀態(tài)約束可表示為

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

根據(jù)式（1），展開θk：

設(shè)零偏為常值

則式（5）化簡為

航向狀態(tài)約束：

對應(yīng)雅可比矩陣為

1.2 UWB 相對測距約束

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

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

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

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

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

2 算法設(shè)計

2.1 圖優(yōu)化算法設(shè)計

基于1.2 節(jié)建立的系統(tǒng)因子圖模型，采用非線性優(yōu)化算法對狀態(tài)進行估計。系統(tǒng)圖模型如圖4 所示。

圖4 系統(tǒng)因子圖模型Fig.4 Factor graph model of system

算法流程步驟如下：

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

步驟2基于上一時刻優(yōu)化結(jié)果進行航位推算，得到的下一時刻預測值作為優(yōu)化初始值，添加到圖的頂點。將歷史時刻的優(yōu)化結(jié)果，設(shè)為歷史時刻狀態(tài)的初值，添加到圖的頂點。

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

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

設(shè)系統(tǒng)優(yōu)化變量為所有時刻的狀態(tài)變量，表示為

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

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

則系統(tǒng)優(yōu)化目標函數(shù)為

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

根據(jù)本文的系統(tǒng)與模型，構(gòu)建信息矩陣的形式為

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

2.2 協(xié)同導航算法設(shè)計

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

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

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

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

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

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

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

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

3 仿真與實驗分析

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

3.1 仿真驗證

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

圖7 仿真場景Fig.7 Simulation scenarios

圖8 仿真估計結(jié)果Fig.8 Simulation results

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

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

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

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

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

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

3.2 綜合模擬實驗

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

圖9 系統(tǒng)節(jié)點單元Fig.9 A platform in system

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

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

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

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

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

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

圖11 實驗結(jié)果Fig.11 Experiment result

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

4 結(jié)論

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

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

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

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

后續(xù)將進一步研究無錨點環(huán)境下的多無人平臺協(xié)同導航問題，解決無錨點條件下分布式協(xié)同導航存在誤差耦合、發(fā)散問題。