为了算法的描述简便，在每个粒子中，机器人位姿和地标位置信息表示为

X ′ t = [ P x t α n ϕ x t α n P L ] = [ x t α n P L ] (13)

其中 x t α n 表示机器人的位姿(包括位置和方向)， P L 表示地标的位置。EKF-SLAM递归遵循两个步骤：预测和更新，这两个步骤分别基于离散时间过程和观测模型 [ 10 ] 。EKF的预测方程为：

P ^ x t + 1 | t α n = P ^ x t | t α n + C ( ϕ ^ x t | t α n ) P ^ x t x t + 1 α n (14)

ϕ ^ x t + 1 | t α n = ϕ ^ x t | t α n + ϕ ^ x t x t + 1 α n (15)

P ^ L t + 1 | t = P ^ L t | t (16)

其中 C ( θ ) = [ cos θ − sin θ sin θ cos θ ] 为2*2的旋转矩阵， X ^ x t α n t + 1 α n = [ P ^ x t α n x t + 1 α n ϕ ^ x t α n x t + 1 α n ] 为机器人从t时刻到 t + 1 时刻基于里程

的运动估计。该估计被高斯白噪声 w t = X x t α n t + 1 α n − X ^ x t α n t + 1 α n 所破坏，该噪声的均值为0，协方差矩阵为 Q t 。

除了状态预测方程，线性化的误差状态方程在EKF中也是非常必要的。真实状态预测方程可以表示为

P x t + 1 | t α n = P x t α n + C ( ϕ x t α n ) P x t α n x t + 1 α n (17)

ϕ x t + 1 | t α n = ϕ x t α n + ϕ x t α n x t + 1 α n (18)

P L t + 1 | t = P L t (19)

将式(17)~(19)和式(14)~(16)相减，得到误差状态估计方程

P ˜ x t + 1 | t α n = P ˜ x t | t α n + C ( ϕ x t α n ) P x t α n x t + 1 α n − C ( ϕ ^ x t | t α n ) P ^ x t α n x t + 1 α n (20)

ϕ ˜ x t + 1 | t α n = ϕ ˜ x t | t α n + ϕ ˜ x t α n x t + 1 α n (21)

P ˜ L t + 1 | t = P ˜ L t | t (22)

显然，式(20)是非线性的，所以我们在估计状态 ϕ ^ x t | t α n 和 P ^ x t α n x t + 1 α n 将其线性化：

P ˜ x t + 1 | t α n = P ˜ x t | t α n + J C ( ϕ ^ x t α n ) P ^ x t α n x t + 1 α n ϕ ˜ x t | t α n + C ( ϕ ^ x t | t α n ) P ˜ x t α n x t + 1 α n (23)

其中 J = [ 0 − 1 1 0 ] 。将误差状态预测方程(21)，(22)，(23)转化为分块矩阵，得到如下所熟悉的方程：

X ˜ ′ t + 1 | t = [ ϕ x t α n 0 3 × 2 0 2 × 3 I 2 ] [ X ˜ t | t α n P ˜ L t | t ] + [ G x t α n 0 2 × 2 ] w t = ϕ t X ˜ ′ t | t + G t w t (24)

其中，状态转移矩阵 ϕ x t α n 和 G x t α n 具体表示为

ϕ x t α n = [ I 2 J C ( ϕ ^ x t | t α n ) P ^ x t α n x t + 1 α n 0 1 × 2 1 ] ≡ [ I 2 J ( P ^ x t + 1 | t α n − P ^ x t | t α n ) 0 1 × 2 1 ] (25)

G x t α n = [ C ( ϕ ^ x t | t α n ) 0 2 × 1 0 1 × 2 1 ] (26)

EKF中使用的观测值是机器人相对地标位置的一个函数：

z t = h ( X ′ t ) + v t = h ( C T ( ϕ x t α n ) ( P L t − P x t α n ) ) + v t (27)

其中， v t 为高斯测量噪声，其均值为0，协方差为 R t 。这里，我们假设函数h为测量函数，且它是可逆的，即通过 z t ，我们可以得到相对于机器人的地标位置的估计值。一般情况下，观测函数是非线性的，因此在EKF中将其线性化。线性化后的观测误差方程为：

z t = [ H x t α n H L t ] [ X ˜ t | t − 1 α n X ˜ L t | t − 1 ] + v t = H t X ˜ ′ t | t − 1 + v t (28)

式中， H x t α n 和 H L t 分别为函数 X ^ ′ t | t − 1 关于机器人位姿和地标位置在状态估计 X ^ ′ t | t − 1 处估计的雅可比矩阵。根据链式法则，我们得到

H x t α n = ( ∇ h t ) C T ( ϕ ^ x t | t − 1 α n ) ⋅ [ − I 2 − J ( P ^ L t | t − 1 − P ^ x t | t − 1 α n ) ] (29)

H L t = ( ∇ h t ) C T ( ϕ ^ x t | t − 1 α n ) (30)

其中 ∇ h t 表示函数h关于相对机器人的地标位置的雅可比矩阵(也就是说，关于 P x t α n L = C T ( ϕ x t α n ) ⋅ ( P L − P x t α n ) 在估计值 X ^ ′ t | t − 1 的雅可比矩阵)。

本文分析了影响一般移动机器人运动一致性的具体原因。黄国权等人 [ 17 ] [ 18 ] [ 19 ] 使用滤波器的误差状态系统模型分析移动机器人一致性不足的问题。

该研究发现标准的EKF-SLAM模型中存在一个不可观测的二维子空间，在全局参考系的位置和角度上有三个不可观测的自由度。当系统和观测模型的雅可比矩阵在每个时间步期间的最近的状态估计中，使用线性误差状态系统进行估计时，可观测子空间的维数高于实际非线性SLAM系统子空间的维数，在状态空间方向上缺乏可用信息导致EKF的协方差估计受到削弱，这是引起不一致性的主要原因。

由于EKF-SLAM的线性化误差状态模型是时变的，因此可以从局部观测矩阵进行可观测性分析。对于EKF-SLAM系统(式(24)和式(28)，t时刻到 t + m 时刻间隔的局部观测矩阵设为

Λ = [ H t H t + 1 ϕ x t α n ⋮ H t + m ϕ x t + m − 1 α n ⋯ ϕ x t α n ] (31)

这个公式可以进一步推广通过代入 ϕ i 和 H i (方程(24)和(28))，可以得到

Λ = [ H x t α n H L t H x t + 1 α n ϕ x t α n H L t + 1 ⋮ ⋮ H x t + m α n ϕ x t + m − 1 α n ⋯ ϕ x t α n H L t + m ] (32)

= diag ( H L t , ⋯ H L t + m ) ⋅ [ H L t − 1 H x t α n I 2 H L t + 1 − 1 H x t + 1 α n ϕ x t α n I 2 ⋮ ⋮ H L t + m − 1 H x t + m α n ⋯ ϕ x t + m − 1 α n I 2 ] Q (33)

其中 diag ( ⋅ ) 为块对角化矩阵。在时间间隔t和 t + m 之间，系统是局部可观的，当且仅当局部可观矩阵 Λ 是满秩的。 diag ( H L t , ⋯ , H L t + m ) 是非奇异的。

在本节中，我们将讨论标准EKF-SLAM的可观性，其中雅可比矩阵是在估计值处计算的。

ϕ x t + 1 α n ϕ x t α n = [ I 2 J ( P ^ x t + 2 | t + 1 α n − P ^ x t + 1 | t + 1 α n ) 0 1 × 2 1 ] ⋅ [ I 2 J ( P ^ x t + 1 | t α n − P ^ x t | t α n ) 0 1 × 2 1 ] = [ I 2 G 0 1 × 2 1 ] (34)

其中 G = J ( P ^ x t + 2 | t + 1 α n − P ^ x t | t α n + ( P ^ x t + 1 | t α n − P ^ x t + 1 | t + 1 α n ) )

通过归纳法，我们可以得到

ϕ x t + i − 1 α n ϕ x t + i − 2 α n ⋯ ϕ x t α n = [ I 2 D 0 1 × 2 1 ] (35)

D = J ( P ^ x t + i | t + i − 1 α n − P ^ x t | t α n − ∑ j = t + 1 t + i − 1 ( P ^ x j | j α n − P ^ x j | j − 1 α n ) ) ，且 i > 0 。结合(28)，(29)，(30)，得到

H L t + i − 1 H x t + i α n ϕ x t + i − 1 α n ⋯ ϕ x t α n = − C − T ( ϕ x t + i | t + i − 1 α n ) ( ∇ h t + i ) − 1 ( ∇ h t + i ) ⋅ C T ( ϕ x t + i | t + i − 1 α n ) [ I 2 J ( P L t + i | t + i − 1 − P x t + i | t + i − 1 α n ) ] [ I 2 E 0 1 × 2 1 ] = − [ I 2 F ] (36)

其中

E = J ( P ^ x t + i | t + i − 1 α n − P ^ x t | t α n + ∑ j = t t + i − 2 ( P ^ x j + 1 | j α n − P ^ x j + 1 | j + 1 α n ) ) , F = J ( P L t + i | t + i − 1 − P ^ x t | t α n + ∑ j = t t + i − 2 ( P ^ x j + 1 | j α n − P ^ x j + 1 | j + 1 α n ) )

利用这个结果，我们可以把(33)中的矩阵Q写成

Q = [ − I 2 A 1 I 2 − I 2 A 2 I 2 − I 2 A 3 I 2 ⋮ ⋮ ⋮ − I 2 A k I 2 ] (37)

其中

A 1 = − J ( P ^ L t | t − 1 − P ^ x t | t − 1 α n ) A 2 = − J ( P ^ L t + 1 | t − P ^ x t | t α n ) A k = − J ( P ^ L t + i | t + i − 1 − P ^ x t | t α n + ∑ j = t t + i − 2 ( P ^ x j + 1 | j α n − P ^ x j + 1 | j + 1 α n ) ) , k = 2 , ⋯ , i

我们注意到，对任何给定状态变量的估计在不同的时间通常是不同的，从而我们可以得到下列不等式一般成立：

P ^ L t | t − 1 ≠ P ^ L t + 1 | t ≠ ⋯ ≠ P ^ L t + i | t + i − 1 (38)

和

P ^ x t + 1 | t α n − P ^ x t + 1 | t + 1 α n ≠ 0 2 × 1 (39)

因此，秩(Q) = 3的矩阵Q的第三列通常是一个具有不规则元素的向量。由此可见，标准EKF-SLAM中采用的线性化误差状态模型的可观性与基本非线性系统不同。具体而言，EKF通过对时间间隔 [ t , t + m ] 采集的观测值进行处理，得到状态空间的三维信息。然而，测量实际上只提供关于状态空间的二维信息。因此，EKF在基本非线性SLAM系统的不可观测方向上获得了错误的信息，从而导致不一致。

通过对矩阵Q的深入研究，我们了解到即使在状态真值处没有估计雅可比矩阵，也可以得到具有三维不可观测子空间的EKF系统模型。我们的分析表明，误差状态模型在计算EKF雅可比矩阵时通过使用每个状态变量的第一个可用估计值可以具有与基本非线性模型相同的可观测性。为了达到这一目的，我们对传统算法做了以下两点修改：首先，状态转移雅可比矩阵 ϕ x t α n 的计算如式(25)所示。这里，我们采用变式形式：

ϕ x t α n = [ I 2 J ( P ^ x t + 1 | t α n − P ^ x t | t − 1 α n ) 0 1 × 2 1 ] (40)

与式(25)不同的是，这里使用的是机器人位置信息的预测值 P ^ x t | t − 1 α n ，而不是估计值 P ^ x t | t α n 。其次，对于观测到的雅可比矩阵 H x t α n 的计算，我们通常使用第一次观测到的路标的值。因此，如果在时间l处第一次观测到一个路标，我们将计算观测到的机器人位姿的雅可比矩阵如下：

H x t α n = ( ∇ h t ) C T ( ϕ ^ x t | t − 1 ) ⋅ [ − I 2 − J ( P ^ L l | l − P ^ x t | t − 1 α n ) ] (41)

其中 P ^ L l | l 是路标的第一个观测值。

基于上述的修改，在滤波算法的雅可比矩阵的计算中，只会出现所有地标位置的第一次观测值和机器人姿态的先验估计。它将影响可观测矩阵Q，这里将得到一个新的矩阵 Q ′ ，这种方法被称为第一估计雅可比矩阵EKF(FEJ-EKF)算法。通过替换传统的FastSLAM算法，得到了本文提出的改进算法。

Q = [ − I 2 − J ( P ^ L l | l − P ^ x t | t − 1 α n ) I 2 − I 2 − J ( P ^ L l | l − P ^ x t | t − 1 α n ) I 2 − I 2 − J ( P ^ L l | l − P ^ x t | t − 1 α n ) I 2 ⋮ ⋮ ⋮ − I 2 − J ( P ^ L l | l − P ^ x t | t − 1 α n ) I 2 ] (42)

我们可以了解到这个矩阵的秩是2。因此，FEJ-EKF算法是基于一个不可观测子空间维数为3的误差状态模型。我们强调FEJ-EKF估计器可以在实际应用中实现，因为它确实不利用任何状态真值。

更新地图后，计算粒子的重要权重

ϖ t [ α n ] = p ( x 1 : t [ α n ] | z 1 : t , u 1 : t , K t ) p ( x 1 : t [ α n ] | z 1 : t − 1 , u 1 : t , K t − 1 ) (43)

最后，在重采样阶段，我们引入了一种采样方法Kullback-Leibler Divergence (KLD) sampling [ 20 ] ，它可以随时改变粒子数，以确定采样过程中保持近似质量所需的最小粒子数。KLD采样的核心思想是：假设从a个有效子空间的离散分布中采样n个粒子，从每个子空间中采样的粒子数表示为向量 X = { x 1 , x 2 , ⋯ , x a } ，子空间对应粒子的真实后验概率为向量 P = { P 1 , P 2 , ⋯ , P a } ，然后用 P ^ = { P ^ 1 , P ^ 2 , ⋯ , P ^ a } = n − 1 X 得到P的最大似然估计，最后得到P的似然概率统计值 γ n 为

lg γ n = ∑ i = 1 a x i lg ( P ^ i P i ) = n ∑ i = 1 a P ^ i lg ( P ^ i P i ) (44)

其中 x i 是第i个子空间中采样的粒子数； P i 是第i个子空间的真实后验概率； P ^ i 是第i个子空间的最大似然估计；n为粒子总数；a是有效子空间的个数。

定义1. (KLD [ 20 ] )真实后验分布P与最大似然估计 P ^ 之间的差值称为Kullback-Leibler散度(KLD)，并且表示为

κ ( P , P ^ ) = ∑ i = 1 a P ^ i lg ( P ^ i P i ) (45)

κ ( P , P ^ ) ≥ 0 ，并且当且仅当两个分布相同时， κ ( P , P ^ ) 等于0。

由(44)和(45)可得

lg γ n = n κ ( P , P ^ ) (46)

当粒子总数n趋于正无穷大时，P的似然概率统计量收敛于 a − 1 个自由度的卡方分布，即

2 lg γ n = 2 n κ ( P , P ^ ) → χ a − 1 2 ,     n → ∞ (47)

KLD小于或等于 ε 的概率定义为 P P ( κ ( P , P ^ ) ≤ ε ) ，所以

P P ( κ ( P , P ^ ) ≤ ε ) = P P ( 2 n κ ( P , P ^ ) ≤ 2 n ε ) = P P ( χ a − 1 2 ≤ 2 n ε ) (48)

其中 ε 是假设P是真实分布下的最小阈值。 χ a − 1 2 的分位数可表示为

P P ( χ a − 1 2 ≤ χ a − 1 , 1 − δ 2 ) = 1 − δ (49)

其中， χ a − 1 , 1 − δ 2 为 χ a − 1 2 在标准正态分布上限 1 − δ 下的分布。选择适当的n，使 2 n ε = χ a − 1 , 1 − δ 2 ，然后可得

P P ( κ ( P , P ^ ) ≤ ε ) = 1 − δ (50)

为了计算n的值，我们可以使用一种很好的估计方法——Wilson-Hilferty变换 [ 21 ] 来近似n的值，得到

n = 1 2 ε χ a − 1 , 1 − δ 2 ≐ a − 1 2 ε { 1 − 2 q ( a − 1 ) + 2 q ( a − 1 ) z 1 − δ } 3 (51)

其中 z 1 − δ 为标准正态分布的上限 1 − δ 的分位数。

这导致了在KLD上具有 ε 上限的近似离散分布所需的样本量的推导。由式(19)可知，n与误差范围 ε 成反比。

在重采样过程中，我们首先对粒子标签数据进行融合，以计算是否添加了新的粒子。如果是，标签检测模型将重新分配粒子的权重；如果不是，则执行低方差重采样。最后，利用KLD采样确定粒子重采样过程中所需的粒子总数，实现了粒子总数的动态修改，降低了算法的计算复杂度。然后根据粒子的权重进行采样。