中文翻译～～.doc

利用延迟的定常时间SLAM方法Joss Knight, Andrew Davison, Ian ReidDepartment of Engineering ScienceUniversity of Oxford摘要 许多解决SLAM问题的方法利用扩展卡尔曼滤波器(EKF)来更新和维护机器人以及环境特征的位置。尽管EKF是一个强大和有用的工具，但经常会带来计算复杂性问题，即在每个递归步骤中的计算量与环境特征数目的平方成正比。本文提出了延迟技术，在何时利用获取的处理时间上允许有更大的灵活性，但又不影响滤波器的优化性能。它基于当前的测量，通过更新常量大小的数据集，能够在延后阶段的不被观察到的环境地图中产生更新。与次优定常时间滤波技术相结合的实验中，表明了如何产生导航地图，而不产生计算复杂性问题。1 引言如果目的是从机器人的传感数据中产生最准确的环境地图，最好的解决方法就是以计算代价最小的方式进行批处理。这种系统在机器视觉的从运动恢复结构中是很常见的。然而，在移动机器人导航的SLAM问题中，由于机器人需要连续地处理获取的数据，逐渐地构造环境地图，因此时间因素是至关重要的。更严格地讲，能够在定常时间内，将新的数据溶进环境模型是最理想的。这个定常时间约束对于EKF是有问题的，也是所有SLAM问题所共有的，由于计算复杂性随地图的增大而增大。为了满足定常时间约束，必须修改最优的EKF。实际上，任何减小EKF计算复杂性的方法都是有用的。Durrant-Whyte，Dissanayake和其他学者都注意到许多地图特征给出了冗余信息，从而可以被删除而不影响定位的精度，此外，Newman的几何投影滤波器，利用了快速构造的相对图。次优的方法能够在损失准确性的情况下，通过忽略一些地图特征之间的耦合，提供更快的滤波速度。 Nebot等明确地给出了一个随地图大小而具有线性复杂性的方法。Castellanos和Leonard都利用静态的子图策略，从而产生定常时间复杂性。次优方法的许多问题，如由于不能从理论上预测从而对滤波器产生不可预料的影响。这些方法通常表明是保守的(整个地图的不确定性并不比应该有的少)，但不能证明是相容一致的(地图将最终收敛于真实值)。在6中给出了解决各种SLAM问题快捷方法的详尽的综述，并对滤波器性能做了讨论。在本文描述的滤波技术，以图1所示的机器人为背景，可以应用于任何SLAM问题的EKF方法中。利用四轴立体手眼系统的固定三角配置提供测量数据。视觉极大地改善了数据相关，并为许多机器人任务提供了大量的信息，但这个技术并未涉及时间代价和深度准确性，这扩展了EKF并改善了计算复杂性问题。图1 Head Yorick 和 GTI 小车结果，在2中描述的原始系统充分考虑了特征的初始化和选择，来产生高质量特征的稀疏地图。下一步，就像文中描述的，利用延迟，提供两倍的特征数目，并允许在较后的阶段执行大多数处理。本质上，它允许收集所有未被在一个常量大小矩阵中观察的特征的变化，并在每一步中更新，而不是当整个地图需要更新时进行。在损失随意性，可控性和更新代价的情况下，大多数滤波步骤将是定常时间的。文中的最后部分定义了如何利用部分解耦来约束这些完全更新的代价。2 概念下面将论述整个论文的概念。向量将以黑体，矩阵以电传打字表示。地图构造概念如2中所述： 第个特征的测量方程 由机器人和特征位置组成的状态向量，下标v是从而来的，不是模糊的 匀称协方差矩阵。注意 3 EKF-完整的滤波器下面首先介绍著名的扩展卡尔曼滤波器：预测在k+1时刻的状态和协方差是基于k时刻的状态、状态转换函数及雅可比矩阵，控制量，和过程噪声协方差来预测的：更新对状态的更新包含修正值，测量值和预测值的差异，修正协方差S和测量噪声R其中3.1 实施前些部分论述了著名的EKF方程。在许多EKF的典型应用中，整个状态向量在每个步骤进行更新，状态转换函数影响整个状态，因此滤波器的复杂性与状态大小成立方关系。在SLAM中，状态转换函数只影响机器人状态，通常只有特征状态的一个子集在每个步骤进行更新。产生的滤波器的复杂性为，n为特征数目。在这一部分，将解释基本方程的重排列，给出特征状态和协方差块的独立方程。由于很清楚参考的是哪一个时间步骤，提出时间依赖性的k概念。初始化是基于在每个时间步骤中单个特征的测量。3.1.1 解释方程首先注意到雅可比阵的稀疏性 其中有特征的测量值。而且处理噪声也具有稀疏性与修正协方差S的乘积表明了这是一个定常时间计算(独立于地图大小) (1)最终增益项的扩展，应该很清楚的表明了下一部分的求和方程为注意S是匀称的，因此。3.1.2 重表达首先定义一些如下的中间向量和矩阵 (2) (3) (4)从这些中间量和前一部分的方程，能够获取图2的滤波器方程，表明如何单独处理每一块状态向量和协方差矩阵。3.1.3 子图在延迟部分将把看作为不止是单个特征的参考，而是一组特征，或一个子图特征。为了强调之间的差异，将子图称为。现在根据有关的雅克比阵和子图中的单个特征，修正协方差为由于，应该很清楚这个方程减为了原始的方程(1)。类似的，在的扩展中用替代是合法的。实际上，在这一部分所有的用替代都是合法的。因此，比如方程(20)更新图2中的，现在影响了矩阵块，它是包含子图中所有特征与特征之间的所有交叉协方差的高矩阵。因此一些中间向量和矩阵(n，B，C和D)由于考虑了这些将会扩展。在2中，延迟没有通过子图获取，并通过删除维护相容性。然而，注意到在以后的推导中，无论是否利用，它可能被取代，因此也表示了一个子图。3.1.4 多个同步测量如果在一个步骤中检测到多个特征，这一部分的推导不受影响，在于可以很容易地表示多个特征。依赖于这些实施，可以很容易地为滤波器提供单个测量，将会给出相同的结果。4 延迟在最后一部分，将推导一个阶的滤波器，每一个地图元素和协方差块都有一个单独的方程。问题在于任何一个特征都将影响状态和协方差的每一部分。这一部分将阐述不直接影响特征测量的数据的变化是如何在一个维数不变的向量和矩阵中累积的，并用这个延迟数据的常量时间递归更新代替滤波步骤的。只有平常的更新影响状态和协方差的定常大小部分。图2 重表达的EKF方程总结。下标指观察到的特征，和指其它特征，因此带有的方程指两个未观察的特征之间的协方差，包括。4.1 推导完整的延迟推导是冗长的，并且最好有足够的空间表示整个算法。读者可参考文献2，这儿只阐述一部分。方程(19)和方程(20)重写为其中 (22) (23) (24) (25)这些方程是相互递归的，也就是，可以在k个更新步骤后根据递归更新矩阵(参考(0)计算和总的更新： (26) (27)其中从到的递归更新为当从相同的子图观察到任何特征是这是正确的，并且是一个受约束的计算量。方程(26)和(27)代入(21)表明了一个类似于的参数，的方程能够以相同的方法操作，预测步骤是很容易包含的。4.2 算法这里总结一下基本的延迟算法。它利用十个矩阵和延迟向量，状态和协方差的常量集总是保持更新，也就是，都是更新的数据。1、 当观察到新的特征时过程开始，状态和协方差当前是最新的。设置所有的延迟数据为零，保存，被设置为单位阵。2、 预测。当传感器运动时，改变更新的数据，和作如下变化：3、 更新在观察到特征后，由式(2)计算向量m和n，由式(3)和(4)计算A到D，由式(22)到(25)计算E和H。现在更新数据，如图2所示，并改变延迟数据如下所示：它仍旧表明了随着延迟时间如何执行完整的状态和协方差的更新(当一个新的子图被观察到时)，这里(0)指最后一个完整更新的值。更新的计算复杂性随子图大小线性增大，因此如果子图特征被多次测量，延迟将减小总的计算时间，当机器人长时间探索同一个子图时，这个时间节约将会很大。4.3 动态子图在2中得到的延迟当相同的特征被重复观察时，往往要延迟完整的更新，这里已经表明这个可以很容易地扩展到相同子图的特征的单个观察，这个扩展产生了如何更好地分割子图的问题。理想上，从计算效率的观点看，最好的子图仅仅是在下一个完整更新之前能够产生最长的时间，换句话说，观察到的下面K个特征，K为选择的子图大小。幸运的是，这是很简单的。我们能够增长延迟数据来表示一个新观察的特征，扩充子图大小直到K个限制。我们有包含M个特征的子图，并观察到特征，不在当前的子图中，首先，利用当前的延迟矩阵产生这个新特征的状态和协方差块，其中后者指子图中所有的特征和特征的交叉协方差。现在如果延迟数据按如下形式修改，这个特征将能够被包含到当前的子图中最后的位置，而不会对下一个完整的地图更新产生影响：增加的块的大小能够从4.2节中完整的更新方程获取，并未给出上式的推导，但是应该很清楚，在的情况下，增加的块应确保的正确更新，它是的一部分，并且无论在哪里零块只是从方程中删除它。4.4 新特征新特征(以前从未观察到的特征)需要所有特征机器人的交叉块的更新值在地图中被重初始化，因此为了不破坏延迟，最初的这些值在新特征被初始化后必须被代替。一个新的特征代表了一个更新协方差矩阵块的完整的行和列，这不便于延迟，并且在当前的实施中，一个新特征的观察引起了整个地图的更新，从这个观点看，新特征能够和其他一样对待。依赖于系统，机器人在探索过程中可能在大的测量步骤中感知到新的特征，因此当前的研究寻求避免完整更新的一些选择。4.5 结果在这个阶段在大规模场景中利用我们的硬件进行修正的滤波器的实验是不可能的，然而，延迟只是滤波器方程的再工作，在仿真环境下的人工场景足以验证系统的性能。我们在包含100个均匀分布的点特征的环境中验证最优滤波器(图3(a)。仿真机器人具有与实际系统相同的参数，并且系统利用许多相同的自主能力来选择观察哪一个特征，这包含数据相关和仿真限制。图3(b)示出了在一个环境遍历后机器人和特征的实际和估计位置(3误差椭圆)。最值得说明的是，图3(c)示出了在第二遍环境遍历过程中滤波消耗时间，完整的滤波和延迟滤波器并行执行。注意到偶尔发生的单步表示了延迟地图的完整更新。在这些过程运动中，机器人每步2cm，只有1.4的时间用于完整的地图更新。总的节省时间大约占整个滤波时间的50。(a) (b) (c)(a)人工场景和机器人运动路径，场景为20m20m，机器人大约是0.5m的立方体，并且所有特征高度为1m。(b)一次探索后，真实的(灰色)和估计的(黑色)地图，误差为。(c)子图大小为10的完整滤波器和带延迟的滤波器处理时间比较，仿真运行于600MHz的奔腾CPU上。图3 延迟实验结果4.6 讨论尽管延迟能够节省处理时间，但这并不是它有用的特点，而是它给系统关于何时利用获取的处理时间足够多的控制。动态子图意味着子图大小不必是固定的，但直到获取执行完整更新的时间内保持增长。此外，当初始化一个新的子图时，不必更新整个地图，只是它的相关部分，能够节省当前的延迟数据，因此下一个完整的地图更新必须利用两组数据在两个阶段内完成。其它包括延迟的等级方法，涉及到在子图内用子图。这将会延迟完整的更新直到机器人在最低的等级上穿过子图的边界。5 部分解耦-获取定常时间性能前些部分解释和讨论了延迟技术，表明了它是如何提供对处理事件的足够多的控制的。然而，完整的地图更新仍旧是复杂度为的操作。在大的地图中这仍旧是问题。为了解决这个问题我们引入了一个简单的次优技术，称为部分解耦。利用这个技术，我们保留尽可能大的用于更新的局部地图，并把它和剩余的以及机器人进行解耦。产生的地图将会失真，不像其他的次优技术，但滤波仍旧是稳定的。它是连续的，对于地图导航是至关重要的，但不准确。这个技术将保留地图的连续性，但地图的准确性总是利用传感数据的离线处理来获得。5.1 定义按如下分割状态和协方差块A包含有机器人的状态和固定数目的特征，块B包含有剩余的特征。在我们的系统中，块A的最好选择是那些最近观察的特征。滤波器将保持和实时更新，剩余的数据将会被滤掉，实质上是被删除。由于A是定常大小，产生的滤波器将以定常时间工作。能够证明这个方法是保守的(6)，意味着在每一步它将产生一个比完整地图较不确定的地图，为了使它稳定，我们必须在观察到有意义的协方差之后使块B的特征移动到块A，简单的设置为零，但仍保留原来的方差来从地图中解耦特征。这对于特征的当前状态引入了有偏，但滤波器仍旧是稳定的。产生的地图失真将取决于块A的大小，表示了包含相容特征的几何区域。5.2 结果再次，利用当前的硬件，对于大量数目特征环境的完整实验是不可能的，然而，由于部分解耦只是一个滤波修正，它足以在仿真中比较最优和次优滤波器的性能。为了更好地阐述部分解耦的性能，我们利用与延迟实验相同的场景，但更大，机器人可探索到地图的外面。结果机器人在回到初始位置时已运动了很长的距离，图4示出了由完整的滤波器和块大小为40、80的次优方法产生的地图，图4(b)示出了机器人返回到初始点的误差何不连续性。这个不连续性由进一步探索地图来消除，因此即使存在总的地图失真，局部相关的特征为职业是大致正确的。图4(c)只是可检出的。根据总的滤波时间，相对于只利用延迟本身，部分解耦产生了50%的时间节省，并且由于完整的地图更新是定常时间的，这对于较大的地图将会增加。(a) (b) (c)(a)由最优滤波器产生的40m40m的走廊地图，观察到264个特征。(b)由部分解耦和块大小为40产生的走廊地图，观察到248个特征。(c) 由部分解耦和块大小为80产生的走廊地图，观察到268个特征。图4 部分解耦实验结果5.3 讨论由于部分解耦还未利用大量的机器人和测量模型被彻底验证，它只是一个实验方法。很清楚的是像我们那样选择块A的特征，滤波器保持稳定并产生了连续的用于导航的地图，也很清楚失真的程度与块大小和同一区域不同时间观察的特征数目的比例有关，这意味着可以随意地限制失真到容许范围内。与延迟相结合块大小能够很快增加，因为处理速度的主要参数就是延迟子图的大小。在这个领域进一步研究将会有更大的潜力，并且有许多建立好的Kalman滤波技术能够保持较小的地图失真。6 总结和结论在这篇论文中我们提出了SLAM中用于增加扩展卡尔曼滤波器计算效率的延迟技术，表明了它能够对于处理时间足够多的控制，并保持最优性能，还引入了部分解耦作为一个潜在的有用技术，用于获取定常时间性能。参考文献1 J. A. Castellanos, M. Devy, and J. D. Tardos. Simultaneous localisation and map building for mobile robots: A landmark-based approach. In IEEE International Conference on Robotics and Automation Workshop on Mobile Robot Navigation and Mapping, 2000.2 A. Davison. Mobile Robot Navigation Using Active Vision. PhD thesis, Robotics Research Group, Oxford University Department of Engineering Science, Feb. 1998. Full text available at www.robots.ox.ac.uk/ajd/.3 G. Dissanayake, H. Durrant-Whyte, and T. Bailey. A computationally efficient solution to the simultaneous localization and map building (SLAM) problem. In IEEE International Conference on Robotics and Automation, San Francisco, USA, Apr. 2000.4 H .F. Durrant-Whyte, M.W. M. G. Dissanayake, and P. W. Gibbens. Toward deployment of large scale simultaneous localisation and map building (SLAM) systems. Technical report, Australian Centre for Field Robotics, University of Sydney, 2000.5 J. Guivant, E. Nebot, and H. Durrant-Whyte. Simultaneous localisation and map building using natural f