輪式AGV沿葡萄園壟道行駛避障導(dǎo)航算法與模擬試驗

謝永良 尹建軍 余承超 賀 坤 胡旭東 李仁旺

(1.浙江理工大學(xué)機械與自動控制學(xué)院，杭州 310018; 2.浙江經(jīng)濟職業(yè)技術(shù)學(xué)院物流技術(shù)學(xué)院，杭州 310018;3.江蘇大學(xué)現(xiàn)代農(nóng)業(yè)裝備與技術(shù)教育部重點實驗室，鎮(zhèn)江 212013)

0 引言

輪式自動導(dǎo)引車(Automated guided vehicle，AGV)目前已廣泛應(yīng)用于車間物料運輸、倉儲物流等領(lǐng)域，對企業(yè)柔性集成化生產(chǎn)模式起到至關(guān)重要的作用[1]。在農(nóng)業(yè)生產(chǎn)領(lǐng)域，AGV主要從事果實套袋、施藥、采摘、搬運等工作，為減少勞動力、 提高生產(chǎn)效率提供了一種解決方案[1-2]。因而，實現(xiàn)AGV按既定路線自主導(dǎo)航行駛和防碰避障的智能化局域路徑優(yōu)化逐漸成為國內(nèi)外眾多學(xué)者的研究重點[3-11]。

環(huán)境信息已知的AGV全局路徑規(guī)劃算法有可視圖法、自由空間法、柵格法等[4-6]，但此類算法不適用于障礙物復(fù)雜多變、環(huán)境信息未知的非結(jié)構(gòu)化果園環(huán)境。對于環(huán)境信息未知的局部路徑規(guī)劃算法，勢場蟻群算法存在易陷入局部極小值問題[12]；遺傳算法[13]計算量大，難以保證在復(fù)雜環(huán)境中的實時性和快速性；模糊邏輯算法[14]控制規(guī)則數(shù)目多達幾十條時會占用大量系統(tǒng)資源。上述算法在靜態(tài)障礙物的避障路徑全局規(guī)劃應(yīng)用中取得了良好效果，但并沒有考慮在規(guī)劃過程中局域路徑優(yōu)化問題，在未知的動態(tài)障礙物環(huán)境中缺乏有效的規(guī)劃方法。席裕庚[15]將預(yù)測控制的基本原理從控制問題推廣到一般廣義控制問題求解，利用機器人實時探測得到的局部環(huán)境信息，以滾動窗口方式實施優(yōu)化與反饋相結(jié)合的在線路徑規(guī)劃。文獻[16-17]運用該原理在動態(tài)不確定環(huán)境中取得了良好的避碰效果。

本文面向葡萄園自主行駛應(yīng)用需求，針對輪式AGV沿葡萄園壟道循跡受到障礙物阻礙的問題，基于超聲波測距與AGV航向偏角檢測，應(yīng)用滾動優(yōu)化原理研究輪式AGV沿直線壟道行駛的避障導(dǎo)航策略和算法，以工控機與PLC構(gòu)建主從控制的AGV試驗系統(tǒng)，通過模擬試驗來驗證導(dǎo)航算法的有效性，以期為輪式拖拉機等前輪導(dǎo)向車輛無人化作業(yè)時沿壟道循跡的防碰預(yù)警與避障技術(shù)提供參考。

1 問題描述與分析

圖1 葡萄園環(huán)境描述Fig.1 Environment description sketches of vineyard

若壟道無障礙，AGV沿壟道中線行駛，設(shè)定為預(yù)定軌跡。若壟道存在靜態(tài)障礙物，則檢測其與AGV運行軌跡的相對位置關(guān)系，判斷是否執(zhí)行避障策略。若壟道存在動態(tài)障礙物，任意相鄰瞬時時間間隔內(nèi)認為其以直線軌跡移動。由于AGV在果園環(huán)境下作業(yè)是一種慢動態(tài)場景，若能預(yù)測動態(tài)障礙物的當前時刻運行軌跡，找到預(yù)測碰撞點，則可引導(dǎo)AGV避讓動態(tài)障礙物。

AGV果園自主行駛分為2種模式：全局預(yù)設(shè)路徑的糾偏行駛和局域軌跡下的防撞避障行駛模式。對于前者，AGV保持在果園壟道勻速行駛，根據(jù)AGV右側(cè)的測距傳感器探測的距離和航向偏角糾偏行駛。對于后者，AGV模擬駕駛員避障經(jīng)驗，分為調(diào)節(jié)AGV航向偏角的繞行靜態(tài)障礙物避障和調(diào)節(jié)AGV車速的動態(tài)障礙物避讓。

2 AGV沿壟道行駛的運動學(xué)模型

對于前輪導(dǎo)向的輪式AGV純滾動轉(zhuǎn)向，左前輪轉(zhuǎn)角φL與前輪期望導(dǎo)向角φ的關(guān)系[18-20]為

(1)

式中L——前、后輪軸距

w——車體寬度

以AGV前進方向為計量基準，左轉(zhuǎn)時φ為正值，右轉(zhuǎn)時φ為負值。

左前輪轉(zhuǎn)角φL與兩后輪中點處轉(zhuǎn)彎半徑R0滿足[18-20]

R0=LcotφL+w/2

(2)

右前輪轉(zhuǎn)角φR與兩后輪中點處轉(zhuǎn)彎半徑R0滿足[18-20]

(3)

前輪左轉(zhuǎn)時，兩后輪的轉(zhuǎn)速比k為[18-20]

(4)

式中RR——右后輪轉(zhuǎn)彎半徑

RL——左后輪轉(zhuǎn)彎半徑

eL——左后輪與車架距離

eR——右后輪與車架距離

前輪右轉(zhuǎn)時

(5)

設(shè)AGV運行速度為v，左、右后輪的轉(zhuǎn)速nL、nR分別為[18-20]

(6)

式中r0——車輪半徑

AGV路徑跟蹤過程中，由模糊控制器輸出前輪期望導(dǎo)向角φ，由式(1)、(3)可確定左、右前輪轉(zhuǎn)角φL、φR，由式(6)計算設(shè)定車速下左、右后輪轉(zhuǎn)速nL、nR，實現(xiàn)AGV純滾動轉(zhuǎn)向與差速驅(qū)動的協(xié)同控制[18]。

3 AGV沿壟道行駛的防碰避障導(dǎo)航算法

滾動優(yōu)化是預(yù)測控制基本原理之一[15]，將AGV自帶傳感器探測得到的有限局部信息看成一個滾動窗口，并在當前窗口內(nèi)實施局部避障策略。在有限時域內(nèi)每結(jié)束一個規(guī)劃周期，AGV將刷新窗口內(nèi)的動態(tài)信息，并重復(fù)執(zhí)行滾動過程，結(jié)合窗口滾動和反饋機制實現(xiàn)AGV全局路徑規(guī)劃。滾動優(yōu)化原理具有實時性強、適用于靜態(tài)和動態(tài)障礙物檢測條件，其滾動窗口算法原理如圖2所示。

圖2 滾動窗口算法原理圖Fig.2 Algorithm principle diagram of rolling windows

以傳感器可以探測得到的最大距離R為半徑的圓形區(qū)域作為窗口Win(PC(t))，PC(t)為AGV當前時刻所在位置。AGV每次規(guī)劃步長ε(與運行速度和規(guī)劃時長有關(guān))作為規(guī)劃窗口的半徑，其中0<ε

(7)

式中l(wèi)——AGV本體長度

3.1 超聲波傳感器檢測障礙物的配置方案

如圖3所示，8路超聲波傳感器安裝在距地高500 mm的AGV周邊，探測范圍0.01～5 m，波束角60°。

圖3 8路超聲波傳感器配置方案Fig.3 Configuration scheme of 8-way ultrasonic sensors

單個超聲波傳感器在60°扇區(qū)內(nèi)僅能獲取障礙物距離信息而無法探測其方位角。為提高超聲波傳感器方位角測量分辨率，在AGV車頭左、右角各交錯布置3個超聲波探頭1、2、3和5、6、7，AGV車頭正中水平布置超聲波探頭4，AGV車尾正中水平布置超聲波探頭0，形成AGV車頭300°范圍測距、車尾60°測距。在AGV車頭180°范圍內(nèi)形成交叉重疊的測距與方位角檢測，超聲波傳感器對單個障礙物的方位角測量分辨率提高到15°或30°，測量空間劃分為9個區(qū)間，取每個區(qū)間夾角平分線對應(yīng)的角度作為障礙物方位角?？紤]到障礙物大小，同一障礙物有可能被2個或3個區(qū)間的超聲波傳感器探測到。若探頭2、3、4或4、5、6同時返回信號視為單探頭3或5感應(yīng)。若探頭3、4、5同時返回信號，則認為該障礙物不可逾越，AGV選擇停車報警。若探頭0檢測到AGV后方障礙物距離車體小于0.5 m時，AGV發(fā)出聲音預(yù)警防碰。這樣，在超聲波探頭有限測量范圍內(nèi)的任意時刻，2個或2個以上障礙物位于單探頭或雙探頭交叉區(qū)間，將所有障礙物視作單個障礙物，單障礙物的方位角θ判定如表1所示。

表1 單障礙物的方位角判定Tab.1 Azimuth determination of single obstacle (°)

3.2 AGV沿壟道行駛的前輪期望導(dǎo)向角獲取

3.2.1基于超聲波測距的壟道邊界探測

如圖4所示，為使AGV能夠沿距離右側(cè)道路邊緣為d的路徑行走，采用右側(cè)超聲波傳感器探測路徑邊界。超聲波傳感器與AGV車頭水平面的安裝夾角為θR，超聲波沿扇形中線發(fā)送，可以保證AGV在進入壟道后，傳感器可探測到路徑右側(cè)柱樁及葡萄樹主莖相對AGV的距離SR。AGV車身中線距右側(cè)柱樁距離dR為

dR=SRcosθR+w/2

(8)

圖4 預(yù)設(shè)路徑的邊界探測Fig.4 Boundary detection of default straight path

AGV車身中線相對預(yù)設(shè)路徑的橫向偏差Δd=d-dR，位置左偏為負，右偏為正。

3.2.2基于航姿傳感器的AGV航向角檢測

以大地正北方向為0°航向角，選取半程壟道按如圖5a所示通過航姿傳感器測取航向角作為預(yù)設(shè)航向角γ0。如圖5b所示，沿AGV縱向中線安裝航姿傳感器，車體初始姿態(tài)沿壟道走向，γ表示AGV沿壟道行駛的方向角。將預(yù)設(shè)航向角γ0與AGV當前航向角γ的角度差Δγ作為AGV航向偏角。

圖5 航向偏角測量Fig.5 Measurement of heading deflection angle

3.2.3基于模糊控制器的AGV前輪期望導(dǎo)向角

將AGV橫向位置偏差Δd與航向偏角Δγ作為路徑跟蹤模糊控制器的輸入，控制器的輸出為前輪期望導(dǎo)向角φ。AGV航向偏角與橫向位置偏差根據(jù)其誤差分別設(shè)置5個模糊等級：左大偏(LB)、左偏(LS)、不偏(Z)、右偏(RS)、右大偏(RB)；前輪期望導(dǎo)向角設(shè)置7個等級：右大轉(zhuǎn)(PB)、右中轉(zhuǎn)(PM)、右小轉(zhuǎn)(PS)、不轉(zhuǎn)(Z)、左小轉(zhuǎn)(NS)、左中轉(zhuǎn)(NM)、左大轉(zhuǎn)(NB)。則模糊控制器輸入與輸出對應(yīng)的模糊集為

Δd={LB,LS,Z,RS,RB}
Δγ={LB,LS,Z,RS,RB}
φ={PB,PM,PS,Z,NS,NM,NB}

其中，航向偏角Δγ∈(-60°,60°)；橫向位置偏差Δd∈(-0.5 m,0.5 m)；前輪期望導(dǎo)向角φ∈(-60°，60°)。

依據(jù)上述輸入輸出關(guān)系制定25條控制規(guī)則，模糊控制器輸出的前輪期望導(dǎo)向角如圖6所示，導(dǎo)向角連續(xù)平緩變化，可使AGV車身姿態(tài)調(diào)節(jié)平穩(wěn)。

圖6 模糊控制器輸出的前輪期望導(dǎo)向角Fig.6 Expected steering angle of front wheel output by fuzzy controller

3.3 障礙物判別及軌跡預(yù)測

3.3.1同一障礙物匹配與類型判別

圖7 障礙物檢測Fig.7 Obstacle detection

如圖7所示，以2個障礙物在車體左、右側(cè)為例，AGV上超聲波探頭測得的位置數(shù)據(jù)有2個規(guī)劃周期的保存時間。首先濾除與AGV相距d的道路兩側(cè)的探頭數(shù)據(jù)，然后提取第1個規(guī)劃周期滾動窗口內(nèi)的2個障礙物位置數(shù)據(jù)ρ1,E1、ρ1,F1和第2個規(guī)劃周期的ρ2,E2、ρ2,F2，將后一規(guī)劃周期的數(shù)據(jù)ρ2,E2分別與前一規(guī)劃周期的2個數(shù)據(jù)ρ1,E1、ρ1,F1組合計算兩點之間的距離

(9)

式中ρ1、ρ2——障礙物方位距離

θ1、θ2——障礙物方位角

按式(9)計算得到HE1E2、HF1E2、HE1F2和HF1F2，判定min{HE1E2,HF1E2}對應(yīng)的兩點為同一障礙物，同理判定min{HE1F2,HF1F2}對應(yīng)的兩點為同一障礙物。

若|ρ2sinθ2-ρ1sinθ1|>δ，δ為設(shè)定的距離閾值，即2個規(guī)劃周期內(nèi)，存在同一障礙物到AGV現(xiàn)階段運行軌跡的垂直距離發(fā)生改變，可以判定為動態(tài)障礙物，否則判定為靜態(tài)障礙物。

3.3.2動態(tài)障礙物的運動軌跡預(yù)測

在AGV動坐標系中，障礙物以一定速度運動。將AGV模型化為點狀質(zhì)心，障礙物膨脹半徑為r。因此，這種碰撞關(guān)系可以簡化為點狀A(yù)GV到障礙物直線運動軌跡的距離關(guān)系問題。相鄰2個規(guī)劃周期的障礙物位置數(shù)據(jù)ρ1與ρ2若滿足|ρ1sinθ1|>|ρ2sinθ2|，則排除遠離AGV的動態(tài)障礙物。由圖7得動態(tài)障礙物的直線軌跡E1E2方程為

Aρcosθ+Bρsinθ+C=0

(10)

其中

A=ρ2sinθ2-ρ1sinθ1

B=ρ1cosθ1-ρ2cosθ2
C=ρ2cosθ2ρ1sinθ1-ρ1cosθ1ρ2sinθ2

3.4 AGV防撞避障策略與算法

3.4.1調(diào)節(jié)繞行偏角Δθ繞行障礙物的避障算法

來自AGV前方的障礙物阻礙包括靜態(tài)障礙物和相向而來的動態(tài)障礙物，此時以改變AGV繞行偏角來避障[21]。如圖8所示，以AGV質(zhì)心為原點建立AGV動坐標系xOy。以AGV質(zhì)心O為原點、AGV前進方向為極軸建立極坐標系(ρ,θ)，將障礙物的動坐標轉(zhuǎn)換為極坐標表示，構(gòu)建由距離ρ和方位角θ組成的超聲波傳感器測量模型。將θ方向上的障礙物按照半徑r進行邊緣膨脹，構(gòu)建防碰撞安全保護區(qū)。

圖8 繞行偏角確定Fig.8 Determination of heading deviation angle

(1)檢測AGV車體與當前障礙物的相對角度

當0≤θ<π，且滿足d=ρsinθ

Δθ=arcsin(r/ρ)-θ

(11)

當π≤θ<2π，且滿足d=-ρsinθ

Δθ=2π-arcsin(r/ρ)-θ

(12)

(2)避障轉(zhuǎn)向模型

為了實現(xiàn)前輪導(dǎo)向式AGV平滑連貫的避障轉(zhuǎn)向，建立AGV避障轉(zhuǎn)向模型，如圖9所示。

圖9 AGV避障轉(zhuǎn)向模型Fig.9 Steering model of AGV obstacle avoidance

AGV在偏角轉(zhuǎn)向時繞瞬心P做圓周運動，AGV前一時刻速度v所在直線與車體后輪軸線交于B點、后一時刻速度v′所在直線與車體前輪軸線相交于A點，切線AO和BO的交點O與P點的連線平分∠AOB，可得

(13)

由式(3)重新計算右前輪轉(zhuǎn)角，則左前輪轉(zhuǎn)角為[16]

(14)

再由式(6)得兩后輪差速控制輸出，實現(xiàn)AGV避障轉(zhuǎn)向行駛。

(3)確定回歸轉(zhuǎn)向點與回歸運動子目標點

為實現(xiàn)多次調(diào)節(jié)繞行偏角Δθ避障繞行，記錄AGV累積姿態(tài)角θsum，θsum=∑Δθi，i為轉(zhuǎn)向次數(shù)。如圖10所示，調(diào)節(jié)繞行偏角Δθ的障礙物避障策略中，確定AGV以累積姿態(tài)角θsum直線行駛為執(zhí)行繞行障礙物的第1階段。當AGV到達回歸轉(zhuǎn)向點Pback時，即超聲波傳感器檢測到障礙物與AGV之間的距離由減小到增大的轉(zhuǎn)折點，表明AGV恰好開始遠離障礙物，此時執(zhí)行全局預(yù)設(shè)軌跡的回歸運動，AGV回歸的姿態(tài)角為-2θsum，左、右前輪轉(zhuǎn)角由式(14)、(3)計算得到。

圖10 調(diào)節(jié)Δθ的避障示意圖Fig.10 Diagram of obstacle avoidance for adjusting Δθ

避障過程中，AGV橫向位置偏差為

Δd=Tv1sinθsum

(15)

式中T——AGV規(guī)劃周期，ms

v1——編碼器測定的車速，m/s

當回歸運動使橫向位置偏差重新為零時，即為回歸運動的子目標點所在位置Prec，AGV完成障礙物繞行的第2階段，結(jié)束避障，再繼續(xù)執(zhí)行沿壟道預(yù)設(shè)軌跡行駛。

(4)調(diào)節(jié)繞行偏角Δθ的繞行避障算法流程

根據(jù)上述避障策略，調(diào)節(jié)繞行偏角Δθ的繞行避障算法流程，如圖11所示。

圖11 調(diào)節(jié)Δθ的避障算法流程圖Fig.11 Flow chart of obstacles avoidance algorithm for adjusting Δθ

3.4.2調(diào)節(jié)Δv的動態(tài)障礙物避障策略

如圖7所示，直線E1E2為障礙物當前時刻預(yù)測軌跡，由點到直線距離方程計算AGV質(zhì)心與障礙物預(yù)測軌跡的垂直距離D為

(16)

當D≥r時，說明動態(tài)障礙物運動路徑在AGV膨脹保護圓以外，對AGV無影響。

當D0，即預(yù)測軌跡與動坐標系x軸正半軸相交，說明障礙物與AGV預(yù)測碰撞點在車身前側(cè)，AGV減速或停車，待傳感器測不到此障礙物再恢復(fù)設(shè)定速度。

當D

3.4.3避障優(yōu)先級

AGV默認以恒定速度沿壟道行駛，采用滾動窗口方式檢測障礙物，設(shè)定AGV循跡、調(diào)節(jié)Δθ避障和調(diào)節(jié)Δv避障的決策優(yōu)先級為：動態(tài)避障、靜態(tài)避障、循跡，規(guī)則制定如下：

(1)無論AGV現(xiàn)階段執(zhí)行何種動作，檢測到動態(tài)障礙物進入滾動窗口，碰撞預(yù)警程度最高，優(yōu)先采取減速或停車規(guī)避。

(2)AGV預(yù)設(shè)行駛軌跡與滾動窗口中靜態(tài)障礙物膨化邊緣存在交集，優(yōu)先執(zhí)行繞行避障策略，忽略路徑跟蹤行駛的模糊控制器輸出。

4 AGV避障控制系統(tǒng)設(shè)計與試驗

4.1 AGV避障控制系統(tǒng)設(shè)計

如圖12所示，輪式AGV控制系統(tǒng)由控制器模塊、驅(qū)動模塊、電源模塊、傳感器模塊、通信模塊等組成。

圖12 AGV控制系統(tǒng)硬件組成Fig.12 Hardware composition of AGV control system

工控機作為主控制器，主要完成傳感器數(shù)據(jù)采集、存儲和處理，避障決策與導(dǎo)航；松下PLC作為AGV轉(zhuǎn)向與差速驅(qū)動的控制器，接收工控機避障算法的輸出指令，完成AGV前輪轉(zhuǎn)向與后輪差速驅(qū)動的協(xié)同行駛控制。8路KS101B型超聲波傳感器通過I2C適配器構(gòu)建數(shù)據(jù)傳輸?shù)目偩€子系統(tǒng)。AH100B型航姿傳感器(方位角測量精度0.1°)用于檢測AGV航向偏角。

為防止超聲波串擾，在每個規(guī)劃周期以10 ms為間隔通過I2C總線順序執(zhí)行超聲波距離數(shù)據(jù)的發(fā)送和接收指令。AGV前方左、右角安裝的超聲波傳感器探測的障礙物與車體距離實際為物體表面到傳感器安裝位置的距離。因此，AGV質(zhì)心與障礙物的相對距離ρ為實測值ρi加上補償距離Δρ，即

ρ=ρi+Δρ

其中

利用LabVIEW平臺編寫AGV導(dǎo)航控制程序，包括主程序、沿壟道直線行駛的模糊控制器以及防碰避障子程序，主程序流程圖如圖13所示。

圖13 AGV沿壟道行駛的導(dǎo)航主程序流程圖Fig.13 Flow chart of main program for AGV navigating along ridge road

AGV沿壟道行駛的導(dǎo)航主程序主要完成系統(tǒng)初始化，在每個規(guī)劃周期獲取傳感器信息，刷新滾動窗口，使AGV沿壟道糾偏行駛，并判斷是否執(zhí)行調(diào)節(jié)Δv或Δθ的避障子程序。

4.2 AGV避障試驗與分析

4.2.1AGV試驗系統(tǒng)

圖15 AGV依次繞行2個大間距靜態(tài)障礙物的避障試驗過程Fig.15 Obstacle avoidance test process when AGV detoured in turn two large space static obstacles

為驗證設(shè)計的AGV沿壟道行駛的避障導(dǎo)航算法的有效性，模擬葡萄園種植方式開展AGV導(dǎo)航試驗。試驗場地為寬3.5 m、長50 m的平整道路。距AGV右側(cè)1.7 m處有0.8 m高的路界，距AGV左側(cè)3.9 m處為墻面。沿道路縱向布置多個方錐形路障模擬實際葡萄園環(huán)境中的靜態(tài)障礙物，方錐底座寬28 cm、高65 cm。如圖14所示，試驗平臺為前輪轉(zhuǎn)向、后輪差速驅(qū)動AGV，軸距L=93.5 cm，輪距49 cm，AGV總質(zhì)量200 kg，行駛速度0.3 m/s。

圖14 AGV試驗系統(tǒng)Fig.14 Test system of AGV

4.2.2AGV沿壟道行駛的避障模擬試驗

試驗中以方錐形路障作為靜態(tài)障礙物、行人作為動態(tài)障礙物，超聲波傳感器在2 m以內(nèi)對障礙物探測穩(wěn)定，以此設(shè)置滾動窗口半徑為2 m，大于2 m的數(shù)據(jù)設(shè)為空值。試驗現(xiàn)場如圖15～17所示，避障數(shù)據(jù)如圖18～20所示。AGV上的8路超聲波傳感器測距數(shù)據(jù)如圖18a、19a、20a所示。記錄AGV航向角隨AGV沿壟道行駛時的采樣數(shù)據(jù)，如圖18b、19b、20b所示。根據(jù)測得的AGV航向角與后輪編碼器記錄數(shù)據(jù)，繪制的AGV實際運行軌跡如圖18c、19c、20c所示，以AGV初始位置的質(zhì)心O為原點建立靜坐標系XOY，藍色圓形為障礙物，黃色曲線為AGV質(zhì)心每個規(guī)劃周期的循跡與避障運行軌跡。

圖16 AGV連續(xù)繞行2個小間距靜態(tài)障礙物的避障試驗過程Fig.16 Obstacle avoidance test process when AGV detoured continuously two small space static obstacles

圖17 AGV繞行1個靜態(tài)障礙物并躲避1個動態(tài)障礙物的避障試驗過程Fig.17 Obstacle avoidance test process when AGV detoured a small static obstacle and avoided a dynamic obstacle

圖18 AGV依次繞行2個大間距靜態(tài)障礙物的避障數(shù)據(jù)Fig.18 Obstacle avoidance data when AGV detoured in turn two large space obstacles

圖19 AGV連續(xù)繞行2個小間距靜態(tài)障礙物的避障數(shù)據(jù)Fig.19 Obstacle avoidance data when AGV detoured continuously two small space static obstacles

圖20 AGV繞行1個靜態(tài)障礙物并躲避1個動態(tài)障礙物的避障試驗數(shù)據(jù)Fig.20 Obstacle avoidance test data when AGV detoured a small static obstacle and avoided a dynamic obstacle

(1)AGV依次繞行2個大間距障礙物試驗

如圖15a所示，AGV從初始位置按預(yù)設(shè)航向角沿距離右側(cè)邊界1.7 m直線行駛，按照模糊控制器設(shè)計規(guī)則，此時以AGV與右側(cè)邊界橫向偏差ΔdR作為模糊控制的輸入。探頭6、7檢測的數(shù)據(jù)維持穩(wěn)定。當探頭3、4檢測到左前方路障滿足碰撞條件后，AGV實施右轉(zhuǎn)繞行，如圖15b所示，回正前輪轉(zhuǎn)角后沿路障安全保護圓右側(cè)切線直線行駛，此時探頭4、3、2、1依次檢測到障礙物，探頭1、2、3、4、6、7距離數(shù)據(jù)逐漸減小，車體與障礙物及道路右側(cè)邊界的距離逐漸縮短。當探頭1感測車體與障礙物距離數(shù)據(jù)增大，判斷為AGV回歸轉(zhuǎn)向點，開始執(zhí)行繞障礙物環(huán)形的回歸轉(zhuǎn)向運動，如圖15c所示。當探頭4、5檢測到第2個障礙物，繼續(xù)執(zhí)行繞行程序,如圖15d～15f所示。當AGV繞過第2個障礙物后，由模糊控制器糾偏行駛，由探頭6判定車身和壟道右側(cè)邊界穩(wěn)定保持1.7 m的設(shè)定距離后停車。

(2)AGV連續(xù)繞行2個小間距靜態(tài)障礙物試驗

圖16給出AGV連續(xù)繞行2個小間距障礙物時的避障結(jié)果。如圖16a所示，探頭4、5依次檢測到偏于壟道右側(cè)的障礙物1，AGV左轉(zhuǎn)繞行。如圖16b所示，障礙物2進入滾動窗口，位于單探頭或雙探頭交叉區(qū)間，AGV將2個小間距靜態(tài)障礙物合并為同一障礙物繼續(xù)左轉(zhuǎn)繞行。障礙物1遠離AGV，第45個采樣點后采樣點依次從探頭5、6探測區(qū)間消失，同時障礙物2進入5、6探測區(qū)間，并逐漸靠近AGV。如圖16c～16f所示，AGV由回歸轉(zhuǎn)向點直線行駛至回歸子目標點，并糾正偏差，在壟道預(yù)設(shè)路徑停車。

(3)AGV繞行1個靜態(tài)障礙物并躲避1個動態(tài)障礙物試驗

圖17給出AGV繞行靜態(tài)障礙物時加入1個動態(tài)障礙物的避障結(jié)果，動態(tài)障礙物的起點坐標(2.8，1.75)，終點坐標(2.8，-1.75)。如圖17a、17b所示，AGV首先繞行壟道右前方靜態(tài)障礙物，向回歸轉(zhuǎn)向點直線行駛過程中，以人為動態(tài)障礙物從AGV滾動窗口內(nèi)橫穿壟道。AGV根據(jù)同一障礙物匹配和類型判別算法，判定動態(tài)障礙物預(yù)測碰撞點在車體前側(cè)，選擇減速停車讓行的避障控制策略。在圖20c中，S點處停車避讓，探頭2、3、4、5依次檢測到車體前側(cè)的動態(tài)障礙物。如圖17c、17d所示，待動態(tài)障礙物遠離，AGV靠近靜態(tài)障礙物，如圖20a中的第100個采樣點，判定合并為同一障礙物后，AGV恢復(fù)設(shè)定速度繼續(xù)繞行，回到預(yù)設(shè)路徑。

由上述模擬試驗可知，AGV在避障行駛過程中可以局域?qū)ふ乙粭l較優(yōu)無碰路徑，避障過程流暢，驗證了本文提出的AGV避障導(dǎo)航算法的有效性。

5 結(jié)束語

針對輪式AGV沿葡萄園壟道直線行駛過程中可能存在障礙物阻礙，采用交叉布置的8路超聲波傳感器檢測壟道邊界與障礙物，提出了同一障礙物匹配與類型判據(jù)及動態(tài)障礙物軌跡預(yù)測模型。基于滾動優(yōu)化原理提出AGV繞行靜態(tài)障礙物、減速或停車避讓動態(tài)障礙物的導(dǎo)航算法，模擬試驗結(jié)果表明，該算法可導(dǎo)引AGV避免與壟道上的障礙物發(fā)生碰撞，驗證了避障導(dǎo)航算法的有效性。