第12卷第4期 智能系统学报 Vol.12 No.4 2017年8月 CAAI Transactions on Intelligent Systems Aug.2017 D0I:10.11992/is.201607007 网s络出版地址：http:/kns.cnki.net/kcms/detail/23.1538.tp.20170407.1734.002.html 多传感器的移动机器人可定位性估计与自定位 孙自飞，钱堃12，马旭东12，戴先中12 (1.东南大学自动化学院，江苏南京210096；2.复杂工程系统测量与控制教育部重点实验室，江苏南京210096) 摘要：针对有人干扰的动态室内环境，利用可定位性估计理论提出一种RGB-D传感器辅助激光传感器的移动机器 人可靠自定位方法。利用RGBD传感器信息快速检测人的位置区域，并通过坐标转换计算激光扫描数据中的动态 障碍物影响因子，结合离散化Fisher信息矩阵在线估计观测信息的可定位性矩阵：同时通过预测模型协方差矩阵评 价里程计信息的可靠性，从而动态补偿观测信息对粒子集的影响。在典型含多人运动的动态室内环境中实验，结果 验证了本文方法能够提高机器人自定位的准确性和可靠性。 关键词：动态环境：自主定位：RGB-D传感器：Fisher信息矩阵：人体检测：可定位性：在线估计：移动机器人 中图分类号：TP24文献标志码：A文章编号：1673-4785(2017)04-0443-07 中文引用格式：孙自飞，钱堃，马旭东，等.多传感器的移动机器人可定位性估计与自定位[J].智能系统学报，2017,12(4)： 443-449. 英文引用格式：SUN Zifei,QIAN Kun,MA Xudong,etal.Self-localization of mobile robot in dynamic environments based on localizability estimation with multi-sensor observation[J].CAAI transactions on intelligent systems,2017,12(4):443-449. Self-localization of mobile robot in dynamic environments based on localizability estimation with multi-sensor observation SUN Zifei',QIAN Kun'2,MA Xudong'2,DAI Xianzhong'2 (1.School of Automation,Southeast University,Nanjing 210096,China;2.Key Laboratory of Measurement and Control of Complex Systems of Engineering,Ministry of Education,Nanjing 210096,China Abstract:Based on the localizability estimation theory,in this paper,we propose a new method for the reliable self-localization of mobile robots in a disturbed dynamic indoor environment by the adoption of an RGB-D sensor to assist the laser scanner.People's location areas are rapidly detected in RGB-D data,which are then transformed to the laser sensor coordinate to compute the influence of the dynamic obstacles on the laser data.In combination with the discrete Fisher information matrix,we estimate the localizability matrix of the observation information online.In addition,we assess the reliability of the information in odometers by the covariance matrix of the prediction model, thereby dynamically compensating for the effect of the observation information on the particle set.We conducted experiments in a dynamic indoor environment and the results confirm the accuracy and reliability of the proposed robot localization method. Keywords:dynamic environment;self-localization;RGB-D sensor;Fisher information matrix;people-detecting; localizability;online estimation;mobile robot 服务机器人长期工作在与人共处的环境中，针 经典粒子滤波算法无法补偿动态环境中未建模障 对采用车载激光、视觉等各种传感器实现的机器人 碍物对机器人定位产生的千扰，R.Valencia等)在 自定位，人的运动会对其定位的准确性造成影响， 经典的蒙特卡罗定位的基础上利用已知的静态地 甚至导致定位失败。 图和在感知更新阶段建立的短期地图进行基于正 以往解决方法大多从改进算法本身人手，由于 态分布变换[]的蒙特卡罗定位，以补偿环境发生短 期变化时对定位精度产生的干扰，该算法满足实时 收稿日期：2016-07-08.网络出版日期：2017-04-07. 性并具有一定的鲁棒性，但是没有考虑静态地图发 基金项目：国家自然科学基金项目(61573100,61573101)：中央高校基 本科研业务费专项基金(2242013K30004). 生永久性变化的情况。J.Rowekamper等[)将蒙特 通信作者：钱堃.E-mail:kqian@(seu.cdu.cm
第 １２ 卷第 ４ 期 智 能 系 统 学 报 Ｖｏｌ．１２ №．４ ２０１７ 年 ８ 月 ＣＡＡＩ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｉｎｔｅｌｌｉｇｅｎｔ Ｓｙｓｔｅｍｓ Ａｕｇ． ２０１７ ＤＯＩ：１０．１１９９２ ／ ｔｉｓ．２０１６０７００７ 网络出版地址：ｈｔｔｐ： ／ ／ ｋｎｓ．ｃｎｋｉ．ｎｅｔ ／ ｋｃｍｓ／ ｄｅｔａｉｌ ／ ２３．１５３８．ｔｐ．２０１７０４０７．１７３４．００２．ｈｔｍｌ 多传感器的移动机器人可定位性估计与自定位 孙自飞１ ，钱堃１，２ ，马旭东１，２ ，戴先中１，２ （１．东南大学 自动化学院，江苏 南京 ２１００９６； ２．复杂工程系统测量与控制教育部重点实验室，江苏 南京 ２１００９６） 摘 要：针对有人干扰的动态室内环境，利用可定位性估计理论提出一种 ＲＧＢ⁃Ｄ 传感器辅助激光传感器的移动机器 人可靠自定位方法。 利用 ＲＧＢ⁃Ｄ 传感器信息快速检测人的位置区域，并通过坐标转换计算激光扫描数据中的动态 障碍物影响因子，结合离散化 Ｆｉｓｈｅｒ 信息矩阵在线估计观测信息的可定位性矩阵；同时通过预测模型协方差矩阵评 价里程计信息的可靠性，从而动态补偿观测信息对粒子集的影响。 在典型含多人运动的动态室内环境中实验，结果 验证了本文方法能够提高机器人自定位的准确性和可靠性。 关键词：动态环境；自主定位；ＲＧＢ⁃Ｄ 传感器；Ｆｉｓｈｅｒ 信息矩阵；人体检测；可定位性；在线估计；移动机器人 中图分类号：ＴＰ２４ 文献标志码：Ａ 文章编号：１６７３－４７８５（２０１７）０４－０４４３－０７ 中文引用格式：孙自飞，钱堃，马旭东，等．多传感器的移动机器人可定位性估计与自定位［ Ｊ］． 智能系统学报， ２０１７， １２ （ ４）： ４４３－４４９． 英文引用格式：ＳＵＮ Ｚｉｆｅｉ， ＱＩＡＮ Ｋｕｎ， ＭＡ Ｘｕｄｏｎｇ， ｅｔ ａｌ． Ｓｅｌｆ⁃ｌｏｃａｌｉｚａｔｉｏｎ ｏｆ ｍｏｂｉｌｅ ｒｏｂｏｔ ｉｎ ｄｙｎａｍｉｃ ｅｎｖｉｒｏｎｍｅｎｔｓ ｂａｓｅｄ ｏｎ ｌｏｃａｌｉｚａｂｉｌｉｔｙ ｅｓｔｉｍａｔｉｏｎ ｗｉｔｈ ｍｕｌｔｉ⁃ｓｅｎｓｏｒ ｏｂｓｅｒｖａｔｉｏｎ［Ｊ］． ＣＡＡＩ ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ ｉｎｔｅｌｌｉｇｅｎｔ ｓｙｓｔｅｍｓ， ２０１７， １２（４）： ４４３－４４９． Ｓｅｌｆ⁃ｌｏｃａｌｉｚａｔｉｏｎ ｏｆ ｍｏｂｉｌｅ ｒｏｂｏｔ ｉｎ ｄｙｎａｍｉｃ ｅｎｖｉｒｏｎｍｅｎｔｓ ｂａｓｅｄ ｏｎ ｌｏｃａｌｉｚａｂｉｌｉｔｙ ｅｓｔｉｍａｔｉｏｎ ｗｉｔｈ ｍｕｌｔｉ⁃ｓｅｎｓｏｒ ｏｂｓｅｒｖａｔｉｏｎ ＳＵＮ Ｚｉｆｅｉ １ ， ＱＩＡＮ Ｋｕｎ １，２ ， ＭＡ Ｘｕｄｏｎｇ １，２ ， ＤＡＩ Ｘｉａｎｚｈｏｎｇ １，２ （１．Ｓｃｈｏｏｌ ｏｆ Ａｕｔｏｍａｔｉｏｎ， Ｓｏｕｔｈｅａｓｔ Ｕｎｉｖｅｒｓｉｔｙ， Ｎａｎｊｉｎｇ ２１００９６， Ｃｈｉｎａ； ２．Ｋｅｙ Ｌａｂｏｒａｔｏｒｙ ｏｆ Ｍｅａｓｕｒｅｍｅｎｔ ａｎｄ Ｃｏｎｔｒｏｌ ｏｆ Ｃｏｍｐｌｅｘ Ｓｙｓｔｅｍｓ ｏｆ Ｅｎｇｉｎｅｅｒｉｎｇ， Ｍｉｎｉｓｔｒｙ ｏｆ Ｅｄｕｃａｔｉｏｎ， Ｎａｎｊｉｎｇ ２１００９６， Ｃｈｉｎａ ） Ａｂｓｔｒａｃｔ：Ｂａｓｅｄ ｏｎ ｔｈｅ ｌｏｃａｌｉｚａｂｉｌｉｔｙ ｅｓｔｉｍａｔｉｏｎ ｔｈｅｏｒｙ， ｉｎ ｔｈｉｓ ｐａｐｅｒ， ｗｅ ｐｒｏｐｏｓｅ ａ ｎｅｗ ｍｅｔｈｏｄ ｆｏｒ ｔｈｅ ｒｅｌｉａｂｌｅ ｓｅｌｆ⁃ｌｏｃａｌｉｚａｔｉｏｎ ｏｆ ｍｏｂｉｌｅ ｒｏｂｏｔｓ ｉｎ ａ ｄｉｓｔｕｒｂｅｄ ｄｙｎａｍｉｃ ｉｎｄｏｏｒ ｅｎｖｉｒｏｎｍｅｎｔ ｂｙ ｔｈｅ ａｄｏｐｔｉｏｎ ｏｆ ａｎ ＲＧＢ⁃Ｄ ｓｅｎｓｏｒ ｔｏ ａｓｓｉｓｔ ｔｈｅ ｌａｓｅｒ ｓｃａｎｎｅｒ． Ｐｅｏｐｌｅ’ｓ ｌｏｃａｔｉｏｎ ａｒｅａｓ ａｒｅ ｒａｐｉｄｌｙ ｄｅｔｅｃｔｅｄ ｉｎ ＲＧＢ－Ｄ ｄａｔａ， ｗｈｉｃｈ ａｒｅ ｔｈｅｎ ｔｒａｎｓｆｏｒｍｅｄ ｔｏ ｔｈｅ ｌａｓｅｒ ｓｅｎｓｏｒ ｃｏｏｒｄｉｎａｔｅ ｔｏ ｃｏｍｐｕｔｅ ｔｈｅ ｉｎｆｌｕｅｎｃｅ ｏｆ ｔｈｅ ｄｙｎａｍｉｃ ｏｂｓｔａｃｌｅｓ ｏｎ ｔｈｅ ｌａｓｅｒ ｄａｔａ． Ｉｎ ｃｏｍｂｉｎａｔｉｏｎ ｗｉｔｈ ｔｈｅ ｄｉｓｃｒｅｔｅ Ｆｉｓｈｅｒ ｉｎｆｏｒｍａｔｉｏｎ ｍａｔｒｉｘ， ｗｅ ｅｓｔｉｍａｔｅ ｔｈｅ ｌｏｃａｌｉｚａｂｉｌｉｔｙ ｍａｔｒｉｘ ｏｆ ｔｈｅ ｏｂｓｅｒｖａｔｉｏｎ ｉｎｆｏｒｍａｔｉｏｎ ｏｎｌｉｎｅ． Ｉｎ ａｄｄｉｔｉｏｎ， ｗｅ ａｓｓｅｓｓ ｔｈｅ ｒｅｌｉａｂｉｌｉｔｙ ｏｆ ｔｈｅ ｉｎｆｏｒｍａｔｉｏｎ ｉｎ ｏｄｏｍｅｔｅｒｓ ｂｙ ｔｈｅ ｃｏｖａｒｉａｎｃｅ ｍａｔｒｉｘ ｏｆ ｔｈｅ ｐｒｅｄｉｃｔｉｏｎ ｍｏｄｅｌ， ｔｈｅｒｅｂｙ ｄｙｎａｍｉｃａｌｌｙ ｃｏｍｐｅｎｓａｔｉｎｇ ｆｏｒ ｔｈｅ ｅｆｆｅｃｔ ｏｆ ｔｈｅ ｏｂｓｅｒｖａｔｉｏｎ ｉｎｆｏｒｍａｔｉｏｎ ｏｎ ｔｈｅ ｐａｒｔｉｃｌｅ ｓｅｔ． Ｗｅ ｃｏｎｄｕｃｔｅｄ ｅｘｐｅｒｉｍｅｎｔｓ ｉｎ ａ ｄｙｎａｍｉｃ ｉｎｄｏｏｒ ｅｎｖｉｒｏｎｍｅｎｔ ａｎｄ ｔｈｅ ｒｅｓｕｌｔｓ ｃｏｎｆｉｒｍ ｔｈｅ ａｃｃｕｒａｃｙ ａｎｄ ｒｅｌｉａｂｉｌｉｔｙ ｏｆ ｔｈｅ ｐｒｏｐｏｓｅｄ ｒｏｂｏｔ ｌｏｃａｌｉｚａｔｉｏｎ ｍｅｔｈｏｄ． Ｋｅｙｗｏｒｄｓ： ｄｙｎａｍｉｃ ｅｎｖｉｒｏｎｍｅｎｔ； ｓｅｌｆ⁃ｌｏｃａｌｉｚａｔｉｏｎ； ＲＧＢ⁃Ｄ ｓｅｎｓｏｒ； Ｆｉｓｈｅｒ ｉｎｆｏｒｍａｔｉｏｎ ｍａｔｒｉｘ； ｐｅｏｐｌｅ⁃ｄｅｔｅｃｔｉｎｇ； ｌｏｃａｌｉｚａｂｉｌｉｔｙ； ｏｎｌｉｎｅ ｅｓｔｉｍａｔｉｏｎ； ｍｏｂｉｌｅ ｒｏｂｏｔ 收稿日期：２０１６－０７－０８． 网络出版日期：２０１７－０４－０７． 基金项目：国家自然科学基金项目（ ６１５７３１００，６１５７３１０１）；中央高校基 本科研业务费专项基金（２２４２０１３Ｋ３０００４）． 通信作者：钱堃． Ｅ⁃ｍａｉｌ：ｋｑｉａｎ＠ ｓｅｕ．ｅｄｕ．ｃｎ． 服务机器人长期工作在与人共处的环境中，针 对采用车载激光、视觉等各种传感器实现的机器人 自定位，人的运动会对其定位的准确性造成影响， 甚至导致定位失败。 以往解决方法大多从改进算法本身入手，由于 经典粒子滤波算法无法补偿动态环境中未建模障 碍物对机器人定位产生的干扰，Ｒ．Ｖａｌｅｎｃｉａ 等［１］ 在 经典的蒙特卡罗定位的基础上利用已知的静态地 图和在感知更新阶段建立的短期地图进行基于正 态分布变换［２］的蒙特卡罗定位，以补偿环境发生短 期变化时对定位精度产生的干扰，该算法满足实时 性并具有一定的鲁棒性，但是没有考虑静态地图发 生永久性变化的情况。 Ｊ．Ｒöｗｅｋäｍｐｅｒ 等［３］ 将蒙特
·444. 智能系统学报 第12卷 卡罗定位[、KLD采样和使用扫描配准)的精细定 体的检测结果估计每一束激光读数中存在未建模 位结合起来，在不改变环境条件的情况下实现机器 动态障碍物的概率，利用动态可定位性估计和预测 人移动作业。这种方法虽然精度高但是实现起来 模型协方差对粒子集进行校正]。两种传感器之 复杂而且实时性难以保证。文献[6]通过对观测模 间的位置关系和总体方法如图1所示。 型可定位性和基于里程计的预测模型协方差两个 K 指标的在线估计，动态调节两者在粒子集)位姿修 正中的比重。这种方法满足一定的实时性，但是对 Kinect坐标系 于动态障碍物影响因子的计算需要离线获得，并没 R位置关系表示 有利用传感器数据来实时地更新未建模目标的分 布信息。 Lx Turtlebot移动平台 以上算法都是单一地利用激光传感器，仅通过 算法改进提高动态环境下的定位精度。近年来，更 激光坐标系 多的研究者倾向于使用多传感器的信息融合技 W 术[⑧]。尤其是随着机器视觉的发展，将视觉与激光 传感器结合使用为动态环境下机器人定位提供了 新思路，如段鹏等采用分布式激光雷达和视觉摄 像机，将基于自然特征的场景识别技术与Monte (a)激光传感器和RGB-D传感器的位置关系 Calo方法有机结合，提出了一种实时的服务机器人 RGB-D感知（人体检测） 位姿估计方法。本文考虑到当环境中未建模动态 障碍物主要为人时，从激光传感器读数中检测人腿 地面投影、坐标转换 激光读数 存在一定困难，而RGB-D传感器相比普通视觉摄像 确定激光数据由人体干 机，不仅提供场景的颜色信息，还提供深度信息，因 扰引起的范围 此利用RGB-D传感器能够快速检测人体躯干的位 置及区域[0)，并利用投影后的人体躯干点云估算 高斯模型估计未建模 动态障碍物先验概率 激光数据受人腿千扰的范围。但Kinect传感器的 精度却比激光传感器低，因此如何发挥两者各自优 动态障碍物影响因子 和可定位性矩阵计算 势、研究一种更易于实现的动态环境下可靠、快速 地机器人自定位方法，是问题的关键。 粒子集动态修正 为此，本文考虑环境中的人为主要未建模动态 (b)总体方法流程 障碍物，提出一种基于多传感器的可定位性在线估 图1两种传感器的位置关系和总体方法 计方法，并用于解决机器人可靠自定位问题。利用 Fig.1 Positional relationship of two sensors and method outline RGB-D传感器对人体的实时检测结果，估算激光数 激光传感器和RGB-D传感器之间的位置关系 据是由人腿干扰引起的范围，并使用高斯模型估计 该范围内每一束激光上的存在未建模动态障碍物 用旋转矩阵R和平移矩阵T表示，图1(a)描述了 的先验概率。解决了动态障碍物影响因子和动态 本文实验平台中两种传感器之间的位置关系，其中： 可定位性矩阵在线估计问题，使动态可定位性矩阵 「1 0 0 0 「100 0.1137 更加真实可靠地反映环境中的未建模动态障碍物。 0 -1 0 0 0 100.012 R= T= 1总体方法 0 0 -10 001 -0.13 0 0 0 00 1 由于RGB-D传感器与激光传感器精度存在较 (1) 大差异，本文并不是将两者数据直接融合，而是在 可定位性理论框架下利用RGB-D传感器信息辅助 2 利用多传感器信息的可定位性估计 激光传感器下的动态可定位性在线估计，从而既不 丢失激光传感器定位精度，又能避免较为困难地从 2.1 动态可定位性矩阵估计 激光读数中检测人腿。即利用RGB-D传感器对人 静态可定位性矩阵是基于统计学理论中的
卡罗定位［４］ 、ＫＬＤ 采样和使用扫描配准［５］ 的精细定 位结合起来，在不改变环境条件的情况下实现机器 人移动作业。 这种方法虽然精度高但是实现起来 复杂而且实时性难以保证。 文献［６］通过对观测模 型可定位性和基于里程计的预测模型协方差两个 指标的在线估计，动态调节两者在粒子集［７］ 位姿修 正中的比重。 这种方法满足一定的实时性，但是对 于动态障碍物影响因子的计算需要离线获得，并没 有利用传感器数据来实时地更新未建模目标的分 布信息。 以上算法都是单一地利用激光传感器，仅通过 算法改进提高动态环境下的定位精度。 近年来，更 多的研究者倾向于使用多传感器的信息融合技 术［８］ 。 尤其是随着机器视觉的发展，将视觉与激光 传感器结合使用为动态环境下机器人定位提供了 新思路，如段鹏等［９］采用分布式激光雷达和视觉摄 像机，将基于自然特征的场景识别技术与 Ｍｏｎｔｅ Ｃａｒｌｏ 方法有机结合，提出了一种实时的服务机器人 位姿估计方法。 本文考虑到当环境中未建模动态 障碍物主要为人时，从激光传感器读数中检测人腿 存在一定困难，而 ＲＧＢ⁃Ｄ 传感器相比普通视觉摄像 机，不仅提供场景的颜色信息，还提供深度信息，因 此利用 ＲＧＢ⁃Ｄ 传感器能够快速检测人体躯干的位 置及区域［１０－１１］ ，并利用投影后的人体躯干点云估算 激光数据受人腿干扰的范围。 但 Ｋｉｎｅｃｔ 传感器的 精度却比激光传感器低，因此如何发挥两者各自优 势、研究一种更易于实现的动态环境下可靠、快速 地机器人自定位方法，是问题的关键。 为此，本文考虑环境中的人为主要未建模动态 障碍物，提出一种基于多传感器的可定位性在线估 计方法，并用于解决机器人可靠自定位问题。 利用 ＲＧＢ⁃Ｄ 传感器对人体的实时检测结果，估算激光数 据是由人腿干扰引起的范围，并使用高斯模型估计 该范围内每一束激光上的存在未建模动态障碍物 的先验概率。 解决了动态障碍物影响因子和动态 可定位性矩阵在线估计问题，使动态可定位性矩阵 更加真实可靠地反映环境中的未建模动态障碍物。 １ 总体方法 由于 ＲＧＢ⁃Ｄ 传感器与激光传感器精度存在较 大差异，本文并不是将两者数据直接融合，而是在 可定位性理论框架下利用 ＲＧＢ⁃Ｄ 传感器信息辅助 激光传感器下的动态可定位性在线估计，从而既不 丢失激光传感器定位精度，又能避免较为困难地从 激光读数中检测人腿。 即利用 ＲＧＢ⁃Ｄ 传感器对人 体的检测结果估计每一束激光读数中存在未建模 动态障碍物的概率，利用动态可定位性估计和预测 模型协方差对粒子集进行校正［１２］ 。 两种传感器之 间的位置关系和总体方法如图 １ 所示。 （ａ）激光传感器和 ＲＧＢ⁃Ｄ 传感器的位置关系 （ｂ）总体方法流程 图 １ 两种传感器的位置关系和总体方法 Ｆｉｇ．１ Ｐｏｓｉｔｉｏｎａｌ ｒｅｌａｔｉｏｎｓｈｉｐ ｏｆ ｔｗｏ ｓｅｎｓｏｒｓ ａｎｄ ｍｅｔｈｏｄ ｏｕｔｌｉｎｅ 激光传感器和 ＲＧＢ⁃Ｄ 传感器之间的位置关系 用旋转矩阵 Ｒ 和平移矩阵 Ｔ 表示，图 １（ ａ）描述了 本文实验平台中两种传感器之间的位置关系，其中： Ｒ ＝ １ ０ ０ ０ ０ － １ ０ ０ ０ ０ － １ ０ ０ ０ ０ １ é ë ê ê ê ê ê ù û ú ú ú ú ú ，Ｔ ＝ １ ０ ０ ０．１１３ ０ １ ０ ０．０１２ ０ ０ １ － ０．１３ ０ ０ ０ １ é ë ê ê ê ê ê ù û ú ú ú ú ú （１） ２ 利用多传感器信息的可定位性估计 ２．１ 动态可定位性矩阵估计 静态可定位性矩阵是基于统计学理论中的 ·４４４· 智 能 系 统 学 报 第 １２ 卷
第4期 孙自飞，等：多传感器的移动机器人可定位性估计与自定位 ·445. Fisher信息矩阵提出的，Fisher信息提供了观测信息 2.2利用RGB-D感知的未建模动态障碍物概率 中蕴含关于待估参数信息量大小的度量。针对服 估计 务机器人依靠激光数据和先验地图进行位姿估计， 由于激光传感器与Kinect传感器的数据已通 A.Censi[13]依据图2的机器人模型将Fisher信息矩 过坐标转换进行了对齐，本文采用Kinect传感器实 阵定义为式(2)所示的激光扫描到环境表面斜率的 现视野中的快速人体检测。在RGB-D数据中使用 函数。 HOG+SVM14]方法实现移动机器人作业场景中人体 的检测并得到关于人体的点云数据。 机器人 每一簇关于人体的点云经过点云投影和坐标 环境中的障碍物 机器人偏转角 变换到激光坐标系下后，利用其中的三维信息确定 机器人纵坐标 很 p.0+e,) 激光数据是由人体干扰引起的角度范围：对于每一 全局坐标系 传感器 簇对齐后的人体点云数据 机器人横坐标 {P:lp:=(:y:),i=1,2,…,n},计算每一个点云对 图2机器人在全局地图中的模型 应的角度6，即 Fig.2 Robot model in global map 0.arctan2(point[i].y,point[i].x)(5) (2) 并得到每一簇人体点云的统计信息，即角度6， i=1,2,…,n的最大和最小值0和0n。当使用 式中：p=(x,y,0)为机器人的位姿，r:为机器人在当 RGB-D传感器检测到场景中存在n个人时，用同样 前位姿下第i个激光束的长度，σ2为激光数据的噪 的方法确定n对0m.和0n,记做(0min,0mx）,k=1, 声方差。王炜o)根据概率栅格地图的特点对Fisher 2,…,n。对于一个特定范围本文用高斯模型确定 信息矩阵r(p)离散化，用△r/△p近似ars/ap,得 该范围内每一束激光上的未建模动态障碍物概率 到静态可定位性矩阵(P)。 并记为e,(i表示第i束激光)，如图3所示。 (p)仅考虑到已知静态概率栅格地图，未考虑 局部动态信息对机器人可定位性的影响。而在实 际环境中，未知障碍物会导致激光数据匹配度降低 甚至失效，无法反映其可定位性。因此在(p)的基 础上，考虑局部激光扫描数据，引入动态障碍物影 受人体干扰的 响因子s,以度量动态障碍物对激光扫描数据的 激光读数 影响。 如图2所示，机器人在参考位置P。= 受人体干扰的 激光读数 [xo,yo,]已知的前提下，设激光测距仪实际获取 π2 的第i个激光束的长度为，：，扫描到已知栅格为事 2 010 件A,扫描到未知障碍物为事件B,则根据贝叶斯公 图3未建模动态障碍物概率估计 式，该激光点扫描到已知栅格的概率为 Fig.3 Estimating the probability of unmodeled obstacles p(r:I A)p(A) p(AI r)=- (3) 则整个视场中激光束的未建模动态障碍物的 p(r:I A)p(A)+p(r:I B)p(B) 概率估计为E,可以用式(6)计算： 设第i个激光束的全局坐标位置(x:,y:)为 1 （9:-4)2 x:=x0+rC0s(9:+8。-T/2) -exp(- (4) 2TO 2u2）,9e(8m,6） yi=yo +r;sin(+0o-T/2) 0. 其他 则p(:A)=,04为原始地图中(，：)处的栅 (6) 格概率值，将1-p(A1r:)定义为未知障碍物影响因 式中：9：为第i束激光的角度，：和ok可用P 子s:,3:值越大该激光数据对机器人可定位性的提 Pan、9m、9amn唯一确定。若场景中出现提取出来的 高越小。 两簇点云有重叠的情形（图4）
Ｆｉｓｈｅｒ 信息矩阵提出的，Ｆｉｓｈｅｒ 信息提供了观测信息 中蕴含关于待估参数信息量大小的度量。 针对服 务机器人依靠激光数据和先验地图进行位姿估计， Ａ． Ｃｅｎｓｉ ［１３］依据图 ２ 的机器人模型将 Ｆｉｓｈｅｒ 信息矩 阵定义为式（２）所示的激光扫描到环境表面斜率的 函数。 图 ２ 机器人在全局地图中的模型 Ｆｉｇ．２ Ｒｏｂｏｔ ｍｏｄｅｌ ｉｎ ｇｌｏｂａｌ ｍａｐ τ（ｐ） ＝ ∑ ｎ ｉ １ σ ２ ∂ ｒ Ｔ ｉ ∂ｐ ∂ ｒｉ ∂ｐ é ë ê ê ù û ú ú （２） 式中：ｐ ＝ （ｘ，ｙ，θ）为机器人的位姿，ｒｉ 为机器人在当 前位姿下第 ｉ 个激光束的长度，σ ２ 为激光数据的噪 声方差。 王炜［６］根据概率栅格地图的特点对 Ｆｉｓｈｅｒ 信息矩阵 τ（ ｐ）离散化，用 ΔｒｉＥ ／ Δｐ 近似∂ｒｉＥ ／ ∂ｐ，得 到静态可定位性矩阵 τ ＾ （ｐ）。 τ ＾ （ｐ）仅考虑到已知静态概率栅格地图，未考虑 局部动态信息对机器人可定位性的影响。 而在实 际环境中，未知障碍物会导致激光数据匹配度降低 甚至失效，无法反映其可定位性。 因此在 τ ＾ （ｐ）的基 础上，考虑局部激光扫描数据，引入动态障碍物影 响因子 ｓｉ 以度量动态障碍物对激光扫描数据的 影响。 如 图 ２ 所 示， 机 器 人 在 参 考 位 置 Ｐ０ ＝ ｘ０ ，ｙ０ ，θ０ [ ] 已知的前提下，设激光测距仪实际获取 的第 ｉ 个激光束的长度为 ｒｉ，扫描到已知栅格为事 件 Ａ，扫描到未知障碍物为事件 Ｂ，则根据贝叶斯公 式，该激光点扫描到已知栅格的概率为 ｐ（Ａ ｜ ｒｉ） ＝ ｐ（ｒｉ ｜ Ａ）ｐ（Ａ） ｐ（ｒｉ ｜ Ａ）ｐ（Ａ） ＋ ｐ（ｒｉ ｜ Ｂ）ｐ（Ｂ） （３） 设第 ｉ 个激光束的全局坐标位置（ｘｉ，ｙｉ）为 ｘｉ ＝ ｘ０ ＋ ｒｉ ｃｏｓ（φｉ ＋ θ０ － π／ ２） ｙｉ ＝ ｙ０ ＋ ｒｉ ｓｉｎ（φｉ ＋ θ０ { － π／ ２） （４） 则 ｐ（ｒｉ ｜ Ａ）＝ μｘｉ ｙｉ 。 μｘｉ ｙｉ为原始地图中（ ｘｉ，ｙｉ）处的栅 格概率值，将 １－ｐ（ Ａ ｜ ｒｉ ）定义为未知障碍物影响因 子 ｓｉ，ｓｉ 值越大该激光数据对机器人可定位性的提 高越小。 ２．２ 利用 ＲＧＢ⁃Ｄ 感知的未建模动态障碍物概率 估计 由于激光传感器与 Ｋｉｎｅｃｔ 传感器的数据已通 过坐标转换进行了对齐，本文采用 Ｋｉｎｅｃｔ 传感器实 现视野中的快速人体检测。 在 ＲＧＢ⁃Ｄ 数据中使用 ＨＯＧ＋ＳＶＭ ［１４］方法实现移动机器人作业场景中人体 的检测并得到关于人体的点云数据。 每一簇关于人体的点云经过点云投影和坐标 变换到激光坐标系下后，利用其中的三维信息确定 激光数据是由人体干扰引起的角度范围：对于每一 簇 对 齐 后 的 人 体 点 云 数 据 ｐｉ ｜ ｐｉ ＝ （ｘｉ，ｙ { ｉ），ｉ ＝ １，２，…，ｎ} ，计算每一个点云对 应的角度 θｉ，即 θｉ ＝ ａｒｃｔａｎ２（ｐｏｉｎｔ［ｉ］．ｙ，ｐｏｉｎｔ［ｉ］．ｘ） （５） 并得到每一簇人体点云的统计信息，即角度 θｉ， ｉ ＝ １，２，…，ｎ 的最大和最小值 θｍａｘ 和 θｍｉｎ 。 当使用 ＲＧＢ⁃Ｄ 传感器检测到场景中存在 ｎ 个人时，用同样 的方法确定 ｎ 对 θｍａｘ和 θｍｉｎ ，记做（θｋｍｉｎ ，θｋｍａｘ），ｋ ＝ １， ２，…，ｎ。 对于一个特定范围本文用高斯模型确定 该范围内每一束激光上的未建模动态障碍物概率 并记为 εｉ（ｉ 表示第 ｉ 束激光），如图 ３ 所示。 图 ３ 未建模动态障碍物概率估计 Ｆｉｇ．３ Ｅｓｔｉｍａｔｉｎｇ ｔｈｅ ｐｒｏｂａｂｉｌｉｔｙ ｏｆ ｕｎｍｏｄｅｌｅｄ ｏｂｓｔａｃｌｅｓ 则整个视场中激光束的未建模动态障碍物的 概率估计为 εｉ，可以用式（６）计算： εｉ ＝ １ ２πσｋ ｅｘｐ（ － （φｉ － μｋ） ２ ２σｋ ２ ）， φｉ ∈ （θｋｍｉｎ ，θｋｍａｘ） ０， 其他 ì î í ï ï ï ï （６） 式中：φｉ 为第 ｉ 束激光的角度，μｋ 和 σｋ 可用 ｐｍａｘ、 ｐｍｉｎ 、θｋｍａｘ、θｋｍｉｎ唯一确定。 若场景中出现提取出来的 两簇点云有重叠的情形（图 ４）。 第 ４ 期 孙自飞，等：多传感器的移动机器人可定位性估计与自定位 ·４４５·
·446 智能系统学报 第12卷 prob(i) 里程计信息由于累积误差也不能单独用于机器人 定位。为此本文估计机器人的观测模型可定位性 p_max 矩阵以及文献[6]中详细介绍的预测模型协方差 c4,并使用其修正方法将两者作为权重指标，调节 粒子集依据观测模型对预测位姿的修正值：设t时 刻粒子滤波通过预测模型得到的机器人位姿为 p min Pb=P-1+f(△山，) (12) -0/rad 式中：P-为上一时刻定位结果，△山，为里程计在该 周期的输入量，∫为运动模型。在预测模型基础 图4点云数据有重叠情形 上，通过观测信息对粒子权重进行更新，得到新的 Fig.4 Cases where point clouds are overlapped 粒子集{Pp,心，}，Ppe为粒子的位姿，，为粒子权 重叠区域中的每一束微光在两个高斯模型中 重。新的表示机器人位姿的粒子集为 对应两个不同的概率值，分别为e和￡2 Pe4=∑w,(a,M)pie (13) E,=（1_)ep(-941 (7) 式中：：为激光测距仪的观测值，M为地图信息。粒 √2T01 2,2) B2=( ep(-(94) 子集根据观测信息对预测位姿的修正值为△P,= 1 (8) Pe一P,。t时刻的观测模型可定位性矩阵为 √2m02 2022 D4,当观测模型可定位性矩阵D的行列式较大 凸：和0：可以用PmPrin、、0n唯一确定 时，粒子集可以较精确反映机器人位姿概率分布， (k=1,2),则该激光束对应的未建模动态障碍物概 然而当观测模型可定位性较低时，粒子集权重分配 率估计为 干扰严重，定位可靠性降低。 i=0+Bs2,s.t.a +B=1 (9) 利用预测模型协方差和观测模型可定位性指 由于p(B)表示激光扫描到未建模动态障碍物 标动态调节基于观测信息对预测位姿的修正值： 的概率，与6：的物理含义相同，则式(3)可重写为 P.=Podo+(DPE +ki Colo.)DPE.APols op(A) p(AI r)= -(10) (14) p0+e,∑p,1n 式中：k,为比例系数，调节观测模型可定位性矩阵 与预测模型协方差的权重比例关系。由此可知，在 式中：P(A)=1-p(B),P(B)为环境中存在未知障碍 动态环境中，观测模型可定位性较低，机器人位姿 物的先验概率。本文中p(B)通过RGB-D传感器对 主要依赖里程计信息减小观测噪声对定位精度产 人体的实时检测来获取，进而解决动态障碍物影响 生的影响；动态环境中未知障碍物较少时，机器人 因子的在线计算问题，使s:更加可靠的反映环境中 观测模型可定位性较高，粒子集根据观测信息修正 未建模的动态障碍物。在得到动态障碍物影响因 预测位姿，消除里程计的累积误差，保证全局定位 子后，结合静态可定位性矩阵可得到动态可定位性 稳定性。 矩阵D(P)(式(11))，D(P)兼顾原始地图和局部 动态信息的影响，反映机器人在P处的可定位性。 4 实验结果 △r2 △r屈 427 本文实验所采用的硬件平台如图5所示。 △x2 △x△Y △x△0 D(P)= 1-s △rE △r 42 RGB-D传感器 △x△y 4y2 △y△0 △r正 △r Ar正 激光测距仪 △x△Y △y△8 △g2 (11) 车载PC 3 位姿修正与更新 经典粒子滤波定位算法依赖里程计数据进行 预测更新，依赖激光数据进行感知更新。当环境中 图5 Turtlebot移动平台 存在动态障碍物时，粒子集权重更新依据丢失：而 Fig.5 Turtlebot mobile platform
图 ４ 点云数据有重叠情形 Ｆｉｇ．４ Ｃａｓｅｓ ｗｈｅｒｅ ｐｏｉｎｔ ｃｌｏｕｄｓ ａｒｅ ｏｖｅｒｌａｐｐｅｄ 重叠区域中的每一束激光在两个高斯模型中 对应两个不同的概率值，分别为 εｉ１和 εｉ２ 。 εｉ１ ＝ （ １ ２πσ１ ）ｅｘｐ（ － （φｉ － μ１ ） ２ ２σ１ ２ ） （７） εｉ２ ＝ （ １ ２πσ２ ）ｅｘｐ（ － （φｉ － μ２ ） ２ ２σ２ ２ ） （８） μｋ 和 σｋ 可以用 ｐｍａｘ、ｐｍｉｎ 、θｋｍａｘ、θｋｍｉｎ 唯一确定 （ｋ ＝ １，２），则该激光束对应的未建模动态障碍物概 率估计为 εｉ ＝ αεｉ１ ＋ βεｉ２ ， ｓ．ｔ． α ＋ β ＝ １ （９） 由于 ｐ（Ｂ）表示激光扫描到未建模动态障碍物 的概率，与 εｉ 的物理含义相同，则式（３）可重写为 ｐ（Ａ ｜ ｒｉ） ＝ μｘｉ ｙｉ ｐ（Ａ） μｘｉ ｙｉ ｐ（Ａ） ＋ ΔｒＢ ｒｉＥ εｉ∑ｐ（ｒｉ ｜ ｒＢ ） （１０） 式中：ｐ（Ａ）＝ １－ｐ（Ｂ），ｐ（Ｂ）为环境中存在未知障碍 物的先验概率。 本文中 ｐ（Ｂ）通过 ＲＧＢ⁃Ｄ 传感器对 人体的实时检测来获取，进而解决动态障碍物影响 因子的在线计算问题，使 ｓｉ 更加可靠的反映环境中 未建模的动态障碍物。 在得到动态障碍物影响因 子后，结合静态可定位性矩阵可得到动态可定位性 矩阵 Ｄ（Ｐ）（式（１１）），Ｄ（Ｐ） 兼顾原始地图和局部 动态信息的影响，反映机器人在 Ｐ 处的可定位性。 Ｄ（Ｐ） ＝ ∑ ｎ ｉ １ － ｓｉ σ ２ Δｒ ２ ｉＥ Δｘ ２ Δｒ ２ ｉＥ ΔｘΔｙ Δｒ ２ ｉＥ ΔｘΔθ Δｒ ２ ｉＥ ΔｘΔｙ Δｒ ２ ｉＥ Δｙ ２ Δｒ ２ ｉＥ ΔｙΔθ Δｒ ２ ｉＥ ΔｘΔｙ Δｒ ２ ｉＥ ΔｙΔθ Δｒ ２ ｉＥ Δθ ２ é ë ê ê ê ê ê ê ê êê ù û ú ú ú ú ú ú ú úú （１１） ３ 位姿修正与更新 经典粒子滤波定位算法依赖里程计数据进行 预测更新，依赖激光数据进行感知更新。 当环境中 存在动态障碍物时，粒子集权重更新依据丢失；而 里程计信息由于累积误差也不能单独用于机器人 定位。 为此本文估计机器人的观测模型可定位性 矩阵以及文献［ ６］ 中详细介绍的预测模型协方差 ｃｏｄｏ，ｔ，并使用其修正方法将两者作为权重指标，调节 粒子集依据观测模型对预测位姿的修正值：设 ｔ 时 刻粒子滤波通过预测模型得到的机器人位姿为 Ｐｏｄｏ，ｔ ＝ Ｐｔ－１ ＋ ｆ ｏｄｏ（Δｕｔ） （１２） 式中：Ｐｔ－１为上一时刻定位结果，Δｕｔ 为里程计在该 周期的输入量，ｆ ｏｄｏ 为运动模型。 在预测模型基础 上，通过观测信息对粒子权重进行更新，得到新的 粒子集｛ ｐＰＦ，ｔ，ｗｔ ｝，ｐＰＦ，ｔ为粒子的位姿，ｗｔ 为粒子权 重。 新的表示机器人位姿的粒子集为 ＰＰＦ，ｔ ＝ ∑ｉ ｗｉ（ｚｉ，Ｍ）ｐ ｉ ＰＦ，ｔ （１３） 式中：ｚｉ 为激光测距仪的观测值，Ｍ 为地图信息。 粒 子集根据观测信息对预测位姿的修正值为 ΔＰｏｂｓ，ｔ ＝ ＰＰＦ，ｔ －Ｐｏｄｏ，ｔ。 ｔ 时刻的观测模型可定位性矩阵为 ＤＰＦ，ｔ，当观测模型可定位性矩阵 ＤＰＦ，ｔ的行列式较大 时，粒子集可以较精确反映机器人位姿概率分布， 然而当观测模型可定位性较低时，粒子集权重分配 干扰严重，定位可靠性降低。 利用预测模型协方差和观测模型可定位性指 标动态调节基于观测信息对预测位姿的修正值： Ｐｔ ＝ Ｐｏｄｏ，ｔ ＋ （ＤＰＦ，ｔ ＋ ｋ１Ｃ －１ ｏｄｏ，ｔ） －１ＤＰＦ，ｔΔＰｏｂｓ，ｔ （１４） 式中：ｋ１ 为比例系数，调节观测模型可定位性矩阵 与预测模型协方差的权重比例关系。 由此可知，在 动态环境中，观测模型可定位性较低，机器人位姿 主要依赖里程计信息减小观测噪声对定位精度产 生的影响；动态环境中未知障碍物较少时，机器人 观测模型可定位性较高，粒子集根据观测信息修正 预测位姿，消除里程计的累积误差，保证全局定位 稳定性。 ４ 实验结果 本文实验所采用的硬件平台如图 ５ 所示。 图 ５ Ｔｕｒｔｌｅｂｏｔ 移动平台 Ｆｉｇ．５ Ｔｕｒｔｌｅｂｏｔ ｍｏｂｉｌｅ ｐｌａｔｆｏｒｍ ·４４６· 智 能 系 统 学 报 第 １２ 卷
第4期 孙自飞，等：多传感器的移动机器人可定位性估计与自定位 ·447. 激光测距仪为Hokuyo的URG-04LX-UG01,其 向的定位均方差在3~5cm之间：改进的定位算法源 距离为0.02-5.60m。采用R0s1(Robot operating 于经典的粒子滤波算法，当环境整体的可定位性较 system)中间件对移动平台内置里程计数据、激光数 好时，其修正方法决定了改进的方法与经典的算法 功能等效：但是当移动机器人的定位周期在200~ 据、点云数据等进行封装，利用OpenNi中间件控制 300之间时，经典定位算法的两个方向的定位均方 Kinect传感器获取场景的三维视觉信息，并通过 差都出现了变大的趋势，而改进的方法没有出现这 PCL(point cloud library)【1-)实现人体检测，进而辅 种现象。这是因为当定位周期在200~300之间时， 助激光测距仪进行机器人自定位。采用gmapping 机器人运动到图6中的A区域，该区域环境结构杂 功能包1]创建实验环境的栅格地图（图6）。 乱而地图创建效果差，影响机器人的定位精度使其 人轨迹 定位均方差增大但是改进的定位方法中的可定位 性矩阵会对地图的创建效果进行评估，在这个区域 中的可定位性较小，主要依靠里程计进行定位，使 定位均方差不会出现较大的变动。 0.05 一经典方法 …改进方 0.04 图6环境模型和机器人行走轨迹 0.03 Fig.6 Environment model and robot trajectory 0.02 为得到对比实验结果，控制机器人从初始状态 0.01 运动到固定轨迹的起始点完成最初的位姿估计，位 姿均方差的收敛过程如图7所示，然后在结构化环 100150200250300350 境和动态环境中控制机器人沿着固定轨迹行走（图 迭代次数/次 6),并分别采用经典的蒙卡罗定位和本文提出的改 (a)x方向定位均方差对比 进方法得到位姿跟踪过程中的误差，最后以跟踪结 0.06 经典方法 果的均方差为标准对这4种定位结果进行比较。 0.05 改进方法 0.5 0.04 0.4 一x方向定位均方差 --一-·y方向定位均方差 0.03 0.3 0.02 0.2 0.01 0.1 050100150200250.300350 迭代次数 102030405060708090 选代次数 (b)y方向定位均方差对比 图7定位均方差收敛过程 图8x和y方向定位均方差对比 Fig.8 Comparison of localization error in x and y-axis Fig.7 Convergence process of localization error 4.1结构化环境中的对比实验 4.2动态环境中的对比实验 在有人干扰的动态环境中控制机器人沿着上 首先在结构化环境中（没有人体干扰的情况 述的固定路线行走，分别采用经典算法和本文提出 下)控制机器人沿着设定好的轨迹行走（如图6），使 的改进方法进行评估，真实场景如图9所示，图10 机器人尽可能遍历整个环境，分别使用经典定位算 为定位过程中两帧激光和点云数据，图9图10均包 法和本文提出的改进算法得出机器人在行走过程 含人体相互重叠和没有人体相互重叠两种情况。 中x方向和y方向的定位均方差，如图8所示。 对于检测出的人体点云有相互重叠的情形按照式 通过对图(8)的观察可知：两种定位方法在结 (9)所示计算动态障碍物影响因子并用于机器人 构化环境中整体定位效果相差不多，x方向和y方 定位
激光测距仪为 Ｈｏｋｕｙｏ 的 ＵＲＧ⁃０４ＬＸ⁃ＵＧ０１，其 距离为 ０．０２～５．６０ ｍ。 采用 ＲＯＳ ［１５］ （Ｒｏｂｏｔ ｏｐｅｒａｔｉｎｇ ｓｙｓｔｅｍ）中间件对移动平台内置里程计数据、激光数 据、点云数据等进行封装，利用 ＯｐｅｎＮｉ 中间件控制 Ｋｉｎｅｃｔ 传感器获取场景的三维视觉信息，并通过 ＰＣＬ（ｐｏｉｎｔ ｃｌｏｕｄ ｌｉｂｒａｒｙ） ［１６－１７］实现人体检测，进而辅 助激光测距仪进行机器人自定位。 采用 ｇｍａｐｐｉｎｇ 功能包［１８］创建实验环境的栅格地图（图 ６）。 图 ６ 环境模型和机器人行走轨迹 Ｆｉｇ．６ Ｅｎｖｉｒｏｎｍｅｎｔ ｍｏｄｅｌ ａｎｄ ｒｏｂｏｔ ｔｒａｊｅｃｔｏｒｙ 为得到对比实验结果，控制机器人从初始状态 运动到固定轨迹的起始点完成最初的位姿估计，位 姿均方差的收敛过程如图 ７ 所示，然后在结构化环 境和动态环境中控制机器人沿着固定轨迹行走（图 ６），并分别采用经典的蒙卡罗定位和本文提出的改 进方法得到位姿跟踪过程中的误差，最后以跟踪结 果的均方差为标准对这 ４ 种定位结果进行比较。 图 ７ 定位均方差收敛过程 Ｆｉｇ．７ Ｃｏｎｖｅｒｇｅｎｃｅ ｐｒｏｃｅｓｓ ｏｆ ｌｏｃａｌｉｚａｔｉｏｎ ｅｒｒｏｒ ４．１ 结构化环境中的对比实验 首先在结构化环境中（没有人体干扰的情况 下）控制机器人沿着设定好的轨迹行走（如图 ６），使 机器人尽可能遍历整个环境，分别使用经典定位算 法和本文提出的改进算法得出机器人在行走过程 中 ｘ 方向和 ｙ 方向的定位均方差，如图 ８ 所示。 通过对图（８）的观察可知：两种定位方法在结 构化环境中整体定位效果相差不多，ｘ 方向和 ｙ 方 向的定位均方差在３～５ ｃｍ 之间；改进的定位算法源 于经典的粒子滤波算法，当环境整体的可定位性较 好时，其修正方法决定了改进的方法与经典的算法 功能等效；但是当移动机器人的定位周期在 ２００ ～ ３００ 之间时，经典定位算法的两个方向的定位均方 差都出现了变大的趋势，而改进的方法没有出现这 种现象。 这是因为当定位周期在 ２００ ～ ３００ 之间时， 机器人运动到图 ６ 中的 Ａ 区域，该区域环境结构杂 乱而地图创建效果差，影响机器人的定位精度使其 定位均方差增大但是改进的定位方法中的可定位 性矩阵会对地图的创建效果进行评估，在这个区域 中的可定位性较小，主要依靠里程计进行定位，使 定位均方差不会出现较大的变动。 （ａ）ｘ 方向定位均方差对比 （ｂ）ｙ 方向定位均方差对比 图 ８ ｘ 和 ｙ 方向定位均方差对比 Ｆｉｇ．８ Ｃｏｍｐａｒｉｓｏｎ ｏｆ ｌｏｃａｌｉｚａｔｉｏｎ ｅｒｒｏｒ ｉｎ ｘ ａｎｄ ｙ⁃ａｘｉｓ ４．２ 动态环境中的对比实验 在有人干扰的动态环境中控制机器人沿着上 述的固定路线行走，分别采用经典算法和本文提出 的改进方法进行评估，真实场景如图 ９ 所示，图 １０ 为定位过程中两帧激光和点云数据，图 ９ 图 １０ 均包 含人体相互重叠和没有人体相互重叠两种情况。 对于检测出的人体点云有相互重叠的情形按照式 （９）所示计算动态障碍物影响因子并用于机器人 定位。 第 ４ 期 孙自飞，等：多传感器的移动机器人可定位性估计与自定位 ·４４７·
·448 智能系统学报 第12卷 0.12 经典方法 0.10 进方法 0.08 0.06 无人重叠的 情况 有人重叠的 0.04 0.02 图9移动机器人定位的动态场景 Fig.9 Robot localization in dynamic environment 50 100150.200250300350 迭代次数次 ·激光数据 ·人体点云数据 (b)y方向定位均方差对比 图11x和y方向定位均方差对比 激光读数 Fig.11 Comparison of localization error in x and y-axis 从图11中可以发现，当机器人在前100个运行 周期中经典的粒子滤波算法以典型的性能进行定 人体点云数据 位，两个方向上的定位均方差在4cm左右。而在 100~200个运行周期中经典算法定位均方差出现增 0 大的趋势：在机器人的定位场景中有人出现，干扰 x/m 了机器人的激光传感器读数，进而影响感知更新使 (a)无重叠情况下的激光数据和点云数据 其粒子更加发散。在第200~300个运行周期中，还 6 是由于地图创建效果的原因使粒子在原来的基础 ·激光数据 、·人体点云数据 上继续发散。由图11中的统计数据可以看到，在这 两个阶段，两个方向的定位均方差最大已经达到 10cm以上，而最小的也在4cm以上，定位性能严重 激光读数 下降。观察本文提出的改进算法的定位结果，发现 虽然部分均方差在5cm以上不及经典算法在静态 环境中的性能，但在相同环境中比经典方法更具鲁 人体点云数据 棒性：可定位性矩阵不仅对静态的结构化环境进行 2 0 x/m 评价，还对环境中的未知障碍物（人）进行检测和评 (b)有重叠情况下的激光数据和点云数据 估，当环境中未知障碍物增多而影响激光数据时， 图10含人体点云坐标转换后数据的激光扫描实例 通过粒子集的动态修正减少感知更新对粒子集的 Fig.10 Example of laser scan data containing point 影响，使机器人主要依靠预测更新进行定位，以保 clouds after coordinate transform 证改进定位算法在动态环境中的鲁棒性。 经典定位算法和本文改进算法的对比效果如 图11。 5结束语 0.12 经典方法 针对动态环境下移动机器人定位性能下降甚 0.10 改进方法 至失效的问题，提出了一种利用RGB-D传感器辅助 激光传感器的动态可定位性在线估计方法，并用于 0.08 E 移动机器人可靠自定位。本算法一方面通过RGB 0.06 D传感器对人体的检测并利用可定位性矩阵估计人 0.04 体对激光数据的影响：另一方面通过预测模型的协 0.02 方差矩阵估计里程计数据的可信度，进而融合这两 个矩阵对机器人位姿进行修正。在结构化环境和 50 100150200250 300350 动态环境中分别使用这两种方法进行测试评估，实 迭代次数/次 验结果表明了本文所提出的算法对提高动态环境 (a)x方向定位均方差对比 下移动机器人定位性能的有效性，同时结果也表明
图 ９ 移动机器人定位的动态场景 Ｆｉｇ．９ Ｒｏｂｏｔ ｌｏｃａｌｉｚａｔｉｏｎ ｉｎ ｄｙｎａｍｉｃ ｅｎｖｉｒｏｎｍｅｎｔ （ａ）无重叠情况下的激光数据和点云数据 （ｂ）有重叠情况下的激光数据和点云数据 图 １０ 含人体点云坐标转换后数据的激光扫描实例 Ｆｉｇ．１０ Ｅｘａｍｐｌｅ ｏｆ ｌａｓｅｒ ｓｃａｎ ｄａｔａ ｃｏｎｔａｉｎｉｎｇ ｐｏｉｎｔ ｃｌｏｕｄｓ ａｆｔｅｒ ｃｏｏｒｄｉｎａｔｅ ｔｒａｎｓｆｏｒｍ 经典定位算法和本文改进算法的对比效果如 图 １１。 （ａ）ｘ 方向定位均方差对比 （ｂ）ｙ 方向定位均方差对比 图 １１ ｘ 和 ｙ 方向定位均方差对比 Ｆｉｇ．１１ Ｃｏｍｐａｒｉｓｏｎ ｏｆ ｌｏｃａｌｉｚａｔｉｏｎ ｅｒｒｏｒ ｉｎ ｘ ａｎｄ ｙ⁃ａｘｉｓ 从图 １１ 中可以发现，当机器人在前 １００ 个运行 周期中经典的粒子滤波算法以典型的性能进行定 位，两个方向上的定位均方差在 ４ ｃｍ 左右。 而在 １００～２００ 个运行周期中经典算法定位均方差出现增 大的趋势：在机器人的定位场景中有人出现，干扰 了机器人的激光传感器读数，进而影响感知更新使 其粒子更加发散。 在第 ２００ ～ ３００ 个运行周期中，还 是由于地图创建效果的原因使粒子在原来的基础 上继续发散。 由图 １１ 中的统计数据可以看到，在这 两个阶段，两个方向的定位均方差最大已经达到 １０ ｃｍ以上，而最小的也在 ４ ｃｍ 以上，定位性能严重 下降。 观察本文提出的改进算法的定位结果，发现 虽然部分均方差在 ５ ｃｍ 以上不及经典算法在静态 环境中的性能，但在相同环境中比经典方法更具鲁 棒性：可定位性矩阵不仅对静态的结构化环境进行 评价，还对环境中的未知障碍物（人）进行检测和评 估，当环境中未知障碍物增多而影响激光数据时， 通过粒子集的动态修正减少感知更新对粒子集的 影响，使机器人主要依靠预测更新进行定位，以保 证改进定位算法在动态环境中的鲁棒性。 ５ 结束语 针对动态环境下移动机器人定位性能下降甚 至失效的问题，提出了一种利用 ＲＧＢ⁃Ｄ 传感器辅助 激光传感器的动态可定位性在线估计方法，并用于 移动机器人可靠自定位。 本算法一方面通过 ＲＧＢ⁃ Ｄ 传感器对人体的检测并利用可定位性矩阵估计人 体对激光数据的影响；另一方面通过预测模型的协 方差矩阵估计里程计数据的可信度，进而融合这两 个矩阵对机器人位姿进行修正。 在结构化环境和 动态环境中分别使用这两种方法进行测试评估，实 验结果表明了本文所提出的算法对提高动态环境 下移动机器人定位性能的有效性，同时结果也表明 ·４４８· 智 能 系 统 学 报 第 １２ 卷
第4期 孙自飞，等：多传感器的移动机器人可定位性估计与自定位 449· 改进的算法保证了定位系统在多人运动干扰情况 detector fusion [C]//2011 IEEE International Conference 下的鲁棒性。 on Computer Vision Workshops.Barcelona,Spain,2011: 1076-1083. 参考文献： [11]SPINELLO L,ARRAS K O.People detection in RGB-D data[C]//2011 IEEE/RSJ International Conference on [1]SAARINEN J,ANDREASSON H,STOYANOV T,et al. Intelligent Robots and Systems.San Francisco,USA, Normal distributions transform Monte-Carlo localization 2011:3838-3843. (NDT-MCL)[C]//2013 IEEE/RSJ Interational Conference [12]王勇，陈卫东，王景川，等.面向拥挤环境的移动机器 on Intelligent Robots and Systems.Tokyo,Japan,2013: 人改进粒子滤波定位[J].机器人，2012,34(5)： 382-389. 596-603. [2]VALENCIA R,SAARINEN J,ANDREASSON H,et al. WANG Yong,CHEN Weidong,WANG Jingchuan,et al. Localization in highly dynamic environments using dual- Improved particle filter localization in crowded timescale NDT-MCL[C]//2014 IEEE International environments for mobile Robots[J].Robot,2012,34(5): Conference on Robotics and Automation (ICRA).Hong Kong, 596-603. China,2014:3956-3962. [13]CENSI A.On achievable accuracy for range-finder localization [3]ROWEKAMPER J,SPRUNK C,TIPALDI G D,et al.On [C]//2007IEEE International conference on Robotics and the position accuracy of mobile robot localization based on Automation.Roma,Italy,2007:4170-4175. particle filters combined with scan matching [C]//2012 [14]DALAL N,TRIGGS B.Histograms of oriented gradients for IEEE/RSJ International Conference on Intelligent Robots human detection [C ]//2005 IEEE Computer Society and Systems.Vilamoura,Portugal,2012:3158-3164. Conference on Computer Vision and Pattern Recognition. [4]DELLAERT F,FOX D,BURGARD W,et al.Monte Carlo San Dieg0,USA,2005:886-893. localization for mobile robots[C]//Proceedings of the 1999 [15]R0S[EB/0L].[2016-05-14].http:/wiki.os.org. IEEE International Conference on Robotics and Automation. [16]RUSU R B,COUSINS S.3D is here:Point Cloud Library Detroit,USA,1999:1322-1328. (PCL)[C ]//2011 IEEE International Conference on [5]CENSI A.An ICP variant using a point-to-line metric[C]// Robotics and Automation (ICRA).Shanghai,China, 2008 IEEE International Conference on Robotics and 2011:1-4. Automation.Pasadena,USA,2008:19-25. [17]MUNARO M,et al.News articles EB/OL].[2016-04- [6]王炜.移动机器人的可定位估计及其应用[D].上海：上 26].http://www.pointclouds.org/news/. 海交通大学，2012：21-43. [18 ]GERKEY B.Gmapping[EB/OL].[2016-06-18].wiki. WANG Wei.Localizability estimation for mobile robot and ros.org/gmapping. its application[D].Shanghai:Shanghai JiaoTong University, 作者简介： 2012:21-43. 孙自飞，男，1990年生，硕士研究 [7]苗少帅，周峰.MM迭代无迹Kalman粒子滤波目标跟踪 生，主要研究方向为移动机器人定位。 算法[J].重庆邮电大学学报：自然科学版，2015,27 (01)：44-48 MIAO Shaoshuai,ZHOU Feng.IMM iterated unscented Kalman particle filter algorithm for target tracking[]]. Journal of Chongqing university of posts and telecommunications:natural science edition,2015,27 (01):44-48. 钱堃，男，1982年生，副教授，EEE [8]张闯，郭晨.二阶自适应权值粒子滤波的多传感器信 会员、中国自动化学会会员，主要研究 息融合[J].哈尔滨工程大学学报，2016,37(10)： 方向为服务机器人技术，主持国家自然 1347-1352 科学基金项目2项并参与多项省部级 ZHANG Chuang,GUO Chen.Multi-sensor information 科研项目，发表学术论文40余篇。 fusion of a particle filter on the basis of a two-stage adaptive weight[J].Journal of Harbin engineering university,2016, 37(10):1347-1352. 马旭东，男，1962年生，教授，主要 [9]段鹏.与人共融的家庭服务机器人定位与导航[D]济 南：山东大学，2015：39-58. 研究方向为网络化移动机器人、工业机 DUAN Peng.Localization and navigation of home service 器人与工业自动化，先后承担或参加国 robot in considering of human-robot harmony[D].Ji'nan, 家、省部级科研项目15项，横向合作课 China:Shandong University,2015:39-58. 题20项，发表学术论文80余篇。 [10]CHOI W,PANTOFARU C,SAVARESE S.Detecting and tracking people using an RGB-D camera via multiple
改进的算法保证了定位系统在多人运动干扰情况 下的鲁棒性。 参考文献： ［１］ ＳＡＡＲＩＮＥＮ Ｊ， ＡＮＤＲＥＡＳＳＯＮ Ｈ， ＳＴＯＹＡＮＯＶ Ｔ， ｅｔ ａｌ． Ｎｏｒｍａｌ ｄｉｓｔｒｉｂｕｔｉｏｎｓ ｔｒａｎｓｆｏｒｍ Ｍｏｎｔｅ⁃Ｃａｒｌｏ ｌｏｃａｌｉｚａｔｉｏｎ （ＮＤＴ⁃ＭＣＬ）［Ｃ］ ／ ／ ２０１３ ＩＥＥＥ／ ＲＳＪ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｉｎｔｅｌｌｉｇｅｎｔ Ｒｏｂｏｔｓ ａｎｄ Ｓｙｓｔｅｍｓ． Ｔｏｋｙｏ， Ｊａｐａｎ， ２０１３： ３８２－３８９． ［ ２ ］ ＶＡＬＥＮＣＩＡ Ｒ， ＳＡＡＲＩＮＥＮ Ｊ， ＡＮＤＲＥＡＳＳＯＮ Ｈ， ｅｔ ａｌ． Ｌｏｃａｌｉｚａｔｉｏｎ ｉｎ ｈｉｇｈｌｙ ｄｙｎａｍｉｃ ｅｎｖｉｒｏｎｍｅｎｔｓ ｕｓｉｎｇ ｄｕａｌ⁃ ｔｉｍｅｓｃａｌｅ ＮＤＴ⁃ＭＣＬ［Ｃ］ ／ ／ ２０１４ ＩＥＥＥ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ （ＩＣＲＡ）． Ｈｏｎｇ Ｋｏｎｇ， Ｃｈｉｎａ， ２０１４： ３９５６－３９６２． ［３］ＲÖＷＥＫÄＭＰＥＲ Ｊ， ＳＰＲＵＮＫ Ｃ， ＴＩＰＡＬＤＩ Ｇ Ｄ， ｅｔ ａｌ． Ｏｎ ｔｈｅ ｐｏｓｉｔｉｏｎ ａｃｃｕｒａｃｙ ｏｆ ｍｏｂｉｌｅ ｒｏｂｏｔ ｌｏｃａｌｉｚａｔｉｏｎ ｂａｓｅｄ ｏｎ ｐａｒｔｉｃｌｅ ｆｉｌｔｅｒｓ ｃｏｍｂｉｎｅｄ ｗｉｔｈ ｓｃａｎ ｍａｔｃｈｉｎｇ ［ Ｃ］ ／ ／ ２０１２ ＩＥＥＥ／ ＲＳＪ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｉｎｔｅｌｌｉｇｅｎｔ Ｒｏｂｏｔｓ ａｎｄ Ｓｙｓｔｅｍｓ． Ｖｉｌａｍｏｕｒａ， Ｐｏｒｔｕｇａｌ， ２０１２： ３１５８－３１６４． ［４］ＤＥＬＬＡＥＲＴ Ｆ， ＦＯＸ Ｄ， ＢＵＲＧＡＲＤ Ｗ， ｅｔ ａｌ． Ｍｏｎｔｅ Ｃａｒｌｏ ｌｏｃａｌｉｚａｔｉｏｎ ｆｏｒ ｍｏｂｉｌｅ ｒｏｂｏｔｓ［Ｃ］ ／ ／ Ｐｒｏｃｅｅｄｉｎｇｓ ｏｆ ｔｈｅ １９９９ ＩＥＥＥ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ． Ｄｅｔｒｏｉｔ， ＵＳＡ， １９９９： １３２２－１３２８． ［５］ＣＥＮＳＩ Ａ． Ａｎ ＩＣＰ ｖａｒｉａｎｔ ｕｓｉｎｇ ａ ｐｏｉｎｔ⁃ｔｏ⁃ｌｉｎｅ ｍｅｔｒｉｃ［Ｃ］ ／ ／ ２００８ ＩＥＥＥ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ． Ｐａｓａｄｅｎａ， ＵＳＡ， ２００８： １９－２５． ［６］王炜． 移动机器人的可定位估计及其应用［Ｄ］．上海：上 海交通大学， ２０１２： ２１－４３． ＷＡＮＧ Ｗｅｉ． Ｌｏｃａｌｉｚａｂｉｌｉｔｙ ｅｓｔｉｍａｔｉｏｎ ｆｏｒ ｍｏｂｉｌｅ ｒｏｂｏｔ ａｎｄ ｉｔｓ ａｐｐｌｉｃａｔｉｏｎ［Ｄ］． Ｓｈａｎｇｈａｉ：Ｓｈａｎｇｈａｉ ＪｉａｏＴｏｎｇ Ｕｎｉｖｅｒｓｉｔｙ， ２０１２： ２１－４３． ［７］苗少帅，周峰． ＩＭＭ 迭代无迹 Ｋａｌｍａｎ 粒子滤波目标跟踪 算法［Ｊ］． 重庆邮电大学学报：自然科学版， ２０１５， ２７ （０１）： ４４－４８． ＭＩＡＯ Ｓｈａｏｓｈｕａｉ， ＺＨＯＵ Ｆｅｎｇ． ＩＭＭ ｉｔｅｒａｔｅｄ ｕｎｓｃｅｎｔｅｄ Ｋａｌｍａｎ ｐａｒｔｉｃｌｅ ｆｉｌｔｅｒ ａｌｇｏｒｉｔｈｍ ｆｏｒ ｔａｒｇｅｔ ｔｒａｃｋｉｎｇ［Ｊ］． Ｊｏｕｒｎａｌ ｏｆ Ｃｈｏｎｇｑｉｎｇ ｕｎｉｖｅｒｓｉｔｙ ｏｆ ｐｏｓｔｓ ａｎｄ ｔｅｌｅｃｏｍｍｕｎｉｃａｔｉｏｎｓ： ｎａｔｕｒａｌ ｓｃｉｅｎｃｅ ｅｄｉｔｉｏｎ， ２０１５， ２７ （０１）： ４４－４８． ［８］张闯，郭晨． 二阶自适应权值粒子滤波的多传感器信 息融合 ［ Ｊ］． 哈尔 滨 工 程 大 学 学 报， ２０１６， ３７ （ １０）： １３４７－１３５２． ＺＨＡＮＧ Ｃｈｕａｎｇ， ＧＵＯ Ｃｈｅｎ． Ｍｕｌｔｉ⁃ｓｅｎｓｏｒ ｉｎｆｏｒｍａｔｉｏｎ ｆｕｓｉｏｎ ｏｆ ａ ｐａｒｔｉｃｌｅ ｆｉｌｔｅｒ ｏｎ ｔｈｅ ｂａｓｉｓ ｏｆ ａ ｔｗｏ⁃ｓｔａｇｅ ａｄａｐｔｉｖｅ ｗｅｉｇｈｔ［Ｊ］． Ｊｏｕｒｎａｌ ｏｆ Ｈａｒｂｉｎ ｅｎｇｉｎｅｅｒｉｎｇ ｕｎｉｖｅｒｓｉｔｙ， ２０１６， ３７（１０）： １３４７－１３５２． ［９］段鹏． 与人共融的家庭服务机器人定位与导航［Ｄ］．济 南： 山东大学， ２０１５： ３９－５８． ＤＵＡＮ Ｐｅｎｇ． Ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ ｎａｖｉｇａｔｉｏｎ ｏｆ ｈｏｍｅ ｓｅｒｖｉｃｅ ｒｏｂｏｔ ｉｎ ｃｏｎｓｉｄｅｒｉｎｇ ｏｆ ｈｕｍａｎ⁃ｒｏｂｏｔ ｈａｒｍｏｎｙ［Ｄ］． Ｊｉ′ｎａｎ， Ｃｈｉｎａ： Ｓｈａｎｄｏｎｇ Ｕｎｉｖｅｒｓｉｔｙ， ２０１５： ３９－５８． ［１０］ＣＨＯＩ Ｗ， ＰＡＮＴＯＦＡＲＵ Ｃ， ＳＡＶＡＲＥＳＥ Ｓ． Ｄｅｔｅｃｔｉｎｇ ａｎｄ ｔｒａｃｋｉｎｇ ｐｅｏｐｌｅ ｕｓｉｎｇ ａｎ ＲＧＢ⁃Ｄ ｃａｍｅｒａ ｖｉａ ｍｕｌｔｉｐｌｅ ｄｅｔｅｃｔｏｒ ｆｕｓｉｏｎ［Ｃ］ ／ ／ ２０１１ ＩＥＥＥ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｃｏｍｐｕｔｅｒ Ｖｉｓｉｏｎ Ｗｏｒｋｓｈｏｐｓ． Ｂａｒｃｅｌｏｎａ， Ｓｐａｉｎ， ２０１１： １０７６－１０８３． ［１１］ ＳＰＩＮＥＬＬＯ Ｌ， ＡＲＲＡＳ Ｋ Ｏ． Ｐｅｏｐｌｅ ｄｅｔｅｃｔｉｏｎ ｉｎ ＲＧＢ⁃Ｄ ｄａｔａ ［ Ｃ］ ／ ／ ２０１１ ＩＥＥＥ／ ＲＳＪ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｉｎｔｅｌｌｉｇｅｎｔ Ｒｏｂｏｔｓ ａｎｄ Ｓｙｓｔｅｍｓ． Ｓａｎ Ｆｒａｎｃｉｓｃｏ， ＵＳＡ， ２０１１： ３８３８－３８４３． ［１２］王勇， 陈卫东， 王景川，等． 面向拥挤环境的移动机器 人改进粒子滤波定位 ［ Ｊ］． 机器人， ２０１２， ３４ （ ５）： ５９６－６０３． ＷＡＮＧ Ｙｏｎｇ， ＣＨＥＮ Ｗｅｉｄｏｎｇ， ＷＡＮＧ Ｊｉｎｇｃｈｕａｎ， ｅｔ ａｌ． Ｉｍｐｒｏｖｅｄ ｐａｒｔｉｃｌｅ ｆｉｌｔｅｒ ｌｏｃａｌｉｚａｔｉｏｎ ｉｎ ｃｒｏｗｄｅｄ ｅｎｖｉｒｏｎｍｅｎｔｓ ｆｏｒ ｍｏｂｉｌｅ Ｒｏｂｏｔｓ［Ｊ］． Ｒｏｂｏｔ， ２０１２， ３４（５）： ５９６－６０３． ［１３］ＣＥＮＳＩ Ａ． Ｏｎ ａｃｈｉｅｖａｂｌｅ ａｃｃｕｒａｃｙ ｆｏｒ ｒａｎｇｅ⁃ｆｉｎｄｅｒ ｌｏｃａｌｉｚａｔｉｏｎ ［Ｃ］ ／ ／ ２００７ＩＥＥＥ Ｉｎｔｅｒｎａｔｉｏｎａｌ ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ． Ｒｏｍａ， Ｉｔａｌｙ， ２００７： ４１７０－４１７５． ［１４］ＤＡＬＡＬ Ｎ， ＴＲＩＧＧＳ Ｂ． Ｈｉｓｔｏｇｒａｍｓ ｏｆ ｏｒｉｅｎｔｅｄ ｇｒａｄｉｅｎｔｓ ｆｏｒ ｈｕｍａｎ ｄｅｔｅｃｔｉｏｎ ［ Ｃ ］ ／ ／ ２００５ ＩＥＥＥ Ｃｏｍｐｕｔｅｒ Ｓｏｃｉｅｔｙ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｃｏｍｐｕｔｅｒ Ｖｉｓｉｏｎ ａｎｄ Ｐａｔｔｅｒｎ Ｒｅｃｏｇｎｉｔｉｏｎ． Ｓａｎ Ｄｉｅｇｏ， ＵＳＡ， ２００５： ８８６－８９３． ［１５］ＲＯＳ［ＥＢ／ ＯＬ］． ［２０１６－０５－１４］．ｈｔｔｐ： ／ ／ ｗｉｋｉ．ｒｏｓ．ｏｒｇ ／ ． ［１６］ＲＵＳＵ Ｒ Ｂ， ＣＯＵＳＩＮＳ Ｓ． ３Ｄ ｉｓ ｈｅｒｅ： Ｐｏｉｎｔ Ｃｌｏｕｄ Ｌｉｂｒａｒｙ （ ＰＣＬ ） ［ Ｃ ］ ／ ／ ２０１１ ＩＥＥＥ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ （ ＩＣＲＡ ）． Ｓｈａｎｇｈａｉ， Ｃｈｉｎａ， ２０１１： １－４． ［１７］ＭＵＮＡＲＯ Ｍ， ｅｔ ａｌ． Ｎｅｗｓ ａｒｔｉｃｌｅｓ［ＥＢ／ ＯＬ］． ［２０１６－０４－ ２６］． ｈｔｔｐ： ／ ／ ｗｗｗ．ｐｏｉｎｔｃｌｏｕｄｓ．ｏｒｇ ／ ｎｅｗｓ／ ． ［１８］ＧＥＲＫＥＹ Ｂ． Ｇｍａｐｐｉｎｇ［ＥＢ／ ＯＬ］． ［２０１６－０６－１８］． ｗｉｋｉ． ｒｏｓ．ｏｒｇ ／ ｇｍａｐｐｉｎｇ． 作者简介： 孙自飞，男，１９９０ 年生，硕士研究 生，主要研究方向为移动机器人定位。 钱堃，男，１９８２ 年生，副教授，ＩＥＥＥ 会员、中国自动化学会会员，主要研究 方向为服务机器人技术，主持国家自然 科学基金项目 ２ 项并参与多项省部级 科研项目，发表学术论文 ４０ 余篇。 马旭东，男，１９６２ 年生，教授，主要 研究方向为网络化移动机器人、工业机 器人与工业自动化，先后承担或参加国 家、省部级科研项目 １５ 项，横向合作课 题 ２０ 项，发表学术论文 ８０ 余篇。 第 ４ 期 孙自飞，等：多传感器的移动机器人可定位性估计与自定位 ·４４９·