劉虎成，程詠梅

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

0 引 言

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

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

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

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

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

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

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

1 因子圖模型

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

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

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

(1)

式中：Sj?{X1,X2,…,Xn}，是X的第j個(gè)變量子空間；f是一個(gè)實(shí)值函數(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)對(duì)應(yīng)的因子圖模型如圖1所示。圖中用圓圈表示變量節(jié)點(diǎn)X={x1,x2,x3,x4,x5}；用實(shí)心黑點(diǎn)表示因子節(jié)點(diǎn)F={fA,fB,fC,fD,fE}，即全局函數(shù)g(x1,x2,x3,x4,x5)的因式，稱為局部函數(shù)。從圖中可以看出，因子圖中每一個(gè)變量節(jié)點(diǎn)對(duì)應(yīng)一個(gè)變量，每一個(gè)因子節(jié)點(diǎn)對(duì)應(yīng)一個(gè)局部函數(shù)，當(dāng)且僅當(dāng)變量是局部函數(shù)的自變量時(shí)，相應(yīng)的變量節(jié)點(diǎn)和函數(shù)節(jié)點(diǎn)之間存在一條連接邊。

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

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

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

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

一般來(lái)說(shuō)，對(duì)一個(gè)動(dòng)態(tài)系統(tǒng)進(jìn)行狀態(tài)估計(jì)時(shí)，首先要建立狀態(tài)方程和量測(cè)方程。狀態(tài)方程可表示為：

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

(3)

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

量測(cè)方程表示為：

zk=hk(xk)+μk

(4)

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

(5)

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

(6)

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

(7)

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

(8)

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

(9)

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

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

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

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

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

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

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

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

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

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

(10)

(11)

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

(12)

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

(13)

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

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

(14)

其中：

(15)

(2) 對(duì)于量測(cè)模型

{Hkδxk}-ck

(16)

其中：

(17)

(18)

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

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

(19)

(20)

(21)

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

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

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

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

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

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

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

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

4 結(jié)束語(yǔ)

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

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