基于信息融合的智能推料机器人设计与试验

张 勤 任海林 胡嘉辉

(华南理工大学机械与汽车工程学院, 广州 510641)

0 引言

全混合日粮(Total mixed rations, TMR)技术广泛应用在规模化牧场的奶牛饲喂中,饲料的定期推送是TMR饲喂过程中的重要环节,可以有效解决奶牛进食过程中,由于挑食、拱料[1]造成的料槽内饲料分布不均,饲料被拱出料槽的问题,保证奶牛正常自由采食行为。饲料的可及性十分重要[2],目前国内外主要通过人工或者机器人完成定期推料任务,人工推料劳动强度大,人畜交叉感染风险高,随着牧场无人化封闭式管理模式的推进,机器人代替人工的推料方式正在成为未来的发展方向。为此国内外学者做了大量研究工作,开发了多种推料机器人,主要有螺旋式和自转式[3-6]两种推送方法。推料机器人可以自主运行,完成全天候、定期、多次推料任务。但现有的推料机器人只能完成单一的推料功能,无法满足觅食奶牛个性化的自由采食需求,在饲料不足时无法收集散料并推送到奶牛面前,而频繁分群又会造成奶牛的应激反应。为了解决这个问题,需要研究智能推料机器人,根据觅食奶牛信息、饲料信息等,自主分析决策,实现推料、集料送料以及清料等多模式的智能推料功能,满足奶牛个体自由采食需求。

奶牛饲喂信息的快速获取、牧场环境下的准确导航是智能推料机器人的关键技术,也是实现准确、有效推料的重要保证。奶牛饲喂信息主要包括觅食奶牛的识别定位信息和饲料信息。

在觅食奶牛的识别与定位方面,基于耳标的奶牛识别方法[7-8],技术较为成熟,但存在读卡速度较慢、识别距离短以及易受干扰的问题。在耳标识别基础上,通过超宽带UWB[9-10]、无线通讯ZigBee[11]等技术,可以实现奶牛的定位,但存在成本高、精度差的问题。基于机器视觉的牛脸识别算法,相比于耳标方法,具有识别速度快、性价比高以及使用灵活等特点,应用范围逐年扩大。通过在牛舍上安装摄像头可实现奶牛识别[12],但不能获取奶牛准确的位置。张勤等[13]以饲喂区内牛颈枷上的二维码作为定位标签,采用YOLO v4模型对牛头与二维码标牌进行识别,再通过牛-码位置匹配确定觅食奶牛的位置,实现了觅食奶牛及其位置的实时监测,但牛舍环境下,二维码容易被污染,需要定期维护或更换。随着计算机技术的发展,即时定位与地图构建技术(Simultaneous localization and mapping, SLAM)因其形式灵活、适应能力强等特点在机器人定位与建图中应用越来越广泛。视觉SLAM可以提供准确的定位信息,同时其使用的图像可以用于物体识别,这为计算物体空间位置提供了新方法。已有学者开发了ORB-SLAM3[14]、LSD-SLAM[15]、S-PTAM[16]等算法。环境中的动态物体会对SLAM的位姿估计造成影响,为了提升定位精度,ZHANG等[17]在ORB-SLAM2的基础上使用Mask R-CNN去除动态物体;ZHAO等[18]使用光流法和Mask R-CNN剔除动态物体;BESCOS等[19]使用多视图几何和Mask R-CNN实现动态物体的检测。上述方法由于所采用的神经网络运行速度较慢,且采用单线程,算法间相互堵塞,很难满足导航的实时性要求。

在饲料余料信息的提取方面,通过饲料上方的摄像头估算饲料的体积[20-21],或使用神经网络估算饲料的质量[22]。也可以根据牧场工人的推料经验,通过投料后时间进行余料估算,时间估算方法简单易行、成本低、速度快。

导航的准确性是实现有效推料的重要保障,目前主要有轨道导航、磁导航、视觉导航、多传感器融合导航、SLAM导航等。轨道导航[23]和磁导航[24]方法运行稳定,但投入成本高、维护难度大。视觉导航[25-26]通过提取环境中的参照物作为基准实现导航,该方法使用灵活、适应性强。

综合国内外现状可以看出,虽然推料机器人在单元技术方面取得了一定进展,但在应用层面上,现有的推料机器人功能单一,无法满足奶牛的自由采食需求。在以上研究的基础上,开发智能推料机器人,选择牛栏上方的方形杆作为牛栏的参照物。通过YOLACT[27]实例分割网络分割图像中的奶牛、饲料等动态物体和方形杆;采用ORB-SLAM3作为定位算法,在SLAM前端利用掩膜进行动态物体剔除;实时获取觅食奶牛位置、机器人与牛栏参照物方形杆的间距;根据觅食奶牛的定位信息、投料后的时间、机器人的导航信息,选择工作模式,自主沿着设定轨迹,实现推料、集料送料、清料等多模式推料功能,满足奶牛个体自由采食需求。

1 智能推料机器人组成与工作机理

1.1 机器人系统组成

如图1所示,智能推料机器人由视觉系统、移动底盘、推板机构、无线通信系统和控制系统组成。视觉系统由双目相机和网络摄像头构成,双目相机安装在机器人正前方,实时采集机器人前方的饲喂图像,网络摄像头用于远程监测机器人的运行状况。移动底盘是机器人的载体。推板机构由推料板、推料板侧面的毛刷及驱动机构组成。推料板可以自由展开、收回以改变其姿态,实现推料、集料送料、清料等多模式推料功能。毛刷可以清洁推料板侧端和饲料区墙壁间隙内的饲料,同时也可以缓冲机器人的导航误差。在非工作状态时,推板机构可以收纳至底盘的侧面,减少空间占用。控制系统根据双目相机采集到的图像,实时获取觅食奶牛信息、机器人的位置信息,选择工作模式,控制推板机构运动;监测机器人与牛栏参照物的距离,控制机器人沿着与料槽平行的方向,按照预定轨迹运动。无线通信系统可以实现远程监测和操控机器人。

图1 机器人系统Fig.1 Robot system1.毛刷 2.推板机构 3.控制系统 4.网络摄像头 5.双目相机 6.无线通信系统 7.移动底盘

1.2 工作机理分析

推料机理如图2所示。当饲料充足时,机器人采用推料模式,如图2中AB段所示,将推料板展开到30°,沿着与料槽平行方向行走,将料槽外侧的饲料推回到料槽内。当饲料较少,且有奶牛觅食时,采用集料送料模式,如图2中的BE段,实时获得觅食奶牛的位置,推料板先调整为90°收集散乱的饲料,如C处所示,然后将饲料推送至D处的奶牛前,推料板再调整为30°将饲料卸载到觅食奶牛采食区前;如果饲料很少且没有奶牛觅食时,采用清料模式,如图2中的EF段所示,推料板角度为90°,清理饲料。饲料的多少可以根据投料后时间或者通过图像判断。

图2 智能推料过程Fig.2 Intelligent feed pushing process

为了保证高效、准确地推料,推料过程中,需要机器人的运行轨迹与牛栏或者料槽之间的距离恒定且与其方向平行,为此选择牛栏上不被遮挡的方形杆为牛栏的参照物,如图2所示,通过实时控制机器人与方形杆间距,保证机器人沿料槽平行方向进行有效推料。

2 智能推料机器人控制算法

2.1 控制算法构成

智能推料机器人的控制算法流程如图3所示。

图3 控制算法流程图Fig.3 Control algorithm flowchart

首先通过双目相机实时获取机器人前方的左目彩色图和深度图,然后执行以下步骤:

(1)基于YOLACT的目标识别与分割:通过YOLACT实例分割网络对获取的彩色图像进行目标识别,得到觅食奶牛、饲料和牛栏参照物方形杆的掩膜图像。

(2)动态物体剔除:为了提高定位精度,利用掩膜图像在ORB-SLAM3的前端剔除奶牛和饲料等动态物体。获取机器人实时位置。

(3)觅食奶牛定位:融合掩膜图像、深度图和ORB-SLAM3的定位信息获得觅食奶牛位置。

(4)机器人移动轨迹控制:利用掩膜图像和深度图实时获取机器人与牛栏参照物方形杆的间距,控制机器人行走过程中与料槽方向平行且间距不变。

(5)机器人智能推料:根据觅食奶牛位置、投料时间、机器人与牛栏间距信息控制机器人沿着与料槽平行方向行走,自主选择工作模式,改变推料板角度,实现智能推料。

2.2 基于YOLACT的目标识别与分割

为获取目标准确的轮廓,同时兼顾算法的运行速度,本文采用一阶段实例分割网络YOLACT作为识别物体的神经网络,特征提取主干网络采用DarkNet53。

为提高YOLACT模型鲁棒性,在不同光照、不同时间条件下,使用双目相机录制奶牛养殖场中机器人实际运行路线视频,视频图像分辨率为1 920像素×1 080像素,相机帧率为30 f/s。之后将视频每30帧输出一幅彩色图像,得到含2 000幅彩色图像的数据集。对图像中的奶牛、饲料和方形杆进行标注,标注结果如图4a所示,在数据集上训练网络。

图4 YOLACT图像标注与目标识别Fig.4 Image labeling and target detection of YOLACT

将训练得到的模型用于双目相机实时采集的左目图像的目标识别与分割。YOLACT网络输入为3通道的RGB图像,输出包括一个k通道的矩阵、检测目标的类别和矩形感兴趣区域 (Region of interest,ROI),其中k为检测目标的数量,矩阵的每个通道为一个二值掩膜图像,合并所有通道,得到一个包括所有检测对象的掩膜图像。实际目标识别分割结果和对应的掩膜图像分别如图4b和图4c所示。

2.3 动态物体剔除

为了实时获取机器人准确的定位信息,需要减小动态物体对机器人位姿估算造成的影响,提高SLAM的定位精度,所以在SLAM的前端对动态物体进行剔除。牛舍环境下,随机运动的奶牛和动态变化的饲料都属于动态物体。本文在ORB-SLAM3基础上新增YOLACT实例分割线程,通过该线程获取动态物体的掩膜。

动态物体剔除算法流程图如图5所示。对双目相机获取到的RGB图像,一方面输入到SLAM的跟踪线程,在提取ORB特征点后,等待实例分割线程的分割结果;另一方面输入到YOLACT实例分割线程,然后将分割获得的掩膜图像等传到SLAM的跟踪线程。跟踪线程获取掩膜图像后,将掩膜进行膨胀处理,以包含动态物体轮廓边缘的高梯度区域,最后将位于掩膜图像上属于奶牛和饲料区域的特征点剔除,保留其它特征点。

图5 动态物体剔除算法流程图Fig.5 Flowchart of dynamic object elimination algorithm

图6a为ORB特征点提取效果图像,图6b为剔除奶牛和饲料上的动态特征点后的图像,对比图6a可见,图6b上已经去除了奶牛和饲料上特征点。

图6 动态特征点剔除效果Fig.6 Dynamic feature points elimination

2.4 觅食奶牛定位

融合掩膜图像、深度图和ORB-SLAM3定位信息来计算觅食奶牛的位置。如图7所示,设机器人参考坐标系为OXYZ,Z轴正方向为机器人的前进方向,Y轴正方向垂直地面向上。设机器人坐标系为O′X′Y′Z′。机器人行进过程中,通过ORB-SLAM3获取机器人实时位置Probot。

图7 奶牛与机器人的相对位置关系Fig.7 Relative position relationship between cow and robot

由于觅食奶牛间的遮挡问题,因此只计算相机视野内、离机器人最近的奶牛的深度,该奶牛位于掩膜图像的最右侧。取掩膜图像中该奶牛ROI区域中心的一个半径为4像素的圆形区域。在圆形区域内,若某像素点对应的深度有效,该像素点为有效像素,读取该像素点的深度di,遍历该圆形区域内所有像素点,计算有效像素的平均深度作为该奶牛与相机的距离dcow,其中n为有效像素个数,dcow计算公式为

(1)

当该奶牛从视野中消失后,再计算下一头奶牛的深度。由于机器人与牛栏的间距不变,奶牛相对于坐标系OXYZ的位置Pcow的几何关系近似表示为

Pcow=Probot+dcow

(2)

2.5 机器人移动轨迹的控制

选择牛栏上方的方形杆作为参照物,通过实时获取机器人与方形杆的距离,并控制该距离不变,引导机器人沿着与料槽平行的预定目标路线运行。

利用掩膜图像和深度图获取机器人与牛栏的间距。遍历掩膜图像中方形杆上所有的像素点,对其中的像素点(u,v)读取深度d,由针孔相机模型可得

(3)

式中fx、cx、fy、cy——相机内参

(x,y,z)——像素点(u,v)对应的三维坐标

适用于混合多端直流输电系统的非线性下垂控制策略//刘英培,杨小龙,梁海平,孙海新,赵玮//(22):178

如图8所示,方形杆与机器人Z′轴平行,有z=d。由式(3)有x=z(u-cx)/fx,取所有三维点(x,y,z)的x值的平均值作为机器人与方形杆(牛栏)的间距D。

图8 机器人与方形杆位置关系Fig.8 Position relationship between robot and square rod

由于方形杆颜色特征单一,且存在反光等因素的影响,双目立体匹配存在误差,导致测量值D不稳定。使用卡尔曼滤波算法对D进行处理。依据机器人行走时与牛栏间距保持不变的特性,设xt=[D],建立xt的状态预测方程为

(4)

式中A——状态转移矩阵,|A|=1

B——外部控制矩阵,|B|=1

ut——外部控制量,|ut|=0

P′t——先验估计协方差矩阵

Q——外部噪声协方差矩阵,|Q|=0.01

(5)

其中

K=P′tHT(HP′tHT+R)-1

K——卡尔曼增益

R——观测噪声协方差矩阵,|R|=0.8

H——观测矩阵,|H|=1

Pt——后验估计协方差矩阵

Zt——观测值,|Zt|=D

根据D的最优估计值t控制机器人的前进方向。

2.6 机器人智能推料

获得机器人与牛栏的间距后,计算出机器人与目标间距的偏差,通过PID调节的方法使偏差为0,保持机器人在前进时与牛栏的间距不变。

机器人根据觅食奶牛位置和饲料量来选择不同的工作模式,控制推料板的角度,实现智能推料。养殖场投料时间一般固定,根据投料后的时间,作为饲料量的判断依据。在投料后3 h内,认为饲料充足,在这期间,机器人将推料板展开至30°,将饲料推回到料槽内;在投料的3 h后,认为饲料不足、分布散乱,机器人根据奶牛的位置信息,推板展开至90°收集饲料并将饲料推送到奶牛面前,之后将推板收回至30°以将饲料卸载到奶牛面前的料槽中。在投料后期、下一次投料前,饲料变得干枯、稀缺,且没有奶牛在觅食,机器人将推料板展开至90°收集并清理饲料。具体过程如1.2节所述。

3 试验

智能推料机器人使用ZED2i双目相机偏光版,Ubuntu 18.04 LTS操作系统,计算机主要配置为:主频3.59 GHz的AMD Ryzen 3600 CPU,8 GB显存的RTX2070s显卡以及16 GB内存。

试验在广东省惠州市某奶牛养殖场进行,该规模化养殖场共饲养有一千多头荷斯坦奶牛。随机选取其中一个牛舍作为试验场所,牛棚长度为110 m,料槽宽度为0.6 m,机器人中心位置距离牛栏1.7 m,推料板展开至90°时,推料板侧端的柔性毛刷与牛栏下方的围墙接触良好。为了减少对奶牛采食的影响,机器人行走速度设为0.25～0.50 m/s。

3.1 动态物体剔除方法测试

为了验证2.3节所提出方法的有效性,采用TUM RGB-D数据集上动态场景较多的walking-halfsphere、walking-xyz、walking-rpy、walking-static数据集对算法进行测试。数据集包含RGB-D数据与相机真实运动轨迹。本文方法使用YOLACT对运动的人进行剔除,原ORB-SLAM3算法不做修改。

采用绝对轨迹误差(Absolute trajectory error, ATE)与相对位姿误差(Relative pose error, RPE)对算法效果进行评价[28]。ATE测量某时刻的相机估计位姿与真实位姿之间的欧几里得距离,反映算法估计的轨迹精度。RPE包含平移误差和旋转误差,描述了算法估计的位姿与实际位姿的差值。计算ATE、RPE的均方根误差(Root mean squared error, RMSE)和标准偏差(Standard deviation, SD)。RMSE描述相机位姿估计值与真实值的偏差,SD描述估计轨迹相对于真实轨迹的离散程度,二者值越小说明算法在动态环境下的定位效果越好。

本文方法与原ORB-SLAM3算法测试结果如表1～3所示。由表可知,本文方法相比于原ORB-SLAM3方法,ATE与RPE的RMSE和SD减小为原来的0.6%～16.9%,说明本文方法能有效降低动态物体对SLAM位姿估计的影响,大幅提升SLAM算法在动态环境的定位精度。

表1 绝对轨迹误差对比结果(ATE)Tab.1 Absolute trajectory error comparison results

表2 相对位移误差对比结果(RPE)Tab.2 Relative displacement error comparison result

表3 相对旋转误差对比结果(RPE)Tab.3 Relative rotation error comparison result

3.2 奶牛定位试验

在机器人推料过程中,根据2.4节方法实时计算觅食奶牛与机器人之间的距离dcow,并计算得到奶牛的位置Pcow,与奶牛实际测量位置相比较。由于牛头的随机摆动,方法计算值会有一定的波动。定位误差如图9所示。由图9可知,获得的奶牛位置识别误差控制在±0.1 m以内,奶牛识别率为100%。奶牛的左右觅食范围一般在0.5 m以内,测量精度满足智能推料要求,证明了奶牛定位方法的可行性。

图9 奶牛位置计算误差Fig.9 Cow position calculation error

3.3 移动轨迹控制试验

机器人与牛栏的间距为1.7 m,通过2.5节所述方法实时获取机器人与牛栏参照物方形杆的距离。试验结果如图10所示。

图10 间距计算Fig.10 Distance calculation

由于牛舍光线不均,方形杆反光、颜色特征单一等,获取的原始数据在1.68～1.72 m之间波动明显,甚至发生突变,偏差达±2 cm,经过卡尔曼滤波处理后,数据稳定在1.692～1.708 m之间,误差为±0.8 cm,有效降低了测量误差,防止由偏差过大造成的机器人左右摇摆现象。

3.4 智能推料试验

试验所在奶牛养殖场每天投料3次,投料时间分别为06:00、14:00、20:00。在投料后3 h以内和3 h以后各进行10次智能推料试验。试验时,天气晴朗,光线充足。试验中奶牛的采食位置和料草的分布情况随机,机器人实时融合觅食奶牛位置、投料后时间信息,自主控制推料板改变推料角度,完成多种推料模式。算法运行速率为12 f/s,PID控制系数为P=10,I=0,D=2。在试验过程中,工作人员全程跟踪、监测记录,并对机器人决策行为进行分析,如果机器人自主决策的模式与人工选择一致,认为机器人决策准确。大量现场试验结果表明,智能推料准确率达到100%。其中图11a为投料后的前3 h,余料充足,机器人自主选用推料模式,将饲料推入料槽内。图11b、11c余料较少,且有奶牛觅食,此时机器人自主选用集料送料模式,计算奶牛位置,收集余料并推送到觅食奶牛前,满足奶牛的个性化采食需求。在饲料稀缺且没有奶牛觅食时,机器人自主选用清料模式,清理饲料。

图11 智能推料试验Fig.11 Intelligent feed pushing experiments

4 结论

(1)针对现有的推料机器人功能单一,无法满足牛只的个性化采食需求问题,开发了奶牛智能推料机器人。构建奶牛、饲料和方形管识别与分割的YOLACT模型;利用掩膜在SLAM的前端剔除动态物体,并获取机器人位置;融合掩膜图像、深度图与机器人位置信息,实现觅食奶牛的快速定位;结合掩膜图像和深度图实现机器人与牛栏的间距获取;最后融合觅食奶牛信息、投料时间信息、机器人移动轨迹信息,智能分析决策,实现推料、集料送料、清料等多模式的智能推料功能。

(2)试验结果表明,开发的多模式推料机器人满足复杂环境下智能推料的要求。觅食奶牛的位置识别精度为±0.1 m,奶牛识别率为100%,机器人导航精度为±0.8 cm,智能推料准确率为100%,算法运行速率为12 f/s。