从零开始搭建一辆无人清扫车：车辆控制、SLAM定位建图、路径规划

前言：最近有时间整理了一下在学校做过的无人清扫车相关的资料，写出来跟大家一块交流学习，主要摘自我毕业论文其中的一部分内容。这部分主要内容是把车站商场里那种有人驾驶的清扫车进行了一下无人化的改造，主要内容涉及车辆控制、SLAM定位建图以及路径规划。改完后的效果大概是这样的。（怎么上传动图的功能不太好使，汗😓，可去某乎看）

1 无人清扫车整体结构

这个无人清扫车主要由三部分组成：由清扫车机械机身构成的机械平台；由各传感器构成的环境感知系统；由数据处理器（上位机）和底盘控制器（下位机）构成的控制系统。设计的无人清扫车组成如下图1所示。

无人清扫车设计有三种控制模式：人工驾驶模式、遥控模式、无人驾驶模式。当处于无人驾驶模式时，控制系统中的数据处理器通过环境感知系统使用传感器采集环境信息进行SLAM定位与建图，收集环境信息做出路径规划策略，下发指令给底盘控制器控制清扫车移动并执行清扫任务。

图1 无人清扫车系统框图

图2 清扫车传感器的连接

图3 机械平台

底盘控制器能够控制机械平台执行各项具体操作，例如前进、后退、转向等。无人清扫车控制架构采用了分层控制，即将算法处理交给数据处理器（上位机）进行处理，将运动控制交给底盘控制器（下位机）进行处理，使运算与控制分离，保证了安全与实时性。

2 无人清扫车的硬件实现

图4 清扫车的硬件改装

本文在清扫车机械车体的基础上，进一步加装硬件以实现后续功能。转向部分，最终选择使用车用无刷直流电机控制车体的转向，并在转向轮处安装了磁编码器用于测量转角以完成转向的闭环控制。在两后轮处分别安装了一个旋转编码器用于测量转速以完成速度闭环控制以及轮式里程计的构建。在控制器处安装开关切换人工驾驶与自动驾驶模式，并可以作为紧急按钮使用，用于停止车辆的自动驾驶模式。激光雷达用于测量周围障碍物的距离信息。深度相机用于获取前方三维障碍物的深度（距离）信息。考虑到成本的限制，使用了一个低成本的毫米波雷达，它可以探测一定角度范围内的障碍物距离信息，但无法返回障碍物的具体位置及大小信息。IMU用于辅助无人清扫车定位。安全触边（防撞条）用于在发生意外碰撞时及时停止车辆的运行。数据处理器选用了英特尔的NUC主机，并作为定位建图，导航及交互的处理平台。无人清扫车底盘控制器选取了STM32F767，负责采集编码器信息、与上位机进行串口通信、控制电机转动以及控制无人机清扫车清扫地面。

3 无人清扫车的软件结构

如下图5所示，右侧为无人清扫车硬件组成以及各自的功能任务，左侧主要为计算平台对应的定位建图与导航功能的软件结构。

图5 无人清扫车软件结构

4 无人清扫车的控制—底盘控制器

无人清扫车整体结构基于分层控制的设计理念，即将算法处理交给数据处理器（上位机）进行处理，将运动控制交给下位机进行处理，运算与控制分离，以保证安全与实时性，底盘控制器（下位机）即负责处理控制相关的功能。

底盘控制器实现了信息采集，运动控制以及与上位机通信等功能。它接收上位机的速度方向等控制指令，控制车体运动并将速度、前轮转向角等信息反馈回上位机。它根据旋转编码器计算轮速从而实现速度控制，并根据车辆运动学模型及航迹推测法计算轮式里程计并上传，用于后续实现基于多传感器的融合定位；根据磁编码器计算前轮当前角度，完成转向闭环控制；当安全触边被触发时，及时使车身停止动作。同时，底盘控制器负责接收遥控器信号，可以遥控车身运动，遥控器端可以选择停车模式，无人驾驶模式，遥控模式三种控制方式。图6为底盘控制器相关功能的结构框图。

图6 底盘控制器功能模块图

5 无人清扫车的SLAM定位与建图

图7 无人清扫车运动学模型

本文使用基于多传感器融合的SLAM方法实现无人清扫车对工作环境的建图与定位功能，使用轮式里程计、IMU、激光雷达以及二维码实现定位。在cartographer的基础上，我们引入轮式里程计和辅助定位二维码。
轮式里程计的计算任务由上述底盘控制器负责，它是基于图7车体的运动学模型以及航迹推算法建立的。

然后，我们在定位功能容易发生漂移的位置张贴二维码（如图8所示）作为地标可以用于辅助定位，当摄像头检测到二维码时，首先计算出二维码相对于摄像头的变换矩阵，再计算出二维码相对于车体坐标系的位置，从而可以得到车体相对二维码地标的变换矩阵，将此位置变换关系添加为约束用于后端优化以提高定位精度。

图8 二维码辅助定位

6 无人清扫车的全覆盖清扫路径规划

目前较多的工作是针对于小型两轮差速驱动的机器人，而大面积的清扫任务一般使用较大结构的机器人。本文使用了基于单元分解的方法来规划清扫路径。规划出来的路径由较长的直线段组成，更适合体型较大或受运动学约束的扫地机器人使用。


图9 基于单元的全覆盖路径规划示意图

图10 清扫路径规划实验过程与结果

7 无人清扫车的避障路径规划

清扫路径的规划中仅考虑了地图中的静态障碍物，没有考虑环境中的动态障碍物以及车辆的运动学约束，因此不能直接用于跟踪控制。本文在动态窗口法DWA的基础上，结合清扫车的运动学模型，控制量选择了车辆速度和前轮转角，并结合清扫场景重新设计了轨迹评价函数。在行驶中遇到障碍时，使用了下图所示的基于有限状态机的避障规则。

图11 用于避障决策的有限状态机

图12 清扫任务中的三种典型场景

当遇到障碍物时，清扫车首先尝试绕行，绕行障碍成功则返回直行状态继续行驶。当绕行失败时，进入后退状态，在此状态下，清扫车首先确定前方目标路径的弯曲方向，优先向着相反的方向后退。例如，图12场景三所示，清扫车遇到无法绕行的障碍物，前方目标路径向着右前方弯曲，那么它将优先向着左后方后退。当退出一定的距离时，再朝着目标规划无碰撞轨迹以绕过障碍物。当后退状态仍旧无法绕过障碍物时，此时进入恢复状态，此状态下会原地等待动态障碍消失或更换目标路径。成功则返回直线状态，否则仍执行恢复行为。

8 运行效果

本文将上述方法集成在ROS中（在navigation导航栈的基础上），并使用无人清扫车在多个场景下验证了方法的有效性。除二维激光雷达外，无人清扫车在前方安装了深度相机，相机获取的三维深度信息被投影压缩到二维以实现对低矮障碍的避障。交互部分是用QT来写的。最终，让我们来看一下实际效果吧。（上传动图的功能不太好使，汗😓，可去某乎看）

图13 无人清扫车的运行效果

写在最后：后面去做了毕业相关的深度强化学习相关的内容。无人车实验是在某个工厂进行的，非常非常非常感谢他们的支持，可惜个人精力有限，这个无人清扫车还是有些遗憾没实现完。PS：23届怎么这么难找工作，差点就没工作了呀，汗😓。