基于因子图的状态估计方法

刘虎成，程咏梅

(1.中国电子科技集团公司第二十研究所，陕西 西安 710129；2.西北工业大学，陕西 西安 710129)

0 引 言

状态估计也称过程估计，是指动态系统依据状态方程和量测方程，由量测数据来估计系统的状态[1-2]。在动态系统中，一般用状态方程描述系统状态随着时间推移的变化过程，用量测方程描述某种测量方式或传感器对状态的观测。

当前的动态系统主要采用卡尔曼滤波器进行状态估计，存在的问题有：

(1) 卡尔曼滤波要求系统的状态方程和量测方程都是线性的，或者在非线性程度不高时使用扩展卡尔曼滤波(EKF)，但不适用于非线性程度较高的场景。

(2) 只对当前时刻的状态进行估计，建立的量测方程只能描述量测与当前状态的映射关系。当量测数据存在延迟时无法直接使用，需要使用特定的配准方法进行数据对准，会造成精度损失。

(3) 当滤波器内只存在当前时刻的状态，当历史时刻状态与当前状态存在约束关系时，滤波器不能利用这种约束对当前状态进行修正。

上述问题限制了卡尔曼滤波在一些特定场景(如同步定位与地图创建(SLAM)、多状态约束和量测数据存在乱序延时等情形)下的使用。2006年起，美国乔治亚理工学院的Frank Dellaert和麻省理工学院的Michael Kaess等人使用一种图形模型(因子图)对自主移动机器人的SLAM问题[3-5]进行表示和分析，建立了基于因子图的状态估计方法。该方法可以在非线性模型下进行机器人轨迹状态的同时滤波与平滑。同时，基于因子图的状态估计方法可以处理多状态相关量测信息(即量测信息的获取与多个时刻的状态相关)。

综上所述，本文基于美国乔治亚理工学院的研究框架，系统阐述基于因子图状态估计方法的基本原理，包括因子图模型理论与数学意义、状态估计问题的因子图模型描述和状态求解方法。

1 因子图模型

因子图这一概念最初在20世纪90年代由F.R.Kschischang等[6-7]在用于分析信道编码的Tanner图[8]、Wiberg图[9]等模型的基础上推广而来，其后便成为研究信道估计、解码及迭代接收机技术的一个通用工具[10-11]。因子图模型的表示简洁，概念清晰，用于表示估计问题中的联合概率密度函数，能够将状态与状态、状态与量测过程之间的关系可视化。

定义：用集合G=(X,F,E)表示因子图模型，它包含有2种节点：变量节点X={X1,X2,…,Xn}、因子节点(也称函数节点)F={f1,f2,…,fm}以及连接2种节点的无向边E。因子节点fj和变量节点Xk之间存在边的充要条件是Xk∈Sj存在，边线E表示因子节点和变量节点间的函数关系[12]。下面用个例子简要介绍下因子图的概念及数学意义。

假设有一全局函数g(X1,X2,…,Xn)，现在将该函数因式分解为m个因式:

(1)

式中：Sj⊆{X1,X2,…,Xn}，是X的第j个变量子空间；f是一个实值函数，如式：

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

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

(2)

根据因子图的规则，式(2)对应的因子图模型如图1所示。图中用圆圈表示变量节点X={x1,x2,x3,x4,x5}；用实心黑点表示因子节点F={fA,fB,fC,fD,fE}，即全局函数g(x1,x2,x3,x4,x5)的因式，称为局部函数。从图中可以看出，因子图中每一个变量节点对应一个变量，每一个因子节点对应一个局部函数，当且仅当变量是局部函数的自变量时，相应的变量节点和函数节点之间存在一条连接边。

综上可知，因子图模型显式地表示了多元函数

图1 公式(2)所对应的因子图模型

的因式分解。使用因子图模型处理一个具有多个变量的全局函数时，可将一个复杂的全局函数分解为若干个简单的局部函数的乘积，进而通过这种分治策略，减小求解全局函数的复杂度。

2 状态估计的因子图模型表示

一般来说，对一个动态系统进行状态估计时，首先要建立状态方程和量测方程。状态方程可表示为：

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

(3)

式中：xk(k=1,2,…)为tk时刻的系统状态；uk为驱动状态动态变化的控制输入；fk(xk-1,uk)为系统动态转移模型；ωk为模型误差噪声。

量测方程表示为：

zk=hk(xk)+μk

(4)

式中：zk为量测输出；hk(xk)为量测模型；μk为量测噪声。

(5)

此时，真实状态xk和理想量测zk的条件概率分布满足：

(6)

假定系统直到tk时刻的状态集合为Xk，量测集合为Zk，则有：

(7)

则直到tk时刻，系统的联合概率密度函数(JPDF)表示为：

(8)

对于状态估计问题，在已知系统状态的JPDF和量测数据下，状态估计就是对系统JPDF的最大后验估计[13](MAP)。系统状态变量的最大后验估计可表示为：

(9)

然而，由式(8)可知，状态估计系统的JPDF是由不同时刻的状态分布和不同传感器的量测分布共同组合的变量众多、形式相当复杂的全局函数，对其进行MAP求解相当困难。考虑到动态系统中的每一个状态转移过程和量测过程都是全局函数的一个分解因式，是一个局部函数，这与因子图模型所表示的数学含义高度一致；因此可以考虑使用因子图模型对状态估计系统的JPDF进行表示(见图2)，进而使用因子图模型的分治策略简化JPDF的MAP求解。

图2 估计系统的JPDF的因子图模型表示

由图2所示，图中用变量节点表示系统待估计的状态，用因子节点表示先验信息、状态转移和量测过程。利用因子图模型对估计系统的JPDF进行表示，可以直观地反映动态系统的动态演化过程和每个状态对应的量测过程。

同时，图形化的表示使系统具有更好的通用性和扩展性：

(1) 系统每进行一次状态转移就等价于在因子图模型中增加一个变量节点；

(2) 系统每增加一个量测信息就等价于在因子图模型中增加一个因子节点；

(3) 甚至当某种量测方式建立的是系统不同时刻状态间的关系，也可以使用一个因子节点将2个状态连接起来，进而在因子图中形成一个闭环。

反之，当系统不需要对某个状态进行估计或不使用某个量测信息时，等价于在因子图中删除相应的节点。

3 基于因子图的状态估计求解

对动态系统的JPDF进行MAP估计时，由于JPDF是多个过程分布函数的连乘形式，如式(8)所示，直接求解较为困难，可以考虑对式(8)进行取对数操作，从而得到式(9)的等价形式如下：

(10)

(11)

若定义代价函数gk为：

(12)

则系统的状态估计可等价为全局代价函数的联合优化：

(13)

显然，式(13)是一种最小二乘的表示形式，由于系统模型fk和量测模型hk通常存在一定的非线性，所以式(13)描述的是一个非线性最小二乘求解问题。

(1) 对于状态模型

(14)

其中：

(15)

(2) 对于量测模型

{Hkδxk}-ck

(16)

其中：

(17)

(18)

式中：Δ={δxi}，为对当前系统变量Xk的更新增量。

显然，式(18)表示的是一个典型的线性加权最小二乘问题。

(19)

(20)

(21)

综上所述，可以给出基于因子图的状态估计方法中全局代价函数g(Xk)求解的高斯-牛顿迭代优化方法流程，如图3所示。

图3 高斯-牛顿非线性迭代优化算法框架

由此可得全局代价函数g(Xk)的迭代优化求解流程，如下所示：

(5) 优化完成，最新的线性化点即为优化求解的状态。

图4给出了基于因子图状态估计方法的流程图。

图4 基于因子图状态估计方法流程图

如图5所示，进一步认为系统雅克比矩阵J(Xk)是因子图的一种线性化表示形式，它的块结构反应了因子图的节点结构：J(Xk)矩阵中的每一行(行块)代表了一个量测过程的代价函数，矩阵中的每一列代表了一个导航状态变量。图中P(z3|x1,x3)节点表示的量测方程建立了2个不同时刻状态间的约束关系，这在基于因子图的状态估计方法中是普遍适用的。

图5 一个典型动态系统的因子图与对应的雅克比矩阵

4 结束语

基于因子图的动态系统状态估计方法将系统的JPDF用一个因子图模型表示；进而利用因子图模型的分治策略将JPDF的MAP转化为一个全局代价函数联合优化；最后对全局代价函数进行线性化和归一化等价，将估计问题转化为最小二乘方程组的求解问题。与此同时，因子图模型的二元节点结构简明直观，便于系统的快速构建和灵活扩展，特别适用于量测信息频繁切换的动态系统。另一方面，基于因子图的估计问题可以将历史时刻的状态都作为估计状态，虽然增加了估计问题的计算量，但是也能充分利用量测信息对当前状态进行估计，对历史估计进行平滑，也能利用历史状态对当前状态进行估计。

但是，基于因子图的状态估计中系统每接收到一个量测，因子图中就会增加相应的因子节点，这等价于在被线性化的量测雅克比矩阵和右手项增加一行新数据。但随着动态系统时间的推移，系统的状态和量测不断增加，系统对应因子图模型的因子节点和变量节点数目也不断增加。当求解方程组的维数快速增加时，联合优化求解的计算量会急剧增加，使实时性难以得到保证。针对基于因子图状态估计方法实时性的常见快速解决方法包括区间平滑法、“矩阵稀疏分解”法和增量平滑算法等，可根据具体实际问题选取适当的快速求解方法。