基于一致性的FastSLAM算法的优化

Optimization of FastSLAM Algorithm Based on Consistency

摘要: 本文从可观测性的角度研究FastSLAM算法的一致性问题并对算法进行了改进。相比较于传统的FastSLAM算法，一种改进的FastSLAM算法被提出从而获得更好的一致性性能。首先，在重要性采样阶段，对象标记法用于清晰地标注单个粒子状态。此外，在地图估计阶段，将第一估计雅可比矩阵(FEJ)与扩展卡尔曼滤波相结合以此提高了算法的一致性。最后，通过仿真实例验证了改进的FastSLAM算法的有效性。

Abstract: In this paper, the consistency of FastSLAM algorithm is improved from the perspective of observability. Rather than the traditional ones, an improved FastSLAM algorithm is proposed to achieve better consistent performance of FastSLAM. First, in the importance sampling phase, the object labeling method is used to clearly label individual particle states. Moreover, in the stage of map estimation, the First Estimates Jacobian (FEJ) is combined with the extended Kalman filtering to improve the consistency of the algorithm. Finally, the effectiveness of the proposed improved FastSLAM algorithm is validated through a simulation example.

1. 引言

同步定位与建图(SLAM)是近年来移动机器人领域的关键问题。随着移动机器人执行任务时面临的环境越来越复杂，SLAM的研究和发展已经吸引越来越多专家、学者和社会的关注。

SLAM问题 [1] ，顾名思义，可以分解为两个问题：1) 确定自己的位姿；2) 周边环境特征建模。SLAM问题的实现大致可以分为两类：一类是基于贝叶斯概率模型，另一类是基于非概率模型。而基于贝叶斯概率模型的方法，可以从扩展卡尔曼滤波、无迹卡尔曼滤波和粒子滤波的角度分析和解决SLAM问题 [2] 。在EKF-SLAM问题中，EKF可以帮助处理移动机器人同时定位和建图过程中的不确定性，但也容易造成计算量大和不一致性的问题。为了减小线性化误差，UKF-SLAM [3] 被提出，但外部干扰的影响会导致UKF-SLAM算法的精度和鲁棒性降低。为了克服基于卡尔曼滤波的SLAM算法的一些问题，Montemerlo等 [4] 提出了基于粒子滤波的FastSLAM算法。FastSLAM算法采用粒子集表示机器人的位姿，并使用EKF更新环境地图的特征 [5] 。

2001年Julier和Uhlmann [6] 首次提出了SLAM的一致性问题。由于SLAM是一个非线性问题，计算出的协方差不能保证与实际估计误差匹配。因此，一致性问题对于SLAM系统的准确性和鲁棒性非常重要。在 [7] 中，我们可以了解到，如果估计误差(i)为零均值，且(ii)协方差矩阵小于或等于由滤波计算出的协方差矩阵，则状态估计器是一致的。近年来基于卡尔曼滤波的SLAM算法一致性研究为解决这一问题提供了新的思路。2004年，J. A. Castellanos [8] 提出了一种新的基于EKF-robocentric mapping的SLAM算法，大大降低了线性化误差，提高了建图的一致性。2017年，黄国权 [9] 通过将正则雅可比矩阵直接投影到信息可用子空间中，或通过寻找最优线性化点间接计算适当的滤波雅可比矩阵，使滤波器仅从每个源的测量中获得实际可用信息，从而大大提高了一致性。

FastSLAM算法结合了粒子滤波和扩展卡尔曼滤波的优点，对SLAM系统进行了分析和研究，因此FastSLAM算法的一致性也是一个值得关注的问题。基于以上讨论，本文将开发一种改进的FastSLAM算法，通过改进扩展卡尔曼滤波算法在地图估计阶段的一致性，以及在采样阶段使用目标标记方法标记单个粒子，增强SLAM系统的一致性。主要贡献如下：1) 提出了一种改进了一致性的FastSLAM算法。2) 提出了一种新的目标标记方法，可以清晰地标记单个粒子的状态。3) 为了提高SLAM过程中估计量的一致性，提出了第一估计雅可比矩阵(FEJ)扩展卡尔曼滤波算法。4) 在重采样阶段提出了Kullback-Leibler散度(KLD)采样方法，动态改变粒子总数。本文剩余部分内容的组织框架如下：第2节介绍了传统的FastSLAM算法。第3节详细介绍了改进的FastSLAM算法，并且这种改进的算法通过改变采样方法，使用每个状态变量的第一个可用估计来计算滤波器雅可比矩阵，从而提高了算法的一致性。第4节介绍了FastSLAM算法的一致性测量方法。此外，在第5节中给出了仿真，以验证改进FastSLAM算法的有效性。最后，在本篇文章的第6节给出总结。

2. 传统的FastSLAM算法

粒子滤波是一种用样本来表示概率密度分布的蒙特卡罗方法。该方法可以用在任何状态模型中，并且当样本空间面积无限大时，可以用来近似任何形式的概率密度分布 [10] 。Murphy等人 [11] 引入了一种更有效的Rao-Blackwelized粒子滤波(RBPF)来解决SLAM问题，更有效地利用了粒子滤波，为SLAM的研究提供了理论基础。RBPF是一种逼近贝叶斯解的方法，贝叶斯方法可以保持多个假设，具有很强的鲁棒性。Montemerlo等人 [4] [12] 提出了基于粒子滤波的FastSLAM算法。FastSLAM算法的基本思想是将联合后验概率分布分解为机器人路径部分和以机器人路径为条件的建图部分。传统FastSLAM算法的主要过程主要包括以下四个步骤：位姿采样与更新、地图更新、粒子权重更新、粒子重采样 [5] ，如图1所示。

Figure 1. Traditional FastSLAM algorithm flow chart

图1. 传统的FastSLAM算法流程图

2.1. 姿态采样与更新

FastSLAM算法是一种基于粒子滤波的SLAM方法。在FastSLAM中，一个粒子代表一个机器人的潜在轨迹和环境地图。具体地，一个粒子包含了机器人的位姿 $x_t$ 和与之对应的M个路标估计器 $l_i,i=1,\cdots ,M$ ，这些估计器用于跟踪机器人与特定路标之间的相对位置。SLAM后验估计分成两部分：机器人自身定位问题和基于条件独立的M个相互独立路标的估计问题后验估计 [11] 。

$p\left(x_{1:t},l_{1:M}|z_{1:t},u_{1:t},K_t\right)=p\left(x_{1:t}|z_{1:t},u_{1:t},K_t\right)\cdot {\displaystyle \underset{i=1}{\overset{M}{\prod }}p}\left(l_i|x_{1:t},z_{1:t},u_{1:t},K_t\right)$ (1)

其中 $z_{1:t}$ 为时间步骤1到t的观测值， $u_{1:t}$ 为时间步骤1到t的控制输入， $K_t=\left\{k_1,\cdots ,k_t\right\}$ 为时刻t感知到的地标指数集。

$X_t={\left\{x_{1:t}^{\left[n\right]}\right\}}_n=\left\{x_1^{\left[n\right]},x_2^{\left[n\right]},\cdots ,x_t^{\left[n\right]}\right\}$ (2)

一个粒子 $x_{1:t}^{\left[n\right]}\in X_t$ 表示一个机器人的位姿。我们使用上标符号 $\left[n\right]$ 来表示集合中的第n个粒子。在FastSLAM过程中，利用粒子滤波算法计算机器人位姿的后验概率分布，并进行重要性采样。然而，后验概率分布一般是事先不知道的，因此很难从后验概率分布中进行采样。因此，假设一个概率分布函数 $x_t^{\left[n\right]}~p\left(X_t|x_{t-1}^{\left[n\right]},u_t\right)$ ，它尽可能接近后验概率分布。这种概率分布函数称为提议分布或重要性密度函数。

2.2. 更新地图

FastSLAM使用EKF来执行有条件的地标位置估计和更新地图。

$p\left(l_i|x_{1:t},z_{1:t},u_{1:t},K_t\right)$ (3)

FastSLAM算法中的完整后验路径和地标位置由集合 $X_t$ 表示，其中 ${\lambda }_m^{\left[n\right]}$ 和 ${\Theta }_m^{\left[n\right]}$ 表示和第n个粒子相关的第m个地标 $l_i$ 的均值和协方差。

$X_t={\left\{x_{1:t}^{\left[n\right]},{\lambda }_1^{\left[n\right]},{\Theta }_1^{\left[n\right]},\cdots ,{\lambda }_m^{\left[n\right]},{\Theta }_m^{\left[n\right]}\right\}}_n$ (4)

可以根据是否 $k_t=m$ 来计算第m个地标位置 $l_m$ 的后验，即在时间t时是否观察到 $l_m$ 。

对于 $k_t=m$ ，我们得到

$p\left(l_m|x_{1:t},z_{1:t},u_{1:t},K_t\right)$

$\stackrel{Bayes}{\propto }p\left(z_t|l_m,x_{1:t},z_{1:t-1},u_{1:t},K_t\right)p\left(l_m|x_{1:t},z_{1:t-1},u_{1:t},K_t\right)$ (5)

$\stackrel{Markov}{=}p\left(z_t|l_m,x_t,k_t\right)p\left(l_m|x_{1:t-1},z_{1:t-1},u_{1:t-1},K_{t-1}\right)$ (6)

对于 $k_t\ne m$ ，我们简单地保持高斯函数不变：

$p\left(l_m|x_{1:t},z_{1:t},u_{1:t},K_t\right)=p\left(l_m|x_{1:t-1},z_{1:t-1},u_{1:t-1},K_{t-1}\right)$ (7)

在FastSLAM算法中，使用EKF来更新方程(5)。机器人的运动模型使用粒子滤波来估计，而测量模型则使用EKF来近似，从而得到一个完全高斯分布 $p\left(l_m|x_{1:t},z_{1:t},u_{1:t},K_t\right)$ ，以便更好地估计机器人的位置和地图的状态。这是利用采样来近似机器人姿态分布的结果 [4] 。

2.3. 重采样粒子

在最后一步，在更新完所有N个粒子后，计算粒子的权重系数 ${\varpi }_t^{\left[n\right]}$ ，粒子的权重系数是由目标分布(target distribution)和提议分布(proposal distribution)的比值组成，然后通常计算有效样本大小 $N_{eff}=\frac{1}{{\displaystyle \underset{n=1}{\overset{N}{\sum }}{\left({\varpi }_t^{\left[n\right]}\right)}^2}}$ ，并选择一个阈值 $N_{th}$ ，如果 $N_{eff}<N_{th}$ ，表示粒子差异较大，则进行重采样，去除权重系数较小的粒子。

粒子权重系数的计算公式如下：

${\varpi }_t^{\left[n\right]}=\frac{p\left(x_{1:t}^{\left[n\right]}|z_{1:t},u_{1:t},K_t\right)}{p\left(x_{1:t}^{\left[n\right]}|z_{1:t-1},u_{1:t},K_{t-1}\right)}$ (8)

3. 改进的FastSLAM算法

注意到传统的FastSLAM算法受到线性化和重采样引起的粒子退化和粒子贫乏问题 [10] 的困扰，以及由这些问题引起的不一致等情况。因此，我们迫切需要开发一种高效、新颖的FastSLAM算法。对于传统FastSLAM算法不一致问题的改进或解决，主要是从改进算法重采样 [13] [14] 的角度着手。结合改进的重采样方法，提出了一种将改进的粒子采样和地图估计相结合的FastSLAM算法。改进后的算法将进一步保证算法的准确性和一致性。改进后的FastSLAM算法伪代码如图2所示。

3.1. 采样新位姿

在粒子采样过程中，依赖于足够数量的粒子的算法确实可以提高算法的精度，但与此同时却也增加了计算复杂度，效率较低。与传统的FastSLAM算法相比，为了保持足够高的算法精度和较小的计算复杂度，该算法进行了改进。我们给采样的粒子添加标签，并根据标签信息修改粒子的权重，这进一步提高了新粒子靠近正确轨迹的概率，因此保证了定位的准确性和效率。

假设粒子数为S。由于在FastSLAM中，一个粒子代表一个机器人的潜在轨迹和环境映射，我们在机器人状态 $x\in X$ 后加上一个标签 $s\in S$ 。一般来说，标签s是离散标签空间 $S=\left\{{\alpha }_n:n\in N\right\}$ 中的一个元素，它的元素 ${\alpha }_j$ 是不同的。此外，空间N表示正整数的集合。对于粒子集，每个粒子 $x\in X$ 都有唯一的标签。因此，标签的RFS [15] 是空间 $X\times S$ 上的有限集值随机变量，其中标签空间的所有粒子都包含唯一的标签。

如 [16] 中所介绍的，为每个粒子分配一个标签 $s=\left(t,n\right)$ ，每个粒子是一对出生时间t和唯一的指标 $n\in N$ ，因此，t时刻新生物体的标记状态空间为 $S_t=t\times N$ ，t时刻之前生成的所有物体的标记状态空间为 $S_{0:t-1}$ 。由于新对象的标签集与现有对象不相交，故t时刻所有对象的标签空间为 $S_{0:t}=S_{0:t-1}\cup S_t$ 。

基于姿态估计，机器人姿态估计对应M个路标估计器 $l_i,i=1,\cdots ,M$

$p\left(x_{1:t}^{{\alpha }_n},l_{1:M}|z_{1:t},u_{1:t},K_t\right)=p\left(x_{1:t}^{{\alpha }_n}|z_{1:t},u_{1:t},K_t\right)\cdot {\displaystyle \underset{i=1}{\overset{M}{\prod }}p}\left(l_i|x_{1:t}^{{\alpha }_n},z_{1:t},u_{1:t},K_t\right)$ (9)

机器人的位姿表示为

$X_t^S={\left\{x_{1:t}^{\left[{\alpha }_n\right]}\right\}}_n={\left\{x_1^{\left[{\alpha }_n\right]},x_2^{\left[{\alpha }_n\right]},\cdots ,x_t^{\left[{\alpha }_n\right]}\right\}}_n$ (10)

Figure 2. Improved FastSLAM algorithm pseudo code

图2. 改进的FastSLAM算法伪代码

3.2. 更新对于每个粒子的地标位置估计

在运动模型中采样新的位姿后，更新每个粒子中地标位置的估计。在这一步中，我们结合FEJ的思想来提高地标位置估计的准确性。

在FastSLAM中，利用EKF计算了标记位置 $P\left(l_i|x_{1:t}^{{\alpha }_n},z_{1:t},u_{1:t},K_t\right)$ 的估计值，很容易得到第i个标记位置 $l_i$ 的后验估计值。其计算取决于在时刻t是否观测到地标 $l_i$ ，即是否满足条件 $i=k_t$ 。

如果在t时刻观察到地标，即 $i=k_t$ ，则

$P\left(l_{i=k_t}|x_{1:t}^{{\alpha }_n},z_{1:t},u_{1:t},K_t\right)\propto P\left(z_{1:t}|l_{k_t},x_t,k_t\right)\cdot P\left(l_{k_t}|x_{1:t-1},z_{1:t-1},u_{1:t-1},K_{t-1}\right)$ (11)

如果在t时刻未观察到地标，即 $i\ne k_t$ ，则：

$P\left(l_{i\ne k_t}|x_{1:t}^{{\alpha }_n},z_{1:t},u_{1:t},K_t\right)\propto P\left(l_i|x_{1:t-1}^{{\alpha }_n},z_{1:t-1},u_{1:t-1},K_{t-1}\right)$ (12)

3.2.1. EKF-SLAM算法的预测与更新

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

${X^{\prime }}_t=\left[\begin{array}{c}{P}_{x_t}^{{\alpha }_n}\\ {\varphi }_{x_t}^{{\alpha }_n}\\ P_L\end{array}\right]=\left[\begin{array}{c}{x}_t^{{\alpha }_n}\\ P_L\end{array}\right]$ (13)

其中 $x_t^{{\alpha }_n}$ 表示机器人的位姿(包括位置和方向)， $P_L$ 表示地标的位置。EKF-SLAM递归遵循两个步骤：预测和更新，这两个步骤分别基于离散时间过程和观测模型 [10] 。EKF的预测方程为：

${\hat{P}}_{x_{t+1|t}}^{{\alpha }_n}={\hat{P}}_{x_{t|t}}^{{\alpha }_n}+C\left({\hat{\varphi }}_{x_{t|t}}^{{\alpha }_n}\right){}^{x_t}\hat{P}{}_{x_{t+1}}^{{\alpha }_n}$ (14)

${\hat{\varphi }}_{x_{t+1|t}}^{{\alpha }_n}={\hat{\varphi }}_{x_{t|t}}^{{\alpha }_n}+{}^{x_t}\hat{\varphi }{}_{x_{t+1}}^{{\alpha }_n}$ (15)

${\hat{P}}_{L_{t+1|t}}={\hat{P}}_{L_{t|t}}$ (16)

其中 $C\left(\theta \right)=\left[\begin{array}{cc}\text{cos}\theta & -\text{sin}\theta \\ \text{sin}\theta & \text{cos}\theta \end{array}\right]$ 为2*2的旋转矩阵， ${}^{x_t^{{\alpha }_n}}\hat{X}{}_{t+1}^{{\alpha }_n}=\left[\begin{array}{l}{}^{x_t^{{\alpha }_n}}\hat{P}{}_{x_{t+1}}^{{\alpha }_n}\\ {}^{x_t^{{\alpha }_n}}\hat{\varphi }{}_{x_{t+1}}^{{\alpha }_n}\end{array}\right]$ 为机器人从t时刻到 $t+1$ 时刻基于里程

的运动估计。该估计被高斯白噪声 $w_t={}^{x_t^{{\alpha }_n}}X{}_{t+1}^{{\alpha }_n}-{}^{x_t^{{\alpha }_n}}\hat{X}{}_{t+1}^{{\alpha }_n}$ 所破坏，该噪声的均值为0，协方差矩阵为 $Q_t$ 。

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

$P_{x_{t+1|t}}^{{\alpha }_n}=P_{x_t}^{{\alpha }_n}+C\left({\varphi }_{x_t}^{{\alpha }_n}\right){}^{x_t^{{\alpha }_n}}P{}_{x_{t+1}}^{{\alpha }_n}$ (17)

${\varphi }_{x_{t+1|t}}^{{\alpha }_n}={\varphi }_{x_t}^{{\alpha }_n}+{}^{x_t^{{\alpha }_n}}\varphi {}_{x_{t+1}}^{{\alpha }_n}$ (18)

$P_{L_{t+1|t}}=P_{L_t}$ (19)

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

${\tilde{P}}_{x_{t+1|t}}^{{\alpha }_n}={\tilde{P}}_{x_{t|t}}^{{\alpha }_n}+C\left({\varphi }_{x_t}^{{\alpha }_n}\right){}^{x_t^{{\alpha }_n}}P{}_{x_{t+1}}^{{\alpha }_n}-C\left({\hat{\varphi }}_{x_{t|t}}^{{\alpha }_n}\right){}^{x_t^{{\alpha }_n}}\hat{P}{}_{x_{t+1}}^{{\alpha }_n}$ (20)

${\tilde{\varphi }}_{x_{t+1|t}}^{{\alpha }_n}={\tilde{\varphi }}_{x_{t|t}}^{{\alpha }_n}+{}^{x_t^{{\alpha }_n}}\tilde{\varphi }{}_{x_{t+1}}^{{\alpha }_n}$ (21)

${\tilde{P}}_{L_{t+1|t}}={\tilde{P}}_{L_{t|t}}$ (22)

显然，式(20)是非线性的，所以我们在估计状态 ${\hat{\varphi }}_{x_{t|t}}^{{\alpha }_n}$ 和 ${}^{x_t^{{\alpha }_n}}\hat{P}{}_{x_{t+1}}^{{\alpha }_n}$ 将其线性化：

${\tilde{P}}_{x_{t+1|t}}^{{\alpha }_n}={\tilde{P}}_{x_{t|t}}^{{\alpha }_n}+JC\left({\hat{\varphi }}_{x_t}^{{\alpha }_n}\right){}^{x_t^{{\alpha }_n}}\hat{P}{}_{x_{t+1}}^{{\alpha }_n}{\tilde{\varphi }}_{x_{t|t}}^{{\alpha }_n}+C\left({\hat{\varphi }}_{x_{t|t}}^{{\alpha }_n}\right){}^{x_t^{{\alpha }_n}}\tilde{P}{}_{x_{t+1}}^{{\alpha }_n}$ (23)

其中 $J=\left[\begin{array}{cc}0& -1\\ 1& 0\end{array}\right]$ 。将误差状态预测方程(21)，(22)，(23)转化为分块矩阵，得到如下所熟悉的方程：

$\begin{array}{c}{{\tilde{X}}^{\prime }}_{t+1|t}=\left[\begin{array}{cc}{\varphi }_{x_t}^{{\alpha }_n}& 0_{3\times 2}\\ 0_{2\times 3}& I_2\end{array}\right]\left[\begin{array}{c}{\tilde{X}}_{t|t}^{{\alpha }_n}\\ {\tilde{P}}_{L_{t|t}}\end{array}\right]+\left[\begin{array}{c}{G}_{x_t}^{{\alpha }_n}\\ 0_{2\times 2}\end{array}\right]w_t\\ ={\varphi }_t{{\tilde{X}}^{\prime }}_{t|t}+G_t{w}_t\end{array}$ (24)

其中，状态转移矩阵 ${\varphi }_{x_t}^{{\alpha }_n}$ 和 $G_{x_t}^{{\alpha }_n}$ 具体表示为

$\begin{array}{c}{\varphi }_{x_t}^{{\alpha }_n}=\left[\begin{array}{cc}{I}_2& JC\left({\hat{\varphi }}_{x_{t|t}}^{{\alpha }_n}\right){}^{x_t^{{\alpha }_n}}\hat{P}{}_{x_{t+1}}^{{\alpha }_n}\\ 0_{1\times 2}& 1\end{array}\right]\\ \equiv \left[\begin{array}{cc}{I}_2& J\left({\hat{P}}_{x_{t+1|t}}^{{\alpha }_n}-{\hat{P}}_{x_{t|t}}^{{\alpha }_n}\right)\\ 0_{1\times 2}& 1\end{array}\right]\end{array}$ (25)

$G_{x_t}^{{\alpha }_n}=\left[\begin{array}{cc}C\left({\hat{\varphi }}_{x_{t|t}}^{{\alpha }_n}\right)& 0_{2\times 1}\\ 0_{1\times 2}& 1\end{array}\right]$ (26)

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

$\begin{array}{c}{z}_t=h\left({X^{\prime }}_t\right)+v_t\\ =h\left(C^{\text{T}}\left({\varphi }_{x_t}^{{\alpha }_n}\right)\left(P_{L_t}-P_{x_t}^{{\alpha }_n}\right)\right)+v_t\end{array}$ (27)

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

$\begin{array}{c}{z}_t=\left[\begin{array}{cc}{H}_{x_t}^{{\alpha }_n}& H_{L_t}\end{array}\right]\left[\begin{array}{c}{\tilde{X}}_{t|t-1}^{{\alpha }_n}\\ {\tilde{X}}_{L_{t|t-1}}\end{array}\right]+v_t\\ =H_t{{\tilde{X}}^{\prime }}_{t|t-1}+v_t\end{array}$ (28)

式中， $H_{x_t}^{{\alpha }_n}$ 和 $H_{L_t}$ 分别为函数 ${{\hat{X}}^{\prime }}_{t|t-1}$ 关于机器人位姿和地标位置在状态估计 ${{\hat{X}}^{\prime }}_{t|t-1}$ 处估计的雅可比矩阵。根据链式法则，我们得到

$H_{x_t}^{{\alpha }_n}=\left(\nabla h_t\right)C^{\text{T}}\left({\hat{\varphi }}_{x_{t|t-1}}^{{\alpha }_n}\right)\cdot \left[\begin{array}{cc}-I_2& -J\left({\hat{P}}_{L_{t|t-1}}-{\hat{P}}_{x_{t|t-1}}^{{\alpha }_n}\right)\end{array}\right]$ (29)

$H_{L_t}=\left(\nabla h_t\right)C^{\text{T}}\left({\hat{\varphi }}_{x_{t|t-1}}^{{\alpha }_n}\right)$ (30)

其中 $\nabla h_t$ 表示函数h关于相对机器人的地标位置的雅可比矩阵(也就是说，关于 ${}^{x_t^{{\alpha }_n}}P{}_L=C^{\text{T}}\left({\varphi }_{x_t}^{{\alpha }_n}\right)\cdot \left(P_L-P_{x_t}^{{\alpha }_n}\right)$ 在估计值 ${{\hat{X}}^{\prime }}_{t|t-1}$ 的雅可比矩阵)。

3.2.2. EKF-SLAM的可观测性分析

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

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

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

$\Lambda =\left[\begin{array}{c}{H}_t\\ H_{t+1}{\varphi }_{x_t}^{{\alpha }_n}\\ ⋮\\ H_{t+m}{\varphi }_{x_{t+m-1}}^{{\alpha }_n}\cdots {\varphi }_{x_t}^{{\alpha }_n}\end{array}\right]$ (31)

这个公式可以进一步推广通过代入 ${\varphi }_i$ 和 $H_i$ (方程(24)和(28))，可以得到

$\Lambda =\left[\begin{array}{cc}{H}_{x_t}^{{\alpha }_n}& H_{L_t}\\ H_{x_{t+1}}^{{\alpha }_n}{\varphi }_{x_t}^{{\alpha }_n}& H_{L_{t+1}}\\ ⋮& ⋮\\ H_{x_{t+m}}^{{\alpha }_n}{\varphi }_{x_{t+m-1}}^{{\alpha }_n}\cdots {\varphi }_{x_t}^{{\alpha }_n}& H_{L_{t+m}}\end{array}\right]$ (32)

$=\text{diag}\left(H_{L_t},\cdots H_{L_{t+m}}\right)\cdot \underset{Q}{\left[\begin{array}{cc}{H}_{L_t}^{-1}{H}_{x_t}^{{\alpha }_n}& I_2\\ H_{L_{t+1}}^{-1}{H}_{x_{t+1}}^{{\alpha }_n}{\varphi }_{x_t}^{{\alpha }_n}& I_2\\ ⋮& ⋮\\ H_{L_{t+m}}^{-1}{H}_{x_{t+m}}^{{\alpha }_n}\cdots {\varphi }_{x_{t+m-1}}^{{\alpha }_n}& I_2\end{array}\right]}$ (33)

其中 $\text{diag}(\cdot )$ 为块对角化矩阵。在时间间隔t和 $t+m$ 之间，系统是局部可观的，当且仅当局部可观矩阵 $\Lambda$ 是满秩的。 $\text{diag}\left(H_{L_t},\cdots ,H_{L_{t+m}}\right)$ 是非奇异的。

3.2.3. 标准EKF-SLAM的可观性

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

$\begin{array}{c}{\varphi }_{x_{t+1}}^{{\alpha }_n}{\varphi }_{x_t}^{{\alpha }_n}=\left[\begin{array}{cc}{I}_2& J\left({\hat{P}}_{x_{t+2|t+1}}^{{\alpha }_n}-{\hat{P}}_{x_{t+1|t+1}}^{{\alpha }_n}\right)\\ 0_{1\times 2}& 1\end{array}\right]\cdot \left[\begin{array}{cc}{I}_2& J\left({\hat{P}}_{x_{t+1|t}}^{{\alpha }_n}-{\hat{P}}_{x_{t|t}}^{{\alpha }_n}\right)\\ 0_{1\times 2}& 1\end{array}\right]\\ =\left[\begin{array}{cc}{I}_2& G\\ 0_{1\times 2}& 1\end{array}\right]\end{array}$ (34)

其中 $G=J\left({\hat{P}}_{x_{t+2|t+1}}^{{\alpha }_n}-{\hat{P}}_{x_{t|t}}^{{\alpha }_n}+\left({\hat{P}}_{x_{t+1|t}}^{{\alpha }_n}-{\hat{P}}_{x_{t+1|t+1}}^{{\alpha }_n}\right)\right)$

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

${\varphi }_{x_{t+i-1}}^{{\alpha }_n}{\varphi }_{x_{t+i-2}}^{{\alpha }_n}\cdots {\varphi }_{x_t}^{{\alpha }_n}=\left[\begin{array}{cc}{I}_2& D\\ 0_{1\times 2}& 1\end{array}\right]$ (35)

$D=J\left({\hat{P}}_{x_{t+i|t+i-1}}^{{\alpha }_n}-{\hat{P}}_{x_{t|t}}^{{\alpha }_n}-{\displaystyle \underset{j=t+1}{\overset{t+i-1}{\sum }}\left({\hat{P}}_{x_{j|j}}^{{\alpha }_n}-{\hat{P}}_{x_{j|j-1}}^{{\alpha }_n}\right)}\right)$ ，且 $i>0$ 。结合(28)，(29)，(30)，得到

$\begin{array}{c}{H}_{L_{t+i}}^{-1}{H}_{x_{t+i}}^{{\alpha }_n}{\varphi }_{x_{t+i-1}}^{{\alpha }_n}\cdots {\varphi }_{x_t}^{{\alpha }_n}=-C^{-\text{T}}\left({\varphi }_{x_{t+i|t+i-1}}^{{\alpha }_n}\right){\left(\nabla h_{t+i}\right)}^{-1}\left(\nabla h_{t+i}\right)\cdot C^{\text{T}}\left({\varphi }_{x_{t+i|t+i-1}}^{{\alpha }_n}\right)\left[\begin{array}{cc}{I}_2& J\left(P_{L_{t+i|t+i-1}}-P_{x_{t+i|t+i-1}}^{{\alpha }_n}\right)\end{array}\right]\left[\begin{array}{cc}{I}_2& E\\ 0_{1\times 2}& 1\end{array}\right]\\ =-\left[\begin{array}{cc}{I}_2& F\end{array}\right]\end{array}$ (36)

其中

$\begin{array}{l}E=J\left({\hat{P}}_{x_{t+i|t+i-1}}^{{\alpha }_n}-{\hat{P}}_{x_{t|t}}^{{\alpha }_n}+{\displaystyle \underset{j=t}{\overset{t+i-2}{\sum }}\left({\hat{P}}_{x_{j+1|j}}^{{\alpha }_n}-{\hat{P}}_{x_{j+1|j+1}}^{{\alpha }_n}\right)}\right),\\ F=J\left(P_{L_{t+i|t+i-1}}-{\hat{P}}_{x_{t|t}}^{{\alpha }_n}+{\displaystyle \underset{j=t}{\overset{t+i-2}{\sum }}\left({\hat{P}}_{x_{j+1|j}}^{{\alpha }_n}-{\hat{P}}_{x_{j+1|j+1}}^{{\alpha }_n}\right)}\right)\end{array}$

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

$Q=\left[\begin{array}{ccc}-I_2& A_1& I_2\\ -I_2& A_2& I_2\\ -I_2& A_3& I_2\\ ⋮& ⋮& ⋮\\ -I_2& A_k& I_2\end{array}\right]$ (37)

其中

$\begin{array}{l}{A}_1=-J\left({\hat{P}}_{L_{t|t-1}}-{\hat{P}}_{x_{t|t-1}}^{{\alpha }_n}\right)\\ A_2=-J\left({\hat{P}}_{L_{t+1|t}}-{\hat{P}}_{x_{t|t}}^{{\alpha }_n}\right)\\ A_k=-J\left({\hat{P}}_{L_{t+i|t+i-1}}-{\hat{P}}_{x_{t|t}}^{{\alpha }_n}+{\displaystyle \underset{j=t}{\overset{t+i-2}{\sum }}\left({\hat{P}}_{x_{j+1|j}}^{{\alpha }_n}-{\hat{P}}_{x_{j+1|j+1}}^{{\alpha }_n}\right)}\right),k=2,\cdots ,i\end{array}$

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

${\hat{P}}_{L_{t|t-1}}\ne {\hat{P}}_{L_{t+1|t}}\ne \cdots \ne {\hat{P}}_{L_{t+i|t+i-1}}$ (38)

和

${\hat{P}}_{x_{t+1|t}}^{{\alpha }_n}-{\hat{P}}_{x_{t+1|t+1}}^{{\alpha }_n}\ne 0_{2\times 1}$ (39)

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

3.2.4. 改进的EKF-SLAM算法

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

${\varphi }_{x_t}^{{\alpha }_n}=\left[\begin{array}{cc}{I}_2& J\left({\hat{P}}_{x_{t+1|t}}^{{\alpha }_n}-{\hat{P}}_{x_{t|t-1}}^{{\alpha }_n}\right)\\ 0_{1\times 2}& 1\end{array}\right]$ (40)

与式(25)不同的是，这里使用的是机器人位置信息的预测值 ${\hat{P}}_{x_{t|t-1}}^{{\alpha }_n}$ ，而不是估计值 ${\hat{P}}_{x_{t|t}}^{{\alpha }_n}$ 。其次，对于观测到的雅可比矩阵 $H_{x_t}^{{\alpha }_n}$ 的计算，我们通常使用第一次观测到的路标的值。因此，如果在时间l处第一次观测到一个路标，我们将计算观测到的机器人位姿的雅可比矩阵如下：

$H_{x_t}^{{\alpha }_n}=\left(\nabla h_t\right)C^{\text{T}}\left({\hat{\varphi }}_{x_{t|t-1}}\right)\cdot \left[\begin{array}{cc}-I_2& -J\left({\hat{P}}_{L_{l|l}}-{\hat{P}}_{x_{t|t-1}}^{{\alpha }_n}\right)\end{array}\right]$ (41)

其中 ${\hat{P}}_{L_{l|l}}$ 是路标的第一个观测值。

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

$Q=\left[\begin{array}{ccc}-I_2& -J\left({\hat{P}}_{L_{l|l}}-{\hat{P}}_{x_{t|t-1}}^{{\alpha }_n}\right)& I_2\\ -I_2& -J\left({\hat{P}}_{L_{l|l}}-{\hat{P}}_{x_{t|t-1}}^{{\alpha }_n}\right)& I_2\\ -I_2& -J\left({\hat{P}}_{L_{l|l}}-{\hat{P}}_{x_{t|t-1}}^{{\alpha }_n}\right)& I_2\\ ⋮& ⋮& ⋮\\ -I_2& -J\left({\hat{P}}_{L_{l|l}}-{\hat{P}}_{x_{t|t-1}}^{{\alpha }_n}\right)& I_2\end{array}\right]$ (42)

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

3.2.5. 计算重要性权重

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

${\varpi }_t^{\left[{\alpha }_n\right]}=\frac{p\left(x_{1:t}^{\left[{\alpha }_n\right]}|z_{1:t},u_{1:t},K_t\right)}{p\left(x_{1:t}^{\left[{\alpha }_n\right]}|z_{1:t-1},u_{1:t},K_{t-1}\right)}$ (43)

3.2.6. 重采样粒子

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

$\lg {\gamma }_n={\displaystyle \underset{i=1}{\overset{a}{\sum }}{x}_i}\lg \left(\frac{{\hat{P}}_i}{P_i}\right)=n{\displaystyle \underset{i=1}{\overset{a}{\sum }}{\hat{P}}_i}\lg \left(\frac{{\hat{P}}_i}{P_i}\right)$ (44)

其中 $x_i$ 是第i个子空间中采样的粒子数； $P_i$ 是第i个子空间的真实后验概率； ${\hat{P}}_i$ 是第i个子空间的最大似然估计；n为粒子总数；a是有效子空间的个数。

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

$\kappa \left(P,\hat{P}\right)={\displaystyle \underset{i=1}{\overset{a}{\sum }}{\hat{P}}_i}\lg \left(\frac{{\hat{P}}_i}{P_i}\right)$ (45)

$\kappa \left(P,\hat{P}\right)\ge 0$ ，并且当且仅当两个分布相同时， $\kappa \left(P,\hat{P}\right)$ 等于0。

由(44)和(45)可得

$\lg {\gamma }_n=n\kappa \left(P,\hat{P}\right)$ (46)

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

$2\lg {\gamma }_n=2n\kappa \left(P,\hat{P}\right)\to {\chi }_{a-1}^2,n\to \infty$ (47)

KLD小于或等于 $\epsilon$ 的概率定义为 $P_P\left(\kappa \left(P,\hat{P}\right)\le \epsilon \right)$ ，所以

$\begin{array}{c}{P}_P\left(\kappa \left(P,\hat{P}\right)\le \epsilon \right)=P_P\left(2n\kappa \left(P,\hat{P}\right)\le 2n\epsilon \right)\\ =P_P\left({\chi }_{a-1}^2\le 2n\epsilon \right)\end{array}$ (48)

其中 $\epsilon$ 是假设P是真实分布下的最小阈值。 ${\chi }_{a-1}^2$ 的分位数可表示为

$P_P\left({\chi }_{a-1}^2\le {\chi }_{a-1,1-\delta }^2\right)=1-\delta$ (49)

其中， ${\chi }_{a-1,1-\delta }^2$ 为 ${\chi }_{a-1}^2$ 在标准正态分布上限 $1-\delta$ 下的分布。选择适当的n，使 $2n\epsilon ={\chi }_{a-1,1-\delta }^2$ ，然后可得

$P_P\left(\kappa \left(P,\hat{P}\right)\le \epsilon \right)=1-\delta$ (50)

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

$\begin{array}{c}n=\frac{1}{2\epsilon }{\chi }_{a-1,1-\delta }^2\\ \doteq \frac{a-1}{2\epsilon }{\left\{1-\frac{2}{q\left(a-1\right)}+\sqrt{\frac{2}{q\left(a-1\right)}}{z}_{1-\delta }\right\}}^3\end{array}$ (51)

其中 $z_{1-\delta }$ 为标准正态分布的上限 $1-\delta$ 的分位数。

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

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

4. FastSLAM一致性的评估

为了更好地评价估计器的性能，有必要对估计算法进行一致性检查。如 [7] 中所述，假设真值为 $X_{g_k}$ ，那么算法保持一致性必须满足以下条件：

$E\left[X_{g_k}-{\hat{X}}_{g_k}\right]=0$ (52)

$E\left[\left(X_{g_k}-{\hat{X}}_{g_k}\right){\left(X_{g_k}-{\hat{X}}_{g_k}\right)}^{\text{T}}\right]\le P_{g_k}$ (53)

一般来说，为了衡量一个算法是否一致，人们会将其估计值与从理想贝叶斯滤波中获得的概率密度函数(PDF)进行比较。然而，这对于FastSLAM算法是不实际的。当“真实”PDF不可用时，真实状态 $x_k$ 是已知的，我们可以使用归一化估计误差平方(NEES) [7] 来表征滤波器的性能，

${\epsilon }_k={\left(x_k-{\hat{x}}_k\right)}^{\text{T}}{P}_k^{-1}\left(x_k-{\hat{x}}_k\right)$ (54)

其中 $\left\{{\hat{x}}_k,P_k\right\}$ 是估计的均值和协方差。

通过检查N次蒙特卡洛运行的滤波的平均NEES，可以发现滤波算法的一种一致性的度量方法。在滤波算法是一致的且近似线性高斯的假设下， ${\epsilon }_k$ 的值为 ${\chi }^2$ (卡方)，分布自由度为 $\dim \left(x_k\right)$ 。当N趋于无穷大时， ${\epsilon }_k$ 的平均值趋于状态维数。

$E\left[{\epsilon }_k\right]=\text{dim}\left(x_k\right)$ (56)

该假设的有效性可以进行一个 ${\chi }^2$ 可接受性检验。

FastSLAM的一致性是通过执行多次蒙特卡洛运行和计算平均NEES来评估的。给定N次运行，平均NEES计算为

${\bar{\epsilon }}_k=\frac{1}{N}{\displaystyle \underset{i=1}{\overset{N}{\sum }}{\epsilon }_{ik}}$ (57)

给定一个一致的线性–高斯滤波的假设， $N{\bar{\epsilon }}_k$ 的密度为 ${\chi }^2$ ，自由度为 $N\dim \left\{x_k\right\}$ 。

5. 仿真结果

在本节中，我们将通过实验来证明新颖的FastSLAM算法的有效性。FastSLAM算法的关键特性是由嵌入在粒子滤波中的蒙特卡罗采样高度说明的。我们使用了一个小的自生成里程数据和一个图形函数 [22] 。粒子的权重通过新的测量方法不断修正和重估。由于新的测量噪声，特征映射有偏差，粒子权重被外部测量错误地操纵 [23] 。

在图3和图4中，黑色虚线表示最佳加权粒子的轨迹，绿色实线表示真实粒子的轨迹。在这个图中有25个地标和100个粒子，其中红色虚线代表传感器测量线。图3为标准FastSLAM算法中粒子的运动轨迹，图4为理想FastSLAM算法中粒子的运动轨迹。

Figure 3. Particle trajectory in standard FastSLAM algorithm

图3. 标准的FastSLAM算法中粒子的运动轨迹

Figure 4. Particle trajectory in the ideal FastSLAM algorithm

图4. 理想的FastSLAM算法中粒子的运动轨迹

Figure 5. Landmark estimation error of standard FastSLAM algorithm

图5. 标准的FastSLAM算法的地标估计误差

Figure 6. The landmark estimation error of the ideal FastSLAM algorithm

图6. 理想的FastSLAM算法的地标估计误差

Figure 7. The landmark estimation error of the improved FastSLAM algorithm

图7. 改进的FastSLAM算法的地标估计误差

图5~7为地图误差，绘制了观测到的地标及其均方位置误差。一开始，误差幅度是相当大的。红色的误差条表示误差值上下变化。红色误差条越长，不确定性越大。通过FastSLAM中红色误差条的表示，我们可以观察到在很长一段时间内存在相当大的不确定性。图5显示了标准FastSLAM算法的误差，我们可以观察到在很长一段时间内存在不确定性。图6为理想的FastSLAM算法的误差。本文提出的FastSLAM算法的误差如图7所示。我们可以观察到，地标估计的均方误差逐渐收敛到0，这意味着不确定性逐渐变小，最终趋于稳定。这很好地说明了我们提出的算法的有效性。对于机器人的轨迹误差，它是相当稳定的，并很好地限制在小于15 m。

图8中的NEES结果提供了更正式的一致性度量，图8中的三条曲线表示的机器人位姿的NEES随时间变化的曲线，在这些图中，青色代表标准的FastSLAM算法关于机器人位姿的NEES结果，紫色代表理想的FastSLAM算法关于机器人位姿的NEES结果，红色代表改进的FastSLAM算法关于机器人位姿的NEES结果。这些结果是基于机器人的位姿进行计算的，在每种情况下，都可以看到滤波器变得迅速地乐观。总的趋势是，改进的FastSLAM算法的关于机器人的位姿的NEES表现较好，所得到的机器人的位姿估计和地图的一致性得到提高。

Figure 8. NEES on robot pose for three FastSLAM algorithms

图8. 三种FastSLAM算法的关于机器人位姿的NEES

6. 结论

在这篇文章中，我们对FastSLAM算法进行了改进。与传统算法相比，改进算法的优点有三个方面。首先，采用目标标记法，清晰地标记单个粒子的状态，从而对粒子权重进行修正，提高了算法的实时性和精度。其次，基于可观测性条件，在地标估计阶段融入FEJ思想，增强地标估计的一致性，构建满足一致性的地图。第三，在重采样阶段，引入了KLD采样方法，该方法可以随时改变采样过程中的粒子数，从而确定采样过程中保持近似质量所需的最小粒子数，动态改变总粒子数，减少算法的计算量。在实际应用中，这将可能更进一步改善机器人同步定位与建图过程中的定位精确度和所构建地图的一致性，同时在此过程中，算法的计算量也会被降低。