基于激光SLAM的牛场智能推翻草机器人自主导航系统

宋怀波 段援朝 李 嵘 焦义涛 王 政

(1.西北农林科技大学机械与电子工程学院，陕西杨凌 712100；2.农业农村部农业物联网重点实验室，陕西杨凌 712100)

0 引言

近年来，奶牛养殖的现代化、规模化、标准化以及产业整体效率和素质水平显著提高，已经从家养等小规模养殖向农场化的现代新型养殖模式转型[1-2]。但是多数养殖场在奶牛饲养方面，仍然依赖传统机械设备，导致养殖场产能低下、奶牛发病率上升[3-4]。因此，研发一款适用于牛场的智能推翻草机器人，对牛场向现代化、智能化转型具有重要的意义[5-6]。自主导航技术是推翻草机器人的核心，现有导航如导轨式、磁条引导式或视觉导航方式等难以适用于复杂牛场环境，尚需进一步研究一款适用于牛场推翻草机器人的导航系统。

目前应用于农业半开放式环境的导航机器人的导航方式可分为依赖外界信息辅助型导航与自主探索型导航[7-9]。依赖外界信息辅助型导航方式主要有磁条导航、二维编码导航等。万畅等[10]提出了一种基于磁条导航方式的饲料推送机器人，机器人通过传感器检测地面预先铺设的磁条信号来引导机器人移动作业。ZHANG等[11]基于双磁条导航机器人设计了双输入单输出型模糊控制器，提高了磁条导航机器人在移动过程中的位姿精度。磁条导航具有精确度高、成本低等特点，但后期维护繁琐，路径改变困难，无法满足任务实时变化的应用场景。二维编码导航通过机器人视觉系统扫描地面二维编码，通过二维编码信息获取机器人当前位置信息[12-13]。KRAJNIK等[14]提出了一种基于单目视觉识别二维编码的机器人定位导航算法，根据二维编码信息建立环境地图从而实现机器人定位导航。该方式在大场景环境中精确度较高，路径改变容易，但对于牛场复杂环境，二维编码易覆盖，需要定期维护，维护成本较高。

自主探索型导航通过激光雷达、视觉传感器、北斗卫星导航系统或其他卫星导航系统等多源数据融合，对外界环境精准感知进行导航定位[15-18]。徐波等[19]基于北斗卫星定位系统，提出了一种与惯性测量单元等多传感器融合定位算法，实现了机器人在室外空旷环境中导航。毛文菊等[20]提出了一种基于激光雷达与导航卫星系统(Global navigation satellite system, GNSS)相融合的果园运输机器人，有效提高了运输机器人在果园工作的效率。CIOFFI等[21]提出了一种基于紧密耦合的非线性优化估计器，将全球定位系统(Global positioning system, GPS)与视觉和惯性导航测量信息融合，有效解决了视觉与惯性导航在长期导航中积累的漂移问题。但北斗导航系统、GNSS或GPS传感器在牛场半开放式环境中信号弱且易丢失，不适用于牛场环境机器人导航。张振乾等[22]提出了一种基于双目视觉导航的香蕉园巡检机器人，但视觉传感器受光线变化影响较大，在复杂牛场环境中的导航效果尚需验证。激光雷达作为新型传感器，具有分辨率高、抗干扰能力强、响应速度快等优点，因此在牛场环境中，使用激光雷达作为机器人感知环境的主要传感器，可以实现推翻草机器人在牛场环境中精准导航[23]。

针对牛场半开放式复杂环境，本研究研发一种基于激光同步定位与建图(Simultaneous localization and mapping, SLAM)的牛场智能推翻草机器人导航系统。该机器人在创建牛场环境地图过程中采用Cartographer算法，利用自适应蒙特卡洛定位(Adaptive Monte Carlo localization, AMCL)算法将激光雷达、姿态惯性传感器(Inertial measurement unit, IMU)和编码器三者信息融合进行定位，提高牛场环境地图的创建精度。机器人在导航过程中使用迪杰斯特拉算法(Dijkstra)算法，利用AMCL算法将激光雷达与IMU两者信息融合进行定位，以解决机器人在推翻草过程中由轮胎打滑造成的编码器数据误差问题，从而实现机器人在牛场复杂环境中精准导航，进而完成推翻牧草任务。

1 智能推翻草机器人导航系统设计

1.1 工作环境

奶牛在饲喂过程中，首先需要将牧草与精饲料按照一定的比例混合均匀[24-25]，投置于围栏底部进行饲喂。但奶牛在进食过程中会因挑食精饲料出现“打窝”现象[26]，如图1所示，这一现象将导致饲料被推向远离围栏的位置，使奶牛无法正常进食，造成食物浪费及奶牛摄食不足。牛场智能推翻草机器人的主要功能是实现牧草与精饲料的再次充分混合，并推、翻至围栏底部奶牛饲喂区域，保证奶牛及时地摄入饲料。

图1 奶牛饲喂环境

机器人在推翻草过程中，需沿着饲喂区域边界进行运动。但基于饲喂区域外部饲料外疏内密的分布特点，使机器人在移动过程中左右轮阻力分布不均，出现轮胎打滑现象，进而影响机器人导航精度。因此，可靠的导航系统是机器人自主完成推翻草任务的关键。

1.2 机器人总体系统组成

牛场智能推翻草机器人总体系统组成如图2所示，主要包括自主导航移动平台、推翻草装置、清扫装置与控制系统。自主导航移动平台基于差分式驱动模型设计，通过直流编码减速电机驱动。该平台长1 200 mm，宽750 mm，高600 mm。推翻草装置主要由螺旋输送器、螺旋输送器支架、螺旋输送器减速电机及升降电机组成。该装置安装于自主导航移动平台前端，主要实现牧草中粗精饲料充分搅拌并推翻至饲喂区域功能。

图2 机器人总体系统组成

清扫装置由边扫和边扫减速电机组成。该装置采用错位安装、同向旋转的方式，实现遗留饲料的清扫功能。机器人控制系统主要由微型计算机、Arduino单片机及电机驱动板等组成，主要负责机器人导航、推翻草装置及清扫装置的控制。同时微型计算机可以与主机PC或手机进行信息交互。PC或手机向机器人远程下发机器人导航的目标位置坐标(x,y,θ)，其中x与y分别为机器人在牛场环境地图中的横坐标与纵坐标，θ为机器人到达目标位置时的姿态角。当机器人到达目标位置后，上位机以话题消息的形式与下位机进行通信，下位机通过继电器工作驱动推翻草装置电机旋转，从而完成推翻草任务。

1.3 机器人硬件结构设计

智能推翻草机器人硬件主要包括：场景感知模块、控制系统、电源模块、执行系统和通信模块。图3为智能推翻草机器人硬件系统组成。

图3 机器人硬件组成框图

1.3.1场景感知模块

场景感知模块主要包括激光雷达、电机编码器和IMU模块。激光雷达用以采集牛场环境地图点云数据，通过激光点束旋转测距依次获得各个角度上的雷达数据，进而形成激光雷达所处平面的点云数据。激光雷达型号为RPLIDAR-S2，测距半径为30 m，测量频率为30 000次/s，采用飞行时间法(Time of flight, TOF)，TOF测距原理为根据激光器发射和接收激光脉冲的时间差与光速乘积的1/2计算出激光雷达与障碍物之间的距离。对于牛场大范围开放式环境，TOF测距型激光雷达能够保证较高的测量精度，减小测距误差。采用BKH3808A-100-L3-2M型电机编码器，每旋转一周发送100个脉冲信号。电机编码器采集电机旋转的圈数，从而计算出机器人移动速度。

IMU模块采用HFI-B6型，模块中通过加速度计测量机器人运动的加速度，使用陀螺仪测量机器人运动的角速度。通过6轴姿态融合算法进行机器人的当前姿态计算。首先利用四元数将陀螺仪基于机体的旋转数据转化为世界坐标，其次通过加速度计的输出修正陀螺仪数据漂移误差，最终经过积分计算机器人当前姿态各方向上的角度与线速度。

1.3.2控制系统及其他模块

控制系统模块中利用Inter NUC微型计算机作为上层控制器，负责建图、导航及与主机PC或移动端之间进行多机通信功能。Arduino Mega2560单片机作为导航系统的底层控制器，驱动执行系统模块，使机器人各个装置电机有序工作。电源模块为各模块供电，保证各装置正常工作，为机器人精准导航、平稳工作奠定基础。

1.4 机器人导航软件系统设计

牛场推翻草机器人导航软件系统如图4所示，主要包括控制层、数据处理层及驱动层。控制层基于Linux系统实现推翻草任务下发及机器人工作状态监控，推翻草任务下发即PC或手机向机器人依次发送目标点坐标，通过多点导航方式实现推翻草任务。工作状态监控可查看机器人当前位置。数据处理层基于Ubuntu 18.04 LTS微型计算机开发，搭载机器人操作系统(Robot operating system, ROS)[27]，并安装Cartographer、AMCL和Dijkstra算法[28-29]。Cartographer算法根据激光雷达传感器数据、IMU传感器和里程计信息创建牛场环境地图，AMCL算法根据里程计数据和IMU信息确定机器人在当前地图中位置，Dijkstra算法规划机器人工作路径。

图4 机器人软件结构

下位机Arduino主控板使用ROS Arduino Bridge算法，实现驱动直流编码减速电机、编码器4倍频测速、比例积分微分(Proportion integration differentiation, PID)控制及与微型计算机通信等功能[30]。ROS Arduino Bridge算法中的PID控制器根据机器人实际运行速度与目标线速度和角速度，进行闭环控制驱动轮电机旋转，达到对自主导航移动平台的精准控制。

2 机器人导航功能实现原理

2.1 牛场环境地图构建

在推翻草机器人导航系统设计中，基于谷歌公司发布的Cartographer 2D SLAM算法构建牛场环境地图[31]。Cartographer算法流程如图5所示，主要分为局部SLAM和全局SLAM两部分。局部SALM主要根据激光雷达数据建立一系列子地图Submap，全局SLAM中结合传感器数据，通过扫描匹配将一系列Submap连接生成全局地图。

图5 Cartographer算法流程图

Cartographer算法中数据来源包括激光雷达、里程计数据和IMU数据。激光雷达数据首先通过Voxel滤波器消除测量误差及噪点，但由于激光雷达数据具有远密近疏特点，使用自适应体素滤波器(Adaptive voxel filter)进行自适应计算合适的采样间隔，获取一定的点云数量。又因为机器人在运动过程中，激光雷达数据会产生运动畸变，需要使用里程计数据和IMU数据矫正激光畸变。

局部SLAM中，每次得到一帧激光点Scan数据之后，与其最近的Submap进行匹配，找到最佳位姿，用以构建 Submap。激光点Scan插入到Submap中需进行坐标转换，其转换公式为

(1)

式中ξx——激光点横坐标

ξy——激光点纵坐标

ξθ——激光点束击中点的观察夹角

Tξ——激光点在Submap坐标系下位姿

p——栅格存在障碍物的概率

一定数量的激光点束构建一个栅格地图Submap，当有一个新的Laser Scan插入Submap时，将对其栅格概率进行更新。

在将激光点云插入Submap之前，使用基于Ceres的扫描匹配器对激光点云相对于当前Submap位姿(ξx,ξy,ξθ)进行优化，找到一个激光点云对应最优的位姿，使Submap中栅格概率最大化。最优位姿是通过计算激光点云与当前Submap匹配程度的误差，使用Ceres扫描匹配器进行迭代位姿使其误差最小。本文使用残差函数来计算匹配程度，残差函数为

(2)

式中Msmooth——平滑函数

hk——激光点云数量

全局SLAM中，通过回环检测和回环优化将Submap形成一幅完整地图，消除Submap之间的累计误差，具体实现过程可参考文献[32]。检测回环过程中，如果当前的Scan和已创建完成的Submap中的某个激光点Scan的位姿相似，则认为检测出回环。为了降低检测的计算量，提高回环检测的效率，Cartographer算法应用分支定界的方法进行优化搜索策略。回环优化过程中，使用稀疏位姿的方法来优化激光点云和子图的位姿。回环优化过程与激光点云插入Submap过程类似，仍使用残差函数计算其误差。

2.2 机器人自主导航实现原理

机器人自主导航分为机器人定位和路径规划两部分。机器人定位采用AMCL算法，实现机器人在二维地图中定位，机器人采用Dijkstra实现路径规划[33-34]。

AMCL定位算法基于粒子滤波，确定机器人在当前已知牛场环境地图中的位置。AMCL算法结构如图6所示，AMCL算法首先将代表机器人位姿的粒子随机分布于已知环境地图中，其次通过IMU数据或使用扩展卡尔曼滤波融合IMU传感器与里程计数据，建立运动模型预测所有随机粒子的位姿，其中运动模型为

图6 AMCL算法结构图

xt=ft(xt-1,ut-1)

(3)

式中xt——机器人t时刻的运动状态

xt-1——机器人t-1时刻的运动状态

ut-1——控制噪声

最终根据激光雷达数据建立测量模型来更新所有随机粒子的权重，粒子最终的收敛位置即为机器人在地图中的实际位置。若机器人初始位姿未知或有误，此时全局将注入更多随机粒子，随机器人的移动粒子再次更新，重新定位机器人在牛场地图中的位置。

Dijkstra路径规划算法基于单源最短路径算法计算从机器人起始位置到目标位置的最短路径。该算法采用广度优先搜索策略，从起始位置开始遍历到起始点最近且未访问顶点的邻接节点，遍历过程中不断迭代当前距离起始位置最近的点，直至遍历到目标位置。

图7为机器人导航框架，使用ROS中所提供的AMCL与Move_base功能包来实现定位和路径规划[35]功能。首先控制机器人在牛场环境中运动，使用Cartographer算法建立牛场环境地图，作为全局代价地图。其次机器人根据主机PC或手机APP下发目标点，使用Dijkstra算法进行全局路径规划，计算机器人到目标位置的最佳路线，作为机器人的全局路线。最后在实际机器人推翻草作业时，为了避免机器人与可能出现的动态障碍物发生碰撞，根据激光雷达实时数据引入本地代价地图，由local_planner模块通过Dynamic Windows Approaches算法计算躲避的多条轨迹，对于每条模拟的运动轨迹分别计算轨迹上是否存在障碍物以及与障碍物的距离的分值CostsObstacle、轨迹上点与局部参考路径最近距离的分值CostsPath和轨迹上点与局部参考路径终点最近距离分值CostsGoal，综合评分计算公式为

图7 机器人导航框架

Score=αCostsObstacle+βCostsPath+

ΓCostsGoal

(4)

式中α、β、Γ——各评分标准的权重

其权重取决于机器人所侧重的性能，通过试验，α、β、Γ分别取值为0.02、0.1和2。通过比较，选择综合评分最小的轨迹作为机器人下一时刻的运动轨迹，并计算该轨迹上机器人前进的线速度与角速度，通过USB串口发送至下位机Arduino主控板，从而实现自主导航移动平台的精准控制。

若机器人在导航过程中出现了障碍物，根据栅格代价地图进行检测障碍物与机器人能否发生碰撞。机器人与障碍物距离关系如图8所示。

图8 代价地图栅格分类图

在代价地图中，根据机器人与障碍物之间距离关系计算其栅格灰度，若栅格灰度大于253，表示机器人与障碍物必然发生碰撞；若栅格灰度处于[128,252]，则表示机器人处于碰撞临界，可能发生碰撞；若栅格灰度处于(0,127]，表示机器人此时处于危险警戒区，未来可能发生碰撞；若栅格灰度为0，表示机器人可以自由越过障碍物。

3 性能测试试验

牛场智能推翻草机器人可靠性取决于机器人导航系统的精度，高精度的导航系统是推翻草机器人能否可靠稳定地完成推翻草作业的前提。推翻草机器人导航性能测试试验地点在西北农林科技大学畜牧实验基地，牛场试验区域纵向尺寸为72.0 m，横向尺寸为8.7 m。图9a为手机APP控制推翻草机器人建立牛场环境地图过程，图9b为机器人进行推翻草任务场景。

图9 机器人建图及推翻草试验

推翻草机器人性能的评价指标包括建图准确度和导航精度。建图准确度衡量标准为所建平面地图与牛场实际环境之间纵向与横向的几何偏差；导航精度的衡量标准为测量机器人预规划的导航路径与机器人实际运动路径的偏差，研究该导航系统精度能否满足机器人推翻草作业的需求。

3.1 牛场环境地图精度对比

机器人在推翻草作业时，由于不同区域的牧草料分布不均匀，造成了机器人在行驶过程中负载不稳定，出现机器人轮胎与地面打滑现象，最终将导致里程计数据误差。因此在本试验设计中，使用 Cartographer算法，分别加载里程计信息与未加载里程计信息构建牛场环境地图。为避免机器人搭载的硬件误差、地面不平整等不可控因素干扰，机器人选择直线行驶方式，以速度0.25 m/s创建环境地图。

创建的环境地图如图10所示，其中红色实线部分为牛场围墙参考线，绿色虚线部分为围栏参考线，红色箭头为机器人运动方向。在所建的两幅环境地图中，分别取8个相对应的关键点A1、B1、C1、D1和A2、B2、C2、D2，利用Rviz软件中Measure标尺工具测量两幅地图每两个关键点之间距离，与实际建图区域尺寸进行对比，结果如表1所示。

图10 Cartographer算法建图精度对比

表1 牛场环境地图相对误差

图10表明，机器人在未加载里程计信息时，所建的牛场环境地图在横向上出现明显的弯曲，并且随着机器人移动距离的增加，地图横向弯曲越明显。而加载里程计信息进行创建地图时，横向未出现弯曲情况。

由表1可知，机器人使用Cartographer算法在未加载里程计信息情况下，仅使用激光雷达与IMU传感器建立牛场环境地图，与实际环境纵向尺寸偏差最大为2.18 m，横向尺寸偏差最大为0.06 m。使用Cartographer算法在加载里程计信息时，所建地图与实际环境纵向尺寸偏差最大为0.14 m，横向尺寸偏差最大为0.02 m，横纵向尺寸偏差均小于未加载里程计信息时的横纵向尺寸偏差。

上述偏差结果表明，加载里程计数据与IMU传感器辅助激光雷达创建的环境地图，横纵向尺寸偏差均小于仅使用IMU传感器数据辅助激光雷达创建的环境地图。

3.2 机器人导航精度测试试验

推翻草机器人在牛场进行推翻草作业时，需要沿着待推翻草区域进行直线行驶。路径规划如图11a所示，其中灰色部分为奶牛的饲喂区域，红色部分表示机器人工作时运动轨迹，机器人将沿着A→B→C→D→E→F→G→H路线进行行驶。考虑到机器人在推翻草作业时，前进方向上会受到牧草阻力，机器人轮胎与地面可能会存在打滑现象，因此在试验设计上，使用AMCL定位算法及Dijkstra路径规划算法，将有无加载里程计信息设置为对照组试验，探究两种情况下推翻草机器人推翻草过程中的导航精度。

评价机器人导航精度指标包括横向偏差、纵向偏差、航向偏角[36]。机器人导航精度测量方法如图11b所示，设置机器人起点为坐标原点，以牛场纵向设为y轴正方向，遵循右手直角坐标系。将机器人导航至目标位置的坐标设置为(x0,y0)，以机器人激光雷达安装位置为测量基准点，以机器人前进方向为y轴正方向，以机器人正右方为x轴正方向建立直角坐标系。机器人行驶至目标位置，记录其基准点坐标位置为(x1,y1)，其中横向偏差X计算公式为

图11 推翻草机器人导航路径及导航精度测量示意图

X=|x1-x0|

(5)

纵向偏差Y计算公式为

Y=|y1-y0|

(6)

航向偏角为机器人正方向与y轴之间的夹角。试验中分别以加载里程计信息与未加载里程计信息设为对照组试验，第1组为加载里程计信息进行导航，第2组为未加载里程计信息进行导航，每组分别试验15次。试验结果如表2所示，根据试验数据分别计算各评价指标的平均值与标准差。

表2 导航精度试验结果

表2表明，机器人在推翻草作业时，加载里程计信息导航，横向偏差最大值、纵向偏差最大值及航向偏角最大值分别小于0.600 m、0.160 m和11.0°，标准差分别小于0.040 m、0.050 m和3.0°；未加载里程计信息时，横向偏差最大值、纵向偏差最大值及航向偏角最大值分别小于0.040 m、0.100 m和11.0°，标准差分别小于0.030 m、0.025 m和3.0°。表明机器人在未加载里程计信息进行导航时，精度明显高于加载里程计信息进行导航，满足牛场复杂环境中推翻草作业自主导航的需求。

未加载里程计信息导航时，横向与纵向偏差平均值均小于加载里程计信息时的横向和纵向偏差平均值，航向偏角误差基本相同。出现这种现象的原因为机器人在推翻草作业时，由于推翻草负载的存在，轮胎会与地面出现轻微打滑现象，导致里程计数据误差随着机器人移动距离的增加而累计增加，进而影响导航过程中纵向偏差。而机器人未加载里程计信息进行推翻草时，机器人导航精度明显增高。

4 结论

(1)针对牛场奶牛推送饲料机械设备智能化低、人工劳动强度大等问题，设计了一款基于激光SLAM的自主导航推翻草机器人。试验表明该机器人建图及导航精度能够实现在牛场环境中推翻草作业，提高了牛场饲养奶牛的智能化水平。

(2)牛场环境地图精度对比试验结果表明，机器人使用Cartographer算法加载里程计信息，以速度0.25 m/s创建地图时，所创建的地图与实际环境的纵向尺寸偏差最大为0.14 m，横向尺寸偏差最大为0.02 m，地图精度明显高于使用Cartographer算法未加载里程计信息情况。

(3)机器人导航精度测试试验结果表明，机器人未加载里程计信息进行导航时，横向偏差最大值、纵向偏差最大值及航向偏角最大值分别小于0.040 m、0.100 m和11.0°，标准差分别小于0.030 m、0.025 m和3.0°，导航精度高于加载里程计信息时的导航精度，满足机器人推翻草作业时的导航精度要求。