基于激光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所示，主要包括控制層、數據處理層及驅動層?？刂茖踊贚inux系統實現推翻草任務下發及機器人工作狀態監控，推翻草任務下發即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算法應用分支定界的方法進行優化搜索策略?；丨h優化過程中，使用稀疏位姿的方法來優化激光點云和子圖的位姿?；丨h優化過程與激光點云插入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)

式中α、β、?！髟u分標準的權重

其權重取決于機器人所側重的性能，通過試驗，α、β、Γ分別取值為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°，導航精度高于加載里程計信息時的導航精度，滿足機器人推翻草作業時的導航精度要求。