基于因子圖的狀態估計方法運算實時性研究

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

(1.北京自動化控制設備研究所， 北京 100074；2.中國航天科工集團有限公司， 北京 100048；3.火箭軍裝備部裝備項目管理中心, 北京 100089)

0 引言

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

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

本文基于第二種研究方式，梳理了因子圖算法及其理論基礎之間的關系，使用消元算法分析了一元觀測因子圖模型的稀疏結構，得到了一種適用于該因子圖模型的增量推斷算法，并驗證了該增量推斷算法在求解一元觀測因子圖模型的狀態估計問題時運算速度的提升能力；同時，研究了固定滯后平滑算法，實現了滑窗在該增量推斷算法中的應用，并驗證了滑窗維持運算速度穩定的能力。

1 理論基礎

圖1中梳理了因子圖算法及其理論基礎之間的關系，標*表示不常用。

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

因子圖算法的理論基礎是非線性優化。根據貝葉斯公式,并在輸入和觀測是相互獨立,狀態和觀測符合多維高斯分布的假設下，對狀態進行最大后驗估計等價于求解非線性最小二乘問題。

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

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

1.1 最大后驗估計

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

(1)

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

所有輸入和觀測是相互獨立的，對觀測概率密度進行因式分解

(2)

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

(3)

對所有狀態進行最大后驗估計可表示為

(4)

對(,|)()取負對數，因為對數函數單調遞增，所以對(,|)()求最大化等價于對(,|)()的負對數求最小化。

(5)

在上述假設下，并忽略常數項及系數可得

(6)

將馬氏范數轉化為2-范數，即

(7)

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

(8)

對狀態進行最大后驗估計等價于求解使目標函數值達到最小值的所有狀態的問題。當(·)或(·)是非線性函數時，該問題為非線性最小二乘問題；當(·)和(·)均是線性函數時，該問題為線性最小二乘問題，求解非線性最小二乘問題的方法也適用于線性最小二乘問題。

1.2 求解方式及方法

如果目標函數的數學形式簡單，則可以用解析方式直接求解，即令目標函數的導數為零，然后求解得到導數為零處的極值點，最后代入目標函數，逐個比較它們的函數值大小即可。

在導函數的數學形式復雜，且隨著目標函數中狀態個數不斷增加的情況下，極值點求解困難且個數會呈幾何倍增長。目前，求解非線性最小二乘問題，通常采用迭代的方式：從一個初始值出發，求解使目標函數值下降的修正量，并修正所有的狀態。大致的求解流程如下：

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

2)在第次迭代，獲取使目標函數值下降的修正量

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

3)若Δ足夠小或使目標函數值上升，則停止迭代；

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

其中，表示第次迭代時的所有狀態；,表示第次迭代時的狀態;Δ,表示第次迭代獲得的的修正量。

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

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

()()Δ=-()()

(9)

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

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

2.1 消元算法

在文獻[7]中，馬爾可夫隨機場與海森矩陣相對應，而馬爾可夫隨機場可體現出海森矩陣的稀疏結構。在因子圖算法中，因子圖與雅可比矩陣相對應，且因子圖可體現出雅可比矩陣的稀疏結構；貝葉斯網絡與上三角矩陣相對應，且貝葉斯網絡可體現上三角矩陣的稀疏結構。

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

算法實現流程如下：

1)選定消元順序；

3)重復步驟2)，直至將因子圖完全轉化為因子化的貝葉斯網絡

(10)

2.2 固定滯后平滑算法

非線性優化是一種批量估計方法，需求解所有狀態的修正量Δ。因此,隨著狀態個數的不斷增加，運算速度會變慢，需通過滑窗限制狀態量個數，維持運算速度穩定。固定滯后平滑算法為實現滑窗提供了理論指導。

固定滯后平滑算法是在添加一個狀態后，對除了最近個狀態的其他狀態進行邊緣化，僅需修正最近的個狀態的算法，類似于滑動窗口操作。

結合消元算法，固定滯后平滑算法的實現步驟如下：

1)使用消元算法，將(迷你)因子圖轉化為因子化的貝葉斯網絡；

2)在因子化的貝葉斯網絡上，對除了最近個狀態的其他狀態進行邊緣化；

3)加入新狀態后，將因子化的貝葉斯網絡中與新狀態相連的狀態轉化回因子圖，與新狀態相連的因子節點一起構成迷你因子圖后，返回步驟1)。

固定滯后平滑算法是對因子化的貝葉斯網絡中的狀態進行邊緣化。簡單地將因子圖中除最近個狀態的其他狀態及相連的因子節點丟掉絕不等價于邊緣化，這會導致信息的丟失。

3 一元觀測因子的因子圖模型

3.1 稀疏結構分析

一元觀測因子的因子圖模型是除了狀態轉移因子外，觀測因子均為一元的因子圖模型，而一元觀測因子并非僅有一種觀測因子,而是指僅與一個狀態相關的觀測因子。典型的一元觀測因子圖模型有速度、位置或姿態匹配的因子圖模型。

如表1所示，一元觀測因子的因子圖模型在增加新的一元觀測因子后，使用消元算法得到的因子化貝葉斯網絡與之前的因子化貝葉斯網絡結構相同，即對應的上三角矩陣具有相同的稀疏結構；而增加新狀態后，因子化的貝葉斯網絡所對應的上三角矩陣的稀疏結構仍具有一定的規律性：在對角線、狀態與其下一個狀態的對應處具有非零矩陣塊，其余均為零矩陣塊,并且僅最新狀態與其上一個狀態的對角線及對應處非零矩陣塊發生變化，標_表示發生變化。

表1 一元觀測因子的因子圖模型

3.2 增量推斷算法

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

()

{

(==1)

{

//(-1)是與-1相連的因子對應的非零矩陣塊

}

{

}

//()是與相連的因子對應的非零矩陣塊

}

(11)

(12)

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

(13)

3.3 滑窗實現

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

滑窗后的因子圖如圖2所示,將因子圖中滑窗之外的其他狀態及相連的因子節點丟掉后，需要對滑窗中第1個狀態添加一個先驗因子。加入先驗因子后的滑窗因子圖轉化為因子化貝葉斯網絡,與滑窗后的因子化貝葉斯網絡相等即可。該實現方法可實現任意的滑窗方式，“窗口為，進1出1”、“窗口為，進”等。

圖2 滑窗后因子化貝葉斯網絡及因子圖Fig.2 Factorized Bayesian networks and factor graphs after sliding window

4 仿真驗證與分析

速度匹配的因子圖模型就是典型的一元觀測因子圖模型。如圖3所示，除狀態轉移因子、、外，觀測因子、、僅與一個狀態相關。因此，本文以15維慣導誤差作為狀態量，使用靜基座條件下兩位置速度匹配，驗證基于因子圖的狀態估計方法的估計能力、該增量推斷算法對運算速度的提升能力及滑窗維持運算速度穩定的能力。

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

4.1 建立模型

系統模型如下

(14)

狀態量為15維慣導誤差

=[δδδδδδ

]

狀態轉移因子為

(15)

其中，=·，為兩個狀態量之間的時間間隔。

觀測因子為

(16)

采用輸出校正的方式，當接收到零速觀測時，添加新狀態并確定相應的狀態轉移因子及觀測因子，否則一直更新狀態轉移因子。

4.2 仿真驗證

如圖4所示，繪制了真實狀態曲線,增量推斷、滑窗增量推斷的估計曲線及增量推斷、滑窗增量推斷與真值的絕對偏差曲線，其中后兩個算法僅存儲對最新狀態的估計值。

(a) 速度誤差

(b) 北速誤差絕對偏差

(c) 天速誤差絕對偏差

(d) 東速誤差絕對偏差

(e) 位置誤差

(f) 緯度誤差絕對偏差

(g) 高度誤差絕對偏差

(h) 經度誤差絕對偏差

(i) 失準角

(j) 北向失準角絕對偏差

(k) 天向失準角絕對偏差

(l) 東向失準角絕對偏差

(m) 加表零位

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

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

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

(q) 陀螺漂移

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

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

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

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

表2 Incre估計與真實狀態對比

本文S_Incre選取“窗口為20，20進10”的滑窗方式,以驗證滑窗維持運算速度穩定的能力。當狀態量個數為20時，在添加新狀態前需要滑掉前10個狀態，保留后10個狀態，添加新狀態依然使用增量推斷。

由圖4可知，該滑窗方式下，不使用滑窗與使用滑窗的狀態估計曲線近乎重合，且由表2與表3可知，最終時刻不滑窗估計、滑窗估計與真實狀態的絕對偏差相當。

表3 SIncre估計與真實狀態對比

文中實現的全局推斷、增量推斷及滑窗增量推斷在相同處理器下運行，且均運行三次及以上,求取平均運行時間,如表4所示。標***表示運算時間過長，超天量級。

表4 運行時間

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

5 結論

本文開展了基于因子圖的狀態估計方法運算實時性研究，主要工作如下：

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

2)結合該增量推斷算法，研究了基于固定滯后平滑算法的滑窗實現方法，通過數學仿真，該滑窗實現方法使運行時長呈線性增長，從而驗證了該滑窗維持運算速度穩定的能力。