本文的第二章节介绍了SLAM预测理论问题的数学结构

c本文的第二章节介绍了SLAM预测理论问题的数学结构。第三章证明并论述了三个收敛性结论。第四章提供了一个例子：如何在户外用MMW（毫米波）雷达实现完整的SLAM算法达到获取对地标的相对性观测得到的数据。一种能对地图并管理其地理数据的算法也被提出。这种算法的提出正好印证了第三章得出的收敛性的结论。第五章讨论了现在随着实践而产生的各种问题。总的来说对SLAM问题的解决方法包括：次优解法的发展，地图数据管理以及数据关联性分析。IITHEESTIMATION-THEORETICSLAM问题这一章节建立了一种在对SLAM问题的研究过程中得出的数学结构。这种结构与在Smithetal.[1]中使用的结构完全相同并且使用与在LeonardandDurrant-Whyte[3]中使用的符号一样的符号。A.车辆与地标模型SLAM问题的设定情景是：一辆载有已知动态模型的车以完全未知的地点为起点，在包括了一定数量的特征和地标的环境中移动。特征与地标会被等效利用。这辆车配备有一个传感器，它可以对任何单个地标与车辆间的相对地理位置以图1所示的方式测量。地标的绝对位置是不可靠的。客观的说，对于移动的车和对地标的观测所建立的一个线性（同步的）离散时间模型是可以采用的。尽管在实际导航问题中，车的运动与对地标的观测总是非线性的，异步的。但是使用线性同步模型不影响章节III中证明的正确性，除了测量时要求达到与卡尔曼滤波器在发展中所能达到的线性度一样。实际上，第二章中所描述的SLAM算法的实现应用于非线性运动的车辆模型与非线性异步观测模型中。这种系统的独特之处在于对所有地标定位的同时对车的位置与运动的测量。车在k时刻的运动状态由表示。车在环境中的移动可通过建立常规线性离散时间状态转移方程模型来描述。或是以如下方程进行过程建模来表示：上述公式中Fv(k)是状态状态转移矩阵；Uv(k)是控制输入量的向量；Vvk是零平均值下的暂态不相关过程噪声协方差（详见于【17】和【18】）。第i个地标的位置由Pi表示。它的状态转移方程式是：因为假设航标不动。不失一般性的考虑，把大量环境中的的地标统一设成N。所有的N地标由描述。T表示转置矩阵，为了节省空间同时在括号内外使用。增广状态向量包括车辆的状态以及所有地标的位置信息。由如下方程表示：整个系统的增广状态装换模型如下表示：Ipl是【Pi]*[Pi]的单位矩阵。Opl是[Pi]的0向量。地标Pi随机运动的情况可能很容易适应这种框架。然而这样做不能深入理解问题的本质，此外，本文所论述的收敛性也未被体现。B.观测模型用来勘测的车辆都配备有一个传感器。这个传感器可以收集观测车与地标相对位置而得来的数据。此外，我们再一次假设观测模型线性同步的。对于地标i的观测模型我们可以如下表示：在上式中Wi（k）是0平均数与变异数Ri(k)下表示暂态不相关观测误差的矢量。变量Hi是观测矩阵。当对地标i进行观测时，它与传感器Zi(k)对状态向量X(k)的输出有关。值得注意的是，对于地标i的观测模型可以如下表示：这种结构反映了一个事实：对于车与地标来说观测是相对的，即观测的数据称为相对位置，或是相对全距以及相对导向（见于章节IV）.C.估测流程在对SLAM问题的理论估计构想中，卡尔曼滤波器被用来对车与地标的位置进行观测。我们把这种方法和这个过程的主要阶段简要的总结为IIIIV章节发展的必要铺垫。详细说明可在【17】，【18】和【3】中找到。状态变量X(k)根据（5）中过程模型展开并且根据（7）中观测模型观测得来，而X(k)由卡尔曼滤波器递归估算得来。卡尔曼滤波器的估算结果与条件平均量，其中是一直到q时刻的的观测序列。估计的误差由表示。在估算时协方差由卡尔曼滤波器进行递归估算。卡尔曼滤波器现在由以下三步实现递归估算：1.预测：给定在（5）（7）中描述的模型，然后同时对k时刻状态变量X(k)与协方差P(k|k)进行估算分别得到和估计值。这种算法首先实现了对状态估计的预测，然后根据式：对k+1时刻的观测值（相对于第i个地标）与协方差分别进行估算。2.观察：在预测的基础上，根据（7）中所述方法对于真实状态变量X(k+1)对应的第i个地标进行观测并得出观测值。假设地标间的相互关系正确的话，一个新的变量可如下计算：，同时还可算出新的协方差矩阵：3.更新：状态估计变量与对应状态估计协方差随后可根据以下方程更新：上式的增益矩阵由如下方程算出：状态估计协方差矩阵的更新对于slam问题最为重要。理解状态变量协方差矩阵的结构与变化是用这种方法解决SLAM问题的关键。III.SLAM问题的结构这一节主要证明了作为SLAM问题结构基础的三个重要结论。这些结论的隐含意义也被详细的验证。在下面的证明中用到了半正定矩阵，它的主要特性在附录中被总结出来。A.地图协方差矩阵的收敛状态协方差矩阵可能以如下矩阵形式给出：其中，表示车辆状态预测误差的协方差矩阵；地标状态预测的地图协方差矩阵；车与地标状态间的互协方差矩阵。定理1：连续观测的实现使地图协方差矩阵的子矩阵的决定性降低。算法被半正定状态协方差矩阵初始化。矩阵Q和Ri都是半正定的，并且因此矩阵和都是半正定的。从（16）得知，对于任意地标i：状态协方差矩阵的行列式可用来衡量与状态估计有关的不确定椭球的体积。方程（18）表明了状态估计的总体不确定性不会在数据更新时增加。一个半正定型矩阵的任何主子矩阵也是半正定的。因此，从式（18）可知地图的协方差矩阵也有以下特性：（19）从（12）得，全局状态预测协方差方程也可能如下表示：其中的Y1=因此，如果假设地标静止，在预测地图状态时没有过程噪音的影响。那么，地图的协方差矩阵以及它的任何主子矩阵均有如下特性：（20）但是以上假设不符合实际,因为过程噪音一定会影响车辆的定位预测。所以在预测时预测数据的协方差一定会增长。由（19）（20）得知：地图协方差矩阵有如下特性：此外，半正定矩阵的总体特性保证了地图协方差矩阵的任何子阵都适用于这种不平等性。尤其是，对地图协方差矩阵（状态方差）的任何对角元素：因此，每个地标的绝对位置的预测误差也在减小。定理2：在极限情况下地标间的估计是相互关联的。随着观测次数趋于无穷，地图协方差的限度下降，这个限度如下表示：（22）。将写成和因为对应记谱的清晰度，SLAM算法的更新可写为如下方程：其中（24）地图协方差矩阵Pmm的更新算法如下：由方程（22）（25）可知矩阵新的协方差矩阵的逆矩阵总是半正定的。因为噪音协方差观测矩阵Ri是半正定的，所以（22）要求M2=0因此极限情况下，Pd=0并且两个地标间的联系可确切得知。需要注意的是这一结论并不意味着地标的协方差矩阵的完全没用。在极限情况下地标的绝对位置不能确定。定理3：极限情况下，在遇到第一个地标时，与任意一个地标估计有关的协方差矩阵的下界仅由车辆估计的初始协方差矩阵决定。正如先前所描述的，地标位置估计的协方差随着连续观测的实现而下降。在车辆移动噪音Q以及观测噪音Ri很小时，估计值精度明显被改善。当车辆被给定极常量Q=0时，限值以及之后的状态协方差矩阵的下界可以被得知。在这种情况下，使用卡尔曼滤波器的信息形式去检测状态协方差矩阵的行为是很方便的。在只有单个地标的环境下，这个地标之后的状态协方差矩阵已被观测到并记为k状态。方法如下：在上式中，由于Q=0，（30）根据(29)(30)可将P（k|k）—1写为:其中：对分块矩阵求逆可得：其中：仔细观察（32）右下角分块矩阵可得知地标的观测不确定性为什么只来自车辆的不确定量P0v。找出状态协方差矩阵的下界并且地标的不确定性在地标的观测次数趋于无穷时反而收敛。方程（33）给定了单个状态估计量方差矩阵的下界：。现在分析一种带有N地标的环境，当地标i在不受其他地标影响下被观测时，可计算的最小的不确定量是。如果多于一个的地标被观测到是，那它就会作为非一般性导航问题的情况之一。，随后这个地标的不确定性将有可能被定理1大大降低。在极限情况下对于第i个地标状态的不确定性的下界可写为：并且仅由车辆定位的估计协方差决定。需要注意的是：当时，汽车不确定性的下界保持在不变，在寻找此下界时Q被置0.当和是单位矩阵时，在极限情况下，对于每一个地标的确切估计可以实现由初始车辆不确定性所给定的下界。当过程噪音非0时，由于过程噪音所造成的信息遗失和由观测得到的信息量的增长之间的矛盾决定了极点协方差。这个问题很难被分析清楚，尽管地图的极点协方差绝不会低于上述的方程而且它也是由P0，Q和R组成的方程。值得注意的是，协方差的适用范围。因为所有的地标都是由车辆逐个的单独的进行观察和预置的。地标估算数据的协方差并不能通过对先前未知的地标进行额外的观测而进一步降低。然而，通过结合外来信息，例如通过对位置信息已通过如GPS等外部设备可靠得知的地标进行观测，可以降低协方差。总的来说，通过以上所描述的三个定理，完整地推导出了地图的收敛性和它的稳态特性。当车辆在环境下行驶时，对于在地图中已精确得知其相对位置的点，对于它的地标位置进行估算所得数据的总不确定性单调下降。在极限情况下，任意一对地标的估测数据的误差完全相关。这意味着，给定了任意一个地标的准确位置，地图中另一个地标的位置夜可以完全确定。因为地图在以上方法中总是收敛，对于每个地标（整个地图）的绝对位置的估测误差可以降低到仅由初次观测时产生的误差决定。因此我们可以得出一般性SLAM问题的解决方法。而且构造一个能描述地标相对位置同时在车和地标完全未知时预算车辆位置的完全精确的地图是完全可能的。IV同步定位与地图构建算法的实现这一章论述了同步定位与地图构建（SLAM）算法在标准公路用车上的实现。这种车辆配备有毫米波雷达（MMWR），这种雷达能对与车辆有关的地标位置进行扫描。这种办法的实现旨在演示（SLAM）算法的主要特性：收敛性，一致性和地图误差的有界性。这种应用也突出体现了（SLAM）算法的许多特性和它的应用发展。尤其是，本应用展示了一般情况下非线性运动的车与观察模型如何在算法中统一；数据关联性问题如何被解决；当算法运行时如何对地标进行初始化和跟踪。然而，这里所说的应用只是实现和拓展完全自动(SLAM)导航系统的第一步。大量更深远的方法如：地标摘取，数据联系，降低估算量和地图构建在以下章节中将被进一步讨论。A.实验装置构建为了评估SLAM算法，很有必要得知车辆真实轨迹与地标真实位置以便于对SLAM算法算出的的估计值进行比较。由于这个原因真实的地标位置需要被精确考察用作与地图构建算法的输出值进行比较。第二个导航系统运用了浮标的位置的知识，它与地图估算在同一个数据集中运行。（这种算法与（20）中所用的十分相似）。这种算法能够提供与SLAM算法的估计值进行比较的真实车辆位置的精确估计量。在下文中，车辆的状态由下式定义：。其中，x和y是处于球形坐标系的，以车的后轴为中心的坐标系。Φ是车辆轴线的方向。地标被以地标点的方式建模，并由一组笛卡尔对表示，如：车与地标的状态在同一个参考系中记录。1）程序模型：图4展示了正在对地标观测的车的原理图。下列运动学方程可以预测输入速率V，操作角度为的车的运动状态。L是车的轮胎基线长度。这些方程可以获取车辆进程模型的离散时间，形式如下：（35）它运用于车辆状态估计器的预测环节。如果把环境中的地标假设为驻点，那么地标的过程模型为（对于从1到N的所有地标）。（35）（36）定义了系统的状态转移矩阵。2）观察模型：实验中用的MMWR回归到限定的范围中，并从向地标i转移。关于图4，观察模型可写为：在上式中，和是测量范围与方位时的噪声数列，是给定的雷达在大域坐标中的方位，计算方法如下：方程（37）定义了特殊地标的观测模型。附录I半正定矩阵的属性1）半正定矩阵的对角元素是非负的。2）若是半正定的，对于任意矩阵，是半正定的。3）若和都是半正定的那么det(A+C)_det(A).4）半正定矩阵的主子矩阵也是半正定的。5）对于和，若是半正定的，那么附录II地标的初始化算法以下说明了地标位置的初始化过程以及对特殊地标的联合观察。这一过程也评价了地标的质量。对于估算过程来