【智能系统】粗匹配和局部尺度压缩搜索下的快速ICP-SLAM

第12卷第3期 智能系统学报 Vol.12 No.3 2017年6月 CAAl Transactions on Intelligent Systems Jun.2017 D0D0I:10.11992/tis.201605029 网络出版地址：http:/kns.cmki.ne/kcms/detail/23.1538.TP.20170705.1656.008.html 粗匹配和局部尺度压缩搜索下的快速ICP-SLAM 张金艺12，梁滨1，唐笛恺，姚维强2，鲍深 (1.上海大学通信与信息工程学院，上海200010：2.上海大学微电子研究与开发中心，上海200010) 摘要：ICP-SLAM在自主机器人和无人驾驶领域得到了极大的关注，但传统ICP.SLAM缺少当前帧和全局地图的相 对位置关系，因此本文ICP算法必须经过大量的迭代之后才能达到收敛条件，这导致传统ICP-SLAM实时性很差。 并且在每一次的迭代过程中，必须通过全局搜索才能完成匹配点搜索，这进一步降低了传统ICP-SLAM的实时性。 为此，提出了一种快速ICP-SLAM方案。首先，通过MEMS磁力计和全局地标计算出初始位姿矩阵，通过该初始位姿 矩阵实现当前帧和全局地图之间粗匹配，进而减少达到收敛条件的迭代次数。其次，在每次迭代过程中，将采用局 部尺度压缩搜索完成匹配点搜索，从而减小ICP-SLAM的计算开销，提高ICP-SLAM实时性：同时，每次迭代完成之 后，还将通过动态阈值缩小搜索范围，达到加快匹配点搜索的速度，进而提高ICP-SLAM实时性。实验结果表明，和 传统ICP.SLAM相比，在理想室内静止场景下，快速ICP-SLAM的迭代次数最高减小了92.34%，ICP算法运行时间最 高降低了98.86%。除此之外，ICP-SLAM的整体负载也被保持在可控范围内，ICP-SLAM的整体性能得到很大的 提升。 关键词：ICP-SLAM;粗匹配；初始姿态矩阵；局部搜索；动态阈值；实时性；点云；迭代： 中图分类号：TP11文献标志码：A文章编号：1673-4785(2017)03-0413-09 中文引用格式：张金艺，梁滨，唐笛恺，等.粗匹配和局部尺度压缩搜索下的快速ICP-SLAM[J].智能系统学报，2017,12(3)： 413-421. 英文引用格式：ZHANG Jinyi,LIANG bin,TANG Dikai,etal.Fast ICP-SLAM with rough alignment and local scale-compressed searching[J].CAAI transactions on intelligent systems,2017,12(3):413-421. Fast ICP-SLAM with rough alignment and local scale-compressed searching ZHANG Jinyi'2,LIANG Bin',TANG Dikai',YAO Weiqiang?,BAO Shen2 (1.School of Communication and Information Engineering,Shanghai University,Shanghai 200010,China;2.Microelectronic Research and Development Center,Shanghai University,Shanghai 200010,China) Abstract:ICP-SLAM has received much attention in the field of autonomous robots and unmanned cars.However, two deficiencies in traditional ICP-SLAM usually result in poor real-time performance.The first is the fact that the relative position between the current scan frame and the global map is not previously known.As a result,the ICP algorithm takes a large number of iterations to reach convergence.The second is that the establishment of correspondence is carried out by global searching and this requires an enormous amount of computational time.To overcome these problems,a fast ICP-SLAM is proposed.To decrease the number of iterations a rough alignment, based on an initial pose matrix,is proposed.In detail,the initial pose matrix is computed using a MEMS magnetometer and global landmarks.Then,a rough alignment is applied between the current scan frame and the global map at the beginning of the ICP algorithm with an initial pose matrix.To accelerate the establishment of correspondence,local scale-compressed searching with a dynamic threshold is proposed where match-points are found within a progressively constrictive range.Compared to traditional ICP-SLAM,under ideal stable conditions, the best experimental results show amount of iteration for ICP algorithm to reach convergence reduces 92.34%and ICP algorithm runtime reduces 98.86%.In addition,computational cost is kept at a stable level due to the elimination of accumulated computational consumption.Moreover,great improvement is observed in the quality and robustness of SLAM Keywords:ICP-SLAM;rough alignment;initial pose matrix;local searching;dynamic threshold;real-time performance;cloud point;iteration 自主机器人和无人驾驶车成为近几年来人工智能领域研究的新热点，该技术在服务业、交通运 收稿日期：2016-05-27.网络出版日期：2017-07-05 输、工业、环境勘探、国防以及生活各个方面都有着 基金项目：国家“863”计划基金项目(2013AA03A1121,2013AABA112):上广阔的应用前景。同时德国工业4.0及中国制造 海市教委重点学科资助项目(J50104). 通信作者：梁滨.E-mail:zhangjinyic@staff.shu.cdu.cn 2025也把焦点聚集在无人化和智能化，智能产业正

第 １２ 卷第 ３ 期 智 能 系 统 学 报 Ｖｏｌ．１２ №．３ ２０１７ 年 ６ 月 ＣＡＡＩ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｉｎｔｅｌｌｉｇｅｎｔ Ｓｙｓｔｅｍｓ Ｊｕｎ． ２０１７ ＤＯＩＤＯＩ：１０．１１９９２ ／ ｔｉｓ．２０１６０５０２９ 网络出版地址：ｈｔｔｐ： ／ ／ ｋｎｓ．ｃｎｋｉ．ｎｅｔ ／ ｋｃｍｓ／ ｄｅｔａｉｌ ／ ２３．１５３８．ＴＰ．２０１７０７０５．１６５６．００８．ｈｔｍｌ 粗匹配和局部尺度压缩搜索下的快速 ＩＣＰ⁃ＳＬＡＭ 张金艺１，２ ，梁滨１ ，唐笛恺１ ，姚维强２ ，鲍深２ （１．上海大学 通信与信息工程学院，上海 ２０００１０；２． 上海大学 微电子研究与开发中心，上海 ２０００１０） 摘 要：ＩＣＰ⁃ＳＬＡＭ 在自主机器人和无人驾驶领域得到了极大的关注，但传统 ＩＣＰ⁃ＳＬＡＭ 缺少当前帧和全局地图的相 对位置关系，因此本文 ＩＣＰ 算法必须经过大量的迭代之后才能达到收敛条件，这导致传统 ＩＣＰ⁃ＳＬＡＭ 实时性很差。 并且在每一次的迭代过程中，必须通过全局搜索才能完成匹配点搜索，这进一步降低了传统 ＩＣＰ⁃ＳＬＡＭ 的实时性。 为此，提出了一种快速 ＩＣＰ⁃ＳＬＡＭ 方案。 首先，通过 ＭＥＭＳ 磁力计和全局地标计算出初始位姿矩阵，通过该初始位姿 矩阵实现当前帧和全局地图之间粗匹配，进而减少达到收敛条件的迭代次数。 其次，在每次迭代过程中，将采用局 部尺度压缩搜索完成匹配点搜索，从而减小 ＩＣＰ⁃ＳＬＡＭ 的计算开销，提高 ＩＣＰ⁃ＳＬＡＭ 实时性；同时，每次迭代完成之 后，还将通过动态阈值缩小搜索范围，达到加快匹配点搜索的速度，进而提高 ＩＣＰ⁃ＳＬＡＭ 实时性。 实验结果表明，和 传统 ＩＣＰ⁃ＳＬＡＭ 相比，在理想室内静止场景下，快速 ＩＣＰ⁃ＳＬＡＭ 的迭代次数最高减小了 ９２．３４％，ＩＣＰ 算法运行时间最 高降低了 ９８．８６％。 除此之外，ＩＣＰ⁃ＳＬＡＭ 的整体负载也被保持在可控范围内，ＩＣＰ⁃ＳＬＡＭ 的整体性能得到很大的 提升。 关键词：ＩＣＰ⁃ＳＬＡＭ；粗匹配；初始姿态矩阵；局部搜索；动态阈值；实时性；点云；迭代； 中图分类号： ＴＰ１１ 文献标志码：Ａ 文章编号：１６７３－４７８５（２０１７）０３－０４１３－０９ 中文引用格式：张金艺，梁滨，唐笛恺，等． 粗匹配和局部尺度压缩搜索下的快速 ＩＣＰ－ＳＬＡＭ［ Ｊ］． 智能系统学报， ２０１７， １２（ ３）： ４１３－４２１． 英文引用格式：ＺＨＡＮＧ Ｊｉｎｙｉ， ＬＩＡＮＧ ｂｉｎ，ＴＡＮＧ Ｄｉｋａｉ，ｅｔ ａｌ． Ｆａｓｔ ＩＣＰ⁃ＳＬＡＭ ｗｉｔｈ ｒｏｕｇｈ ａｌｉｇｎｍｅｎｔ ａｎｄ ｌｏｃａｌ ｓｃａｌｅ⁃ｃｏｍｐｒｅｓｓｅｄ ｓｅａｒｃｈｉｎｇ［Ｊ］． ＣＡＡＩ ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ ｉｎｔｅｌｌｉｇｅｎｔ ｓｙｓｔｅｍｓ， ２０１７， １２（３）： ４１３－４２１． Ｆａｓｔ ＩＣＰ⁃ＳＬＡＭ ｗｉｔｈ ｒｏｕｇｈ ａｌｉｇｎｍｅｎｔ ａｎｄ ｌｏｃａｌ ｓｃａｌｅ⁃ｃｏｍｐｒｅｓｓｅｄ ｓｅａｒｃｈｉｎｇ ＺＨＡＮＧ Ｊｉｎｙｉ １，２ ， ＬＩＡＮＧ Ｂｉｎ １ ，ＴＡＮＧ Ｄｉｋａｉ １ ，ＹＡＯ Ｗｅｉｑｉａｎｇ ２ ，ＢＡＯ Ｓｈｅｎ ２ （１．Ｓｃｈｏｏｌ ｏｆ Ｃｏｍｍｕｎｉｃａｔｉｏｎ ａｎｄ Ｉｎｆｏｒｍａｔｉｏｎ Ｅｎｇｉｎｅｅｒｉｎｇ， Ｓｈａｎｇｈａｉ Ｕｎｉｖｅｒｓｉｔｙ， Ｓｈａｎｇｈａｉ ２０００１０， Ｃｈｉｎａ； ２． Ｍｉｃｒｏｅｌｅｃｔｒｏｎｉｃ Ｒｅｓｅａｒｃｈ ａｎｄ Ｄｅｖｅｌｏｐｍｅｎｔ Ｃｅｎｔｅｒ， Ｓｈａｎｇｈａｉ Ｕｎｉｖｅｒｓｉｔｙ， Ｓｈａｎｇｈａｉ ２０００１０， Ｃｈｉｎａ） Ａｂｓｔｒａｃｔ：ＩＣＰ⁃ＳＬＡＭ ｈａｓ ｒｅｃｅｉｖｅｄ ｍｕｃｈ ａｔｔｅｎｔｉｏｎ ｉｎ ｔｈｅ ｆｉｅｌｄ ｏｆ ａｕｔｏｎｏｍｏｕｓ ｒｏｂｏｔｓ ａｎｄ ｕｎｍａｎｎｅｄ ｃａｒｓ． Ｈｏｗｅｖｅｒ， ｔｗｏ ｄｅｆｉｃｉｅｎｃｉｅｓ ｉｎ ｔｒａｄｉｔｉｏｎａｌ ＩＣＰ⁃ＳＬＡＭ ｕｓｕａｌｌｙ ｒｅｓｕｌｔ ｉｎ ｐｏｏｒ ｒｅａｌ⁃ｔｉｍｅ ｐｅｒｆｏｒｍａｎｃｅ． Ｔｈｅ ｆｉｒｓｔ ｉｓ ｔｈｅ ｆａｃｔ ｔｈａｔ ｔｈｅ ｒｅｌａｔｉｖｅ ｐｏｓｉｔｉｏｎ ｂｅｔｗｅｅｎ ｔｈｅ ｃｕｒｒｅｎｔ ｓｃａｎ ｆｒａｍｅ ａｎｄ ｔｈｅ ｇｌｏｂａｌ ｍａｐ ｉｓ ｎｏｔ ｐｒｅｖｉｏｕｓｌｙ ｋｎｏｗｎ． Ａｓ ａ ｒｅｓｕｌｔ， ｔｈｅ ＩＣＰ ａｌｇｏｒｉｔｈｍ ｔａｋｅｓ ａ ｌａｒｇｅ ｎｕｍｂｅｒ ｏｆ ｉｔｅｒａｔｉｏｎｓ ｔｏ ｒｅａｃｈ ｃｏｎｖｅｒｇｅｎｃｅ． Ｔｈｅ ｓｅｃｏｎｄ ｉｓ ｔｈａｔ ｔｈｅ ｅｓｔａｂｌｉｓｈｍｅｎｔ ｏｆ ｃｏｒｒｅｓｐｏｎｄｅｎｃｅ ｉｓ ｃａｒｒｉｅｄ ｏｕｔ ｂｙ ｇｌｏｂａｌ ｓｅａｒｃｈｉｎｇ ａｎｄ ｔｈｉｓ ｒｅｑｕｉｒｅｓ ａｎ ｅｎｏｒｍｏｕｓ ａｍｏｕｎｔ ｏｆ ｃｏｍｐｕｔａｔｉｏｎａｌ ｔｉｍｅ． Ｔｏ ｏｖｅｒｃｏｍｅ ｔｈｅｓｅ ｐｒｏｂｌｅｍｓ， ａ ｆａｓｔ ＩＣＰ⁃ＳＬＡＭ ｉｓ ｐｒｏｐｏｓｅｄ． Ｔｏ ｄｅｃｒｅａｓｅ ｔｈｅ ｎｕｍｂｅｒ ｏｆ ｉｔｅｒａｔｉｏｎｓ ａ ｒｏｕｇｈ ａｌｉｇｎｍｅｎｔ， ｂａｓｅｄ ｏｎ ａｎ ｉｎｉｔｉａｌ ｐｏｓｅ ｍａｔｒｉｘ， ｉｓ ｐｒｏｐｏｓｅｄ． Ｉｎ ｄｅｔａｉｌ， ｔｈｅ ｉｎｉｔｉａｌ ｐｏｓｅ ｍａｔｒｉｘ ｉｓ ｃｏｍｐｕｔｅｄ ｕｓｉｎｇ ａ ＭＥＭＳ ｍａｇｎｅｔｏｍｅｔｅｒ ａｎｄ ｇｌｏｂａｌ ｌａｎｄｍａｒｋｓ． Ｔｈｅｎ， ａ ｒｏｕｇｈ ａｌｉｇｎｍｅｎｔ ｉｓ ａｐｐｌｉｅｄ ｂｅｔｗｅｅｎ ｔｈｅ ｃｕｒｒｅｎｔ ｓｃａｎ ｆｒａｍｅ ａｎｄ ｔｈｅ ｇｌｏｂａｌ ｍａｐ ａｔ ｔｈｅ ｂｅｇｉｎｎｉｎｇ ｏｆ ｔｈｅ ＩＣＰ ａｌｇｏｒｉｔｈｍ ｗｉｔｈ ａｎ ｉｎｉｔｉａｌ ｐｏｓｅ ｍａｔｒｉｘ． Ｔｏ ａｃｃｅｌｅｒａｔｅ ｔｈｅ ｅｓｔａｂｌｉｓｈｍｅｎｔ ｏｆ ｃｏｒｒｅｓｐｏｎｄｅｎｃｅ， ｌｏｃａｌ ｓｃａｌｅ⁃ｃｏｍｐｒｅｓｓｅｄ ｓｅａｒｃｈｉｎｇ ｗｉｔｈ ａ ｄｙｎａｍｉｃ ｔｈｒｅｓｈｏｌｄ ｉｓ ｐｒｏｐｏｓｅｄ ｗｈｅｒｅ ｍａｔｃｈ⁃ｐｏｉｎｔｓ ａｒｅ ｆｏｕｎｄ ｗｉｔｈｉｎ ａ ｐｒｏｇｒｅｓｓｉｖｅｌｙ ｃｏｎｓｔｒｉｃｔｉｖｅ ｒａｎｇｅ．Ｃｏｍｐａｒｅｄ ｔｏ ｔｒａｄｉｔｉｏｎａｌ ＩＣＰ⁃ＳＬＡＭ， ｕｎｄｅｒ ｉｄｅａｌ ｓｔａｂｌｅ ｃｏｎｄｉｔｉｏｎｓ， ｔｈｅ ｂｅｓｔ ｅｘｐｅｒｉｍｅｎｔａｌ ｒｅｓｕｌｔｓ ｓｈｏｗ ａｍｏｕｎｔ ｏｆ ｉｔｅｒａｔｉｏｎ ｆｏｒ ＩＣＰ ａｌｇｏｒｉｔｈｍ ｔｏ ｒｅａｃｈ ｃｏｎｖｅｒｇｅｎｃｅ ｒｅｄｕｃｅｓ ９２．３４％ ａｎｄ ＩＣＰ ａｌｇｏｒｉｔｈｍ ｒｕｎｔｉｍｅ ｒｅｄｕｃｅｓ ９８． ８６％． Ｉｎ ａｄｄｉｔｉｏｎ， ｃｏｍｐｕｔａｔｉｏｎａｌ ｃｏｓｔ ｉｓ ｋｅｐｔ ａｔ ａ ｓｔａｂｌｅ ｌｅｖｅｌ ｄｕｅ ｔｏ ｔｈｅ ｅｌｉｍｉｎａｔｉｏｎ ｏｆ ａｃｃｕｍｕｌａｔｅｄ ｃｏｍｐｕｔａｔｉｏｎａｌ ｃｏｎｓｕｍｐｔｉｏｎ． Ｍｏｒｅｏｖｅｒ， ｇｒｅａｔ ｉｍｐｒｏｖｅｍｅｎｔ ｉｓ ｏｂｓｅｒｖｅｄ ｉｎ ｔｈｅ ｑｕａｌｉｔｙ ａｎｄ ｒｏｂｕｓｔｎｅｓｓ ｏｆ ＳＬＡＭ Ｋｅｙｗｏｒｄｓ： ＩＣＰ⁃ＳＬＡＭ； ｒｏｕｇｈ ａｌｉｇｎｍｅｎｔ； ｉｎｉｔｉａｌ ｐｏｓｅ ｍａｔｒｉｘ； ｌｏｃａｌ ｓｅａｒｃｈｉｎｇ； ｄｙｎａｍｉｃ ｔｈｒｅｓｈｏｌｄ； ｒｅａｌ⁃ｔｉｍｅ ｐｅｒｆｏｒｍａｎｃｅ； ｃｌｏｕｄ ｐｏｉｎｔ； ｉｔｅｒａｔｉｏｎ 收稿日期：２０１６－０５－２７． 网络出版日期：２０１７－０７－０５． 基金项目：国家“８６３”计划基金项目（２０１３ＡＡ０３Ａ１１２１，２０１３ＡＡ０３Ａ１１２２）；上 海市教委重点学科资助项目（Ｊ５０１０４）． 通信作者：梁滨．Ｅ⁃ｍａｉｌ： ｚｈａｎｇｊｉｎｙｉ＠ ｓｔａｆｆ．ｓｈｕ．ｅｄｕ．ｃｎ． 自主机器人和无人驾驶车成为近几年来人工 智能领域研究的新热点，该技术在服务业、交通运 输、工业、环境勘探、国防以及生活各个方面都有着 广阔的应用前景。 同时德国工业 ４． ０ 及中国制造 ２０２５ 也把焦点聚集在无人化和智能化，智能产业正

·414 智能系统学报 第12卷 在迎来前所未有的发展机遇，将催生庞大的市场。 Matching Rule方法[u。该方法通过在极坐标系下， SLAM(同时定位和地图创建，simultaneous 寻找相近的旋转角度，实现了快速的匹配点搜索。 localization and mapping,)是自主机器人和无人驾驶 但该方法并不能提供初始姿态矩阵实现粗匹配，所 车领域的关键技术。当前SLAM的实现方案可以分 以仍然采用全局搜索实现匹配点搜索。综上，可以 为基于概率的方案1-]和基于非概率的方案[4)。 看出大多数ICP算法都采用了全局搜索。但随着全 ICP(最近邻点迭代，iterative closest point)-SLAM作 局地图的扩大，全局搜索会导致计算消耗量的累计 为基于非概率的方案之一，由于其具有原理简单、 效应。虽然局部ICP-SLAM20-2某种程度上可以解 成本较低等优点而得到了广泛关注[6】。并且相比 决这个问题，但是局部ICP-SLAM首先把全局地图 于传统的导航技术，如蓝牙定位、惯性导航系统 划分成多个局部地图，然后对每个局部地图进行局 等[9-，ICP-SLAM不仅能实现定位，还能实现地图 部ICP-SLAM。所以，局部ICP-SLAM的本质上还是 创建。但随着自主机器人和无人驾驶车的速度越 采用了全局搜索。并且局部ICP-SLAM还要保存各 来越快，所处环境变得越来越复杂，这对SLAM实时 个局部地图的相对位置，增加了额外内存开销。 性提出了更高要求。由于传统ICP-SLAM实时性 综上所述，为了有效提高ICP-SLAM的实时性， 差，同时建模精度和鲁棒性也不高，显然不能再满 本文提出基于粗匹配和局部尺度压缩搜索的快速 足这方面的要求。造成传统ICP-SLAM实时性差的 ICP-SLAM。该快速ICP-SLAM在激光雷达的基础 原因：一方面，因为传统ICP算法不能提供初始姿态 上增加了MEMS磁力计，这个MEMS磁力计能直接 矩阵进行粗匹配，从而导致达到收敛条件的迭代次 输出当前航向角。并且引入全局地标，通过激光雷 数大量增加：另一方面，由于缺少粗匹配，必须通过 达扫描并测量全局地标，计算出机器人当前的位置 全局搜索才能完成匹配点搜索，这也大大增加了计 信息。最后由航向角和机器人当前的位置信息共 算开销，降低了传统ICP-SLAM实时性。因此，为了 同构成完整的初值姿态矩阵。当得到了初值姿态 提高ICP-SLAM的实时性，必须对传统ICP-SLAM 矩阵后，首先通过该初值姿态矩阵实现当前帧和全 进行优化。 局地图的粗匹配。其次，在ICP算法的每次迭代中， 当前，对传统ICP-SLAM的优化大都是选取 采用局部尺度压缩搜索完成匹配点搜索，从而避免 ICP算法的某个步骤进行优化。ICP算法最早由 了由全局搜索带来的巨大计算开销。同时，每次迭 Besl和Mckay为解决3-D物体对准问题而提出 代完成之后，匹配点的搜索范围通过动态阈值被缩 的。根据Rusinkiewicz和Levoy的理论[a)],ICP 小，加快匹配点搜索的速度，这将进一步提高ICP 算法可以分成下面6个步骤：选择控制点，匹配点搜 SLAM实时性。实验结果显示，本文提出的快速 索，计算匹配点权重，设定一个匹配点误差方程，通 ICP-SLAM相比于传统ICP-SLAM,迭代次数减少了 过最小化匹配点误差方程求出旋转矩阵和平移矩 92.34%,ICP算法运行时间减少了98.86%。同时， 阵，同时定位和地图创建。当前ICP算法的研究主 ICP-SLAM的系统负载也被控制在稳定状态，其整 要集中在优化匹配点搜索策略上。主流的匹配点 体性能得到较大的改善。 搜索策略可以大致分为下面几种。基于特征的匹 1 基于初始位姿矩阵的粗匹配 配，点搜索方法[14-6]包括如基于直线特征、基于曲率 特征和基于斜率特征等，该方法在提取几何特征时 从上述分析可以看出，粗匹配是实现快速ICP 增加了额外的计算开销，降低了ICP-SLAM实时性。 SLAM的首要步骤。粗匹配的过程为：通过旋转和 并且几何特征的提取容易受测量噪声和移动物体 平移变换，使得两个形状类似但处于不同空间位置 的影响，降低了ICP-SLAM的鲁棒性。另外，基于几 的物体大致重合。这是因为，当形状相同的两个物 何划分的匹配点搜索也被广泛引用，如Delaunay划 体处于二维平面的不同位置时，必然能找到一个平 分)和K-D树划分)。几何划分可以提高匹配，点 移矩阵和旋转角使得这两个物体完全重合。而机 搜索的质量和效率。但当相邻两帧的重叠区域很 器人的初始位姿矩阵就包含了这样一个平移矩阵 小时，该方法则不再适用。除此之外，考虑到激光 和旋转角。其中，机器人当前的坐标(x,y,)可以看 雷达作为ICP-SLAM的主要传感器，所以为了能更 作平移矩阵，机器人的航向角0，可以看作旋转角。 好地利用激光雷达特殊的数据结构，提出了基于 那么机器人t时刻的姿态矩阵可以表示成一个1×3 Polar-Cartesian Hybrid Transforms Polar Point 矩阵Pose,(x,y,0,)。Pose,(x,y,0,)可以通过

在迎来前所未有的发展机遇，将催生庞大的市场。 ＳＬＡＭ （ 同 时 定 位 和 地 图 创 建， ｓｉｍｕｌｔａｎｅｏｕｓ ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ ｍａｐｐｉｎｇ，）是自主机器人和无人驾驶 车领域的关键技术。 当前 ＳＬＡＭ 的实现方案可以分 为基于概率的方案［１－３］ 和基于非概率的方案［４－５］ 。 ＩＣＰ（最近邻点迭代，ｉｔｅｒａｔｉｖｅ ｃｌｏｓｅｓｔ ｐｏｉｎｔ）⁃ＳＬＡＭ 作 为基于非概率的方案之一，由于其具有原理简单、 成本较低等优点而得到了广泛关注［６－８］ 。 并且相比 于传统的导航技术， 如蓝牙定位、 惯性导航系统 等［９－１１］ ，ＩＣＰ⁃ＳＬＡＭ 不仅能实现定位，还能实现地图 创建。 但随着自主机器人和无人驾驶车的速度越 来越快，所处环境变得越来越复杂，这对 ＳＬＡＭ 实时 性提出了更高要求。 由于传统 ＩＣＰ⁃ＳＬＡＭ 实时性 差，同时建模精度和鲁棒性也不高，显然不能再满 足这方面的要求。 造成传统 ＩＣＰ⁃ＳＬＡＭ 实时性差的 原因：一方面，因为传统 ＩＣＰ 算法不能提供初始姿态 矩阵进行粗匹配，从而导致达到收敛条件的迭代次 数大量增加；另一方面，由于缺少粗匹配，必须通过 全局搜索才能完成匹配点搜索，这也大大增加了计 算开销，降低了传统 ＩＣＰ⁃ＳＬＡＭ 实时性。 因此，为了 提高 ＩＣＰ⁃ＳＬＡＭ 的实时性，必须对传统 ＩＣＰ⁃ＳＬＡＭ 进行优化。 当前，对传统 ＩＣＰ － ＳＬＡＭ 的优化大都是选取 ＩＣＰ 算法的某个步骤进行优化。 ＩＣＰ 算法最早由 Ｂｅｓｌ 和 Ｍｃｋａｙ 为解决 ３⁃Ｄ 物体对准问题而提出 的［１２］ 。 根据 Ｒｕｓｉｎｋｉｅｗｉｃｚ 和 Ｌｅｖｏｙ 的理论［１３］ ，ＩＣＰ 算法可以分成下面 ６ 个步骤：选择控制点，匹配点搜 索，计算匹配点权重，设定一个匹配点误差方程，通 过最小化匹配点误差方程求出旋转矩阵和平移矩 阵，同时定位和地图创建。 当前 ＩＣＰ 算法的研究主 要集中在优化匹配点搜索策略上。 主流的匹配点 搜索策略可以大致分为下面几种。 基于特征的匹 配点搜索方法［１４－１６］包括如基于直线特征、基于曲率 特征和基于斜率特征等，该方法在提取几何特征时 增加了额外的计算开销，降低了 ＩＣＰ⁃ＳＬＡＭ 实时性。 并且几何特征的提取容易受测量噪声和移动物体 的影响，降低了 ＩＣＰ⁃ＳＬＡＭ 的鲁棒性。 另外，基于几 何划分的匹配点搜索也被广泛引用，如 Ｄｅｌａｕｎａｙ 划 分［１７］和 Ｋ⁃Ｄ 树划分［１８］ 。 几何划分可以提高匹配点 搜索的质量和效率。 但当相邻两帧的重叠区域很 小时，该方法则不再适用。 除此之外，考虑到激光 雷达作为 ＩＣＰ⁃ＳＬＡＭ 的主要传感器，所以为了能更 好地利用激光雷达特殊的数据结构，提出了基于 Ｐｏｌａｒ⁃Ｃａｒｔｅｓｉａｎ Ｈｙｂｒｉｄ Ｔｒａｎｓｆｏｒｍｓ 的 Ｐｏｌａｒ Ｐｏｉｎｔ Ｍａｔｃｈｉｎｇ Ｒｕｌｅ 方法［１９］ 。 该方法通过在极坐标系下， 寻找相近的旋转角度，实现了快速的匹配点搜索。 但该方法并不能提供初始姿态矩阵实现粗匹配，所 以仍然采用全局搜索实现匹配点搜索。 综上，可以 看出大多数 ＩＣＰ 算法都采用了全局搜索。 但随着全 局地图的扩大，全局搜索会导致计算消耗量的累计 效应。 虽然局部 ＩＣＰ⁃ＳＬＡＭ ［２０－２１］某种程度上可以解 决这个问题，但是局部 ＩＣＰ⁃ＳＬＡＭ 首先把全局地图 划分成多个局部地图，然后对每个局部地图进行局 部 ＩＣＰ⁃ＳＬＡＭ。 所以，局部 ＩＣＰ⁃ＳＬＡＭ 的本质上还是 采用了全局搜索。 并且局部 ＩＣＰ⁃ＳＬＡＭ 还要保存各 个局部地图的相对位置，增加了额外内存开销。 综上所述，为了有效提高 ＩＣＰ⁃ＳＬＡＭ 的实时性， 本文提出基于粗匹配和局部尺度压缩搜索的快速 ＩＣＰ⁃ＳＬＡＭ。 该快速 ＩＣＰ⁃ＳＬＡＭ 在激光雷达的基础 上增加了 ＭＥＭＳ 磁力计，这个 ＭＥＭＳ 磁力计能直接 输出当前航向角。 并且引入全局地标，通过激光雷 达扫描并测量全局地标，计算出机器人当前的位置 信息。 最后由航向角和机器人当前的位置信息共 同构成完整的初值姿态矩阵。 当得到了初值姿态 矩阵后，首先通过该初值姿态矩阵实现当前帧和全 局地图的粗匹配。 其次，在 ＩＣＰ 算法的每次迭代中， 采用局部尺度压缩搜索完成匹配点搜索，从而避免 了由全局搜索带来的巨大计算开销。 同时，每次迭 代完成之后，匹配点的搜索范围通过动态阈值被缩 小，加快匹配点搜索的速度，这将进一步提高 ＩＣＰ⁃ ＳＬＡＭ 实时性。 实验结果显示，本文提出的快速 ＩＣＰ⁃ＳＬＡＭ 相比于传统 ＩＣＰ⁃ＳＬＡＭ，迭代次数减少了 ９２．３４％，ＩＣＰ 算法运行时间减少了 ９８．８６％。 同时， ＩＣＰ⁃ＳＬＡＭ 的系统负载也被控制在稳定状态，其整 体性能得到较大的改善。 １ 基于初始位姿矩阵的粗匹配 从上述分析可以看出，粗匹配是实现快速 ＩＣＰ⁃ ＳＬＡＭ 的首要步骤。 粗匹配的过程为：通过旋转和 平移变换，使得两个形状类似但处于不同空间位置 的物体大致重合。 这是因为，当形状相同的两个物 体处于二维平面的不同位置时，必然能找到一个平 移矩阵和旋转角使得这两个物体完全重合。 而机 器人的初始位姿矩阵就包含了这样一个平移矩阵 和旋转角。 其中，机器人当前的坐标（ｘｔ，ｙｔ）可以看 作平移矩阵，机器人的航向角 θｔ 可以看作旋转角。 那么机器人 ｔ 时刻的姿态矩阵可以表示成一个 １×３ 矩阵 Ｐｏｓｅｔ （ ｘｔ， ｙｔ，θｔ ）。 Ｐｏｓｅｔ （ ｘｔ， ｙｔ， θｔ ） 可以通过 ·４１４· 智 能 系 统 学 报 第 １２ 卷

第3期 张金艺，等：粗匹配和局部尺度压缩搜索下的快速ICP-SLAM ·415 MEMS磁力计和全局地标计算得到。这样在ICP算 虚线)，结合式(5)，则F,可以通过式(6)得到，其中 法开始前，可以通过矩阵Pose,(x,y,0,)完成当前 F,'和F,都是一个N×2矩阵，如式(7)和式(8) 帧和全局地图的粗匹配。全局地标在全局坐标系 所示： 中的坐标为已知信息，设图1中的第n个全局地标 cos 0. sin 0, 在全局坐标系中的坐标为Pn-c(xa-c,yLa-c)。同时 (5) sin 0.cos 6. 设第n个全局地标在机器人局部坐标系的坐标为 「x,y: P-(x1,y-L),则(x,y,)可以通过式(1)计算 x y 得到： F =F'Trolate (6) (x:xin-c xLn-Lcos 0:yi-Lsin 0 (1) y:=yin-c xu-Lsin 0:-yi-Lcos0. AY x2 y2 △全局地标nPL（代na) F= (7) Pind(xiG YiG) XN yN x y1 人全局地标1 X2 y2 F,= (8) XN yN Puu yu 通过粗匹配后，ICP算法达到收敛条件的迭代 Pud(xug yuG 次数将会大大减小。最后，当ICP算法达到收敛条 0 件时，得到最终的变换矩阵T,(x,,y,0)。 图1计算姿态矩阵 P(x.v.0) Fig.1 Initial pose matrix 当机器人移动到图2中的(x,y,)位置时，设其 姿态矩阵为Pose,(x,y,0)。激光雷达获取当前扫 描帧并记为F,'(图2中的灰色圆部分)，并测出第n 个全局地标在机器人局部坐标系下的坐标P (x-Ly-L)。假设激光雷达的角度分辨率为p,则 F'中的点数N可通过式(2)得到 N=o/p (2) 式中p为激光雷达的可视角（例如360°）。根据 图2粗匹配示意图 (1)式可计算出(x,y),如式(3)所示： Fig.2 Rough align TPun-L Pin-c (3) y 2基于动态阈值的局部尺度压缩搜索 式中T如公式(4)所示： 粗匹配完成之后，当前帧中的点和其匹配点的 -c0s0, sin 0, 距离被大大缩小，所以本文中的匹配点搜索将通过 T= (4) sin 0, cos 0, 局部搜索完成。同时，每次迭代完成之后，匹配点 因为全局地标为全局的静态参照物，所以通过 之间的距离都比上一次迭代时更小，此时便可以通 该方法计算得到的Pose,(x,y,0,)不存在累计 过动态阈值达到尺度压缩，从而缩小匹配，点搜索的 误差。 范围。局部尺度压缩搜索不仅能加快匹配点搜索 同样在图2中记全局地图为Mh(图2中的点 速度，还能提高匹配点搜索质量。同时在本文中， 线)，并记F,'和M粗匹配的结果为F(图2中的 因为SLAM创建的地图为栅格地图，所以本小节将

ＭＥＭＳ 磁力计和全局地标计算得到。 这样在 ＩＣＰ 算 法开始前，可以通过矩阵 Ｐｏｓｅｔ（ ｘｔ，ｙｔ，θｔ ）完成当前 帧和全局地图的粗匹配。 全局地标在全局坐标系 中的坐标为已知信息，设图 １ 中的第 ｎ 个全局地标 在全局坐标系中的坐标为 ＰＬｎ－Ｇ（ ｘＬｎ－Ｇ ，ｙＬｎ－Ｇ ）。 同时 设第 ｎ 个全局地标在机器人局部坐标系的坐标为 ＰＬｎ－Ｌ（ｘＬｎ－Ｌ ，ｙＬｎ－Ｌ ），则（ ｘｔ，ｙｔ ） 可以通过式（ １） 计算 得到： ｘｔ ＝ ｘＬｎ－Ｇ － ｘＬｎ－Ｌ ｃｏｓ θｔ － ｙＬｎ－Ｌ ｓｉｎ θｔ ｙｔ ＝ ｙＬｎ－Ｇ ＋ ｘＬｎ－Ｌ ｓｉｎ θｔ － ｙＬｎ－Ｌ ｃｏｓθｔ { （１） 图 １ 计算姿态矩阵 Ｆｉｇ．１ Ｉｎｉｔｉａｌ ｐｏｓｅ ｍａｔｒｉｘ 当机器人移动到图 ２ 中的（ｘｔ，ｙｔ）位置时，设其 姿态矩阵为 Ｐｏｓｅｔ（ｘｔ，ｙｔ，θｔ）。 激光雷达获取当前扫 描帧并记为 Ｆｔ ′（图 ２ 中的灰色圆部分），并测出第 ｎ 个全局地标在机器人局部坐标系下的坐标 ＰＬｎ－Ｌ （ｘＬｎ－Ｌ ，ｙＬｎ－Ｌ ）。 假设激光雷达的角度分辨率为 ρ，则 Ｆｔ ′中的点数 Ｎ 可通过式（２）得到 Ｎ ＝ φ／ ρ （２） 式中 φ 为激光雷达的可视角（ 例如 ３６０°）。 根据 （１）式可计算出（ｘｔ，ｙｔ），如式（３）所示： ｘｔ ｙｔ é ë ê ê ù û ú ú ＝ ＴＰＬｎ－Ｌ ＋ ＰＬｎ－Ｇ （３） 式中 Ｔ 如公式（４）所示： Ｔ ＝ － ｃｏｓ θｔ － ｓｉｎ θｔ ｓｉｎ θｔ － ｃｏｓ θｔ é ë ê ê ù û ú ú （４） 因为全局地标为全局的静态参照物，所以通过 该方法计算得到的 Ｐｏｓｅｔ （ ｘｔ， ｙｔ， θｔ ） 不存在累计 误差。 同样在图 ２ 中记全局地图为Ｍｇｌｏｂａｌ（图 ２ 中的点 线），并记 Ｆｔ ′和 Ｍｇｌｏｂａｌ粗匹配的结果为 Ｆｔ（图 ２ 中的 虚线），结合式（５），则 Ｆｔ 可以通过式（６）得到，其中 Ｆｔ ′和 Ｆｔ 都是一个 Ｎ × ２ 矩阵，如式（ ７） 和式（ ８） 所示： Ｔｒｏｔａｔｅ ＝ ｃｏｓ θｔ ｓｉｎ θｔ － ｓｉｎ θｔ ｃｏｓ θｔ é ë ê ê ù û ú ú （５） Ｆｔ ＝ Ｆｔ ′Ｔｒｏｔａｔｅ ＋ ｘｔ ｙｔ ｘｔ ｙｔ ︙ ︙ ｘｔ ｙｔ é ë ê ê ê ê ê ê ù û ú ú ú ú ú ú Ｎ×２ （６） Ｆｔ ′ ＝ ｘ１ ′ ｙ１ ′ ｘ２ ′ ｙ２ ′ ︙ ︙ ｘＮ ′ ｙＮ ′ é ë ê ê ê ê ê ê ù û ú ú ú ú ú ú （７） Ｆｔ ＝ ｘ１ ｙ１ ｘ２ ｙ２ ︙ ︙ ｘＮ ｙＮ é ë ê ê ê ê ê ê ù û ú ú ú ú ú ú （８） 通过粗匹配后，ＩＣＰ 算法达到收敛条件的迭代 次数将会大大减小。 最后，当 ＩＣＰ 算法达到收敛条 件时，得到最终的变换矩阵 Ｔｔ ｘ Ｔ ｔ ，ｙ Ｔ ｔ ，θ Ｔ ｔ ( ) 。 图 ２ 粗匹配示意图 Ｆｉｇ．２ Ｒｏｕｇｈ ａｌｉｇｎ ２ 基于动态阈值的局部尺度压缩搜索 粗匹配完成之后，当前帧中的点和其匹配点的 距离被大大缩小，所以本文中的匹配点搜索将通过 局部搜索完成。 同时，每次迭代完成之后，匹配点 之间的距离都比上一次迭代时更小，此时便可以通 过动态阈值达到尺度压缩，从而缩小匹配点搜索的 范围。 局部尺度压缩搜索不仅能加快匹配点搜索 速度，还能提高匹配点搜索质量。 同时在本文中， 因为 ＳＬＡＭ 创建的地图为栅格地图，所以本小节将 第 ３ 期 张金艺，等：粗匹配和局部尺度压缩搜索下的快速 ＩＣＰ－ＳＬＡＭ ·４１５·

·416 智能系统学报 第12卷 先阐述本文中采用的栅格地图的坐标表征方式，接 cy"min cy,-SearchingRange (13) 着再详细剖析局部搜索和尺度压缩。 cy"max cy,SearchingRange (14) 2.1栅格地图的坐标表征 其中SearchingRange的值是自己设定的，在图4 SLAM创建栅格地图时，栅格地图中的某个单 中SearchingRange的值为l。在确定搜索范围后，下 元格只能处于两种状态中的一种：被物体占据或没 一步便计算出点a/b与所有落在搜索范围内的点的 有被物体占据。当单元格被物体占据时用深色表 欧式距离d。当d,满足： 示，单元格没有被物体占据时用浅色表示。在栅格 d<thresholdDist (15) 地图中，水平面被分割为一个包含m×n个正方形单 则记该点为匹配点(Point--to-Region策略，即一个 元格平面，记正方形单元格边长为L,并设栅格地图 点可以存在多个匹配点)。比如F,中的第广个点表 能表示的范围为xnxy。y,同时把正方 示为F,(),则F,(G)的第i个匹配点记为F(j)。 形单元格的几何中心作为该正方形单元格的坐标。 在图3中，阴影正方形单元格的坐标(xu4,Jyt)可 以表示为 (black y plack)=(3.5L,3.5L) (9) 同时可以把该阴影正方形单元格在栅格地图 中的标号(xty-hd)表示为 (x-blacky:-het)=(int）(xuat-xn）/L, (int)(yuek -y)/L)(10) L (a地图扩建前 b)地图扩建后 L1m=12 n=14 图4扩建栅格地图 Fig.4 Grid map expansion 2.3基于动态阈值的尺度压缩 ICP算法的最终目的是为了得到一个平移矩阵 和旋转角。所以当完成匹配，点搜索后，下一步便是 +- -- 通过最小化匹配点误差方程得到平移矩阵和旋转 角。但在得到最终的平移矩阵和旋转角之前，当 y'nn ICP算法完成一次迭代之后，当前帧和全局地图的 图3栅格地图示意图 匹配能更加准确。这意味着，F,中的点与其匹配点 Fig.3 Grid map 之间的距离缩短。所以ICP算法在下一次的匹配点 当有更多的点落入到某一个正方形单元格时， 搜索中，搜索范围可以缩小。这不但可以减小匹配 该正方形单元格的颜色会加深。 点搜索的计算量，还能减少误匹配。完整的算法流 2.2基于Point-to-Region的局部搜索 程如图5所示。 局部搜索只有当完成粗匹配后才有意义。因 本文将选择匹配点之间的欧式距离作为匹配 为经过粗匹配后，当前帧中的某一点的匹配点只能 点误差方程，匹配点的欧式距离如式(16)： 出现在距离该点某一距离的范围内。所以为了能 )=[F]-x[E,0)])2+ 采用局部搜索，必须在匹配点搜索前，将F'通过 Pose,(x,y,0,)和(5)式投影到Ma(记为F,),然 6[F1-[F.0)1) (16) 后再在M和F,之间通过局部搜索完成匹配，点搜 匹配点的权重通过式(17)计算： 索。以图4中的a点和b点为例，通过式(9)计算出 (0, Ie0)|≥E a点和b点所在正方形单元的标号，记为(cx,cy.) 0=1. (17) 其他 和(cx。,cy)。局部搜索的搜索范围可以表示为 式中E为动态阈值。匹配点的数量为 cxmin =cx,-SearchingRange (11) -Az0 (18) cx"max =cx,SearchingRange (12)

先阐述本文中采用的栅格地图的坐标表征方式，接 着再详细剖析局部搜索和尺度压缩。 ２．１ 栅格地图的坐标表征 ＳＬＡＭ 创建栅格地图时，栅格地图中的某个单 元格只能处于两种状态中的一种：被物体占据或没 有被物体占据。 当单元格被物体占据时用深色表 示，单元格没有被物体占据时用浅色表示。 在栅格 地图中，水平面被分割为一个包含 ｍ×ｎ 个正方形单 元格平面，记正方形单元格边长为 Ｌ，并设栅格地图 能表示的范围为 ｘ＾ｍｉｎ ，ｘ＾ｍａｘ，ｙ＾ｍｉｎ ，ｙ＾ｍａｘ，同时把正方 形单元格的几何中心作为该正方形单元格的坐标。 在图 ３ 中，阴影正方形单元格的坐标（ｘｂｌａｃｋ ，ｙｂｌａｃｋ ）可 以表示为 ｘｂｌａｃｋ ，ｙｂｌａｃｋ ( ) ＝ (３．５Ｌ，３．５Ｌ) （９） 同时可以把该阴影正方形单元格在栅格地图 中的标号（ｘｉ－ｂｌａｃｋ ，ｙｉ－ｂｌａｃｋ ）表示为 ｘｉ－ｂｌａｃｋ ，ｙｉ－ｂｌａｃｋ ( ) ＝ 〈ｉｎｔ〉 ｘｂｌａｃｋ － ｘ＾ｍｉｎ ( ( ) ／ Ｌ， 〈ｉｎｔ〉（ｙｂｌａｃｋ － ｙ＾ｍｉｎ ) ／ Ｌ） （１０） 图 ３ 栅格地图示意图 Ｆｉｇ．３ Ｇｒｉｄ ｍａｐ 当有更多的点落入到某一个正方形单元格时， 该正方形单元格的颜色会加深。 ２．２ 基于 Ｐｏｉｎｔ⁃ｔｏ⁃Ｒｅｇｉｏｎ 的局部搜索 局部搜索只有当完成粗匹配后才有意义。 因 为经过粗匹配后，当前帧中的某一点的匹配点只能 出现在距离该点某一距离的范围内。 所以为了能 采用局部搜索，必须在匹配点搜索前，将 Ｆｔ ′通过 Ｐｏｓｅｔ（ｘｔ，ｙｔ，θｔ）和（５）式投影到Ｍｇｌｏｂａｌ（记为 Ｆｔ），然 后再在Ｍｇｌｏｂａｌ和 Ｆｔ 之间通过局部搜索完成匹配点搜 索。 以图 ４ 中的 ａ 点和 ｂ 点为例，通过式（９）计算出 ａ 点和 ｂ 点所在正方形单元的标号，记为（ｃｘａ，ｃｙａ） 和（ｃｘｂ ，ｃｙｂ ）。 局部搜索的搜索范围可以表示为 ｃｘ＾ｍｉｎ ＝ ｃｘｘ － ＳｅａｒｃｈｉｎｇＲａｎｇｅ （１１） ｃｘ＾ｍａｘ ＝ ｃｘｘ ＋ ＳｅａｒｃｈｉｎｇＲａｎｇｅ （１２） ｃｙ＾ｍｉｎ ＝ ｃｙｘ － ＳｅａｒｃｈｉｎｇＲａｎｇｅ （１３） ｃｙ＾ｍａｘ ＝ ｃｙｘ ＋ ＳｅａｒｃｈｉｎｇＲａｎｇｅ （１４） 其中 ＳｅａｒｃｈｉｎｇＲａｎｇｅ 的值是自己设定的，在图 ４ 中 ＳｅａｒｃｈｉｎｇＲａｎｇｅ 的值为 １。 在确定搜索范围后，下 一步便计算出点 ａ ／ ｂ 与所有落在搜索范围内的点的 欧式距离 ｄｉ。 当 ｄｉ 满足： ｄｉ ＜ ｔｈｒｅｓｈｏｌｄＤｉｓｔ （１５） 则记该点为匹配点（Ｐｏｉｎｔ－ｔｏ－Ｒｅｇｉｏｎ 策略，即一个 点可以存在多个匹配点）。 比如Ｆｔ 中的第 ｊ 个点表 示为 Ｆｔ（ｊ），则 Ｆｔ（ｊ）的第 ｉ 个匹配点记为Ｆ ｉ ｔ（ｊ） ｍａｔｃｈ 。 图 ４ 扩建栅格地图 Ｆｉｇ．４ Ｇｒｉｄ ｍａｐ ｅｘｐａｎｓｉｏｎ ２．３ 基于动态阈值的尺度压缩 ＩＣＰ 算法的最终目的是为了得到一个平移矩阵 和旋转角。 所以当完成匹配点搜索后，下一步便是 通过最小化匹配点误差方程得到平移矩阵和旋转 角。 但在得到最终的平移矩阵和旋转角之前，当 ＩＣＰ 算法完成一次迭代之后，当前帧和全局地图的 匹配能更加准确。 这意味着，Ｆｔ 中的点与其匹配点 之间的距离缩短。 所以 ＩＣＰ 算法在下一次的匹配点 搜索中，搜索范围可以缩小。 这不但可以减小匹配 点搜索的计算量，还能减少误匹配。 完整的算法流 程如图 ５ 所示。 本文将选择匹配点之间的欧式距离作为匹配 点误差方程，匹配点的欧式距离如式（１６）： ｅ ｉ ｔ（ｊ） ＝ ｘ［Ｆ ｉ ｔ（ｊ） ｍａｔｃｈ ( ］ － ｘ［Ｆｔ（ｊ）］ ) ２ ＋ ｙ［Ｆ ｉ ｔ（ｊ） ｍａｔｃｈ ( ］ － ｙ［Ｆｔ（ｊ）］ ) ２ （１６） 匹配点的权重通过式（１７）计算： ｗ ｉ ｔ (ｊ) ＝ ０， ｅ ｉ ｔ (ｊ) ≥ Ｅ {１， 其他 （１７） 式中 Ｅ 为动态阈值。 匹配点的数量为 ｎｔ ＝ ∑ Ｎ ｊ ＝ ０ ∑ｉ ｗ ｉ ｔ (ｊ) （１８） ·４１６· 智 能 系 统 学 报 第 １２ 卷

第3期 张金艺，等：粗匹配和局部尺度压缩搜索下的快速ICP-SLAM ·417. 开始 al, =0→yc (22) 扫描得到F,计算出P,化，y) ay, al, 将F转换为F, =0→0m (23) a0. 通过局部尺度压缩搜索完成配点搜索 完成一次迭代之后，E通过下式被缩小： E=EFe,Fe∈(0，l) (24) ICP算法迭代 人 当L,达到阈值后，ICP算法停止并得到最终的 是否达到收敛条件 N E-EFa 变换矩阵T,(x,y,O)。F,通过该变换矩阵转换成 Y F(图4中的虚线)，并最终实现Mm的扩建。 得到T,男) F+1 3实验结果与分析 LAM院成N 为了有效验证粗匹配和局部尺度压缩搜索对 结束 ICP-SLAM实时性的改善，本文搭建了一辆可定位 图5算法总流程图 的小车作为验证平台。该小车搭载了一个 Fig.5 Algorithm flow chart RobotPeak激光雷达、一个Uranus MEMS磁力计和 当前帧与全局地图的重合度可以表示为 一块Raspberry Pi主板，如图6所示，各传感器规格 n. 见表1。 0,=N*1 (19) 综上，最终的误差方程可以表示为 ΣG0601 Raspberry Pi I,= j=0 (20) n,OL RobotPeak激光雷达 通过求导可以完成(20)式的最小化，得出变换 矩阵(x",y",): al, Uranus MEMS磁力计 =0→x (21) ox, 图6实验平台 Fig.6 Experiment platform 表1 RobotPeak激光雷达和Uranus MEMS磁力计参数 Table 1 Features of Robot Peak and Uranus MEMS 传感器 测量范围 采样频率/Hz 距离分辨率/cm角度分辨率/(o)FOV(Field of View)/(o) RobotPeak激光雷达 6m 2000 0.2 1 360 Uranus MEMS磁力计 ±4800uT 100 0.01 本小节将从两方面验证快速ICP-SLAM。第 ICP-SLAM实时性的改善情况，同时还可以验证 一方面是快速ICP-SLAM实时性改善验证。首先， ICP-SLAM在建模精度、鲁棒性和计算开销等方面 对比未进行粗匹配和进行粗匹配下ICP-SLAM的 的提升。 迭代次数和ICP算法运行时间，验证粗匹配对 3.1 快速ICP-SLAM实时性改善验证 ICP-SLAM实时性的改善；其次，记录F的值从1 通过第1节和第2节的分析可知，ICP-SLAM实 到O.3变化过程中ICP-SLAM的迭代次数和ICP 时性受初始姿态矩阵、局部搜素和参数F影响。 算法运行时间，观察其变化趋势，进而验证局部尺 本小节将采用复合开环航迹和复合闭环航迹，验证 度压缩搜索对ICP-SLAM实时性的改善。第二方 粗匹配和局部尺度压缩搜索对ICP-SLAM实时性的 面是快速ICP-SLAM整体性改善验证。其不仅可 改善。复合开环航迹和复合闭环航迹分别如图7中 以验证同时采用粗匹配和局部尺度压缩搜索时， 的(a)和(b)所示

图 ５ 算法总流程图 Ｆｉｇ．５ Ａｌｇｏｒｉｔｈｍ ｆｌｏｗ ｃｈａｒｔ 当前帧与全局地图的重合度可以表示为 ＯＬｔ ＝ ｎｔ Ｎ ＋ １ （１９） 综上，最终的误差方程可以表示为 Ｉｔ ＝ ∑ Ｎ ｊ ＝ ０ ∑ｉ ｗ ｉ ｔ (ｊ) ｅ ｉ ｔ [ (ｊ) ] ｎｔＯＬｔ （２０） 通过求导可以完成（２０）式的最小化，得出变换 矩阵 ｘ ｎｅｗ ｔ ，ｙ ｎｅｗ ｔ ，θ ｎｅｗ ｔ ( ) ： ∂Ｉｔ ∂ｘｔ ＝ ０⇒ｘ ｎｅｗ ｔ （２１） ∂Ｉｔ ∂ｙｔ ＝ ０⇒ｙ ｎｅｗ ｔ （２２） ∂Ｉｔ ∂θｔ ＝ ０⇒θ ｎｅｗ ｔ （２３） 完成一次迭代之后，Ｅ 通过下式被缩小： Ｅ ＝ ＥＦｄｅｃ， Ｆｄｅｃ ∈ (０，１) （２４） 当 Ｉｔ 达到阈值后，ＩＣＰ 算法停止并得到最终的 变换矩阵Ｔｔ ｘ Ｔ ｔ ，ｙ Ｔ ｔ ，θ Ｔ ｔ ( ) 。 Ｆｔ 通过该变换矩阵转换成 Ｆ ｃ ｔ（图 ４ 中的虚线），并最终实现Ｍｇｌｏｂａｌ的扩建。 ３ 实验结果与分析 为了有效验证粗匹配和局部尺度压缩搜索对 ＩＣＰ－ＳＬＡＭ 实时性的改善，本文搭建了一辆可定位 的小 车 作 为 验 证 平 台。 该 小 车 搭 载 了 一 个 ＲｏｂｏｔＰｅａｋ 激光雷达、一个 Ｕｒａｎｕｓ ＭＥＭＳ 磁力计和 一块 Ｒａｓｐｂｅｒｒｙ Ｐｉ 主板，如图 ６ 所示，各传感器规格 见表 １。 图 ６ 实验平台 Ｆｉｇ．６ Ｅｘｐｅｒｉｍｅｎｔ ｐｌａｔｆｏｒｍ 表 １ ＲｏｂｏｔＰｅａｋ 激光雷达和 Ｕｒａｎｕｓ ＭＥＭＳ 磁力计参数 Ｔａｂｌｅ １ Ｆｅａｔｕｒｅｓ ｏｆ Ｒｏｂｏｔ Ｐｅａｋ ａｎｄ Ｕｒａｎｕｓ ＭＥＭＳ 传感器 测量范围 采样频率／ Ｈｚ 距离分辨率／ ｃｍ 角度分辨率／ （°） ＦＯＶ（Ｆｉｅｌｄ ｏｆ Ｖｉｅｗ） ／ （°） ＲｏｂｏｔＰｅａｋ 激光雷达 ６ ｍ ２ ０００ ０．２ １ ３６０ Ｕｒａｎｕｓ ＭＥＭＳ 磁力计 ±４ ８００ μＴ １００ — ０．０１° — 本小节将从两方面验证快速 ＩＣＰ⁃ＳＬＡＭ。 第 一方面是快速 ＩＣＰ⁃ＳＬＡＭ 实时性改善验证。 首先， 对比未进行粗匹配和进行粗匹配下 ＩＣＰ⁃ＳＬＡＭ 的 迭代次 数 和 ＩＣＰ 算 法 运 行 时 间，验 证 粗 匹 配 对 ＩＣＰ⁃ＳＬＡＭ 实时性的改善；其次，记录 Ｆｄｅｃ的值从 １ 到 ０．３ 变化过程中 ＩＣＰ⁃ＳＬＡＭ 的迭代次数和 ＩＣＰ 算法运行时间，观察其变化趋势，进而验证局部尺 度压缩搜索对 ＩＣＰ⁃ＳＬＡＭ 实时性的改善。 第二方 面是快速 ＩＣＰ⁃ＳＬＡＭ 整体性改善验证。 其不仅可 以验证同时采用粗匹配和局部尺度压缩搜索时， ＩＣＰ⁃ＳＬＡＭ 实时性的改善情况，同时还可以验证 ＩＣＰ⁃ＳＬＡＭ 在建模精度、鲁棒性和计算开销等方面 的提升。 ３．１ 快速 ＩＣＰ⁃ＳＬＡＭ 实时性改善验证 通过第 １ 节和第 ２ 节的分析可知，ＩＣＰ⁃ＳＬＡＭ 实 时性受初始姿态矩阵、局部搜素和参数 Ｆｄｅｃ 影响。 本小节将采用复合开环航迹和复合闭环航迹，验证 粗匹配和局部尺度压缩搜索对 ＩＣＰ⁃ＳＬＡＭ 实时性的 改善。 复合开环航迹和复合闭环航迹分别如图 ７ 中 的（ａ）和（ｂ）所示。 第 ３ 期 张金艺，等：粗匹配和局部尺度压缩搜索下的快速 ＩＣＰ－ＳＬＡＭ ·４１７·

·418 智能系统学报 第12卷 粗匹配对迭代次数和ICP算法运行时间的改善 如表2和表3所示。 从表2和表3中可以看出，在每一组对比实验 中，当进行粗匹配后，迭代次数和ICP算法运行时间 都被大大减小。所以粗匹配改善了ICP-SLAM实时 性。局部尺度搜索对迭代次数和ICP算法运行时间 的提升结果如图8和图9所示，从图中可以看出当 F从1渐变到0.3时，大大减少迭代次数，缩短了 →X 0 (a)复合开环航迹 (b)复合闭环航迹 ICP算法的运行时间。 图7复合开环航迹和复合闭环航迹 Fig.7 Mix-open track and mix-close track 表2小车在复合开环航迹下的实验结果 Table 2 Result in mix-open track 迭代次数 编号是否粗匹配 P-to-P/P-to-R 迭代次数 ICP算法运行时间/ms ICP运行时间减少/% 减少/% 否 1 P-to-P 87748 8068 3.3 13.7 是 1 P-to-P 84814 6961 否 0.7 P-to-P 20770 3110 2 26.1 31.9 是 0.7 P-to-P 15341 2116 否 0.5 P-to-P 18906 2550 24.9 24.4 是 0.5 P-to-P 14190 1929 否 0.3 P-to-P 13339 2001 19.6 14.0 是 0.3 P-to-P 9423 1720 否 0.5 P-to-R 22276 3925 10.8 11.8 是 0.5 P-to-R 19880 3463 表3小车在复合闭环航迹下的实验结果 Table 3 Result in mix-close track 迭代次数 编号是否粗匹配 F P-to-P/P-to-R 选代次数 ICP算法运行时间/ms ICP运行时间减少/% 减少/% 否 1 P-to-P 152870 12697 6 7.2 6.2 是 P-to-P 141892 11918 否 0.7 P-to-P 31059 > 3901 18.2 12.8 是 0.7 P-to-P 25426 3402 否 0.5 P-to-P 27646 3692 8 19.9 23.6 是 0.5 P-to-P 22125 2821 否 0.3 P-to-P 19534 2620 9 24.4 15.2 是 0.3 P-to-P 14752 2221 10 否 0.5 P-to-R 42231 13.3 6346 14.6 是 0.5 P-to-R 36611 5422

图 ７ 复合开环航迹和复合闭环航迹 Ｆｉｇ．７ Ｍｉｘ⁃ｏｐｅｎ ｔｒａｃｋ ａｎｄ ｍｉｘ⁃ｃｌｏｓｅ ｔｒａｃｋ 粗匹配对迭代次数和 ＩＣＰ 算法运行时间的改善 如表 ２ 和表 ３ 所示。 从表 ２ 和表 ３ 中可以看出，在每一组对比实验 中，当进行粗匹配后，迭代次数和 ＩＣＰ 算法运行时间 都被大大减小。 所以粗匹配改善了 ＩＣＰ⁃ＳＬＡＭ 实时 性。 局部尺度搜索对迭代次数和 ＩＣＰ 算法运行时间 的提升结果如图 ８ 和图 ９ 所示，从图中可以看出当 Ｆｄｅｃ从 １ 渐变到 ０．３ 时，大大减少迭代次数，缩短了 ＩＣＰ 算法的运行时间。 表 ２ 小车在复合开环航迹下的实验结果 Ｔａｂｌｅ ２ Ｒｅｓｕｌｔ ｉｎ ｍｉｘ⁃ｏｐｅｎ ｔｒａｃｋ 编号 是否粗匹配 Ｆｄｅｃ Ｐ⁃ｔｏ⁃Ｐ ／ Ｐ⁃ｔｏ⁃Ｒ 迭代次数 迭代次数 减少／ ％ ＩＣＰ 算法运行时间／ ｍｓ ＩＣＰ 运行时间减少／ ％ １ 否 １ Ｐ⁃ｔｏ⁃Ｐ ８７ ７４８ 是 １ Ｐ⁃ｔｏ⁃Ｐ ８４ ８１４ ３．３ ８ ０６８ ６ ９６１ １３．７ ２ 否 ０．７ Ｐ⁃ｔｏ⁃Ｐ ２０ ７７０ 是 ０．７ Ｐ⁃ｔｏ⁃Ｐ １５ ３４１ ２６．１ ３ １１０ ２ １１６ ３１．９ ３ 否 ０．５ Ｐ⁃ｔｏ⁃Ｐ １８ ９０６ 是 ０．５ Ｐ⁃ｔｏ⁃Ｐ １４ １９０ ２４．９ ２ ５５０ １ ９２９ ２４．４ ４ 否 ０．３ Ｐ⁃ｔｏ⁃Ｐ １３ ３３９ 是 ０．３ Ｐ⁃ｔｏ⁃Ｐ ９ ４２３ １９．６ ２ ００１ １ ７２０ １４．０ ５ 否 ０．５ Ｐ⁃ｔｏ⁃Ｒ ２２ ２７６ 是 ０．５ Ｐ⁃ｔｏ⁃Ｒ １９ ８８０ １０．８ ３ ９２５ ３ ４６３ １１．８ 表 ３ 小车在复合闭环航迹下的实验结果 Ｔａｂｌｅ ３ Ｒｅｓｕｌｔ ｉｎ ｍｉｘ⁃ｃｌｏｓｅ ｔｒａｃｋ 编号 是否粗匹配 Ｆｄｅｃ Ｐ⁃ｔｏ⁃Ｐ ／ Ｐ⁃ｔｏ⁃Ｒ 迭代次数 迭代次数 减少／ ％ ＩＣＰ 算法运行时间／ ｍｓ ＩＣＰ 运行时间减少／ ％ ６ 否 １ Ｐ⁃ｔｏ⁃Ｐ １５２ ８７０ 是 １ Ｐ⁃ｔｏ⁃Ｐ １４１ ８９２ ７．２ １２ ６９７ １１ ９１８ ６．２ ７ 否 ０．７ Ｐ⁃ｔｏ⁃Ｐ ３１ ０５９ 是 ０．７ Ｐ⁃ｔｏ⁃Ｐ ２５ ４２６ １８．２ ３ ９０１ ３ ４０２ １２．８ ８ 否 ０．５ Ｐ⁃ｔｏ⁃Ｐ ２７ ６４６ 是 ０．５ Ｐ⁃ｔｏ⁃Ｐ ２２ １２５ １９．９ ３ ６９２ ２ ８２１ ２３．６ ９ 否 ０．３ Ｐ⁃ｔｏ⁃Ｐ １９ ５３４ 是 ０．３ Ｐ⁃ｔｏ⁃Ｐ １４ ７５２ ２４．４ ２ ６２０ ２ ２２１ １５．２ １０ 否 ０．５ Ｐ⁃ｔｏ⁃Ｒ ４２ ２３１ 是 ０．５ Ｐ⁃ｔｏ⁃Ｒ ３６ ６１１ １３．３ ６ ３４６ ５ ４２２ １４．６ ·４１８· 智 能 系 统 学 报 第 １２ 卷

第3期 张金艺，等：粗匹配和局部尺度压缩搜索下的快速ICP-SLAM .419. ×10 10r 除此之外，采用全局搜索时，ICP-SLAM的计算 No.I 细进行粗匹配 开销会随着全局地图的增大而增大。但当采用局 没进行粗匹配 部尺度压缩搜索时，ICP-SLAM的计算开销被控制 6 在稳定状态，如图10所示。 全局搜索 No.2 No.3 No.4 10 1.0 0.70.50.3 (a)迭代次数改善情况 10 ×10 10 国进行粗匹配 局部尺度压缩搜索 ×10 No. 没进行粗匹配 102 0.2 0.40.6 0.8 1.0 ICP参数 图10ICP.SLAM的计算开销对比 Fig.10 Comparison of computation in ICP-SLAM No.2 No.3 No.4 从图10中可以看出，当采用局部尺度压缩搜索 时，随着ICP算法迭代次数的增加，ICP算法的运行 07 时间被保持在一个稳定值，这表明ICP-SLAM的计 1.0 0.5 0.3 算开销被控制在稳定状态。但采用全局搜索时， (b)ICP运行时间改善情况 ICP-SLAM的计算开销却随着ICP算法迭代次数的 图8F在复合开环航迹下的实验结果 增加而不断增加。所以局部尺度压缩搜索对提高 Fig.8 Result of Fa in mix-open track ICP-SLAM实时性有着显著的效果。对比图10和 2.0,*10 图11可知，本文提出的ICP-SLAM比文献[20]的局 趣进行粗匹配 部SLAM在计算负载的稳定性上更有优势。 No.6 没进行粗匹配 800 1.5 700 全局搜索 1.0 600 500 0.5 400 No.7 No.8 No.9 文献[20中的SLAM 200 1.0 0.7 0.5 0.3 100 BM121 (a)迭代次数改善情况 00.51.01.52.02.53.03.5 ICP参数 ×10 图11文献[20]中的SLAM波动过程 No.6 画进行粗匹配 没进行粗匹配 Fig.11 SLAM unstable process in [20] 10 3.2快速ICP-SLAM整体性改善验证 在该验证环节中，实验平台首先以低速走过一 5 段弧度较小曲线，用来验证P(x,y,0)的作用。 No.7 No.8 No.9 然后快速走完一段直线，用来验证(x,y,)的作用。 接着作一个急速转弯，此时0，会产生巨大的变化。 1.0 0.70.5 0.3 最后一段小车慢速走过一段短直线和慢速转弯。 F 图12为对比结果。在图12(a)的方法中，ICP算法 (b)ICP运行时间改善情况 开始时会采用粗匹配，并且通过局部尺度压缩搜索 图9F在复合闭环航迹下的实验结果 来完成匹配点搜索(F=0.5)。图12(b)中的对照 Fig.9 Result of Fa in mix-close track 组为传统的ICP-SLAM算法。完成整个SLAM过程

图 ８ Ｆｄｅｃ在复合开环航迹下的实验结果 Ｆｉｇ．８ Ｒｅｓｕｌｔ ｏｆ Ｆｄｅｃ ｉｎ ｍｉｘ⁃ｏｐｅｎ ｔｒａｃｋ 图 ９ Ｆｄｅｃ在复合闭环航迹下的实验结果 Ｆｉｇ．９ Ｒｅｓｕｌｔ ｏｆ Ｆｄｅｃ ｉｎ ｍｉｘ⁃ｃｌｏｓｅ ｔｒａｃｋ 除此之外，采用全局搜索时，ＩＣＰ⁃ＳＬＡＭ 的计算 开销会随着全局地图的增大而增大。 但当采用局 部尺度压缩搜索时，ＩＣＰ⁃ＳＬＡＭ 的计算开销被控制 在稳定状态，如图 １０ 所示。 图 １０ ＩＣＰ⁃ＳＬＡＭ 的计算开销对比 Ｆｉｇ．１０ Ｃｏｍｐａｒｉｓｏｎ ｏｆ ｃｏｍｐｕｔａｔｉｏｎ ｉｎ ＩＣＰ⁃ＳＬＡＭ 从图 １０ 中可以看出，当采用局部尺度压缩搜索 时，随着 ＩＣＰ 算法迭代次数的增加，ＩＣＰ 算法的运行 时间被保持在一个稳定值，这表明 ＩＣＰ⁃ＳＬＡＭ 的计 算开销被控制在稳定状态。 但采用全局搜索时， ＩＣＰ⁃ＳＬＡＭ 的计算开销却随着 ＩＣＰ 算法迭代次数的 增加而不断增加。 所以局部尺度压缩搜索对提高 ＩＣＰ⁃ＳＬＡＭ 实时性有着显著的效果。 对比图 １０ 和 图 １１ 可知，本文提出的 ＩＣＰ⁃ＳＬＡＭ 比文献［２０］的局 部 ＳＬＡＭ 在计算负载的稳定性上更有优势。 图 １１ 文献［２０］中的 ＳＬＡＭ 波动过程 Ｆｉｇ．１１ ＳＬＡＭ ｕｎｓｔａｂｌｅ ｐｒｏｃｅｓｓ ｉｎ ［２０］ ３．２ 快速 ＩＣＰ⁃ＳＬＡＭ 整体性改善验证 在该验证环节中，实验平台首先以低速走过一 段弧度较小曲线，用来验证 Ｐｔ（ ｘｔ，ｙｔ，θｔ ） 的作用。 然后快速走完一段直线，用来验证（ ｘｔ，ｙｔ ） 的作用。 接着作一个急速转弯，此时 θｔ 会产生巨大的变化。 最后一段小车慢速走过一段短直线和慢速转弯。 图 １２ 为对比结果。 在图 １２（ ａ）的方法中，ＩＣＰ 算法 开始时会采用粗匹配，并且通过局部尺度压缩搜索 来完成匹配点搜索（Ｆｄｅｃ ＝ ０．５）。 图 １２（ｂ）中的对照 组为传统的 ＩＣＰ⁃ＳＬＡＭ 算法。 完成整个 ＳＬＡＭ 过程 第 ３ 期 张金艺，等：粗匹配和局部尺度压缩搜索下的快速 ＩＣＰ－ＳＬＡＭ ·４１９·

·420 智能系统学报 第12卷 中，图12(a)的方法总共进行了28200次迭代，1CP Automation.Jinan:IEEE,2010:1045-1050 算法运行时间为4414ms。图12(b)中的方法进行 [2]BARRAU A,BONNABEL S.Invariant filtering for Pose 了399109次迭代，I1CP算法运行时间为301152ms。 EKF-SLAM aided by an IMU[C]//Proceedings of 2015 可见，图12(a)的方法比图12(b)的方法减少了 IEEE Conference on Decision and Control.Osaka:IEEE, 2015:2133-2138. 92.34%的迭代次数和98.86%的运行时间。此外，在 「31季晓玲，贺青，迟宗涛.基于EKF的SLAM算法在机器 图12(a)方法的结果中，累积误差被大大消除了，创 人定位中的应用[J].科技经济导刊，2016(13)： 建的地图精度也比图12(b)的好，并且没有出现匹 17-19 配ICP算法失锁。综上，图12(a)的方法不仅提高 [4]ZANDARA S,RIDAO P,RIBAS D,et al.Probabilistic 了ICP-SLAM实时性，而且ICP-SLAM的整体性能 surface matching for bathymetry based SLAM[C]//Pro- 得到很大的提升。 ceedings of 2013 IEEE International Conference on 真实路径 理论路径 Robotics and Automation.Karlsruhe:IEEE,2013:40-45. [5]ALBERT P,RIDAO P,RIBAS D,et al.Bathymetry-based SLAM with difference of normals point-cloud subsampling and probabilistic ICP registration[C]//Proceedings of 2013 MTS/IEEE OCEANS-Bergen.Bergen:IEEE,2013: 1-8 [6]TREHARD G,ALSAYED Z,POLLARD E,et al. Credibilist simultaneous Localization And Mapping with a LIDAR[C]//Proceedings of 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems.Chicago, L:EEE,2014:2699-2706. [7]ARTH C,PIRCHHEIM C,VENTURA J,et al.Instant out- door localization and SLAM initialization from 2.5D maps [J].IEEE transactions on visualization and computer graphics,2015,21(11):1309-1318. (a)快速ICP-SLAM (b)传统ICP-SLAM [8]CHOUDHARY S,INDELMAN V,CHRISTENSEN H I,et 图12快速ICP.SLAM整体性改善验证结果 al.Information-based reduced landmark SLAM[C]//Pro- Fig.12 Overall improvement in fast ICP-SLAM ceedings of 2015 IEEE International Conference on Robotics and Automation ICRA).Seattle,WA:IEEE. 4结束语 2015:4620-4627. [9]陈兴秀，张金艺，晏理，等.三维复杂运动模式航迹推 针对传统ICP-SLAM实时性差，本文提出了粗 算惯性导航室内定位[J].应用科学学报.2014,32(4)： 匹配和局部尺度压缩搜索。在进行ICP算法开始之 349-350. 前，通过MEMS磁力计和全局地标计算出机器人当 CHEN Xingxiu ZHANG Jinyi YAN Li et al.Inertial 前位姿矩阵，并基于该位姿矩阵完成当前帧和全局 indoor navigation with 3D complex motion mode of 地图的粗匹配，从而减少ICP算法的迭代次数。同 pedestrian dead reckoning[J].Journal of appliend sciences- 时在ICP算法每次迭代中，采用局部尺度压缩搜索 electronics and information engineering,2014,32 (4): 替代全局搜索完成匹配点搜索，加快匹配点搜索速 349-350. 度。实验结果表明ICP-SLAM实时性得到了很大提 [10]张苍松，郭军，崔娇，等.基于RSI的室内定位算法优 升，迭代次数和系统运行时间分别降低92.34%和 化技术[J].计算机工程与应用，2015,51(3)： 235-238. 98.86%。此外，ICP-SLAM的整体性能得到很大的 ZHANG Cangsong,GUO Jun,CUI Jiao,et al.Indoor po- 提升。 sitioning optimization techniques based on RSSI[J].Com- 参考文献： puter engineering and applications,2015,51 (3): 235-238. [1]LI Hai,CHEN Qijun.Towards a non-probabilistic approach [11]王益健.蓝牙室内定位关键技术的研究与实现[D].南 to hybrid geometry-topological SLAM[C]//Proceedings of 京：东南大学，2015. 2010 8th World Congress of IEEE on Intelligent Control and WANG Yijian.Research and implementation on key tech-

中，图１２（ａ）的方法总共进行了 ２８ ２００ 次迭代，ＩＣＰ 算法运行时间为 ４ ４１４ ｍｓ。 图 １２（ｂ）中的方法进行 了３９９ １０９次迭代，ＩＣＰ 算法运行时间为 ３０１ １５２ ｍｓ。 可见，图 １２ （ ａ） 的方法比图 １２ （ ｂ） 的方法减少了 ９２．３４％的迭代次数和 ９８．８６％的运行时间。 此外，在 图 １２（ａ）方法的结果中，累积误差被大大消除了，创 建的地图精度也比图 １２（ ｂ）的好，并且没有出现匹 配 ＩＣＰ 算法失锁。 综上，图 １２（ａ）的方法不仅提高 了 ＩＣＰ⁃ＳＬＡＭ 实时性，而且 ＩＣＰ⁃ＳＬＡＭ 的整体性能 得到很大的提升。 图 １２ 快速 ＩＣＰ⁃ＳＬＡＭ 整体性改善验证结果 Ｆｉｇ．１２ Ｏｖｅｒａｌｌ ｉｍｐｒｏｖｅｍｅｎｔ ｉｎ ｆａｓｔ ＩＣＰ⁃ＳＬＡＭ ４ 结束语 针对传统 ＩＣＰ⁃ＳＬＡＭ 实时性差，本文提出了粗 匹配和局部尺度压缩搜索。 在进行 ＩＣＰ 算法开始之 前，通过 ＭＥＭＳ 磁力计和全局地标计算出机器人当 前位姿矩阵，并基于该位姿矩阵完成当前帧和全局 地图的粗匹配，从而减少 ＩＣＰ 算法的迭代次数。 同 时在 ＩＣＰ 算法每次迭代中，采用局部尺度压缩搜索 替代全局搜索完成匹配点搜索，加快匹配点搜索速 度。 实验结果表明 ＩＣＰ⁃ＳＬＡＭ 实时性得到了很大提 升，迭代次数和系统运行时间分别降低 ９２．３４％和 ９８．８６％。 此外，ＩＣＰ⁃ＳＬＡＭ 的整体性能得到很大的 提升。 参考文献： ［１］ ＬＩ Ｈａｉ， ＣＨＥＮ Ｑｉｊｕｎ． Ｔｏｗａｒｄｓ ａ ｎｏｎ⁃ｐｒｏｂａｂｉｌｉｓｔｉｃ ａｐｐｒｏａｃｈ ｔｏ ｈｙｂｒｉｄ ｇｅｏｍｅｔｒｙ⁃ｔｏｐｏｌｏｇｉｃａｌ ＳＬＡＭ［Ｃ］ ／ ／ Ｐｒｏｃｅｅｄｉｎｇｓ ｏｆ ２０１０ ８ｔｈ Ｗｏｒｌｄ Ｃｏｎｇｒｅｓｓ ｏｆ ＩＥＥＥ ｏｎ Ｉｎｔｅｌｌｉｇｅｎｔ Ｃｏｎｔｒｏｌ ａｎｄ Ａｕｔｏｍａｔｉｏｎ． Ｊｉｎａｎ： ＩＥＥＥ， ２０１０： １０４５－１０５０． ［２］ ＢＡＲＲＡＵ Ａ， ＢＯＮＮＡＢＥＬ Ｓ． Ｉｎｖａｒｉａｎｔ ｆｉｌｔｅｒｉｎｇ ｆｏｒ Ｐｏｓｅ ＥＫＦ⁃ＳＬＡＭ ａｉｄｅｄ ｂｙ ａｎ ＩＭＵ ［ Ｃ］ ／ ／ Ｐｒｏｃｅｅｄｉｎｇｓ ｏｆ ２０１５ ＩＥＥＥ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｄｅｃｉｓｉｏｎ ａｎｄ Ｃｏｎｔｒｏｌ． Ｏｓａｋａ： ＩＥＥＥ， ２０１５： ２１３３－２１３８． ［３］季晓玲， 贺青， 迟宗涛． 基于 ＥＫＦ 的 ＳＬＡＭ 算法在机器 人定位 中 的 应 用 ［ Ｊ］． 科 技 经 济 导 刊， ２０１６ （ １３ ）： １７－１９． ［４］ ＺＡＮＤＡＲＡ Ｓ， ＲＩＤＡＯ Ｐ， ＲＩＢＡＳ Ｄ， ｅｔ ａｌ． Ｐｒｏｂａｂｉｌｉｓｔｉｃ ｓｕｒｆａｃｅ ｍａｔｃｈｉｎｇ ｆｏｒ ｂａｔｈｙｍｅｔｒｙ ｂａｓｅｄ ＳＬＡＭ ［ Ｃ］ ／ ／ Ｐｒｏ⁃ ｃｅｅｄｉｎｇｓ ｏｆ ２０１３ ＩＥＥＥ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ． Ｋａｒｌｓｒｕｈｅ： ＩＥＥＥ， ２０１３： ４０－４５． ［５］ＡＬＢＥＲＴ Ｐ， ＲＩＤＡＯ Ｐ， ＲＩＢＡＳ Ｄ， ｅｔ ａｌ． Ｂａｔｈｙｍｅｔｒｙ⁃ｂａｓｅｄ ＳＬＡＭ ｗｉｔｈ ｄｉｆｆｅｒｅｎｃｅ ｏｆ ｎｏｒｍａｌｓ ｐｏｉｎｔ⁃ｃｌｏｕｄ ｓｕｂｓａｍｐｌｉｎｇ ａｎｄ ｐｒｏｂａｂｉｌｉｓｔｉｃ ＩＣＰ ｒｅｇｉｓｔｒａｔｉｏｎ［Ｃ］ ／ ／ Ｐｒｏｃｅｅｄｉｎｇｓ ｏｆ ２０１３ ＭＴＳ ／ ＩＥＥＥ ＯＣＥＡＮＳ⁃Ｂｅｒｇｅｎ． Ｂｅｒｇｅｎ： ＩＥＥＥ， ２０１３： １－８． ［ ６ ］ ＴＲＥＨＡＲＤ Ｇ， ＡＬＳＡＹＥＤ Ｚ， ＰＯＬＬＡＲＤ Ｅ， ｅｔ ａｌ． Ｃｒｅｄｉｂｉｌｉｓｔ ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ Ａｎｄ Ｍａｐｐｉｎｇ ｗｉｔｈ ａ ＬＩＤＡＲ［Ｃ］ ／ ／ Ｐｒｏｃｅｅｄｉｎｇｓ ｏｆ ２０１４ ＩＥＥＥ／ ＲＳＪ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｉｎｔｅｌｌｉｇｅｎｔ Ｒｏｂｏｔｓ ａｎｄ Ｓｙｓｔｅｍｓ． Ｃｈｉｃａｇｏ， ＩＬ： ＩＥＥＥ， ２０１４： ２６９９－２７０６． ［７］ＡＲＴＨ Ｃ， ＰＩＲＣＨＨＥＩＭ Ｃ， ＶＥＮＴＵＲＡ Ｊ， ｅｔ ａｌ． Ｉｎｓｔａｎｔ ｏｕｔ⁃ ｄｏｏｒ ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ ＳＬＡＭ ｉｎｉｔｉａｌｉｚａｔｉｏｎ ｆｒｏｍ ２． ５Ｄ ｍａｐｓ ［ Ｊ ］． ＩＥＥＥ ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ ｖｉｓｕａｌｉｚａｔｉｏｎ ａｎｄ ｃｏｍｐｕｔｅｒ ｇｒａｐｈｉｃｓ， ２０１５， ２１（１１）： １３０９－１３１８． ［８］ＣＨＯＵＤＨＡＲＹ Ｓ， ＩＮＤＥＬＭＡＮ Ｖ， ＣＨＲＩＳＴＥＮＳＥＮ Ｈ Ｉ， ｅｔ ａｌ． Ｉｎｆｏｒｍａｔｉｏｎ⁃ｂａｓｅｄ ｒｅｄｕｃｅｄ ｌａｎｄｍａｒｋ ＳＬＡＭ［ Ｃ］ ／ ／ Ｐｒｏ⁃ ｃｅｅｄｉｎｇｓ ｏｆ ２０１５ ＩＥＥＥ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ （ ＩＣＲＡ）． Ｓｅａｔｔｌｅ， ＷＡ： ＩＥＥＥ， ２０１５： ４６２０－４６２７． ［９］陈兴秀， 张金艺， 晏理， 等． 三维复杂运动模式航迹推 算惯性导航室内定位［Ｊ］． 应用科学学报， ２０１４， ３２（４）： ３４９－３５０． ＣＨＥＮ Ｘｉｎｇｘｉｕ ， ＺＨＡＮＧ Ｊｉｎｙｉ ， ＹＡＮ Ｌｉ ， ｅｔ ａｌ． Ｉｎｅｒｔｉａｌ ｉｎｄｏｏｒ ｎａｖｉｇａｔｉｏｎ ｗｉｔｈ ３Ｄ ｃｏｍｐｌｅｘ ｍｏｔｉｏｎ ｍｏｄｅ ｏｆ ｐｅｄｅｓｔｒｉａｎ ｄｅａｄ ｒｅｃｋｏｎｉｎｇ［ Ｊ］．Ｊｏｕｒｎａｌ ｏｆ ａｐｐｌｉｅｎｄ ｓｃｉｅｎｃｅｓ⁃ ｅｌｅｃｔｒｏｎｉｃｓ ａｎｄ ｉｎｆｏｒｍａｔｉｏｎ ｅｎｇｉｎｅｅｒｉｎｇ， ２０１４， ３２ （ ４ ）： ３４９－３５０． ［１０］张苍松， 郭军， 崔娇， 等． 基于 ＲＳＳＩ 的室内定位算法优 化技 术 ［ Ｊ ］． 计 算 机 工 程 与 应 用， ２０１５， ５１ （ ３ ）： ２３５－２３８． ＺＨＡＮＧ Ｃａｎｇｓｏｎｇ， ＧＵＯ Ｊｕｎ， ＣＵＩ Ｊｉａｏ， ｅｔ ａｌ． Ｉｎｄｏｏｒ ｐｏ⁃ ｓｉｔｉｏｎｉｎｇ ｏｐｔｉｍｉｚａｔｉｏｎ ｔｅｃｈｎｉｑｕｅｓ ｂａｓｅｄ ｏｎ ＲＳＳＩ［Ｊ］． Ｃｏｍ⁃ ｐｕｔｅｒ ｅｎｇｉｎｅｅｒｉｎｇ ａｎｄ ａｐｐｌｉｃａｔｉｏｎｓ， ２０１５， ５１ （ ３ ）： ２３５－２３８． ［１１］王益健． 蓝牙室内定位关键技术的研究与实现［Ｄ］． 南 京： 东南大学， ２０１５． ＷＡＮＧ Ｙｉｊｉａｎ． Ｒｅｓｅａｒｃｈ ａｎｄ ｉｍｐｌｅｍｅｎｔａｔｉｏｎ ｏｎ ｋｅｙ ｔｅｃｈ⁃ ·４２０· 智 能 系 统 学 报 第 １２ 卷

第3期 张金艺，等：粗匹配和局部尺度压缩搜索下的快速ICP-SLAM ·421. nologies of bluetooth indoor positioning D].Nanjing: [20]TIAR R,LAKROUF M,AZOUAOUI O.FAST ICP-SLAM Southeast University,2015. for a bi-steerable mobile robot in large environments[C]// [12]BESL P J,MCKAY N D.Method for registration of 3-D Proceedings of 2015 International Conference on Advanced shapes[J.IEEE transactions on pattern analysis and ma- Robotics (ICAR).Istanbul:IEEE,2015:1-6. chine intelligence,1992,14(2):239-256. [13]RUSINKIEWICZ S,LEVOY M.Efficient variants of the [21]TIAR R,OUADAH N,AZOUAOUI O,et al.ICP-SLAM ICP algorithm[C]//Proceedings of the Third International methods implementation on a bi-steerable mobile robot Conference on 3-D Digital Imaging and Modeling.Quebec [C]//Proceedings of IEEE 11th International Workshop of City,Que:IEEE,2001:145. Electronics,Control,Measurement,Signals and their Ap- [14]BLANCO J L,GONZGLEZ-JIMeNEZ J,FERNANDEZ- plication to Mechatronics ECMSM).Toulouse:IEEE, MADRIGAL JA.A robust,multi-hypothesis approach to 2013:1-6. matching occupancy grid maps[].Robotica,2013,31 作者简介： (5):687-701. 张金艺，男，1965年生，研究员主 [15]XU Haixia,ZHOU Wei,ZHU Jiang.3D visual SLAM with 要研究方向为通信类SC设计与室内 a time-of-flight camera C]//Proceedings of 2015 IEEE 无线定位技术。发表学术论文40余 Workshop on Signal Processing Systems SiPS ) 篇，近3年授权与申请专利30项。 Hangzhou:IEEE,2015:1-6. [16 ULAS C,TEMELTAS H.A robust feature extraction method and semantic data association for 6D SLAM[C]/ Proceedings of 2015 IEEE World Automation Congress 梁滨，男，1991年生，硕士研究生， WAC).Mexico:IEEE,2012:1-6. 主要研究方向为基于激光雷达的室内 [17]GONG Zizhen,HUA Xianghong,YI Chongzheng,et al. The research and implementation of ICP based on Delaunay SLAM. triangulation[].Engineering of surveying and mapping, 2010,19(5):29-31. [18]HU Linjia,NOOSHABADI S,AHMADI M.Massively par- allel KD-tree construction and nearest neighbor search al- gorithms [C]//Proceedings of 2015 IEEE International 唐笛恺，男，1991年生，硕士研究 Symposium on Circuits and Systems (ISCAS).Lisbon: 生，主要研究方向为基于激光雷达的室 IEEE.2015:2752-2755. 内SLAM。 [19]ZHANG Lei,CHOI S I,PARK S Y.Polar-Cartesian hybrid transforms:a novel 2D range scan registration algo- rithm[].International journal of control automation and systems,2013,11(5):1001-1008

ｎｏｌｏｇｉｅｓ ｏｆ ｂｌｕｅｔｏｏｔｈ ｉｎｄｏｏｒ ｐｏｓｉｔｉｏｎｉｎｇ ［ Ｄ ］． Ｎａｎｊｉｎｇ： Ｓｏｕｔｈｅａｓｔ Ｕｎｉｖｅｒｓｉｔｙ，２０１５． ［１２］ＢＥＳＬ Ｐ Ｊ， ＭＣＫＡＹ Ｎ Ｄ． Ｍｅｔｈｏｄ ｆｏｒ ｒｅｇｉｓｔｒａｔｉｏｎ ｏｆ ３－Ｄ ｓｈａｐｅｓ［Ｊ］． ＩＥＥＥ ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ ｐａｔｔｅｒｎ ａｎａｌｙｓｉｓ ａｎｄ ｍａ⁃ ｃｈｉｎｅ ｉｎｔｅｌｌｉｇｅｎｃｅ， １９９２， １４（２）： ２３９－２５６． ［１３］ ＲＵＳＩＮＫＩＥＷＩＣＺ Ｓ， ＬＥＶＯＹ Ｍ． Ｅｆｆｉｃｉｅｎｔ ｖａｒｉａｎｔｓ ｏｆ ｔｈｅ ＩＣＰ ａｌｇｏｒｉｔｈｍ［Ｃ］ ／ ／ Ｐｒｏｃｅｅｄｉｎｇｓ ｏｆ ｔｈｅ Ｔｈｉｒｄ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ ３⁃Ｄ Ｄｉｇｉｔａｌ Ｉｍａｇｉｎｇ ａｎｄ Ｍｏｄｅｌｉｎｇ． Ｑｕｅｂｅｃ Ｃｉｔｙ， Ｑｕｅ： ＩＥＥＥ， ２００１： １４５． ［１４］ ＢＬＡＮＣＯ Ｊ Ｌ， ＧＯＮＺáＬＥＺ⁃ＪＩＭéＮＥＺ Ｊ， ＦＥＲＮáＮＤＥＺ⁃ ＭＡＤＲＩＧＡＬ Ｊ Ａ． Ａ ｒｏｂｕｓｔ， ｍｕｌｔｉ⁃ｈｙｐｏｔｈｅｓｉｓ ａｐｐｒｏａｃｈ ｔｏ ｍａｔｃｈｉｎｇ ｏｃｃｕｐａｎｃｙ ｇｒｉｄ ｍａｐｓ ［ Ｊ］． Ｒｏｂｏｔｉｃａ， ２０１３， ３１ （５）： ６８７－７０１． ［１５］ＸＵ Ｈａｉｘｉａ， ＺＨＯＵ Ｗｅｉ， ＺＨＵ Ｊｉａｎｇ． ３Ｄ ｖｉｓｕａｌ ＳＬＡＭ ｗｉｔｈ ａ ｔｉｍｅ⁃ｏｆ⁃ｆｌｉｇｈｔ ｃａｍｅｒａ ［ Ｃ］ ／ ／ Ｐｒｏｃｅｅｄｉｎｇｓ ｏｆ ２０１５ ＩＥＥＥ Ｗｏｒｋｓｈｏｐ ｏｎ Ｓｉｇｎａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｓｙｓｔｅｍｓ （ ＳｉＰＳ ）． Ｈａｎｇｚｈｏｕ： ＩＥＥＥ， ２０１５： １－６． ［ １６ ］ ＵＬＡＳ Ｃ， ＴＥＭＥＬＴＡＳ Ｈ． Ａ ｒｏｂｕｓｔ ｆｅａｔｕｒｅ ｅｘｔｒａｃｔｉｏｎ ｍｅｔｈｏｄ ａｎｄ ｓｅｍａｎｔｉｃ ｄａｔａ ａｓｓｏｃｉａｔｉｏｎ ｆｏｒ ６Ｄ ＳＬＡＭ［Ｃ］ ／ ／ Ｐｒｏｃｅｅｄｉｎｇｓ ｏｆ ２０１５ ＩＥＥＥ Ｗｏｒｌｄ Ａｕｔｏｍａｔｉｏｎ Ｃｏｎｇｒｅｓｓ （ＷＡＣ）． Ｍｅｘｉｃｏ： ＩＥＥＥ， ２０１２： １－６． ［１７］ ＧＯＮＧ Ｚｉｚｈｅｎ， ＨＵＡ Ｘｉａｎｇｈｏｎｇ， ＹＩ Ｃｈｏｎｇｚｈｅｎｇ， ｅｔ ａｌ． Ｔｈｅ ｒｅｓｅａｒｃｈ ａｎｄ ｉｍｐｌｅｍｅｎｔａｔｉｏｎ ｏｆ ＩＣＰ ｂａｓｅｄ ｏｎ Ｄｅｌａｕｎａｙ ｔｒｉａｎｇｕｌａｔｉｏｎ［ Ｊ］． Ｅｎｇｉｎｅｅｒｉｎｇ ｏｆ ｓｕｒｖｅｙｉｎｇ ａｎｄ ｍａｐｐｉｎｇ， ２０１０， １９（５）： ２９－３１． ［１８］ＨＵ Ｌｉｎｊｉａ， ＮＯＯＳＨＡＢＡＤＩ Ｓ， ＡＨＭＡＤＩ Ｍ． Ｍａｓｓｉｖｅｌｙ ｐａｒ⁃ ａｌｌｅｌ ＫＤ⁃ｔｒｅｅ ｃｏｎｓｔｒｕｃｔｉｏｎ ａｎｄ ｎｅａｒｅｓｔ ｎｅｉｇｈｂｏｒ ｓｅａｒｃｈ ａｌ⁃ ｇｏｒｉｔｈｍｓ ［ Ｃ］ ／ ／ Ｐｒｏｃｅｅｄｉｎｇｓ ｏｆ ２０１５ ＩＥＥＥ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｓｙｍｐｏｓｉｕｍ ｏｎ Ｃｉｒｃｕｉｔｓ ａｎｄ Ｓｙｓｔｅｍｓ （ ＩＳＣＡＳ ）． Ｌｉｓｂｏｎ： ＩＥＥＥ， ２０１５： ２７５２－２７５５． ［ １９ ］ ＺＨＡＮＧ Ｌｅｉ， ＣＨＯＩ Ｓ Ｉ， ＰＡＲＫ Ｓ Ｙ． Ｐｏｌａｒ⁃Ｃａｒｔｅｓｉａｎ ｈｙｂｒｉｄ ｔｒａｎｓｆｏｒｍｓ： ａ ｎｏｖｅｌ ２Ｄ ｒａｎｇｅ ｓｃａｎ ｒｅｇｉｓｔｒａｔｉｏｎ ａｌｇｏ⁃ ｒｉｔｈｍ［ Ｊ］． Ｉｎｔｅｒｎａｔｉｏｎａｌ ｊｏｕｒｎａｌ ｏｆ ｃｏｎｔｒｏｌ ａｕｔｏｍａｔｉｏｎ ａｎｄ ｓｙｓｔｅｍｓ， ２０１３， １１（５）： １００１－１００８． ［２０］ＴＩＡＲ Ｒ， ＬＡＫＲＯＵＦ Ｍ， ＡＺＯＵＡＯＵＩ Ｏ． ＦＡＳＴ ＩＣＰ⁃ＳＬＡＭ ｆｏｒ ａ ｂｉ⁃ｓｔｅｅｒａｂｌｅ ｍｏｂｉｌｅ ｒｏｂｏｔ ｉｎ ｌａｒｇｅ ｅｎｖｉｒｏｎｍｅｎｔｓ［Ｃ］ ／ ／ Ｐｒｏｃｅｅｄｉｎｇｓ ｏｆ ２０１５ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ａｄｖａｎｃｅｄ Ｒｏｂｏｔｉｃｓ （ＩＣＡＲ）． Ｉｓｔａｎｂｕｌ： ＩＥＥＥ， ２０１５： １－６． ［２１］ＴＩＡＲ Ｒ， ＯＵＡＤＡＨ Ｎ， ＡＺＯＵＡＯＵＩ Ｏ， ｅｔ ａｌ． ＩＣＰ－ＳＬＡＭ ｍｅｔｈｏｄｓ ｉｍｐｌｅｍｅｎｔａｔｉｏｎ ｏｎ ａ ｂｉ － ｓｔｅｅｒａｂｌｅ ｍｏｂｉｌｅ ｒｏｂｏｔ ［Ｃ］ ／ ／ Ｐｒｏｃｅｅｄｉｎｇｓ ｏｆ ＩＥＥＥ １１ｔｈ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｗｏｒｋｓｈｏｐ ｏｆ Ｅｌｅｃｔｒｏｎｉｃｓ， Ｃｏｎｔｒｏｌ， Ｍｅａｓｕｒｅｍｅｎｔ， Ｓｉｇｎａｌｓ ａｎｄ ｔｈｅｉｒ Ａｐ⁃ ｐｌｉｃａｔｉｏｎ ｔｏ Ｍｅｃｈａｔｒｏｎｉｃｓ （ ＥＣＭＳＭ）． Ｔｏｕｌｏｕｓｅ： ＩＥＥＥ， ２０１３： １－６． 作者简介： 张金艺，男，１９６５ 年生，研究员，主 要研究方向为通信类 ＳｏＣ 设计与室内 无线定位技术。 发表学术论文 ４０ 余 篇，近 ３ 年授权与申请专利 ３０ 项。 梁滨，男，１９９１ 年生，硕士研究生， 主要研究方向为基于激光雷达的室内 ＳＬＡＭ。 唐笛恺，男，１９９１ 年生，硕士研究 生，主要研究方向为基于激光雷达的室 内 ＳＬＡＭ。 第 ３ 期 张金艺，等：粗匹配和局部尺度压缩搜索下的快速 ＩＣＰ－ＳＬＡＭ ·４２１·