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