结合多维泰勒网策略优化EKF-SLAM算法一致性的新方法研究

A New Method for Optimizing the Consistency of EKF-SLAM Algorithm Based on Multi-Dimensional Taylor-Net Strategy

摘要: 本文旨在非线性SLAM系统中设计一种改进的EKF-SLAM算法进行状态估计和构建一致性地图。通过引入多维泰勒网和第一估计雅可比矩阵方法，我们提出了一种新颖的估计算法，旨在提高系统的鲁棒性和构建地图的一致性。本研究首先介绍了多维泰勒网的基本原理，数学模型和EKF-SLAM算法的可观性分析，然后详细描述了改进的EKF-SLAM算法的设计和实现过程。我们通过仿真实验验证了该改进算法在非线性SLAM系统中的有效性和优越性，结果表明，该改进算法在对系统状态估计和构建一致地图方面有着很好的效果。

Abstract: This manuscript aims to design an improved EKF-SLAM algorithm for state estimation and consistency map construction in nonlinear SLAM systems. By introducing multi-dimensional Taylor nets and the first estimate Jacobian matrix method, we propose a novel estimation algorithm aimed at improving the robustness of the system and the consistency of the constructed map. This manuscript first introduces the basic principle of multi-dimensional Taylor net, mathematical model and observability analysis of EKF-SLAM algorithm, and then describes the design and implementation process of improved EKF-SLAM algorithm in detail. The effectiveness and superiority of the improved algorithm in nonlinear SLAM system are verified by simulation experiments. The results show that the improved algorithm has a good effect on the estimation of the system and the construction of a consistent map.

1. 引言

在当今科技迅速发展的时代，人工智能开始日益引起人们的关注。无人系统、自动驾驶、机器人导航等领域逐渐吸引越来越多的人进行深入研究并发展。SLAM (同步定位与建图)技术是该领域的一种关键的技术，SLAM的概念最早是由Smith [1] 提出，SLAM技术 [2] [3] 是指在未探索的环境里，移动机器人逐渐独自创建一个与所处环境相符的地图，并用这张地图来确定它在该环境里的位置。SLAM系统是非线性系统，那么对于非线性系统进行更好地估计并构建出更一致性的地图，设计出一种有效的估计算法很有必要。

贝叶斯概率模型是一种基于贝叶斯定理的概率模型，是处理不确定性问题的有力工具。在SLAM中，贝叶斯概率模型方法可以使系统对未知环境有更好的估计，提高系统对环境的适应能力。基于贝叶斯概率模型，SLAM问题可以从扩展卡尔曼滤波(EKF)这个角度进行分析和解决。在EKF-SLAM问题中，EKF可以帮助处理移动机器人同时定位和建图过程中的不确定性，但也容易造成计算量大和不一致的问题。

SLAM的一致性问题最早由Julier和Uhlmann [4] 于2001年提出。由于SLAM是一个非线性问题，不能保证计算出的协方差与实际估计误差相匹配，一致性问题对SLAM系统的精度和鲁棒性至关重要。由 [5] 可知，如果估计误差(i)为零均值，且(ii)协方差矩阵小于或等于滤波计算的协方差矩阵，则状态估计器是一致的。在SLAM领域，扩展卡尔曼滤波(EKF)广泛应用作为一种基础工具。EKF-SLAM技术 [6] [7] [8] [9] 通过状态向量来描述机器人的位置和地标的位置，并用误差协方差矩阵来表征状态的不确定度，其中协方差矩阵反映了机器人位置和地标位置之间的不确定性。在静态机器人对同一地标进行重复测量的情况下，对方向的估计不确定性会减少。但值得注意的是，观测到的地标特征不准确，可能导致EKF-SLAM算法中状态估计的不一致性，这是因为估计出的协方差小于实际值。线性化过程中产生的误差是系统估计不一致的主要根源。Bailey等研究者 [10] 通过蒙特卡洛模拟探讨了这种不一致的程度及EKF-SLAM在何种情况下能够提供可靠结果。Li等人 [11] 在理论上展示了测量噪声和观测时间对算法不一致性的影响，指出随着测量噪声的增加及对地标测量次数的提升，算法的不一致性会不断上升。在执行更新步骤时，通过估计测量噪声分布来得到噪声协方差矩阵，这是由Choi [12] 提出的一种对测量噪声进行估计的EKF-SLAM算法。多维泰勒网(Multi-Dimensional Taylor Network, MTN)是由东南大学教授严洪森于2010年首次提出的具有前瞻性的网络架构。该网络由三层构成，包括输入层、中间层和输出层。在最初的研究阶段，多维泰勒网主要应用于非线性系统的识别与预测问题 [13] 。多维泰勒网具有以下特征 [14] ：(1) 结构简单；(2) 优良的逼近性能；(3) 泛化能力强；(4) 学习能力强。

该改进的算法结合了多维泰勒网和扩展卡尔曼滤波的优点来分析和研究SLAM系统，因此SLAM系统构建地图的一致性也是一个值得关注的问题。在上述讨论的推动下，本文将开发一种改进的EKF-SLAM算法，通过改进扩展卡尔曼滤波算法在地图估计阶段的一致性和系统的鲁棒性，来增强SLAM系统的一致性。主要贡献如下所示：1) 提出了一种改进一致性的EKF-SLAM算法。2) 为了提高SLAM算法的精度，在用EKF估计SLAM系统的状态和构建地图时，结合了第一估计雅可比矩阵(FEJ)和多维泰勒网。本篇文章的整个大纲如下。第二节介绍了SLAM模型。第三节介绍了多维泰勒网的基本原理。第四节对EKF-SLAM算法的能观性进行了分析。第五节中介绍了改进的EKF-SLAM算法。此外，在第六节中给出了仿真，以验证所提出算法的有效性。

记号：我们用 $N^{+}$ 表示正整数空间。 $E(\cdot )$ 为期望值。对于矩阵 $A$ ， $A^{\text{T}}$ 和 $A^{-1}$ 分别表示它的转置和逆。其中 $I_n$ 为维数为n的单位矩阵， $0_{m\times n}$ 为维数为 $m\times n$ 的零矩阵。

2. SLAM模型

SLAM问题，即同步定位与建图，顾名思义，主要解决两个问题，估计自己的位姿和构建周围环境地图，那么就需要高效的算法去帮助优化和计算。在非线性系统中，估计算法如何帮助实现SLAM技术的系统框图如图1所示，在此过程中，SLAM的运动学模型和观测模型如下所示：

$x_k=f\left(x_{k-1},u_k\right)+w_k$ (1)

$z_{k,j}=h\left(y_j,x_k\right)+v_{k,j}$ (2)

$x_k$ 为k时刻机器人的位姿， $z_k$ 为k时刻机器人的观测数据， $u_k$ 为输入数据(机器人记录自身运动的传感器获取的数据)， $y_j$ 为观测点， $w_k$ 和 $v_k$ 分别为k时刻的运动噪声和观测噪声， $f(\cdot )$ 和 $h(\cdot )$ 分别为运动方程和观测方程。

Figure 1. SLAM system frame diagram

图1. SLAM系统框架图

3. 多维泰勒网的基本原理

多维泰勒网优化控制是由东南大学的严洪森教授在2010年首次提出。多维泰勒网是由多项式连接而成的网状结构，其特点是结构简单和学习能力强，并且具有非线性、自适应性等特点 [14] 。多维泰勒网由输入层、中间层和输出层三层网络组成。多维泰勒网的数学表达 [14] 如下：

$f_{MTN}\left(x\right)={\displaystyle \underset{j=1}{\overset{N(n,m)}{\sum }}{\vartheta }_j{\displaystyle \underset{i=1}{\overset{n}{\prod }}{x}_i^{{\zeta }_{j,i}}}}$ (3)

其中 $x_1,x_2,\cdots ,x_n$ 是多维泰勒网的输入，输入向量 $x=\left[x_1,x_2,\cdots ,x_n\right]\in {\Omega }_n\subset R^n$ ，中间层是n个输入量组合形成的最高次幂为m的多项式集合，作用是将输入空间映射到另外一个新的空间， ${\vartheta }_j$ 是中间层第j个乘机项之前的权值， $N\left(n,m\right)$ 是中间层乘积项的数目， ${\zeta }_{j,i}$ 为中间层第j个乘积项中变量 $x_i$ 的幂次，且 ${\displaystyle \sum {\zeta }_{j,i}\le m}$ 。

定理 [15] 设定义在闭区间上的连续n元函数 $f\left(s_1,s_2,\cdots ,s_n\right)$ 在点 $M_0$ 的某个邻域内 $N\left(M_0\right)$ 内有 $m+1$ 阶连续偏导数，则 $f\left(s_1,s_2,\cdots ,s_n\right)$ 在点 $M_0$ 处可以展开如下形式：

$f\left(s_1,s_2,\cdots ,s_n\right)={\displaystyle \underset{j=1}{\overset{N(n,m)}{\sum }}{\vartheta }_j{\displaystyle \underset{i=1}{\overset{n}{\prod }}{s}_i{}^{{\zeta }_{j,i}}}}+R_m\left(s_1,s_2,\cdots ,s_n\right)$ (4)

其中， $R_m(\cdot )$ 是中间层乘积项的幂次之和大于m的余项， ${\vartheta }_j$ 是中间层第j个乘机项之前的权值， $N\left(n,m\right)$ 是中间层乘积项的数目， ${\zeta }_{j,i}$ 为中间层第j个乘积项中变量 $x_i$ 的幂次，且 ${\displaystyle \sum {\zeta }_{j,i}\le m}$ 。

引理 [16] (Weierstrass第一定理) 设 $f\left(x\right)$ 是定义在闭区间[0, 1]上的连续函数，则存在一个多项式

$p_n\left(x\right)\in P_n$ ，使得 $\underset{n\to \infty }{\lim }\underset{0\le x\le 1}{\max }\left|f\left(x\right)-p_n\left(x\right)\right|=0$ 成立。

4. EKF-SLAM的能观性分析

本节深入分析了影响一般移动机器人运动一致性的具体原因。黄国权等人 [17] [18] [19] 利用滤波器的误差状态系统模型，对移动机器人一致性不足的问题进行了深入研究。

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

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

${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]$ (5)

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

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

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

其中 $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}$ (9)

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

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

将式(9)~(11)和式(8)~(6)相减，得到误差状态估计方程

${\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}$ (12)

${\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}$ (13)

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

显然，式(12)是非线性的，所以我们在估计状态 ${\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}$ (15)

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

$\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}$ (16)

其中，状态转移矩阵 ${\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}$ (17)

$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]$ (18)

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}$ (19)

其中， $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}$ (20)

式中， $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]$ (21)

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

其中 $\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}$ 的雅可比矩阵)。

由于EKF-SLAM的线性化误差状态模型是时变的，因此可以对局部观测矩阵进行可观测性分析。对于EKF-SLAM系统(式(16)和式(20)，我们将从特定时刻到下一时刻的局部观测矩阵视为研究对象。这样做的目的是为了更好地理解系统的动态特性，从而有针对性地改进模型和算法，提高移动机器人的运动一致性。这种分析方法有望为相关领域的研究人员提供有益的参考和启示，促进移动机器人技术的进一步发展。

$\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]$ (23)

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

$\begin{array}{ll}\Lambda \hfill & =\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]\hfill \end{array}$ (24)

$=\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]}$ (25)

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

接下来，我们将讨论标准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}$ (26)

其中 $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]$ (27)

$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$ 。结合(20)，(21)，(22)，得到

$\begin{array}{l}{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}$ (28)

其中

$\begin{array}{c}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}$

利用这个结果，我们可以把(25)中的矩阵 $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]$ (29)

其中

$\begin{array}{c}{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}}$ (30)

和

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

因此，秩为3的矩阵 $Q$ 的第三列通常包含一组不规则元素，这表明它是一个具有不规则元素的向量。从这个角度看，标准EKF-SLAM所采用的线性化误差状态模型与基本非线性系统有所不同。具体来说，EKF通过对特定时间间隔采集的观测值进行处理，得到了状态空间的三维信息。然而，实际测量只提供了关于状态空间的二维信息。这就意味着，EKF在基本非线性SLAM系统中获取了错误的不可观测方向信息，从而导致了不一致性。这种不一致性主要是由于EKF处理观测数据的方式与基本非线性SLAM系统的特性之间的差异造成的。

5. 改进的EKF-SLAM算法

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

${\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]$ (32)

与式(17)不同的是，这里使用的是机器人位置信息的预测值 ${\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]$ (33)

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

基于上述的修改，在滤波算法中对雅可比矩阵的这些调整意味着，只有地标的首次观测值和对机器人姿态的初始估计会被考虑。它将影响可观测矩阵 $Q$ ，这里将得到一个新的矩阵 $Q^{\prime }$ ，这种方法被称为第一估计雅可比矩阵EKF(FEJ-EKF)算法。通过替换传统的EKF-SLAM算法，得到了本文提出的改进算法。

$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]$ (34)

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

在这里，我们将加上多维泰勒网，以促进改进算法的有效性，使得SLAM系统更具有鲁棒性，并构建更一致性的地图。这里在EKF-SLAM的线性化阶段，结合多维泰勒网，并在传播阶段使用机器人先验位姿估计，更新阶段使用地标的第一次观测值。

6. 仿真结果

仿真实验通过比较标准的、理想的EKF-SLAM算法和改进算法的表现来进行。对于每个场景，我们记录算法的一致性评价性能指标：使用NEES (Normalized Estimation Error Squared)指标来衡量估计误差的一致性。

仿真实验结果显示，在所有三种算法中，改进的EKF-SLAM算法在一致性方面均优于理想的和标准的EKF-SLAM算法。图2表示的是标准的EKF-SLAM算法在机器人位姿估计的NEES的结果，图3表示的是理想的EKF-SLAM算法在机器人位姿估计的NEES的结果。图4表示的是改进的EKF-SLAM算法在机器人位姿估计的NEES的结果。通过NEES指标评估的一致性结果表明，改进算法展现了更高的一致性水平。

这些实验结果验证了我们提出的改进方案能有效解决EKF-SLAM算法在一致性问题上的不足。我们的改进主要通过优化状态更新机制实现，这些改进提高了算法对环境的适应能力，从而提高了SLAM过程的整体性能。

Figure 2. Standard EKF-SLAM algorithm on robot pose NEES

图2. 标准的EKF-SLAM算法关于机器人位姿的NEES

Figure 3. Ideal EKF-SLAM algorithm on robot pose NEES

图3. 理想的EKF-SLAM算法关于机器人位姿的NEES

Figure 4. Improved EKF-SLAM algorithm on robot pose NEES

图4. 改进的EKF-SLAM算法关于机器人位姿的NEES

7. 结论

本文深入探讨了扩展卡尔曼滤波器(EKF)在同时定位与地图构建(SLAM)应用中存在的不一致性问题，并提出了一系列创新的改进措施。这些措施显著增强了基于EKF的SLAM系统在面对复杂环境时的鲁棒性和一致性。通过详尽的仿真实验，本研究不仅验证了改进算法的有效性，还展示了其在处理长时间序列数据时对线性化误差和估计误差累积的明显改善。

尽管本文的改进方法通过引入多维泰勒网和FEJ策略，表现出优越的性能，但我们也认识到算法仍有改进空间。例如，当面对极端的环境动态变化和高度噪声时，当前算法的稳定性和准确性可能受到影响。因此，未来的研究将需要聚焦于进一步优化算法结构，提高其适应性和准确性。

此外，考虑到实际应用中的硬件限制和实时处理需求，未来的研究也应探索更为高效的算法实现方式，以满足实时性的要求。综上所述，本文的研究成果为EKF-SLAM系统的发展提供了有价值的理论基础。