基于因子圖的狀態(tài)估計方法

劉虎成，程詠梅

(1.中國電子科技集團公司第二十研究所，陜西 西安 710129；2.西北工業(yè)大學(xué)，陜西 西安 710129)

0 引 言

狀態(tài)估計也稱過程估計，是指動態(tài)系統(tǒng)依據(jù)狀態(tài)方程和量測方程，由量測數(shù)據(jù)來估計系統(tǒng)的狀態(tài)[1-2]。在動態(tài)系統(tǒng)中，一般用狀態(tài)方程描述系統(tǒng)狀態(tài)隨著時間推移的變化過程，用量測方程描述某種測量方式或傳感器對狀態(tài)的觀測。

當(dāng)前的動態(tài)系統(tǒng)主要采用卡爾曼濾波器進行狀態(tài)估計，存在的問題有：

(1) 卡爾曼濾波要求系統(tǒng)的狀態(tài)方程和量測方程都是線性的，或者在非線性程度不高時使用擴展卡爾曼濾波(EKF)，但不適用于非線性程度較高的場景。

(2) 只對當(dāng)前時刻的狀態(tài)進行估計，建立的量測方程只能描述量測與當(dāng)前狀態(tài)的映射關(guān)系。當(dāng)量測數(shù)據(jù)存在延遲時無法直接使用，需要使用特定的配準方法進行數(shù)據(jù)對準，會造成精度損失。

(3) 當(dāng)濾波器內(nèi)只存在當(dāng)前時刻的狀態(tài)，當(dāng)歷史時刻狀態(tài)與當(dāng)前狀態(tài)存在約束關(guān)系時，濾波器不能利用這種約束對當(dāng)前狀態(tài)進行修正。

上述問題限制了卡爾曼濾波在一些特定場景(如同步定位與地圖創(chuàng)建(SLAM)、多狀態(tài)約束和量測數(shù)據(jù)存在亂序延時等情形)下的使用。2006年起，美國喬治亞理工學(xué)院的Frank Dellaert和麻省理工學(xué)院的Michael Kaess等人使用一種圖形模型(因子圖)對自主移動機器人的SLAM問題[3-5]進行表示和分析，建立了基于因子圖的狀態(tài)估計方法。該方法可以在非線性模型下進行機器人軌跡狀態(tài)的同時濾波與平滑。同時，基于因子圖的狀態(tài)估計方法可以處理多狀態(tài)相關(guān)量測信息(即量測信息的獲取與多個時刻的狀態(tài)相關(guān))。

綜上所述，本文基于美國喬治亞理工學(xué)院的研究框架，系統(tǒng)闡述基于因子圖狀態(tài)估計方法的基本原理，包括因子圖模型理論與數(shù)學(xué)意義、狀態(tài)估計問題的因子圖模型描述和狀態(tài)求解方法。

1 因子圖模型

因子圖這一概念最初在20世紀90年代由F.R.Kschischang等[6-7]在用于分析信道編碼的Tanner圖[8]、Wiberg圖[9]等模型的基礎(chǔ)上推廣而來，其后便成為研究信道估計、解碼及迭代接收機技術(shù)的一個通用工具[10-11]。因子圖模型的表示簡潔，概念清晰，用于表示估計問題中的聯(lián)合概率密度函數(shù)，能夠?qū)顟B(tài)與狀態(tài)、狀態(tài)與量測過程之間的關(guān)系可視化。

定義：用集合G=(X,F,E)表示因子圖模型，它包含有2種節(jié)點：變量節(jié)點X={X1,X2,…,Xn}、因子節(jié)點(也稱函數(shù)節(jié)點)F={f1,f2,…,fm}以及連接2種節(jié)點的無向邊E。因子節(jié)點fj和變量節(jié)點Xk之間存在邊的充要條件是Xk∈Sj存在，邊線E表示因子節(jié)點和變量節(jié)點間的函數(shù)關(guān)系[12]。下面用個例子簡要介紹下因子圖的概念及數(shù)學(xué)意義。

假設(shè)有一全局函數(shù)g(X1,X2,…,Xn)，現(xiàn)在將該函數(shù)因式分解為m個因式:

(1)

式中：Sj?{X1,X2,…,Xn}，是X的第j個變量子空間；f是一個實值函數(shù)，如式：

g(x1,x2,x3,x4,x5)=

fA(x1)fB(x2)fC(x1,x2,x3)fD(x3,x4)fE(x3,x5)

(2)

根據(jù)因子圖的規(guī)則，式(2)對應(yīng)的因子圖模型如圖1所示。圖中用圓圈表示變量節(jié)點X={x1,x2,x3,x4,x5}；用實心黑點表示因子節(jié)點F={fA,fB,fC,fD,fE}，即全局函數(shù)g(x1,x2,x3,x4,x5)的因式，稱為局部函數(shù)。從圖中可以看出，因子圖中每一個變量節(jié)點對應(yīng)一個變量，每一個因子節(jié)點對應(yīng)一個局部函數(shù)，當(dāng)且僅當(dāng)變量是局部函數(shù)的自變量時，相應(yīng)的變量節(jié)點和函數(shù)節(jié)點之間存在一條連接邊。

綜上可知，因子圖模型顯式地表示了多元函數(shù)

圖1 公式(2)所對應(yīng)的因子圖模型

的因式分解。使用因子圖模型處理一個具有多個變量的全局函數(shù)時，可將一個復(fù)雜的全局函數(shù)分解為若干個簡單的局部函數(shù)的乘積，進而通過這種分治策略，減小求解全局函數(shù)的復(fù)雜度。

2 狀態(tài)估計的因子圖模型表示

一般來說，對一個動態(tài)系統(tǒng)進行狀態(tài)估計時，首先要建立狀態(tài)方程和量測方程。狀態(tài)方程可表示為：

xk=fk(xk-1,uk)+ωk

(3)

式中：xk(k=1,2,…)為tk時刻的系統(tǒng)狀態(tài)；uk為驅(qū)動狀態(tài)動態(tài)變化的控制輸入；fk(xk-1,uk)為系統(tǒng)動態(tài)轉(zhuǎn)移模型；ωk為模型誤差噪聲。

量測方程表示為：

zk=hk(xk)+μk

(4)

式中：zk為量測輸出；hk(xk)為量測模型；μk為量測噪聲。

(5)

此時，真實狀態(tài)xk和理想量測zk的條件概率分布滿足：

(6)

假定系統(tǒng)直到tk時刻的狀態(tài)集合為Xk，量測集合為Zk，則有：

(7)

則直到tk時刻，系統(tǒng)的聯(lián)合概率密度函數(shù)(JPDF)表示為：

(8)

對于狀態(tài)估計問題，在已知系統(tǒng)狀態(tài)的JPDF和量測數(shù)據(jù)下，狀態(tài)估計就是對系統(tǒng)JPDF的最大后驗估計[13](MAP)。系統(tǒng)狀態(tài)變量的最大后驗估計可表示為：

(9)

然而，由式(8)可知，狀態(tài)估計系統(tǒng)的JPDF是由不同時刻的狀態(tài)分布和不同傳感器的量測分布共同組合的變量眾多、形式相當(dāng)復(fù)雜的全局函數(shù)，對其進行MAP求解相當(dāng)困難?？紤]到動態(tài)系統(tǒng)中的每一個狀態(tài)轉(zhuǎn)移過程和量測過程都是全局函數(shù)的一個分解因式，是一個局部函數(shù)，這與因子圖模型所表示的數(shù)學(xué)含義高度一致；因此可以考慮使用因子圖模型對狀態(tài)估計系統(tǒng)的JPDF進行表示(見圖2)，進而使用因子圖模型的分治策略簡化JPDF的MAP求解。

圖2 估計系統(tǒng)的JPDF的因子圖模型表示

由圖2所示，圖中用變量節(jié)點表示系統(tǒng)待估計的狀態(tài)，用因子節(jié)點表示先驗信息、狀態(tài)轉(zhuǎn)移和量測過程。利用因子圖模型對估計系統(tǒng)的JPDF進行表示，可以直觀地反映動態(tài)系統(tǒng)的動態(tài)演化過程和每個狀態(tài)對應(yīng)的量測過程。

同時，圖形化的表示使系統(tǒng)具有更好的通用性和擴展性：

(1) 系統(tǒng)每進行一次狀態(tài)轉(zhuǎn)移就等價于在因子圖模型中增加一個變量節(jié)點；

(2) 系統(tǒng)每增加一個量測信息就等價于在因子圖模型中增加一個因子節(jié)點；

(3) 甚至當(dāng)某種量測方式建立的是系統(tǒng)不同時刻狀態(tài)間的關(guān)系，也可以使用一個因子節(jié)點將2個狀態(tài)連接起來，進而在因子圖中形成一個閉環(huán)。

反之，當(dāng)系統(tǒng)不需要對某個狀態(tài)進行估計或不使用某個量測信息時，等價于在因子圖中刪除相應(yīng)的節(jié)點。

3 基于因子圖的狀態(tài)估計求解

對動態(tài)系統(tǒng)的JPDF進行MAP估計時，由于JPDF是多個過程分布函數(shù)的連乘形式，如式(8)所示，直接求解較為困難，可以考慮對式(8)進行取對數(shù)操作，從而得到式(9)的等價形式如下：

(10)

(11)

若定義代價函數(shù)gk為：

(12)

則系統(tǒng)的狀態(tài)估計可等價為全局代價函數(shù)的聯(lián)合優(yōu)化：

(13)

顯然，式(13)是一種最小二乘的表示形式，由于系統(tǒng)模型fk和量測模型hk通常存在一定的非線性，所以式(13)描述的是一個非線性最小二乘求解問題。

(1) 對于狀態(tài)模型

(14)

其中：

(15)

(2) 對于量測模型

{Hkδxk}-ck

(16)

其中：

(17)

(18)

式中：Δ={δxi}，為對當(dāng)前系統(tǒng)變量Xk的更新增量。

顯然，式(18)表示的是一個典型的線性加權(quán)最小二乘問題。

(19)

(20)

(21)

綜上所述，可以給出基于因子圖的狀態(tài)估計方法中全局代價函數(shù)g(Xk)求解的高斯-牛頓迭代優(yōu)化方法流程，如圖3所示。

圖3 高斯-牛頓非線性迭代優(yōu)化算法框架

由此可得全局代價函數(shù)g(Xk)的迭代優(yōu)化求解流程，如下所示：

(5) 優(yōu)化完成，最新的線性化點即為優(yōu)化求解的狀態(tài)。

圖4給出了基于因子圖狀態(tài)估計方法的流程圖。

圖4 基于因子圖狀態(tài)估計方法流程圖

如圖5所示，進一步認為系統(tǒng)雅克比矩陣J(Xk)是因子圖的一種線性化表示形式，它的塊結(jié)構(gòu)反應(yīng)了因子圖的節(jié)點結(jié)構(gòu)：J(Xk)矩陣中的每一行(行塊)代表了一個量測過程的代價函數(shù)，矩陣中的每一列代表了一個導(dǎo)航狀態(tài)變量。圖中P(z3|x1,x3)節(jié)點表示的量測方程建立了2個不同時刻狀態(tài)間的約束關(guān)系，這在基于因子圖的狀態(tài)估計方法中是普遍適用的。

圖5 一個典型動態(tài)系統(tǒng)的因子圖與對應(yīng)的雅克比矩陣

4 結(jié)束語

基于因子圖的動態(tài)系統(tǒng)狀態(tài)估計方法將系統(tǒng)的JPDF用一個因子圖模型表示；進而利用因子圖模型的分治策略將JPDF的MAP轉(zhuǎn)化為一個全局代價函數(shù)聯(lián)合優(yōu)化；最后對全局代價函數(shù)進行線性化和歸一化等價，將估計問題轉(zhuǎn)化為最小二乘方程組的求解問題。與此同時，因子圖模型的二元節(jié)點結(jié)構(gòu)簡明直觀，便于系統(tǒng)的快速構(gòu)建和靈活擴展，特別適用于量測信息頻繁切換的動態(tài)系統(tǒng)。另一方面，基于因子圖的估計問題可以將歷史時刻的狀態(tài)都作為估計狀態(tài)，雖然增加了估計問題的計算量，但是也能充分利用量測信息對當(dāng)前狀態(tài)進行估計，對歷史估計進行平滑，也能利用歷史狀態(tài)對當(dāng)前狀態(tài)進行估計。

但是，基于因子圖的狀態(tài)估計中系統(tǒng)每接收到一個量測，因子圖中就會增加相應(yīng)的因子節(jié)點，這等價于在被線性化的量測雅克比矩陣和右手項增加一行新數(shù)據(jù)。但隨著動態(tài)系統(tǒng)時間的推移，系統(tǒng)的狀態(tài)和量測不斷增加，系統(tǒng)對應(yīng)因子圖模型的因子節(jié)點和變量節(jié)點數(shù)目也不斷增加。當(dāng)求解方程組的維數(shù)快速增加時，聯(lián)合優(yōu)化求解的計算量會急劇增加，使實時性難以得到保證。針對基于因子圖狀態(tài)估計方法實時性的常見快速解決方法包括區(qū)間平滑法、“矩陣稀疏分解”法和增量平滑算法等，可根據(jù)具體實際問題選取適當(dāng)?shù)目焖偾蠼夥椒ā?/p>