基于视觉SLAM的履带传动式智能拾蛋机器人

王志豪，李锐圳，韩利芬，黄永欣，邓 君

（东莞理工学院机械工程学院，广东 东莞 523000）

0 引言

随着国家的发展，我国即将踏入中等发达国家行列，而我国养殖业在农业中的比重还是比较低。近几年随着国家的政策扶持，我国的农村养殖业明显增多。但是在这些农村养殖业中，养殖的自动化水平普遍较低，甚至还有许多的农户纯人工养殖。蛋类商品家禽养殖和大多数养殖业一样，同样是自动化水平低。蛋类商品家禽，以蛋鸭为例，个体户养殖蛋鸭的过程中一般会选择一次养殖5000～6000只，而在蛋鸭的产蛋期内，蛋鸭群平均每天的产蛋率能达到70%以上，在这样的情况下，一天需要拾取的鸭蛋数为3500～4200个，而且还要进行装箱称重，再拿出进行装袋。因为劳动量大，所以一般都会在产蛋期雇佣他人帮忙拾取鸭蛋，这也就减少了养殖的利润。

SLAM技术有着非常广阔的前景，无人驾驶技术也有涉及到该种技术，该技术在国内外都很流行。但在工业车间、农业生产中，未能很好地普及该技术，享受该技术所带来的便利。所以本文设计了一款应用于农村养殖业的智能拾蛋机器人。所设计的智能拾蛋机器人运用目前主流的视觉SLAM技术和机器人操作系统（ROS）来实现机器人在养殖场内进行路径规划和导航，相对于其他可供参考的拾蛋机器人[1]，更加注重于智能化实现。参考其他智能导航机器人[2]，设计的智能拾蛋机器人应用在养殖场环境中，其搭载着深度摄像头，来对周围环境进行采集，用内置的主控板和驱动板来处理和驱动机器人行驶。其任务是减少劳动量，帮助人们大量拾取蛋类，增加其养殖利润。

1 机器人整体设计

1.1 组成部分

智能拾蛋机器人在目前市场上并没有相同或相似的产品。其整体由电脑端和机器人端组成。电脑端主要负责启动程序运行，监看机器人端的执行情况。机器人端由视觉传感器、控制部分、拾捡部分、底盘驱动部分等组成。

1.2 系统设计

电脑端远程启动机器人端进行建图。开始时，由人手动控制电脑端来控制机器人行走进行地图的建立，由视觉传感器采集的图像经由树莓派处理得到养殖场整体的地图。在建立好地图后，机器人端进行路径规划。机器人端通过建立的地图进行路径规划处理，设计合适的路径大致走遍养殖场。且机器人在沿着路径规划设定的路线行走时，对鸭蛋进行拾捡。机器人的组成框架如图1所示。机器人整体如图2所示。

图1 组成框架 图2机器人整体

2 机器人硬件结构

2.1 视觉传感器

视觉传感器部分采用奥比中光OrbbecAstrapro深度摄像头。该摄像头可以采集1080 p的彩色图像信息，且通过发收红外光源来采集深度图像信息，深度范围为0.3～1 m，深度图像分辨率为1280×720。该摄像头延迟低，可以很好地应用在视觉SLAM中。

2.2 控制部分

机器人的控制部分由树莓派4B（4G）主控板组成。树莓派主控板相当于小型的电脑，可以实现部分电脑功能，有I/O口和usb口供与外部输入输出。树莓派体积小，算力充足，可以用于在小型机器人上接受和处理图像信息。

树莓派主控板中使用ubuntu18.04LTS（Linux）系统，并安装了ROS（Robot Operating System）机器人操作系统，以控制机器人。树莓派也内置有WiFi模块，用来与电脑端通信。树莓派与底盘单片机通信是用usb接口来实现的。

2.3 底盘驱动

2.3.1 电源的选择

本机器人采用双电池供电。机器人整体质量较大，所需的编码器电机的扭矩较大，所以采用12 V锂电池来给驱动电机和拾捡装置的电机供电。由于树莓派运行视觉SLAM和ROS，并进行路径规划，所需的计算量较大，耗电较高，故使用另外的电池对其进行独立供电。采用12V锂电池转接DC-DC降压模块（12 V转5 V）再给树莓派进行供电。

2.3.2 电机和驱动板的选择

为了精确行驶，采用了带霍尔编码器直流减速电机（减速比1∶30），该电机扭矩大、可靠性强。在电机中使用霍尔编码器可以反馈得到电机的转速，从而进行闭环控制电机速度，更好地控制机器人行驶。驱动板选择L298N电机驱动板来驱动电机转动。

2.3.3 车轮的确定

车轮采取履带式设计并加弹簧减震以实现机器人在不规则地面上的正常行走，由电机带动齿轮带动履带前进。履带的着地面积大，可以减小机器人对养殖场地面的压强，并能增加牵引能力，能单次承载更多的鸭蛋。也便于在养殖场松软泥泞或不平的地面上行驶。

2.3.4 驱动控制

树莓派主控板发出的信息传输给底盘的Arduino单片机来控制各电机的运动。Arduino单片机通过单片机板上的I/O口来输出PWM信号传输给L298N电机驱动板来驱动电机。

2.4 拾捡部分

机器人前的拾取装置为履带传动式鸭蛋拾捡机械结构，其上板与底板均使用亚克力板，其中部使用长螺丝进行支撑，支撑点处加有轴承以实现轴的自由转动。上板处加有直流电机用以驱动拾取装置的运行，该直流电机可以通过调节输入的PWM值来实现电机转速的控制，从而控制鸭蛋拾取装置的拾取速率。

拾取装置结构如图3～5所示。拾取装置工作时，电机转动驱动齿轮组的转动，齿轮组通过连接件与履带驱动齿轮相结合，经过齿轮组的传动实现左右履带驱动齿轮共速，从而实现左右履带共速相向转动。左右履带上每个一定距离均固定一挡板，每块挡板均为厚度为2 mm的亚克力板，其外形如图6所示，挡板面上贴有一定厚度的海棉以实现行进拾取过程中对鸭蛋的缓冲作用，另外，挡板设有30°缺角，在实现挡板长度加长的同时使得挡板工作时能够贴近地面。

图3 拾捡结构

图4 拾捡结构正视图

图5 拾捡结构俯视图

图6 挡板

当履带运转时，挡板在履带的带动下转动，左右对称挡板位置存在一定差异以实现两板交汇时能有一定长度上的重合，当左右挡板交汇时两板会共同将位于交汇点的鸭蛋带入装置，在左右挡板重合时鸭蛋被带到拾蛋机器人底盘位置，并在左右挡板结束重合时被带入机器人的储蛋空间中，从而达到拾蛋的目的。拾蛋机器人行进过程的同时拾取装置持续转动，因此只需确定鸭蛋所在方位便能在机器人行进的同时实现对一个或多个鸭蛋的拾取，缩短了拾取大量鸭蛋所需时长。

3 软件程序设计

3.1 ORB-SLAM2算法

SLAM（Simultaneous Localization and Mapping）技术用于解决将一个机器人放入未知环境中的未知位置，是否有办法让机器人一边移动一边逐步描绘出此环境完全的地图。对于养殖场的未知环境，采用SLAM算法来用于建立地图、进行导航是有效的。对于采用深度摄像头（RGB-D）进行视觉SLAM技术，目前主流采用的视觉SLAM技术 有RGB-D_SLAM_V2、ORB_SLAM、RTABMap这3种方法。本文运用的是ORB-SLAM2来进行运动估计和建立地图，ORB_SLAM2系统功能齐全，可以很好地在CPU上运行，各检测环节使用同一特征，较其他方法有更高的效率和精确度，对运动估计和建立地图的效果更好[3]。

ORB-SLAM2采用3个主要线程：跟踪、局部建图和回环检测以及一个额外线程（全局BA）。包括基于ORB（Oriented FAST and Rotated BRIEF）图像特征的词袋（Bag of Words，BoW）用于位置识别及回环检测、信息关联视图，用G2o图优化来作为通用框架[4]。对于大范围的地图构建，该算法应用了尺度感知的回环检测。该算法在全部的处理当中只用到了ORB来作特征点的检测和描述，这样一来提高了算法在位置识别和回环检测中的实现效果。ORB-SLAM2中每个模块中都采用ORB描述子，而不像之前的其他系统那样跟踪和回环检测使用不同的特征点，这种做法一方面是节省提取时间，另一方面可以使得整个系统更加简洁。

通过提取图像特征点，与上帧图像的特征点匹配，来得出相机的位姿，计算得出视觉里程计[5]。图7所示相机图像上的绿色部分为从当前提取的特征点，实时建立稀疏点云地图。图中的红/黑色点云（红色为局部地图点，黑色为全局地图点）为地图点，蓝色框为相机位姿。将ORB-SLAM2文件修改，将其与ROS系统结合，将相机的位姿和里程计生成和发布节点，用于后面的建图和导航。ORB-SLAM2的系统框架如图8所示。

图7 特征点提取

图8 ORB-SLAM2系统框架

3.2 建立地图

3.2.1 稀疏点云地图

在ORB-SLAM2中通过选取的关键帧来构建的稀疏点云地图（图9）对外部环境的描述较少，所获取的信息不足以支持用于机器人的路径规划和导航，所以需要在ORB-SLAM2中新增加一个线程来对深度摄像头所探测到的外部环境进行稠密建图，获取更多的信息。

图9 实时建立的稀疏点云地图

3.2.2 稠密点云地图

通过ORB-SLAM2发布的节点信息执行稠密点云功能包来实时建立稠密点云地图。可以在ROS系统中的RVIZ图像化工具来查看实时生成的稠密点云地图。稠密点云地图的建立流程如图10所示。图11所示为实时生成的稠密点云地图[6]。

图10 稠密点云图建立流程

图11 实时建立的稠密点云地图

稠密点云地图不能直接用于导航，其本质上是一堆点的集合，数据过于冗余，无法大规模使用，要对其进行去噪和cluster处理（集群技术）才能使用。比较流行的方式是使用3D稠密点云地图处理生成八叉树地图（Octomap）[7]。

3.2.3 八叉树地图

八叉树地图较其他地图灵活，压缩率高，又可以随时更新。该种地图是由其最大递归深度不断地进行分割来实现的，如图12所示[8]。由于其为树状结构，子节点是否被占据是通过概率来表达的，但其不确定时，使用父节点来进行表达，这样可以节约很多的空间。

图12 八叉树地图

在ROS中有基于八叉树地图的功能包，可以将接收的稠密点云信息实时转为八叉树地图信息，如图13所示。八叉树地图相对于稠密点云地图所需的内存大大缩小，更便于用在机器人的导航上[9]。使用八叉树地图进行压缩滤除地面信息，再通过2D投影生成栅格地图，处理后生成代价地图（Costmap），最后利用代价地图进行全局和局部路径规划，来进行实时避障和导航。图中底部白色部分为投影的2D栅格地图。

图13 实时建立的八叉树地图

3.2.4 代价地图

代价地图的生成需要确定划分栅格的大小，对栅格的标识和对栅格信息的编码。

2D栅格地图的栅格根据八叉树地图来确定其大小。代价地图的对栅格的标识方法主要用坐标标识法来进行标识。栅格信息编码主要是设定代价函数，根据与障碍的距离代入所设定的代价函数中进行编码。栅格代价分布如图14所示[10]。

图14 栅格代价分布

3.3 路径规划

3.3.1 全局路径规划

全局路径规划的算法有多种，例如Dijikstra、A*算法、rrt算法、BFS算法等。而在ROS框架下有Dijkstra和A*算法[11]。Dijikstra可以支持搜索较大面积的地图空间，该算法开始是以起点向周围扩散。而A*算法则更专注于目标，选择的是近似的最优路径，对比于其他算法更加优化。结合实际环境，该机器人选择的是A*算法。

A*算法由下列函数计算得到每个节点的优先级：

式中：f（n）为节点n的综合优先级，当选择下个需要遍历的节点时，总选取优先级最高、即其值最小的节点；g（n）为节点n与路径起点的代价值；h（n）为节点与路径终点所预测的代价值。这也是A*算法的启发函数。

3.3.2 局部路径规划

机器人在建立好了地图后，通过全局路径规划得到的算法可以得到一条路线，但对于实际环境中的变化，还是需要进行局部路径规划来进行调整，避开之前所建立的地图中所未被检测到的、或者动态的障碍。在ROS中，是基于DWA算法来进行和实现局部路径规划部分的，每当进行移动时，在ROS中内置的导航系统就会自主调用该DWA算法来分析和规划函数，来计算得出机器人的最优运动指令来控制机器人进行移动[12]。DWA算法实现的流程如图15所示。

图15 DWA算法流程

4 验证和结果分析

4.1 测试环境搭建

在实验室搭建一个大致的可供拾蛋机器人行走的室内环境，放置了障碍物和鸭蛋。障碍物主要测试机器人的建图和避障功能，随机放置的鸭蛋主要测试拾蛋机器人拾蛋结构的拾蛋能力。

4.2 软件部分

在建图和避障部分的测试，拾蛋机器人能较好地完成。在建立地图部分，由于深度摄像头的检测范围有限，并不能像激光雷达可以环绕着四周进行地图的建立，所以建图的时间长。避障方面，在大多数情况下都能很好地避开障碍，继续向规划好的路径行走，但在一些特殊的角落，没能很好地进行处理，拾蛋机器人行走缓慢、有时会卡顿。

4.3 硬件部分

拾蛋结构可以很好地拾捡鸭蛋，在拾蛋机器人前端较大的范围内遇到的鸭蛋都能很好地进行收集。收集的过程中，鸭蛋并无破损，能够很好地完成拾蛋作业。

5 结束语

本文系统地介绍了通过视觉SLAM技术结合ROS系统实现地图构建、路径规划、自主导航的具体过程和自主设计的蛋类拾捡机械装置的结构。通过各种渠道进行相关信息的收集，最后设计出了一种能够综合实现拾捡鸭蛋和路径规划的实用性强的智能机器人。能够很好地减少劳动量、节约劳动成本、提高生产效率。