基于粒子濾波的微小型移動機(jī)器人紅外定位研究

翟亞芳， 顧釗源， 張大偉

(1. 安陽工學(xué)院 電子信息與電氣工程學(xué)院 河南 安陽 455000; 2. 鄭州大學(xué) 信息工程學(xué)院 河南 鄭州 450001)

0 引言

定位問題是實(shí)現(xiàn)微小型移動機(jī)器人自主工作的基礎(chǔ)[1].從概率論的角度，機(jī)器人的定位問題也可以看作結(jié)合傳感器信息實(shí)現(xiàn)狀態(tài)估計(jì)的問題.經(jīng)典的機(jī)器人自定位方法有基于貝葉斯理論的卡爾曼濾波[2]、擴(kuò)展卡爾曼濾波[3]和粒子濾波[4]等.其中粒子濾波由于能夠解決非線性、非高斯系統(tǒng)的狀態(tài)估計(jì)問題[5]而受到廣泛關(guān)注.受尺寸設(shè)計(jì)的限制，選用尺寸小、功耗低的紅外傳感器設(shè)計(jì)微小型移動機(jī)器人的定位測量系統(tǒng).近年來，基于紅外傳感器的機(jī)器人定位系統(tǒng)研究取得了一定的進(jìn)展.文獻(xiàn)[6]基于Khepera Ⅲ本體上的紅外傳感器，在結(jié)構(gòu)化規(guī)則的環(huán)境中(1.8 m范圍內(nèi))，結(jié)合擴(kuò)展卡爾曼濾波，對消除微小型機(jī)器人定位過程中的累計(jì)誤差進(jìn)行了研究，定位精度達(dá)到1.9 cm. 文獻(xiàn)[7]提出了一種基于改進(jìn)IMM-UKF算法的紅外路標(biāo)室內(nèi)定位方法，該系統(tǒng)在室內(nèi)已知環(huán)境下配備單個大功率紅外發(fā)射管作為路標(biāo)，通過機(jī)器人本體上的紅外攝像機(jī)作為接收傳感器，定位精度可達(dá)4.5 cm，但該紅外定位測量系統(tǒng)設(shè)計(jì)尺寸較大，無法應(yīng)用到微小型機(jī)器人本體上.針對于此，文獻(xiàn)[8]設(shè)計(jì)了一套基于六對紅外收發(fā)傳感器的小型定位系統(tǒng)，結(jié)合粒子濾波算法，實(shí)現(xiàn)了微小型機(jī)器人之間的相對定位，精度達(dá)到7.4 mm.

為進(jìn)一步解決微小型移動機(jī)器人的定位問題，本文設(shè)計(jì)了一套小尺寸的紅外定位系統(tǒng)，可根據(jù)紅外接收信號強(qiáng)度得到機(jī)器人之間的相對位置信息，并在已知環(huán)境中通過將坐標(biāo)確定的微小型機(jī)器人(指可根據(jù)周圍環(huán)境進(jìn)行自定位的機(jī)器人)作為地標(biāo)，基于粒子濾波算法可對微小型移動機(jī)器人之間進(jìn)行協(xié)作定位.

1 紅外定位模型

機(jī)器人根據(jù)紅外接收到的信號強(qiáng)度，換算出地標(biāo)與當(dāng)前機(jī)器人的相對距離和方位.假設(shè)紅外接收信號的入射角為α，此時機(jī)器人相鄰的n個接收端都接收到了信號，將這n個接收端定義為一個感應(yīng)扇區(qū)，需要根據(jù)每個接收端的接收強(qiáng)度來近似入射信號強(qiáng)度r.假設(shè)n個接收端的接收信號強(qiáng)度沿順時針方向依次為r′-m,r′-m+1,…，r′0,…,r′m-1,r′m，其中m=n/2.單個接收端的接收信號強(qiáng)度與入射信號強(qiáng)度的關(guān)系為r′=rcosδ，其中δ為該接收端與入射信號的夾角.則第i個接收端的接收信號強(qiáng)度為

r′i=rcos(α-φi)，

(1)

式中：α為入射角；φi為第i個接收端與扇區(qū)中心接收端的夾角，且φi=-φ-i.因此，有

r′i+r′-i=2rcosαcosφi；r′i-r′-i=2rsinαsinφi.

(2)

令

(3)

利用三角函數(shù)變換公式，可得

(4)

當(dāng)n=3時，機(jī)器人紅外定位模型如圖1所示.此時，3個接收端的接收信號強(qiáng)度分別為r′0=rcosα，r′1=rcos(α-φ1)，r′-1=rcos(α-φ-1).

令

(5)

將式(5)代入式(4)，得到3個傳感器的接收光強(qiáng)度，即可得到r和α.

2 粒子濾波定位

2.1 里程計(jì)運(yùn)動模型

由于受尺寸限制，機(jī)器人利用里程計(jì)作為內(nèi)部傳感器.圖2為機(jī)器人的里程計(jì)運(yùn)動模型，假設(shè)機(jī)器人在很短的時間間隔內(nèi)進(jìn)行半徑為R的圓弧運(yùn)動，經(jīng)過幾何推算就可以估計(jì)出機(jī)器人兩時刻之間的位姿增量.

圖1 紅外定位模型Fig.1 Infrared localization model

圖2 里程計(jì)運(yùn)動模型Fig.2 Odometer motion model

機(jī)器人相鄰時刻的直線距離增量Δd可以近似表示為

(6)

式中：ΔSL和ΔSR分別為里程計(jì)測得的時間間隔內(nèi)左、右輪平滑移動距離；ΔS為機(jī)器人中心的平滑移動距離.

根據(jù)機(jī)器人的差動運(yùn)動學(xué)方程，機(jī)器人上一時刻與本時刻之間的位姿增量為

(7)

機(jī)器人的里程計(jì)運(yùn)動模型可表示為

(8)

粒子濾波中，可以根據(jù)式(8)的運(yùn)動模型，由機(jī)器人k-1時刻的位姿信息，結(jié)合里程計(jì)的觀測數(shù)據(jù)，來預(yù)測k時刻該機(jī)器人的位姿信息.

2.2 粒子濾波定位算法

(9)

(10)

(11)

若某一時刻同時觀測到M個地標(biāo)，則權(quán)值可表示為

(12)

式中：dj、βj(j=1,2，…，M)為觀測到相對第j個地標(biāo)的距離和方位.

(13)

3 實(shí)驗(yàn)部分與結(jié)果

為了驗(yàn)證本文的定位方法，對實(shí)際機(jī)器人系統(tǒng)進(jìn)行了定位實(shí)驗(yàn)，整個實(shí)驗(yàn)過程如圖3所示.

圖3 定位實(shí)驗(yàn)Fig.3 Localization experiment

圖4 運(yùn)動軌跡Fig.4 Moving trail

在該實(shí)驗(yàn)中，選擇4個機(jī)器人作為路標(biāo)，1個待定位機(jī)器人按圖4中的理想軌跡移動，在移動的過程中，通過4個路標(biāo)機(jī)器人進(jìn)行定位和路徑規(guī)劃，定位過程共進(jìn)行300次位置更新，時間間隔為1 s，粒子集為500.定位過程中不同時刻的距離誤差如圖5(a)所示.可以看出，距離誤差整體上集中在0.5～1.5 cm，隨著時間增加變化幅度不大，基本滿足定位需求，其中最大距離誤差約為2.2 cm.不同時刻的航向角誤差如圖5(b)所示.可以看出，航向角誤差變化趨勢比較平穩(wěn)，整體上集中在0.02～0.10 rad，基本滿足定位需求，其中最大航向角誤差約為0.169 rad.從圖4中定位機(jī)器人的真實(shí)軌跡可以看出，定位機(jī)器人通過4個路標(biāo)機(jī)器人可以較好地完成自身的定位.

圖5 定位誤差Fig.5 Localization error

4 結(jié)論

本文以微小型移動機(jī)器人為基礎(chǔ)，設(shè)計(jì)了一套基于紅外傳感器的粒子濾波定位方法.該方法通過紅外傳感器定位模型確定機(jī)器人之間的相對位置，然后再將部分位置置信估計(jì)比較大的機(jī)器人(指可根據(jù)周圍環(huán)境進(jìn)行自定位的機(jī)器人)作為路標(biāo)，根據(jù)里程計(jì)運(yùn)動模型獲取位姿狀態(tài)預(yù)測信息，并將以上這些信息作為粒子濾波算法的預(yù)測和更新依據(jù)，通過更新粒子集來逼近機(jī)器人在全局坐標(biāo)系中的位置.該方法降低了里程計(jì)累計(jì)誤差對機(jī)器人定位的影響，可在相對定位的基礎(chǔ)上進(jìn)一步提高機(jī)器人的定位精度，比較適合復(fù)雜的非結(jié)構(gòu)環(huán)境，機(jī)器人之間可以實(shí)現(xiàn)協(xié)作定位.