基于信息融合的智能推料機器人設(shè)計與試驗

張 勤 任海林 胡嘉輝

(華南理工大學(xué)機械與汽車工程學(xué)院, 廣州 510641)

0 引言

全混合日糧(Total mixed rations, TMR)技術(shù)廣泛應(yīng)用在規(guī)模化牧場的奶牛飼喂中,飼料的定期推送是TMR飼喂過程中的重要環(huán)節(jié),可以有效解決奶牛進食過程中,由于挑食、拱料[1]造成的料槽內(nèi)飼料分布不均,飼料被拱出料槽的問題,保證奶牛正常自由采食行為。飼料的可及性十分重要[2],目前國內(nèi)外主要通過人工或者機器人完成定期推料任務(wù),人工推料勞動強度大,人畜交叉感染風(fēng)險高,隨著牧場無人化封閉式管理模式的推進,機器人代替人工的推料方式正在成為未來的發(fā)展方向。為此國內(nèi)外學(xué)者做了大量研究工作,開發(fā)了多種推料機器人,主要有螺旋式和自轉(zhuǎn)式[3-6]兩種推送方法。推料機器人可以自主運行,完成全天候、定期、多次推料任務(wù)。但現(xiàn)有的推料機器人只能完成單一的推料功能,無法滿足覓食奶牛個性化的自由采食需求,在飼料不足時無法收集散料并推送到奶牛面前,而頻繁分群又會造成奶牛的應(yīng)激反應(yīng)。為了解決這個問題,需要研究智能推料機器人,根據(jù)覓食奶牛信息、飼料信息等,自主分析決策,實現(xiàn)推料、集料送料以及清料等多模式的智能推料功能,滿足奶牛個體自由采食需求。

奶牛飼喂信息的快速獲取、牧場環(huán)境下的準(zhǔn)確導(dǎo)航是智能推料機器人的關(guān)鍵技術(shù),也是實現(xiàn)準(zhǔn)確、有效推料的重要保證。奶牛飼喂信息主要包括覓食奶牛的識別定位信息和飼料信息。

在覓食奶牛的識別與定位方面,基于耳標(biāo)的奶牛識別方法[7-8],技術(shù)較為成熟,但存在讀卡速度較慢、識別距離短以及易受干擾的問題。在耳標(biāo)識別基礎(chǔ)上,通過超寬帶UWB[9-10]、無線通訊ZigBee[11]等技術(shù),可以實現(xiàn)奶牛的定位,但存在成本高、精度差的問題。基于機器視覺的牛臉識別算法,相比于耳標(biāo)方法,具有識別速度快、性價比高以及使用靈活等特點,應(yīng)用范圍逐年擴大。通過在牛舍上安裝攝像頭可實現(xiàn)奶牛識別[12],但不能獲取奶牛準(zhǔn)確的位置。張勤等[13]以飼喂區(qū)內(nèi)牛頸枷上的二維碼作為定位標(biāo)簽,采用YOLO v4模型對牛頭與二維碼標(biāo)牌進行識別,再通過牛-碼位置匹配確定覓食奶牛的位置,實現(xiàn)了覓食奶牛及其位置的實時監(jiān)測,但牛舍環(huán)境下,二維碼容易被污染,需要定期維護或更換。隨著計算機技術(shù)的發(fā)展,即時定位與地圖構(gòu)建技術(shù)(Simultaneous localization and mapping, SLAM)因其形式靈活、適應(yīng)能力強等特點在機器人定位與建圖中應(yīng)用越來越廣泛。視覺SLAM可以提供準(zhǔn)確的定位信息,同時其使用的圖像可以用于物體識別,這為計算物體空間位置提供了新方法。已有學(xué)者開發(fā)了ORB-SLAM3[14]、LSD-SLAM[15]、S-PTAM[16]等算法。環(huán)境中的動態(tài)物體會對SLAM的位姿估計造成影響,為了提升定位精度,ZHANG等[17]在ORB-SLAM2的基礎(chǔ)上使用Mask R-CNN去除動態(tài)物體;ZHAO等[18]使用光流法和Mask R-CNN剔除動態(tài)物體;BESCOS等[19]使用多視圖幾何和Mask R-CNN實現(xiàn)動態(tài)物體的檢測。上述方法由于所采用的神經(jīng)網(wǎng)絡(luò)運行速度較慢,且采用單線程,算法間相互堵塞,很難滿足導(dǎo)航的實時性要求。

在飼料余料信息的提取方面,通過飼料上方的攝像頭估算飼料的體積[20-21],或使用神經(jīng)網(wǎng)絡(luò)估算飼料的質(zhì)量[22]。也可以根據(jù)牧場工人的推料經(jīng)驗,通過投料后時間進行余料估算,時間估算方法簡單易行、成本低、速度快。

導(dǎo)航的準(zhǔn)確性是實現(xiàn)有效推料的重要保障,目前主要有軌道導(dǎo)航、磁導(dǎo)航、視覺導(dǎo)航、多傳感器融合導(dǎo)航、SLAM導(dǎo)航等。軌道導(dǎo)航[23]和磁導(dǎo)航[24]方法運行穩(wěn)定,但投入成本高、維護難度大。視覺導(dǎo)航[25-26]通過提取環(huán)境中的參照物作為基準(zhǔn)實現(xiàn)導(dǎo)航,該方法使用靈活、適應(yīng)性強。

綜合國內(nèi)外現(xiàn)狀可以看出,雖然推料機器人在單元技術(shù)方面取得了一定進展,但在應(yīng)用層面上,現(xiàn)有的推料機器人功能單一,無法滿足奶牛的自由采食需求。在以上研究的基礎(chǔ)上,開發(fā)智能推料機器人,選擇牛欄上方的方形桿作為牛欄的參照物。通過YOLACT[27]實例分割網(wǎng)絡(luò)分割圖像中的奶牛、飼料等動態(tài)物體和方形桿;采用ORB-SLAM3作為定位算法,在SLAM前端利用掩膜進行動態(tài)物體剔除;實時獲取覓食奶牛位置、機器人與牛欄參照物方形桿的間距;根據(jù)覓食奶牛的定位信息、投料后的時間、機器人的導(dǎo)航信息,選擇工作模式,自主沿著設(shè)定軌跡,實現(xiàn)推料、集料送料、清料等多模式推料功能,滿足奶牛個體自由采食需求。

1 智能推料機器人組成與工作機理

1.1 機器人系統(tǒng)組成

如圖1所示,智能推料機器人由視覺系統(tǒng)、移動底盤、推板機構(gòu)、無線通信系統(tǒng)和控制系統(tǒng)組成。視覺系統(tǒng)由雙目相機和網(wǎng)絡(luò)攝像頭構(gòu)成,雙目相機安裝在機器人正前方,實時采集機器人前方的飼喂圖像,網(wǎng)絡(luò)攝像頭用于遠程監(jiān)測機器人的運行狀況。移動底盤是機器人的載體。推板機構(gòu)由推料板、推料板側(cè)面的毛刷及驅(qū)動機構(gòu)組成。推料板可以自由展開、收回以改變其姿態(tài),實現(xiàn)推料、集料送料、清料等多模式推料功能。毛刷可以清潔推料板側(cè)端和飼料區(qū)墻壁間隙內(nèi)的飼料,同時也可以緩沖機器人的導(dǎo)航誤差。在非工作狀態(tài)時,推板機構(gòu)可以收納至底盤的側(cè)面,減少空間占用。控制系統(tǒng)根據(jù)雙目相機采集到的圖像,實時獲取覓食奶牛信息、機器人的位置信息,選擇工作模式,控制推板機構(gòu)運動;監(jiān)測機器人與牛欄參照物的距離,控制機器人沿著與料槽平行的方向,按照預(yù)定軌跡運動。無線通信系統(tǒng)可以實現(xiàn)遠程監(jiān)測和操控機器人。

圖1 機器人系統(tǒng)Fig.1 Robot system1.毛刷 2.推板機構(gòu) 3.控制系統(tǒng) 4.網(wǎng)絡(luò)攝像頭 5.雙目相機 6.無線通信系統(tǒng) 7.移動底盤

1.2 工作機理分析

推料機理如圖2所示。當(dāng)飼料充足時,機器人采用推料模式,如圖2中AB段所示,將推料板展開到30°,沿著與料槽平行方向行走,將料槽外側(cè)的飼料推回到料槽內(nèi)。當(dāng)飼料較少,且有奶牛覓食時,采用集料送料模式,如圖2中的BE段,實時獲得覓食奶牛的位置,推料板先調(diào)整為90°收集散亂的飼料,如C處所示,然后將飼料推送至D處的奶牛前,推料板再調(diào)整為30°將飼料卸載到覓食奶牛采食區(qū)前;如果飼料很少且沒有奶牛覓食時,采用清料模式,如圖2中的EF段所示,推料板角度為90°,清理飼料。飼料的多少可以根據(jù)投料后時間或者通過圖像判斷。

圖2 智能推料過程Fig.2 Intelligent feed pushing process

為了保證高效、準(zhǔn)確地推料,推料過程中,需要機器人的運行軌跡與牛欄或者料槽之間的距離恒定且與其方向平行,為此選擇牛欄上不被遮擋的方形桿為牛欄的參照物,如圖2所示,通過實時控制機器人與方形桿間距,保證機器人沿料槽平行方向進行有效推料。

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

2.1 控制算法構(gòu)成

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

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

首先通過雙目相機實時獲取機器人前方的左目彩色圖和深度圖,然后執(zhí)行以下步驟:

(1)基于YOLACT的目標(biāo)識別與分割:通過YOLACT實例分割網(wǎng)絡(luò)對獲取的彩色圖像進行目標(biāo)識別,得到覓食奶牛、飼料和牛欄參照物方形桿的掩膜圖像。

(2)動態(tài)物體剔除:為了提高定位精度,利用掩膜圖像在ORB-SLAM3的前端剔除奶牛和飼料等動態(tài)物體。獲取機器人實時位置。

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

(4)機器人移動軌跡控制:利用掩膜圖像和深度圖實時獲取機器人與牛欄參照物方形桿的間距,控制機器人行走過程中與料槽方向平行且間距不變。

(5)機器人智能推料:根據(jù)覓食奶牛位置、投料時間、機器人與牛欄間距信息控制機器人沿著與料槽平行方向行走,自主選擇工作模式,改變推料板角度,實現(xiàn)智能推料。

2.2 基于YOLACT的目標(biāo)識別與分割

為獲取目標(biāo)準(zhǔn)確的輪廓,同時兼顧算法的運行速度,本文采用一階段實例分割網(wǎng)絡(luò)YOLACT作為識別物體的神經(jīng)網(wǎng)絡(luò),特征提取主干網(wǎng)絡(luò)采用DarkNet53。

為提高YOLACT模型魯棒性,在不同光照、不同時間條件下,使用雙目相機錄制奶牛養(yǎng)殖場中機器人實際運行路線視頻,視頻圖像分辨率為1 920像素×1 080像素,相機幀率為30 f/s。之后將視頻每30幀輸出一幅彩色圖像,得到含2 000幅彩色圖像的數(shù)據(jù)集。對圖像中的奶牛、飼料和方形桿進行標(biāo)注,標(biāo)注結(jié)果如圖4a所示,在數(shù)據(jù)集上訓(xùn)練網(wǎng)絡(luò)。

圖4 YOLACT圖像標(biāo)注與目標(biāo)識別Fig.4 Image labeling and target detection of YOLACT

將訓(xùn)練得到的模型用于雙目相機實時采集的左目圖像的目標(biāo)識別與分割。YOLACT網(wǎng)絡(luò)輸入為3通道的RGB圖像,輸出包括一個k通道的矩陣、檢測目標(biāo)的類別和矩形感興趣區(qū)域 (Region of interest,ROI),其中k為檢測目標(biāo)的數(shù)量,矩陣的每個通道為一個二值掩膜圖像,合并所有通道,得到一個包括所有檢測對象的掩膜圖像。實際目標(biāo)識別分割結(jié)果和對應(yīng)的掩膜圖像分別如圖4b和圖4c所示。

2.3 動態(tài)物體剔除

為了實時獲取機器人準(zhǔn)確的定位信息,需要減小動態(tài)物體對機器人位姿估算造成的影響,提高SLAM的定位精度,所以在SLAM的前端對動態(tài)物體進行剔除。牛舍環(huán)境下,隨機運動的奶牛和動態(tài)變化的飼料都屬于動態(tài)物體。本文在ORB-SLAM3基礎(chǔ)上新增YOLACT實例分割線程,通過該線程獲取動態(tài)物體的掩膜。

動態(tài)物體剔除算法流程圖如圖5所示。對雙目相機獲取到的RGB圖像,一方面輸入到SLAM的跟蹤線程,在提取ORB特征點后,等待實例分割線程的分割結(jié)果;另一方面輸入到Y(jié)OLACT實例分割線程,然后將分割獲得的掩膜圖像等傳到SLAM的跟蹤線程。跟蹤線程獲取掩膜圖像后,將掩膜進行膨脹處理,以包含動態(tài)物體輪廓邊緣的高梯度區(qū)域,最后將位于掩膜圖像上屬于奶牛和飼料區(qū)域的特征點剔除,保留其它特征點。

圖5 動態(tài)物體剔除算法流程圖Fig.5 Flowchart of dynamic object elimination algorithm

圖6a為ORB特征點提取效果圖像,圖6b為剔除奶牛和飼料上的動態(tài)特征點后的圖像,對比圖6a可見,圖6b上已經(jīng)去除了奶牛和飼料上特征點。

圖6 動態(tài)特征點剔除效果Fig.6 Dynamic feature points elimination

2.4 覓食奶牛定位

融合掩膜圖像、深度圖和ORB-SLAM3定位信息來計算覓食奶牛的位置。如圖7所示,設(shè)機器人參考坐標(biāo)系為OXYZ,Z軸正方向為機器人的前進方向,Y軸正方向垂直地面向上。設(shè)機器人坐標(biāo)系為O′X′Y′Z′。機器人行進過程中,通過ORB-SLAM3獲取機器人實時位置Probot。

圖7 奶牛與機器人的相對位置關(guān)系Fig.7 Relative position relationship between cow and robot

由于覓食奶牛間的遮擋問題,因此只計算相機視野內(nèi)、離機器人最近的奶牛的深度,該奶牛位于掩膜圖像的最右側(cè)。取掩膜圖像中該奶牛ROI區(qū)域中心的一個半徑為4像素的圓形區(qū)域。在圓形區(qū)域內(nèi),若某像素點對應(yīng)的深度有效,該像素點為有效像素,讀取該像素點的深度di,遍歷該圓形區(qū)域內(nèi)所有像素點,計算有效像素的平均深度作為該奶牛與相機的距離dcow,其中n為有效像素個數(shù),dcow計算公式為

(1)

當(dāng)該奶牛從視野中消失后,再計算下一頭奶牛的深度。由于機器人與牛欄的間距不變,奶牛相對于坐標(biāo)系OXYZ的位置Pcow的幾何關(guān)系近似表示為

Pcow=Probot+dcow

(2)

2.5 機器人移動軌跡的控制

選擇牛欄上方的方形桿作為參照物,通過實時獲取機器人與方形桿的距離,并控制該距離不變,引導(dǎo)機器人沿著與料槽平行的預(yù)定目標(biāo)路線運行。

利用掩膜圖像和深度圖獲取機器人與牛欄的間距。遍歷掩膜圖像中方形桿上所有的像素點,對其中的像素點(u,v)讀取深度d,由針孔相機模型可得

(3)

式中fx、cx、fy、cy——相機內(nèi)參

(x,y,z)——像素點(u,v)對應(yīng)的三維坐標(biāo)

適用于混合多端直流輸電系統(tǒng)的非線性下垂控制策略//劉英培,楊小龍,梁海平,孫海新,趙瑋//(22):178

如圖8所示,方形桿與機器人Z′軸平行,有z=d。由式(3)有x=z(u-cx)/fx,取所有三維點(x,y,z)的x值的平均值作為機器人與方形桿(牛欄)的間距D。

圖8 機器人與方形桿位置關(guān)系Fig.8 Position relationship between robot and square rod

由于方形桿顏色特征單一,且存在反光等因素的影響,雙目立體匹配存在誤差,導(dǎo)致測量值D不穩(wěn)定。使用卡爾曼濾波算法對D進行處理。依據(jù)機器人行走時與牛欄間距保持不變的特性,設(shè)xt=[D],建立xt的狀態(tài)預(yù)測方程為

(4)

式中A——狀態(tài)轉(zhuǎn)移矩陣,|A|=1

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

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

P′t——先驗估計協(xié)方差矩陣

Q——外部噪聲協(xié)方差矩陣,|Q|=0.01

(5)

其中

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

K——卡爾曼增益

R——觀測噪聲協(xié)方差矩陣,|R|=0.8

H——觀測矩陣,|H|=1

Pt——后驗估計協(xié)方差矩陣

Zt——觀測值,|Zt|=D

根據(jù)D的最優(yōu)估計值t控制機器人的前進方向。

2.6 機器人智能推料

獲得機器人與牛欄的間距后,計算出機器人與目標(biāo)間距的偏差,通過PID調(diào)節(jié)的方法使偏差為0,保持機器人在前進時與牛欄的間距不變。

機器人根據(jù)覓食奶牛位置和飼料量來選擇不同的工作模式,控制推料板的角度,實現(xiàn)智能推料。養(yǎng)殖場投料時間一般固定,根據(jù)投料后的時間,作為飼料量的判斷依據(jù)。在投料后3 h內(nèi),認為飼料充足,在這期間,機器人將推料板展開至30°,將飼料推回到料槽內(nèi);在投料的3 h后,認為飼料不足、分布散亂,機器人根據(jù)奶牛的位置信息,推板展開至90°收集飼料并將飼料推送到奶牛面前,之后將推板收回至30°以將飼料卸載到奶牛面前的料槽中。在投料后期、下一次投料前,飼料變得干枯、稀缺,且沒有奶牛在覓食,機器人將推料板展開至90°收集并清理飼料。具體過程如1.2節(jié)所述。

3 試驗

智能推料機器人使用ZED2i雙目相機偏光版,Ubuntu 18.04 LTS操作系統(tǒng),計算機主要配置為:主頻3.59 GHz的AMD Ryzen 3600 CPU,8 GB顯存的RTX2070s顯卡以及16 GB內(nèi)存。

試驗在廣東省惠州市某奶牛養(yǎng)殖場進行,該規(guī)模化養(yǎng)殖場共飼養(yǎng)有一千多頭荷斯坦奶牛。隨機選取其中一個牛舍作為試驗場所,牛棚長度為110 m,料槽寬度為0.6 m,機器人中心位置距離牛欄1.7 m,推料板展開至90°時,推料板側(cè)端的柔性毛刷與牛欄下方的圍墻接觸良好。為了減少對奶牛采食的影響,機器人行走速度設(shè)為0.25～0.50 m/s。

3.1 動態(tài)物體剔除方法測試

為了驗證2.3節(jié)所提出方法的有效性,采用TUM RGB-D數(shù)據(jù)集上動態(tài)場景較多的walking-halfsphere、walking-xyz、walking-rpy、walking-static數(shù)據(jù)集對算法進行測試。數(shù)據(jù)集包含RGB-D數(shù)據(jù)與相機真實運動軌跡。本文方法使用YOLACT對運動的人進行剔除,原ORB-SLAM3算法不做修改。

采用絕對軌跡誤差(Absolute trajectory error, ATE)與相對位姿誤差(Relative pose error, RPE)對算法效果進行評價[28]。ATE測量某時刻的相機估計位姿與真實位姿之間的歐幾里得距離,反映算法估計的軌跡精度。RPE包含平移誤差和旋轉(zhuǎn)誤差,描述了算法估計的位姿與實際位姿的差值。計算ATE、RPE的均方根誤差(Root mean squared error, RMSE)和標(biāo)準(zhǔn)偏差(Standard deviation, SD)。RMSE描述相機位姿估計值與真實值的偏差,SD描述估計軌跡相對于真實軌跡的離散程度,二者值越小說明算法在動態(tài)環(huán)境下的定位效果越好。

本文方法與原ORB-SLAM3算法測試結(jié)果如表1～3所示。由表可知,本文方法相比于原ORB-SLAM3方法,ATE與RPE的RMSE和SD減小為原來的0.6%～16.9%,說明本文方法能有效降低動態(tài)物體對SLAM位姿估計的影響,大幅提升SLAM算法在動態(tài)環(huán)境的定位精度。

表1 絕對軌跡誤差對比結(jié)果(ATE)Tab.1 Absolute trajectory error comparison results

表2 相對位移誤差對比結(jié)果(RPE)Tab.2 Relative displacement error comparison result

表3 相對旋轉(zhuǎn)誤差對比結(jié)果(RPE)Tab.3 Relative rotation error comparison result

3.2 奶牛定位試驗

在機器人推料過程中,根據(jù)2.4節(jié)方法實時計算覓食奶牛與機器人之間的距離dcow,并計算得到奶牛的位置Pcow,與奶牛實際測量位置相比較。由于牛頭的隨機擺動,方法計算值會有一定的波動。定位誤差如圖9所示。由圖9可知,獲得的奶牛位置識別誤差控制在±0.1 m以內(nèi),奶牛識別率為100%。奶牛的左右覓食范圍一般在0.5 m以內(nèi),測量精度滿足智能推料要求,證明了奶牛定位方法的可行性。

圖9 奶牛位置計算誤差Fig.9 Cow position calculation error

3.3 移動軌跡控制試驗

機器人與牛欄的間距為1.7 m,通過2.5節(jié)所述方法實時獲取機器人與牛欄參照物方形桿的距離。試驗結(jié)果如圖10所示。

圖10 間距計算Fig.10 Distance calculation

由于牛舍光線不均,方形桿反光、顏色特征單一等,獲取的原始數(shù)據(jù)在1.68～1.72 m之間波動明顯,甚至發(fā)生突變,偏差達±2 cm,經(jīng)過卡爾曼濾波處理后,數(shù)據(jù)穩(wěn)定在1.692～1.708 m之間,誤差為±0.8 cm,有效降低了測量誤差,防止由偏差過大造成的機器人左右搖擺現(xiàn)象。

3.4 智能推料試驗

試驗所在奶牛養(yǎng)殖場每天投料3次,投料時間分別為06:00、14:00、20:00。在投料后3 h以內(nèi)和3 h以后各進行10次智能推料試驗。試驗時,天氣晴朗,光線充足。試驗中奶牛的采食位置和料草的分布情況隨機,機器人實時融合覓食奶牛位置、投料后時間信息,自主控制推料板改變推料角度,完成多種推料模式。算法運行速率為12 f/s,PID控制系數(shù)為P=10,I=0,D=2。在試驗過程中,工作人員全程跟蹤、監(jiān)測記錄,并對機器人決策行為進行分析,如果機器人自主決策的模式與人工選擇一致,認為機器人決策準(zhǔn)確。大量現(xiàn)場試驗結(jié)果表明,智能推料準(zhǔn)確率達到100%。其中圖11a為投料后的前3 h,余料充足,機器人自主選用推料模式,將飼料推入料槽內(nèi)。圖11b、11c余料較少,且有奶牛覓食,此時機器人自主選用集料送料模式,計算奶牛位置,收集余料并推送到覓食奶牛前,滿足奶牛的個性化采食需求。在飼料稀缺且沒有奶牛覓食時,機器人自主選用清料模式,清理飼料。

圖11 智能推料試驗Fig.11 Intelligent feed pushing experiments

4 結(jié)論

(1)針對現(xiàn)有的推料機器人功能單一,無法滿足牛只的個性化采食需求問題,開發(fā)了奶牛智能推料機器人。構(gòu)建奶牛、飼料和方形管識別與分割的YOLACT模型;利用掩膜在SLAM的前端剔除動態(tài)物體,并獲取機器人位置;融合掩膜圖像、深度圖與機器人位置信息,實現(xiàn)覓食奶牛的快速定位;結(jié)合掩膜圖像和深度圖實現(xiàn)機器人與牛欄的間距獲取;最后融合覓食奶牛信息、投料時間信息、機器人移動軌跡信息,智能分析決策,實現(xiàn)推料、集料送料、清料等多模式的智能推料功能。

(2)試驗結(jié)果表明,開發(fā)的多模式推料機器人滿足復(fù)雜環(huán)境下智能推料的要求。覓食奶牛的位置識別精度為±0.1 m,奶牛識別率為100%,機器人導(dǎo)航精度為±0.8 cm,智能推料準(zhǔn)確率為100%,算法運行速率為12 f/s。