基于树莓派和STM32的室外智能清洁机器人

文/陈昌宇 邱广萍 谢伟彬

清洁劳动是一项重复性很强的工作，随着人们生活质量的改善，这项工作对现代年轻人来说毫无吸引力，但是清洁工作又是必不可少的，所以，一部室外智能清洁机器人的出现是大势所趋。

本系统研究内容有以下几方面：

（1）地图构建，能够在一定区域范围内获取云点数据，最后通过二维栅格建图形成可用于其他功能的地图。

（2）自主导航和自动清洁，室外智能清洁机器人可在地图内进行定点导航并清扫。

（3）动态避障，遇到障碍物时室外智能清洁机器人自动绕过障碍物到达指定地点。

（4）规避悬崖，室外智能清洁机器人运行在悬崖边上，机器人的一端检测到悬崖信号，机器人会立即停止运行并重新规划路线，绝不会掉入悬崖或深谷。

1 系统总体方案

本系统选用树莓派和STM32 单片机为控制器，由电源电路模块、电机驱动模块、激光雷达、摄像头、IMU 九轴惯性传感器、编码器、悬崖检测模块等其他外围设备组成。完成构建地图、规避障碍物、自动跟随、自主导航、自动清扫垃圾等功能。

系统总体框图如图1所示。

2 模块设计

2.1 地图构建

机器人在环境中实现无人驾驶的功能需要解决定位与建图的问题。目前比较成熟的技术有激光雷达和视觉传感器。由于激光雷达相对于摄像头视觉距离测量比较准确，误差模型较为简单，在强光直射以外的环境中能稳定运行，点云的处理也比较容易，相比之下更适合用于室外建图。同时，点云信息本身包含直接的几何关系，使得机器人的路径规划和导航变得直观，激光SLAM（Simultaneous Localization and Mapping 同步定位与建图）理论研究也相对成熟，对CPU 的计算能力要求远低于视觉SLAM。所以在室外智能清洁机器人顶部中央安装了RPLIDAR-A1 激光雷达，由发射系统、接收系统、信息处理等部分组成。

系统运行时，通过激光雷达、编码器和9轴姿态惯性传感器重复采集得到地图特征定位自身位置和姿态，再根据自身位置增量式地构建地图，从而达到同步定位和地图构建的目的。并使用概率统计的方法，通过多特征匹配来达到定位和减少定位的误差。

建图系统框图如图2所示。

2.1.1 回环检测

激光雷达获得的点云数据会由于传感器的移动而导致采集的数据产生畸变，由此产生累计误差。为了减小累计误差，引进回环检测。在SLAM中，位姿约束建立在前一帧的基础上，不可避免产生逐帧误差。回环检测就是把历史所有帧和当前帧做匹配，建立位姿约束的历史帧，减少约束数，起到了减小累计误差的作用。回环检测对SALM 系统的意义重大，关系到估计的轨迹和地图在长时间下的正确性，当然也提供了当前数据与所有历史数据的关联。回环检测对整个SLAM 系统精度与稳健性的提升是非常明显的。

2.1.2 算法比较

激光SLAM 大致有Gmapping、Hector、Cartographer 三种算法。其中hector 不适合复杂的环境，如果环境特别凌乱的话，建图的效果不是很好，相对来说gmapping 建图的效果会更好；Cartographer 虽然采用了回环检测，鲁棒性好，可以消除累计误差，但是运算量要远大于Gmapping 和Hector ，所以采用openslam_gampping 在ROS 中二次封装的软件包slam_gmapping。真正的核心代码实现都在openslam_gampping 里面。

图1：系统总体框图

图2：建图系统框图

Gmapping 是根据激光雷达点云数据来构建二维栅格地图的Rao-Blackwellized 粒子滤波SLAM 算法。其中所有粒子都相当于一个独立的地图，该软件包采用自适应蒙特卡洛算法在损失较小精度的前提下，减少重采样的次数和采样的粒子数，提高算法的效率。Hector算法与Gmapping 算法对原图处理后的对比图如图3所示。

2.1.3 建图

建图时，设定坐标系，坐标原点，调用GridSlamProcessor:init ,初始化粒子数，设定子地图大小。初始化一些重要的参数以及采样函数随机种子，将SLAM 里的参数传递到openslam 里，用节点 scan 获取激光器云点数据和运动姿态，每隔throttle_scans_帧计算一次数据。

接着进行扫描匹配，在score 函数里，计算障碍物坐标phit，再将phit 转换成网格坐标iPhit，计算光束上与障碍物相邻的非障碍物网格坐标pfree,pfree 由phit 沿激光束方向移动一个网格的距离得到，把pfree 转换成网格坐标ipfree（增量，并不是实际值）在iphit 及其附近8 个栅格，根据最优准则搜索最优可能是障碍物的栅格。接着进行了一次权重计算updateTreeWeights(false)，重采样之后再进行了一次权重计算。重采样得到的粒子集对目标分布的近似越差，则权重的方差越大。最后利用最优粒子对地图进行更新。

图3：Hector 算法与Gmapping 算法对原图处理后的对比

在SLAM 建立地图模型中，构建的地图就是激光雷达采集的所有路标点的集合，一旦确定了各个路标点的位置，就可以说完成了建图。

2.2 自主导航

机器人的导航需要先做好自身的可靠定位，系统通过机器人的位姿和观测得到的特征运用概率定位法与建好的地图进行特征对比定位；导航应用高效的A*算法，它能从起点到终点探测代价最小路径的广度优先搜索方式，如果失败则选择其它路径直到得到最优路径。

2.2.1 实现定位

使用ROS 中的amcl 定位包，通过自适应的KLD 法进行粒子滤波更新粒子来定位的，属于蒙特卡洛定位的升级版本。更新得到的粒子与建好的地图作对比，赋予粒子一个概率，再根据这些概率重新生成粒子，概率越高生成的概率越大，如此循环，不断迭代，机器人的位置就可以推算出来了。

2.2.2 实现导航

设定好导航目标。用IMU 九轴惯性传感器获得室外智能清洁机器人的加速度、陀螺仪和方向角数据，进行姿态融合解算，得到室外智能清洁机器人的运行姿态，同时用编码器检测室外智能清洁机器人的速度和运行距离。然后把运行姿态、运行速度以及运行距离反馈给树莓派上位机，通过 ROS 的中用于让机器人在制定框架内移动到目标位置move_base 包进行融合解算，得到室外智能清洁机器人的运行路径，再通过树莓派上位机传送信号给下位机来控制L298N 电机驱动，最终实现自主导航功能，到达目标地点。

2.3 动态避障和规避悬崖

激光雷达通过飞行时间测距。飞行时间测距指激光器发射一个激光脉冲，并由计时器记录下出射的时间，回返光经接收器接收，并由计时器记录下回返的时间。两个时间相减即得到了光的“飞行时间”，而光速是一定的，因此在已知速度和时间后很容易就可以计算出距离。测量的距离=（（回返的时间-出射的时间）*光速）/2。

在建好的地图的基础上，导航系统把机器人位置、传感器姿态、障碍物信息和作为SLAM 地图的结果而获得的占用网格地图调用到固定地图，用作占用区域、自由区域和未知区域。使用move_base 提供的ROS 导航功能，设置四个配置文件base_local_planner_params、costmap_common_params、global_costmap_params、local_costmap_params，分别定义了障碍物的代价、机器人半径、路径规划时要考虑未来多长的路、移动速度等等。

在自动导航运行时，会通过SLAM 以机器人为中心的部分限定范围中规划路径时，计算障碍物区域、预计会和障碍物碰撞的区域以及机器人可移动区域，生成躲避障碍物时用的地图，接着系统自动重新规划路径并绕过障碍物，实现动态避障的功能。

规避悬崖是通过超声波传感器测距实现的，超声波测量室外智能清洁机器人的底座到地面距离，设置一个最大值参数，若大于该参数，判断为存在悬崖，系统重新规划路线并绕过该悬崖。

3 数据处理

3.1 前端配准

前端配准就是将两个或两个以上坐标系中的点云数据转换到统一坐标系统中的空间坐标变换。配准操作实际是要找到从坐标系1 到坐标系2 的一个刚性变换。

通过ICP 算法把不同坐标的点云数据合并到同一坐标系统中。ICP 算法是为了将点云中的数据点进行匹配。ICP 配准过程：计算点云P 中的每一个点云在点云Q 中的对应最近点；解算使对应点对平均距离最小的刚体变换，得到平移参数和旋转参数；对P 使用得到的平移参数和旋转参数，得到变换后的点云P’；新的变换后，如果点云与参考点集满足以上目标函数需求，就是两个点云的平均距离小于给定的阈值，那么就停止迭代计算，否则新的变换点集P’作为新的P 继续进行迭代，最终达到目标函数的要求。

3.2 后端优化

前端配准可以给出一个短时间内的轨迹和地图，由于存在累计误差，地图在长时间内并不是非常准确的。所以在前端配准的基础上，加上了后端优化，用于长时间的最优轨迹和地图。

粒子滤波算法的核心思想是利用一系列随机样本的加权和近似后验概率密度函数，通过求和来近似积分操作。粒子滤波器简单、易于实现，提供了有效解决非线性动态系统的方法。粒子滤波根据状态转移方程解算生成大量的采样，这些采样就称之为粒子，通过加权这些粒子的和逼近后验概率密度。采样到达一定数量，每个粒子计算对应的重要性权值，代表了预测位姿取第个粒子时获得观测的概率。粒子获得观测的概率越高，权重越高。接着根据权值的比例重新分布采样粒子。下一轮滤波中，再将重采样过后的粒子集输入到状态转移方程中，就能获得新的预测粒子。最后对每个采样粒子，通过其采样的轨迹与观测计算出相应的地图估计。

粒子滤波算法可以有效作为SLAM 的问题，但是算法存在着一些问题。其中最主要的问题是需要用大量的样本数量才能很好地近似系统的后验概率密度。机器人构建越复杂的环境时，描述后验概率分布所需要的样本数量就越多，算法的复杂度就越高。因此，该算法的重点是能够有效地减少样本数量的自适应采样策略。

4 结语

室外智能清洁机器人通过树莓派上位机和STM32 下位机为控制核心，加上其他模块构成，实际检验表明，该机器人偶尔存在定位不够精确，导航路线规划不够理想的问题；在户外环境可以实现小区域建立地图，自主规划路线，动态避障，最终实现清扫垃圾。