第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方 定位。激光测距仪为 Ｈｏｋｕｙｏ 的 ＵＲＧ⁃０４ＬＸ⁃ＵＧ０１，其 距离为 ０．０２～５．６０ ｍ。 采用 ＲＯＳ ［１５］ （Ｒｏｂｏｔ ｏｐｅｒａｔｉｎｇ ｓｙｓｔｅｍ）中间件对移动平台内置里程计数据、激光数 据、点云数据等进行封装，利用 ＯｐｅｎＮｉ 中间件控制 Ｋｉｎｅｃｔ 传感器获取场景的三维视觉信息，并通过 ＰＣＬ（ｐｏｉｎｔ ｃｌｏｕｄ ｌｉｂｒａｒｙ） ［１６－１７］实现人体检测，进而辅 助激光测距仪进行机器人自定位。 采用 ｇｍａｐｐｉｎｇ 功能包［１８］创建实验环境的栅格地图（图 ６）。 图 ６ 环境模型和机器人行走轨迹 Ｆｉｇ．６ Ｅｎｖｉｒｏｎｍｅｎｔ ｍｏｄｅｌ ａｎｄ ｒｏｂｏｔ ｔｒａｊｅｃｔｏｒｙ 为得到对比实验结果，控制机器人从初始状态 运动到固定轨迹的起始点完成最初的位姿估计，位 姿均方差的收敛过程如图 ７ 所示，然后在结构化环 境和动态环境中控制机器人沿着固定轨迹行走（图 ６），并分别采用经典的蒙卡罗定位和本文提出的改 进方法得到位姿跟踪过程中的误差，最后以跟踪结 果的均方差为标准对这 ４ 种定位结果进行比较。 图 ７ 定位均方差收敛过程 Ｆｉｇ．７ Ｃｏｎｖｅｒｇｅｎｃｅ ｐｒｏｃｅｓｓ ｏｆ ｌｏｃａｌｉｚａｔｉｏｎ ｅｒｒｏｒ ４．１ 结构化环境中的对比实验 首先在结构化环境中（没有人体干扰的情况 下）控制机器人沿着设定好的轨迹行走（如图 ６），使 机器人尽可能遍历整个环境，分别使用经典定位算 法和本文提出的改进算法得出机器人在行走过程 中 ｘ 方向和 ｙ 方向的定位均方差，如图 ８ 所示。 通过对图（８）的观察可知：两种定位方法在结 构化环境中整体定位效果相差不多，ｘ 方向和 ｙ 方 向的定位均方差在３～５ ｃｍ 之间；改进的定位算法源 于经典的粒子滤波算法，当环境整体的可定位性较 好时，其修正方法决定了改进的方法与经典的算法 功能等效；但是当移动机器人的定位周期在 ２００ ～ ３００ 之间时，经典定位算法的两个方向的定位均方 差都出现了变大的趋势，而改进的方法没有出现这 种现象。 这是因为当定位周期在 ２００ ～ ３００ 之间时， 机器人运动到图 ６ 中的 Ａ 区域，该区域环境结构杂 乱而地图创建效果差，影响机器人的定位精度使其 定位均方差增大但是改进的定位方法中的可定位 性矩阵会对地图的创建效果进行评估，在这个区域 中的可定位性较小，主要依靠里程计进行定位，使 定位均方差不会出现较大的变动。 （ａ）ｘ 方向定位均方差对比 （ｂ）ｙ 方向定位均方差对比 图 ８ ｘ 和 ｙ 方向定位均方差对比 Ｆｉｇ．８ Ｃｏｍｐａｒｉｓｏｎ ｏｆ ｌｏｃａｌｉｚａｔｉｏｎ ｅｒｒｏｒ ｉｎ ｘ ａｎｄ ｙ⁃ａｘｉｓ ４．２ 动态环境中的对比实验 在有人干扰的动态环境中控制机器人沿着上 述的固定路线行走，分别采用经典算法和本文提出 的改进方法进行评估，真实场景如图 ９ 所示，图 １０ 为定位过程中两帧激光和点云数据，图 ９ 图 １０ 均包 含人体相互重叠和没有人体相互重叠两种情况。 对于检测出的人体点云有相互重叠的情形按照式 （９）所示计算动态障碍物影响因子并用于机器人 定位。 第 ４ 期 孙自飞，等：多传感器的移动机器人可定位性估计与自定位 ·４４７·