中国矿业大学（北京） 黄忠睿 兰晓玉 周 健 李润峰 李 傲 胡世博

本文针对室内移动机器人要求精准定位的应用需求，在实验分析激光雷达定位优势的基础上，研究其与惯性传感器融合的定位方法。对基于粒子滤波的SLAM定位算法进行研究，分析其实现原理、推导其实现过程。利用树莓派和STM32构建的实验平台，对改进算法的构图和定位效果进行验证。具体实验结果表明多设备融合定位更加精确，构建的地图边界平滑。

1 研究背景、现状及实验测试环境

1.1 研究背景

随着经济的发展和生活水平的提高，人们对服务机器人的需求日益增加。据统计，我国各类残疾人总数已达到8500万，约占中国总人口的比例的6.21%，居世界前列。此外，截止2018年年底，我国60岁以上的人口已达2.49亿人，占全国总人口比重的17.9%，65周岁以上的人口达到1.7亿人，占全国总人口比重的11.9%。因此未来残疾人和老年人的护理工作将是一大社会问题。若服务类机器人能提供基本的护理工作，将大大减轻社会负担，提高老年人、残疾人等特殊群体的生活质量。

服务类机器人的应用通常发生在室内环境，对机器人室内定位技术的研究已成为热点。不同于室外定位，室内定位不能使用GPS、北斗等卫星服务，只能依靠其他的室内无线设施，比如WIFI、蓝牙、ZIGBEE、超声波、激光、UWB、射频识别标签、LED、磁场等。室内定位精度要求高，但室内环境却复杂多变。因此要把服务机器人引入室内，首先要解决的就是室内定位问题。

1.2 国内外研究现状

目前室内定位的主流技术主要有WIFI、超声波、超宽带、射频识别、蓝牙、ZIGBEE、计算机视觉、激光雷达等。其中，WIFI定位、蓝牙定位等技术成本低但精度不高，在室内面积有限的环境里无法找到机器人的准确位置；超声波定位成本低但容易受环境干扰；激光雷达定位精度高、响应时间短，成本适中。

室内定位种类虽然比较多，但是室内定位算法一般都是通用的。总结起来室内定位有4种常见的定位算法：邻近法、三角测量定位算法、指纹定位算法、TDOA定位算法。4种定位算法相比较而言，近邻法定位精度不高，一般在5-8米，胜在部署简便；三角测量定位算法理论上精度较高，但在实际的室内定位中，时间、角度这些参数的获取比较困难。指纹采集定位算法前期地图绘制工作量大，而且场地布置临时有变化各位置上的信号就会变化，从而导致定位不准。也受到非视距和多径环境的影响，精度误差大，一般在2-3米左右。TDOA定位算法定位精度高，微能信息的UWB定位精度一般根据定位场景，误差在10-30cm，抗干扰能力和穿透能力较强，但是UWB基站和标签部署成本比较高。

最早的室内定位技术是由AT&T公司开发的Active Badges，该系统由Roy Want等人设计研发，但单一的硬件设备的定位精确度无法保证。所以在成本有限的情况下，更高数量级的精度要求就需要一些算法和方法的配合才能够实现，即多传感器的融合。根据这一构想，我们提出了室内移动机器人多融合定位方法。

1.3 本文的实验测试环境

本课题组四轮小车定位机器人的实验范围为中国矿业大学（北京）学院路校区逸夫科研实验楼七楼的环形走廊。通过这一有代表性的区域的实验，验证想法，完善装置。在完成基本任务的基础上，会考虑在更复杂多变的室内环境中实现定位。

2 本文所使用的设备及定位方法

2.1 本文选用的实验平台

根据现有技术设备的优点和不足，我们选用了基于ROS、STM32和树莓派平台构建的轮式机器人为本文的实验设备（图1所示）。其拥有的里程计、陀螺仪等惯性导航设备和激光雷达测距装置可以提供定位原始数据，室内地图的构建，还可以通过Wi-Fi与上位机进行通讯，完成数据回传。

图1 本文使用的实验设备

2.1.1 惯性导航设备

惯性导航设备是基于惯性传感器（Inertial Measurement Units,IMU），对状态进行预测的定位设备，其利用加速度计、磁力计和陀螺仪等传感器对前一时刻的数据信息进行处理，由已知的位置和姿态开始，测量车体在惯性坐标系的加速度和角度，将它们在时间上连续积分，并且变换到导航坐标系中，推算出车体在导航坐标系中的速度、航向和位置等信息，得到当前时刻的相对位置。

IMU具有比较强的自主性，短时间内的定位精度和连续性较高，但惯性传感器不可避免地会随着时间的变化而产生累积误差，需要依靠其他定位信息源对位置推算结果进行校验。移动机器人的IMU控制结构如图2所示。

图2 移动机器人的IMU控制结构

2.1.2 激光雷达（RPLIDAR）

激光传感器定位方法利用激光的反射到达时间（Time of Arrival,TOA）、信号到达角度（Angle of Arrival，AOA）和接收信号强度(Received Signal Strength,RSS)等来测量到障碍物的距离，实现相对定位。

虽然其硬件成本较高，但激光具有的高精度、高分辨率、便携安装、不受可见光影响等优点，对精确的室内定位至关重要。通过分析处理雷达数据，可以对里程计的位置姿态信息进行修正，较为准确地估计移动机器人的位置姿态，极大弥补惯性传感器的短板，从而大幅提高定位精确度。

2.2 本文所使用的定位方法

自主移动机器人具备的自主导航功能是其完成其他任务的前提，自主导航功能包括环境的识别和路径的规划。环境识别是路径规划的前提，通过机器人携带的传感器探测周围环境，得到环境中的信息，并且确定其在环境中的相对位置关系，故环境识别即为Simultaneous Localization And Mapping——同时定位与地图建设技术（SLAM）。

本文使用的激光雷达利用的是三角测距原理，在每次激光扫描测距过程中，发射光束信号，遇到目标物体之后产生的反射信号被其采集系统接收，然后通过内部处理器的实时计算，得到激光雷达到目标物体之间的距离。

为了克服航迹推算误差随时间累积的固有缺陷，本文使用卡尔曼滤波算法来进行里程计和激光雷达的融合定位。卡尔曼滤波是基于状态空间的一套递推滤波算法，在状态空间中引入状态变量的概念。卡尔曼滤波模型包括状态向量和观测向量，状态向量始终在变化，这种变化可用一个方程表示，而观测向量也可用一个方程表示，这两个方程组成了卡尔曼滤波模型。卡尔曼滤波首先通过状态转移方程预测下一个状态，再根据观测变量来得到系统的最优估计。但实验环境的边界清晰，地图描绘的较为准确，卡尔曼滤波图解如图3所示。

图3 卡尔曼滤波图解

3 研究所需的环境地图构建

3.1 基本方法

环境地图的构建方法较多，常用的有拓扑地图、栅格地图和特征地图等等，其中最适用于导航的是栅格地图。

栅格地图将观测量转换为栅格，再将地图变为紧密排列的方格，栅格地图的构建过程称为栅格化。将观测量根据机器人的坐标位置投影到坐标系的栅格当中，这样每一个栅格都会被多次监测，提高了精度。小车的精确位姿（坐标和朝向）可通过其在参考坐标下的朝向和相对障碍物的距离计算。

采用bresenham直线段扫面算法，障碍物所处的栅格标注为occupy，在机器人所处的栅格与障碍物所处的栅格之间画直线，直线所到之处都为free，栅格的占据用概率表示而不只是0、1两种状态。若某一个栅格在激光束a扫描下标识为occupy，在激光束b扫描下也标识为occupy，那该栅格的占据概率就变大，反之，则变小。这样，机器人所处的环境就通过二维栅格地图得以表征。

3.2 实验过程

（1）在现实环境中选取一个合适的点，将机器人放置在选取的点上，开启行机器人的启动节点。

（2）在ssh连接的窗口内编写launch1程序。打开机器人键盘控制节点，方便在建图时移动机器人。在新的终端内ssh连接到树莓派，编写运行launch2程序。按键盘控制机器人时，确保鼠标光标在keyboard_teleop.launch运行的窗口内。

（3）运行Gmapping建图包。在新的终端内将ssh连接到树莓派，然后编写运行launch3程序。

（4）以上三个launch程序运行正常后，在主机上打开新的终端，运行rviz，在数据窗口内查看地图和机器人姿态数据。

地图、激光点、小车位姿数据如图4所示。

图4 地图、激光点、小车位姿数据

3.3 存在的误差

从图5所示中可以看出实验环境的边界清晰，地图描绘的较为准确，但其中有一些散射形状的白色噪点，这主要是因为小车在运行过程中触碰到墙壁造成的。通过进一步优化卡尔曼滤波算法可以解决这一问题，本组将在后续的实验中继续完善算法，改进实验。

图5 实验环境的地图构建

4 机器人的室内精准定位

4.1 基本方法

迭代最近点法（ICP）是小车室内定位中最常用的方法之一，由Besl和Mc Kay等人在20世纪90年代提出，主要是用于两个点集之间的对位匹配，ICP最初用于三维点云的配准，但其也适用于移动机器人所使用的二维激光，其基本原理是对于相互关系未知的情况下的两组点集，构造最小二乘问题来求解出点集之间的相互配准。

I C P 存在迭代不收敛的情况，故比较经典的蒙特卡洛定位法和迭代最近点法（ICP）等定位方法，本文最后选用了AMCL（Adaptive Monte Carlo Localization）定位法，A M C L 法是一种升级版的蒙特卡洛定位法，蒙特卡洛定位法使用的是粒子滤波的方法来进行定位，AMCL使用的是自适应KLD方法来更新粒子。卡尔曼滤波再定位过程中的应用是一开始在地图空间均匀地布下大量粒子，然后通过获取机器人的动作信息来移动粒子，例如机器人向前移动了一米，所有的粒子也向前移动一米。利用每个粒子所处的位置建立一个虚拟传感器，其传递的信息跟实际观测到的传感器的信息作对比，赋给每一个粒子一个概率。再根据产生的概率来重新生成粒子，概率越高的生成粒子的几率越大。这样迭代之后，所有粒子会收敛到一起，这样即可推算出机器人的确切位置。

AMCL的算法步骤如下：

4.2 实验过程

实验的核心是在ROS中运行AMCL算法。

（1）通过激光雷达发布一个新的话题，再通过各个坐标系的tf转换，使初始位置给定，并用map_server加载地图到ROS空间。

（2）将机器人放到地图的起点，连接树莓派开放的WIFI。

（3）打开新的终端ssh连接到树莓派，运行HiBot启动节点和AMCL节点，在主机上打开新的终端运行rviz，可以看到机器人初始定位状态下的粒子分布。

起初粒子（红色）的分布比较散乱。随着机器人的运动，粒子逐渐收敛，通过计算机可以看到在此过程中我们成功实现了机器人的室内精准定位。如图6所示，图6中的黑色定位点为小车的定位位置。

图6 小车的定位结果

4.3 实验结果与分析

本课题组在实验环境中选取了几个特定点进行测试。经过实验，机器人在地图上的定位点比较精准，误差不超过10cm。本实验结果说明了多个传感器进行融合可以实现多种传感器之间的互补，增强定位算法的效率和稳定性，这也是本文研究多传感器融合定位算法的意义所在。但是由于不可避免的因为小车形状产生的物理误差，定位时的环境探测有一些问题，后期的实验中我们会尝试进一步修改算法，使小车的定位精度进一步提高。

5 总结与展望

5.1 课题总结

针对目前室内定位方案中存在的定位精度差的问题，我们提出了一种基于激光与航迹推算的融合定位系统。本文的研究工作主要分为三部分，第一部分是对本课题所选用算法的研究，提出了多融合传感器的优化算法；第二部分是对实验测试环境地图的构建研究；第三部分是对小车精确定位的研究，实现了课题计划的目标，证明了多融合算法的优越性。课题使用的里程计、陀螺仪等惯性导航设备和激光雷达测距装置为小车提供主要定位原始数据。通过对数据的分析处理，比对同时建立的环境地图，将处理后的位置信号作用于相应的车轮驱动电路，实现小车的即时定位，并通过WIFI与上位机通信，完成位置信息的回传。通过实验我们证明了多传感器融合定位的精确性和必要性，提高了小车的定位精度。

5.2 实际应用价值及展望

网络互联的时代，获取信息的便捷极大地促进了人们对位置的需求，只有在位置精确的基础上，机器人的移动才有意义，才能为室内机器人实现更多的功能提供了最基础的前提，如物品的运送、人员的引导、环境的检测等实用实际性的工作，人们可以发挥想象利用此最基础的功能来设计更为复杂的功能来应用于紧急救援、生活社交、安全监控等领域，让机器人更好地服务于我们，我们能更好地体会到机器人对于生活的帮助和自身生活质量的提升，更大程度的让普通老百姓感受到前端科技带给我们的影响，从而能实现机器人作为服务工具的使用价值。

未来，我们会进一步优化算法，并引入人工智能技术，让机器人可以自主实现地图构建和路径规划，更加方便地实现它的功能。