羊舍自主巡檢機(jī)器人設(shè)計(jì)與試驗(yàn)

摘要：

針對規(guī)模化羊舍人工勞動強(qiáng)度大、作業(yè)效率低等問題，設(shè)計(jì)羊舍自主巡檢機(jī)器人系統(tǒng)。該系統(tǒng)基于2D激光雷達(dá)的即時定位與建圖（SLAM）技術(shù)，自主導(dǎo)航系統(tǒng)使用基于擴(kuò)展卡爾曼濾波數(shù)據(jù)融合的AMCL定位算法實(shí)現(xiàn)機(jī)器人的定位，采用Gmapping算法構(gòu)建羊舍環(huán)境地圖，并采用A^*和DWA算法規(guī)劃機(jī)器人巡檢路徑，最終實(shí)現(xiàn)舍內(nèi)地圖構(gòu)建、系統(tǒng)實(shí)時定位、多目標(biāo)點(diǎn)路徑規(guī)劃以及動態(tài)避障。機(jī)器人導(dǎo)航精度試驗(yàn)表明，巡檢機(jī)器人橫向偏差均值不超過8.3cm，縱向偏差均值不超過7.1cm；航向偏差的均值不超過6.1°。該系統(tǒng)構(gòu)建的環(huán)境地圖精度較高，能夠?qū)崿F(xiàn)對機(jī)器人的有效定位，并且能夠以實(shí)現(xiàn)羊舍的循環(huán)巡航和自動躲避周邊障礙物，導(dǎo)航精度滿足羊舍自主導(dǎo)航需求。

關(guān)鍵詞：羊舍；巡檢機(jī)器人；多傳感器融合；路徑規(guī)劃；擴(kuò)展卡爾曼濾波

中圖分類號：S24

文獻(xiàn)標(biāo)識碼：A

文章編號：2095-5553 （2025） 01-0078-07

Design and experiment of autonomous inspection robot for sheep house

Zhao Ninglei¹， Cheng Man¹， Yuan Hongbo¹， Wang Yuan²， Cai Zhenjiang¹

（1. College of Mechanical and Electrical Engineering， Hebei Agricultural University， Baoding， 071001， China;

2. Hengshui Zhihao Animal Husbandry Technology Co.， Ltd.， Hengshui， 053400， China）

Abstract：

In order to solve the problems of high labor intensity and low working efficiency of large-scale sheep house， an autonomous inspection robot system for sheep house is designed. The system is based on the real-time positioning and mapping （SLAM） technology of 2D LiDAR. The autonomous navigation system uses the AMCL positioning algorithm based on extended Kalman filter data fusion to realize the positioning of the robot， uses the Gmapping algorithm to construct the environment map of the sheep shed， and uses the A^*"and DWA algorithms to plan the inspection path of the robot. Finally， it realizes the construction of the map in the house， real-time positioning of the system， multi-objective point path planning and dynamic obstacle avoidance. The robot navigation accuracy test shows that the average lateral deviation of the patrol robot does not exceed 8.3cm， the average longitudinal deviation does not exceed 7.1cm， and the average heading deviation does not exceed 6.1°. The environmental map constructed by this system has high accuracy， can effectively locate the robot， and can realize the cyclic cruising and cruising of the sheep house.

Keywords：

sheep house; inspection robot; multi-sensor fusion; path planning; extended kalman filtering

0"引言

近年來，中國畜禽養(yǎng)殖業(yè)規(guī)模和產(chǎn)值不斷增加，2022年總產(chǎn)值達(dá)到4.1萬億元，約占農(nóng)業(yè)總產(chǎn)值的30%^［1］，且畜禽產(chǎn)品種類、總量和產(chǎn)值等均距世界前列。然而，隨著畜禽養(yǎng)殖集約化程度不斷提高，高密度養(yǎng)殖對畜禽舍內(nèi)動物的健康狀況形成巨大的挑戰(zhàn)^［2］。近年來，隨著養(yǎng)殖模式集約化和標(biāo)準(zhǔn)化程度的不斷提高，研發(fā)和應(yīng)用全自動移動式機(jī)器人，可有效促進(jìn)養(yǎng)殖舍內(nèi)智能化高效管理^［3］。傳統(tǒng)的羊場巡檢方式中，人工巡檢方式不僅存在工作強(qiáng)度大、作業(yè)效率低等不足^［4］，且羊自身的傳染病還有很大幾率傳染給巡檢人員，存在較大的人身安全隱患問題^［5］。傳統(tǒng)的視頻監(jiān)控系統(tǒng)，由于存在較大的監(jiān)控盲區(qū)很難做到視頻監(jiān)控全方位覆蓋，同時由于整個系統(tǒng)使用攝像頭數(shù)量多、安裝布線復(fù)雜、綜合系統(tǒng)龐大，因此故障率較高，維護(hù)成本大^［6］。使用智能移動機(jī)器人進(jìn)行羊場環(huán)境的巡檢，不僅可以為企業(yè)降低生產(chǎn)成本、提高生產(chǎn)效率、擴(kuò)大企業(yè)的行業(yè)競爭力^［7］，也可以有效避免人工巡檢及傳統(tǒng)視頻監(jiān)控過程中存在的缺陷和不足。

針對羊場羊只巡檢的需要，設(shè)計(jì)一種上下位機(jī)結(jié)合的智能巡檢機(jī)器人，智能巡檢機(jī)器人采用PID控制的四輪差速移動底盤，保證其靈活性以及足夠的檢測范圍。應(yīng)用Gmapping算法并開發(fā)上位機(jī)界面，實(shí)現(xiàn)機(jī)器人的自主構(gòu)建地圖和實(shí)時獲取和存儲巡檢目標(biāo)點(diǎn)的功能。同時利用A^*和DWA路徑規(guī)劃算法對每一個記錄的目標(biāo)點(diǎn)依次進(jìn)行路徑規(guī)劃和自主導(dǎo)航從而滿足羊場巡檢的需要。

1"巡檢機(jī)器人系統(tǒng)設(shè)計(jì)

1.1"巡檢機(jī)器人結(jié)構(gòu)設(shè)計(jì)

機(jī)器人整體框架由40mm×40mm的歐標(biāo)鋁型材搭建，整車長為55cm、寬為55cm、高為65cm。機(jī)器人在室內(nèi)巡檢時的數(shù)據(jù)采集場景如圖1所示。整車采用四輪四驅(qū)獨(dú)立轉(zhuǎn)向機(jī)構(gòu)，由４組直角直流減速電機(jī)、４組驅(qū)動器、４組AGV充氣輪組成，并通過４組長為255mm，寬為84mm，厚度為5mm的自制電機(jī)架將電機(jī)固定在鋁型材上，系統(tǒng)所選用的電機(jī)安裝于內(nèi)側(cè)，電機(jī)與電機(jī)架的整體寬度約為95mm。機(jī)器人正前方中央放置一個RPLIDAR S1單線激光雷達(dá)，可以獲取其探測范圍內(nèi)雷達(dá)平面范圍內(nèi)的二維點(diǎn)云數(shù)據(jù)。底盤中央安置了一組24V 40Ah的鋰電池，是所有傳感器、動力裝置的動力來源，能保證平臺在設(shè)備滿功率狀態(tài)下續(xù)航4h。主控器和從控制器單元放置在電源旁邊的盒子里。

1.2"巡檢機(jī)器人硬件設(shè)計(jì)與選型

機(jī)器人主要硬件組成如表1所示，該機(jī)器人硬件主要由傳感器模塊、控制模塊和驅(qū)動模塊組成。機(jī)器人系統(tǒng)的硬件框架如圖2所示。Jetson Nano主控制器通過串口驅(qū)動激光雷達(dá)獲取平面點(diǎn)云信息，STM32通過獲取編碼器信息，計(jì)算得出機(jī)器人的線速度與角速度，并通過PWM驅(qū)動電機(jī)控制板，進(jìn)而控制電機(jī)的速度，從而控制機(jī)器人的移動。此外，還通過模擬I2C協(xié)議獲取JY901的陀螺儀和加速度信息，將機(jī)器人的速度、陀螺儀和加速度等信息通過串口上傳到主控制器。主控制器通過獲取從控制器發(fā)過來的速度信息計(jì)算得出機(jī)器人的里程計(jì)、陀螺儀和加速度信息。

2"巡檢功能設(shè)計(jì)

2.1"基于擴(kuò)展卡爾曼濾波數(shù)據(jù)融合的AMCL定位算法

機(jī)器人定位采用擴(kuò)展卡爾曼濾波數(shù)據(jù)融合的AMCL算法，AMCL定位算法基于粒子濾波，確定機(jī)器人當(dāng)前的位置^［8］。首先，該算法將代表機(jī)器人位姿的粒子隨機(jī)分布于已知環(huán)境地圖中。其次，通過使用擴(kuò)展卡爾曼濾波融合IMU傳感器與里程計(jì)數(shù)據(jù)，建立運(yùn)動模型預(yù)測所有隨機(jī)粒子的位姿。擴(kuò)展卡爾曼濾波算法通過系統(tǒng)的觀測數(shù)據(jù)及系統(tǒng)狀態(tài)對全局最優(yōu)狀態(tài)進(jìn)行推算，并通過上一狀態(tài)和下一狀態(tài)來預(yù)測當(dāng)前狀態(tài)，其計(jì)算如式（1）所示。

u_t-=Gtut-1+Htut

（1）

式中：

Gt——

當(dāng)前時刻的狀態(tài)轉(zhuǎn)移矩陣（就是指從上一狀態(tài)轉(zhuǎn)變?yōu)橄乱粻顟B(tài)的關(guān)系矩陣）;

Ht——

當(dāng)前時刻的控制矩陣（就是指影響控制量的控制矩陣）；

ut——

當(dāng)前時刻的控制量；

ut-1——

上一時刻最優(yōu)估計(jì)值；

u_t-——當(dāng)前時刻估計(jì)值。

預(yù)測均值的協(xié)方差來計(jì)算Kalman增益，其中協(xié)方差的計(jì)算如式（2）所示。

t=Gtt-1Gt^T+R_t

（2）

式中：

t-1——上一時刻預(yù)測值方差矩陣;

t——不確定性的協(xié)方差矩陣;

Rt——當(dāng)前時刻測量值噪聲矩陣。

Kalman增益的計(jì)算如式（3）所示。

Kt=tWt^T（Wtt+Qt）^-1

（3）

式中：

Kt——卡爾曼增益（指的就是權(quán)值）；

Wt——測量方程；

Qt——觀察量的協(xié)方差矩陣。

通過Kalman增益來計(jì)算狀態(tài)估計(jì)值，其估計(jì)位姿計(jì)算如式（4）所示。

pt=u_t-+Kt［zt-h（u_t-）］

（4）

式中：

zt——傳感器在t時刻的測量值；

pt——估計(jì)位姿。

預(yù)測當(dāng)前狀態(tài)需要用到上一狀態(tài)的協(xié)方差，還需要計(jì)算當(dāng)前狀態(tài)的協(xié)方差用于下一次迭代。其中，當(dāng)前狀態(tài)的協(xié)方差矩陣計(jì)算如式（5）所示。

t=（1-KtWt）t

（5）

由式（4）和式（5）得到了里程計(jì)與IMU數(shù)據(jù)融合后的最佳估計(jì)位姿ut，以及它的協(xié)方差矩陣t。最后根據(jù)激光雷達(dá)數(shù)據(jù)建立測量模型來更新所有隨機(jī)粒子的權(quán)重，粒子最終的收斂位置即為機(jī)器人在地圖中的實(shí)際位置。若機(jī)器人初始位姿未知或有誤，此時全局將注入更多隨機(jī)粒子，隨機(jī)器人的移動粒子再次更新，重新定位機(jī)器人在羊舍地圖中的位置。

2.2"羊舍地圖構(gòu)建

采用Gmapping算法對羊舍進(jìn)行地圖的構(gòu)建。Gmapping是一種基于Rao-Blackwellized粒子濾波的二維激光SLAM方法^［9］。即將定位和建圖過程分離，先進(jìn)行定位再進(jìn)行建圖。為了解決RBPF頻繁重采樣的缺點(diǎn)，Gmapping提出了選擇性重采樣，即減少重采樣的次數(shù)，設(shè)定閾值；當(dāng)粒子權(quán)重大于所設(shè)閾值才執(zhí)行重采樣，這樣便減少了采樣的次數(shù)，即減緩了粒子的退化^［10］。里程計(jì)根據(jù)測量的速度可以推導(dǎo)出機(jī)器人的路程和方向，Gmapping算法依據(jù)里程計(jì)的測量數(shù)據(jù)估計(jì)機(jī)器人的運(yùn)動模型，高精度的里程計(jì)對于建圖和定位性能的提高十分重要。因此，首先對Gmapping功能包中的雷達(dá)最大檢測距離、粒子個數(shù)以及機(jī)器人激光匹配頻率進(jìn)行調(diào)試并取得最優(yōu)值，最后利用Gmapping功能包訂閱RPLIDAR S1發(fā)出的雷達(dá)話題信息和擴(kuò)展卡爾曼濾波融合的位姿信息進(jìn)行小車的精確定位和建圖。

2.3"巡檢路線規(guī)劃

2.3.1"巡檢目標(biāo)點(diǎn)獲取

在巡檢機(jī)器人對羊舍環(huán)境進(jìn)行建模并得到二維柵格地圖后，需要獲取地圖上的目標(biāo)點(diǎn)坐標(biāo)信息從而實(shí)現(xiàn)巡檢的功能，為了獲取地圖上的目標(biāo)點(diǎn)坐標(biāo)信息，用Python QT構(gòu)建一個簡單的界面，界面如圖3所示。

該界面通過點(diǎn)擊獲取節(jié)點(diǎn)的按鍵監(jiān)聽base_footprint與map坐標(biāo)的tf，從而得到機(jī)器人實(shí)時位置，通過點(diǎn)擊記錄位點(diǎn)的按鍵將獲取的機(jī)器人位置以json的數(shù)據(jù)格式存儲在當(dāng)前功能包的data目錄下?？梢栽谏瞎?jié)進(jìn)行手動建圖時獲取并記錄各個目標(biāo)點(diǎn)的位置信息，以便在導(dǎo)航時可以直接讀取各個目標(biāo)點(diǎn)的位置信息進(jìn)行巡檢導(dǎo)航。

2.3.2"巡檢路線生成

根據(jù)已經(jīng)獲取的巡檢線路上的目標(biāo)點(diǎn)信息，生成一條巡檢路線，并發(fā)布到路徑規(guī)劃器中。開啟前需要將機(jī)器人放置到巡檢的固定起點(diǎn)，打開定位節(jié)點(diǎn)，并載入建好的羊舍地圖，機(jī)器人實(shí)際巡檢路線如圖4所示。

如圖4所示，機(jī)器人將沿著A→Ｂ→Ｃ→Ｄ→Ｅ→D→C→B→A路線行駛，車體的正前方為x軸正方向，左方為y軸正方向。2.3.1節(jié)已經(jīng)通過QT界面獲取到各個目標(biāo)點(diǎn)相對于地圖的xy坐標(biāo)和朝向信息并存儲在指定文件夾下。用C++中的ifstream類依次讀取文件中的坐標(biāo)，之后用ROS中的LINE_STRIP類型將每個點(diǎn)用作一組連接的線中的頂點(diǎn)，其中點(diǎn)A連接到點(diǎn)B，點(diǎn)B連接到點(diǎn)C，點(diǎn)C連接到點(diǎn)D等，這樣就生成一條巡檢線路，最后以trajectory命名建立一個話題，把坐標(biāo)點(diǎn)位姿信息發(fā)布出去。在RVIZ可視化界面訂閱此話題，路線就會顯示在建好的地圖上。

機(jī)器人的巡檢方式實(shí)際是依次遍歷巡檢線路上的每個目標(biāo)點(diǎn)位置信息從而實(shí)現(xiàn)循環(huán)往返，這是通過訂閱發(fā)布的trajectory話題將規(guī)劃線路傳入到自定義的全局規(guī)劃器。此過程首先通過監(jiān)聽base_footprint與map坐標(biāo)系的tf獲取機(jī)器人當(dāng)前位置，以當(dāng)前位置作為開始的規(guī)劃變量。然后，在for循環(huán)中依次遍歷插入巡檢路線上的各個目標(biāo)點(diǎn)的位置，同時對tf進(jìn)行監(jiān)聽獲取導(dǎo)航線上各個點(diǎn)的位置，獲取到所有目標(biāo)點(diǎn)位置后判斷機(jī)器人當(dāng)前位置與目標(biāo)點(diǎn)的相對距離，其中，目標(biāo)點(diǎn)與當(dāng)前位置相對距離的計(jì)算如式（6）所示。最后，根據(jù)式（6）將距離機(jī)器人當(dāng)前位置最近的目標(biāo)位置相對距離插入規(guī)劃。這里設(shè)置為首先規(guī)劃距離當(dāng)前位置最近的目標(biāo)點(diǎn)是為了防止機(jī)器人在巡檢過程中出現(xiàn)意外情況被迫停止巡檢時需要回到起點(diǎn)從頭開始巡檢的情況，這樣即使在巡檢過程中出現(xiàn)情況也可以在當(dāng)前位置直接規(guī)劃離自己最近的目標(biāo)點(diǎn)從而之后按照目標(biāo)點(diǎn)順序繼續(xù)巡檢。

Distance=［p2（x）-p1（x）］²+［p2（y）-p₁（y）］²

（6）

式中：

Distance——目標(biāo)點(diǎn)與當(dāng)前位置相對距離；

p2（x）——目標(biāo)點(diǎn)相對于地圖的x坐標(biāo)；

p1（x）——當(dāng)前位置點(diǎn)相對于地圖的x坐標(biāo)；

p2（y）——目標(biāo)點(diǎn)相對于地圖的y坐標(biāo)；

p1（y）——當(dāng)前位置點(diǎn)相對于地圖的y坐標(biāo)。

除了發(fā)布相對距離之外還需要發(fā)表各個目標(biāo)點(diǎn)朝向來使巡檢過程更加順暢，機(jī)器人的姿態(tài)信息通過訂閱IMU話題信息得到，其姿態(tài)信息由四元數(shù)表示。由圖4的巡檢路線可知，A-B段路線上的目標(biāo)點(diǎn)朝向只需和機(jī)器人起點(diǎn)的姿態(tài)保持一致，而B-C段路線上的目標(biāo)點(diǎn)朝向相比起點(diǎn)姿態(tài)需要繞Z軸逆時針旋轉(zhuǎn)90°；C-D段路線上的目標(biāo)點(diǎn)朝向相比起點(diǎn)姿態(tài)同樣跟開機(jī)方向保持一致；D-E段路線上的目標(biāo)點(diǎn)朝向相比起點(diǎn)姿態(tài)需要繞Z軸順時針旋轉(zhuǎn)90°。給定一個歐拉旋轉(zhuǎn)（X，Y，Z）（即分別繞x軸、y軸和z軸旋轉(zhuǎn)X、Y、Z角度），歐拉角與四元數(shù)的轉(zhuǎn)換計(jì)算如式（7）所示。

x=

sinY2sinZ2cosX2+cosY2

cosZ2sinX2

y=

sinY2cosZ2cosX2+cosY2

sinZ2sinX2

z=

cosY2sinZ2cosX2-sinY2

cosZ2sinX2

w=

cosY2sinZ2cosX2-sinY2

cosZ2sinX2

（7）

機(jī)器人開機(jī)的朝向即為機(jī)器人正方向，如圖4所示機(jī)器人在A-B段路線的目標(biāo)點(diǎn)朝向與開機(jī)方向一致，所以A-B段路線上目標(biāo)點(diǎn)的歐拉旋轉(zhuǎn)為（0，0，0）對應(yīng)的四元數(shù)為：x=0，y=0，z=0，w=1。B-C段路線上的目標(biāo)點(diǎn)的歐拉旋轉(zhuǎn)為（0，0，-90），對應(yīng)的四元數(shù)為：x=0，y=0，z=-0.707，w=-0.707。C-D段路線上目標(biāo)點(diǎn)的歐拉旋轉(zhuǎn)為（0，0，0）對應(yīng)的四元數(shù)為：x=0，y=0，z=0，w=1。D-E段路線上目標(biāo)點(diǎn)的歐拉旋轉(zhuǎn)為（0，0，90），對應(yīng)的四元數(shù)為：x=0，y=0，z=0.707，w=0.707。

最后，將各個目標(biāo)點(diǎn)之間的相對距離和朝向信息依次發(fā)送到move_base全局規(guī)劃模塊，返回原點(diǎn)的各段目標(biāo)點(diǎn)朝向可以根據(jù)上述公式類推得到。該模塊將通過ROS話題global_plan進(jìn)行發(fā)布進(jìn)行路徑規(guī)劃，然后將由本地規(guī)劃模塊接收并發(fā)布控制指令。這樣就實(shí)現(xiàn)了機(jī)器人巡檢線路生成以及路線上目標(biāo)點(diǎn)位姿的發(fā)布。

2.4"路徑規(guī)劃算法

2.4.1"A^*全局路徑規(guī)劃

在將目標(biāo)點(diǎn)路徑和朝向信息發(fā)布后，機(jī)器人按照規(guī)劃的路徑進(jìn)行移動。在全局路徑規(guī)劃中通過點(diǎn)對點(diǎn)路徑規(guī)劃可以實(shí)現(xiàn)巡檢機(jī)器人從起始點(diǎn)到目標(biāo)點(diǎn)的最優(yōu)路徑選擇，本文采用A^*算法進(jìn)行全局路徑規(guī)劃。

A^*算法因?yàn)榭梢钥焖俚匾?guī)劃出最短路徑，所以多用在全局路徑規(guī)劃算法。A^*算法將啟發(fā)式算法融入到評估函數(shù)里^［11］，其評估函數(shù)計(jì)算如式（8）所示。

F（n）=G（n）+H（n）

（8）

式中：

F（n）——綜合代價(jià)值，即評估函數(shù)；

G（n）——起點(diǎn)到當(dāng)前點(diǎn)的實(shí)際代價(jià)值；

H（n）——當(dāng)前點(diǎn)到目標(biāo)點(diǎn)的估計(jì)代價(jià)值。

A^*算法中的估計(jì)代價(jià)值H（n）是優(yōu)于其他算法的核心，在路徑規(guī)劃過程中必須保證當(dāng)前點(diǎn)到目標(biāo)點(diǎn)的估計(jì)代價(jià)值低于實(shí)際距離。選用歐幾里得距離作為啟發(fā)函數(shù)以保證降低A^*算法計(jì)算量的同時，進(jìn)一步優(yōu)化全局最優(yōu)的表達(dá)。

2.4.2"DWA局部路徑規(guī)劃

由于羊場環(huán)境的復(fù)雜性，若機(jī)器人在巡檢過程中突然出現(xiàn)一個動態(tài)障礙物，這時僅使用A^*算法進(jìn)行路徑規(guī)劃是不能實(shí)現(xiàn)動態(tài)避障的。DWA算法是一種局部路徑規(guī)劃算法，適用于移動機(jī)器人等需要避開障礙物的場景。其主要流程包含：初始化、采樣速度樣本、樣本評分以及發(fā)布方案四部分^［12］。首先，通過類加載模塊載入ROS局部路徑規(guī)劃包 Base Local Planner中子類的實(shí)例，并調(diào)用其初始化函數(shù)獲取機(jī)器人的初始狀態(tài)信息，進(jìn)一步得到巡檢機(jī)器人的軌跡運(yùn)動模型。基于軌跡運(yùn)動模型，機(jī)器人根據(jù)其運(yùn)動速度即可推算出其運(yùn)動軌跡。為了獲得機(jī)器人運(yùn)動速度樣本，需在一定時間間隔范圍內(nèi)通過傳感器對機(jī)器人各個維度的線速度和角速度進(jìn)行采集，并將其以結(jié)構(gòu)體的形式存儲到相應(yīng)的容器內(nèi)。在得到機(jī)器人速度樣本后，根據(jù)機(jī)器人采樣速度模擬推演出其相對應(yīng)的運(yùn)動軌跡，并通過軌跡評價(jià)函數(shù)對每一條軌跡進(jìn)行評價(jià)。評價(jià)函數(shù)的計(jì)算如式（9）所示。

G（v，w）=

max（φhead（v，w）+βdist（v，w））+δvelo（v，w））

（9）

式中：

head（v，w）——速度軌跡與目標(biāo)點(diǎn)的接近程度；

dist（v，w）——

該采樣速度下運(yùn)動估計(jì)距離最近障礙物的距離；

velo（v，w）——

機(jī)器人在該速度組下的前進(jìn)效率；

φ、β、δ——

三個不同子項(xiàng)在評價(jià)函數(shù)中所占比重。

利用式（9）評價(jià)所有軌跡，選擇評分最高的軌跡為局部規(guī)劃的結(jié)果輸出給move_base控制機(jī)器人移動。機(jī)器人在路徑規(guī)劃過程中通過RPLIDAR S1激光雷達(dá)檢測周圍環(huán)境障礙物信息，若機(jī)器人在導(dǎo)航過程中出現(xiàn)了動態(tài)障礙物，根據(jù)move_base中的柵格代價(jià)地圖（Cost map）模塊進(jìn)行檢測障礙物與機(jī)器人能否發(fā)生碰撞。代價(jià)地圖通常采用網(wǎng)格（grid）的形式，每個網(wǎng)格可以存儲0～255之間不同的數(shù)值，不同的數(shù)值代表該網(wǎng)格離障礙物的遠(yuǎn)近程度。根據(jù)數(shù)值所在區(qū)間判斷當(dāng)前屬于行駛自由區(qū)域還是受限區(qū)域。若為行駛自由區(qū)域，則不需要進(jìn)行其他操作，與之前軌跡保持一致即可；若為行駛受限區(qū)域，則進(jìn)一步獲取機(jī)器人模擬軌跡的角速度，當(dāng)角速度超過一定閾值（若以該閾值進(jìn)行移動，將進(jìn)入受限區(qū)域）后，對該軌跡的權(quán)重加大，并降低軌跡評分，進(jìn)而選擇更優(yōu)軌跡進(jìn)行運(yùn)動控制。最終實(shí)現(xiàn)機(jī)器人的局部路徑規(guī)劃與實(shí)時避障。

3"現(xiàn)場試驗(yàn)與結(jié)果分析

現(xiàn)場試驗(yàn)地點(diǎn)位于河北省衡水市某公司的8號和9號種羊舍。兩個羊舍分別長為60m，寬為8m；總計(jì)70個羊圈。在試驗(yàn)過程中需要將參數(shù)設(shè)置統(tǒng)一，線速度和角速度分別調(diào)整為0.30m/s和0.3rad/s。另外采用RVIZ進(jìn)行地圖可視化。RVIZ是ROS中常用的可視化工具，可以在建模地圖中顯示各傳感器的數(shù)據(jù)。保證PC端與機(jī)器人端連上統(tǒng)一的局域網(wǎng)，然后通過終端控制使得在PC端輸入指令可以控制機(jī)器人。

3.1"機(jī)器人建圖試驗(yàn)

在實(shí)際測試環(huán)境中分別采用Hector SLAM、Cartographer SLAM及Gmapping SLAM算法構(gòu)建環(huán)境地圖，并分析相同環(huán)境下的地圖精度。3種SLAM在現(xiàn)場環(huán)境中構(gòu)建的地圖如圖5所示。3種算法都完成了羊舍地圖的構(gòu)建，但從圖5（a）中橢圓標(biāo)注的地方可看出，Hector SLAM的建圖結(jié)果產(chǎn)生了與實(shí)際環(huán)境不符的偏移。Cartographer SLAM算法雖采用回環(huán)檢測以校正誤差，但構(gòu)建的環(huán)境地圖出現(xiàn)了部分墻壁線條傾斜偏離實(shí)際環(huán)境的問題，如圖5（b）圓圈所示。相比而言，采用經(jīng)過擴(kuò)展卡爾曼濾波融合里程計(jì)和IMU信息后的Gmapping算法構(gòu)建的環(huán)境地圖邊緣更平整且整體方正，如圖5（c）所示，最符合羊舍環(huán)境的實(shí)際情況。

3.2"機(jī)器人定位精度測試試驗(yàn)

為驗(yàn)證巡檢機(jī)器人定位精度，試驗(yàn)將在羊舍場景下對機(jī)器人定位精度進(jìn)行測試。評價(jià)機(jī)器人導(dǎo)航精度指標(biāo)包括橫向偏差、縱向偏差、航向偏角^［13］。設(shè)置機(jī)器人起點(diǎn)為坐標(biāo)原點(diǎn)，隨機(jī)選取巡航路徑上的4個巡航點(diǎn)。將設(shè)置好的巡航點(diǎn)設(shè)置為（x0，y0），以機(jī)器人底盤中心為測量基準(zhǔn)點(diǎn)，機(jī)器人行駛至目標(biāo)位置，記錄其基準(zhǔn)點(diǎn)坐標(biāo)位置為（x1，y1），巡檢機(jī)器人定位精度測試試驗(yàn)示意圖如圖6所示，其中橫向偏差d_x和縱向偏差d_y的計(jì)算如式（10）和式（11）所示。

d_x=x1-x0

（10）

d_y=y1-y0

（11）

航向偏角為機(jī)器人正方向與y軸之間的夾角。現(xiàn)場測量采樣點(diǎn)與實(shí)際到達(dá)點(diǎn)的位置偏差如圖7所示。圖7（b）中箭頭為設(shè)置的實(shí)際目標(biāo)點(diǎn)，兩條實(shí)線為到達(dá)指定位置時機(jī)器人的鋁型材橫杠相對于地面的投影，虛線為兩條實(shí)線中心點(diǎn)的連線，其用于確定機(jī)器人到達(dá)目標(biāo)點(diǎn)位置時機(jī)器人的中心，虛線中心的短橫線即為機(jī)器人到達(dá)指定位置時的底盤中心點(diǎn)。

其中，該機(jī)器人的起點(diǎn)和目標(biāo)點(diǎn)在全局坐標(biāo)系中的位置和角度如表2所示。

將各目標(biāo)點(diǎn)處10次試驗(yàn)的各指標(biāo)結(jié)果分別取平均值作為巡檢機(jī)器人在該目標(biāo)點(diǎn)處的定位偏差。機(jī)器人到達(dá)各個采樣點(diǎn)的實(shí)際位置與采樣點(diǎn)位置的橫向絕對偏差、縱向絕對偏差和航向角度絕對偏差如圖8所示。

由圖8可知，巡檢機(jī)器人在0.3m/s的速度下，橫向平均絕對位置偏差小于7cm，縱向平均絕對位置偏差小于8cm，航向偏角平均絕對位置偏差小于6°。該精度滿足巡檢機(jī)器人在羊舍環(huán)境中的定位要求。

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

機(jī)器人在羊舍進(jìn)行巡檢作業(yè)時，需要沿著羊舍可通行區(qū)域進(jìn)行直線行駛。路徑規(guī)劃如圖4所示。機(jī)器人將沿著A→B→C→D→E→D→C→B→A路線進(jìn)行行駛。機(jī)器人在巡檢線路上設(shè)置了77個目標(biāo)點(diǎn)，這些目標(biāo)點(diǎn)的位姿信息以json文件格式存儲在系統(tǒng)中，在導(dǎo)航時通過訂閱amcl_pose話題來實(shí)時獲取機(jī)器人在map坐標(biāo)系下位姿信息。通過比較設(shè)置目標(biāo)點(diǎn)位姿與導(dǎo)航時實(shí)時獲取到的目標(biāo)點(diǎn)位姿的誤差來評價(jià)其導(dǎo)航精度。將各目標(biāo)點(diǎn)處3次試驗(yàn)的各指標(biāo)結(jié)果分別取平均值作為巡檢機(jī)器人在該目標(biāo)點(diǎn)處的導(dǎo)航偏差。

機(jī)器人到達(dá)各個目標(biāo)點(diǎn)時實(shí)時采集的實(shí)際位置與設(shè)置目標(biāo)點(diǎn)的橫向絕對誤差、縱向絕對誤差和航向偏角絕對誤差如圖9所示。

總體而言，巡檢機(jī)器人在對設(shè)置目標(biāo)點(diǎn)進(jìn)行巡檢導(dǎo)航時橫向偏差最大值小于21cm，平均值不超過8.3cm，縱向偏差最大值小于19cm，平均值不超過7.1cm；航向偏差的平均值不超過6.1°。該精度可滿足巡檢機(jī)器人在羊舍環(huán)境下的導(dǎo)航精度需求。

3.4"機(jī)器人避障試驗(yàn)

為了測試機(jī)器人的路徑規(guī)劃和導(dǎo)航避障實(shí)際效果，進(jìn)行導(dǎo)航避障試驗(yàn)，試驗(yàn)步驟與定位精度試驗(yàn)大體一致。不同之處在于路徑規(guī)劃之后，在其規(guī)劃的路徑上設(shè)置動態(tài)障礙物，當(dāng)RPLIDAR S1激光雷達(dá)檢測到障礙物并通過代價(jià)地圖模塊判斷到機(jī)器人即將與障礙物相撞時，機(jī)器人內(nèi)部程序啟動DWA算法進(jìn)行避障繞行，經(jīng)一系列計(jì)算后，機(jī)器人得到最優(yōu)軌跡作為避障路徑，這條路徑對應(yīng)的角速度和線速度組合則通過ROS系統(tǒng)傳遞給STM32控制板塊，機(jī)器人得到速度信息后進(jìn)行接下來的避讓行進(jìn)，如圖10所示。

由圖10可知，該機(jī)器人在巡檢過程中能夠檢測并識別出動態(tài)人形障礙物，并且可以順利避開動態(tài)障礙物并到達(dá)目標(biāo)點(diǎn)，綜上所述，該機(jī)器人符合在羊舍中的避障要求。

4"結(jié)論

1） 設(shè)計(jì)一種基于2D激光雷達(dá)SLAM技術(shù)的羊舍巡檢機(jī)器人導(dǎo)航控制系統(tǒng)，提出利用擴(kuò)展卡爾曼濾波融合里程計(jì)和IMU數(shù)據(jù)得到的最佳位姿作為預(yù)測所有隨機(jī)粒子的位姿，該方法提高激光雷達(dá)作為測量模型進(jìn)行更新的準(zhǔn)確性，實(shí)現(xiàn)羊舍的建圖與定位功能。

2） 針對傳統(tǒng)多目標(biāo)點(diǎn)路徑規(guī)劃需要在每次巡檢之前在RVIZ界面上手動設(shè)置目標(biāo)點(diǎn)的問題，利用QT界面在手動控制機(jī)器人建圖時進(jìn)行目標(biāo)點(diǎn)的設(shè)置與保存，在之后的巡檢中直接依次遍歷保存的目標(biāo)點(diǎn)信息即可實(shí)現(xiàn)羊舍的往返巡檢。

3） 將建圖定位與路徑規(guī)劃算法應(yīng)用到巡檢機(jī)器人上，結(jié)果表明該機(jī)器人可以完整地在羊舍實(shí)現(xiàn)往返巡檢并規(guī)避障礙物。巡檢機(jī)器人導(dǎo)航橫向平均偏差小于8.3cm，導(dǎo)航縱向平均偏差小于7.1cm，航向平均偏差小于 6.1°。通過試驗(yàn)證明，該方法能夠滿足羊舍的自主導(dǎo)航需求。

參"考"文"獻(xiàn)

［1］

何沛桐， 張建華， 張凝， 等. 基于視覺感知的畜禽智慧養(yǎng)殖管理與疫病診斷研究進(jìn)展［J］. 中國農(nóng)業(yè)大學(xué)學(xué)報(bào)， 2023， 28（10）： 141-165.

He Peitong， Zhang Jianhua， Zhang Ning， et al. Research progress in intelligent livestock and poultry breeding management and disease diagnosis based on visual perception ［J］. Journal of China Agricultural University， 2023， 28（10）： 141-165.

［2］

姚文， 鄧為， 許毅， 等. 家禽亞健康狀態(tài)監(jiān)測與健康預(yù)警展望［J］. 南京農(nóng)業(yè)大學(xué)學(xué)報(bào)， 2023， 46（4）： 635-644.

Yao Wen， Deng Wei， Xu Yi， et al. Poultry sub-health status monitoring and health warning prospect ［J］. Journal of Nanjing Agricultural University， 2023， 46（4）： 635-644.

［3］

Hu D， Zhong H， Li S， et al. Segmenting areas of potential contamination for adaptive robotic disinfection in built environments ［J］. Building and Environment， 2020， 184： 107226.

［4］

賀曉峰， 廖志偉， 肖華明， 等. 智能巡檢機(jī)器人傳感與控制系統(tǒng)研究與設(shè)計(jì)［J］. 中國煤炭， 2022， 48（S1）： 1-5.

He Xiaofeng， Liao Zhiwei， Xiao Huaming， et al. Research and design of intelligent sensing and control system of inspection robot ［J］. China Coal， 2022， 48 （S1）： 1-5.

［5］

汪陽， 劉文鍇， 張凌， 等. 羊巴氏桿菌病研究進(jìn)展［J］. 畜牧與獸醫(yī)， 2023， 55（8）： 126-132.

Wang Yang， Liu Wenkai， Zhang Ling， et al. Research progress on pasteurellosis in sheep and goats ［J］. Animal Husbandry Veterinary Medicine， 2023， 55（8）： 126-132.

［6］

曾志雄， 董冰， 呂恩利， 等. 豬舍環(huán)境無線多點(diǎn)多源遠(yuǎn)程監(jiān)測系統(tǒng)設(shè)計(jì)與試驗(yàn)［J］. 農(nóng)業(yè)機(jī)械學(xué)報(bào)， 2020， 51（2）： 332-340，349.

Zeng Zhixiong， Dong Bing， Lü Enli， et al. Design and experiment of wireless multi-point and multi-source remote monitoring system for pig house environment ［J］. Transactions of the Chinese Society for Agricultural Machinery， 2020， 51（2）： 332-340，349.

［7］

安石莉. 防疫消殺巡檢機(jī)器人定位導(dǎo)航系統(tǒng)設(shè)計(jì)［D］.銀川： 寧夏大學(xué)， 2023.

An Shili. Design of positioning and navigation system for inspection robot epidemic prevention and elimination ［D］. Yinchuan： Ningxia University， 2023.

［8］

宋懷波， 段援朝， 李嶸， 等.基于激光SLAM的牛場智能推翻草機(jī)器人自主導(dǎo)航系統(tǒng)［J］. 農(nóng)業(yè)機(jī)械學(xué)報(bào)， 2023， 54（2）： 293-301.

Song Huaibo， Duan Yuanchao， Li Rong， et al. Autonomous navigation system for pasture intelligent overthrowing grass robot based on laser SLAM［J］. Transactions of the Chinese Society for Agricultural Machinery， 2023， 54（2）： 293-301.

［9］

薛金林， 王培曉， 周俊， 等. 基于改進(jìn)Gmapping算法的果園二維環(huán)境地圖精準(zhǔn)構(gòu)建［J］. 農(nóng)業(yè)機(jī)械學(xué)報(bào)， 2023， 54（7）： 26-34， 55.

Xue Jinlin， Wang Peixiao， Zhou Jun， et al. Accurate construction of orchard two-dimensional environmental map based on improved Gmapping algorithm ［J］. Transactions of the Chinese Society for Agricultural Machinery， 2023， 54（7）： 26-34， 55.

［10］

Zhao J， Li J， Zhou J. Research ontwo-round self-balancing robot SLAM based on the Gmapping algorithm ［J］. Sensors， 2023， 23（5）： 2489.

［11］

溫淑慧， 問澤藤， 劉鑫， 等. 基于ROS的移動機(jī)器人自主建圖與路徑規(guī)劃［J］. 沈陽工業(yè)大學(xué)學(xué)報(bào)， 2022， 44（1）： 90-94.

Wen Shuhui， Wen Zeteng， Liu Xin， et al. Autonomous mapping and path planning of mobile robot based on ROS ［J］. Journal of Shenyang University of Technology， 2022， 44（1）： 90-94.

［12］

王豪杰， 馬向華， 代婉玉， 等. 改進(jìn)DWA算法的移動機(jī)器人避障研究［J］. 計(jì)算機(jī)工程與應(yīng)用， 2023， 59（6）： 326-332.

Wang Haojie， Ma Xianghua， Dai Wanyu， et al. Research on obstacle avoidance of mobile robot with improved DWA algorithm ［J］. Computer Engineering and Applications， 2023， 59（6）： 326-332.

［13］

姜賽珂. 溫室巡檢機(jī)器人導(dǎo)航控制系統(tǒng)研究［D］.鎮(zhèn)江：江蘇大學(xué)， 2023.

Jiang Saike. Research on navigation control system of greenhouse inspection robot ［D］. Zhenjiang： Jiangsu University， 2023.