一种改进的AMCL机器人定位方法

王 宁，王 坚，李丽华

一种改进的AMCL机器人定位方法

王 宁1，王 坚1，李丽华2

（1. 北京建筑大学 测绘与城市空间信息学院，北京 102616；2. 中国地质大学 土地科学技术学院，北京 100083）

针对移动机器人技术中的蒙特卡罗定位算法存在计算量大、实时处理能力差、粒子退化等问题，提出一种基于AMCL算法的自适应定位模型：改进ROS中的move_base节点的路径规划功能，并加载学院楼CAD地图实现路径规划；基于ROS中slam_gmapping节点在未知环境下创建OGM地图。仿真实验结果表明：AMCL定位模型能够减小定位误差，提高路径规划的准确性和实时性；采样部分利用重采样与KLD采样交替进行的方法，根据粒子在状态空间的分布情况，能够实时在线调整粒子数，有效减少算法计算量。

自适应蒙特卡罗定位模型；机器人操作系统；静态地图；移动机器人；路径规划

0 引言

移动机器人在各种复杂环境下的自主操作中，自主定位是其最基本的任务之一。移动机器人的自主定位是基于上一时刻的姿态估计，利用已有的环境地图信息和传感器检测感受到的被测量信息，再经过信息的传输、处理、显示、存储、控制等，最后利用当前时刻的姿态估计来对移动机器人在环境地图中进行定位[1-3]。

自适应蒙特卡罗定位（adaptive Monte Carlo localization,AMCL）算法是一种基于粒子滤波的适用于2维环境下移动机器人的鲁棒的概率统计定位方法。它所有可能的姿态假设及其概率分布通过使用一系列加权粒子来表示。机器人的初始位姿及其路径规划过程中的位姿通过采样一定数量的粒子来跟踪。采样的方法是重采样（在首次采样之后用于确定路径规划过程中机器人新位姿时对当前粒子集进行采样）和Kullback-Leibler距离即K-L距离（Kullback-Leibler distance, KLD）采样交替进行。机器人的运动模型、传感器模型和实时数据通过迭代算法不断连续更新粒子及其权重，直至趋于真实概率分布。AMCL可以有效地处理非线性和非高斯系统。因此，它广泛地应用在移动机器人导航定位等领域[4-10]。

本文研究和改进AMCL定位算法：采用自适应定位模型，定位原理依赖重采样和KLD采样交替进行的原则；并在机器人操作系统（robot operating system, ROS）上加载学院楼计算机辅助设计（computer aided design, CAD）地图，通过全局规划和局部规划对移动机器人定位中的重要状态进行跟踪；然后在不同的条件下调整和控制AMCL参数和重采样规则，利用slam_gmapping节点在未知环境下创建覆盖栅格地图（occupancy grid map, OGM）。

2 AMCL自适应定位

2.1 MCL算法

式中表示样本估计概率。

产生新的粒子集为

并且使其对所有粒子都满足[1-2]

自适应样本大小对基于线性传感器的模型是极其有益的，因为应用这些模型将会需要大量的几何计算。

2.2 KLD采样

根据（8）式，KLD定义为

式中：表示K-L距离。

在重采样中，复制权重较大的粒子，权重较小的粒子予以忽略。利用KLD采样确定下一次重采样在粒子复制过程中所需的粒子数，实现了粒子数的在线调整，减少了算法计算量。

2.3 重采样和KLD采样交替进行的AMCL

基于重采样和KLD采样交替进行的AMCL算法流程[1,9,14]为：

2）将当前粒子集中粒子的权重作归一化处理。

3）对更新处理后的粒子集进行重采样和KLD交替采样，利用式（14）对重采样的粒子数进行更新。

式中E[]为数学期望。

该算法的滤波效率与传统的自适应滤波算法相比得到了提升，它的滤波额外计算复杂度得到了降低。这种改进在粒子数目庞大的情况下，极大地提高了系统的性能。

3 ROS框架下的模型验证

ROS是基于机器人的开放源码的元操作系统。它可以提供许多类似于传统操作系统的功能，如底层设备控制、硬件抽象、进程间消息传递和功能包管理、公共功能实现等[9]。此外，ROS采用分布式网络系统，不同节点之间的通信利用主题、服务或参数服务器来实现。比如：在移动机器人导航定位中，tf节点控制坐标转换；激光雷达和里程计采集环境数据，并通过laser scan节点接收传输数据；AMCL算法接收传感器数据并对其进行处理，将处理结果通过amcl节点显示出来。某个节点订阅其他节点发布的不同主题，自身发布的主题也被其他节点所订阅；消息是主题的载体，彼此的沟通通过消息来完成。这种系统结构确保实时更新定位数据和同步传输。节点是ROS计算图级中主要的计算执行进程。它们之间相互独立，将代码和功能解耦，提高了系统容错率和可维护性。ROS主要在Linux系统下开发，在Ubuntu下支持最为完全；整合多个主流函数库，拥有多传感器驱动程序[15-16]。

3.1 仿真研究

机器人3D模型或部分结构模型主要用于仿真机器人或者简化开发者的日常工作，在ROS中模型文件有URDF文件和XACRO文件。仿真环境由rviz或gazebo节点控制。

为了在rviz节点中查看仿真机器人3维模型，需要编写启动文件（display.launch），ROS中的启动文件可以同时启动节点管理器（roscore）和多个节点，启动命令为roslaunch package-name launch- file-name。rviz仿真结果如图1所示。

Gazebo是ROS系统下模拟室内室外复杂3维环境，并可以在3维场景中添加多个机器人、不同传感器和常见物体来模拟真实世界对物体的物理响应和实际反馈的仿真环境。在Gazebo中进行机器人仿真，就需要添加物理属性和碰撞属性，否则Gazebo无法使用这些模型。本文使用柳树车库（Willow Garage）办公室模型地图。Gazebo允许使用左边菜单插入和放置物体，本文添加了书架、桌子、罐子，来监测传感器对它们如何反应。

图1 rviz仿真结果

最后创建一个新的启动文件（gazebo.launch）来同时加载地图和机器人。Gazebo仿真结果如图2所示。

图2 Gazebo仿真结果

3.2 基于AMCL算法的移动机器人定位

自适应蒙特卡罗定位[9-10]是一种用于2维环境下移动机器人概率统计定位方法。这种算法在ROS框架下的具体实现主要是通过在已知地图上使用粒子滤波算法跟踪机器人的位姿。

自适应蒙特卡罗定位节点主要使用激光扫描和激光雷达地图。本文以北京建筑大学测绘与城市空间信息学院二层地图为例进行模拟。图3所示是加载学院楼CAD地图的情况。以此为基础，模拟激光扫描和激光雷达数据，采用本文算法，通过传递消息完成位姿估计的计算。

图3 学院楼地图

首先通过tf坐标变换（如图4所示）节点来配置机器人。利用tf坐标变换功能包对机器人进行坐标变换，并定义了不同坐标系之间的偏移量。从图上可以看出，本文的移动机器人使用base_link基座坐标系和hokuyo_link激光传感器2个坐标系。为了实现机器人的避障功能，通过订阅主题把激光传感器的数据发送给机器人。之后利用基座坐标系来控制机器人的移动方向和旋转姿态，因此有必要构造base_link和hokuyo_link之间的变换矩阵。为此先定义base_link基本连接坐标系，在visual字段中定义它的几何形状（本文定义为立方体）、材质（颜色为白色，无纹理）和原点；然后创建广播器（tf_broadcaster），主要用来发布base_link和hokuyo_link的转换四元数和激光传感器相对于base_link坐标系原点的位置；接着创建侦听器（tf_listener），主要用来监听激光传感器采集到的移动机器人上各点的坐标偏移量或者关节之间的相对位置，同时将偏移量或者位置数据发布到主题robot_tf_listener上。激光传感器采集到的环境数据和机器人偏移数据如图5所示。广播器通过订阅这个主题来获取机器人的偏移数据，最后经过四元转换把机器人偏移数据发布到主题robot_tf_ broadcaster上。

图4 tf坐标变换

图5 传感器数据

然后针对ROS框架提供的各个初始化参数，完成自适应蒙特卡罗定位算法粒子滤波器的初始化。如果没有设定初始化位姿，自适应蒙特卡罗定位算法会假定机器人从坐标系原点运行，如此会增加计算的复杂度。文中利用rviz界面中2D Pose Estimate按钮来设定初始化位姿。机器人位姿利用采样所得的粒子分布来跟踪，采样结果如图6所示。

最后ROS通过nav_msgs/Odometry消息发布关于2个坐标系之间的位移变换信息。该消息提供机器人的位姿信息和速度信息。ROS通过sensor_msgs/LaserScan或sensor_msgs/PointCloud2消息发布传感器数据。该消息中的stamp域存储时间信息，领域存储激光扫描数据来实现机器人的动态避障。

图6 采样结果

根据上述步骤逐一编写代码，最后在ROS系统下利用重采样和KLD采样交替进行的AMCL算法改进move_base功能包，基于离散粒子的权值来对机器人的位姿PoseArray进行估计。AMCL定位算法的结果如图7所示。进而在图3学院楼静态地图下实现局部规划（如图8(a)所示）与全局规划（如图8(b)所示）。slam_gmapping功能包可以在未知环境地图下由激光扫描器节点（/robot/laser/scan）发布的数据创建地图。图8(c)为激光扫描器创建的OGM地图。

图7 AMCL算法定位结果

在Linux终端输入rosrun rqt_graph rqt_graph可以调出ROS系统在线检测工具。图9是move_base功能包运行时节点之间的相互调用情况。

ROS框架下的模型验证实验中不同节点发布不同的主题，并订阅其他节点所发布的主题，通过调用主题之上的载体消息来完成彼此之间的沟通，保证移动机器人定位数据的同步更新和其路径规划数据的实时关联。同时，在ROS系统下进行移动机器人的程序开发会提高效率，其分布式的管理结构使得代码和功能包可以方便地移植应用到其他机器人系统开发中。

图9 ROS系统在线检测节点状态

4 结束语

本文在ROS系统下研究了移动机器人在路径规划中的问题，分析了AMCL定位的数学模型。通过分析MCL算法与传统KLD自适应粒子滤波算法中的优缺点，确定采用重采样和KLD采样交替进行的AMCL算法。该算法改进了ROS中的move_base节点的路径规划功能；在路径规划方面，着重研究了基于激光雷达的传感器数据关联问题；通过该定位模型实现了移动机器人的局部规划与全局规划，并利用slam_gmapping节点在未知环境下创建OGM地图。但本文没有考虑3维环境下移动机器人如何定位，只是利用该定位模型验证分析了2维环境中移动机器人的定位情况。因此对于该算法模型的优化提升以及在3维环境下移动机器人的定位问题等方面是今后仍需研究的方向。

[1] 罗元, 庞冬雪, 张毅, 等. 基于自适应多提议分布粒子滤波的蒙特卡洛定位算法[J]. 计算机应用, 2016, 36(8): 2352-2356.

[2] HANTEN R, BUCK S, OTTE S, et al. Vector-AMCL: vector based adaptive Monte Carlo localization for indoor maps[EB/OL]. [2018-08-08]. https: //www. researchgate. net/publication/305999066_Vector-AMCL_Vector_ based_Adaptive_Monte_Carlo_Localization_for_Indoor_Maps.

[3] THRUN S, FOX D, BURGARD W, et al. Robust Monte Carlo localization for mobile robots[J]. Artificial Intelligence, 2001, 128(1/2): 99-141.

[4] DELLAERT F,FOX D,BURGARD W,et al.Monte Carlo localization for mobile robots[EB/OL.[2018-08-27].https://www.cc.gatech.edu/~dellaert/pub/Dellaert99icra.pdf.

[5] RÖFER T,JÜNGEL M.Vision-based fast and reactive Monte-Carlo localization[EB/OL.[2018-08-27].http://www.cs.utexas.edu/~pstone/Courses/393Rfall13/readings/german_localize_icra03.pdf.

[6] THOMPSON S, KAGAMI S.Humanoid robot localisation using stereo vision[C]//The Institute of Electrical and Electronic Engineers(IEEE).Proceedings of the 5th IEEE-RAS International Conference on Humanoid Robots.Tsukuba,Japan：IEEE, 2005:19 - 25.DOI: 10.1109/ICHR.2005.1573539.

[7] 张霄汉, 陈小平, 李嘉玲, 等. 一种基于视觉的步行机器人Monte Carlo自定位系统[J]. 机器人, 2006, 28(4): 415-421.

[8] 洪伟, 周长久, 田彦涛. 一种针对人形足球机器人的分域自适应蒙特卡洛定位方法[J]. 机器人, 2012, 34(6): 652-659.

[9] MARTINEZ A, FERNANDEZ E, MAHTANI A, et al. ROS机器人程序设计[M]. 刘锦涛, 张瑞雷, 译. 北京: 机械工业出版社, 2014.

[10] MACENSKI S. Wiki: amcl[EB/OL]. (2018-09-24)[2018-08-08]. http: //www. ros. org/wiki/amcl.

[11] THRUN S, BURGARD W, FOX D. Probabilistic robotics (intelligent robotics and autonomous agents)[M]. Cambridge: The MIT Press, 2005.

[12] FOX D, BURGARD W, DELLAERT F, et al. Monte Carlo localization: efficient position estimation for mobile robots[EB/OL]. [2018-08-08]. http: //www. aaai. org/Papers/AAAI/1999/AAAI99-050. pdf.

[13] FOX D. Adapting the sample size in particle filters through KLD-sampling[J]. International Journal of Robotics Research, 2016, 22(12): 985-1003.

[14] 殷波. 正弦信号频率估计算法研究[D]. 哈尔滨: 哈尔滨工程大学, 2015.

[15] 方洵. 基于ROS的移动机器人研究与设计[J]. 科技资讯, 2017(36): 76-80.

[16] 陈卓, 苏卫华, 安慰宁, 等. 移动机器人SLAM与路径规划在ROS框架下的实现[J]. 医疗卫生装备, 2017, 38(2): 109-113.

An improved adaptive Monte Carlo localization method for robot

WANG Ning1, WANG Jian1, LI Lihua2

（1. School of Geomatics and Urban Spatial Informatics, Beijing University of Civil Engineering and Architecture, Beijing 102616, China;2. School of Land Science and Technology, China University of Geosciences, Beijing 100083, China）

Aiming at the problems of large computation, poor real-time processing ability and particle degradation in Monte Carlo location algorithm, the paper proposed an adaptive positioning model based on AMCL algorithm: the path planning function of move_base node in ROS was improved, and the CAD map of the college building was loaded to realize the path planning; an OGM map was created in an unknown environment based on slam_gmapping node in ROS. Simulational result showed that AMCL positioning model could reduce the positioning errors and improve the accuracy and real time of path planning; and the alternate sampling between resampling and KLD sampling could adjust the number of particles online in real time so that the calculation of the algorithm could be reduced effectively according to the distribution of the particles in the state space.

adaptive Monte Carlo localization model; robot operating system; static map; mobile robot; path planning

P228

A

2095-4999(2019)03-0031-07

2018-10-11

国家重点研发计划项目（2016YFC0803103）。

王宁（1995—），男，山西长治人，硕士生，研究方向为测绘工程、基于ROS的移动机器人定位等。

王坚（1980—），男，江苏淮阴人，博士，教授，研究方向为大地测量学与测量工程、应急定位、卫星导航应用服务等。

王宁，王坚，李丽华.一种改进的AMCL机器人定位方法[J].导航定位学报,2019,7(3):31-37.（WANG Ning,WANG Jian,LI Lihua.An improved adaptive Monte Carlo localization method for robot[J].Journal of Navigation and Positioning,2019,7(3):31-37.）

10.16547/j.cnki.10-1096.20190306.