基于多傳感器融合的跟隨AGV復(fù)合導(dǎo)引技術(shù)

錢曉明 黃宇軒 樓佩煌 孫 天

(南京航空航天大學(xué)機(jī)電學(xué)院， 南京 210016)

0 引言

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

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

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

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

1 復(fù)合導(dǎo)引方案設(shè)計(jì)

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

圖1 跟隨AGV復(fù)合導(dǎo)引方案Fig.1 Compound navigation scheme of following AGV

2 跟隨AGV多目視覺(jué)導(dǎo)航方法

2.1 多目視覺(jué)導(dǎo)航技術(shù)框架

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

圖2 多目視覺(jué)導(dǎo)航技術(shù)框架Fig.2 Framework of multi-vision navigation technology

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

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

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

(2)RGB-D視覺(jué)導(dǎo)航方法。RGB-D深度相機(jī)實(shí)時(shí)采集領(lǐng)航AGV的外殼輪廓圖像，并匹配相鄰兩幅圖像的特征點(diǎn)，通過(guò)ICP算法求取跟隨AGV的相對(duì)位姿。

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

2.2 路徑跟蹤導(dǎo)航

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

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

圖4 導(dǎo)引路徑識(shí)別圖像Fig.4 Recognition images of navigation path

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

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

2.3 RGB-D立體視覺(jué)導(dǎo)航

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

圖6 RGB-D導(dǎo)航位姿測(cè)量流程Fig.6 RGB-D navigation measurement process

2.3.1圖像預(yù)處理和特征點(diǎn)檢測(cè)

RGB立體視覺(jué)導(dǎo)航方法用于觀測(cè)領(lǐng)航AGV，首先進(jìn)行觀測(cè)圖像預(yù)處理。使用文獻(xiàn)[12]中的最佳閾值分割方法來(lái)進(jìn)行圖像閾值的選取，通過(guò)閾值分割和平面提取算法分割出領(lǐng)航AGV的外殼輪廓。圖像分割結(jié)果如圖7所示。

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

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

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

2.3.2改進(jìn)RANSAC特征點(diǎn)匹配

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

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

(1)特征點(diǎn)初始化

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

(2)特征點(diǎn)粗匹配

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

(3)雙向交叉濾波篩選

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

(4)余弦相似度提純

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

(1)

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

(5)使用RANSAC算法對(duì)錯(cuò)誤匹配點(diǎn)對(duì)進(jìn)行最后一次篩除。

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

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

圖9 特征點(diǎn)匹配效果對(duì)比Fig.9 Comparison of matching effect of feature points

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

2.3.3相機(jī)位姿估計(jì)

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

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

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

(2)

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

(3)

基于李代數(shù)擾動(dòng)模型，單個(gè)誤差項(xiàng)對(duì)位姿的導(dǎo)數(shù)可表示為

(4)

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

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

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

3.1 自適應(yīng)無(wú)跡卡爾曼濾波

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

(5)

式中k——某一時(shí)刻

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

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

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

uk——當(dāng)前時(shí)刻的外部指令輸入

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

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

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

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

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

(6)

(2)計(jì)算UT變換Sigma采樣點(diǎn)和其對(duì)應(yīng)的權(quán)重系數(shù)

(7)

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

ξi,k——Sigma采樣點(diǎn)

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

κ——第三刻度因子

(8)

(3)對(duì)Sigma點(diǎn)進(jìn)行非線性傳播，計(jì)算傳播結(jié)果為

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

(9)

當(dāng)時(shí)間為k時(shí)，基于Sigma采樣點(diǎn)的一步預(yù)測(cè)矩陣和協(xié)方差矩陣為

(10)

(4)進(jìn)行觀測(cè)更新

(11)

(12)

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

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

(13)

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

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

(14)

(15)

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

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

(1)計(jì)算運(yùn)動(dòng)噪聲均值

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

(16)

(17)

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

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

(2)計(jì)算運(yùn)動(dòng)噪聲的協(xié)方差矩陣

(18)

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

(3)計(jì)算觀測(cè)噪聲均值

k+1=E(k+1)=

(19)

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

(20)

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

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

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

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

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

3.2.1跟隨AGV運(yùn)動(dòng)學(xué)模型

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

(21)

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

L——1/2車長(zhǎng)

W——1/2車寬

R——麥克納姆輪半徑

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

對(duì)于領(lǐng)航AGV有

(22)

對(duì)于跟隨AGV有

(23)

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

vy1——領(lǐng)航AGV速度在y方向上的分量

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

3.2.2復(fù)合導(dǎo)航系統(tǒng)狀態(tài)方程

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

(24)

式中Gd——加速度計(jì)計(jì)算得到的位移

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

Gα——橫向加速度

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

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

Gω——陀螺儀當(dāng)前角速度

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

(25)

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

yj——標(biāo)識(shí)點(diǎn)位置

zk,j——標(biāo)識(shí)點(diǎn)在圖像中的像素位置

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

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

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

szk,j=Kexp()yj

(26)

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

s——像素點(diǎn)深度信息

將yj用齊次坐標(biāo)描述為

(27)

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

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

(28)

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

(29)

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

(30)

其中

(31)

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

4 實(shí)驗(yàn)與結(jié)果分析

4.1 AUKF仿真實(shí)驗(yàn)

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

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

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

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

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

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

4.2 雙車協(xié)同搬運(yùn)實(shí)驗(yàn)

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

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

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

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

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

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

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

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

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

5 結(jié)束語(yǔ)

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