錢曉明 黃宇軒 樓佩煌 孫 天

(南京航空航天大學機電學院， 南京 210016)

0 引言

自動導引小車(Automated guided vehicle, AGV)是一種沿指定路徑自動行駛，在不同工位點之間進行物料搬運的輪式移動機器人[1]，廣泛應用于工業(yè)生產(chǎn)和物流運輸[2]。隨著AGV應用場景需求的復雜化，多AGV協(xié)同搬運是機器人技術發(fā)展的必然趨勢。多臺AGV協(xié)同搬運可以突破單臺AGV尺寸、承載方式和載荷量等限制，充分發(fā)揮AGV的運輸能力和編隊協(xié)作靈活性。目前，多AGV協(xié)同大多采用“主從式”(Leader-Follower)的編隊策略。

導引技術是保證AGV自主性和移動性的關鍵技術。對于“主從式”雙車協(xié)同搬運系統(tǒng)，領航AGV的導引方式與單臺AGV類似，技術上已趨于成熟；跟隨AGV的定位和導引，除了獲取環(huán)境信息之外，還需要觀測領航AGV進行路徑跟隨，對精度和穩(wěn)定性有更高的要求。目前廣泛采用的AGV導航技術在定位精度、導引靈活性、運行可靠性和使用成本等多種指標之間難以取得較好的協(xié)調(diào)平衡。當前復合導引大多采用GPS導航、慣性導航、視覺導航等導航技術中的若干種，通過數(shù)據(jù)融合算法綜合多種異構多源傳感器采集的信息，使AGV導引更為穩(wěn)定可靠。

多傳感器融合技術[3-4]是將多種傳感器的信息進行綜合，輸出與真實值較為接近的觀測信息，實現(xiàn)各傳感器的優(yōu)勢互補。視覺里程計和IMU的數(shù)據(jù)融合又稱為視覺慣性里程計(Visual inertial odometry, VIO)，根據(jù)數(shù)據(jù)融合方式的差異可以分為緊耦合和松耦合兩種[5]。KONOLIGE等[6]通過擴展卡爾曼濾波(Extended Kalman filter,EKF)對慣導-視覺系統(tǒng)進行松耦合，提高了視覺里程計的精度。INDELMAN等[7]基于因子圖的慣導增量平滑方式，提出一個通用的信息融合框架。MOURIKIS等[8]提出了多狀態(tài)限制卡爾曼濾波(Multi-state constraint Kalman filter, MSCKF)算法，為VIO提供了第一個緊耦合框架。LI等[9]通過改進算法提高了MSCKF的實時性。TSAO等[10]提出基于EKF算法、視覺-慣導融合的相機參數(shù)標定方法。除了科研機構外，一些大型設備制造商也開展了視覺/慣導的相關研究，例如蘋果公司推出的ARKit，將單目視覺和慣導信息通過EKF進行融合。隨著智能化的深入，視覺里程計/慣導結(jié)合的多傳感器融合技術已成為機器人領域熱門的研究方向。

本文基于Leader-Follower編隊中跟隨式AGV的定位和導引需求，提出一種視覺/慣導組合導航方案，其中視覺系統(tǒng)采用路徑跟蹤導航和RGB-D導航相結(jié)合的多目視覺導航。針對多目視覺系統(tǒng)和慣性導航的數(shù)據(jù)融合問題，提出一種基于AUKF算法的跟隨AGV最優(yōu)位姿估計方法，以保障跟隨AGV復合導引的精度和穩(wěn)定性。

1 復合導引方案設計

AGV的導引和定位方式可以分為絕對定位方式和相對定位方式[11]。考慮到雙車協(xié)同搬運中跟隨AGV在編隊中的導引需求，將相對定位與絕對定位方式相結(jié)合。跟隨AGV的復合導引方案如圖1所示。慣性導引作為一種成熟的相對定位方法，采樣頻率高、抗干擾性強，能夠滿足AGV位姿估計實時性和準確性的需求。但其在測量數(shù)據(jù)時存在累積誤差，一般用于短時位姿的估計，不能單獨使用。本文使用視覺導引作為絕對定位方式，與慣性導航配合，實現(xiàn)對慣導偏移誤差的實時校正。

圖1 跟隨AGV復合導引方案Fig.1 Compound navigation scheme of following AGV

2 跟隨AGV多目視覺導航方法

2.1 多目視覺導航技術框架

AGV協(xié)同搬運的工作場景多為復雜工況環(huán)境，如果選用單一的視覺傳感單元會使跟隨AGV感知信息不全面、易受外部環(huán)境影響，無法保證雙車協(xié)同搬運系統(tǒng)長期穩(wěn)定運行。本文提出一種路徑跟蹤導航與RGB-D視覺導航相結(jié)合的多目視覺導航方法，如圖2所示。

圖2 多目視覺導航技術框架Fig.2 Framework of multi-vision navigation technology

跟隨AGV的視覺系統(tǒng)布局如圖3所示，本文多目視覺導航系統(tǒng)由路徑跟蹤導航方法和RGB-D視覺導航方法組成：

圖3 跟隨AGV視覺系統(tǒng)布局Fig.3 Visual system layout of following AGV

(1)路徑跟蹤導航方法。在AGV行駛路徑上的兩側(cè)設置導引線，通過跟隨AGV兩側(cè)安裝的CCD攝像機采集導引線圖像信息。兩側(cè)導引線限制跟隨AGV在導引線劃定的區(qū)域內(nèi)運行。AGV路徑跟蹤導航技術目前發(fā)展比較成熟，本文不作重點描述。

(2)RGB-D視覺導航方法。RGB-D深度相機實時采集領航AGV的外殼輪廓圖像，并匹配相鄰兩幅圖像的特征點，通過ICP算法求取跟隨AGV的相對位姿。

復雜環(huán)境同時影響兩種視覺導航方法的可能性極小，可以認為任何時刻至少有一種視覺導航方法有效且穩(wěn)定。受外界環(huán)境干擾時，其中一種視覺導航方法性能降低，另一種導航方法可以保證AGV平穩(wěn)運行。多目視覺系統(tǒng)未受外界環(huán)境干擾時，兩種導航方法的精度均得到保障。兩種視覺導航信息相互融合和校正，可以獲得盡可能全面、準確的外部環(huán)境觀測。

2.2 路徑跟蹤導航

為保證跟隨AGV具有較高的預測控制能力，車載相機傾斜向下安裝。兩側(cè)CCD攝像機實時采集路徑導引線的信息，經(jīng)視頻解碼器解碼后，輸出至圖像處理模塊中處理。路徑跟蹤導航使用導引線，鋪設在水磨石地面上，在YCbCr色彩空間下識別效果較好，故進行色彩空間轉(zhuǎn)換，使用YCbCr圖像中Cb分量進行閾值分割。

如圖4所示，首先使用Otsu方法進行圖像分割，得到路徑導引線的二值化圖像，經(jīng)過形態(tài)學核運算獲得較為光滑平整的路徑軌跡輪廓，最后通過骨架提取算法提取路徑特征，得到導引路徑中心線。

圖4 導引路徑識別圖像Fig.4 Recognition images of navigation path

直線導引路徑可以用方程y=β0x+β1來表示，而曲線路徑可以采用“以直代曲”思想視作多段較短的連續(xù)直線段拼接而成。對導引路徑圖像進行擬合，擬合結(jié)果如圖5所示，并求解路徑參數(shù)β0和β1。通過幾何關系可以獲取跟隨AGV相對路徑中心線的角度偏差eθ及距離偏差ed。

圖5 路徑擬合結(jié)果Fig.5 Result of path fitting

2.3 RGB-D立體視覺導航

RGB-D相機能夠?qū)崟r獲取AGV的RGB圖像和深度圖像。RGB-D導航的位姿測量流程如圖6所示。首先對原始圖像進行預處理，通過閾值分割獲得領航AGV的外殼輪廓。然后對相鄰兩幀圖像進行特征點的提取和匹配，最后通過對點云數(shù)據(jù)的配準，得到深度相機的相對位姿(相機的旋轉(zhuǎn)矩陣R和平移向量t)。由于深度相機固連在跟隨AGV車身上，深度相機的運動軌跡與跟隨AGV一致。實驗選用Intel公司研發(fā)的RealSense D435i型深度相機。

圖6 RGB-D導航位姿測量流程Fig.6 RGB-D navigation measurement process

2.3.1圖像預處理和特征點檢測

RGB立體視覺導航方法用于觀測領航AGV，首先進行觀測圖像預處理。使用文獻[12]中的最佳閾值分割方法來進行圖像閾值的選取，通過閾值分割和平面提取算法分割出領航AGV的外殼輪廓。圖像分割結(jié)果如圖7所示。

圖7 圖像分割結(jié)果Fig.7 Result of image segmentation

完成對領航AGV外殼輪廓的分割后，對預處理后的圖像進行特征點檢測?？紤]到雙車協(xié)同搬運系統(tǒng)對實時性的要求，選擇ORB(Oriented FAST and rotated BRIEF)特征點來進行領航AGV輪廓特征點的檢測，特征提取后得到圖像的ORB特征點集如圖8所示。

圖8 特征點檢測結(jié)果Fig.8 Result of prominent points detection

2.3.2改進RANSAC特征點匹配

隨機抽樣一致(Random sample consensus, RANSAC)算法對于數(shù)據(jù)排異和提高模型估計精度有良好的效果[13]。傳統(tǒng)RANSAC算法隨機抽取特征點對迭代，算法復雜度較高；同時算法具有一定的隨機性，不能保證特征點匹配的精度，為了提高匹配精度，必須提高迭代的次數(shù)。本文基于RANSAC算法提出一種改良算法，用于深度圖像的特征點匹配。

改進RANSAC的特征點匹配算法具體實現(xiàn)步驟如下：

(1)特征點初始化

通過ORB特征點檢測方法獲得相鄰兩幅圖像的特征點集。前一幅圖像的特征點集X={xi∈X,i=1,2,…,n}，后一幅圖像的特征點集Y={yi∈Y,i=1,2,…,n}。

(2)特征點粗匹配

ORB特征點通過漢明距離進行圖像特征點匹配，在匹配過程中會存在一些錯誤匹配的特征點對。使用基于閾值的粗匹配算法初步過濾這些離群點：首先計算兩幅圖像特征點的漢明距離，采用FLANN算法進行特征點的配準，計算所有匹配點對的最近鄰距離d1和次近鄰距離d2。設定閾值T，如果兩者的比值r=d1/d2小于T，則保留該匹配點對。本文T取0.7。

(3)雙向交叉濾波篩選

通過步驟(2)可以得到粗匹配的圖像，新的特征點集記作X′和Y′。由于RealSense D435i型相機對于深度值較大特征點的測量存在一定誤差，因此采用雙向交叉濾波方法對圖像的特征點進行進一步篩選：在點集X′中尋找特征點xi，對應Y′中特征點yi。在點集Y′中尋找特征點yi，對應X′中特征點xi?；诜聪騻鞑テヅ湓恚绻卣鼽cxi和yi在特征點集中互相對應，則認為是正確的匹配點對，保留該特征點對，否則予以篩除。

(4)余弦相似度提純

針對步驟(3)雙向濾波后仍存在的誤匹配點對，引入余弦相似度的概念進行進一步過濾。兩個特征向量間的余弦值越大，則特征向量相似程度越高。設兩個特征向量X和Y之間夾角為θ，則余弦相似度可以表示為

(1)

式中cosθ的取值范圍為[-1,1]，取值越接近1，這2個特征描述向量的相關性就越高。設定經(jīng)驗閾值Te。計算每一對特征點對應特征向量間的余弦相似度，如果c>Te，則接受該特征點對，否則予以剔除。經(jīng)過該步驟得到精匹配的圖像。

(5)使用RANSAC算法對錯誤匹配點對進行最后一次篩除。

(6)獲得最終匹配完成的特征點對，用于后續(xù)的相機位姿估計。

為驗證該特征點匹配算法的性能，與傳統(tǒng)RANSAC算法進行圖像匹配性能對比。特征點匹配結(jié)果如圖9和表1所示。

圖9 特征點匹配效果對比Fig.9 Comparison of matching effect of feature points

表1中改進RANSAC算法得到的匹配點數(shù)量少于傳統(tǒng)RANSAC算法，說明改進RANSAC算法更有效地剔除了誤匹配點對。在特征點提取過程中，定義特征點的期望個數(shù)為400，剔誤率以原始ORB算法作為基準。改進RANSAC耗時較少，匹配速度相對RANSAC算法有所提升。在其他運行位置，特征點匹配結(jié)果也基本類似。從實驗結(jié)果來看，改進RANSAC算法對于特征點匹配具有更高的實時性和匹配精度。

2.3.3相機位姿估計

使用ICP算法實現(xiàn)點云間的精確配準，得到深度相機的位姿估計。ICP算法的基本思路是在2組點云集合中找出最鄰近點(xi,yi)，采用非線性優(yōu)化的方式，通過位姿估計計算出最優(yōu)匹配參數(shù)R和t，通過不斷優(yōu)化迭代直到誤差小于某一閾值，以此確定最終的位姿估計。其中，相機的位姿通常使用李代數(shù)或四元數(shù)來表達。

從兩組點云集合X″和Y″中取出點xi和yi，使得‖xi-yi‖取值最小。

求解R和t，使誤差函數(shù)E(R,t)值最小。

(2)

采用李代數(shù)表達位姿，則位姿函數(shù)可以表示為

(3)

基于李代數(shù)擾動模型，單個誤差項對位姿的導數(shù)可表示為

(4)

式中 ⊙——算符,定義為歐氏變化SE(3)上的李代數(shù)求導模型[14]

經(jīng)過反復迭代運算，使得誤差函數(shù)收斂到極小值。由文獻[15]可知，當ICP存在唯一解時，該極小值即為全局最優(yōu)解。在某些情況下，某些像素的深度數(shù)據(jù)獲取可能出現(xiàn)缺失，ICP算法的精度難以得到保證。此時需要結(jié)合PnP算法進行3D-2D的點對運動求解，以獲得較好的姿態(tài)估計。

3 多傳感器融合算法和最優(yōu)位姿估計

3.1 自適應無跡卡爾曼濾波

對于一個非線性系統(tǒng)，系統(tǒng)方程可以表示為[16]

(5)

式中k——某一時刻

Xk、Xk+1——k、k+1時刻系統(tǒng)狀態(tài)向量

f——非線性系統(tǒng)狀態(tài)轉(zhuǎn)移函數(shù)

hk——觀測變量到狀態(tài)變量的轉(zhuǎn)換關系

uk——當前時刻的外部指令輸入

Zk+1——k+1時刻系統(tǒng)觀測向量

Wk——n維系統(tǒng)過程噪聲

Vk+1——k+1時刻n維系統(tǒng)觀測噪聲

本文復合導引系統(tǒng)屬于非線性系統(tǒng)，誤差應在一定范圍內(nèi)，故首先使用無跡卡爾曼濾波(Unscented Kalman filter, UKF)來融合多傳感器信息。標準UKF算法的流程[17]如下：

(1)選取k=0時的濾波初值來初始化。狀態(tài)向量的初始協(xié)方差P0和先驗均值0為

(6)

(2)計算UT變換Sigma采樣點和其對應的權重系數(shù)

(7)

式中n——狀態(tài)向量的維數(shù)

ξi,k——Sigma采樣點

Pk——k時刻狀態(tài)向量的協(xié)方差

κ——第三刻度因子

(8)

(3)對Sigma點進行非線性傳播，計算傳播結(jié)果為

ξi,k/k-1=f(ξi,k-1) (i=0,1,…,2n)

(9)

當時間為k時，基于Sigma采樣點的一步預測矩陣和協(xié)方差矩陣為

(10)

(4)進行觀測更新

(11)

(12)

式中Qk、Rk——不相關零均值白噪聲序列Wk和Vk的協(xié)方差矩陣

(5)根據(jù)Zk進行濾波更新。計算最佳濾波增益

(13)

更新系統(tǒng)狀態(tài)向量和方差的預測值

k=k/k-1+Kk(Zk-k/k-1)

(14)

(15)

UKF算法通過對概率密度的近似來處理非線性問題，對系統(tǒng)噪聲的統(tǒng)計信息準確性要求較高[18]。如果系統(tǒng)模型或者噪聲統(tǒng)計特性存在偏差，則可能使系統(tǒng)估計的方差偏大，導致濾波發(fā)散。跟隨AGV在任務執(zhí)行過程中，可能面臨較為復雜的工況環(huán)境，難以獲得準確的噪聲統(tǒng)計特性，影響最終的濾波效果。

針對上述問題，基于自適應濾波原理，提出一種自適應無跡卡爾曼濾波(Adaptive unscented Kalman filter, AUKF)算法，在UKF算法的基礎上加入Sage-Husa噪聲估計器[19]，通過實時調(diào)整系統(tǒng)量測噪聲，提高UKF算法的環(huán)境適應能力。具體實現(xiàn)步驟如下：

(1)計算運動噪聲均值

E(k+1)=(1-dk+1)k+1+

(16)

(17)

式中b——遺忘因子，通常取[0.950,0.995]

f(ξi,k,uk)——Sigma點ξi,k沿非線性函數(shù)f的傳播結(jié)果

(2)計算運動噪聲的協(xié)方差矩陣

(18)

式中ek+1——觀測值的殘差，滿足ek=Zk-k

(3)計算觀測噪聲均值

k+1=E(k+1)=

(19)

(4)求解觀測噪聲的協(xié)方差矩陣

(20)

AUKF濾波流程如圖10所示，對于k時刻的系統(tǒng)噪聲特性估計值{k,k,k,k}，計算得到k時刻下的系統(tǒng)狀態(tài)誤差的協(xié)方差和估計值。將得到的結(jié)果和前一時刻過程噪聲特性的估計值，通過式(16)～(20)，對k時刻噪聲統(tǒng)計特性進行遞推估計。最后更新k并重復該過程。

圖10 AUKF算法流程圖Fig.10 AUKF algorithm process chart

3.2 跟隨AGV最優(yōu)位姿估計方法

使用AUKF算法對雙車協(xié)同系統(tǒng)中跟隨AGV進行最優(yōu)位姿估計，位姿計算流程如圖11所示。AGV車載視覺系統(tǒng)得到的環(huán)境視覺觀測，作為系統(tǒng)觀測量；IMU傳感器觀測值結(jié)合運動學模型作為預測量，推導AGV運行時的位姿狀態(tài)遞推方程，建立預測、觀測反饋閉環(huán)的最優(yōu)位姿估計系統(tǒng)。將最優(yōu)位姿估計作為導引參數(shù)輸入運動控制系統(tǒng)，實現(xiàn)對跟隨AGV的實時魯棒導航。

圖11 基于AUKF融合算法的AGV最優(yōu)位姿估計Fig.11 Optimal pose estimation of AGV based on AUKF fusion algorithm

3.2.1跟隨AGV運動學模型

以麥克納姆輪全向移動AGV作為研究對象，角度偏差eθ和距離偏差ed用于描述小車的運動狀態(tài)，通過分析單個麥克納姆輪及整車運動得到全向AGV的運動學方程。由vx、vy、ωz這3個獨立的運動學參數(shù)可以實現(xiàn)AGV的位姿糾偏，即

(21)

式中ω1、ω2、ω3、ω4——麥克納姆輪旋轉(zhuǎn)角速度

L——1/2車長

W——1/2車寬

R——麥克納姆輪半徑

設全向AGV的控制周期為Ts，某一時刻k全向AGV相對于預設路徑的角度偏差和距離偏差分別為eθ(k)和ed(k)。通過運動學以及路徑運動偏差分析，得到基于路徑跟蹤的雙車協(xié)同搬運系統(tǒng)的運動學模型[20]：

對于領航AGV有

(22)

對于跟隨AGV有

(23)

式中vx1、vx2——領航AGV、跟隨AGV速度在x方向的分量

vy1——領航AGV速度在y方向上的分量

ΔL(k)——k時刻兩車實際中心距與期望距離的差值

3.2.2復合導航系統(tǒng)狀態(tài)方程

根據(jù)慣導傳感器原理，結(jié)合車輛運行狀態(tài)建立狀態(tài)方程

(24)

式中Gd——加速度計計算得到的位移

ΔGd——遞推得到的位移增量

Gα——橫向加速度

Gθ——陀螺儀計算出的AGV姿態(tài)角

ΔGθ——遞推得到的角度增量

Gω——陀螺儀當前角速度

根據(jù)RGB-D視覺導航原理以及針孔相機模型，建立深度相機系統(tǒng)狀態(tài)變量R、t的狀態(tài)方程。由文獻[21]可知，RGB-D深度相機的運動過程，由一個運動方程和一個觀測方程構成，觀測方程即針孔相機模型

(25)

式中xk——深度相機當前時刻的位姿，可以用李代數(shù)表示

yj——標識點位置

zk,j——標識點在圖像中的像素位置

wk——系統(tǒng)過程噪聲

vk,j——系統(tǒng)觀測噪聲

將式(25)轉(zhuǎn)換為具體參數(shù)化形式。對于位姿xk，相機外參數(shù)R、t可以用Tk或者exp(k)來表示。假設在xk處對標識點yj進行一次預測，映射到圖像中的像素位置zk,j，其觀測方程可以描述為

szk,j=Kexp()yj

(26)

式中K——相機內(nèi)參數(shù)

s——像素點深度信息

將yj用齊次坐標描述為

(27)

3.2.3基于AUKF的跟隨AGV最優(yōu)位姿估計

結(jié)合前文跟隨AGV的運動學模型以及傳感器的狀態(tài)方程，可以得到系統(tǒng)狀態(tài)變量的狀態(tài)空間模型。由于復雜外部環(huán)境的影響，AGV在運動過程中存在噪聲干擾，即過程噪聲wk，在模型中以三維齊次坐標形式描述，滿足均值為零的高斯分布wk～N(0,Qk)。慣導短時精度高，本文將慣導傳感器的輸出信號用于跟隨AGV的姿態(tài)預測，系統(tǒng)狀態(tài)預測方程用齊次坐標的矩陣形式描述為

(28)

視覺系統(tǒng)不存在累計誤差，故將其作為觀測修正慣導傳感器產(chǎn)生的累積誤差。AGV路徑跟蹤導航系統(tǒng)采集的跟隨AGV與導引路徑中心線的偏差ed、eθ，以及RGB-D導航系統(tǒng)觀測的位姿信息R、t，作為觀測量，構建系統(tǒng)狀態(tài)觀測方程。觀測過程同樣存在噪聲干擾，即觀測噪聲vk，滿足均值為零的高斯分布vk～N(0,Rk)。用齊次坐標描述為

(29)

其中，ed(k)和eθ(k)的展開式為式(23)。系統(tǒng)過程噪聲wk和系統(tǒng)觀測噪聲vk二者線性無關，且均滿足均值為零的高斯分布wk,vk～N(0,Rk)，其統(tǒng)計特性關系滿足等式

(30)

其中

(31)

式中δkj——Kronecker-δ函數(shù)

4 實驗與結(jié)果分析

4.1 AUKF仿真實驗

針對3.2節(jié)中提出的跟隨AGV最優(yōu)位姿估計方法進行仿真實驗。本文實驗使用的全向移動AGV驅(qū)動輪軸向間距(1/2車長)L=0.5 m，設置車速v=0.5 m/s，CCD相機采樣頻率H1=25幀/s，RealSense D435i型深度相機采樣頻率H2=30幀/s，慣導傳感器采樣頻率H3=50幀/s。AUKF濾波采樣周期Ts=0.04 s。

仿真初始狀態(tài)變量設置為ed(0)=0.6 mm和eθ(0)=0.2°。AUKF算法參數(shù)設置：κ=0.1，α=0.01，β=2；同時設定初始狀態(tài)噪聲Q(0)=diag([0.1,0.02,0.005])，初始觀測噪聲R(0)=diag([0.05,0.1,0.01])。

圖12和圖13分別為距離偏差ed和角度偏差eθ的濾波仿真曲線。在復雜環(huán)境中，距離偏差最大值為16 mm，角度偏差最大值為13°。經(jīng)過AUKF濾波后，距離偏差和角度偏差明顯降低，變化曲線趨于平穩(wěn)。AUKF濾波后偏差的均值和方差均小于UKF。相比于UKF算法，本文AUKF濾波算法對于復雜環(huán)境有更好的適應能力。

圖12 ed濾波曲線Fig.12 Filtering curves of ed

圖13 eθ濾波曲線Fig.13 Filtering curves of eθ

由仿真結(jié)果可知，本文基于AUKF算法的跟隨AGV最優(yōu)位姿估計方法具有良好的性能，能夠在一定程度上抑制復雜環(huán)境的干擾，該方法適用于跟隨AGV的多傳感器信息融合。

4.2 雙車協(xié)同搬運實驗

為了驗證前述復合導引方案以及數(shù)據(jù)融合算法的整體有效性，進行雙車協(xié)同搬運的對比實驗驗證。實驗環(huán)境整體布局如圖14所示，實驗平臺為2臺麥克納姆輪全向移動AGV組成的雙車協(xié)同搬運系統(tǒng)。如圖15所示，領航AGV在前，跟隨AGV在后，共同完成較長物件的搬運任務。運行過程中，跟隨AGV在導引線劃定的區(qū)域范圍內(nèi)運行，兩側(cè)的CCD相機通過實時采集導引路徑圖像，防止AGV出界。跟隨AGV車頭安裝RGB-D相機觀測領航AGV，用于獲取相對前車的位姿信息。

圖14 實驗環(huán)境布局圖Fig.14 Layout of experimental environment

圖15 雙車協(xié)同搬運實物Fig.15 Two AGVs cooperative handling

設置AGV初始狀態(tài)及狀態(tài)變量如下：跟隨AGV初始距離偏差ed(0)=-20 mm，初始角度偏差eθ(0)=-4°。領航AGV和跟隨AGV的初始運行速度為0.4 m/s，兩車初始距離誤差ΔL(0)=50 mm。在k=0時刻同時啟動領航AGV和跟隨AGV，在執(zhí)行雙車搬運任務的同時，通過跟隨AGV車載裝配平臺上的角度傳感器和直線距離傳感器實時采集兩車間的距離偏差和角度偏差。設置一組對照組，將本文提出的整套復合導引方案與僅使用單一路徑跟蹤導航進行導引性能的對比，并截取中間一段具有代表性的路徑偏差數(shù)據(jù)進行分析。

圖16為跟隨AGV運行時距離偏差ed和角度偏差eθ的變化曲線。其中，在0～4 s時間段內(nèi)跟隨AGV在直線路段上運行，第5秒開始進入圓弧段，第20秒駛出圓弧段回到直線路段。對于初始偏差，圖16a中實線在1 s內(nèi)下降到-3 mm左右，相比虛線收斂速度更快，說明本文復合導引糾偏更及時。直線路段實線的距離偏差穩(wěn)定在10 mm以內(nèi)，圓弧路段距離偏差在15 mm內(nèi)，距離偏差均值較小。圖16b中實線的角度偏差始終小于虛線，其中直線路段角度偏差最大為8°，圓弧路段最大為19°，偏差變化過程較為平滑。說明本文復合導引方案在圓弧路段運行較為平穩(wěn)，未出現(xiàn)位姿突變。

圖16 跟隨AGV距離偏差和角度偏差變化曲線Fig.16 Distance deviation and angle deviation of following AGV

圖17為運行過程中前后兩車距離偏差ΔL的變化曲線。實線在2 s內(nèi)收斂至0 mm左右，后續(xù)距離偏差保持在5 mm內(nèi)；而虛線收斂速度慢，且上下波動相對較大。由圖17可知，在雙車隊形保持方面，本文復合導引方案的性能同樣優(yōu)于單一的路徑跟蹤導航。

圖17 雙車距離偏差變化曲線Fig.17 Variation of distance deviation between two AGVs

實驗結(jié)果表明，本文提出的復合導航方案具有更快的偏差收斂速度、更穩(wěn)定的路徑跟蹤狀態(tài)和隊形保持。實驗證明該復合導航方案具有良好的導引精度，能夠提高本文雙車協(xié)同搬運系統(tǒng)的實時性和魯棒性。

5 結(jié)束語

針對Leader-Follower編隊策略下的多AGV協(xié)同搬運，研究了面向跟隨AGV定位和導引的慣導/視覺組合導航方法；對于多目視覺系統(tǒng)和慣性導引的數(shù)據(jù)融合問題，提出了一種最優(yōu)位姿估計方法，將慣導觀測數(shù)據(jù)、路徑跟蹤誤差以及RGB-D位姿觀測作為聯(lián)合觀測數(shù)據(jù)，與跟隨AGV的運動模型結(jié)合，進行多傳感器的數(shù)據(jù)融合。實驗表明本文提出的復合導引方案具有良好的導引精度，能夠適應雙車協(xié)同搬運系統(tǒng)實時性、準確性和魯棒性的要求。