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

团购合买资源类别：文库，文档格式：PDF，文档页数：9，文件大小：3.5MB

第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)所示

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

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

第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

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

点击进入文档下载页（PDF格式）

已到末页，全文结束

点击下载（PDF格式）

浏览记录