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

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

(1.浙江理工大學機械與自動控制學院，杭州 310018; 2.浙江經濟職業技術學院物流技術學院，杭州 310018;3.江蘇大學現代農業裝備與技術教育部重點實驗室，鎮江 212013)

0 引言

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

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

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

1 問題描述與分析

圖1 葡萄園環境描述Fig.1 Environment description sketches of vineyard

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

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

2 AGV沿壟道行駛的運動學模型

對于前輪導向的輪式AGV純滾動轉向，左前輪轉角φL與前輪期望導向角φ的關系[18-20]為

(1)

式中L——前、后輪軸距

w——車體寬度

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

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

R0=LcotφL+w/2

(2)

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

(3)

前輪左轉時，兩后輪的轉速比k為[18-20]

(4)

式中RR——右后輪轉彎半徑

RL——左后輪轉彎半徑

eL——左后輪與車架距離

eR——右后輪與車架距離

前輪右轉時

(5)

設AGV運行速度為v，左、右后輪的轉速nL、nR分別為[18-20]

(6)

式中r0——車輪半徑

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

3 AGV沿壟道行駛的防碰避障導航算法

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

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

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

(7)

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

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

3.2 AGV沿壟道行駛的前輪期望導向角獲取

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

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

dR=SRcosθR+w/2

(8)

圖4 預設路徑的邊界探測Fig.4 Boundary detection of default straight path

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

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

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

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

3.2.3基于模糊控制器的AGV前輪期望導向角

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

Δ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)；前輪期望導向角φ∈(-60°，60°)。

依據上述輸入輸出關系制定25條控制規則，模糊控制器輸出的前輪期望導向角如圖6所示，導向角連續平緩變化，可使AGV車身姿態調節平穩。

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

3.3 障礙物判別及軌跡預測

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

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

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

(9)

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

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

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

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

3.3.2動態障礙物的運動軌跡預測

在AGV動坐標系中，障礙物以一定速度運動。將AGV模型化為點狀質心，障礙物膨脹半徑為r。因此，這種碰撞關系可以簡化為點狀AGV到障礙物直線運動軌跡的距離關系問題。相鄰2個規劃周期的障礙物位置數據ρ1與ρ2若滿足|ρ1sinθ1|>|ρ2sinθ2|，則排除遠離AGV的動態障礙物。由圖7得動態障礙物的直線軌跡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調節繞行偏角Δθ繞行障礙物的避障算法

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

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

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

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

Δθ=arcsin(r/ρ)-θ

(11)

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

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

(12)

(2)避障轉向模型

為了實現前輪導向式AGV平滑連貫的避障轉向，建立AGV避障轉向模型，如圖9所示。

圖9 AGV避障轉向模型Fig.9 Steering model of AGV obstacle avoidance

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

(13)

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

(14)

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

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

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

圖10 調節Δθ的避障示意圖Fig.10 Diagram of obstacle avoidance for adjusting Δθ

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

Δd=Tv1sinθsum

(15)

式中T——AGV規劃周期，ms

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

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

(4)調節繞行偏角Δθ的繞行避障算法流程

根據上述避障策略，調節繞行偏角Δθ的繞行避障算法流程，如圖11所示。

圖11 調節Δθ的避障算法流程圖Fig.11 Flow chart of obstacles avoidance algorithm for adjusting Δθ

3.4.2調節Δv的動態障礙物避障策略

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

(16)

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

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

當D

3.4.3避障優先級

AGV默認以恒定速度沿壟道行駛，采用滾動窗口方式檢測障礙物，設定AGV循跡、調節Δθ避障和調節Δv避障的決策優先級為：動態避障、靜態避障、循跡，規則制定如下：

(1)無論AGV現階段執行何種動作，檢測到動態障礙物進入滾動窗口，碰撞預警程度最高，優先采取減速或停車規避。

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

4 AGV避障控制系統設計與試驗

4.1 AGV避障控制系統設計

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

圖12 AGV控制系統硬件組成Fig.12 Hardware composition of AGV control system

工控機作為主控制器，主要完成傳感器數據采集、存儲和處理，避障決策與導航；松下PLC作為AGV轉向與差速驅動的控制器，接收工控機避障算法的輸出指令，完成AGV前輪轉向與后輪差速驅動的協同行駛控制。8路KS101B型超聲波傳感器通過I2C適配器構建數據傳輸的總線子系統。AH100B型航姿傳感器(方位角測量精度0.1°)用于檢測AGV航向偏角。

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

ρ=ρi+Δρ

其中

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

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

AGV沿壟道行駛的導航主程序主要完成系統初始化，在每個規劃周期獲取傳感器信息，刷新滾動窗口，使AGV沿壟道糾偏行駛，并判斷是否執行調節Δv或Δθ的避障子程序。

4.2 AGV避障試驗與分析

4.2.1AGV試驗系統

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

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

圖14 AGV試驗系統Fig.14 Test system of AGV

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

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

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

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

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

圖19 AGV連續繞行2個小間距靜態障礙物的避障數據Fig.19 Obstacle avoidance data when AGV detoured continuously two small space static obstacles

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

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

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

(2)AGV連續繞行2個小間距靜態障礙物試驗

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

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

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

由上述模擬試驗可知，AGV在避障行駛過程中可以局域尋找一條較優無碰路徑，避障過程流暢，驗證了本文提出的AGV避障導航算法的有效性。

5 結束語

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