基于里程計和超聲波群多傳感器融合機器人定位導航方法

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

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

0 引言

移動機器人的定位技術就是要回答“我在哪里”，精準的定位是移動機器人的基礎，只有正確地回答了“我在哪里”，我要去哪里以及如何到達的問題才能有意義和保障。

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

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

1 移動機器人定位方法

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

1.1 基于里程計的定位

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

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

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

ΔUL(i)=cmNL(i)

(2)

ΔUR(i)=cmNR(i)

(3)

因此，機器人行走的長度ΔU(i)為

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

式中，L是兩輪之間的長度。

記第i個采樣時間時，移動小車在絕對坐標系下的位型為{x(i),y(i),θ(i)}，則在i時刻的位型計算如式(6)。

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

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

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

(6)

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

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

(7)

式中，δd是移動機器人走過的位移；δθ是機器人航向角的變化。

在k時刻，移動機器人的位姿記為ξ〈k〉如式(8)。

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

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

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

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

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

(12)

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

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

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

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

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

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

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

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

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

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

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

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

圖3超聲波定位方法

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

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

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

AX=b

(20)

對式(20)兩邊同時乘以A-1得到小車位置如式(21)。

X=A-1b

(21)

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

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

將觀測方程記為

z=h(xv,xf,w)

(22)

式中，xv是車子的狀態(tài)；xf是已知坐標的超聲波接收點。

設第i個特征點的坐標為xfi=(xi,yi)，那么

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

在式(23)中機器人根據(jù)估計的小車位置和感知到的已知點的坐標，計算出距離和航向角。當觀測值和估計值之間有偏差時，說明此時估計的位型存在誤差，寫為式(24)。

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

式中，Hx是雅克比矩陣，是h對xv的偏導；Hw是雅克比矩陣，是h對xw的偏導。

式(25)中，Hx、Hw見式(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 實驗

圖4超聲校準的軌跡圖

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

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

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

圖5 不確定橢圓圖

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

3 結(jié)論

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