基于激光SLAM的牛場(chǎng)智能推翻草機(jī)器人自主導(dǎo)航系統(tǒng)

宋懷波 段援朝 李 嶸 焦義濤 王 政

(1.西北農(nóng)林科技大學(xué)機(jī)械與電子工程學(xué)院，陜西楊凌 712100；2.農(nóng)業(yè)農(nóng)村部農(nóng)業(yè)物聯(lián)網(wǎng)重點(diǎn)實(shí)驗(yàn)室，陜西楊凌 712100)

0 引言

近年來(lái)，奶牛養(yǎng)殖的現(xiàn)代化、規(guī)?；?、標(biāo)準(zhǔn)化以及產(chǎn)業(yè)整體效率和素質(zhì)水平顯著提高，已經(jīng)從家養(yǎng)等小規(guī)模養(yǎng)殖向農(nóng)場(chǎng)化的現(xiàn)代新型養(yǎng)殖模式轉(zhuǎn)型[1-2]。但是多數(shù)養(yǎng)殖場(chǎng)在奶牛飼養(yǎng)方面，仍然依賴傳統(tǒng)機(jī)械設(shè)備，導(dǎo)致養(yǎng)殖場(chǎng)產(chǎn)能低下、奶牛發(fā)病率上升[3-4]。因此，研發(fā)一款適用于牛場(chǎng)的智能推翻草機(jī)器人，對(duì)牛場(chǎng)向現(xiàn)代化、智能化轉(zhuǎn)型具有重要的意義[5-6]。自主導(dǎo)航技術(shù)是推翻草機(jī)器人的核心，現(xiàn)有導(dǎo)航如導(dǎo)軌式、磁條引導(dǎo)式或視覺導(dǎo)航方式等難以適用于復(fù)雜牛場(chǎng)環(huán)境，尚需進(jìn)一步研究一款適用于牛場(chǎng)推翻草機(jī)器人的導(dǎo)航系統(tǒng)。

目前應(yīng)用于農(nóng)業(yè)半開放式環(huán)境的導(dǎo)航機(jī)器人的導(dǎo)航方式可分為依賴外界信息輔助型導(dǎo)航與自主探索型導(dǎo)航[7-9]。依賴外界信息輔助型導(dǎo)航方式主要有磁條導(dǎo)航、二維編碼導(dǎo)航等。萬(wàn)暢等[10]提出了一種基于磁條導(dǎo)航方式的飼料推送機(jī)器人，機(jī)器人通過傳感器檢測(cè)地面預(yù)先鋪設(shè)的磁條信號(hào)來(lái)引導(dǎo)機(jī)器人移動(dòng)作業(yè)。ZHANG等[11]基于雙磁條導(dǎo)航機(jī)器人設(shè)計(jì)了雙輸入單輸出型模糊控制器，提高了磁條導(dǎo)航機(jī)器人在移動(dòng)過程中的位姿精度。磁條導(dǎo)航具有精確度高、成本低等特點(diǎn)，但后期維護(hù)繁瑣，路徑改變困難，無(wú)法滿足任務(wù)實(shí)時(shí)變化的應(yīng)用場(chǎng)景。二維編碼導(dǎo)航通過機(jī)器人視覺系統(tǒng)掃描地面二維編碼，通過二維編碼信息獲取機(jī)器人當(dāng)前位置信息[12-13]。KRAJNIK等[14]提出了一種基于單目視覺識(shí)別二維編碼的機(jī)器人定位導(dǎo)航算法，根據(jù)二維編碼信息建立環(huán)境地圖從而實(shí)現(xiàn)機(jī)器人定位導(dǎo)航。該方式在大場(chǎng)景環(huán)境中精確度較高，路徑改變?nèi)菀?，但?duì)于牛場(chǎng)復(fù)雜環(huán)境，二維編碼易覆蓋，需要定期維護(hù)，維護(hù)成本較高。

自主探索型導(dǎo)航通過激光雷達(dá)、視覺傳感器、北斗衛(wèi)星導(dǎo)航系統(tǒng)或其他衛(wèi)星導(dǎo)航系統(tǒng)等多源數(shù)據(jù)融合，對(duì)外界環(huán)境精準(zhǔn)感知進(jìn)行導(dǎo)航定位[15-18]。徐波等[19]基于北斗衛(wèi)星定位系統(tǒng)，提出了一種與慣性測(cè)量單元等多傳感器融合定位算法，實(shí)現(xiàn)了機(jī)器人在室外空曠環(huán)境中導(dǎo)航。毛文菊等[20]提出了一種基于激光雷達(dá)與導(dǎo)航衛(wèi)星系統(tǒng)(Global navigation satellite system, GNSS)相融合的果園運(yùn)輸機(jī)器人，有效提高了運(yùn)輸機(jī)器人在果園工作的效率。CIOFFI等[21]提出了一種基于緊密耦合的非線性優(yōu)化估計(jì)器，將全球定位系統(tǒng)(Global positioning system, GPS)與視覺和慣性導(dǎo)航測(cè)量信息融合，有效解決了視覺與慣性導(dǎo)航在長(zhǎng)期導(dǎo)航中積累的漂移問題。但北斗導(dǎo)航系統(tǒng)、GNSS或GPS傳感器在牛場(chǎng)半開放式環(huán)境中信號(hào)弱且易丟失，不適用于牛場(chǎng)環(huán)境機(jī)器人導(dǎo)航。張振乾等[22]提出了一種基于雙目視覺導(dǎo)航的香蕉園巡檢機(jī)器人，但視覺傳感器受光線變化影響較大，在復(fù)雜牛場(chǎng)環(huán)境中的導(dǎo)航效果尚需驗(yàn)證。激光雷達(dá)作為新型傳感器，具有分辨率高、抗干擾能力強(qiáng)、響應(yīng)速度快等優(yōu)點(diǎn)，因此在牛場(chǎng)環(huán)境中，使用激光雷達(dá)作為機(jī)器人感知環(huán)境的主要傳感器，可以實(shí)現(xiàn)推翻草機(jī)器人在牛場(chǎng)環(huán)境中精準(zhǔn)導(dǎo)航[23]。

針對(duì)牛場(chǎng)半開放式復(fù)雜環(huán)境，本研究研發(fā)一種基于激光同步定位與建圖(Simultaneous localization and mapping, SLAM)的牛場(chǎng)智能推翻草機(jī)器人導(dǎo)航系統(tǒng)。該機(jī)器人在創(chuàng)建牛場(chǎng)環(huán)境地圖過程中采用Cartographer算法，利用自適應(yīng)蒙特卡洛定位(Adaptive Monte Carlo localization, AMCL)算法將激光雷達(dá)、姿態(tài)慣性傳感器(Inertial measurement unit, IMU)和編碼器三者信息融合進(jìn)行定位，提高牛場(chǎng)環(huán)境地圖的創(chuàng)建精度。機(jī)器人在導(dǎo)航過程中使用迪杰斯特拉算法(Dijkstra)算法，利用AMCL算法將激光雷達(dá)與IMU兩者信息融合進(jìn)行定位，以解決機(jī)器人在推翻草過程中由輪胎打滑造成的編碼器數(shù)據(jù)誤差問題，從而實(shí)現(xiàn)機(jī)器人在牛場(chǎng)復(fù)雜環(huán)境中精準(zhǔn)導(dǎo)航，進(jìn)而完成推翻牧草任務(wù)。

1 智能推翻草機(jī)器人導(dǎo)航系統(tǒng)設(shè)計(jì)

1.1 工作環(huán)境

奶牛在飼喂過程中，首先需要將牧草與精飼料按照一定的比例混合均勻[24-25]，投置于圍欄底部進(jìn)行飼喂。但奶牛在進(jìn)食過程中會(huì)因挑食精飼料出現(xiàn)“打窩”現(xiàn)象[26]，如圖1所示，這一現(xiàn)象將導(dǎo)致飼料被推向遠(yuǎn)離圍欄的位置，使奶牛無(wú)法正常進(jìn)食，造成食物浪費(fèi)及奶牛攝食不足。牛場(chǎng)智能推翻草機(jī)器人的主要功能是實(shí)現(xiàn)牧草與精飼料的再次充分混合，并推、翻至圍欄底部奶牛飼喂區(qū)域，保證奶牛及時(shí)地?cái)z入飼料。

圖1 奶牛飼喂環(huán)境

機(jī)器人在推翻草過程中，需沿著飼喂區(qū)域邊界進(jìn)行運(yùn)動(dòng)。但基于飼喂區(qū)域外部飼料外疏內(nèi)密的分布特點(diǎn)，使機(jī)器人在移動(dòng)過程中左右輪阻力分布不均，出現(xiàn)輪胎打滑現(xiàn)象，進(jìn)而影響機(jī)器人導(dǎo)航精度。因此，可靠的導(dǎo)航系統(tǒng)是機(jī)器人自主完成推翻草任務(wù)的關(guān)鍵。

1.2 機(jī)器人總體系統(tǒng)組成

牛場(chǎng)智能推翻草機(jī)器人總體系統(tǒng)組成如圖2所示，主要包括自主導(dǎo)航移動(dòng)平臺(tái)、推翻草裝置、清掃裝置與控制系統(tǒng)。自主導(dǎo)航移動(dòng)平臺(tái)基于差分式驅(qū)動(dòng)模型設(shè)計(jì)，通過直流編碼減速電機(jī)驅(qū)動(dòng)。該平臺(tái)長(zhǎng)1 200 mm，寬750 mm，高600 mm。推翻草裝置主要由螺旋輸送器、螺旋輸送器支架、螺旋輸送器減速電機(jī)及升降電機(jī)組成。該裝置安裝于自主導(dǎo)航移動(dòng)平臺(tái)前端，主要實(shí)現(xiàn)牧草中粗精飼料充分?jǐn)嚢璨⑼品溜曃箙^(qū)域功能。

圖2 機(jī)器人總體系統(tǒng)組成

清掃裝置由邊掃和邊掃減速電機(jī)組成。該裝置采用錯(cuò)位安裝、同向旋轉(zhuǎn)的方式，實(shí)現(xiàn)遺留飼料的清掃功能。機(jī)器人控制系統(tǒng)主要由微型計(jì)算機(jī)、Arduino單片機(jī)及電機(jī)驅(qū)動(dòng)板等組成，主要負(fù)責(zé)機(jī)器人導(dǎo)航、推翻草裝置及清掃裝置的控制。同時(shí)微型計(jì)算機(jī)可以與主機(jī)PC或手機(jī)進(jìn)行信息交互。PC或手機(jī)向機(jī)器人遠(yuǎn)程下發(fā)機(jī)器人導(dǎo)航的目標(biāo)位置坐標(biāo)(x,y,θ)，其中x與y分別為機(jī)器人在牛場(chǎng)環(huán)境地圖中的橫坐標(biāo)與縱坐標(biāo)，θ為機(jī)器人到達(dá)目標(biāo)位置時(shí)的姿態(tài)角。當(dāng)機(jī)器人到達(dá)目標(biāo)位置后，上位機(jī)以話題消息的形式與下位機(jī)進(jìn)行通信，下位機(jī)通過繼電器工作驅(qū)動(dòng)推翻草裝置電機(jī)旋轉(zhuǎn)，從而完成推翻草任務(wù)。

1.3 機(jī)器人硬件結(jié)構(gòu)設(shè)計(jì)

智能推翻草機(jī)器人硬件主要包括：場(chǎng)景感知模塊、控制系統(tǒng)、電源模塊、執(zhí)行系統(tǒng)和通信模塊。圖3為智能推翻草機(jī)器人硬件系統(tǒng)組成。

圖3 機(jī)器人硬件組成框圖

1.3.1場(chǎng)景感知模塊

場(chǎng)景感知模塊主要包括激光雷達(dá)、電機(jī)編碼器和IMU模塊。激光雷達(dá)用以采集牛場(chǎng)環(huán)境地圖點(diǎn)云數(shù)據(jù)，通過激光點(diǎn)束旋轉(zhuǎn)測(cè)距依次獲得各個(gè)角度上的雷達(dá)數(shù)據(jù)，進(jìn)而形成激光雷達(dá)所處平面的點(diǎn)云數(shù)據(jù)。激光雷達(dá)型號(hào)為RPLIDAR-S2，測(cè)距半徑為30 m，測(cè)量頻率為30 000次/s，采用飛行時(shí)間法(Time of flight, TOF)，TOF測(cè)距原理為根據(jù)激光器發(fā)射和接收激光脈沖的時(shí)間差與光速乘積的1/2計(jì)算出激光雷達(dá)與障礙物之間的距離。對(duì)于牛場(chǎng)大范圍開放式環(huán)境，TOF測(cè)距型激光雷達(dá)能夠保證較高的測(cè)量精度，減小測(cè)距誤差。采用BKH3808A-100-L3-2M型電機(jī)編碼器，每旋轉(zhuǎn)一周發(fā)送100個(gè)脈沖信號(hào)。電機(jī)編碼器采集電機(jī)旋轉(zhuǎn)的圈數(shù)，從而計(jì)算出機(jī)器人移動(dòng)速度。

IMU模塊采用HFI-B6型，模塊中通過加速度計(jì)測(cè)量機(jī)器人運(yùn)動(dòng)的加速度，使用陀螺儀測(cè)量機(jī)器人運(yùn)動(dòng)的角速度。通過6軸姿態(tài)融合算法進(jìn)行機(jī)器人的當(dāng)前姿態(tài)計(jì)算。首先利用四元數(shù)將陀螺儀基于機(jī)體的旋轉(zhuǎn)數(shù)據(jù)轉(zhuǎn)化為世界坐標(biāo)，其次通過加速度計(jì)的輸出修正陀螺儀數(shù)據(jù)漂移誤差，最終經(jīng)過積分計(jì)算機(jī)器人當(dāng)前姿態(tài)各方向上的角度與線速度。

1.3.2控制系統(tǒng)及其他模塊

控制系統(tǒng)模塊中利用Inter NUC微型計(jì)算機(jī)作為上層控制器，負(fù)責(zé)建圖、導(dǎo)航及與主機(jī)PC或移動(dòng)端之間進(jìn)行多機(jī)通信功能。Arduino Mega2560單片機(jī)作為導(dǎo)航系統(tǒng)的底層控制器，驅(qū)動(dòng)執(zhí)行系統(tǒng)模塊，使機(jī)器人各個(gè)裝置電機(jī)有序工作。電源模塊為各模塊供電，保證各裝置正常工作，為機(jī)器人精準(zhǔn)導(dǎo)航、平穩(wěn)工作奠定基礎(chǔ)。

1.4 機(jī)器人導(dǎo)航軟件系統(tǒng)設(shè)計(jì)

牛場(chǎng)推翻草機(jī)器人導(dǎo)航軟件系統(tǒng)如圖4所示，主要包括控制層、數(shù)據(jù)處理層及驅(qū)動(dòng)層?？刂茖踊贚inux系統(tǒng)實(shí)現(xiàn)推翻草任務(wù)下發(fā)及機(jī)器人工作狀態(tài)監(jiān)控，推翻草任務(wù)下發(fā)即PC或手機(jī)向機(jī)器人依次發(fā)送目標(biāo)點(diǎn)坐標(biāo)，通過多點(diǎn)導(dǎo)航方式實(shí)現(xiàn)推翻草任務(wù)。工作狀態(tài)監(jiān)控可查看機(jī)器人當(dāng)前位置。數(shù)據(jù)處理層基于Ubuntu 18.04 LTS微型計(jì)算機(jī)開發(fā)，搭載機(jī)器人操作系統(tǒng)(Robot operating system, ROS)[27]，并安裝Cartographer、AMCL和Dijkstra算法[28-29]。Cartographer算法根據(jù)激光雷達(dá)傳感器數(shù)據(jù)、IMU傳感器和里程計(jì)信息創(chuàng)建牛場(chǎng)環(huán)境地圖，AMCL算法根據(jù)里程計(jì)數(shù)據(jù)和IMU信息確定機(jī)器人在當(dāng)前地圖中位置，Dijkstra算法規(guī)劃?rùn)C(jī)器人工作路徑。

圖4 機(jī)器人軟件結(jié)構(gòu)

下位機(jī)Arduino主控板使用ROS Arduino Bridge算法，實(shí)現(xiàn)驅(qū)動(dòng)直流編碼減速電機(jī)、編碼器4倍頻測(cè)速、比例積分微分(Proportion integration differentiation, PID)控制及與微型計(jì)算機(jī)通信等功能[30]。ROS Arduino Bridge算法中的PID控制器根據(jù)機(jī)器人實(shí)際運(yùn)行速度與目標(biāo)線速度和角速度，進(jìn)行閉環(huán)控制驅(qū)動(dòng)輪電機(jī)旋轉(zhuǎn)，達(dá)到對(duì)自主導(dǎo)航移動(dòng)平臺(tái)的精準(zhǔn)控制。

2 機(jī)器人導(dǎo)航功能實(shí)現(xiàn)原理

2.1 牛場(chǎng)環(huán)境地圖構(gòu)建

在推翻草機(jī)器人導(dǎo)航系統(tǒng)設(shè)計(jì)中，基于谷歌公司發(fā)布的Cartographer 2D SLAM算法構(gòu)建牛場(chǎng)環(huán)境地圖[31]。Cartographer算法流程如圖5所示，主要分為局部SLAM和全局SLAM兩部分。局部SALM主要根據(jù)激光雷達(dá)數(shù)據(jù)建立一系列子地圖Submap，全局SLAM中結(jié)合傳感器數(shù)據(jù)，通過掃描匹配將一系列Submap連接生成全局地圖。

圖5 Cartographer算法流程圖

Cartographer算法中數(shù)據(jù)來(lái)源包括激光雷達(dá)、里程計(jì)數(shù)據(jù)和IMU數(shù)據(jù)。激光雷達(dá)數(shù)據(jù)首先通過Voxel濾波器消除測(cè)量誤差及噪點(diǎn)，但由于激光雷達(dá)數(shù)據(jù)具有遠(yuǎn)密近疏特點(diǎn)，使用自適應(yīng)體素濾波器(Adaptive voxel filter)進(jìn)行自適應(yīng)計(jì)算合適的采樣間隔，獲取一定的點(diǎn)云數(shù)量。又因?yàn)闄C(jī)器人在運(yùn)動(dòng)過程中，激光雷達(dá)數(shù)據(jù)會(huì)產(chǎn)生運(yùn)動(dòng)畸變，需要使用里程計(jì)數(shù)據(jù)和IMU數(shù)據(jù)矯正激光畸變。

局部SLAM中，每次得到一幀激光點(diǎn)Scan數(shù)據(jù)之后，與其最近的Submap進(jìn)行匹配，找到最佳位姿，用以構(gòu)建 Submap。激光點(diǎn)Scan插入到Submap中需進(jìn)行坐標(biāo)轉(zhuǎn)換，其轉(zhuǎn)換公式為

(1)

式中ξx——激光點(diǎn)橫坐標(biāo)

ξy——激光點(diǎn)縱坐標(biāo)

ξθ——激光點(diǎn)束擊中點(diǎn)的觀察夾角

Tξ——激光點(diǎn)在Submap坐標(biāo)系下位姿

p——柵格存在障礙物的概率

一定數(shù)量的激光點(diǎn)束構(gòu)建一個(gè)柵格地圖Submap，當(dāng)有一個(gè)新的Laser Scan插入Submap時(shí)，將對(duì)其柵格概率進(jìn)行更新。

在將激光點(diǎn)云插入Submap之前，使用基于Ceres的掃描匹配器對(duì)激光點(diǎn)云相對(duì)于當(dāng)前Submap位姿(ξx,ξy,ξθ)進(jìn)行優(yōu)化，找到一個(gè)激光點(diǎn)云對(duì)應(yīng)最優(yōu)的位姿，使Submap中柵格概率最大化。最優(yōu)位姿是通過計(jì)算激光點(diǎn)云與當(dāng)前Submap匹配程度的誤差，使用Ceres掃描匹配器進(jìn)行迭代位姿使其誤差最小。本文使用殘差函數(shù)來(lái)計(jì)算匹配程度，殘差函數(shù)為

(2)

式中Msmooth——平滑函數(shù)

hk——激光點(diǎn)云數(shù)量

全局SLAM中，通過回環(huán)檢測(cè)和回環(huán)優(yōu)化將Submap形成一幅完整地圖，消除Submap之間的累計(jì)誤差，具體實(shí)現(xiàn)過程可參考文獻(xiàn)[32]。檢測(cè)回環(huán)過程中，如果當(dāng)前的Scan和已創(chuàng)建完成的Submap中的某個(gè)激光點(diǎn)Scan的位姿相似，則認(rèn)為檢測(cè)出回環(huán)。為了降低檢測(cè)的計(jì)算量，提高回環(huán)檢測(cè)的效率，Cartographer算法應(yīng)用分支定界的方法進(jìn)行優(yōu)化搜索策略?；丨h(huán)優(yōu)化過程中，使用稀疏位姿的方法來(lái)優(yōu)化激光點(diǎn)云和子圖的位姿?；丨h(huán)優(yōu)化過程與激光點(diǎn)云插入Submap過程類似，仍使用殘差函數(shù)計(jì)算其誤差。

2.2 機(jī)器人自主導(dǎo)航實(shí)現(xiàn)原理

機(jī)器人自主導(dǎo)航分為機(jī)器人定位和路徑規(guī)劃兩部分。機(jī)器人定位采用AMCL算法，實(shí)現(xiàn)機(jī)器人在二維地圖中定位，機(jī)器人采用Dijkstra實(shí)現(xiàn)路徑規(guī)劃[33-34]。

AMCL定位算法基于粒子濾波，確定機(jī)器人在當(dāng)前已知牛場(chǎng)環(huán)境地圖中的位置。AMCL算法結(jié)構(gòu)如圖6所示，AMCL算法首先將代表機(jī)器人位姿的粒子隨機(jī)分布于已知環(huán)境地圖中，其次通過IMU數(shù)據(jù)或使用擴(kuò)展卡爾曼濾波融合IMU傳感器與里程計(jì)數(shù)據(jù)，建立運(yùn)動(dòng)模型預(yù)測(cè)所有隨機(jī)粒子的位姿，其中運(yùn)動(dòng)模型為

圖6 AMCL算法結(jié)構(gòu)圖

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

(3)

式中xt——機(jī)器人t時(shí)刻的運(yùn)動(dòng)狀態(tài)

xt-1——機(jī)器人t-1時(shí)刻的運(yùn)動(dòng)狀態(tài)

ut-1——控制噪聲

最終根據(jù)激光雷達(dá)數(shù)據(jù)建立測(cè)量模型來(lái)更新所有隨機(jī)粒子的權(quán)重，粒子最終的收斂位置即為機(jī)器人在地圖中的實(shí)際位置。若機(jī)器人初始位姿未知或有誤，此時(shí)全局將注入更多隨機(jī)粒子，隨機(jī)器人的移動(dòng)粒子再次更新，重新定位機(jī)器人在牛場(chǎng)地圖中的位置。

Dijkstra路徑規(guī)劃算法基于單源最短路徑算法計(jì)算從機(jī)器人起始位置到目標(biāo)位置的最短路徑。該算法采用廣度優(yōu)先搜索策略，從起始位置開始遍歷到起始點(diǎn)最近且未訪問頂點(diǎn)的鄰接節(jié)點(diǎn)，遍歷過程中不斷迭代當(dāng)前距離起始位置最近的點(diǎn)，直至遍歷到目標(biāo)位置。

圖7為機(jī)器人導(dǎo)航框架，使用ROS中所提供的AMCL與Move_base功能包來(lái)實(shí)現(xiàn)定位和路徑規(guī)劃[35]功能。首先控制機(jī)器人在牛場(chǎng)環(huán)境中運(yùn)動(dòng)，使用Cartographer算法建立牛場(chǎng)環(huán)境地圖，作為全局代價(jià)地圖。其次機(jī)器人根據(jù)主機(jī)PC或手機(jī)APP下發(fā)目標(biāo)點(diǎn)，使用Dijkstra算法進(jìn)行全局路徑規(guī)劃，計(jì)算機(jī)器人到目標(biāo)位置的最佳路線，作為機(jī)器人的全局路線。最后在實(shí)際機(jī)器人推翻草作業(yè)時(shí)，為了避免機(jī)器人與可能出現(xiàn)的動(dòng)態(tài)障礙物發(fā)生碰撞，根據(jù)激光雷達(dá)實(shí)時(shí)數(shù)據(jù)引入本地代價(jià)地圖，由local_planner模塊通過Dynamic Windows Approaches算法計(jì)算躲避的多條軌跡，對(duì)于每條模擬的運(yùn)動(dòng)軌跡分別計(jì)算軌跡上是否存在障礙物以及與障礙物的距離的分值CostsObstacle、軌跡上點(diǎn)與局部參考路徑最近距離的分值CostsPath和軌跡上點(diǎn)與局部參考路徑終點(diǎn)最近距離分值CostsGoal，綜合評(píng)分計(jì)算公式為

圖7 機(jī)器人導(dǎo)航框架

Score=αCostsObstacle+βCostsPath+

ΓCostsGoal

(4)

式中α、β、?！髟u(píng)分標(biāo)準(zhǔn)的權(quán)重

其權(quán)重取決于機(jī)器人所側(cè)重的性能，通過試驗(yàn)，α、β、Γ分別取值為0.02、0.1和2。通過比較，選擇綜合評(píng)分最小的軌跡作為機(jī)器人下一時(shí)刻的運(yùn)動(dòng)軌跡，并計(jì)算該軌跡上機(jī)器人前進(jìn)的線速度與角速度，通過USB串口發(fā)送至下位機(jī)Arduino主控板，從而實(shí)現(xiàn)自主導(dǎo)航移動(dòng)平臺(tái)的精準(zhǔn)控制。

若機(jī)器人在導(dǎo)航過程中出現(xiàn)了障礙物，根據(jù)柵格代價(jià)地圖進(jìn)行檢測(cè)障礙物與機(jī)器人能否發(fā)生碰撞。機(jī)器人與障礙物距離關(guān)系如圖8所示。

圖8 代價(jià)地圖柵格分類圖

在代價(jià)地圖中，根據(jù)機(jī)器人與障礙物之間距離關(guān)系計(jì)算其柵格灰度，若柵格灰度大于253，表示機(jī)器人與障礙物必然發(fā)生碰撞；若柵格灰度處于[128,252]，則表示機(jī)器人處于碰撞臨界，可能發(fā)生碰撞；若柵格灰度處于(0,127]，表示機(jī)器人此時(shí)處于危險(xiǎn)警戒區(qū)，未來(lái)可能發(fā)生碰撞；若柵格灰度為0，表示機(jī)器人可以自由越過障礙物。

3 性能測(cè)試試驗(yàn)

牛場(chǎng)智能推翻草機(jī)器人可靠性取決于機(jī)器人導(dǎo)航系統(tǒng)的精度，高精度的導(dǎo)航系統(tǒng)是推翻草機(jī)器人能否可靠穩(wěn)定地完成推翻草作業(yè)的前提。推翻草機(jī)器人導(dǎo)航性能測(cè)試試驗(yàn)地點(diǎn)在西北農(nóng)林科技大學(xué)畜牧實(shí)驗(yàn)基地，牛場(chǎng)試驗(yàn)區(qū)域縱向尺寸為72.0 m，橫向尺寸為8.7 m。圖9a為手機(jī)APP控制推翻草機(jī)器人建立牛場(chǎng)環(huán)境地圖過程，圖9b為機(jī)器人進(jìn)行推翻草任務(wù)場(chǎng)景。

圖9 機(jī)器人建圖及推翻草試驗(yàn)

推翻草機(jī)器人性能的評(píng)價(jià)指標(biāo)包括建圖準(zhǔn)確度和導(dǎo)航精度。建圖準(zhǔn)確度衡量標(biāo)準(zhǔn)為所建平面地圖與牛場(chǎng)實(shí)際環(huán)境之間縱向與橫向的幾何偏差；導(dǎo)航精度的衡量標(biāo)準(zhǔn)為測(cè)量機(jī)器人預(yù)規(guī)劃的導(dǎo)航路徑與機(jī)器人實(shí)際運(yùn)動(dòng)路徑的偏差，研究該導(dǎo)航系統(tǒng)精度能否滿足機(jī)器人推翻草作業(yè)的需求。

3.1 牛場(chǎng)環(huán)境地圖精度對(duì)比

機(jī)器人在推翻草作業(yè)時(shí)，由于不同區(qū)域的牧草料分布不均勻，造成了機(jī)器人在行駛過程中負(fù)載不穩(wěn)定，出現(xiàn)機(jī)器人輪胎與地面打滑現(xiàn)象，最終將導(dǎo)致里程計(jì)數(shù)據(jù)誤差。因此在本試驗(yàn)設(shè)計(jì)中，使用 Cartographer算法，分別加載里程計(jì)信息與未加載里程計(jì)信息構(gòu)建牛場(chǎng)環(huán)境地圖。為避免機(jī)器人搭載的硬件誤差、地面不平整等不可控因素干擾，機(jī)器人選擇直線行駛方式，以速度0.25 m/s創(chuàng)建環(huán)境地圖。

創(chuàng)建的環(huán)境地圖如圖10所示，其中紅色實(shí)線部分為牛場(chǎng)圍墻參考線，綠色虛線部分為圍欄參考線，紅色箭頭為機(jī)器人運(yùn)動(dòng)方向。在所建的兩幅環(huán)境地圖中，分別取8個(gè)相對(duì)應(yīng)的關(guān)鍵點(diǎn)A1、B1、C1、D1和A2、B2、C2、D2，利用Rviz軟件中Measure標(biāo)尺工具測(cè)量?jī)煞貓D每?jī)蓚€(gè)關(guān)鍵點(diǎn)之間距離，與實(shí)際建圖區(qū)域尺寸進(jìn)行對(duì)比，結(jié)果如表1所示。

圖10 Cartographer算法建圖精度對(duì)比

表1 牛場(chǎng)環(huán)境地圖相對(duì)誤差

圖10表明，機(jī)器人在未加載里程計(jì)信息時(shí)，所建的牛場(chǎng)環(huán)境地圖在橫向上出現(xiàn)明顯的彎曲，并且隨著機(jī)器人移動(dòng)距離的增加，地圖橫向彎曲越明顯。而加載里程計(jì)信息進(jìn)行創(chuàng)建地圖時(shí)，橫向未出現(xiàn)彎曲情況。

由表1可知，機(jī)器人使用Cartographer算法在未加載里程計(jì)信息情況下，僅使用激光雷達(dá)與IMU傳感器建立牛場(chǎng)環(huán)境地圖，與實(shí)際環(huán)境縱向尺寸偏差最大為2.18 m，橫向尺寸偏差最大為0.06 m。使用Cartographer算法在加載里程計(jì)信息時(shí)，所建地圖與實(shí)際環(huán)境縱向尺寸偏差最大為0.14 m，橫向尺寸偏差最大為0.02 m，橫縱向尺寸偏差均小于未加載里程計(jì)信息時(shí)的橫縱向尺寸偏差。

上述偏差結(jié)果表明，加載里程計(jì)數(shù)據(jù)與IMU傳感器輔助激光雷達(dá)創(chuàng)建的環(huán)境地圖，橫縱向尺寸偏差均小于僅使用IMU傳感器數(shù)據(jù)輔助激光雷達(dá)創(chuàng)建的環(huán)境地圖。

3.2 機(jī)器人導(dǎo)航精度測(cè)試試驗(yàn)

推翻草機(jī)器人在牛場(chǎng)進(jìn)行推翻草作業(yè)時(shí)，需要沿著待推翻草區(qū)域進(jìn)行直線行駛。路徑規(guī)劃如圖11a所示，其中灰色部分為奶牛的飼喂區(qū)域，紅色部分表示機(jī)器人工作時(shí)運(yùn)動(dòng)軌跡，機(jī)器人將沿著A→B→C→D→E→F→G→H路線進(jìn)行行駛?？紤]到機(jī)器人在推翻草作業(yè)時(shí)，前進(jìn)方向上會(huì)受到牧草阻力，機(jī)器人輪胎與地面可能會(huì)存在打滑現(xiàn)象，因此在試驗(yàn)設(shè)計(jì)上，使用AMCL定位算法及Dijkstra路徑規(guī)劃算法，將有無(wú)加載里程計(jì)信息設(shè)置為對(duì)照組試驗(yàn)，探究?jī)煞N情況下推翻草機(jī)器人推翻草過程中的導(dǎo)航精度。

評(píng)價(jià)機(jī)器人導(dǎo)航精度指標(biāo)包括橫向偏差、縱向偏差、航向偏角[36]。機(jī)器人導(dǎo)航精度測(cè)量方法如圖11b所示，設(shè)置機(jī)器人起點(diǎn)為坐標(biāo)原點(diǎn)，以牛場(chǎng)縱向設(shè)為y軸正方向，遵循右手直角坐標(biāo)系。將機(jī)器人導(dǎo)航至目標(biāo)位置的坐標(biāo)設(shè)置為(x0,y0)，以機(jī)器人激光雷達(dá)安裝位置為測(cè)量基準(zhǔn)點(diǎn)，以機(jī)器人前進(jìn)方向?yàn)閥軸正方向，以機(jī)器人正右方為x軸正方向建立直角坐標(biāo)系。機(jī)器人行駛至目標(biāo)位置，記錄其基準(zhǔn)點(diǎn)坐標(biāo)位置為(x1,y1)，其中橫向偏差X計(jì)算公式為

圖11 推翻草機(jī)器人導(dǎo)航路徑及導(dǎo)航精度測(cè)量示意圖

X=|x1-x0|

(5)

縱向偏差Y計(jì)算公式為

Y=|y1-y0|

(6)

航向偏角為機(jī)器人正方向與y軸之間的夾角。試驗(yàn)中分別以加載里程計(jì)信息與未加載里程計(jì)信息設(shè)為對(duì)照組試驗(yàn)，第1組為加載里程計(jì)信息進(jìn)行導(dǎo)航，第2組為未加載里程計(jì)信息進(jìn)行導(dǎo)航，每組分別試驗(yàn)15次。試驗(yàn)結(jié)果如表2所示，根據(jù)試驗(yàn)數(shù)據(jù)分別計(jì)算各評(píng)價(jià)指標(biāo)的平均值與標(biāo)準(zhǔn)差。

表2 導(dǎo)航精度試驗(yàn)結(jié)果

表2表明，機(jī)器人在推翻草作業(yè)時(shí)，加載里程計(jì)信息導(dǎo)航，橫向偏差最大值、縱向偏差最大值及航向偏角最大值分別小于0.600 m、0.160 m和11.0°，標(biāo)準(zhǔn)差分別小于0.040 m、0.050 m和3.0°；未加載里程計(jì)信息時(shí)，橫向偏差最大值、縱向偏差最大值及航向偏角最大值分別小于0.040 m、0.100 m和11.0°，標(biāo)準(zhǔn)差分別小于0.030 m、0.025 m和3.0°。表明機(jī)器人在未加載里程計(jì)信息進(jìn)行導(dǎo)航時(shí)，精度明顯高于加載里程計(jì)信息進(jìn)行導(dǎo)航，滿足牛場(chǎng)復(fù)雜環(huán)境中推翻草作業(yè)自主導(dǎo)航的需求。

未加載里程計(jì)信息導(dǎo)航時(shí)，橫向與縱向偏差平均值均小于加載里程計(jì)信息時(shí)的橫向和縱向偏差平均值，航向偏角誤差基本相同。出現(xiàn)這種現(xiàn)象的原因?yàn)闄C(jī)器人在推翻草作業(yè)時(shí)，由于推翻草負(fù)載的存在，輪胎會(huì)與地面出現(xiàn)輕微打滑現(xiàn)象，導(dǎo)致里程計(jì)數(shù)據(jù)誤差隨著機(jī)器人移動(dòng)距離的增加而累計(jì)增加，進(jìn)而影響導(dǎo)航過程中縱向偏差。而機(jī)器人未加載里程計(jì)信息進(jìn)行推翻草時(shí)，機(jī)器人導(dǎo)航精度明顯增高。

4 結(jié)論

(1)針對(duì)牛場(chǎng)奶牛推送飼料機(jī)械設(shè)備智能化低、人工勞動(dòng)強(qiáng)度大等問題，設(shè)計(jì)了一款基于激光SLAM的自主導(dǎo)航推翻草機(jī)器人。試驗(yàn)表明該機(jī)器人建圖及導(dǎo)航精度能夠?qū)崿F(xiàn)在牛場(chǎng)環(huán)境中推翻草作業(yè)，提高了牛場(chǎng)飼養(yǎng)奶牛的智能化水平。

(2)牛場(chǎng)環(huán)境地圖精度對(duì)比試驗(yàn)結(jié)果表明，機(jī)器人使用Cartographer算法加載里程計(jì)信息，以速度0.25 m/s創(chuàng)建地圖時(shí)，所創(chuàng)建的地圖與實(shí)際環(huán)境的縱向尺寸偏差最大為0.14 m，橫向尺寸偏差最大為0.02 m，地圖精度明顯高于使用Cartographer算法未加載里程計(jì)信息情況。

(3)機(jī)器人導(dǎo)航精度測(cè)試試驗(yàn)結(jié)果表明，機(jī)器人未加載里程計(jì)信息進(jìn)行導(dǎo)航時(shí)，橫向偏差最大值、縱向偏差最大值及航向偏角最大值分別小于0.040 m、0.100 m和11.0°，標(biāo)準(zhǔn)差分別小于0.030 m、0.025 m和3.0°，導(dǎo)航精度高于加載里程計(jì)信息時(shí)的導(dǎo)航精度，滿足機(jī)器人推翻草作業(yè)時(shí)的導(dǎo)航精度要求。