毛文菊 劉 恒 王小樂 楊福增 劉志杰 汪宗陽

(1.西北農(nóng)林科技大學(xué)機(jī)械與電子工程學(xué)院， 陜西楊凌 712100；2.農(nóng)業(yè)農(nóng)村部蘋果全程機(jī)械化科研基地， 陜西楊凌 712100； 3.上海一坤電氣工程有限公司， 上海 200233)

0 引言

蘋果收獲是一種季節(jié)性很強(qiáng)且勞動密集型的作業(yè)，該環(huán)節(jié)的作業(yè)量約占蘋果生產(chǎn)所有作業(yè)量的40%[1-2]，但收獲過程的采后運(yùn)輸作業(yè)仍采用傳統(tǒng)的人和運(yùn)輸車互相配合的方式[3]。果園生產(chǎn)逐步向現(xiàn)代化和智能化方向轉(zhuǎn)變，研發(fā)適用于果園的運(yùn)輸機(jī)器人是果園現(xiàn)代化、智能化發(fā)展的必然趨勢[4-5]。

自主導(dǎo)航技術(shù)是果園運(yùn)輸機(jī)器人智能化作業(yè)的關(guān)鍵。在果園運(yùn)輸機(jī)器人導(dǎo)航技術(shù)研究方面，MARK等[6]采用多層激光識別獼猴桃果樹行的方法，實現(xiàn)了機(jī)器人行內(nèi)直線行駛，但未體現(xiàn)地頭自主轉(zhuǎn)彎這一技術(shù)難點；MARCEL等[7]開發(fā)了輪式自主移動果園運(yùn)輸設(shè)備，該設(shè)備通過2D激光檢測果樹行兩端設(shè)定的固定點獲得果樹行地頭轉(zhuǎn)彎信息，并根據(jù)這些信息規(guī)劃固定路徑實現(xiàn)自動導(dǎo)航，但在行間起步、停車時仍需要人工按鍵完成；國內(nèi)學(xué)者[8-13]優(yōu)化和改善了果園運(yùn)輸設(shè)備的機(jī)械性能，但對該類設(shè)備自主導(dǎo)航技術(shù)的相關(guān)研究未見報道。

本研究針對現(xiàn)有果園運(yùn)輸機(jī)器人導(dǎo)航模式單一，自主作業(yè)時無法實現(xiàn)任意點的起步、停車，智能化程度偏低等問題，設(shè)計一種雙導(dǎo)航模式果園運(yùn)輸機(jī)器人，以實現(xiàn)行人引領(lǐng)導(dǎo)航和定點導(dǎo)航。以果農(nóng)的安全為前提，機(jī)器人可以跟隨果農(nóng)在果園任意點起步、停車實現(xiàn)運(yùn)輸作業(yè)，一旦滿載則可以自主行駛至目標(biāo)點，由地頭的果農(nóng)卸果后再自主返回至果園內(nèi)果農(nóng)所在位置，開始下一輪的跟隨運(yùn)輸作業(yè)，提高果園運(yùn)輸作業(yè)的智能化程度。

1 系統(tǒng)設(shè)計

1.1 硬件系統(tǒng)設(shè)計

雙導(dǎo)航模式果園運(yùn)輸機(jī)器人的整體硬件由行人引領(lǐng)導(dǎo)航模塊、定點導(dǎo)航模塊、控制模塊、運(yùn)動模塊和動力模塊等5個模塊組成，其結(jié)構(gòu)如圖1所示。

圖1 機(jī)器人整體硬件結(jié)構(gòu)圖Fig.1 Overall hardware of robot

(1)行人引領(lǐng)導(dǎo)航模塊

雙導(dǎo)航模式果園運(yùn)輸機(jī)器人的行人引領(lǐng)導(dǎo)航模塊由Intel RealSense D435i型深度相機(jī)和編碼器構(gòu)成。D435i型深度相機(jī)由1個紅外點陣投射器、1個RGB相機(jī)和2個紅外相機(jī)組成，其中紅外點陣投射器補(bǔ)齊燈光，RGB相機(jī)采集彩色圖像的信息，兩個紅外(Infra-Red)相機(jī)測量圖像距離相機(jī)的深度信息，為機(jī)器人提供相機(jī)前方的圖像信息和圖像像素點距離機(jī)器人的深度信息。D435i深度相機(jī)架設(shè)在機(jī)器人中心正前方，激光雷達(dá)的下方。

編碼器安裝在機(jī)器人底盤驅(qū)動電機(jī)上，根據(jù)電機(jī)轉(zhuǎn)速為行人引領(lǐng)導(dǎo)航模塊提供機(jī)器人當(dāng)前時刻的速度信息。

(2)定點導(dǎo)航模塊

雙導(dǎo)航模式果園運(yùn)輸機(jī)器人的CGI-410型定點導(dǎo)航模塊(華測)由實時動態(tài)全球?qū)Ш叫l(wèi)星定位系統(tǒng)(Real time kinematic-global navigation satellite system，RTK-GNSS)和RS_Lidar-16激光雷達(dá)(速騰)兩部分構(gòu)成。RTK-GNSS導(dǎo)航基于差分定位的原理，即基站通過電臺和天線將定位經(jīng)緯度定位坐標(biāo)信息傳遞給移動站，移動站接收到RTK-GNSS的載波相位以及基準(zhǔn)站的信息后進(jìn)行及時處理，最后得到高精度的定位結(jié)果。其中移動站包括移動站接收機(jī)、RTK-GNSS雙天線和外掛電臺，移動站接收機(jī)安裝在運(yùn)輸機(jī)器人裝載箱下方，RTK-GNSS雙天線分別架設(shè)在機(jī)器人正中心前后端，雙天線連線的基線與真北方向的夾角為機(jī)器人當(dāng)前航向角(方向)。所采用的CGI-410型 RTK-GNSS接收機(jī)內(nèi)置MEMS(Micro-electro-mechanical system)陀螺儀與加速度計，可進(jìn)行RTK-GNSS和慣性組合導(dǎo)航。即當(dāng)移動站在蘋果園中受果樹等遮擋物影響丟失信號時，可采用慣性導(dǎo)航提供的姿態(tài)和位置信息，適用于運(yùn)輸機(jī)器人在果園自主導(dǎo)航。為保證該定位導(dǎo)航模塊能進(jìn)行組合導(dǎo)航，需要在空曠位置處對該定位導(dǎo)航模塊進(jìn)行“繞8字”校準(zhǔn)后再使用。

激光雷達(dá)安裝在機(jī)器人正中心前方，具有水平方向±180°，垂直方向±5°的掃描范圍， 0.38°水平角分辨率，150 m測距量程和20 Hz的掃描頻率，可實現(xiàn)對果園障礙物信息的高效獲取，保證運(yùn)輸機(jī)器人定點導(dǎo)航的安全性。

(3)控制模塊

雙導(dǎo)航模式果園運(yùn)輸機(jī)器人的控制模塊如圖1所示，整個機(jī)器人由嵌入式主板進(jìn)行控制，嵌入式主板采用ARM A57處理器，8 GB內(nèi)存，32 GB硬盤。遠(yuǎn)程監(jiān)控平臺借助WiFi通信模塊與嵌入式主板連接，可遠(yuǎn)程操縱運(yùn)輸機(jī)器人并監(jiān)控其工作狀態(tài)。此外，遙控手柄采用藍(lán)牙與嵌入式主板通信，根據(jù)遙控按鈕的方向和力度，向嵌入式主板發(fā)布調(diào)整底盤的轉(zhuǎn)向和行駛速度指令。嵌入式主板通過串口向底層伺服控制器發(fā)送速度控制信息，底層伺服控制器通過串口通信的方式向嵌入式主板反饋底盤實時速度信息。

(4)運(yùn)動及動力模塊

雙導(dǎo)航模式果園運(yùn)輸機(jī)器人的履帶式底盤具有較好的地形適應(yīng)性和越障能力以及轉(zhuǎn)向靈活等優(yōu)點。蘋果園運(yùn)輸機(jī)器人采用履帶式差速驅(qū)動的運(yùn)動模式，左右驅(qū)動輪由獨立的伺服電機(jī)進(jìn)行控制，驅(qū)動輪與伺服電機(jī)之間連接有減速器，最大行駛速度為1.2 m/s。運(yùn)輸機(jī)器人工作時底層伺服控制器通過控制伺服電機(jī)的轉(zhuǎn)速使運(yùn)輸機(jī)器人運(yùn)動。

采用48 V鋰電池為運(yùn)輸機(jī)器人提供動力，充電電源電壓為220 V。

雙導(dǎo)航模式機(jī)器人各模塊硬件安裝的3D示意圖和實物圖如圖2所示，改裝后運(yùn)輸機(jī)器人車身長1.175 m，寬0.835 m，高0.470 m，額定負(fù)載200 kg。

圖2 雙導(dǎo)航模式果園運(yùn)輸機(jī)器人整體結(jié)構(gòu)圖Fig.2 Overall structure of dual navigation mode orchard transport robot1.RTK-GNSS定向天線 2.裝載箱 3.履帶底盤 4.屏幕 5.充電口 6.急停按鈕 7.WiFi天線 8.RTK-GNSS定位天線 9.D435i深度相機(jī) 10.激光雷達(dá)

1.2 軟件系統(tǒng)設(shè)計

雙導(dǎo)航模式果園運(yùn)輸機(jī)器人的軟件系統(tǒng)由命令交互層、信息處理層和執(zhí)行層共3個模塊構(gòu)成，軟件系統(tǒng)結(jié)構(gòu)如圖3所示。

圖3 雙導(dǎo)航模式果園運(yùn)輸機(jī)器人軟件系統(tǒng)結(jié)構(gòu)圖Fig.3 Dual navigation mode orchard transport robot software system structure diagram

命令交互層是基于Linux系統(tǒng)的模式選擇程序，可根據(jù)不同的需求選擇運(yùn)輸機(jī)器人的工作模式，目前可選擇的工作模式有兩種：行人引領(lǐng)導(dǎo)航模式，根據(jù)設(shè)定的領(lǐng)航員手勢命令，啟動行人引領(lǐng)導(dǎo)航功能，即機(jī)器人會在視覺范圍內(nèi)跟隨第一個向相機(jī)揮右手的行人行走；定點導(dǎo)航模式，機(jī)器人根據(jù)用戶輸入的目標(biāo)點到達(dá)果園內(nèi)任意位置。信息處理層為基于ROS (Robot operating system)的行人引領(lǐng)導(dǎo)航及定點導(dǎo)航程序，在行人引領(lǐng)導(dǎo)航模式下實時獲取領(lǐng)航員目標(biāo)點的信息并調(diào)整跟蹤運(yùn)動的速度，在定點導(dǎo)航模式下實時獲取機(jī)器人位姿、障礙物信息并調(diào)整自主運(yùn)動的速度。執(zhí)行層為基于Linux系統(tǒng)的移動平臺控制，將信息處理層向底層伺服控制器輸出的速度信息，轉(zhuǎn)換為電機(jī)轉(zhuǎn)速信息，實現(xiàn)采后果實的自主運(yùn)輸。

2 自主導(dǎo)航功能實現(xiàn)原理

雙導(dǎo)航模式果園運(yùn)輸機(jī)器人的自主導(dǎo)航功能主要通過軟件系統(tǒng)的信息處理層模塊來實現(xiàn)，該信息處理層模塊的軟件運(yùn)行如圖4所示。

圖4 信息處理層軟件運(yùn)行流程示意圖Fig.4 Information processing layer software operation flow diagram

在行人引領(lǐng)導(dǎo)航運(yùn)輸模式中有人體姿態(tài)估計、深度信息對齊、提取目標(biāo)點和目標(biāo)跟蹤4個步驟，人體姿態(tài)估計借助了OpenPose[14]開源算法包獲取人體實時運(yùn)動姿態(tài)的骨骼關(guān)鍵點信息，縮短軟件開發(fā)周期，節(jié)省開發(fā)成本；再通過骨骼關(guān)鍵點計算得到人體中心點，匹配中心點圖像信息和深度信息，獲取帶有深度信息的目標(biāo)點；最后利用比例-積分(Proportional integral，PI)控制實現(xiàn)目標(biāo)跟蹤功能。定點導(dǎo)航自主運(yùn)輸模式有D-A控制和障礙物判別2個步驟，機(jī)器人勻速朝目標(biāo)點方向前進(jìn)時，根據(jù)RTK-GNSS提供位姿信息不斷調(diào)整到達(dá)目標(biāo)點的方向，且根據(jù)有無障礙物時判別是否急停，最終輸出機(jī)器人下一時刻的速度信息，直至到達(dá)目標(biāo)點。

2.1 行人引領(lǐng)導(dǎo)航

在行人引領(lǐng)導(dǎo)航運(yùn)輸模式中，實時定位目標(biāo)是順利實現(xiàn)導(dǎo)航的核心難點，尤其在果園多人采摘場景下，機(jī)器人需要辨認(rèn)出領(lǐng)航員。目前基于視覺的方法在領(lǐng)航員的定位以及多人環(huán)境中分辨領(lǐng)航人員方面具有很大的潛力[15]，與其他視覺識別人體姿態(tài)算法相比，OpenPose[16]是基于深度學(xué)習(xí)的實時多人二維姿態(tài)估計算法，并以卷積神經(jīng)網(wǎng)絡(luò)為框架開發(fā)了開源庫，能為實現(xiàn)運(yùn)輸機(jī)器人行人引領(lǐng)導(dǎo)航節(jié)省大量的開發(fā)時間。因此，本文行人引領(lǐng)導(dǎo)航模式的開發(fā)是將相機(jī)采集的RGB圖像先通過OpenPose算法獲得多人2D人體姿態(tài)估計后，利用深度信息對齊獲取多人2D人體姿態(tài)圖像的深度信息，再根據(jù)特征手勢(揮動右手)在多人場景中提取出領(lǐng)航員姿態(tài)的中心點，最后根據(jù)該中心點的位置信息進(jìn)行目標(biāo)跟蹤控制實現(xiàn)行人引領(lǐng)導(dǎo)航。

(1)人體姿態(tài)估計

OpenPose先通過卷積神經(jīng)網(wǎng)絡(luò)得到人體特征圖，分別通過CMP (Part detection confidence maps)得到25個關(guān)節(jié)點(如頸、肩、腕、腰、膝、踝等)的置信度，PAFs(Part affinity fields)得到25個關(guān)節(jié)點在骨架中的走向(關(guān)節(jié)點位置和方向的2D矢量)，即肢體的置信度。為減少計算量，利用貪婪分析算法(Greedy parsing algorithm)，對置信度信息進(jìn)行編碼和存儲，再采用圖論法對關(guān)節(jié)點和肢體的置信度信息進(jìn)行二分法匹配(Bipartite matching)，并使用匈牙利算法(Hungarian algorithm)得到關(guān)節(jié)點和肢體最優(yōu)化的匹配，即將同一個人的關(guān)節(jié)點連接起來，最終將每個人都拼接成由25個關(guān)節(jié)點組合而成的人體骨架。詳細(xì)的實施步驟參見文獻(xiàn)[14,16]。

(2)深度信息對齊

通過人體姿態(tài)估計輸出的是多個人的RGB彩色圖像信息，由于D435i相機(jī)采集RGB彩色圖像與Infra-Red紅外圖像的鏡頭位置不同，所以這兩者數(shù)據(jù)的空間坐標(biāo)系是不同的，需要將RGB的圖像彩色視頻流與Infra-Red圖像的深度流對齊得到RGB彩色圖像像素點的深度信息。即通過顏色流和深度流不同的分辨率，獲取深度信息的深度比例單位，刪除圖像中深度距離超過最大值的像素，根據(jù)顏色和深度的框架集，將深度框與顏色框?qū)R，獲取對齊的幀并驗證兩幀是否有效，最后用該信息渲染圖像。

(3)提取目標(biāo)點

通過在人體姿態(tài)估計中得到的多個由25個關(guān)節(jié)點組合而成的人體骨架，經(jīng)過深度信息對齊后，多個25個關(guān)節(jié)點組成的人體骨架RGB圖像具備了深度信息。如圖5a所示，其中第2、3、4個骨架關(guān)鍵點分別為人體的右肩、右肘和右手腕，第1、5、6骨架關(guān)鍵點分別為人體的脖頸、右髖和左髖。當(dāng)在圖像中出現(xiàn)多個人時，設(shè)置第1個在視覺相機(jī)識別范圍內(nèi)并抬起右手揮動的人為運(yùn)輸機(jī)器人跟蹤目標(biāo)。然后提取該組人體骨骼的重心關(guān)鍵點1、5、6，并在1、5、6這個三角形中尋找重心點作為目標(biāo)點，最后輸出目標(biāo)點的當(dāng)前位置信息

圖5 行人引領(lǐng)導(dǎo)航原理圖Fig.5 Pedestrian led navigation schematic

(1)

式中xtargert——機(jī)器人實時觀測到的目標(biāo)點橫坐標(biāo)

ytargert——機(jī)器人實時觀測到的目標(biāo)點縱坐標(biāo)

x1、x2、x3、x4、x5、x6——人體骨架關(guān)鍵點1、2、3、4、5、6的橫坐標(biāo)

y1、y2、y3、y4、y5、y6——人體骨架關(guān)鍵點1、2、3、4、5、6的縱坐標(biāo)

(4)距離-方向(Distance-angle，D-A)目標(biāo)跟蹤控制

(2)

(3)

式中vt——編碼器測量的機(jī)器人t時刻線速度

ωt——編碼器測量的機(jī)器人t時刻角速度

vo——機(jī)器人行駛時設(shè)置的初始線速度

ωo——機(jī)器人行駛時設(shè)置的初始角速度

Δt——時間差

(4)

式中rmin——目標(biāo)跟隨距離

θj——從開始到時刻t機(jī)器人旋轉(zhuǎn)角度

kpv——運(yùn)輸機(jī)器人PI控制器的線速度比例增益

kpω——運(yùn)輸機(jī)器人PI控制器的角速度比例增益

kiω——運(yùn)輸機(jī)器人積分控制參數(shù)

運(yùn)輸機(jī)器人t+1時刻目標(biāo)跟蹤的運(yùn)動距離和角度計算式為

(5)

式中 Δxt+1——t+1時刻目的點的橫坐標(biāo)

Δyt+1——t+1時刻目的點的縱坐標(biāo)

上述行人引領(lǐng)導(dǎo)航功能算法的實現(xiàn)流程如圖6所示。

圖6 行人引領(lǐng)導(dǎo)航功能實現(xiàn)流程圖Fig.6 Implementation flow of pedestrian led navigation function

2.2 定點導(dǎo)航

目前RTK-GNSS導(dǎo)航定位技術(shù)在大田中的應(yīng)用已經(jīng)基本成熟[17-18]，實現(xiàn)了復(fù)雜農(nóng)田環(huán)境下農(nóng)業(yè)機(jī)械自動導(dǎo)航[19]，田間試驗作業(yè)精度可達(dá)到厘米級[20]。隨著衛(wèi)星定位精度的不斷提高，有學(xué)者嘗試將該技術(shù)應(yīng)用在果園環(huán)境中，如熊斌等[21]研制的BDS果園施藥機(jī)，ZACHARY等[22]研制的果園自動收獲作業(yè)，試驗結(jié)果表明基于RTK-GNSS的機(jī)器人能滿足果園作業(yè)要求?？紤]到果園遮擋對RTK-GNSS定位精度有一定的影響，從而影響運(yùn)輸機(jī)器人定點導(dǎo)航的精度，本文根據(jù)JJF 1118—2019《全球定位系統(tǒng)(GPS)接收機(jī)(測地型和導(dǎo)航型)校準(zhǔn)規(guī)范》，在果園中對運(yùn)輸機(jī)器人CGI-410型定位導(dǎo)航模塊的精度進(jìn)行測量,在機(jī)器人距離基站5～50 m的試驗區(qū)內(nèi)，每間隔5 m選取差分定位點測量點,每次測量時間間隔30 min，如圖7所示，測試結(jié)果表明：在果園試驗區(qū)基于CGI-410型定位導(dǎo)航模塊的果園運(yùn)輸機(jī)器人靜態(tài)定位漂移不明顯。因此，本文設(shè)計運(yùn)輸機(jī)器人在果園定點導(dǎo)航的位姿信息由CGI-410型定位導(dǎo)航模塊直接提供。

圖7 基于CGI-410型定位導(dǎo)航模塊的果園運(yùn)輸機(jī)器人靜態(tài)定位漂移測試結(jié)果Fig.7 Static positioning drift test results of RTK-GNSS-based orchard transport robot

目前基于RTK-GNSS的割草機(jī)器人[23]、噴藥機(jī)器人[24]、巡檢機(jī)器人[25]導(dǎo)航時，其作業(yè)路徑通常是固定的，即機(jī)器人以作業(yè)軌跡為追蹤對象，沿著設(shè)定好的路徑行走，并在路徑兩端停車。但機(jī)器人的位置會根據(jù)果園內(nèi)果農(nóng)實際作業(yè)情況發(fā)生變化，從而導(dǎo)致運(yùn)輸機(jī)器人作業(yè)時的停駛點不同，作業(yè)路徑無法固定。如圖8所示，果園內(nèi)有多個不同的停駛點，果園內(nèi)果農(nóng)可隨意按照箭頭運(yùn)動，機(jī)器人根據(jù)果園內(nèi)果農(nóng)運(yùn)動的方向，停駛在不同停駛點上(如3、5、7、10)，再從這些不同的停駛點返回至地頭1時，會使機(jī)器人有4種不同長度的行駛路徑(如行駛路徑1、2、3、4)。而果園環(huán)境無法大規(guī)模鋪設(shè)類似于植物工廠物流搬運(yùn)機(jī)器人[26]行走的軌道。因此，為保證運(yùn)輸機(jī)器人作業(yè)時到達(dá)給定的任意目標(biāo)點，本文以目標(biāo)點為追蹤對象，運(yùn)輸機(jī)器人基于RTK-GNSS導(dǎo)航采用距離-方向(Distance-angle，D-A)跟蹤控制的方法，輸入由RTK-GNSS靜態(tài)時測得的目標(biāo)點經(jīng)緯度信息后，先后計算當(dāng)前點和位置點的方向差和距離，并以該方向差和距離與設(shè)定閾值的關(guān)系為判斷條件，使機(jī)器人到達(dá)給定的任意目標(biāo)點，并在導(dǎo)航過程中加入障礙物判別的步驟，避免碰撞行人。

圖8 運(yùn)輸機(jī)器人在果園作業(yè)場景示意圖Fig.8 Schematic of transport robot operating in orchard

(1)距離-方向(Distance-angle，D-A)控制

如圖9所示，用RTK-GNSS測量提供的世界大地坐標(biāo)建立坐標(biāo)系BOL，運(yùn)輸機(jī)器人在該坐標(biāo)系上的位置點坐標(biāo)唯一，點坐標(biāo)由經(jīng)度坐標(biāo)B和緯度坐標(biāo)L組成。先通過經(jīng)緯度坐標(biāo)計算目標(biāo)點和機(jī)器人當(dāng)前位置點連線與正北方向的夾角θtarget，并根據(jù)θtarget與當(dāng)前位置點在正北方向夾角θtarget(由RTK-GNSS根據(jù)機(jī)器人實際情況測量得到)角度差的絕對值θ，調(diào)整機(jī)器人的角速度(設(shè)定機(jī)器人逆時針旋轉(zhuǎn)方向為正)，使機(jī)器人當(dāng)前位置點方向與目標(biāo)點所在方向一致。其中目標(biāo)點與當(dāng)前位置點連線與正北方向的夾角θtarget的計算式為

圖9 定點導(dǎo)航距離-方向(Distance-angel，D-A)控制原理圖Fig.9 Distance-angel (D-A) control schematic for fixed-point navigation

(6)

式中Btarget——目標(biāo)點的經(jīng)度坐標(biāo)

Ltarget——目標(biāo)點的緯度坐標(biāo)

Bcurrent——當(dāng)前位置點的經(jīng)度坐標(biāo)

Lcurrent——當(dāng)前位置點的緯度坐標(biāo)

式(6)的4個方程分別對應(yīng)圖9a、9b、9c、9d。

再參考文獻(xiàn)[27]中的兩點經(jīng)緯度坐標(biāo)間距離求解公式(該公式將地球視為一個正球體，多用于無法進(jìn)行大運(yùn)算量的場合中)，計算機(jī)器人當(dāng)前位置點與目標(biāo)點之間取整后的距離S

(7)

式中 []——取整函數(shù)

式中6 378 137為地球半徑，單位m。為減少UTM坐標(biāo)轉(zhuǎn)換的計算量，提高程序運(yùn)行速度，在計算機(jī)器人的距離和方向差信息時均采用RTK-GNSS提供的經(jīng)緯度信息直接計算。

定點導(dǎo)航時分別以目標(biāo)點方向θtarget與當(dāng)前位置方向θcurrent差值絕對值θ，當(dāng)前位置點與目標(biāo)點之間的距離S(以機(jī)器人車身長度為參考，設(shè)定S判斷閾值為1 m)，先后作為定點導(dǎo)航時輸出角速度和線速度的判定條件。根據(jù)表1執(zhí)行定點導(dǎo)航參數(shù)設(shè)定規(guī)則，當(dāng)角速度ω為正時，機(jī)器人逆時針旋轉(zhuǎn)，角速度ω為負(fù)時，機(jī)器人順時針旋轉(zhuǎn)。

如表1所示，先計算目標(biāo)點方向θtarget與當(dāng)前位置方向θcurrent的角度差絕對值θ，當(dāng)|θtarget-θcurrent|≤1°時，角速度ω為0 rad/s，當(dāng)|θtarget-θcurrent|>1°時，判斷機(jī)器人轉(zhuǎn)動的方向，機(jī)器人按照設(shè)定的角速度參數(shù)行駛，使機(jī)器人當(dāng)前位置點與目標(biāo)點方向的角度差|θtarget-θcurrent|≤1°。再計算位置點與目標(biāo)點之間的距離S，當(dāng)S≤1 m時，機(jī)器人線速度v為0 m/s，當(dāng)機(jī)器人S>1 m時，機(jī)器人按照設(shè)定的線速度v開始行駛。若在運(yùn)輸機(jī)器人追蹤目標(biāo)點的過程中，出現(xiàn)|θtarget-θcurrent|>1°時，則采用角速度參數(shù)修正方向，直至運(yùn)輸機(jī)器人到達(dá)目的點。

表1 定點導(dǎo)航參數(shù)設(shè)定規(guī)則Tab.1 Fixed-point navigation parameter setting rules

(2)避障

為減少安全隱患的發(fā)生，獲取激光雷達(dá)垂直方向±3°，水平方向±90°，掃描距離在[0.15 m, 50 m]的信息，并將該范圍劃分為多個等分區(qū)域。如圖10所示，當(dāng)?shù)确謪^(qū)域中任何一個區(qū)域內(nèi)有物體(紅色星號)遮擋，則雷達(dá)掃描距離小于1 m即判定存在障礙物，輸出速度為0 m/s，機(jī)器人急停；反之機(jī)器人繼續(xù)行走。

圖10 避障原理圖Fig.10 Obstacle avoidance schematic

3 果園運(yùn)輸試驗

3.1 果園環(huán)境

選取位于陜西省渭南市白水縣杜康鎮(zhèn)西北農(nóng)林科技大學(xué)白水蘋果實驗站西400 m處非標(biāo)準(zhǔn)果園，該果園的面積約1 hm2，果園內(nèi)種植了14行生長15年左右的紅富士蘋果樹，蘋果樹行間距為4 m左右，蘋果樹株距為3 m左右，高約為3 m，蘋果樹單行長178.1 m。如圖11所示，部分果樹樹干的周圍地面上長有雜草。試驗時間為2020年8月24日—9月5日，此時為果實成熟期，果園內(nèi)果樹枝干交叉，部分結(jié)果的果樹枝干因受重力影響距離地面較近，尚未摘取蘋果的果樹，樹下沿著果樹行方向的地面鋪設(shè)反光薄膜，用于蘋果上色。為保證運(yùn)輸機(jī)器人順利通行，選擇園內(nèi)枝干距離地面約為1.2 m的果樹行進(jìn)行試驗。

圖11 果園試驗環(huán)境Fig.11 Orchard test environment

3.2 行人引領(lǐng)導(dǎo)航試驗

運(yùn)輸機(jī)器人在果園中的行人引領(lǐng)導(dǎo)航試驗分為兩步，先對運(yùn)輸機(jī)器人在多個目標(biāo)場景下順利跟蹤領(lǐng)航員進(jìn)行測試，再對運(yùn)輸機(jī)器人跟隨領(lǐng)航員行間作業(yè)時的目標(biāo)跟蹤誤差進(jìn)行測試。試驗參考ISO 15623—2013《運(yùn)輸信息和控制系統(tǒng)標(biāo)準(zhǔn)》和果園行株距，將目標(biāo)跟蹤的安全距離Ds設(shè)為1 m，目標(biāo)距機(jī)器人最大距離的閾值Dmax設(shè)為5 m。且在測試行人引領(lǐng)導(dǎo)航時，發(fā)現(xiàn)機(jī)器人目標(biāo)跟蹤效果最好的行駛速度為0.5 m/s，因此設(shè)定機(jī)器人在跟隨領(lǐng)航員的初始速度為0.5 m/s。

為測試在果園多目標(biāo)場景下能準(zhǔn)確識別出領(lǐng)航員，參考文獻(xiàn)[28-29]對運(yùn)輸機(jī)器人在多目標(biāo)場景下識別領(lǐng)航員設(shè)計試驗，如圖12a所示，分別在果園天晴、多云、傍晚3組不同光線條件下，運(yùn)輸機(jī)器人目標(biāo)跟蹤范圍內(nèi)(5 m)出現(xiàn)多個行人時，領(lǐng)航員在距離機(jī)器人1 m處向相機(jī)揮動右手后，機(jī)器人識別出揮動右手的領(lǐng)航員(紅色星號)并跟隨其行走，直至機(jī)器人跟隨領(lǐng)航員走完一行跨行進(jìn)入下一行結(jié)束，其中機(jī)器人跟隨領(lǐng)航員行走時其他行人的位置和運(yùn)動狀態(tài)為隨機(jī)選擇。每組試驗重復(fù)測試3次，試驗視頻由架設(shè)在機(jī)器人深度相機(jī)上方的攝像頭進(jìn)行第一視角錄制，最后參考文獻(xiàn)[30]計算目標(biāo)識別準(zhǔn)確度和目標(biāo)識別精度。

(8)

(9)

式中Ttp——實際為領(lǐng)航員并識別出領(lǐng)航員的樣本

Ttn——實際為行人并識別出行人的樣本

Tfn——錯誤識別出行人的樣本

Tfp——錯誤識別領(lǐng)航員的樣本

參考文獻(xiàn)[31-32]對運(yùn)輸機(jī)器人跟隨領(lǐng)航員行間作業(yè)時的目標(biāo)跟蹤誤差設(shè)計試驗。如圖12b所示，在行人引領(lǐng)導(dǎo)航模式中，分別在果園晴天、多云、傍晚3組光線條件下，測試在果園環(huán)境中領(lǐng)航員非勻速地運(yùn)動結(jié)束后，運(yùn)輸機(jī)器人與領(lǐng)航員之間Dt的變化過程。每間隔5 m領(lǐng)航員停駛后，測量一次機(jī)器人前方相機(jī)與領(lǐng)航員當(dāng)前位置的直線距離Dtarget，最后根據(jù)Dtarget和Ds得到Dt，根據(jù)設(shè)計的試驗步驟重復(fù)3組記錄Dt值。機(jī)器人跨行跟隨行人作業(yè)時，行人沿著固定軌跡行駛，機(jī)器人以0.5 m/s跟隨，行人每移動1步(約0.8 m)測量1次Dtarget，行人共計移動10步，試驗步驟重復(fù)3次記錄Dt值。

圖12 果園運(yùn)輸機(jī)器人行人引領(lǐng)導(dǎo)航試驗示意圖Fig.12 Schematics of transport robot orchard pedestrian led navigation experiment

運(yùn)輸機(jī)器人在果園作業(yè)時的行人引領(lǐng)導(dǎo)航試驗場景如圖13所示(①～⑧為目標(biāo)動態(tài)識別與跟蹤的順序，橘色虛線框為領(lǐng)航員，黃色點劃線框為其他行人)，在不同光線條件下機(jī)器人能夠在多個行人中識別出視覺范圍內(nèi)的領(lǐng)航員(向相機(jī)揮右手示意的行人)，并跟蹤領(lǐng)航員向前行走，且不受旁邊突然經(jīng)過的行人干擾，先后經(jīng)過旁邊的行人，順利進(jìn)入下一個視覺識別范圍，繼續(xù)跟蹤領(lǐng)航員行走，直至完成測試。

圖13 不同光線條件下行人引領(lǐng)導(dǎo)航試驗場景Fig.13 Pedestrian led navigation test scenarios under different lighting conditions

(1)多目標(biāo)場景下領(lǐng)航員識別

果園多目標(biāo)場景下識別領(lǐng)航員的試驗結(jié)果如表2所示，對比不同環(huán)境條件下的跟蹤準(zhǔn)確度和跟蹤精度，發(fā)現(xiàn)機(jī)器人在視覺范圍內(nèi)的目標(biāo)跟蹤準(zhǔn)確度和跟蹤精度都能達(dá)到100%，但晴天時機(jī)器人目標(biāo)跟蹤反應(yīng)較慢，會出現(xiàn)停頓現(xiàn)象，導(dǎo)致運(yùn)行的時間較長。這是因為晴天時光線過強(qiáng)，強(qiáng)光導(dǎo)致深度相機(jī)投影圖像較差，從而使圖像處理的速度變慢，但在多云或傍晚時，相機(jī)受強(qiáng)光影響小，特別是在傍晚光線較弱時，機(jī)器人反應(yīng)速度最快。

表2 多目標(biāo)場景下的領(lǐng)航員識別試驗結(jié)果Tab.2 Experimental results of navigator identification in multi-target scenarios

(2)機(jī)器人目標(biāo)跟蹤誤差

運(yùn)輸機(jī)器人跟隨領(lǐng)航員果園作業(yè)時的目標(biāo)跟蹤誤差結(jié)果如表3、4所示。從表3中可以看出，機(jī)器人以0.5 m/s速度行駛時，行人停駛后機(jī)器人的目標(biāo)跟蹤誤差平均值及標(biāo)準(zhǔn)差均小于9 cm和4 cm。從表4中可以看出，機(jī)器人以0.5 m/s速度行駛時，行人停駛目標(biāo)跟蹤誤差的平均值及標(biāo)準(zhǔn)差均小于6 cm和3 cm。即能夠在安全的基礎(chǔ)上，滿足行人引領(lǐng)導(dǎo)航機(jī)器人運(yùn)輸?shù)男枨?。橫向?qū)Ρ缺?、4相同速度下不同距離的停駛點，發(fā)現(xiàn)采樣距離越短跟蹤誤差越小。縱向?qū)Ρ缺?、4中相同速度下多次目標(biāo)跟蹤誤差及其標(biāo)準(zhǔn)差，跟蹤誤差會隨著試驗次數(shù)增多而增大，這可能是因為行人運(yùn)動速度不同，而在目標(biāo)跟蹤控制中對編碼器提供的數(shù)值進(jìn)行了積分，導(dǎo)致累積誤差增大。

表3 行間作業(yè)時目標(biāo)跟蹤誤差Tab.3 Target tracking error in inter-row transportation m

表4 跨行作業(yè)時目標(biāo)跟蹤誤差Tab.4 Target tracking error in interline turn transportation m

3.3 定點導(dǎo)航試驗

定點導(dǎo)航試驗主要是用于測試在果園內(nèi)，當(dāng)果農(nóng)隨機(jī)輸入指定目標(biāo)點后，采用D-A控制方法機(jī)器人能順利到達(dá)指定目標(biāo)點。且在定點導(dǎo)航過程中存在障礙物時，依然能正常到達(dá)目標(biāo)點。該試驗分為兩部分，先測試定點導(dǎo)航的目標(biāo)跟蹤功能，再測試避障功能。參考文獻(xiàn)[32-33]對運(yùn)輸機(jī)器人的定點導(dǎo)航運(yùn)輸功能設(shè)計了試驗，果園試驗場景如圖14a所示，試驗步驟示意圖如圖14b。設(shè)定機(jī)器人在定點導(dǎo)航模式下以0.5 m/s自主行駛，根據(jù)果園內(nèi)果樹生長情況，隨機(jī)地選擇多個目標(biāo)點，使機(jī)器人實時計算當(dāng)前位置點到目標(biāo)點的位姿信息，利用D-A控制到達(dá)給定目標(biāo)點，機(jī)器人到達(dá)目標(biāo)點的先后順序為A1、B1、B2、A2、A3、B3、B1、A1，重復(fù)試驗3組，記錄運(yùn)輸機(jī)器人到達(dá)給定目標(biāo)點的位置信息。

為保證機(jī)器人自主運(yùn)輸時避免與行人等障礙物發(fā)生碰撞，參考文獻(xiàn)[31]設(shè)計了定點導(dǎo)航避障試驗，果園試驗場景如圖14c所示，試驗步驟示意圖如圖14d。試驗選取在機(jī)器人行駛路徑上每隔5 m出現(xiàn)一次行人(常見障礙物)，共設(shè)置4次，當(dāng)機(jī)器人檢測到障礙物停駛后再移開障礙物使機(jī)器人繼續(xù)行走，直至到達(dá)目標(biāo)點。在相同路徑上重復(fù)進(jìn)行3組試驗，每組試驗機(jī)器人共停駛5次。

圖14 運(yùn)輸機(jī)器人果園定點導(dǎo)航試驗Fig.14 Transport robot orchard spot navigation experiment

(1)多個目標(biāo)點定點導(dǎo)航試驗結(jié)果

選用絕對誤差和相對誤差來評價多點導(dǎo)航的試驗結(jié)果[30]，先計算3組試驗每次試驗之間的絕對誤差均值和標(biāo)準(zhǔn)差，結(jié)果如表5所示，再計算3組試驗相對誤差的平均值和標(biāo)準(zhǔn)差，結(jié)果如表6所示。

表6 定點導(dǎo)航相對誤差Tab.6 Relative error in fixed-point navigation m

由表5、6可知，機(jī)器人果園定點導(dǎo)航的絕對誤差及其標(biāo)準(zhǔn)差分別小于7 cm和0.5 cm。相對誤差及其標(biāo)準(zhǔn)差分別小于13 cm和1.5 cm，能夠滿足果園采后運(yùn)輸?shù)男枨蟆７治?個定位點3組試驗的絕對誤差的平均值和標(biāo)準(zhǔn)差以及相對誤差的平均值和標(biāo)準(zhǔn)差，縱向?qū)Ρ?個定位點的相對誤差和絕對誤差，發(fā)現(xiàn)當(dāng)兩定位點之間距離較遠(yuǎn)時，導(dǎo)航誤差變化明顯。對照圖15中機(jī)器人實際行走的軌跡，認(rèn)為出現(xiàn)這種現(xiàn)象的原因可能是沒有考慮機(jī)器人在果園中出現(xiàn)的滑移，缺少對過大橫向偏差的限制，從而導(dǎo)致機(jī)器人長距離行駛的橫向偏離較大，而機(jī)器人在果園跨行行駛轉(zhuǎn)向時再次加大了這種橫向偏差。

表5 定點導(dǎo)航絕對誤差Tab.5 Absolute error of fixed-point navigation m

(2)定點導(dǎo)航避障試驗結(jié)果

試驗結(jié)果如圖15所示，機(jī)器人在設(shè)定有障礙物的路段上能安全避障，即停在原地不同，當(dāng)移走障礙物后機(jī)器人繼續(xù)駛向目標(biāo)點，且在機(jī)器人行駛時橫向偏差會隨著航向不斷變化逐漸減少，直至到達(dá)目標(biāo)點。其中機(jī)器人行駛在路徑中間時的橫向偏差最大，停駛在目標(biāo)點的航向偏差變化最大。分別計算3組試驗中機(jī)器人行駛路徑與理想路徑的橫向航向偏差均值、標(biāo)準(zhǔn)差，結(jié)果如表7所示。

圖15 避障試驗結(jié)果Fig.15 Obstacle avoidance experiment results

從表7可以看到，機(jī)器人行駛路徑與理想行駛路徑間的橫向偏差的均值、標(biāo)準(zhǔn)差分別小于56 cm和0.54 cm，航向偏差均值和標(biāo)準(zhǔn)差分別小于8°和1.6°。這表明機(jī)器人能夠在有障礙物的環(huán)境中，采用D-A控制和急停避障的方法安全地到達(dá)給定目標(biāo)點，滿足運(yùn)輸機(jī)器人果園自主運(yùn)輸?shù)男枨??？v向?qū)Ρ?組試驗在相同停駛點的橫向和航向偏差，發(fā)現(xiàn)最后一組的數(shù)據(jù)最小，這可能是因為重復(fù)行駛過的道路較為平順，利于機(jī)器人依賴D-A控制到達(dá)目標(biāo)點。

表7 位姿偏差Tab.7 Position deviation

4 結(jié)論

(1)針對現(xiàn)有果園采后運(yùn)輸設(shè)備智能化程度偏低的問題，設(shè)計了一種行人引領(lǐng)導(dǎo)航和定點導(dǎo)航的雙模式小型果園運(yùn)輸機(jī)器人。

(2)果園行人引領(lǐng)導(dǎo)航試驗表明，該運(yùn)輸機(jī)器人在蘋果園中根據(jù)設(shè)定手勢執(zhí)行行人引領(lǐng)導(dǎo)航功能時，在多人場景下能正常識別領(lǐng)航員，跟蹤領(lǐng)航員的誤差平均值及標(biāo)準(zhǔn)差均小于9 cm和4 cm，可以滿足機(jī)器人在果園跟隨行人運(yùn)輸果實或物資的需求。

(3)果園定點導(dǎo)航試驗表明，該運(yùn)輸機(jī)器人采用D-A控制方法在蘋果園中進(jìn)行定點導(dǎo)航時，到達(dá)不同目標(biāo)點的絕對誤差及標(biāo)準(zhǔn)差分別小于7 cm和0.5 cm，相對誤差及標(biāo)準(zhǔn)差分別小于13 cm和1.5 cm，可以滿足機(jī)器人在果園自主運(yùn)輸果實或物資定點導(dǎo)航的需求。

(4)果園定點導(dǎo)航避障試驗表明，該運(yùn)輸機(jī)器人在蘋果園行內(nèi)行駛時，能及時停駛實現(xiàn)安全避障(常見障礙物——行人)，且行駛路徑與理想路徑間的橫向偏差和航向偏差均小于56 cm和8°，可以滿足機(jī)器人在果園自主運(yùn)輸果實或物資安全避障的需求。