徐宇航 吴静静 赵仕龙 王养坤 巴尔娜·巴拉提

（江南大学，江苏 无锡 214122）

0 引言

随着计算机技术和人工智能技术的发展，机器人的研究日益受到关注，智能移动机器人（Automated Guided Vehicle，AGV）成为机器人领域的一个主要研究方向。其中，AGV小车的定位和地图创建（Simultaneous Localization and Mapping，SLAM）是AGV领域的热点研究问题。目前现有的解决方案仅针对已知环境中的机器人自主定位问题和已知机器人位置的地图创建问题。然而在大多数环境中机器人无法利用全局定位系统进行定位，而且事先获取机器人工作环境的地图较为困难，甚至无法实现。这时机器人需要在自身位置不确定的条件下，实时重建地图，同时利用地图进行自主定位和导航。这就是使用移动机器人的同时定位与地图创建问题。

虽然目前关于SLAM的研究取得了一定的进展，并且相关研究已经广泛应用于室内、水下以及室外等环境。阳映雯[1]等人利用激光雷达和深度相机，实现小车对环境数据的采集，通过ROS系统来对智能小车进行环境的模拟和仿真；张晨阳[2]等人提出了一种适用于动态场景的RGB-D SLAM算法，利用全新的跨平台神经网络深度学习框架检测场景中的动态语义特征。但是目前市场上的AGV小车大多通过ROS一体控制，只能处理如激光数据之类的小量数据，而智能化的SLAM需要更加精确和大量的传感器采集数据，对于硬件和处理算法的性能要求更高，单纯依靠ROS系统无法满足对于大量深度数据的处理需求。且单个深度相机的视野角小于180°，导致机器人的定位和姿态精度下降。

综上所述，为了增大视野广角，同时满足对于大量深度数据处理与建模的需求，从而提高运动精度，本文对AGV小车整体系统进行了创新改进。并提出了一种基于3D点云SLAM的AGV小车创新改进方法。

1 系统整体方案设计

为了实现AGV小车在室内环境中的精确定位与环境地图构建，对小车系统的硬件和软件部分分别进行了改进。在硬件系统方面，为了扩大视角，使用两个深度相机作为系统的数据采集设备。在软件系统方面，对现有AGV小车的控制方式进行改进，设计PC-ROS主从控制系统，通过PC端完成大量点云数据集的处理与建模，再通过编写PC-ROS的通信协议，完成PC端的数据传输到ROS端的工作，减轻了ROS的计算负担。本文的系统框架图如图1所示。

图1 系统框架图

应用层主要为小车的操作系统和传感器，实现人机交互和信息传输。导航层以移动库为基础，根据地图服务器和小车状态来规划路径发送控制信号，并通过反馈数据不断调整。硬件驱动实时检测并向导航层发送小车位置信号。硬件层通过Rosserial通信协议接受控制信号，并根据串口算法来驱动电机。

2 硬件系统改进

本文的硬件系统框架图如图2所示，2个RGBD相机采用的是Intel RealSense L515深度相机和乐视体感深度相机。其中Intel RealSense L515深度相机连接在主机上，AGV小车运行前通过SLAM系统对AGV小车的整体运行环境进行点云构图，运行过程中对小车的位置进行实时监控。AGV小车运行过程中，主机通过SLAM系统获得小车的运动特征，并将此运动特征传递给从机以实现反馈调节，提高AGV小车初始姿态和运行过程精度。

图2 硬件框架图

3 软件系统改进

3.1 SLAM框架介绍

如图3所示为整体视觉SLAM框架，视觉SLAM框架包括传感器数据模块、前端视觉里程计模块、后端非线性优化模块以及回环检测模块。

图3 SLAM框架

传感器数据模块：包括数据采集模块和数据预处理模块，深度传感器采集到深度图像数据之后，将两个不同传感器采集到的深度图像进行配准，然后将配准后的数据信息传送到前端视觉里程计和回环检测。

前端视觉里程计模块：视觉里程计是视觉SLAM前端的主要构成部分，根据图像帧的帧间关系还原相机的位姿变化，从而给后端模块提供较好的初值。本文采用ORB特征点法。特征点由关键点和描述子组成，关键点称为“Oriented FAST”，是一种改进的FAST角点，描述子称为“BRIEF”。关于特征匹配，采用快速近似最近邻算法，这种算法不仅可以满足SLAM中的实时性需求，也适用于匹配点数极多的情况。

后端非线性优化模块：后端的任务为使用非线性误差函数优化位姿图中相机的位姿信息。

回环检测模块：回环检测的前提是相机运行轨迹构成回环，SLAM系统通过图像帧之间的相似度来判断是否产生回环，本框架中采用词袋模型进行回环检测，设一幅图像为m，他所有单词数为n，则该图像的描述可如式（2）所有。其中为图像中出现的单词，为该单词对应的权重。具体如图4所示。

图4 回环检测示意图

则图像S1和S2之间的相似度可以记为：

后端非线性优化模块：接受不同时刻前端视觉里程计测量的相机位姿，以及回环检测信息，得到全局一致的轨迹和地图；处理SLAM中的噪声问题，后端优化要考虑的是如何从带有噪声的数据中，估计整个系统的状态，以及这个状态估计的不确定性有多大——这称为最大后验概率估计，这个状态包括机器人自身轨迹也包含地图。该块主要是滤波与非线性优化算法。

3.2 深度相机配准

近年来，随着价格亲民的Kinect、RealSense等深度相机的出现[3]，研究者们可以方便地获得深度图像数据，因此，基于深度信息的场景建模越来越得到广泛的应用。本文所采用的深度相机为Intel RealSense L515深度相机[4]和乐视体感深度相机，通过深度相机获取到的三维数据，可以准确得知图像中各个像素点与相机之间的距离，从而获取各点在三维空间上的位置坐标，最后通过三维坐标位置进行场景建模[5]，还原真实的小车环境。

由于两个深度相机之间存在空间位置与角度的物理偏差，从而使得采集到的深度图的像素位置不一致，深度图像之间存在不对齐的现象，导致目标物的深度信息不准确。为了实现反馈调节，以达到更高的运动精度，需将两个深度相机获取的深度图像进行像素级配准。

深度配准的算法可以选择以其中一幅深度图为目标。配准算法的原理图如图5所示，将深度图一的图像坐标系进行转换与深度图二的图像坐标系进行统一。首先，将深度图一的像素点映射到深度坐标系下，直接获取深度图一坐标系与深度图二坐标系之间的欧式变换矩阵Td2c；其次，将深度图二坐标系下的深度点投影到Z=1的平面上。具体算法如下式（3）所示：

图5 深度图配准原理图

式中：Td2c表示深度图一坐标系与深度图二坐标系的转换矩阵；Tw2c表示世界坐标系到深度图二坐标系的转换矩阵；Tw2d表示世界坐标系到深度坐标系的转换矩阵。根据此公式可以将深度图一与深度图二进行配准，能够得到更准确的深度数据，准确而有效地补偿了深度图一与深度图二之间的像素偏移量。

3.3 主从机通信

对于软件系统方面，主机通过SLAM系统得到小车的运动特征，将此运动特征传递给从机以实现反馈调节，从而提高AGV小车初始姿态和运行过程精度。主机与从机的之间通信功能是基于ROS系统的Rosserial模块实现的。具体交互流程如图6所示，本文使用Rosserial_server功能包。首先从机安装Rosserial-Server和Rosserial-Windows，再生成与ROS主设备对话所需的代码，在消息信息节点侧启动主服务器节点；然后在主机上编写客户端通讯代码，使得ROS侧与Windows端都可以看到订阅话题中的信息。至此，主机和从机完成通信，ROS端可接收到Windows端发送的小车运动特征信号。主从机之间的通信协议如表1所示。

表1 PC-树莓派之间的通讯协议

图6 交互流程示意图

4 实验与结果分析

根据本文提出的改进方法，在实际应用场景下进行了仿真实验，记录并绘制了AGV小车的运行轨迹，轨迹图如图7所示。从图7中可明显看出小车运行精度较改进前总体上有了较大的提升，更加贴合预定轨迹。

图7 仿真试验结果图

5 结语

本文所设计的基于3D点云SLAM的AGV小车系统具有良好的理论与应用价值。一般情况下RGBD相机的视野角小于180°，采用两个RGBD相机不仅可以起到扩大观察角和视野的作用，两个RGBD相机获取的小车姿态信息还可以相互配准，实现反馈调节，从而可以达到更高的运动精度。此外，本文使用的主从机控制方法可以规避ROS系统无法处理大量3D点云数据的弊端，利用主机可以更加快速地处理数据和建模，并将处理结果实时传输到ROS系统，从而提升定位精度。