基于SLAM的校园配送AGV地图构建及路径规划研究*

朱伟枝，谢娟烘，卢子荣

（广东理工学院，广东肇庆 526100）

0 引言

随着科技的发展，无接触配送、无人配送等多个热门词汇开始进入我们的视线[1]。无人配送是指物品流通环节中没有或是少量人工参与，用机器替代人工或者人机协作的配送方式，达到提高效率、减少成本的目的。无人配送方式需求场景众多，包括快递、外卖、B2C零售、商超便利、生鲜宅配、餐馆/KTV配送、C2C配送需求等，为生产生活需求提供了相当大的便利性[2]。在国内外疫情防控形势依然严峻的情况下，无人配送无疑是最安全、最有效率的一种配送方式，在未来将成为主流趋势。

在无人配送方面，国内外都做了许多研究与实践。Nasri等[3]讨论了在不确定交通条件下，如何优化无人驾驶车辆的路线和速度。张明雨[4]利用代价地图的cost信息来构建状态空间，结合Q-earning算法以达到修改全局路径实现机器人的动态导航。2018年，阿里菜鸟物流无人车实现了支持用户扫码和刷脸功能。2021年，美团发布了新一代自研L4级别无人驾驶车，并投入到了买菜服务配送中。

在当今大学校园里，学生人数众多，学校可以说是传染病突发公共事件的高发场所[5]。基于此，本文提供一种校园AGV（Automated Guided Vehicle）地图构建及路径规划设计方案。AGV是一辆自动导引运输车，其搭载的激光雷达会实时扫描周边环境，采集周围的环境信息，对环境监测信息发出反应，识别移动与非移动信号，从而映射到AGV系统的传动机构，实现避障，并根据设定地点进行路线规划及导航。

1 AGV小车总体设计

1.1 控制部分

小车的控制部分由一块树莓派4B作为主控，Ardui⁃no作为编码电机控制器。在树莓派4B上烧录好Ubuntu系统，并安装ROS机器人操作平台。电脑可通过树莓派上的蓝牙和WiFi串口上传或下载相关驱动和数据，通过Rviz观察SLAM建图情况并监控小车实时动态。Arduino通过电脑端的Arduino IDE将编码电机的驱动程序下载下来，并通过USB线及相应串口通信驱动连接至树莓派。

1.2 动力部分

小车的动力部分由一对12 V带编码器的有刷直流电机、一套34∶1的齿轮减速器、一对轮胎、一个导向轮以及一块12 V蓄电池组成。小车通过Arduino可以接收到电机上的编码器传达的传动数据，通过齿轮减速箱可以有效地放大扭矩。

1.3 雷达部分

小车搭载的激光雷达型号是思岚RPLIDAR A1，采用激光三角测距技术，扫描频率5.5 Hz，采样频率8 k/s，扫描范围最远12 m，测距核心顺时针旋转，可实现对周围环境的360°扫描测距检测，从而获得周围环境的轮廓图[6]。该激光雷达对ROS系统具有良好的整合适配性，让机器人能更快速、精确地建图。

2 基于SLAM的全局地图扫描

2.1 定位导航

定位导航包括定位、建图和路径规划。定位和建图通常由SLAM（Simultaneous Localization and Mapping）来实现，而路径规划则是根据现有的地图，在点与点之间计算好最优路线[7]。SLAM和运动规划是AGV的两个核心技术，SLAM技术可以得到稀疏的定位地图，结合后处理可以得到稠密的三维点云地图[8]。此时再将其转化为栅格化地图，AGV就可以在这个地图的基础上，结合传感器获取外部环境信息进行运动规划，找到一条适合自身行走的最佳路径。

2.2 基于LOAM算法的地图构建

2.2.1 特征提取

LOAM（Lidar Odometry and Mapping）算法[9]的两个核心模块为特征提取（Lidar Registration）和里程计解算（Odometry and Mapping）。本文提取的两种特征分别为平面点和边缘点。提取特征点的常用方法有3种：特征向量法、直方图法和旋转图像法。然而，虽然这些方法能够准确地覆盖一帧的大部分信息，但是由于计算量大，很难用于激光SLAM的相邻帧匹配，因此本文引用曲率计算方法，公式如下：

式中：c为计算得到的曲率；k为一次扫描周期；L为雷达当前坐标系；i为第k次扫描点云的第i个点；为第k次扫描点云中的第i个点在雷达当前坐标系L中的位置向量；S为点前后指定数量的点，具体在实际代码中实现就是计算前后5个点。

经计算后，选取曲率特别大、平滑度高的特征点作为边缘点，曲率特别小、平滑度低的特征点作为平面点。

2.2.2 特征匹配

提取特征点后，采用scan-to-scan方法对连续特征帧进行匹配，并建立误差模型。由于每个特征点都会随着雷达的运动而改变，因此本文为了方便处理，将所有的特征点都重投影到每一帧的初始时刻，从而获得对应的姿态变换信息。本文采用点到线的距离以及点到面的距离计算方法分别构造边缘点以及平面点的约束公式。

边缘点约束公式：

平面点约束公式：

式中：k为一次扫描周期；为第k次扫描点云中的第i个点在雷达当前坐标系L中的位置向量；为经过旋转和平移后的位置向量。

优化式（2）和（3），列出第k+1次扫描点云中的第i个点在雷达当前坐标系L中的位置向量和变化后之间的关系为：

经过运动补偿后为：

式中：R是满足罗德里格斯变换的旋转矩阵；是k+1帧中第i点相对于t k+1时刻的平移。

R变换得：

式中：T L k+1是k+1帧起始时刻点在雷达坐标系中的平移，即：

设函数：

采用列文伯格-马夸特法（LM）算法求解，迭代过程为：

式中：J为雅可比矩阵。

将上述式子不断匹配求解，直到收敛，可以获得雷达坐标系L下的结果，实现相邻帧之间的变换。而为了定位和建图，还需要转化到全局地图坐标系W下。本文在环境建图中采用map-to-map方式匹配，并运用PCA主成分思想提取点云中的边缘点和平面点，再结合式（2）和（3），利用LM法来求解。

3 路径规划

3.1 全局路径规划

全局路径规划[10]主要有三种基本算法：贪婪算法、Dijkstra算法和A*算法[11]。贪婪算法在规划路径时寻找的是离目标节点最近的节点作为下一次搜索的起点，但不能保证能找到最优路径。Dijkstra算法考虑的是当前节点与其周围节点之间的距离，遍历完所有节点才可以找到最优路径。而A*算法是结合了贪婪算法和Dijkstra算法的思想，综合了两种算法的优点，是目前应用最广泛的路径规划算法之一[12]。

A*算法是一种典型的启发式搜索算法，用路径优劣评价公式表示：

式中：f(n)为节点n的估价函数；g(n)为初始节点到当前节点n的实际代价；h(n)为从节点n到目标节点最佳路径的估计代价。

算法基本实现过程：从起始点开始计算其每一个子节点的f(n)值，从中选择f(n)值最小的子节点作为搜索的下一点，往复迭代，直到下一子节点为目标节点[13]。

3.2 局部路径规划

AGV在获得目的地信息后，已经通过全局路径规划获得一条最优路径，但是环境一旦发生变化，未及时更新地图时，AGV就未必能做出相应的行动策略。因此，为了应对突发事件，需要AGV能够实时获取环境障碍物的信息，即局部路径规划[14]，以提高对环境的适应能力。本文采用动态窗口法（Dynamic Window Approach，DWA）[15]来实现AGV的局部路径规划。

DWA算法的原理是在线速度v和角速度ω构成的速度空间(v,ω)中不断采样，模拟AGV在采样得到的速度下的运动轨迹，并针对这些轨迹进行评价，从而选取最优的轨迹来驱动AGV运动。

4 实验与测试

4.1 上位机算法装载与调试

本文所采用的算法分别为LOAM-Slam建图算法、A*全局路径规划算法和DWA局部避障算法。其中，A*算法和DWA算法已经集成在ROS功能包中，因此只需要搭建LOAM算法并完成算法切换。

本文选用的系统与依赖库的搭配是Ubuntu 20.04+ROS+PCL1.12.1编译器+Ceres依赖库，其中，PCL1.12.1编译器和Ceres依赖库都是开源社区的C++库。PCL用于在AGV扫描建图中编译平滑的映射数据，Ceres用于解决地图建模时的大型复杂的优化问题，两者相辅相成，适合在大型开放场景中进行地图扫描与建模，并能够在较低的功耗中实现算法。

当配置好系统环境，并下载安装好Ceres依赖库和LOAM算法之后，还需要配置重定向测试文件。测试文件Bag由LOAM算法社区提供，可以实现无实体小车的模拟场景测试。

运行LOAM时，通过roslaunch命令启动雷达进行扫描。在地图创建完成后，退出终端命令窗，系统会自动将4个代表地图不同数据的pcd文件保存在/tmp目录下，而每次重启系统，/tmp目录下的文件都会被清空。因此在退出系统前，要注意在Rviz中将Map Cloud勾选，才能在测试包运行完毕暂停后，将最终的地图扫描数据保存到PCL编译器目录下层。

4.2 Gazebo环境搭建

Gazebo是ROS系统中的一个动态环境模拟软件，通常用来测试在复杂环境下的多机器人协同运作实验。Ga⁃zebo提供了高保真的物理引擎测试环境，以及一整套的传感器模型，能够无限贴近现实地模拟出机器人在实际场景中的运作方式。

如图1所示，采集一张某高校的卫星俯瞰图，并勾勒标注好需要进行测试的主要配送道路。在Gazebo中打开场景绘制器，根据卫星俯瞰图勾勒出的轮廓，在场景绘制器中使用Wall工具绘制出需要测试的封闭道路并保存场景。如图2所示，在相应的轮廓线条上双击编辑每堵Wall的参数，并对每堵不同高度位置的Wall进行填色以作区分。

图1 某高校主要配送道路的卫星俯瞰图

图2 绘制墙面效果

在搭建完仿真环境后，将模拟的小车放入地图当中，小车可以是Gazebo仿真库提供的模型，也可以是通过建模外部导入的Urdf模型。本次选用Gazebo仿真库的模型小车，并在其上添加激光雷达、相机等传感器，如图3所示。在Gazebo终端不关闭的情况下，再开启一个终端来打开Rviz可视化界面。配置LOAM算法后，将雷达和深度相机的传感关系建立，在Rviz界面中将地图添加进去。当小车加载完后，通过巡逻整张地图进行建图。通过LOAM算法优化并进行灰化处理后的地图模型如图4所示。

图3 小车模型导入

图4 地图构建

在建完地图模型后，通过roscore命令切换出2DSlam来运行A*和DWA算法。通过plan_node.cpp函数构建好Global全局环境变量，运行后的效果如图5所示，为配送道路某转角处的路径处理。同理，在A*算法的基础上，切换至DWA动态窗口算法后，小车对路径的选择会明显更加谨慎。由于没有回环检测，对噪点的过滤效果也不如前者，但是在局部避障方面，DWA算法因为评分机制的存在，能够让其在近距离内有效的测算出移动障碍物的相对位姿并及时优化路径来避开。

4.3 室内场景模拟

AGV小车的开启步骤与在Gazebo环境下配置流程大致相同。在正式开启前，需要启动安装在工作环境中的激光雷达ROS驱动，并发出tf，以转换Arduino端发送过来的车轮里程计中的坐标系信息。而APM驾驶仪是与Arduino直连的，可以在树莓派上通过COMP串口通信获取数据。完成以上流程后，就可以编辑启动文件来开启AGV小车了。图6所示为室内环境下AGV小车模拟实际场景的定位导航及运动规划效果。

5 结束语

本文提供了一种可适用于校园AGV配送的定位导航设计方案，就该方案展开了地图构建和路径规划两大步骤的研究。地图构建部分采用基于LOAM的算法，将复杂的SLAM问题分为高频的运动估计和低频的环境建图。在运动估计中通过曲率计算提取平面点和边缘点两种特征，再采用scan-to-scan方式对连续特征帧进行匹配，并建立误差模型。在环境建图中采用map-to-map方式匹配，运用PCA主成分思想提取点云中的特征点，再利用LM法来求解。路径规划部分先采用A*算法获得一条最优的全局路径规划路径，再采用DWA算法对局部进行规划，以提高对环境的适应能力。实验证明，本文搭建的AGV配送小车能够满足在校园环境多种不同模拟场景中的定位导航需求，为校园AGV配送系统的设计及相关产业的发展提供了一定的技术支持。