第10卷第5期 智能系统学报 Vol.10 No.5 2015年10月 CAAI Transactions on Intelligent Systems 0ct.2015 D0I:10.11992/is.201403017 网s络出版t地址：htp:/ww.cmki.net/kcms/detail/23.1538.tp.20150930.1556.018.html 测距式传感器同时定位与地图创建综述 刘建华12，刘华平3，杨建国，高蒙2，孙富春3 (1.东华大学机械工程学院，上海201620：2.石家庄铁道大学电气与电子工程学院，河北石家庄050043：3.清华 大学智能技术与系统国家重点实验室，北京100084) 摘要：移动机器人的同时定位与地图创建(SLAM)是当前机器人领域中的热点问题，基于测距传感器SLAM实质 是距离信息的在线提取和描述，本文针对这一问题进行了详细介绍。此外，阐述了测距式机器人SLAM的解决思路、 关键技术和实现方法：重点探讨了扩展卡尔曼滤波(EKF)、粒子滤波(P℉)和矩阵奇异值分解(SVD)3种主要方法的 实现过程和存在的问题：对基于测距式的SLAM方法需要解决问题、完善的方面及研究前景进行了阐述。 关键词：移动机器人：测距：扩展卡尔曼滤波：粒子滤波：奇异值分解：同时定位与地图创建 中图分类号：TP24文献标志码：A文章编号：1673-4785(2015)05-0655-08 中文引用格式：刘建华，刘华平，杨建国，等.测距式传感器同时定位与地图创建综述[J].智能系统学报，2015,10(5)：655-662. 英文引用格式：LIUJianhua,LIU Huaping,YANG Jianguo,etal.A survey of range-only SLAM for mobile robots[J].CAAl Transactions on Intelligent Systems,2015,10(5):655-662. A survey of range-only SLAM for mobile robots LIU Jianhua'2,LIU Huaping3,YANG Jianguo',GAO Meng?,SUN Fuchun? (1.College of Mechanical Engineering,Donghua University,Shanghai 201620,China:2.College of Electrical and Electronic Engi- neering,Shijiazhuang Tiedao University,Shijiazhuang 050043,China;3.Key Laboratory of Intelligent Technology and Systems,Tsin- ghua University,Beijing 100084,China) Abstract:This paper details the research on simultaneous localization and mapping (SLAM)for mobile robots, currently a hot spot in robotics.The essence of range-only SLAM is online extraction and representation of range in- formation.The basic idea,key technology,and solution for range-only SLAM are discussed;then a performance comparison of the fundamental methods is made.Three of the most commonly used methods:Kalman filter,particle filter and singular value decomposition are discussed in detail,emphasizing the realization process and some practi- cal problems.Further research,on the problems,improvements,and development prospects for range-only SLAM, is discussed. Keywords:mobile robot;range-only;Kalman filter;particle filter:singular value decomposition:simultaneous lo- calization and mapping (SLAM) 同时定位与地图创建(simultaneous localization 计)。因此，机器人位姿估计和地图构建之间的相 and mapping,SLAM)是移动机器人研究领域中一个 互依赖使SLAM问题变得极富挑战性，需要在高维 关键技术)，是在未知环境中通过自定位过程获取 空间中搜索解决方法[)。 周围环境信息，并在行进过程中逐步构建周围环境 目前，国内外的研究重点侧重于基于特征地图 的增量式地图，同时不断地重复观测已构建的地图 的定位方法[)，即通过存储在地图中的环境特征与 来消除不确定因素，进而完成自身位姿的精确估 抽象所感知真实环境的特征是否匹配及相对于机器 人SLAM的位置来确定机器人的当前位姿，因此环 收稿日期：2014-03-05.网络出版日期：2015-09.30. 基金项目：国家高技术研究发展计划资助项目(2007AA04Z232):国 境地图的表示方式是否合适以及环境特征提取是否 家自然科学基金资助项目(61075027,91120011)：河北省 准确，直接影响环境特征匹配的效率，进而影响机器 自然科学基金资助项目(F2010001106,F2013210094) 通信作者：杨建国.E-mail:lfengju(@163.com 人定位和地图构建的准确性和实时性：测距传感器
第 １０ 卷第 ５ 期 智 能 系 统 学 报 Ｖｏｌ．１０ №．５ ２０１５ 年 １０ 月 ＣＡＡＩ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｉｎｔｅｌｌｉｇｅｎｔ Ｓｙｓｔｅｍｓ Ｏｃｔ． ２０１５ ＤＯＩ：１０．１１９９２ ／ ｔｉｓ．２０１４０３０１７ 网络出版地址：ｈｔｔｐ： ／ ／ ｗｗｗ．ｃｎｋｉ．ｎｅｔ ／ ｋｃｍｓ／ ｄｅｔａｉｌ ／ ２３．１５３８．ｔｐ．２０１５０９３０．１５５６．０１８．ｈｔｍｌ 测距式传感器同时定位与地图创建综述 刘建华１，２ ，刘华平３ ，杨建国１ ，高蒙２ ，孙富春３ （１． 东华大学 机械工程学院，上海 ２０１６２０； ２． 石家庄铁道大学 电气与电子工程学院，河北 石家庄 ０５００４３； ３． 清华 大学 智能技术与系统国家重点实验室，北京 １０００８４） 摘 要：移动机器人的同时定位与地图创建（ＳＬＡＭ）是当前机器人领域中的热点问题，基于测距传感器 ＳＬＡＭ 实质 是距离信息的在线提取和描述，本文针对这一问题进行了详细介绍。 此外，阐述了测距式机器人 ＳＬＡＭ 的解决思路、 关键技术和实现方法；重点探讨了扩展卡尔曼滤波（ＥＫＦ）、粒子滤波（ＰＦ）和矩阵奇异值分解（ ＳＶＤ）３ 种主要方法的 实现过程和存在的问题；对基于测距式的 ＳＬＡＭ 方法需要解决问题、完善的方面及研究前景进行了阐述。 关键词：移动机器人；测距；扩展卡尔曼滤波；粒子滤波；奇异值分解；同时定位与地图创建 中图分类号：ＴＰ２４ 文献标志码：Ａ 文章编号：１６７３⁃４７８５（２０１５）０５⁃０６５５⁃０８ 中文引用格式：刘建华，刘华平，杨建国，等． 测距式传感器同时定位与地图创建综述［Ｊ］． 智能系统学报， ２０１５， １０（５）： ６５５⁃６６２． 英文引用格式：ＬＩＵ Ｊｉａｎｈｕａ， ＬＩＵ Ｈｕａｐｉｎｇ， ＹＡＮＧ Ｊｉａｎｇｕｏ， ｅｔ ａｌ． Ａ ｓｕｒｖｅｙ ｏｆ ｒａｎｇｅ⁃ｏｎｌｙ ＳＬＡＭ ｆｏｒ ｍｏｂｉｌｅ ｒｏｂｏｔｓ［ Ｊ］． ＣＡＡＩ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｉｎｔｅｌｌｉｇｅｎｔ Ｓｙｓｔｅｍｓ， ２０１５， １０（５）： ６５５⁃６６２． Ａ ｓｕｒｖｅｙ ｏｆ ｒａｎｇｅ⁃ｏｎｌｙ ＳＬＡＭ ｆｏｒ ｍｏｂｉｌｅ ｒｏｂｏｔｓ ＬＩＵ Ｊｉａｎｈｕａ １，２ ， ＬＩＵ Ｈｕａｐｉｎｇ ３ ， ＹＡＮＧ Ｊｉａｎｇｕｏ １ ， ＧＡＯ Ｍｅｎｇ ２ ， ＳＵＮ Ｆｕｃｈｕｎ ３ （１． Ｃｏｌｌｅｇｅ ｏｆ Ｍｅｃｈａｎｉｃａｌ Ｅｎｇｉｎｅｅｒｉｎｇ， Ｄｏｎｇｈｕａ Ｕｎｉｖｅｒｓｉｔｙ， Ｓｈａｎｇｈａｉ ２０１６２０， Ｃｈｉｎａ； ２． Ｃｏｌｌｅｇｅ ｏｆ Ｅｌｅｃｔｒｉｃａｌ ａｎｄ Ｅｌｅｃｔｒｏｎｉｃ Ｅｎｇｉ⁃ ｎｅｅｒｉｎｇ， Ｓｈｉｊｉａｚｈｕａｎｇ Ｔｉｅｄａｏ Ｕｎｉｖｅｒｓｉｔｙ， Ｓｈｉｊｉａｚｈｕａｎｇ ０５００４３， Ｃｈｉｎａ； ３． Ｋｅｙ Ｌａｂｏｒａｔｏｒｙ ｏｆ Ｉｎｔｅｌｌｉｇｅｎｔ Ｔｅｃｈｎｏｌｏｇｙ ａｎｄ Ｓｙｓｔｅｍｓ， Ｔｓｉｎ⁃ ｇｈｕａ Ｕｎｉｖｅｒｓｉｔｙ， Ｂｅｉｊｉｎｇ １０００８４， Ｃｈｉｎａ） Ａｂｓｔｒａｃｔ：Ｔｈｉｓ ｐａｐｅｒ ｄｅｔａｉｌｓ ｔｈｅ ｒｅｓｅａｒｃｈ ｏｎ ｓｉｍｕｌｔａｎｅｏｕｓ ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ ｍａｐｐｉｎｇ （ ＳＬＡＭ） ｆｏｒ ｍｏｂｉｌｅ ｒｏｂｏｔｓ， ｃｕｒｒｅｎｔｌｙ ａ ｈｏｔ ｓｐｏｔ ｉｎ ｒｏｂｏｔｉｃｓ． Ｔｈｅ ｅｓｓｅｎｃｅ ｏｆ ｒａｎｇｅ⁃ｏｎｌｙ ＳＬＡＭ ｉｓ ｏｎｌｉｎｅ ｅｘｔｒａｃｔｉｏｎ ａｎｄ ｒｅｐｒｅｓｅｎｔａｔｉｏｎ ｏｆ ｒａｎｇｅ ｉｎ⁃ ｆｏｒｍａｔｉｏｎ． Ｔｈｅ ｂａｓｉｃ ｉｄｅａ， ｋｅｙ ｔｅｃｈｎｏｌｏｇｙ， ａｎｄ ｓｏｌｕｔｉｏｎ ｆｏｒ ｒａｎｇｅ⁃ｏｎｌｙ ＳＬＡＭ ａｒｅ ｄｉｓｃｕｓｓｅｄ； ｔｈｅｎ ａ ｐｅｒｆｏｒｍａｎｃｅ ｃｏｍｐａｒｉｓｏｎ ｏｆ ｔｈｅ ｆｕｎｄａｍｅｎｔａｌ ｍｅｔｈｏｄｓ ｉｓ ｍａｄｅ． Ｔｈｒｅｅ ｏｆ ｔｈｅ ｍｏｓｔ ｃｏｍｍｏｎｌｙ ｕｓｅｄ ｍｅｔｈｏｄｓ： Ｋａｌｍａｎ ｆｉｌｔｅｒ， ｐａｒｔｉｃｌｅ ｆｉｌｔｅｒ ａｎｄ ｓｉｎｇｕｌａｒ ｖａｌｕｅ ｄｅｃｏｍｐｏｓｉｔｉｏｎ ａｒｅ ｄｉｓｃｕｓｓｅｄ ｉｎ ｄｅｔａｉｌ， ｅｍｐｈａｓｉｚｉｎｇ ｔｈｅ ｒｅａｌｉｚａｔｉｏｎ ｐｒｏｃｅｓｓ ａｎｄ ｓｏｍｅ ｐｒａｃｔｉ⁃ ｃａｌ ｐｒｏｂｌｅｍｓ． Ｆｕｒｔｈｅｒ ｒｅｓｅａｒｃｈ， ｏｎ ｔｈｅ ｐｒｏｂｌｅｍｓ， ｉｍｐｒｏｖｅｍｅｎｔｓ， ａｎｄ ｄｅｖｅｌｏｐｍｅｎｔ ｐｒｏｓｐｅｃｔｓ ｆｏｒ ｒａｎｇｅ⁃ｏｎｌｙ ＳＬＡＭ， ｉｓ ｄｉｓｃｕｓｓｅｄ． Ｋｅｙｗｏｒｄｓ：ｍｏｂｉｌｅ ｒｏｂｏｔ； ｒａｎｇｅ⁃ｏｎｌｙ； Ｋａｌｍａｎ ｆｉｌｔｅｒ； ｐａｒｔｉｃｌｅ ｆｉｌｔｅｒ； ｓｉｎｇｕｌａｒ ｖａｌｕｅ ｄｅｃｏｍｐｏｓｉｔｉｏｎ； ｓｉｍｕｌｔａｎｅｏｕｓ ｌｏ⁃ ｃａｌｉｚａｔｉｏｎ ａｎｄ ｍａｐｐｉｎｇ （ＳＬＡＭ） 收稿日期：２０１４⁃０３⁃０５． 网络出版日期：２０１５⁃０９⁃３０． 基金项目：国家高技术研究发展计划资助项目（ ２００７ＡＡ０４Ｚ２３２）；国 家自然科学基金资助项目（ ６１０７５０２７，９１１２００１１）；河北省 自然科学基金资助项目（Ｆ２０１０００１１０６，Ｆ２０１３２１００９４）． 通信作者：杨建国． Ｅ⁃ｍａｉｌ：ｌｆｅｎｇｊｕ＠ １６３．ｃｏｍ． 同时定位与地图创建（ ｓｉｍｕｌｔａｎｅｏｕｓ ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ ｍａｐｐｉｎｇ，ＳＬＡＭ）是移动机器人研究领域中一个 关键技术［１］ ，是在未知环境中通过自定位过程获取 周围环境信息，并在行进过程中逐步构建周围环境 的增量式地图，同时不断地重复观测已构建的地图 来消除不确定因素，进而完成自身位姿的精确估 计［２］ 。 因此，机器人位姿估计和地图构建之间的相 互依赖使 ＳＬＡＭ 问题变得极富挑战性，需要在高维 空间中搜索解决方法［３］ 。 目前，国内外的研究重点侧重于基于特征地图 的定位方法［４］ ，即通过存储在地图中的环境特征与 抽象所感知真实环境的特征是否匹配及相对于机器 人 ＳＬＡＭ 的位置来确定机器人的当前位姿，因此环 境地图的表示方式是否合适以及环境特征提取是否 准确，直接影响环境特征匹配的效率，进而影响机器 人定位和地图构建的准确性和实时性；测距传感器
·656 智能系统学报 第10卷 由于具有数据处理简单、实时性高和鲁棒性好等优 定位：此外，根据机器人所在环境的特点，提取具有 点，得到了广泛的应用；基于测距式传感器的机器人 独特信息的特征作为路标信息，文献[10-12]提取环 定位和地图构建的基本思想是通过融合传感器信 境中圆作为定位路标信息，文献[6]提取环境中矩 息，把自身与参照点（如障碍物、环境特征等）及其 形特征作为定位路标信息。随着环境的复杂性越来 相互关系的距离信息和方位信息在线抽象描述为全 越高，对环境二维建模不能满足需要，文献[13-14] 局地图信息。 等提取环境三维特征作为定位路标信息。栅格地图 基于测距式传感器机器人SLAM主要从3方面 模型是根据测距传感器的原始数据获得障碍物占据 考虑：环境特征的提取方法、地图的表示方式和解决 栅格情况，从而构建环境地图】。 SLAM问题的核心算法[]。本文总结目前流行的基 测距式传感器采集真实环境的数据是点的集 于测距式传感器的SLAM方法，从以上3方面进行 合，如何对这些离散的点处理获得环境的抽象描述， 了归类和比较，指出需要解决的关键问题，并根据已 即局部地图，很多文献采用了Hough变换的方法进 有的研究成果提出未来的研究方向。 行局部地图的构建，但其计算量非常大，很难保证局 部地图构建实时性的要求，文献[15]比较详细地分 1典型传感器类型及其作用 析比较了11种数据点提取特征直线的算法，其中以 根据机器人所携带传感器在SLAM系统中的主 数据分割拟合(split--and-merge)算法较优，因此该算 要作用，传感器大致可分为两大类，一类是本体传感 法得到了广泛应用：此后，研究者在分析总结上述算 器（也称内部传感器），如里程计、陀螺仪、指南针、 法的基础上，对这些算法做了改进，也提出了很多不 同的特征提取算法，如文献[16]通过建立角点函数 速度或加速度计等：另一类是外部传感器，如激光雷 达、超声波、红外线、微波雷达、kinect'),机器人通过 来衡量扫描点是角点特征的程度，利用非极大值抑 外部传感器可以获得真实环境特征的抽象描述及距 制方法去除干扰信息来提取环境的角点特征，文献 [l7]根据split-and-merge算法思想，采用模糊聚类 离信息或图像深度信息。基于测距式的SLAM主要 算法分割数据，利用最小二乘法拟合。 框架是采用本体传感器和外部传感器相融合的定位 直线参数使线段分割的鲁棒性和线段提取的精 方法，利用本体传感器来实现机器人航迹推算，通过 度得到了很大提高，也显著提高了算法效率。此外， 外部传感器（一种或多种）抽象获得的外部环境特 有研究者将比较成熟的优化算法应用到环境特征的 征及其距离信息与已构建环境地图的特征匹配，消 提取，如文献[18]将自组织竞争学习神经网络(slf 除航迹推算的累积误差，获得当前机器人的位姿偏 organizing feature mapping,SOM)方法用于激光雷达 差，并不断修正机器人自身位姿，构建增量式的环境 数据的处理，提取更加精确的环境线段、角点等特 地图。 征，提高了算法的计算效率以及分类性能得到进一 2环境模型及测距式传感器特征提取 步的提高。也有研究者采用卡尔曼滤波对环境的线 段等特征进行提取，不仅对线段特征的误差标准等 真实环境的抽象描述也称为地图构建，即用点 概率特性进行较好地描述和更新，而且受传感器的 和线或栅格的集合近似表达真实环境，是将真实环 非线性影响也较小。 境中连续场景表示成2-D区段，是移动机器人 SLAM的一个重要环节6)。地图中环境特征表征 3基于测距式传感器SLAM常用方法 的准确与否直接影响机器人定位和构建地图的精确 目前，未知环境中机器人利用测距式传感器的 性，因此环境特征的表示要注重特征的鲁棒性和稳 距离信息完成SLAM任务，其基本思想是通过寻找 定性。测距式传感器SALM的环境模型主要有2 当前局部环境地图与已获得的增量式全局环境地图 类，基于特征的地图和密集型地图（栅格地图），前 间的对应关系，减少航迹推算误差造成的移动机器 者主要应用于稀疏障碍物或结构化环境，将环境特 人定位的不确定性9)，获得尽可能精确的机器人当 征作为路标信息，后者主要应用于非结构化环境。 前位姿信息，完备全局环境地图。目前主要方法可 在平面坐标系中机器人根据2条相交的直线就可以 分为基于贝叶斯理论的概率推理定位算法和基于矩 完成自定位，由于环境中存在诸如角、线的特征较 阵分解的定位算法两大类。前者把概率理论应用到 多，而从测距式传感器检测到的原始数据中提取这 移动机器人定位过程中，即在机器人每个可能的位 些特征相对容易，其存储与匹配也简洁方便，因此采 置设置一种概率值，根据机器人运动过程中测距式 用角、线描述环境成为机器人SLAM问题中最常用 传感器感知到的新环境距离信息更新机器人可能位 的特征描述方式。针对缺少角、线特征的环境，文献 置的概率值，从而减少机器人位置不确定性。其实 [6,8-9]通过提取天花板的角、线特征来完成机器人 质是把移动机器人SLAM问题认为是贝叶斯评估过
由于具有数据处理简单、实时性高和鲁棒性好等优 点，得到了广泛的应用；基于测距式传感器的机器人 定位和地图构建的基本思想是通过融合传感器信 息，把自身与参照点（如障碍物、环境特征等） 及其 相互关系的距离信息和方位信息在线抽象描述为全 局地图信息。 基于测距式传感器机器人 ＳＬＡＭ 主要从 ３ 方面 考虑：环境特征的提取方法、地图的表示方式和解决 ＳＬＡＭ 问题的核心算法［２］ 。 本文总结目前流行的基 于测距式传感器的 ＳＬＡＭ 方法，从以上 ３ 方面进行 了归类和比较，指出需要解决的关键问题，并根据已 有的研究成果提出未来的研究方向。 １ 典型传感器类型及其作用 根据机器人所携带传感器在 ＳＬＡＭ 系统中的主 要作用，传感器大致可分为两大类，一类是本体传感 器（也称内部传感器），如里程计、陀螺仪、指南针、 速度或加速度计等；另一类是外部传感器，如激光雷 达、超声波、红外线、微波雷达、ｋｉｎｅｃｔ ［５］ ，机器人通过 外部传感器可以获得真实环境特征的抽象描述及距 离信息或图像深度信息。 基于测距式的 ＳＬＡＭ 主要 框架是采用本体传感器和外部传感器相融合的定位 方法，利用本体传感器来实现机器人航迹推算，通过 外部传感器（一种或多种）抽象获得的外部环境特 征及其距离信息与已构建环境地图的特征匹配，消 除航迹推算的累积误差，获得当前机器人的位姿偏 差，并不断修正机器人自身位姿，构建增量式的环境 地图。 ２ 环境模型及测距式传感器特征提取 真实环境的抽象描述也称为地图构建，即用点 和线或栅格的集合近似表达真实环境，是将真实环 境中连 续 场 景 表 示 成 ２⁃Ｄ 区 段， 是 移 动 机 器 人 ＳＬＡＭ 的一个重要环节［６⁃７］ 。 地图中环境特征表征 的准确与否直接影响机器人定位和构建地图的精确 性，因此环境特征的表示要注重特征的鲁棒性和稳 定性。 测距式传感器 ＳＡＬＭ 的环境模型主要有 ２ 类，基于特征的地图和密集型地图（栅格地图），前 者主要应用于稀疏障碍物或结构化环境，将环境特 征作为路标信息，后者主要应用于非结构化环境。 在平面坐标系中机器人根据 ２ 条相交的直线就可以 完成自定位，由于环境中存在诸如角、线的特征较 多，而从测距式传感器检测到的原始数据中提取这 些特征相对容易，其存储与匹配也简洁方便，因此采 用角、线描述环境成为机器人 ＳＬＡＭ 问题中最常用 的特征描述方式。 针对缺少角、线特征的环境，文献 ［６，８⁃９］通过提取天花板的角、线特征来完成机器人 定位；此外，根据机器人所在环境的特点，提取具有 独特信息的特征作为路标信息，文献［１０⁃１２］提取环 境中圆作为定位路标信息，文献［６］提取环境中矩 形特征作为定位路标信息。 随着环境的复杂性越来 越高，对环境二维建模不能满足需要，文献［１３⁃１４］ 等提取环境三维特征作为定位路标信息。 栅格地图 模型是根据测距传感器的原始数据获得障碍物占据 栅格情况，从而构建环境地图［２］ 。 测距式传感器采集真实环境的数据是点的集 合，如何对这些离散的点处理获得环境的抽象描述， 即局部地图，很多文献采用了 Ｈｏｕｇｈ 变换的方法进 行局部地图的构建，但其计算量非常大，很难保证局 部地图构建实时性的要求，文献［１５］比较详细地分 析比较了 １１ 种数据点提取特征直线的算法，其中以 数据分割拟合（ｓｐｌｉｔ⁃ａｎｄ⁃ｍｅｒｇｅ）算法较优，因此该算 法得到了广泛应用；此后，研究者在分析总结上述算 法的基础上，对这些算法做了改进，也提出了很多不 同的特征提取算法，如文献［１６］通过建立角点函数 来衡量扫描点是角点特征的程度，利用非极大值抑 制方法去除干扰信息来提取环境的角点特征，文献 ［１７］根据 ｓｐｌｉｔ⁃ａｎｄ⁃ｍｅｒｇｅ 算法思想，采用模糊聚类 算法分割数据，利用最小二乘法拟合。 直线参数使线段分割的鲁棒性和线段提取的精 度得到了很大提高，也显著提高了算法效率。 此外， 有研究者将比较成熟的优化算法应用到环境特征的 提取，如文献［１８］将自组织竞争学习神经网络（ｓｅｌｆ⁃ ｏｒｇａｎｉｚｉｎｇ ｆｅａｔｕｒｅ ｍａｐｐｉｎｇ，ＳＯＭ）方法用于激光雷达 数据的处理，提取更加精确的环境线段、角点等特 征，提高了算法的计算效率以及分类性能得到进一 步的提高。 也有研究者采用卡尔曼滤波对环境的线 段等特征进行提取，不仅对线段特征的误差标准等 概率特性进行较好地描述和更新，而且受传感器的 非线性影响也较小。 ３ 基于测距式传感器 ＳＬＡＭ 常用方法 目前，未知环境中机器人利用测距式传感器的 距离信息完成 ＳＬＡＭ 任务，其基本思想是通过寻找 当前局部环境地图与已获得的增量式全局环境地图 间的对应关系，减少航迹推算误差造成的移动机器 人定位的不确定性［１９］ ，获得尽可能精确的机器人当 前位姿信息，完备全局环境地图。 目前主要方法可 分为基于贝叶斯理论的概率推理定位算法和基于矩 阵分解的定位算法两大类。 前者把概率理论应用到 移动机器人定位过程中，即在机器人每个可能的位 置设置一种概率值，根据机器人运动过程中测距式 传感器感知到的新环境距离信息更新机器人可能位 置的概率值，从而减少机器人位置不确定性。 其实 质是把移动机器人 ＳＬＡＭ 问题认为是贝叶斯评估过 ·６５６· 智 能 系 统 学 报 第 １０ 卷
第5期 刘建华，等：测距式传感器同时定位与地图创建综述 ·657 程，通过融合内部传感器输入数据和外部传感器观 大：文献[22]针对EKF中观测噪声方差估计不准确 测数据，完成移动机器人隐式位姿的预测及更新，使 导致滤波器性能下降甚至发散的问题，将模糊卡尔 其隐式位姿状态信度达到最优。比较典型的方法 曼滤波技术应用于基于声呐的移动机器人的定位 有：基于扩展卡尔曼滤波(extended kalman filter, 中，提出了根据信息方差理论值和实际值的一致程 EKF)和基于粒子滤波(particle filter,PF)SLAM。 度在线调整观测噪声方差的自适应调节算法，解决 后者通过分析数据矩阵的特点，把矩阵分解出能反 了系统观测噪声水平不确定的问题，其性能优于 映原矩阵的某些特征的因式矩阵，通过回归计算因 EKF;文献[23]在光电系统中利用差分方式实现非 式矩阵获得当前机器人的位姿和环境信息，完成 线性函数线性化的EKF算法，这种方法无需计算复 SLAM任务。 杂的雅克比阵，且能得到更好的定位跟踪精度：文献 3.1基于EKF的SLAM定位方法 [2,24]提出将随机梯度下降法引入SLAM问题，达 EKF是将非线性模型（运动模型和观测模型） 到最小化系统非线性误差的影响，但是系统的收敛 在状态向量的邻域内泰勒展开，并取其一阶或二阶 速度变慢了。 项近似表示机器人的运动模型与观测模型。该方法 基于扩展的卡尔曼滤波器方法得到机器人领域 的基本思想是通过对包含移动机器人与环境特征之 广泛地接受，但随着SLAM问题研究的深入，出现了 间的关系以及机器人运动模型、观测模型不确定性 计算复杂度和数据关联问题，即缺陷二。协方差阵 信息的雅可比矩阵的迭代过程，完成误差协方差矩 的引入是EKF的最大特点，对协方差阵的计算也是 阵和增益矩阵的递推计算。正常情况下，雅可比矩 该算法的核心。当环境中的不确定因素和环境特征 阵是单调递减且收敛的，因此，对雅可比矩阵的处理 增加时，协方差阵的元素平方级增加，计算量复杂度 是EKF的核心。 为0(K)(K为特征数目)[2，即使在某一刻只观 基于扩展卡尔曼滤波器的SLAM方法是通过增 测到一个特征，整个协方差阵元素都必须更新，计算 量评估移动机器人位姿和环境特征位置的联合后验 量急剧增加。此外，机器人与环境特征及环境特征 概率来估计机器人位姿和构建地图，是一个循环迭 与环境特征之间的交叉相关性进一步增加了计算复 代地估计与校正过程。即机器人根据运动模型估计 杂度。文献[26]采用FPGA技术和并行处理流程 其新位置，根据机器人观测模型估计其可能观测到 完成多个特征同时匹配过程，提高了特征匹配效率； 的环境特征的距离信息：计算估计的观测值间与实 文献[27]采用表示环境特征之间相对位置的相对 际的观测值之间的误差，利用协方差计算卡尔曼滤 地图代替绝对地图，解决了环境特征之间的相关问 波增益参数，并利用该参数对机器人的估计位置校 题：文献[28]用稀疏化信息矩阵代替协方差矩阵， 正：将新观测到的环境特征信息加入地图，逐步完成 即稀疏扩展信息滤波方法，使更新时间变为常数，不 增量式环境地图的构建。在估计与校正过程中若系 随特征的增加而增加。 统状态的协方差较大，侧用观测值校正，使系统偏差 另外，基于EKF的SLAM算法存在2个较难解 变小：若观测协方差较大，则认为观测值不可靠，对 决的问题：1)假设观测量与环境特征的数据关联为 观测值做轻微校正20)。 已知，即认为它们之间必存在数据关联，一旦观测值 以EKF理论为基础的SLAM的方法存在一些 和环境特征之间出现微小的关联错误，将会导致 固有的缺陷。缺陷一是非线性误差的存在)，由于 EKF滤波器发散而使SLAM过程失败。2)机器人 EKF算法在估计点处通过泰勒展开式对非线性系 状态向量无法表征机器人的历史路径，无法解耦地 统进行局部线性化，舍去了高阶项，虽然简化了问 图信息与机器人状态的关系，当环境地图增大时导 题，但使系统产生了截断误差。如系统非线性程度 致计算量急剧增大。因此，该方法不适合大规模环 较高时，由于系统无法满足上述线性化假设而产生 境和动态环境的SLAM。 很大的误差，从而造成系统不收敛的严重后果，因而 目前，在EKF的SLAM方法的基础上，提出了 基于EKF的SLAM的精度和鲁棒性受到影响。针 一些改进算法，如无迹变换的UKF-SLAM、容积卡尔 对EKF线性化存在的误差，研究者在如何提高 曼滤波SLAM、平方根容积卡尔曼滤波SLAM等。 SLAM问题的精度上展开了研究，提出了许多改进 通过不同的方法获得SLAM的后验概率密度，提高 算法。如文献[21]提出迭代扩展卡尔曼滤波算法 了定位精度。 (iterated extended Kalman filter,IEKF),结合微光雷 3.2基于PF的SLAM定位方法 达传感器采集的外部环境特征信息，在测量更新阶 P℉是20世纪90年代中后期发展起来的一种 段多次迭代滤波估计值，从而减少由于泰勒展开局 比较新的滤波算法。文献[29]将P℉与机器人运动 部线性化带来的截断误差，提高了SLAM的定位精 及感知的概率模型相结合，提出了P℉的定位方法； 度，但是其缺点是由于迭代次数的增加，计算量会增 其主要思想是用多个滤波器来估计机器人的各个可
程，通过融合内部传感器输入数据和外部传感器观 测数据，完成移动机器人隐式位姿的预测及更新，使 其隐式位姿状态信度达到最优。 比较典型的方法 有：基于扩展卡尔曼滤波 （ ｅｘｔｅｎｄｅｄ ｋａｌｍａｎ ｆｉｌｔｅｒ， ＥＫＦ） 和基于粒子滤波（ ｐａｒｔｉｃｌｅ ｆｉｌｔｅｒ， ＰＦ） ＳＬＡＭ。 后者通过分析数据矩阵的特点，把矩阵分解出能反 映原矩阵的某些特征的因式矩阵，通过回归计算因 式矩阵获得当前机器人的位姿和环境信息，完成 ＳＬＡＭ 任务。 ３．１ 基于 ＥＫＦ 的 ＳＬＡＭ 定位方法 ＥＫＦ 是将非线性模型（运动模型和观测模型） 在状态向量的邻域内泰勒展开，并取其一阶或二阶 项近似表示机器人的运动模型与观测模型。 该方法 的基本思想是通过对包含移动机器人与环境特征之 间的关系以及机器人运动模型、观测模型不确定性 信息的雅可比矩阵的迭代过程，完成误差协方差矩 阵和增益矩阵的递推计算。 正常情况下，雅可比矩 阵是单调递减且收敛的，因此，对雅可比矩阵的处理 是 ＥＫＦ 的核心。 基于扩展卡尔曼滤波器的 ＳＬＡＭ 方法是通过增 量评估移动机器人位姿和环境特征位置的联合后验 概率来估计机器人位姿和构建地图，是一个循环迭 代地估计与校正过程。 即机器人根据运动模型估计 其新位置，根据机器人观测模型估计其可能观测到 的环境特征的距离信息；计算估计的观测值间与实 际的观测值之间的误差，利用协方差计算卡尔曼滤 波增益参数，并利用该参数对机器人的估计位置校 正；将新观测到的环境特征信息加入地图，逐步完成 增量式环境地图的构建。 在估计与校正过程中若系 统状态的协方差较大，则用观测值校正，使系统偏差 变小；若观测协方差较大，则认为观测值不可靠，对 观测值做轻微校正［２０］ 。 以 ＥＫＦ 理论为基础的 ＳＬＡＭ 的方法存在一些 固有的缺陷。 缺陷一是非线性误差的存在［２］ ，由于 ＥＫＦ 算法在估计点处通过泰勒展开式对非线性系 统进行局部线性化，舍去了高阶项，虽然简化了问 题，但使系统产生了截断误差。 如系统非线性程度 较高时，由于系统无法满足上述线性化假设而产生 很大的误差，从而造成系统不收敛的严重后果，因而 基于 ＥＫＦ 的 ＳＬＡＭ 的精度和鲁棒性受到影响。 针 对 ＥＫＦ 线性化存在的误差， 研究者在如何提高 ＳＬＡＭ 问题的精度上展开了研究，提出了许多改进 算法。 如文献［２１］提出迭代扩展卡尔曼滤波算法 （ｉｔｅｒａｔｅｄ ｅｘｔｅｎｄｅｄ Ｋａｌｍａｎ ｆｉｌｔｅｒ，ＩＥＫＦ），结合激光雷 达传感器采集的外部环境特征信息，在测量更新阶 段多次迭代滤波估计值，从而减少由于泰勒展开局 部线性化带来的截断误差，提高了 ＳＬＡＭ 的定位精 度，但是其缺点是由于迭代次数的增加，计算量会增 大；文献［２２］针对 ＥＫＦ 中观测噪声方差估计不准确 导致滤波器性能下降甚至发散的问题，将模糊卡尔 曼滤波技术应用于基于声呐的移动机器人的定位 中，提出了根据信息方差理论值和实际值的一致程 度在线调整观测噪声方差的自适应调节算法，解决 了系统观测噪声水平不确定的问题，其性能优于 ＥＫＦ；文献［２３］在光电系统中利用差分方式实现非 线性函数线性化的 ＥＫＦ 算法，这种方法无需计算复 杂的雅克比阵，且能得到更好的定位跟踪精度；文献 ［２，２４］提出将随机梯度下降法引入 ＳＬＡＭ 问题，达 到最小化系统非线性误差的影响，但是系统的收敛 速度变慢了。 基于扩展的卡尔曼滤波器方法得到机器人领域 广泛地接受，但随着 ＳＬＡＭ 问题研究的深入，出现了 计算复杂度和数据关联问题，即缺陷二。 协方差阵 的引入是 ＥＫＦ 的最大特点，对协方差阵的计算也是 该算法的核心。 当环境中的不确定因素和环境特征 增加时，协方差阵的元素平方级增加，计算量复杂度 为 Ｏ（Ｋ ２ ） （ Ｋ 为特征数目） ［２５］ ，即使在某一刻只观 测到一个特征，整个协方差阵元素都必须更新，计算 量急剧增加。 此外，机器人与环境特征及环境特征 与环境特征之间的交叉相关性进一步增加了计算复 杂度。 文献［２６］ 采用 ＦＰＧＡ 技术和并行处理流程 完成多个特征同时匹配过程，提高了特征匹配效率； 文献［２７］采用表示环境特征之间相对位置的相对 地图代替绝对地图，解决了环境特征之间的相关问 题；文献［２８］用稀疏化信息矩阵代替协方差矩阵， 即稀疏扩展信息滤波方法，使更新时间变为常数，不 随特征的增加而增加。 另外，基于 ＥＫＦ 的 ＳＬＡＭ 算法存在 ２ 个较难解 决的问题：１）假设观测量与环境特征的数据关联为 已知，即认为它们之间必存在数据关联，一旦观测值 和环境特征之间出现微小的关联错误，将会导致 ＥＫＦ 滤波器发散而使 ＳＬＡＭ 过程失败。 ２） 机器人 状态向量无法表征机器人的历史路径，无法解耦地 图信息与机器人状态的关系，当环境地图增大时导 致计算量急剧增大。 因此，该方法不适合大规模环 境和动态环境的 ＳＬＡＭ。 目前，在 ＥＫＦ 的 ＳＬＡＭ 方法的基础上，提出了 一些改进算法，如无迹变换的 ＵＫＦ⁃ＳＬＡＭ、容积卡尔 曼滤波 ＳＬＡＭ、平方根容积卡尔曼滤波 ＳＬＡＭ 等。 通过不同的方法获得 ＳＬＡＭ 的后验概率密度，提高 了定位精度。 ３．２ 基于 ＰＦ 的 ＳＬＡＭ 定位方法 ＰＦ 是 ２０ 世纪 ９０ 年代中后期发展起来的一种 比较新的滤波算法。 文献［２９］将 ＰＦ 与机器人运动 及感知的概率模型相结合，提出了 ＰＦ 的定位方法； 其主要思想是用多个滤波器来估计机器人的各个可 第 ５ 期 刘建华，等：测距式传感器同时定位与地图创建综述 ·６５７·
·658 智能系统学报 第10卷 能位置的概率，利用观测对每个滤波器进行加权处 的行进轨迹，利用观侧值计算各个粒子的权重。具 理，使机器人最有可能位置的概率越来越大[0。在 体到每个粒子，机器人的行进轨迹是确定的，环境特 实际应用中，基于PF的SLAM方法是用一组相关 征标志之间相互独立，特征标志的观测信息仅与机 权重的随机样本集合，以及基于这些样本的估计来 器人的位姿有关。每个粒子采用N个卡尔曼滤波 近似表征后验概率密度函数，采用样本均值代替积 器分别估计地图中N个特征的位置。对于 分运算来获得状态最小方差估计的过程。 FastSLAM算法的复杂度，如果采用K个粒子实现 SLAM主要任务是位姿估计和地图构建，2者之 SLAM,总共有KN个卡尔曼滤波器，其复杂度为 间又存在相互依赖的关系，这使基于PF的SLAM O(KN),利用一些算法（如树型数据结构）进行优 问题变得非常复杂。文献[31]提出了SLAM问题 化，其复杂度可以进一步减小到O(KogN)2)。针 的一种有效解决方案，即后验概率估计的处理方法， 对数据关联错误，通过减小该粒子的权重来降低关 其基本思想是通过Rao-Blackwellized将SLAM问题 联错误对定位的影响[3]。FastSLAM方法的另一个 分解为轨迹估计过程与地图估计过程。先用粒子滤 优点是相比较于卡尔曼滤波定位方法，可以较好地 波器估计移动机器人的运动轨迹，再用卡尔曼滤波 解决机器人的非线性和非高斯运动问题。 估计环境地图，从而将SLAM问题转化为低维估计 以PF为理论基础的FastSLAM算法， 问题[32.3 FastSLAM1.0和FastSLAM2.0有2个版本。 p(s,m|z14,山1-1）=p(s1|4,u1-1） FastsLAM1.0用粒子滤波器估计移动机器人位姿，用 p(m1之14,41-1)=p(s1114,山1t-1)p(m|5141） EKF估计环境特征。但其直接把SLAM过程模型作 式中：s,为移动机器人轨迹，山1-1为里程计测量， 为采样粒子重要性函数，从重要性函数抽取的样本 m为观测z1,得到的地图。 不能准确表达真实的后验概率密度函数，当环境观 在已知s,和的条件下，可以得到地图的后 测值噪声较小时，易造成严重的样本退化问题和定 验概率p(mlsa,it),而建图的Rao-Blackweiiized 位精度下降。FatSLAM2.0用EKF递归估计机器人 通过P℉方法得到机器人潜在轨迹的后验概率 位姿，构建融入机器人前期位姿信息的高斯分布函 数为重要函数，缓解了样本退化问题，提高了算法的 p(s1114,山1-1)，并为每个粒子分配一个合适的 精度。针对测距式传感器，如激光雷达，影响粒子滤 “粒子地图”，该地图是在给定了由相应粒子所表示 波器性能主要因素是建议分布的选择和选择性采 的s1和1条件下建立；换言之，机器人在未知环 境中循环运行一圈，每个粒子都估计机器人的轨迹， 样。由于激光扫描仪的精度导致了峰值很高的似然 函数，与大部分粒子滤波器选用里程计运动模型 并相应保留一个“粒子地图”，该地图根据轨迹估计 不断更新，即根据已知位姿构建地图。 p(s,Is-1,山，-1)为建议分布不同，其选用高斯建议 分布。这样既考虑了最近观测数据，也为高效采样 常用的Rao-Blackweiiized粒子滤波器建图过程 提供了可能，其结果比用里程计运动模型更具有确 主要有以下4个步骤： 定性)。在重采样过程中，低权值粒子被高权值粒 1)采样，子代粒子是从当前粒子中根据建议分 子代替，这有可能删除较好的粒子。在构建图过程 布g采样得到： 中，尤其在嵌套环路环境中，外层环路构建图中需要 2)每个粒子赋重要性权值： 的粒子并不一定是内层环路高权值的粒子。文献 p(x|x4,之14，u1） ω= [35]介绍了一种衡量有效粒子数的方法来评价当 q(x1xi,21,山1t） 前粒子集能否表征后验估计，只有在需要时重采样 3)重采样，其包含2层意思：1)用有限粒子近 才被执行，降低了较好粒子被替换的几率。文献 似一个连续分布，2)重采样允许在真实分布与建议 [36]通过迭代卡尔曼滤波方法融入新的观测值，用 分布有差异的情况下使用粒子滤波器： 复制样本与剔除样本的线性组合降低样本退化及样 4)地图估计，通过已知位姿更新每个粒子的 本贫化。 “粒子地图”。 综上所述，FastSLAM算法获取建议分布函数与 在Rao-Blackwellized粒子滤波的基础上，Monte- 估计环境特征位置依然是基于EKF理论，同样存在 merlo等[3]提出FastSLAM算法，与基于EKF的 样本退化问题。因此，FastSLAM算法发展趋势主要 SLAM方法不同，其用不同的粒子表征机器人的历 从以下2方面考虑： 史路径，能够解耦地图特征信息与机器人状态信息： 1)改进机器人位姿路径评估的粒子滤波。改 即将SLAM问题看成机器人行进轨迹的PF估计和 进建议分布使其近似真实分布：改进重采样策略：改 机器人位姿估计上环境特征位置卡尔曼滤波估计过 进自适应机制，如自适应粒子数目、自适应重采样技 程。粒子滤波器中每个粒子代表机器人的一条可能 术及自适应运动模型
能位置的概率，利用观测对每个滤波器进行加权处 理，使机器人最有可能位置的概率越来越大［３０］ 。 在 实际应用中，基于 ＰＦ 的 ＳＬＡＭ 方法是用一组相关 权重的随机样本集合，以及基于这些样本的估计来 近似表征后验概率密度函数，采用样本均值代替积 分运算来获得状态最小方差估计的过程。 ＳＬＡＭ 主要任务是位姿估计和地图构建，２ 者之 间又存在相互依赖的关系，这使基于 ＰＦ 的 ＳＬＡＭ 问题变得非常复杂。 文献［３１］ 提出了 ＳＬＡＭ 问题 的一种有效解决方案，即后验概率估计的处理方法， 其基本思想是通过 Ｒａｏ⁃Ｂｌａｃｋｗｅｌｌｉｚｅｄ 将 ＳＬＡＭ 问题 分解为轨迹估计过程与地图估计过程。 先用粒子滤 波器估计移动机器人的运动轨迹，再用卡尔曼滤波 估计环境地图，从而将 ＳＬＡＭ 问题转化为低维估计 问题［３２⁃３３］ 。 ｐ（ｓ１：ｔ，ｍ ｜ ｚ１：ｔ，ｕ１：ｔ－１ ） ＝ ｐ（ｓ１：ｔ ｜ ｚ１：ｔ，ｕ１：ｔ－１ ） ｐ（ｍ ｜ ｚ１：ｔ，ｕ１：ｔ－１ ） ＝ ｐ（ｓ１：ｔ ｜ ｚ１：ｔ，ｕ１：ｔ－１ ）ｐ（ｍ ｜ ｓ１：ｔ，ｚ１：ｔ） 式中： ｓ１：ｔ 为移动机器人轨迹， ｕ１：ｔ－１ 为里程计测量， ｍ 为观测 ｚ１：ｔ 得到的地图。 在已知 ｓ１：ｔ 和 ｚ１：ｔ 的条件下，可以得到地图的后 验概率 ｐ（ｍ ｜ ｓ１：ｔ，ｚ１：ｔ） ，而建图的 Ｒａｏ⁃Ｂｌａｃｋｗｅｉｉｉｚｅｄ 通过 ＰＦ 方法得到机器人潜在轨迹的后验概率 ｐ（ｓ１：ｔ ｜ ｚ１：ｔ，ｕ１：ｔ－１ ） ，并为每个粒子分配一个合适的 “粒子地图”，该地图是在给定了由相应粒子所表示 的 ｓ１：ｔ 和 ｚ１：ｔ 条件下建立；换言之，机器人在未知环 境中循环运行一圈，每个粒子都估计机器人的轨迹， 并相应保留一个“粒子地图”，该地图根据轨迹估计 不断更新，即根据已知位姿构建地图。 常用的 Ｒａｏ⁃Ｂｌａｃｋｗｅｉｉｉｚｅｄ 粒子滤波器建图过程 主要有以下 ４ 个步骤： １）采样，子代粒子是从当前粒子中根据建议分 布 ｑ 采样得到； ２）每个粒子赋重要性权值： ω ｉ ｔ ＝ ｐ（ｘ ｉ ｔ ｜ ｘ ｉ １：ｔ，ｚ１：ｔ，ｕ１：ｔ） ｑ（ｘ ｉ ｔ ｜ ｘ ｉ １：ｔ，ｚ１：ｔ，ｕ１：ｔ） ３）重采样，其包含 ２ 层意思：１） 用有限粒子近 似一个连续分布，２）重采样允许在真实分布与建议 分布有差异的情况下使用粒子滤波器； ４）地图估计，通过已知位姿更新每个粒子的 “粒子地图”。 在 Ｒａｏ⁃Ｂｌａｃｋｗｅｌｌｉｚｅｄ 粒子滤波的基础上，Ｍｏｎｔｅ⁃ ｍｅｒｌｏ 等［３３］ 提 出 ＦａｓｔＳＬＡＭ 算 法， 与 基 于 ＥＫＦ 的 ＳＬＡＭ 方法不同，其用不同的粒子表征机器人的历 史路径，能够解耦地图特征信息与机器人状态信息； 即将 ＳＬＡＭ 问题看成机器人行进轨迹的 ＰＦ 估计和 机器人位姿估计上环境特征位置卡尔曼滤波估计过 程。 粒子滤波器中每个粒子代表机器人的一条可能 的行进轨迹，利用观测值计算各个粒子的权重。 具 体到每个粒子，机器人的行进轨迹是确定的，环境特 征标志之间相互独立，特征标志的观测信息仅与机 器人的位姿有关。 每个粒子采用 Ｎ 个卡尔曼滤波 器分 别 估 计 地 图 中 Ｎ 个 特 征 的 位 置。 对 于 ＦａｓｔＳＬＡＭ 算法的复杂度，如果采用 Ｋ 个粒子实现 ＳＬＡＭ，总共有 ＫＮ 个卡尔曼滤波器，其复杂度为 Ｏ（ＫＮ），利用一些算法（如树型数据结构） 进行优 化，其复杂度可以进一步减小到 Ｏ（ＫｌｏｇＮ） ［２５］ 。 针 对数据关联错误，通过减小该粒子的权重来降低关 联错误对定位的影响［３４］ 。 ＦａｓｔＳＬＡＭ 方法的另一个 优点是相比较于卡尔曼滤波定位方法，可以较好地 解决机器人的非线性和非高斯运动问题。 以 ＰＦ 为 理 论 基 础 的 ＦａｓｔＳＬＡＭ 算 法， ＦａｓｔＳＬＡＭ１． ０ 和 ＦａｓｔＳＬＡＭ２． ０ 有 ２ 个 版 本。 ＦａｓｔｓＬＡＭ１．０ 用粒子滤波器估计移动机器人位姿，用 ＥＫＦ 估计环境特征。 但其直接把 ＳＬＡＭ 过程模型作 为采样粒子重要性函数，从重要性函数抽取的样本 不能准确表达真实的后验概率密度函数，当环境观 测值噪声较小时，易造成严重的样本退化问题和定 位精度下降。 ＦａｔＳＬＡＭ２．０ 用 ＥＫＦ 递归估计机器人 位姿，构建融入机器人前期位姿信息的高斯分布函 数为重要函数，缓解了样本退化问题，提高了算法的 精度。 针对测距式传感器，如激光雷达，影响粒子滤 波器性能主要因素是建议分布的选择和选择性采 样。 由于激光扫描仪的精度导致了峰值很高的似然 函数，与大部分粒子滤波器选用里程计运动模型 ｐ（ｓｔ ｜ ｓｔ－１ ，ｕｔ－１ ） 为建议分布不同，其选用高斯建议 分布。 这样既考虑了最近观测数据，也为高效采样 提供了可能，其结果比用里程计运动模型更具有确 定性［３］ 。 在重采样过程中，低权值粒子被高权值粒 子代替，这有可能删除较好的粒子。 在构建图过程 中，尤其在嵌套环路环境中，外层环路构建图中需要 的粒子并不一定是内层环路高权值的粒子。 文献 ［３５］介绍了一种衡量有效粒子数的方法来评价当 前粒子集能否表征后验估计，只有在需要时重采样 才被执行，降低了较好粒子被替换的几率。 文献 ［３６］通过迭代卡尔曼滤波方法融入新的观测值，用 复制样本与剔除样本的线性组合降低样本退化及样 本贫化。 综上所述，ＦａｓｔＳＬＡＭ 算法获取建议分布函数与 估计环境特征位置依然是基于 ＥＫＦ 理论，同样存在 样本退化问题。 因此，ＦａｓｔＳＬＡＭ 算法发展趋势主要 从以下 ２ 方面考虑： １）改进机器人位姿路径评估的粒子滤波。 改 进建议分布使其近似真实分布；改进重采样策略；改 进自适应机制，如自适应粒子数目、自适应重采样技 术及自适应运动模型。 ·６５８· 智 能 系 统 学 报 第 １０ 卷
第5期 刘建华，等：测距式传感器同时定位与地图创建综述 ·659. 2)改进环境特征评估的EKF。采用混合卡尔 出最优的关联匹配，利用SVD获得路标在全局坐标 曼滤波器、UT变换的无迹卡尔曼滤波和无迹粒子滤 系和局部坐标系下的关系，完成地图的更新。文献 波等方法提高计算的有效性。 [40]将测距式SALM问题看成距离信息矩阵SVD 3.3基于矩阵奇异值分解(SVD)的SLAM方法 的过程，通过把距离矩阵SVD得到包含有路标信息 矩阵的分解因式是原矩阵特殊存在形式，能明 和机器人位姿的2个矩阵：通过线性回归算法或机 显反映出原矩阵的某些特征，从而为系统分析提供 器学习算法得到路标信息和机器人位姿，较好解决 有效的计算方法和理论分析根据。矩阵SVD是一 了传感器数据不同步和数据遗漏问题，其实质是在 种应用比较广泛的分解形式。在基于卡尔曼滤波定 系统状态空间搜索识别子空间，与其它定位算法相 位算法中，协方差矩阵对舍入误差较敏感，特别是高 比，不需要线性化模型，具有计算量小、跟踪性能好 维的复杂系统，在迭代过程中经常出现矩阵病态问 等优点。 题，导致协方差矩阵失去对称性和正定性，从而导致 文献[39-40]将基于矩阵SVD的SLAM与传统 算法失效。文献[37-38]通过协方差矩阵迭代分解 的基于概率“位置预测-观测-测量-预测-匹配更 计算，避免了协方差阵的求逆运算，减少了计算量， 新”过程比较，只依靠测距传感器来完成机器人的 提高了算法的稳定性。文献[39]通过激光雷达获 LAM问题，从理论上避免了运动模型的偏差和“绑 得障碍物和方向距离信息，求取直线交点坐标作为 架”问题。 路标，建立路标的相关矩阵，由直线、路标和相关矩 表1列出了3种方法的优缺点及其改进算法。 阵构成局部地图，通过匹配局部地图与全局地图找 表13种方法的优缺点及其改进算法 Table 1 The characteristics of the three methods and the improved algorithm 方法 优缺点 研究成果 改进的思路及性能 优点：计算量小（小环 无迹卡尔曼滤波 通过无迹变换估计后验密度，避免EKF中忽 境)，易于实现： UKF-SLAMC4] 略高阶项带来的问题，无需计算雅克比矩阵，达 缺点：概率密度函数 基于容积卡尔曼滤波到3阶精度，精度和稳定性优于EKF-SLAM; 满足高斯分布、线性化处SLAM2] 通过相同权重点经过非线性系统方程转换 EKF-SLAM 理误差大、对错误数据关 平方根容积卡尔曼滤后产生新的点预测系统状态，避免非线性模型的 联敏感、计算复杂（大环波SLAM) 线性化处理。 境)。 平方根容积卡尔曼滤波计算后验概率密度」 降低线性化误差，稳定性高于EKF-SLAM,UKF- SLAM,定位精度较EKF-SLAM提高1倍。 优点：模型非线性、高 UFastSLAM(44-4] 无迹变换估计转移概率密度和粒子建议分 维运算降为低维运算，在 辅助UFastSLAM算法可布函数，具有3阶非线性估计精度，估计精度高 计算复杂度、数据关联等 CFastSLAM[4-9] 于EKF-SLAM、UKF-SLAM、FastSLAM: 方面具有优势： 平方根容积FastSLAM倒 建议分布函数引入最近的观测值，估计精度 缺点：样本退化、样本 高于UFastSLAM、FastSLAM算法，复杂度高于 FastSLAM 贫化。 FastSLAM: 利用容积卡尔曼滤波产生建议分布函数，估 计精度高于UFastSLAM、FastSLAM算法，耗时少： 容积律计算非线性转移概率密度，容积律具 有3阶非线性估计精度，粒子数目少时，性能优 于UFastSLAM与FastSLAM.。 优点：不需要线性化 协方差矩阵奇异值分 避免了协方差阵的求逆运算、计算误差和舍 模型，降维，具有计算量解3】 入误差，保证了状态协方差矩阵失去正定性，减 小、跟踪性能好，避免了运 路标矩阵奇异值分少计算量，提高了算法的稳定性： SVD-SLAM 动模型的偏差和“绑架”问解[o 把SLAM看做矩阵分解过程，解耦距离矩阵 题； 为路标信息矩阵和机器人位姿矩阵，有效抑制噪 缺点：依据实际情况 声对滤波的影响，定位精度高，实时性高。 不同，如病态矩阵
２）改进环境特征评估的 ＥＫＦ。 采用混合卡尔 曼滤波器、ＵＴ 变换的无迹卡尔曼滤波和无迹粒子滤 波等方法提高计算的有效性。 ３．３ 基于矩阵奇异值分解（ＳＶＤ）的 ＳＬＡＭ 方法 矩阵的分解因式是原矩阵特殊存在形式，能明 显反映出原矩阵的某些特征，从而为系统分析提供 有效的计算方法和理论分析根据。 矩阵 ＳＶＤ 是一 种应用比较广泛的分解形式。 在基于卡尔曼滤波定 位算法中，协方差矩阵对舍入误差较敏感，特别是高 维的复杂系统，在迭代过程中经常出现矩阵病态问 题，导致协方差矩阵失去对称性和正定性，从而导致 算法失效。 文献［３７⁃３８］通过协方差矩阵迭代分解 计算，避免了协方差阵的求逆运算，减少了计算量， 提高了算法的稳定性。 文献［３９］ 通过激光雷达获 得障碍物和方向距离信息，求取直线交点坐标作为 路标，建立路标的相关矩阵，由直线、路标和相关矩 阵构成局部地图，通过匹配局部地图与全局地图找 出最优的关联匹配，利用 ＳＶＤ 获得路标在全局坐标 系和局部坐标系下的关系，完成地图的更新。 文献 ［４０］将测距式 ＳＡＬＭ 问题看成距离信息矩阵 ＳＶＤ 的过程，通过把距离矩阵 ＳＶＤ 得到包含有路标信息 和机器人位姿的 ２ 个矩阵；通过线性回归算法或机 器学习算法得到路标信息和机器人位姿，较好解决 了传感器数据不同步和数据遗漏问题，其实质是在 系统状态空间搜索识别子空间，与其它定位算法相 比，不需要线性化模型，具有计算量小、跟踪性能好 等优点。 文献［３９⁃４０］将基于矩阵 ＳＶＤ 的 ＳＬＡＭ 与传统 的基于概率“位置预测－观测－测量－预测－匹配更 新”过程比较，只依靠测距传感器来完成机器人的 ＳＬＡＭ 问题，从理论上避免了运动模型的偏差和“绑 架”问题。 表 １ 列出了 ３ 种方法的优缺点及其改进算法。 表 １ ３ 种方法的优缺点及其改进算法 Ｔａｂｌｅ １ Ｔｈｅ ｃｈａｒａｃｔｅｒｉｓｔｉｃｓ ｏｆ ｔｈｅ ｔｈｒｅｅ ｍｅｔｈｏｄｓ ａｎｄ ｔｈｅ ｉｍｐｒｏｖｅｄ ａｌｇｏｒｉｔｈｍ 方法 优缺点 研究成果 改进的思路及性能 ＥＫＦ⁃ＳＬＡＭ 优点：计算量小（小环 境），易于实现； 缺点：概率密度函数 满足高斯分布、线性化处 理误差大、对错误数据关 联敏 感、 计 算 复 杂 （ 大 环 境）。 无迹卡尔曼滤波 ＵＫＦ⁃ＳＬＡＭ ［４１］ 基于容积卡尔曼滤波 ＳＬＡＭ ［４２］ 平方根容积卡尔曼滤 波 ＳＬＡＭ ［４３］ 通过无迹变换估计后验密度，避免 ＥＫＦ 中忽 略高阶项带来的问题，无需计算雅克比矩阵，达 到 ３ 阶精度，精度和稳定性优于 ＥＫＦ⁃ＳＬＡＭ； 通过相同权重点经过非线性系统方程转换 后产生新的点预测系统状态，避免非线性模型的 线性化处理。 平方根容积卡尔曼滤波计算后验概率密度， 降低线性化误差，稳定性高于 ＥＫＦ⁃ＳＬＡＭ、ＵＫＦ⁃ ＳＬＡＭ，定位精度较 ＥＫＦ⁃ＳＬＡＭ 提高 １ 倍。 ＦａｓｔＳＬＡＭ 优点：模型非线性、高 维运算降为低维运算，在 计算复杂度、数据关联等 方面具有优势； 缺点：样本退化、样本 贫化。 ＵＦａｓｔＳＬＡＭ ［４４⁃４６］ 辅助 ＵＦａｓｔＳＬＡＭ 算法［４７］ ＣＦａｓｔＳＬＡＭ ［４８⁃４９］ 平方根容积 ＦａｓｔＳＬＡＭ ［３２］ 无迹变换估计转移概率密度和粒子建议分 布函数，具有 ３ 阶非线性估计精度，估计精度高 于 ＥＫＦ⁃ＳＬＡＭ、ＵＫＦ⁃ＳＬＡＭ、ＦａｓｔＳＬＡＭ； 建议分布函数引入最近的观测值，估计精度 高于 ＵＦａｓｔＳＬＡＭ、 ＦａｓｔＳＬＡＭ 算 法， 复 杂 度 高 于 ＦａｓｔＳＬＡＭ； 利用容积卡尔曼滤波产生建议分布函数，估 计精度高于 ＵＦａｓｔＳＬＡＭ、ＦａｓｔＳＬＡＭ 算法，耗时少； 容积律计算非线性转移概率密度，容积律具 有 ３ 阶非线性估计精度，粒子数目少时，性能优 于 ＵＦａｓｔＳＬＡＭ 与 ＦａｓｔＳＬＡＭ。 ＳＶＤ⁃ＳＬＡＭ 优点：不需要线性化 模型， 降 维， 具 有 计 算 量 小、跟踪性能好，避免了运 动模型的偏差和“绑架”问 题； 缺点：依据实际情况 不同，如病态矩阵。 协方差矩阵奇异值分 解［３７⁃３８］ 路标 矩 阵 奇 异 值 分 解［４０］ 避免了协方差阵的求逆运算、计算误差和舍 入误差，保证了状态协方差矩阵失去正定性，减 少计算量，提高了算法的稳定性； 把 ＳＬＡＭ 看做矩阵分解过程，解耦距离矩阵 为路标信息矩阵和机器人位姿矩阵，有效抑制噪 声对滤波的影响，定位精度高，实时性高。 第 ５ 期 刘建华，等：测距式传感器同时定位与地图创建综述 ·６５９·