孫 濤，鄭 辛，常 琦，吳亮華

(1.北京自動(dòng)化控制設(shè)備研究所， 北京 100074；2.中國(guó)航天科工集團(tuán)有限公司， 北京 100048；3.火箭軍裝備部裝備項(xiàng)目管理中心, 北京 100089)

0 引言

目前，國(guó)內(nèi)主流的研究因子圖算法的方式是通過(guò)開源庫(kù)GTSAM(Georgia Tech Smoothing and Mapping)，根據(jù)確定好的因子圖模型，定義并添加需要的因子節(jié)點(diǎn)，利用庫(kù)中實(shí)現(xiàn)的因子圖算法來(lái)解決狀態(tài)估計(jì)問(wèn)題。文獻(xiàn)[1]中針對(duì)水面無(wú)人艇導(dǎo)航系統(tǒng)確定了慣性/衛(wèi)星/多普勒計(jì)程儀組合導(dǎo)航的因子圖模型,建立了慣性傳感器(Inertial Measurement Unit，IMU)、全球衛(wèi)星導(dǎo)航系統(tǒng)(Global Navigation Satellite Syst-em，GNSS)、多普勒計(jì)程儀(Doppler Velocity Log，DVL)的因子節(jié)點(diǎn),并研究了觀測(cè)值異常的處理方法；文獻(xiàn)[2]中針對(duì)船用導(dǎo)航系統(tǒng)確定了慣性/北斗/多普勒計(jì)程儀/天文組合導(dǎo)航的因子圖模型、建立了IMU、北斗衛(wèi)星導(dǎo)航系統(tǒng)(BeiDou Navigation Satellite System，BDS)、DVL、天文導(dǎo)航系統(tǒng)(Celestial Naviga-tion System，CNS)的因子節(jié)點(diǎn),并研究了傳感器的故障檢測(cè)方法；文獻(xiàn)[3]中針對(duì)陸用車載導(dǎo)航系統(tǒng)確定了慣性/衛(wèi)星/里程計(jì)組合導(dǎo)航的因子圖模型、建立了慣性導(dǎo)航系統(tǒng)(Inertial Navigation System，INS)、GNSS、里程計(jì)(Odometer，OD)的因子節(jié)點(diǎn),并驗(yàn)證了其處理傳感器異步和時(shí)延的性能。

但與確定因子圖模型和定義因子節(jié)點(diǎn)相比，因子圖算法才是GTSAM的核心。因子圖算法有消元算法、增量平滑與地圖構(gòu)建(Incremental Smoothing and Mapping，iSAM)算法、iSAM2 (Incremental Smoothing and Mapping using the Bayes tree)算法。其中iSAM算法和iSAM2算法是一種通用的增量推斷算法，適用于任意的因子圖模型，但算法復(fù)雜且不易實(shí)現(xiàn)。利用因子圖算法去分析具體的因子圖模型，并獲得適用于該因子圖模型的增量推斷算法是另一種研究方式。

本文基于第二種研究方式，梳理了因子圖算法及其理論基礎(chǔ)之間的關(guān)系，使用消元算法分析了一元觀測(cè)因子圖模型的稀疏結(jié)構(gòu)，得到了一種適用于該因子圖模型的增量推斷算法，并驗(yàn)證了該增量推斷算法在求解一元觀測(cè)因子圖模型的狀態(tài)估計(jì)問(wèn)題時(shí)運(yùn)算速度的提升能力；同時(shí)，研究了固定滯后平滑算法，實(shí)現(xiàn)了滑窗在該增量推斷算法中的應(yīng)用，并驗(yàn)證了滑窗維持運(yùn)算速度穩(wěn)定的能力。

1 理論基礎(chǔ)

圖1中梳理了因子圖算法及其理論基礎(chǔ)之間的關(guān)系，標(biāo)*表示不常用。

圖1 因子圖算法及其理論基礎(chǔ)Fig.1 Factor graph algorithm and its theoretical basis

因子圖算法的理論基礎(chǔ)是非線性優(yōu)化。根據(jù)貝葉斯公式,并在輸入和觀測(cè)是相互獨(dú)立,狀態(tài)和觀測(cè)符合多維高斯分布的假設(shè)下，對(duì)狀態(tài)進(jìn)行最大后驗(yàn)估計(jì)等價(jià)于求解非線性最小二乘問(wèn)題。

求解非線性最小二乘問(wèn)題的方式有解析方式和迭代方式，而迭代方式將求解非線性最小二乘問(wèn)題轉(zhuǎn)化為獲取使目標(biāo)函數(shù)值下降的修正量的問(wèn)題，其中獲取修正量的方法有梯度下降法、牛頓法、高斯牛頓法、列文伯格-馬夸爾特法及Dogleg法等。

求解高斯牛頓方程的方式可分為全局推斷、稀疏推斷及增量推斷。全局推斷，直接對(duì)高斯牛頓方程進(jìn)行求解，方法有直接求逆法、QR分解法、Cholesky分解法；稀疏推斷，利用稀疏結(jié)構(gòu)進(jìn)行求解，方法有稀疏QR分解法、稀疏Cholesky分解法、消元算法等；增量推斷，無(wú)需重新分解并利用前上三角矩陣，僅更新受新因子影響的部分，方法有iSAM算法、iSAM2算法。

1.1 最大后驗(yàn)估計(jì)

根據(jù)貝葉斯公式，并且在給定、的情況下，有

(1)

即后驗(yàn)概率密度正比于觀測(cè)概率密度(,|)與先驗(yàn)概率密度()的乘積，其中，={,,…,}為所有狀態(tài)；={,,…,}為所有觀測(cè)；={,,…,}為所有輸入。

所有輸入和觀測(cè)是相互獨(dú)立的，對(duì)觀測(cè)概率密度進(jìn)行因式分解

(2)

各狀態(tài)和觀測(cè)符合多維高斯分布，已知多維高斯分布～(,∑),∈×1的概率密度函數(shù)表示為

(3)

對(duì)所有狀態(tài)進(jìn)行最大后驗(yàn)估計(jì)可表示為

(4)

對(duì)(,|)()取負(fù)對(duì)數(shù)，因?yàn)閷?duì)數(shù)函數(shù)單調(diào)遞增，所以對(duì)(,|)()求最大化等價(jià)于對(duì)(,|)()的負(fù)對(duì)數(shù)求最小化。

(5)

在上述假設(shè)下，并忽略常數(shù)項(xiàng)及系數(shù)可得

(6)

將馬氏范數(shù)轉(zhuǎn)化為2-范數(shù)，即

(7)

將式(7)代入式(6)中，可得

(8)

對(duì)狀態(tài)進(jìn)行最大后驗(yàn)估計(jì)等價(jià)于求解使目標(biāo)函數(shù)值達(dá)到最小值的所有狀態(tài)的問(wèn)題。當(dāng)(·)或(·)是非線性函數(shù)時(shí)，該問(wèn)題為非線性最小二乘問(wèn)題；當(dāng)(·)和(·)均是線性函數(shù)時(shí)，該問(wèn)題為線性最小二乘問(wèn)題，求解非線性最小二乘問(wèn)題的方法也適用于線性最小二乘問(wèn)題。

1.2 求解方式及方法

如果目標(biāo)函數(shù)的數(shù)學(xué)形式簡(jiǎn)單，則可以用解析方式直接求解，即令目標(biāo)函數(shù)的導(dǎo)數(shù)為零，然后求解得到導(dǎo)數(shù)為零處的極值點(diǎn)，最后代入目標(biāo)函數(shù)，逐個(gè)比較它們的函數(shù)值大小即可。

在導(dǎo)函數(shù)的數(shù)學(xué)形式復(fù)雜，且隨著目標(biāo)函數(shù)中狀態(tài)個(gè)數(shù)不斷增加的情況下，極值點(diǎn)求解困難且個(gè)數(shù)會(huì)呈幾何倍增長(zhǎng)。目前，求解非線性最小二乘問(wèn)題，通常采用迭代的方式：從一個(gè)初始值出發(fā)，求解使目標(biāo)函數(shù)值下降的修正量，并修正所有的狀態(tài)。大致的求解流程如下：

1)給定初始值={,,…,,0}；

2)在第次迭代，獲取使目標(biāo)函數(shù)值下降的修正量

Δ={Δ0,,1,,…,,}

3)若Δ足夠小或使目標(biāo)函數(shù)值上升，則停止迭代；

4)否則，令+1=+Δ，返回第2)步。

其中，表示第次迭代時(shí)的所有狀態(tài)；,表示第次迭代時(shí)的狀態(tài);Δ,表示第次迭代獲得的的修正量。

因此，迭代方式中獲取使目標(biāo)函數(shù)值下降的修正量是關(guān)鍵，方法有梯度下降法、牛頓法、高斯牛頓法、列文伯格-馬夸爾特法以及Dogleg最小化法。本文主要開展基于高斯牛頓法的推斷算法研究。

第次迭代時(shí)，高斯牛頓法獲取的修正量公式為

()()Δ=-()()

(9)

式(9)被稱為增量方程或高斯牛頓方程。

2 消元算法及固定滯后平滑算法

2.1 消元算法

在文獻(xiàn)[7]中，馬爾可夫隨機(jī)場(chǎng)與海森矩陣相對(duì)應(yīng)，而馬爾可夫隨機(jī)場(chǎng)可體現(xiàn)出海森矩陣的稀疏結(jié)構(gòu)。在因子圖算法中，因子圖與雅可比矩陣相對(duì)應(yīng)，且因子圖可體現(xiàn)出雅可比矩陣的稀疏結(jié)構(gòu)；貝葉斯網(wǎng)絡(luò)與上三角矩陣相對(duì)應(yīng)，且貝葉斯網(wǎng)絡(luò)可體現(xiàn)上三角矩陣的稀疏結(jié)構(gòu)。

消元算法是一種將因子圖轉(zhuǎn)化為因子化的貝葉斯網(wǎng)絡(luò)的算法，實(shí)質(zhì)上是將稀疏雅可比矩陣分解為上三角矩陣的算法。但與稀疏QR分解和稀疏Cholesky分解相比，消元算法是一種更加通用的稀疏矩陣分解法，它適用于任何能夠表示為因子圖的稀疏矩陣。

算法實(shí)現(xiàn)流程如下：

1)選定消元順序；

3)重復(fù)步驟2)，直至將因子圖完全轉(zhuǎn)化為因子化的貝葉斯網(wǎng)絡(luò)

(10)

2.2 固定滯后平滑算法

非線性優(yōu)化是一種批量估計(jì)方法，需求解所有狀態(tài)的修正量Δ。因此,隨著狀態(tài)個(gè)數(shù)的不斷增加，運(yùn)算速度會(huì)變慢，需通過(guò)滑窗限制狀態(tài)量個(gè)數(shù)，維持運(yùn)算速度穩(wěn)定。固定滯后平滑算法為實(shí)現(xiàn)滑窗提供了理論指導(dǎo)。

固定滯后平滑算法是在添加一個(gè)狀態(tài)后，對(duì)除了最近個(gè)狀態(tài)的其他狀態(tài)進(jìn)行邊緣化，僅需修正最近的個(gè)狀態(tài)的算法，類似于滑動(dòng)窗口操作。

結(jié)合消元算法，固定滯后平滑算法的實(shí)現(xiàn)步驟如下：

1)使用消元算法，將(迷你)因子圖轉(zhuǎn)化為因子化的貝葉斯網(wǎng)絡(luò)；

2)在因子化的貝葉斯網(wǎng)絡(luò)上，對(duì)除了最近個(gè)狀態(tài)的其他狀態(tài)進(jìn)行邊緣化；

3)加入新?tīng)顟B(tài)后，將因子化的貝葉斯網(wǎng)絡(luò)中與新?tīng)顟B(tài)相連的狀態(tài)轉(zhuǎn)化回因子圖，與新?tīng)顟B(tài)相連的因子節(jié)點(diǎn)一起構(gòu)成迷你因子圖后，返回步驟1)。

固定滯后平滑算法是對(duì)因子化的貝葉斯網(wǎng)絡(luò)中的狀態(tài)進(jìn)行邊緣化。簡(jiǎn)單地將因子圖中除最近個(gè)狀態(tài)的其他狀態(tài)及相連的因子節(jié)點(diǎn)丟掉絕不等價(jià)于邊緣化，這會(huì)導(dǎo)致信息的丟失。

3 一元觀測(cè)因子的因子圖模型

3.1 稀疏結(jié)構(gòu)分析

一元觀測(cè)因子的因子圖模型是除了狀態(tài)轉(zhuǎn)移因子外，觀測(cè)因子均為一元的因子圖模型，而一元觀測(cè)因子并非僅有一種觀測(cè)因子,而是指僅與一個(gè)狀態(tài)相關(guān)的觀測(cè)因子。典型的一元觀測(cè)因子圖模型有速度、位置或姿態(tài)匹配的因子圖模型。

如表1所示，一元觀測(cè)因子的因子圖模型在增加新的一元觀測(cè)因子后，使用消元算法得到的因子化貝葉斯網(wǎng)絡(luò)與之前的因子化貝葉斯網(wǎng)絡(luò)結(jié)構(gòu)相同，即對(duì)應(yīng)的上三角矩陣具有相同的稀疏結(jié)構(gòu)；而增加新?tīng)顟B(tài)后，因子化的貝葉斯網(wǎng)絡(luò)所對(duì)應(yīng)的上三角矩陣的稀疏結(jié)構(gòu)仍具有一定的規(guī)律性：在對(duì)角線、狀態(tài)與其下一個(gè)狀態(tài)的對(duì)應(yīng)處具有非零矩陣塊，其余均為零矩陣塊,并且僅最新?tīng)顟B(tài)與其上一個(gè)狀態(tài)的對(duì)角線及對(duì)應(yīng)處非零矩陣塊發(fā)生變化，標(biāo)_表示發(fā)生變化。

表1 一元觀測(cè)因子的因子圖模型

3.2 增量推斷算法

已知=，由3.1節(jié)的規(guī)律性及增量推斷思想，提出了一種適用于一元觀測(cè)因子圖模型的增量推斷算法，偽代碼如下：

()

{

(==1)

{

//(-1)是與-1相連的因子對(duì)應(yīng)的非零矩陣塊

}

{

}

//()是與相連的因子對(duì)應(yīng)的非零矩陣塊

}

(11)

(12)

再通過(guò)反向替代求解Δ=Δ得到修正量Δ。

(13)

3.3 滑窗實(shí)現(xiàn)

根據(jù)固定滯后平滑算法，滑窗需要在因子化的貝葉斯網(wǎng)絡(luò)上進(jìn)行，但是為了求解Δ=Δ=-獲得修正量Δ，還需要獲得滑窗后的因子圖。

滑窗后的因子圖如圖2所示,將因子圖中滑窗之外的其他狀態(tài)及相連的因子節(jié)點(diǎn)丟掉后，需要對(duì)滑窗中第1個(gè)狀態(tài)添加一個(gè)先驗(yàn)因子。加入先驗(yàn)因子后的滑窗因子圖轉(zhuǎn)化為因子化貝葉斯網(wǎng)絡(luò),與滑窗后的因子化貝葉斯網(wǎng)絡(luò)相等即可。該實(shí)現(xiàn)方法可實(shí)現(xiàn)任意的滑窗方式，“窗口為，進(jìn)1出1”、“窗口為，進(jìn)”等。

圖2 滑窗后因子化貝葉斯網(wǎng)絡(luò)及因子圖Fig.2 Factorized Bayesian networks and factor graphs after sliding window

4 仿真驗(yàn)證與分析

速度匹配的因子圖模型就是典型的一元觀測(cè)因子圖模型。如圖3所示，除狀態(tài)轉(zhuǎn)移因子、、外，觀測(cè)因子、、僅與一個(gè)狀態(tài)相關(guān)。因此，本文以15維慣導(dǎo)誤差作為狀態(tài)量，使用靜基座條件下兩位置速度匹配，驗(yàn)證基于因子圖的狀態(tài)估計(jì)方法的估計(jì)能力、該增量推斷算法對(duì)運(yùn)算速度的提升能力及滑窗維持運(yùn)算速度穩(wěn)定的能力。

圖3 速度匹配的因子圖模型Fig.3 Factor graph model for velocity matching

4.1 建立模型

系統(tǒng)模型如下

(14)

狀態(tài)量為15維慣導(dǎo)誤差

=[δδδδδδ

]

狀態(tài)轉(zhuǎn)移因子為

(15)

其中，=·，為兩個(gè)狀態(tài)量之間的時(shí)間間隔。

觀測(cè)因子為

(16)

采用輸出校正的方式，當(dāng)接收到零速觀測(cè)時(shí)，添加新?tīng)顟B(tài)并確定相應(yīng)的狀態(tài)轉(zhuǎn)移因子及觀測(cè)因子，否則一直更新?tīng)顟B(tài)轉(zhuǎn)移因子。

4.2 仿真驗(yàn)證

如圖4所示，繪制了真實(shí)狀態(tài)曲線,增量推斷、滑窗增量推斷的估計(jì)曲線及增量推斷、滑窗增量推斷與真值的絕對(duì)偏差曲線，其中后兩個(gè)算法僅存儲(chǔ)對(duì)最新?tīng)顟B(tài)的估計(jì)值。

(a) 速度誤差

(b) 北速誤差絕對(duì)偏差

(c) 天速誤差絕對(duì)偏差

(d) 東速誤差絕對(duì)偏差

(e) 位置誤差

(f) 緯度誤差絕對(duì)偏差

(g) 高度誤差絕對(duì)偏差

(h) 經(jīng)度誤差絕對(duì)偏差

(i) 失準(zhǔn)角

(j) 北向失準(zhǔn)角絕對(duì)偏差

(k) 天向失準(zhǔn)角絕對(duì)偏差

(l) 東向失準(zhǔn)角絕對(duì)偏差

(m) 加表零位

(n) X向加表零位絕對(duì)偏差

(o) Y向加表零位絕對(duì)偏差

(p) Z向加表零位絕對(duì)偏差

(q) 陀螺漂移

(r) X向陀螺漂移絕對(duì)偏差

(s) Y向陀螺漂移絕對(duì)偏差

(t) Z向陀螺漂移絕對(duì)偏差

如圖4和表2所示，與真實(shí)狀態(tài)相比，基于因子圖的狀態(tài)估計(jì)方法的速度誤差絕對(duì)偏差的量級(jí)在10m/s；位置誤差在最終時(shí)刻偏差最大，最大偏差與真實(shí)狀態(tài)之比為8.33m/-10582.6m、1.10m/1176.6m、12.89m/9441.4m，量級(jí)均在10以下；最終時(shí)刻北向、天向、東向失準(zhǔn)角與真實(shí)失準(zhǔn)角的偏差為(9.69×10)°、(2.38×10)°、(1.55×10)°；除向陀螺漂移，其他慣性器件誤差與真實(shí)值偏差逐漸減小，從而驗(yàn)證了基于因子圖的狀態(tài)估計(jì)方法的狀態(tài)估計(jì)能力。

表2 Incre估計(jì)與真實(shí)狀態(tài)對(duì)比

本文S_Incre選取“窗口為20，20進(jìn)10”的滑窗方式,以驗(yàn)證滑窗維持運(yùn)算速度穩(wěn)定的能力。當(dāng)狀態(tài)量個(gè)數(shù)為20時(shí)，在添加新?tīng)顟B(tài)前需要滑掉前10個(gè)狀態(tài)，保留后10個(gè)狀態(tài)，添加新?tīng)顟B(tài)依然使用增量推斷。

由圖4可知，該滑窗方式下，不使用滑窗與使用滑窗的狀態(tài)估計(jì)曲線近乎重合，且由表2與表3可知，最終時(shí)刻不滑窗估計(jì)、滑窗估計(jì)與真實(shí)狀態(tài)的絕對(duì)偏差相當(dāng)。

表3 SIncre估計(jì)與真實(shí)狀態(tài)對(duì)比

文中實(shí)現(xiàn)的全局推斷、增量推斷及滑窗增量推斷在相同處理器下運(yùn)行，且均運(yùn)行三次及以上,求取平均運(yùn)行時(shí)間,如表4所示。標(biāo)***表示運(yùn)算時(shí)間過(guò)長(zhǎng)，超天量級(jí)。

表4 運(yùn)行時(shí)間

由表4中的運(yùn)行時(shí)間可知，當(dāng)處理5min的仿真數(shù)據(jù)時(shí)，全局推斷耗時(shí)24h15min59s，而增量推斷僅耗時(shí)2.074s。對(duì)于一元觀測(cè)因子圖模型，與全局推斷相比，增量推斷算法對(duì)運(yùn)算速度提升明顯。但是隨著狀態(tài)個(gè)數(shù)的不斷增加，處理仿真數(shù)據(jù)(1min)的平均時(shí)間不斷上升，與理論分析結(jié)果一致。使用滑窗后，滑窗內(nèi)的狀態(tài)個(gè)數(shù)至多20個(gè)，處理仿真數(shù)據(jù)(1min)的平均時(shí)間在0.15s左右，運(yùn)行時(shí)間呈線性增長(zhǎng)，驗(yàn)證了滑窗使運(yùn)算速度維持穩(wěn)定的能力。

5 結(jié)論

本文開展了基于因子圖的狀態(tài)估計(jì)方法運(yùn)算實(shí)時(shí)性研究，主要工作如下：

1)研究了因子圖算法中的消元算法，并使用該算法分析了一元觀測(cè)因子的因子圖模型，結(jié)合增量推斷思想，提出了一種適用于該模型的增量推斷算法；以靜基座條件下速度匹配為例，通過(guò)數(shù)學(xué)仿真，與真實(shí)狀態(tài)進(jìn)行對(duì)比，驗(yàn)證了該算法的狀態(tài)估計(jì)能力；與全局推斷進(jìn)行對(duì)比，驗(yàn)證了該增量推斷算法對(duì)于求解一元觀測(cè)因子圖模型的狀態(tài)估計(jì)問(wèn)題的運(yùn)算速度提升能力；

2)結(jié)合該增量推斷算法，研究了基于固定滯后平滑算法的滑窗實(shí)現(xiàn)方法，通過(guò)數(shù)學(xué)仿真，該滑窗實(shí)現(xiàn)方法使運(yùn)行時(shí)長(zhǎng)呈線性增長(zhǎng)，從而驗(yàn)證了該滑窗維持運(yùn)算速度穩(wěn)定的能力。