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

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

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

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

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

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

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

1 問(wèn)題建模

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

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

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

1.1 航位推算約束

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

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

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

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

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

對(duì)殘差進(jìn)行求偏導(dǎo)，對(duì)應(yīng)雅可比矩陣為

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

其中，天向陀螺零偏bk影響航向角的誤差，以當(dāng)前時(shí)刻航向角估計(jì)值與量測(cè)值建立殘差：

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

設(shè)零偏為常值

則式（5）化簡(jiǎn)為

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

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

1.2 UWB 相對(duì)測(cè)距約束

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

式中：dk為k時(shí)刻載體通過(guò)UWB 相對(duì)測(cè)距獲得的與協(xié)同節(jié)點(diǎn)（或錨點(diǎn)）之間的距離觀測(cè)值。坐標(biāo)（xc，yc)為協(xié)同節(jié)點(diǎn)（或錨點(diǎn)）坐標(biāo)值。對(duì)殘差進(jìn)行求偏導(dǎo)，對(duì)應(yīng)雅可比矩陣為

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

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

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

2 算法設(shè)計(jì)

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

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

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

算法流程步驟如下：

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

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

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

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

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

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

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

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

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

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

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

2.2 協(xié)同導(dǎo)航算法設(shè)計(jì)

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

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

在協(xié)同終端為室外錨點(diǎn)的情況下，相對(duì)測(cè)距因子的數(shù)學(xué)形式為

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

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

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

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

式中：(xcoop，ycoop)來(lái)自協(xié)同節(jié)點(diǎn)在當(dāng)前時(shí)刻下通過(guò)自身觀測(cè)信息進(jìn)行位置校準(zhǔn)后的定位結(jié)果。下面將分別設(shè)計(jì)典型場(chǎng)景對(duì)算法進(jìn)行驗(yàn)證。

3 仿真與實(shí)驗(yàn)分析

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

3.1 仿真驗(yàn)證

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

圖7 仿真場(chǎng)景Fig.7 Simulation scenarios

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

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

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

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

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

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

節(jié)點(diǎn)4 在400～600 s 期間處于通信最?lèi)毫拥臒o(wú)錨點(diǎn)觀測(cè)環(huán)境下，僅在0～200 s 期間通過(guò)室外接收錨點(diǎn)觀測(cè)估計(jì)出的零偏進(jìn)行補(bǔ)償，抑制導(dǎo)航誤差的發(fā)散。在長(zhǎng)時(shí)間惡劣環(huán)境下進(jìn)行導(dǎo)航，誤差呈線性發(fā)散，發(fā)散程度取決于零偏估計(jì)的精度，由于處于室外時(shí)間較短，零偏估計(jì)與真值之間存在一定誤差，但綜合定位精度高于航位推算方法，證明了本文方法的有效性。

3.2 綜合模擬實(shí)驗(yàn)

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

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

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

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

圖10 實(shí)驗(yàn)場(chǎng)景分布示意圖Fig.10 Scene map of the experiment

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

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

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

圖11 實(shí)驗(yàn)結(jié)果Fig.11 Experiment result

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

4 結(jié)論

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

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

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

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

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