基于自適應(yīng)聯(lián)邦濾波的AGV定位研究

溫尊旺，王堯堯，陳柏，姚佳烽

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

0 引言

隨著“工業(yè)4.0”、“中國制造2025”等戰(zhàn)略的提出，傳統(tǒng)工業(yè)逐漸向自動(dòng)化及智能化的方向發(fā)展。由于自動(dòng)引導(dǎo)運(yùn)輸車(AGV)智能化高，靈活性強(qiáng)的特點(diǎn)，廣泛應(yīng)用于智能車間、倉儲(chǔ)運(yùn)輸?shù)阮I(lǐng)域[1]。常見的AGV室內(nèi)定位方式主要有WiFi定位[2]、激光雷達(dá)、UWB定位[3]、視覺[4]、動(dòng)作捕捉、電磁導(dǎo)航等。這些定位方式各有特點(diǎn)，在不同場合均有使用。

為了進(jìn)一步提高定位精度，便有了多傳感器的組合導(dǎo)航，即采用兩種或兩種以上的非相似性導(dǎo)航系統(tǒng)對(duì)同一導(dǎo)航信息做測量并解算以形成量測量[5]。COONEY J A等[6]介紹了麥克納姆輪(Mecanum wheel)中的打滑問題，使用視覺航位推算來處理和計(jì)算車輛的速度及方向，以此來完成防滑的功能。QIN H L等[7]提出了基于小波閾值去噪和函數(shù)系數(shù)回歸(FAR)建模的慣性組合數(shù)據(jù)處理方法，引入GPS姿態(tài)信息，提高了MEMS慣性傳感器誤差的估計(jì)精度。孫自飛等[8]提出一種RGB-D傳感器對(duì)激光傳感器進(jìn)行輔助的自定位方法，具有較好的準(zhǔn)確性和可靠性。李沛等[9]提出了一種基于視覺定位的AGV避障方案，將超聲波傳感器和紅外傳感器相結(jié)合，基于多傳感器信息融合技術(shù)，實(shí)現(xiàn)避障算法。

本文基于Mecanum輪全向機(jī)器人設(shè)計(jì)了一種多傳感器融合的低成本定位方案。結(jié)合各傳感器的特點(diǎn)，采用自適應(yīng)聯(lián)邦卡爾曼濾波及多尺度融合的手段，提高了定位精度。

1 Mecanum輪全向機(jī)器人運(yùn)動(dòng)學(xué)分析

所設(shè)計(jì)的Mecanum輪全向機(jī)器人系統(tǒng)包含動(dòng)力系統(tǒng)、傳感器系統(tǒng)以及控制系統(tǒng)。動(dòng)力系統(tǒng)包括電機(jī)驅(qū)動(dòng)器、步進(jìn)電機(jī)以及麥克納姆輪；傳感器系統(tǒng)包括編碼器、加速度計(jì)、陀螺儀以及OpenMV，所用的傳感器成本都較低?？刂葡到y(tǒng)由STM32 F103組成，主要用于傳感器數(shù)據(jù)讀取、解算。

Mecanum輪全向機(jī)器人的安裝方式主要為X型和O型，本文采用X型方式，如圖1所示[10]。R為Mecanum輪半徑；a、b分別為平臺(tái)寬度、長度的一半；vx、vy、ωz分別為平臺(tái)的x方向速度、y方向的速度以及角速度；v1、v2、v3、v4分別為4個(gè)麥克納姆輪的分速度；α為輪轂軸和輥?zhàn)虞S的夾角。

圖1 Mecanum輪全向機(jī)器人結(jié)構(gòu)圖

Mecanum輪全向機(jī)器人的運(yùn)動(dòng)可以分解為3個(gè)獨(dú)立的變量，分別為：x軸的平移、y軸的平移以及繞z軸的旋轉(zhuǎn)。如圖1所示建立坐標(biāo)系，假設(shè)小車的運(yùn)動(dòng)速度為V=[vx,vy,ωz]，對(duì)小車的運(yùn)動(dòng)速度進(jìn)行運(yùn)動(dòng)分解，根據(jù)疊加原理，各個(gè)輪子的角速度為

(1)

式中：kx在理想狀態(tài)=1/R；ky=1/(Rtanα)；kz=(atanα+b)/(Rtanα)。

2 傳感器定位原理

為了獲取準(zhǔn)確的位姿，采用多種傳感器實(shí)時(shí)獲得與位姿相關(guān)的信息，包括：OpenMV單目相機(jī)通過AprilTag標(biāo)簽獲得x、y方向的位置、z軸的姿態(tài)；通過4個(gè)電機(jī)的編碼器獲得當(dāng)前的速度和角速度；陀螺儀得到z軸角度及角速度；MPU6050獲得x、y方向的加速度。

2.1 坐標(biāo)系規(guī)定及坐標(biāo)變換

為了方便后續(xù)的計(jì)算，對(duì)各個(gè)傳感器的坐標(biāo)系進(jìn)行規(guī)定，均采用右手定則，如圖2所示。其中包括世界坐標(biāo)系、AprilTag坐標(biāo)系、OpenMv(單目相機(jī))坐標(biāo)系、AGV坐標(biāo)系。相對(duì)于世界坐標(biāo)系，AprilTag坐標(biāo)系是不變的，其余坐標(biāo)系都隨著AGV坐標(biāo)系而變化，假設(shè)初始位置AGV坐標(biāo)系和世界坐標(biāo)系重合。

圖2 坐標(biāo)系約定

(2)

2.2 AprilTag定位原理

AprilTag是一個(gè)二維平面目標(biāo)基準(zhǔn)系統(tǒng)，被廣泛應(yīng)用于視覺定位，當(dāng)標(biāo)簽貼在目標(biāo)物上時(shí)，便可識(shí)別出相機(jī)和標(biāo)簽之間的3D位姿關(guān)系[11]。為了保證在AGV運(yùn)動(dòng)過程中AprilTag信息不丟失，擴(kuò)大識(shí)別范圍，在天花板上貼4塊ID不一樣的標(biāo)簽。以天花板某一點(diǎn)O為原點(diǎn)，創(chuàng)建AprilTag坐標(biāo)系，對(duì)稱分布4塊標(biāo)簽。其中AprilTag坐標(biāo)系可以由世界坐標(biāo)系繞x軸旋轉(zhuǎn)180°后再平移得到，可以計(jì)算出旋轉(zhuǎn)矩陣和平移向量為

(3)

式中：Cθ=cosθ；Sθ=sinθ；φ、θ、ψ分別為繞x軸、y軸、z軸的旋轉(zhuǎn)角度；t1、t2、t3為水平偏移。

齊次變換矩陣為

(4)

在AGV運(yùn)動(dòng)過程中，若識(shí)別到多個(gè)標(biāo)簽時(shí)，為了提高識(shí)別精度，取最近的標(biāo)簽識(shí)別值為測量值。

2.3 里程計(jì)定位原理

4個(gè)麥克納姆輪的電機(jī)上，均安裝編碼器，在AGV運(yùn)動(dòng)時(shí)，讀取每個(gè)電機(jī)編碼器的數(shù)值，根據(jù)式(1)計(jì)算出AGV當(dāng)前的速度和角速度，通過航跡推算的方法來獲得當(dāng)前的位姿，若采樣間隔為Δt，計(jì)算公式如下[12]：

(5)

從公式(5)可以看出，航跡推算的方法是非線性的，將其轉(zhuǎn)化到世界坐標(biāo)系下進(jìn)行線性化。AGV在世界坐標(biāo)系下的速度為

(6)

其中，速度和角速度可以根據(jù)式(1)計(jì)算，而式(6)也可以寫成

(7)

2.4 加速度計(jì)定位原理

加速度計(jì)可以獲得車體x、y、z方向的加速度，在靜止?fàn)顟B(tài)下，x、y方向的加速度為0，z方向的加速度值為-g。為了減小加速度的誤差，需要對(duì)加速度計(jì)進(jìn)行校準(zhǔn)和補(bǔ)償。當(dāng)加速度計(jì)靜止時(shí)，誤差為當(dāng)前的測量值，表示為

(8)

校準(zhǔn)后的實(shí)際加速度值為

(9)

將所測量的加速度值轉(zhuǎn)換到世界坐標(biāo)系下，計(jì)算為

(10)

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

AGV的定位包括x、y方向的平移和z軸的旋轉(zhuǎn)。單一的傳感器難以獲得較準(zhǔn)確的位姿信息，單目相機(jī)獲得位姿存在較大誤差，且更新頻率較慢，而通過航跡推算計(jì)算的位姿存在明顯的累計(jì)誤差。因此，把第2節(jié)介紹的多種傳感系統(tǒng)進(jìn)行組合，充分利用各導(dǎo)航的信息進(jìn)行互補(bǔ)，最終使整個(gè)系統(tǒng)的精度和可靠性得到提高。

3.1 整體融合方案

整個(gè)系統(tǒng)的狀態(tài)變量包括[xvxaxyvyayψωz]，如果將所有狀態(tài)變量一起計(jì)算，每次進(jìn)行矩陣運(yùn)算次數(shù)是83，極大地增加了計(jì)算量。為了降低計(jì)算量，將狀態(tài)變量分為[xvxax]、[yvyay]、[ψωz]單獨(dú)估計(jì)。其中[ψωz]的估計(jì)采用陀螺儀和AprilTag標(biāo)簽偏航角的測量值進(jìn)行融合。[xvxax]、[yvyay]的估計(jì)采用單目相機(jī)、里程計(jì)、加速度計(jì)，x、y方向的數(shù)據(jù)進(jìn)行卡爾曼濾波融合。

根據(jù)傳感器的特點(diǎn)，陀螺儀在一定時(shí)間內(nèi)可具有較高的精度，但長時(shí)間存在積分誤差，而單目相機(jī)在AGV速度較慢或者停止時(shí)具有較好的精度。因此，每次運(yùn)動(dòng)到任務(wù)點(diǎn)時(shí)，AGV速度較慢，此時(shí)用單目相機(jī)去校準(zhǔn)陀螺儀。通過這樣的融合，[ψωz]具有較好的全局精度。

3.2 經(jīng)典卡爾曼濾波

以x方向?yàn)槔?，?duì)單目相機(jī)和加速度計(jì)的數(shù)據(jù)進(jìn)行卡爾曼濾波，估計(jì)最優(yōu)的系統(tǒng)狀態(tài)，假設(shè)系統(tǒng)的當(dāng)前狀態(tài)為x1,t，包括了位移、速度、加速度3個(gè)狀態(tài)變量，x1,t-1為上一時(shí)刻狀態(tài)，系統(tǒng)的狀態(tài)方程和量測方程為

(11)

式中：F為上一時(shí)刻到下一時(shí)刻的變換矩陣；B為外部輸入u1,t的輸入控制矩陣；w1,t為系統(tǒng)的過程噪聲；H1是系統(tǒng)的觀測矩陣；v1,t為觀測噪聲；zx1,t為單目相機(jī)獲得的x方向的位移量。其中：

(12)

卡爾曼濾波的步驟包括兩步：預(yù)測和更新。預(yù)測過程計(jì)算如下[13]：

(13)

更新過程計(jì)算如下：

(14)

式中：Kx1,t為最優(yōu)卡爾曼增益；Rx1為量測噪聲；Px1,t為當(dāng)前時(shí)刻協(xié)方差矩陣；I為單位陣。

3.3 局部濾波器的自適應(yīng)卡爾曼濾波

根據(jù)卡爾曼濾波計(jì)算過程可知，只有當(dāng)動(dòng)態(tài)系統(tǒng)的參數(shù)和噪聲特性參數(shù)都準(zhǔn)確已知的條件下，經(jīng)典卡爾曼濾波才能得到最優(yōu)估計(jì)。然而，在實(shí)際系統(tǒng)中，一些參數(shù)無法獲取或是實(shí)時(shí)變化的，可能導(dǎo)致濾波器的精度降低，甚至可能出現(xiàn)估計(jì)值和實(shí)際值的誤差越來越大，從而引起濾波發(fā)散。為了消除這種誤差，通過自適應(yīng)濾波的方法，采用噪聲估計(jì)器，實(shí)時(shí)修正和估計(jì)觀測噪聲的特性，從而達(dá)到提高濾波精度的目的。

在卡爾曼濾波中，局部濾波器的量測預(yù)測誤差的公式計(jì)算為

(15)

量測噪聲方向的更新過程為

(16)

當(dāng)t趨于無窮大時(shí)，1/t趨于0，此時(shí)濾波器的自適應(yīng)能力將逐漸減弱。為了始終保持自適應(yīng)能力，將1/t替換為

(17)

當(dāng)t趨于無窮大時(shí)，γt趨于1-c，始終保持一定的自適應(yīng)能力，其中c取0.999。

3.4 多尺度融合及容錯(cuò)設(shè)計(jì)

在實(shí)際系統(tǒng)中，各個(gè)傳感器的更新頻率并不相同。如果卡爾曼濾波器的更新頻率較大，單目相機(jī)的測量值有著嚴(yán)重的滯后；如果更新頻率較小，則不能保證位置估計(jì)的實(shí)時(shí)性。因此，本文提出一種多尺度融合方法，兼有一定的容錯(cuò)能力。

里程計(jì)更新頻率較快，以里程計(jì)的更新頻率作為基準(zhǔn)，通過串口中斷檢測單目相機(jī)是否更新。若更新，則按照式(13)進(jìn)行計(jì)算；若不發(fā)生更新，則認(rèn)為測量值不準(zhǔn)確，完全信任預(yù)測值。整個(gè)過程可以表示為：

(18)

式中：當(dāng)單目相機(jī)更新時(shí)δ(t)=1；沒有更新時(shí)δ(t)=0。

通過上述方法，不僅實(shí)現(xiàn)了非均勻采樣的多尺度系統(tǒng)的融合，也具有一定的容錯(cuò)能力，當(dāng)標(biāo)簽被遮擋或者AGV行駛范圍超出標(biāo)簽范圍而無法識(shí)別時(shí)，卡爾曼濾波仍然能夠起作用。當(dāng)單目相機(jī)工作正常時(shí)，狀態(tài)變量的精度會(huì)逐漸恢復(fù)到正常值。

3.5 聯(lián)邦卡爾曼濾波

多傳感器數(shù)據(jù)融合包括集中式和分散式濾波。集中式濾波即將所有的傳感器數(shù)據(jù)放到同一個(gè)濾波器中進(jìn)行濾波，分散濾波即利用子濾波器處理各系統(tǒng)的信息再經(jīng)過全局濾波實(shí)現(xiàn)融合。集中式濾波雖然精度較好，但由于狀態(tài)維數(shù)高，導(dǎo)致計(jì)算量較大，容錯(cuò)性低，且不便于故障排除。因此，本文采用聯(lián)邦濾波器，雖然輸出結(jié)果是次優(yōu)的，但容錯(cuò)性得到極大的提升，計(jì)算量顯著減少。

所設(shè)計(jì)的整體濾波器結(jié)構(gòu)如圖3所示，加速度計(jì)數(shù)據(jù)為參考系統(tǒng)，里程計(jì)和視覺定位數(shù)據(jù)為子系統(tǒng)，經(jīng)自適應(yīng)局部濾波器1、濾波器2后，進(jìn)入主濾波器，最終得到系統(tǒng)的狀態(tài)變量。

圖3 濾波器結(jié)構(gòu)圖

將局部濾波器的信息融合為新的全局狀態(tài)估計(jì)的過程為

(19)

得到全局估計(jì)后，再經(jīng)反饋得到子濾波器的狀態(tài)、協(xié)方差矩陣：

(20)

式中β1、β2為各子系統(tǒng)分配系數(shù)，這里均取為0.5。

4 仿真和實(shí)驗(yàn)

4.1 仿真與數(shù)據(jù)分析

根據(jù)上一節(jié)設(shè)計(jì)的自適應(yīng)聯(lián)邦卡爾曼濾波器，進(jìn)行仿真驗(yàn)證。假設(shè)載體初始狀態(tài)為x=[0 m 1 m/s 0 m/s2]，保持1 m/s的勻速運(yùn)動(dòng)，0.1 s更新一次。假定Openmv的方均根誤差為1 m，里程計(jì)方均根誤差為0.5 m/s，陀螺儀方均根誤差為0.5 m/s2，經(jīng)濾波后偏航角方均根誤差為0.5°。Openmv未濾波前的誤差、聯(lián)邦卡爾曼濾波、自適應(yīng)聯(lián)邦卡爾曼濾波后誤差的仿真結(jié)果如圖4所示，并計(jì)算得到未濾波前的方均根誤差約為1 m，聯(lián)邦濾波后的方均根誤差約為0.49 m，自適應(yīng)聯(lián)邦濾波后的方均根誤差約為0.35 m。從仿真圖和統(tǒng)計(jì)數(shù)據(jù)可以明顯地看出，自適應(yīng)聯(lián)邦卡爾曼濾波器濾波后精度有明顯的提升。

圖4 自適應(yīng)聯(lián)邦濾波器仿真圖

為了驗(yàn)證多尺度融合效果，考慮傳感器的更新頻率并進(jìn)行仿真，仿真條件除更新頻率外與上面仿真一致。假定里程計(jì)0.1 s更新一次，單目相機(jī)1 s更新一次，均采用自適應(yīng)聯(lián)邦卡爾曼濾波，分別使用多尺度融合和不使用多尺度融合。濾波后誤差的仿真結(jié)果如圖5所示，可以發(fā)現(xiàn)沒有進(jìn)行多尺度融合的濾波效果明顯變差，這是因?yàn)楦螺^慢的單目相機(jī)存在滯后，通過多尺度融合減小了這種滯后。

圖5 多尺度融合仿真圖

4.2 實(shí)驗(yàn)與數(shù)據(jù)分析

為了進(jìn)一步驗(yàn)證所設(shè)計(jì)的濾波算法，本節(jié)采用麥克納姆輪全向機(jī)器人平臺(tái)進(jìn)行實(shí)驗(yàn)驗(yàn)證，對(duì)2.6 m×2.35 m的矩形軌跡進(jìn)行跟蹤，小車由A點(diǎn)出發(fā)，經(jīng)過B、C、D點(diǎn)，最終回到A點(diǎn)。實(shí)驗(yàn)平臺(tái)如圖6所示，軌跡測量設(shè)備為激光跟蹤系統(tǒng)，將靶球固定在AGV上，激光跟蹤儀實(shí)時(shí)追蹤靶球，獲取靶球的位置。

圖6 實(shí)驗(yàn)及測量平臺(tái)

一共進(jìn)行4組實(shí)驗(yàn)，實(shí)驗(yàn)結(jié)果如表1所示，由于里程計(jì)存在累計(jì)誤差，雖然一開始具有較好的精度，但隨著運(yùn)動(dòng)距離的增加，實(shí)際軌跡與期望軌跡越來越遠(yuǎn)，無法形成完整的閉合軌跡。采用單目相機(jī)定位未濾波得到的軌跡如圖7所示，可以看出具有較大的跟蹤誤差且軌跡不平滑。采用聯(lián)邦卡爾曼濾波的軌跡如圖8所示，相比于沒有濾波之前的軌跡得到了一些改善。采用自適應(yīng)聯(lián)邦卡爾曼濾波得到的軌跡如圖9所示，通過對(duì)比軌跡圖，可以看出定位精度有明顯的改善。圖10為在擾動(dòng)下的軌跡，AGV初始位置并不在原點(diǎn)，但隨著時(shí)間的推移，整個(gè)系統(tǒng)會(huì)趨于穩(wěn)定，并得到一個(gè)較為準(zhǔn)確的位姿估計(jì)。圖11是3種濾波方式的誤差比較，從圖中可以看出自適應(yīng)聯(lián)邦卡爾曼濾波具有較好的濾波效果。根據(jù)表1可知，經(jīng)自適應(yīng)聯(lián)邦卡爾曼濾波后最大誤差從50.162 9 mm減小為15.603 0 mm，方均根誤差從13.671 0 mm減小到5.922 4 mm，平均誤差從10.370 9 mm減小到4.698 7 mm，最大定位誤差減小了69%，方均根誤差減小了56.6%，可以看出所設(shè)計(jì)的濾波器可以提高定位精度。

表1 幾種濾波方式的誤差統(tǒng)計(jì) 單位：mm

圖7 未濾波軌跡

圖8 聯(lián)邦濾波軌跡

圖9 自適應(yīng)聯(lián)邦濾波軌跡

圖10 初始位置干擾后軌跡

圖11 幾種濾波方式的誤差圖

根據(jù)上述的仿真和實(shí)驗(yàn)的結(jié)果，可以推斷出所設(shè)計(jì)的濾波器具有一定的容錯(cuò)能力。當(dāng)單目相機(jī)被遮擋或超出工作范圍時(shí)，此時(shí)單目相機(jī)數(shù)據(jù)具有嚴(yán)重的滯后性，但是里程計(jì)仍然能夠進(jìn)行工作，濾波器此時(shí)只信任里程計(jì)。盡管由于里程計(jì)的特性會(huì)導(dǎo)致誤差逐漸增大，但是只要當(dāng)單目相機(jī)重新正常工作，AGV將逐漸減小誤差并回到正常的狀態(tài)。

5 結(jié)語

本文設(shè)計(jì)了一套Mecanum輪全向機(jī)器人系統(tǒng)，推導(dǎo)了AGV的正逆運(yùn)動(dòng)學(xué)方程。建立了AGV及各傳感器之間的坐標(biāo)關(guān)系，基于單目相機(jī)的AprilTag定位、里程計(jì)定位、加速度計(jì)定位建立定位模型，并將傳感器的非線性問題轉(zhuǎn)換為線性問題。根據(jù)各傳感器的特點(diǎn)，設(shè)計(jì)自適應(yīng)聯(lián)邦卡爾曼濾波器，并將不同更新頻率的傳感器進(jìn)行多尺度融合。通過仿真和實(shí)驗(yàn)表明：所設(shè)計(jì)濾波器能夠大幅提高AGV的精度，并具有良好的魯棒性。