全方位自動(dòng)導(dǎo)引車的導(dǎo)航與控制系統(tǒng)研究

韓 松，劉曉平，趙云龍，王 剛

(北京郵電大學(xué)， 北京 100876)

引 言

隨著電子商務(wù)的蓬勃發(fā)展以及人力成本的不斷上升，越來越多的電子商務(wù)、物流公司建造了自動(dòng)化的物流倉儲中心，自動(dòng)化和無人化的程度越來越高。目前，越來越多的物流倉儲中心在運(yùn)輸、分揀等環(huán)節(jié)均使用自動(dòng)導(dǎo)引車(AGV)來取代人力，減少人力成本，提高工作效率。其中，基于麥克納姆輪[1]的全方位AGV可以實(shí)現(xiàn)前后、左右和自轉(zhuǎn)3個(gè)自由度的運(yùn)動(dòng)。良好的靈活性使得全方位AGV的控制方式更加多樣，對不同工作環(huán)境和復(fù)雜任務(wù)的適應(yīng)能力更強(qiáng)，有很廣闊的應(yīng)用前景。本文以全方位AGV為研究對象，對其導(dǎo)航和控制系統(tǒng)展開了研究。

為了提高AGV運(yùn)行路徑的靈活性和多樣性，越來越多的工廠摒棄傳統(tǒng)的磁條導(dǎo)引方式，采用無軌的導(dǎo)航方式。同時(shí)，國內(nèi)外學(xué)者對不同原理的無軌導(dǎo)航方式進(jìn)行了深入研究，并取得了良好的應(yīng)用效果[2-6]。這些方式大致可以分為兩類：基于信標(biāo)的無軌導(dǎo)航和基于航跡推算的無軌導(dǎo)航。其中，基于信標(biāo)的導(dǎo)航如激光導(dǎo)航、視覺導(dǎo)航等，局部定位精準(zhǔn)，但存在實(shí)時(shí)性和環(huán)境適應(yīng)性不夠及易受光線、粉塵因素影響等問題；基于航跡推算的導(dǎo)航多使用IMU(慣性測量單元)或基于編碼器的里程計(jì)系統(tǒng)定位，無需依賴外部環(huán)境，但是會(huì)隨時(shí)間累積測量誤差，無法長期單獨(dú)使用。針對上述問題，本文結(jié)合使用兩類定位方法，使用多傳感器融合算法彌補(bǔ)兩者各自缺點(diǎn)，提高定位精度。

此外，針對全方位AGV的運(yùn)動(dòng)模型，本文提出了一種使用PID控制進(jìn)行路徑跟蹤的算法，并通過實(shí)驗(yàn)驗(yàn)證了算法的可靠性。

1 運(yùn)動(dòng)學(xué)分析

基于麥克納姆輪的全方位AGV多采用四輪對稱布置，如圖1所示。

圖1 全方位AGV輪系結(jié)構(gòu)簡圖

麥克納姆輪輪轂軸線和輥?zhàn)虞S線夾角為α(本文中α=45°)。點(diǎn)or為車體的中心，并以此為原點(diǎn)建立車載坐標(biāo)系Σor。AGV在坐標(biāo)系Σor的位姿為Pr=[xryrθr]T，其運(yùn)動(dòng)學(xué)公式可以表示為：

(1)

(2)

如圖1所示，AGV在世界坐標(biāo)系Σoq的速度可以通過下式進(jìn)行轉(zhuǎn)換：

(3)

式中：R(θ)為坐標(biāo)系Σor到坐標(biāo)系Σoq的坐標(biāo)變換：

(4)

在車載坐標(biāo)系下，由式(1)可以求得移動(dòng)機(jī)器人的逆運(yùn)動(dòng)學(xué)公式：

(5)

2 導(dǎo)航系統(tǒng)設(shè)計(jì)

2.1 多傳感器融合定位

(6)

然而，單獨(dú)使用基于編碼器的里程計(jì)進(jìn)行定位時(shí)，由于AGV機(jī)械加工過程中的固有缺陷，或受路面打滑等外界因素影響，產(chǎn)生較大的瞬時(shí)偏差，且這些偏差會(huì)隨著式(6)的累積求和不斷累加。以此坐標(biāo)作為定位信息進(jìn)行導(dǎo)航，容易造成車體向某一方向偏移。所以，需要使用其他可以感知外界環(huán)境的傳感器對上述定位結(jié)果進(jìn)行校正。

圖2 紅外圖像傳感器實(shí)物及工作原理

(7)

上述紅外定位的方式可以用來消除編碼器進(jìn)行航跡推算造成的誤差?？紤]到StarGazer的定位可能受到其他紅外設(shè)備的干擾，如室內(nèi)的紅外監(jiān)控器，造成檢測的錯(cuò)誤，所以不能直接使用StarGazer的定位結(jié)果取代編碼器航跡推算出的位姿。需要采用具有較強(qiáng)魯棒性的多傳感器融合算法，對二者數(shù)據(jù)進(jìn)行濾波，得到局部校正的結(jié)果。本文使用卡爾曼濾波[7-8]作為融合兩種傳感器的方法，濾波方法表述如下。

系統(tǒng)的狀態(tài)方程和觀測方程為

(8)

式中：A為狀態(tài)轉(zhuǎn)移矩陣；B為控制矩陣；H為觀測矩陣；wk-1是系統(tǒng)噪聲；vk是觀測噪聲。狀態(tài)變量選取為：

X=[xyθ]T

(9)

輸入變量選取為

u=[vxvyωz]T

(10)

輸入變量由里程計(jì)系統(tǒng)確定。第k時(shí)刻對k+1時(shí)刻的預(yù)測值表示為：

(11)

其中，

A=I3×3

(12)

控制矩陣B定義為：

B=ToI3×3

(13)

(14)

式中：Zk+1是k+1時(shí)刻的實(shí)際測量值，測量值選用StarGazer的測量值[x(s)y(s)θ(s)]T；觀測矩陣Hk+1取為單位陣I3×3；Kk+1是k+1時(shí)刻的卡爾曼增益，通過式(15)得到，其中的P(k+1|k)是k時(shí)刻對k+1時(shí)刻的預(yù)測誤差協(xié)方差矩陣。

(15)

P(k+1|k)=APkAT+Q

(16)

式中：Q為過程激勵(lì)噪聲協(xié)方差矩陣，R為觀測噪聲協(xié)方差矩陣，均為對角陣，分別由編碼器和StarGazer的測量誤差確定；Pk為k時(shí)刻狀態(tài)變量的誤差協(xié)方差矩陣。最后，k+1時(shí)刻的誤差協(xié)方差矩陣通過式(17)更新。

Pk+1=(I-Kk+1Hk+1)P(k+1|k)

(17)

經(jīng)過對里程計(jì)航跡推算結(jié)果進(jìn)行線性預(yù)測，對齊StarGazer的測量周期Ts，系統(tǒng)每隔Ts使用卡爾曼濾波進(jìn)行一次傳感器融合，其余時(shí)刻使用里程計(jì)系統(tǒng)以上一次的融合結(jié)果為新的初始位姿，單獨(dú)進(jìn)行航跡推算，得到每一時(shí)刻AGV的定位[xy]T和偏航角θ。

經(jīng)過多傳感器融合，不僅可以消除里程計(jì)系統(tǒng)的累積誤差，還提高了單獨(dú)使用StarGazer時(shí)的定位頻率。在保證實(shí)時(shí)性的前提下，減小了外界環(huán)境對定位系統(tǒng)的影響，提高了定位系統(tǒng)的魯棒性和穩(wěn)定性。

2.2 路徑跟蹤

AGV通過定位和融合算法得到自身在世界坐標(biāo)系下的實(shí)際位姿[xyθ]T，取距離AGV幾何中心最近的可微點(diǎn)[xpyp]T作為目標(biāo)點(diǎn)，如圖3所示。圖中es、ed和eθ分別表示實(shí)際位姿與期望位姿的前向偏差、橫向偏差和角度偏差。

圖3 路徑跟蹤示意圖

(18)

則根據(jù)式(5)可以得到4個(gè)麥克納姆輪的速度為：

(19)

3 實(shí)驗(yàn)與分析

本文全方位AGV的機(jī)械設(shè)計(jì)圖和實(shí)物圖如圖4所示。

圖4 全方位AGV機(jī)械設(shè)計(jì)圖與實(shí)物圖

AGV控制系統(tǒng)結(jié)構(gòu)組成如圖5所示，選用固高嵌入式運(yùn)動(dòng)控制器作為核心控制器；使用編碼器反饋數(shù)據(jù)和StarGazer紅外定位傳感器采集到的信息進(jìn)行傳感器融合實(shí)現(xiàn)定位和導(dǎo)航；避障傳感器用于應(yīng)對突發(fā)情況，保證運(yùn)行安全；無線手柄用于對AGV進(jìn)行手動(dòng)遙控，方便調(diào)試和進(jìn)行緊急控制。

圖5 AGV控制系統(tǒng)結(jié)構(gòu)圖

使用本文中的卡爾曼濾波對StarGazer和里程計(jì)系統(tǒng)的數(shù)據(jù)進(jìn)行融合，使用路徑跟蹤算法對6 m × 4 m的圓角矩形軌跡進(jìn)行跟蹤。實(shí)驗(yàn)結(jié)果如圖6所示，單獨(dú)使用里程計(jì)系統(tǒng)由于定位累積誤差過大，無法完成跟蹤。圖6(a)的點(diǎn)集為進(jìn)行濾波前單獨(dú)使用StarGazer進(jìn)行跟蹤的結(jié)果，可以看出跟蹤效果精度較低，且出現(xiàn)很多間斷點(diǎn)和突變點(diǎn)，這是由于紅外干擾造成的檢測失效；圖6(b)的點(diǎn)集為進(jìn)行數(shù)據(jù)融合后得到的結(jié)果，軌跡平滑連續(xù)，更加接近期望的軌跡。

圖6 卡爾曼濾波前后路徑跟蹤效果對比

經(jīng)過測試，在0.3 m/s的速度下，AGV自動(dòng)導(dǎo)航的行駛精度可以達(dá)到10 mm，且沒有位姿檢測突變點(diǎn)。最后，對上述算法的運(yùn)行效率進(jìn)行了測試，系統(tǒng)進(jìn)行定位和路徑跟蹤的頻率可以達(dá)到15 Hz，滿足物流倉儲對AGV導(dǎo)航精度和實(shí)時(shí)性的需求。

4 結(jié)束語

本文使用卡爾曼濾波對兩種不同機(jī)理的傳感器進(jìn)行了數(shù)據(jù)融合，提出并驗(yàn)證了基于PID控制的全方位AGV的路徑跟蹤算法。實(shí)驗(yàn)證明，融合算法彌補(bǔ)了兩種傳感器的缺陷，提高了定位精度，濾波器模型可拓展應(yīng)用于其他種類的定位傳感器的融合；路徑跟蹤算法可靠、實(shí)時(shí)，具有很高的行駛精度和穩(wěn)定性，達(dá)到了實(shí)際應(yīng)用的要求，離散PID控制器適用于其他結(jié)構(gòu)的全方位AGV的導(dǎo)航控制。

另一方面，由于本文采用原始的卡爾曼濾波模型，對環(huán)境中噪聲變化的適應(yīng)能力不夠。所以，在后續(xù)工作中，將對本文的卡爾曼濾波模型進(jìn)行改進(jìn)和優(yōu)化，提出一種自適應(yīng)的融合定位算法，實(shí)時(shí)調(diào)整模型的激勵(lì)噪聲協(xié)方差矩陣和觀測噪聲協(xié)方差矩陣，進(jìn)一步提高全方位AGV的定位與導(dǎo)航精度。