李 婷，崔艷艷，楊海斌，劉游龍 ，樊澤明

(1.陜西航空電氣有限責(zé)任公司 民機(jī)事業(yè)部，陜西 興平 713107;2.陜西航空電氣有限責(zé)任公司秦嶺電機(jī)分公司 管理創(chuàng)新部，陜西 興平 713107;3.西安慶安電氣控制有限責(zé)任公司 公司辦公室，西安710077;4.西北工業(yè)大學(xué) 自動(dòng)化學(xué)院，西安 710129)

0 引言

移動(dòng)機(jī)器人的定位技術(shù)就是要回答“我在哪里”，精準(zhǔn)的定位是移動(dòng)機(jī)器人的基礎(chǔ)，只有正確地回答了“我在哪里”，我要去哪里以及如何到達(dá)的問(wèn)題才能有意義和保障。

文獻(xiàn)[1]綜合考慮了雙目視覺(jué)和二維碼定位方法，將不同的傳感器數(shù)據(jù)融合，改善了定位精度，但計(jì)算量較大。文獻(xiàn)[2]提出采用二維碼作為信標(biāo)提供全局的位置參考，二維碼中包含標(biāo)號(hào)及位置信息，通過(guò)安裝在移動(dòng)機(jī)器人上的工業(yè)攝像頭快讀讀取二維碼信息，通過(guò)檢測(cè)到的若干個(gè)二維碼的位置信息計(jì)算出機(jī)器人的位姿該方法在二維碼之間的精度一般較差。文獻(xiàn)[3]提出一種基于信標(biāo)的傳感器融合定位方法，該方法采用一些人造的信標(biāo)，一個(gè)廉價(jià)的GPS和安裝在輪子上的里程計(jì)，通過(guò)數(shù)據(jù)融合，可以達(dá)到厘米級(jí)的定位精度，由于用到GPS系統(tǒng)，不適應(yīng)于室內(nèi)定位，文獻(xiàn)[4]提出一種基于隨機(jī)信標(biāo)的導(dǎo)航定位方法，通過(guò)測(cè)量隨機(jī)分布的信標(biāo)與航行器之間的距離和方位，用SLAM方法估計(jì)出航行器的位姿。這種方法適應(yīng)于跟蹤問(wèn)題。

針對(duì)機(jī)器人上述定位導(dǎo)航方法的不足，本文給出了采用里程計(jì)和采用超聲波的定位方法。里程計(jì)的定位方法不可避免會(huì)產(chǎn)生累計(jì)誤差，無(wú)法長(zhǎng)時(shí)間使用，而基于超聲波的定位方法沒(méi)有累計(jì)誤差，但定位周期較長(zhǎng)，實(shí)時(shí)性不夠好。因此，文章綜合兩種定位方法的特點(diǎn)，通過(guò)融合兩種傳感器的數(shù)據(jù)，改善定位精度，縮短定位周期。

1 移動(dòng)機(jī)器人定位方法

要回答移動(dòng)機(jī)器人“在哪兒”的問(wèn)題，需要借助于移動(dòng)機(jī)器人自身攜帶的傳感器，以確定自身的位置，傳感器是機(jī)器人感知外界環(huán)境與自身狀態(tài)的主要設(shè)備。機(jī)器人的自主性很大程度取決于所配備的傳感器數(shù)量以及種類(lèi)。傳感器的精度和數(shù)量直接影響了定位的精度。本文研究所用的教學(xué)平臺(tái)工作于室內(nèi)，分別采用了基于里程計(jì)的定位方法和基于超聲波傳感器的定位方法，最后將這兩種傳感器的數(shù)據(jù)進(jìn)行融合，以改善定位的精度。

1.1 基于里程計(jì)的定位

里程計(jì)是一種常用的位置傳感器，機(jī)器人的驅(qū)動(dòng)輪上安裝有位置編碼盤(pán)，可以通過(guò)編碼盤(pán)獲取機(jī)器人的距離信息及速度信息，根據(jù)返回的里程值和速度值推算位置。下面闡述基于里程計(jì)定位的原理。

假設(shè)設(shè)置的采樣周期足夠小，可以認(rèn)為移動(dòng)機(jī)器人左邊和右邊兩個(gè)輪的移動(dòng)速度為常數(shù)。在第i個(gè)周期內(nèi)，左右光電碼盤(pán)輸出的增量脈沖為NL(i)和NR(i)，且轉(zhuǎn)換因子表示為式(1)。

式中,Cm是轉(zhuǎn)換因子；Dn是驅(qū)動(dòng)輪的標(biāo)稱(chēng)直徑；Ce是碼盤(pán)的分別率；n是齒輪減速比。則在這一采樣時(shí)間內(nèi)，機(jī)器人左右輪行走的長(zhǎng)度ΔUL(i)和ΔUR(i)分別如式(2)、(3)所示。

ΔUL(i)=cmNL(i)

(2)

ΔUR(i)=cmNR(i)

(3)

因此，機(jī)器人行走的長(zhǎng)度ΔU(i)為

機(jī)器人轉(zhuǎn)過(guò)的角度Δθ(i)為

式中，L是兩輪之間的長(zhǎng)度。

記第i個(gè)采樣時(shí)間時(shí)，移動(dòng)小車(chē)在絕對(duì)坐標(biāo)系下的位型為{x(i),y(i),θ(i)}，則在i時(shí)刻的位型計(jì)算如式(6)。

θ(i)=θ(i-1)+Δθ(i)

x(i)=x(i-1)+ΔU(i)cos[θ(i)]

y(i)=y(i-1)+ΔU(i)sin[θ(i)]

(6)

假定里程計(jì)返回的位姿變化信息表示為式(7)。

δ〈k〉=(δd,δθ)

(7)

式中，δd是移動(dòng)機(jī)器人走過(guò)的位移；δθ是機(jī)器人航向角的變化。

在k時(shí)刻，移動(dòng)機(jī)器人的位姿記為ξ〈k〉如式(8)。

又假定采樣的時(shí)間間隔很小，認(rèn)為平移和旋轉(zhuǎn)的先后順序不重要，我們采用在車(chē)子的X方向移動(dòng)δd，然后轉(zhuǎn)動(dòng)δθ的方式來(lái)計(jì)算k+1時(shí)刻的位型，記為ξ〈k+1〉如式(9)。

寫(xiě)成3維列向量如式(10)。

上式給出了根據(jù)里程計(jì)的輸入信息推算出的下一個(gè)時(shí)刻的位姿，但是上面的式子中我們認(rèn)為里程計(jì)的反饋是理想值，但現(xiàn)實(shí)中，存在著模型誤差，因此我們給δd和δθ分別添加隨機(jī)噪聲vd和vθ，這樣包含里程計(jì)誤差的位姿更新公式如式(11)。

綜上，將x〈k+1〉，x〈k〉 ，δ〈k〉，v〈k〉之間的關(guān)系用f函數(shù)記為式(12)。

x(k+1)=f(x〈k，δ〈k〉，v〈k〉)

(12)

式中，x〈k〉 是k時(shí)刻的位姿；δ〈k〉是k時(shí)刻里程計(jì)的輸出數(shù)據(jù)；v〈k〉是k時(shí)刻的隨機(jī)測(cè)量噪聲。

式(12)中，v=(vd,vθ)～N(0，V)是一個(gè)高斯噪聲如式(13)。

式(13)的協(xié)方差矩陣反對(duì)角的元素值均為0，是因?yàn)槲覀冋J(rèn)為距離和航向之間是獨(dú)立分布的。通常要得到V的值是不容易的，但是我們可以根據(jù)經(jīng)驗(yàn)作出合理的假設(shè)，本文設(shè)置σd=2 cm，σθ=0.5°。

我們采用卡爾曼濾波器來(lái)估計(jì)移動(dòng)機(jī)器人的位姿，認(rèn)為測(cè)量噪聲都是0均值的高斯噪聲?？柭鼮V波器是線性濾波器，但是式(12)不是一個(gè)線性方程，需要先對(duì)系統(tǒng)進(jìn)行線性化，將卡爾曼濾波器應(yīng)用于非線性系統(tǒng)，稱(chēng)為擴(kuò)展卡爾曼濾波(EKF)[5]。對(duì)式(12)取泰勒公式的一階展開(kāi)式得到式(14)。

式中，F(xiàn)x是雅克比矩陣，是f對(duì)x求偏導(dǎo)；FV是雅克比矩陣，是f對(duì)v求偏導(dǎo)。

式(14)中Fx，F(xiàn)v見(jiàn)式(14)、(15)、(16)。

現(xiàn)在我們得到EKF的預(yù)測(cè)方程如式(17)。

1.2 基于超聲波傳感器的定位

在自然界中，蝙蝠可以通過(guò)嘴巴發(fā)出超聲波，耳朵接受回聲來(lái)避開(kāi)障礙物，根據(jù)同一物體的回聲到達(dá)兩耳朵時(shí)間的細(xì)微差別，判斷物體的方位。人類(lèi)從蝙蝠身上得到啟示，設(shè)計(jì)出超聲波模組，可以用于測(cè)距，而如果有三個(gè)以上位置確定的點(diǎn)，測(cè)得待定位點(diǎn)到環(huán)境中位置確定的點(diǎn)的距離，就可以實(shí)現(xiàn)定位，這種方式是基于信標(biāo)的定位，前提是需要知道環(huán)境中某幾個(gè)特定點(diǎn)的坐標(biāo)，以此為基準(zhǔn)，計(jì)算出未知點(diǎn)的坐標(biāo)。本文采用基于超聲波的定位系統(tǒng)，在實(shí)驗(yàn)室的上方有8個(gè)位置已知的接收頭，如圖1。然后在二自由度轉(zhuǎn)臺(tái)的上方安裝有超聲波的發(fā)射頭，如圖2。機(jī)器人通過(guò)發(fā)射超聲波，然后測(cè)量到達(dá)接收的時(shí)間，計(jì)算出當(dāng)前點(diǎn)到接收點(diǎn)的長(zhǎng)度信息，計(jì)算出小車(chē)的位置。

圖1 天花板上位置已知的超聲波接收頭

圖2裝在小車(chē)上方的超聲波發(fā)射頭

假設(shè)第i個(gè)采樣周期，機(jī)器人測(cè)得某三個(gè)位置已知的坐標(biāo)點(diǎn)到發(fā)射點(diǎn)的距離分別為d1、d2、d3，三個(gè)位置已知的坐標(biāo)點(diǎn)分別為u1={x1,y1,z1}，u2={x2,y2,z2}，u3={x3,y3,z3}，設(shè)待定位點(diǎn)的坐標(biāo)為P={x,y,z}，其定位方法如圖3所示。

圖3超聲波定位方法

當(dāng)已知d1、d2、d3的距離和u1、u2、u3的坐標(biāo)，就可以得到三個(gè)方程如式(18)所示，聯(lián)立可以求得P的位置。

移動(dòng)機(jī)器人在運(yùn)動(dòng)的過(guò)程位置不斷發(fā)生改變，因此需要周期性的進(jìn)行測(cè)量發(fā)射頭到接收頭的距離，程序會(huì)選出離發(fā)射點(diǎn)最近的三個(gè)定位點(diǎn)進(jìn)行計(jì)算。由于接收頭是安裝在天花板上的，因此可以認(rèn)為z=z1=z2=z3用等式①依次減去等式②③簡(jiǎn)化可以得到式(19)。

式(19)可以轉(zhuǎn)換為成線性形式如式(20)。

AX=b

(20)

對(duì)式(20)兩邊同時(shí)乘以A-1得到小車(chē)位置如式(21)。

X=A-1b

(21)

1.3 基于多傳感器融合的定位方法

上述兩小節(jié)中分別采用了基于里程計(jì)和基于超聲的定位方式，基于里程計(jì)的定位方式有累計(jì)誤差，隨著時(shí)間的增加，置信度也越低。而采用超聲波的定位雖然沒(méi)有累積誤差，但是計(jì)算速度較慢，因此考慮將兩種傳感器的信息，通過(guò)擴(kuò)展卡爾曼濾波器進(jìn)行融合[7]。假定在某一個(gè)空間內(nèi)分布有若干個(gè)位置已知的超聲波接收點(diǎn)，在機(jī)器人上安裝有發(fā)射點(diǎn)，機(jī)器人可以通過(guò)感知已知點(diǎn)來(lái)修正誤差。

將觀測(cè)方程記為

z=h(xv,xf,w)

(22)

式中，xv是車(chē)子的狀態(tài)；xf是已知坐標(biāo)的超聲波接收點(diǎn)。

設(shè)第i個(gè)特征點(diǎn)的坐標(biāo)為xfi=(xi,yi)，那么

取σr=0.1 m，σβ=1°。

在式(23)中機(jī)器人根據(jù)估計(jì)的小車(chē)位置和感知到的已知點(diǎn)的坐標(biāo)，計(jì)算出距離和航向角。當(dāng)觀測(cè)值和估計(jì)值之間有偏差時(shí)，說(shuō)明此時(shí)估計(jì)的位型存在誤差，寫(xiě)為式(24)。

卡爾曼濾波器就是使用上式對(duì)小車(chē)的位姿和協(xié)方差進(jìn)行更新的，和之前的處理方式一樣，我們將式(22)進(jìn)行線性化得到式(25)。

式中，Hx是雅克比矩陣，是h對(duì)xv的偏導(dǎo)；Hw是雅克比矩陣，是h對(duì)xw的偏導(dǎo)。

式(25)中，Hx、Hw見(jiàn)式(26)、(27)。

綜上，我們得到EKF的更新公式如式(28)所示。

式中，K是卡爾曼增益。式(28)，卡爾曼益K如式(29)。

K〈k+1〉=P＾〈k+1|k〉Hx〈k+1〉TS〈k+1〉-1

S〈k+1〉=Hx〈k+1〉P＾〈k+1|k〉Hx〈k+1〉T+Hw〈k+1〉W＾〈k+1〉Hw〈k+1〉T

(29)

2 實(shí)驗(yàn)

圖4超聲校準(zhǔn)的軌跡圖

采用移動(dòng)機(jī)器人和基于超聲波的定位系統(tǒng)，在實(shí)驗(yàn)室的上方有8個(gè)位置已知的接收頭，如圖1所示。然后在小車(chē)的上方安裝有超聲波的發(fā)射頭。

利用卡爾曼更新公式，加上之前的預(yù)測(cè)公式，實(shí)現(xiàn)了最優(yōu)估計(jì)，并將圖1所示超聲波和裝在小車(chē)上方圖2超聲波定位環(huán)境下，運(yùn)行300步，定位軌跡如圖4所示。

同樣我們采用橢圓的面積來(lái)表示位姿的不確定性，在圖4中，就已包含繪制出的橢圓。

圖5 不確定橢圓圖

僅僅采用里程計(jì)的定位方法，隨著時(shí)間的增加，誤差會(huì)越來(lái)越大，而通過(guò)融合超聲數(shù)據(jù)和里程計(jì)數(shù)據(jù)，可以定期進(jìn)行校準(zhǔn)，保證位姿的不確定在一個(gè)可接受的范圍內(nèi)。

3 結(jié)論

分析兩種定位方法的不足，即里程計(jì)的定位方法不可避免會(huì)產(chǎn)生累計(jì)誤差，無(wú)法長(zhǎng)時(shí)間使用，而基于超聲波的定位方法沒(méi)有累計(jì)誤差，但定位周期較長(zhǎng)，實(shí)時(shí)性不夠好針對(duì)機(jī)器人定位問(wèn)題。結(jié)合二者的優(yōu)點(diǎn)，互補(bǔ)其缺點(diǎn)，提出了采用里程計(jì)和超聲波的融合定位方法。實(shí)驗(yàn)結(jié)果表明，與里程計(jì)的定位方法和超聲波的定位方法相比，二者融合得到較高的定位精度。該方法的不足是超聲波的安裝精度對(duì)定位有一定影響，需要后續(xù)繼續(xù)研究。