基于ROS和激光雷达的室内移动机器人定位和 导航系统设计与实现

，，，

(同济大学电子与信息工程学院, 上海 200092)

0 引言

移动机器人必须具备在环境中移动的能力，即能够使用自身搭载的传感器感知环境信息，并据此实现定位和导航功能。其中，同步定位与地图构建SLAM(simultaneous localization and mapping)技术被认为是机器人自主运动的关键[1-3]，具有重要的理论研究价值和实际应用意义。

本文在研究和分析SLAM算法的基础上，使用卡片式电脑树莓派3B作为核心控制器，基于激光雷达搭建硬件平台，在Linux操作系统下使用ROS分布式机器人操作系统框架进行软件开发，设计并实现开源、低成本、差速驱动的室内定位和导航系统。

1 同步定位与地图构建

SLAM模型如图1a所示，数学表达式可描述为：给定机器人的控制序列u1:t={u1,u2,,ut}和观测序列z1:t={z1,z2,,zt}，求环境地图m和机器人的位姿x0:t={x0,x1,,xt}，其中，xt表示t时刻机器人的位姿；mi表示环境特征；ut表示t时刻给机器人的控制量；zt表示t时刻传感器的观测量。t时刻机器人的状态xt只与t-1时刻的状态xt-1，当前时刻的控制量ut以及传感器的观测量zt有关。机器人从未知环境的未知地点出发，在运动过程中通过重复观测到的环境特征，如墙角、立柱等，定位自身位置和姿态，再根据自身位置增量式地构建地图，从而达到同时定位和地图构建的目的。

由于SLAM过程具有不确定性，某一时刻机器人的位姿无法精确表示，需要使用分布进行估计。传感器对机器人相对运动的辨别存在固有的不确定性，加之外界环境的噪声影响，在构建地图的每一次迭代中，附加到原有地图上的增量会产生误差，随着迭代次数的增加，误差会逐渐累积，从而对地图构建、机器人定位以及导航等的精确度产生极大影响。因此，需要使用图像匹配法、环路闭合检测法、卡尔曼滤波或粒子滤波等方法对累积误差进行补偿[2-4]。同步定位与地图构建过程可以简化为“估计--观测--校正”，其系统框图如图1b所示。

图1 SLAM模型及其系统框图

2 基于ROS和激光雷达的系统实现

基于ROS和激光雷达的室内机器人定位和导航系统，实物照片如图2a所示，可作为室内移动机器人SLAM、导航以及ROS框架的基础实验平台，主要硬件模块有：树莓派3B、RPLIDAR A1激光雷达、Arduino Pro Mini、A4950双路电机驱动模块、减速电机以及降压/稳压直流电源模块，各模块的硬件连接关系如图2b所示，加入其他传感器或执行机构后，可实现更多的功能。

2.1 硬件核心模块

2.1.1 激光雷达

激光雷达LiDAR是基于激光的距离测量系统，它通过向目标发射激光束，并接收目标反射的光束来测量目标的距离等参数，常见的激光传感系统[5-6]有：对单个特征点进行测距的距离传感器、对平面进行二维扫描的激光雷达以及三维立体扫描的激光雷达。本文所使用的RPLIDAR A1是由SLAMTEC公司开发的低成本二维激光雷达解决方案，利用三角测距原理对二维平面6 m半径范围内进行激光测距扫描，生成平面点云地图信息，在5.5 Hz的扫描频率下，测距分辨率可达当前距离值的0.2%。

2.1.2 控制器

室内移动机器人的核心控制器树莓派3B是一款基于ARM架构，运行Linux操作系统的开源卡片式电脑[7]。由于基于Linux内核的Ubuntu Mate操作系统是分时系统而非实时系统，而移动机器人的运动控制对实时性有一定要求，因此需要使用额外的控制器Arduino Pro Mini最小系统来控制机器人的运动。主控制器和驱动控制器之间使用高速串口通信完成数据和命令的发送和接收。

2.1.3 其它模块

移动机器人平台使用A4950双路功率驱动分别驱动两个减速比为1∶30、额定功率7 W、最大转速500 r/min的减速电机，实现差速运动。电机自带A/B相编码器，可用作转速反馈以及里程计。移动机器人使用大电流输出的12 V航模电池供电，而树莓派、Arduino Pro Mini和传感器模块的工作电压为5 V，因此需要对电池电压进行降压稳压。需要注意的是，树莓派的USB接口输出电流有限制，当激光雷达旋转并开始工作时，所需供电电流可能会超过树莓派的输出能力。为保证机器人稳定运行，激光雷达另使用独立的USB集线器供电。

2.2 软件核心模块

2.2.1 机器人操作系统ROS

ROS是一套专为机器人软件开发所设计的开源元级操作系统，集成了硬件描述、底层驱动程序管理、程序间传递消息、程序发行包管理等一系列的服务。ROS采用分布式处理框架，使得不同架构的处理平台成为机器人系统的一部分，其核心使用通信模块实现模块间对等式网络的解耦合和网络连接的处理，包括基于服务的同步RPC(远程过程调用)通信和基于话题的异步数据流通信。ROS的分布式处理框架可以使每个可执行程序被单独设计，并且在运行时松散耦合，按照功能包和功能包集的方式进行分组，以便于共享和分发。自2010年正式发布后，由于其开源和易用等特性，迅速得到机器人研究者的关注和应用[8-9]。

2.2.2 系统软件设计

得益于ROS的分布式架构，根据移动机器人的不同需求和功能，可对机器人的系统软件进行模块化设计和开发[9]，主要包括底盘运动控制、主控制器和驱动控制器通信、机器人模型、键盘远程控制、地图构建、导航以及用户界面等功能模块。各功能模块对应ROS中的软件包基本组织单元，定义为base_controller、rosserial、myrobot、slam、move_base以及amcl包等，每个包中又有多个不同的用于实现具体功能的节点。软件包及模块间的关系如图3所示。其中myrobot包包含机器人的描述文件，机器人启动时，使用ROS中的工具加载，向系统发布机器人的坐标变换关系，可用于实际系统，也可用于仿真测试；base_controller包负责机器人的运动控制，接受机器人的线速度和角速度运动指令，并通过计算转换发送至驱动控制器，同时接收驱动控制器发布的驱动轮速度消息，并转换为里程信息和里程坐标变换，发布给其他节点，该包还提供键盘远程控制机器人移动的节点，可在构建地图阶段通过Wi-Fi局域网远程控制机器人移动；rplidar_ros包为RPLIDAR的ROS驱动，负责和激光雷达通信，配置激光雷达参数，并读取传感器数据；rosserial包负责通过USB在主控制器和驱动控制器之间进行通信；slam、move_base和amcl包则分别负责实现机器人的SLAM地图构建、导航以及定位功能。

图3 室内移动机器人ROS系统软件架构

2.2.3 驱动控制算法

驱动控制器主要实现的功能有：电机控制、电机测速以及和主控制器间的通信。为精确控制机器人的运动，同时保持较好的实时性，驱动控制器的运行周期为20 ms，采用回调方式协调实现多个任务，其流程如图4所示。机器人底盘使用2个减速电机驱动，通过输出TTL电平以及PWM信号驱动A4950功率电路分别控制电机的方向和转速。电机控制使用增量式PID控制算法，其表达式为：

Δu(k) =u(k)-u(k-1)

=KPΔe(k) +KIe(k) +

KD[Δe(k)-Δe(k-1)]

(1)

Δu为控制量u的增量，即输出电机驱动PWM信号占空比的变化量。PID控制器的输出与前一时刻控制量叠加，得到当前时刻控制器的实际输出值。

图4 驱动控制器运行流程

2.2.4 SLAM功能实现

HectorSLAM算法基于扫描匹配和惯性传感系统，得到激光雷达的扫描数据后，对其中的环境特征进行处理，提取出特征点集，再使用高斯-牛顿方程求解得到最佳匹配激光束的刚性变换，实现对机器人位姿的校正和占用栅格地图的构建[5,10]，其流程、机器人各个节点的关系以及数据流则如图5所示。

图5 HectorSLAM功能实现

①加载URDF机器人描述文件，发布机器人的非固定关节的连接状态以及各坐标系间的变换关系；②启动驱动控制器和base_controller、odom_publisher节点，用于控制命令解析、当前速度获取以及里程数据的获取、转换和发布，同时启动键盘远程控制节点；③按照预设工作参数配置并启动激光雷达开始工作；④启动地图构建节点hector_mapping，用于HectorSLAM的具体实现；⑤启动Rviz界面，用于实时观察地图构建和机器人的位姿估计情况，若使用仿真测试系统，则还需使用Gazebo加载虚拟环境地图、机器人三维模型和机器人运动控制插件。

2.2.5 导航功能实现

机器人导航功能的实现主要使用ROS中的move_base软件包，主要功能包括全局和局部的路径规划、计算里程信息以及输出控制命令等。配置好move_base包的节点、参数以及代价地图，将其加入机器人导航功能的启动文件，即可实现导航功能，在已建立好的地图中指定目标位置和方向后，根据机器人的传感器信息，规划路径并控制机器人到达目标状态。

move_base包运行必需的参数有：机器人在环境中的期望位姿move_base_simple/goal；机器人各个坐标系的变换关系tf；根据机器人左右驱动轮推算出的里程信息odom(坐标及航向角)；传感器采集的环境数据sensor topics，即激光雷达发布的环境数据话题sensor_msgs/LaserScan。move_base包根据传感器的实时信息和基于Dijkstra最优路径算法规划的路径，在cmd_vel话题上发布Twist消息，控制机器人的运动。此外导航包可选输入：使用自适应蒙特卡罗方法的移动机器人二维环境下的概率定位系统amcl包，利用粒子滤波器跟踪和预测机器人的位姿；map_server则提供先验地图信息。

3 实验结果和讨论

如前所述，基于ROS和激光雷达的室内定位和导航系统，系统控制器模块主要分为主控制器和驱动控制器：主控制器运行ROS和SLAM算法，获取激光雷达等传感器的数据，构建地图、估算位姿并输出控制命令；驱动控制器主要负责控制机器人的运动，通过串口接收主控制器的控制命令，经PID控制算法转换为PWM占空比控制信号，输出控制驱动电机转速。系统软件部分充分利用ROS分布式架构，基于所要实现的不同功能将软件分为不同模块，包括坐标变换、驱动控制、地图构建等功能模块，模块之间采用ROS提供的话题发布和订阅机制进行通信，实现SLAM和导航定位功能。

3.1 仿真测试

仿真测试使用ROS中的Gazebo工具进行。为使仿真环境与实际环境更加接近，建立机器人的物理模型与实际机器人平台使用相同的URDF机器人描述文件，通过XML描述文件的方式添加Gazebo插件，修改机器人模型的颜色、摩擦力、驱动轮扭矩等参数和实际一致。搭建虚拟的仿真环境如图6a所示，对机器人的SLAM功能和导航功能进行测试，其中SLAM功能选用HectorSLAM算法实现，使用键盘通过Wi-Fi局域网对机器人的运动进行远程控制，让其从起始位置(虚拟环境的中心)出发，完成环境的遍历，实现同步定位和地图构建。根据最终的测试结果，HectorSLAM虽然对激光雷达的精度以及帧率要求较高，但是由于其采用基于优化的算法来解决最小二乘问题，计算量小，且不需要里程计，对环境适应性良好，构建出的地图精度和准确度也满足要求，故本文使用HectorSLAM算法实现机器人室内定位和导航系统，其在仿真环境下运动过程中地图更新的大致过程如图6b ～c所示，最后构建的地图如图6d所示。

通过上述的地图构建过程，可以得到虚拟环境的先验地图，配置move_base包的各节点关系以及代价地图等参数，先指定机器人的起始位姿，再指定目标位置和方向，即可启动机器人的导航。导航启动后，算法自动规划路径，并控制机器人朝目标位置运动，与此同时可开启机器人位姿预测的粒子云实时显示，其过程和结果如图6e ～f所示。

图6 仿真测试过程(使用HectorSLAM算法)

3.2 实际环境测试

基于ROS和激光雷达搭建如前图2中所示的室内机器人原型系统，在真实环境下进行定位与导航的测试，实际测试环境如下图7a中所示，使用隔板围起不规则长条形密闭区域，内部设置有障碍物，机器人使用HectorSLAM算法进行地图构建和定位，其结果如图7b～d中所示，最后构建的地图如图7e所示，准确度较高，且实测过程中没有与障碍物发生碰撞。

图7 实际测试环境及测试结果

4 结束语

室内移动机器人以树莓派为核心控制器，利用RPLIADR A1激光雷达采集环境信息，在ROS分布式框架下进行软件算法的开发，通过软件仿真和实际实验测试机器人系统的性能和表现，结果表明，可实现基于扫描匹配算法的SLAM功能、路径规划以及基于粒子滤波的导航功能，具有成本低、模块化、灵活性强、拓展方便的特点，可作为室内机器人SLAM、导航及ROS框架的基础实验平台。