基于信息融合和卡爾曼濾波的移動機器人定位技術(shù)

張國麗

（河北軟件職業(yè)技術(shù)學院，保定 071000）

0 引言

近年來移動機器人的研究及相關(guān)技術(shù)應用得到了前所未有的發(fā)展。世界各地投入相當可觀的財力和人力，研究各種不同的功能及應用領(lǐng)域的機器人。國外機器人的研究起步較早，相關(guān)技術(shù)較為成熟，所研發(fā)出來的機器人，也被應用在不同領(lǐng)域上，包括日常生活中的機器狗，以及用于太空的環(huán)境探測機器人。雖然國內(nèi)機器人的研究成果不如國外豐碩，但許多產(chǎn)、學界也正努力的研發(fā)許多不同功能的機器人，取得了相當可觀的成績。

避障、導航和路徑規(guī)劃是移動機器人領(lǐng)域的三個研究熱點。在移動機器人自動導航中，對未知環(huán)境中特別是一些動態(tài)環(huán)境下的環(huán)境辨識和自身的地位是當前亟待解決的一個問題。定位技術(shù)能夠獲得在任何時刻移動機器人的相對位置，因此是實現(xiàn)導航和保證機器人自主導航的關(guān)鍵，充分體現(xiàn)了機器人的自主智能能力[1]。

基于航位推算的位置估計方法是一種傳統(tǒng)的簡單定位方法。里程計信息記錄了機器人的運動信息，可以通過航位推算來估計機器人的位置。但是因為移動機器人的輪子存在隨機滑動，里程計存在漂移誤差，移動機器人的真實運動模型不理想等原因，關(guān)于機器人的位置估計存在很大的誤差。隨著時間的增加，這種誤差也會累計到很大。目前已經(jīng)發(fā)展出來很多定位算法，如基于環(huán)境模型的定位方法、位姿跟蹤方法、三角定位和概率定位方法等。其中，概率定位方法是目前定位方法中比較系統(tǒng)和流行的方法，它在貝葉斯規(guī)則下，通過不斷更新姿態(tài)空間與位置空間的概率分布實現(xiàn)定位?？柭鼮V波是一種典型的概率定位方法。

盡管卡爾曼濾波是移動機器人自主定位的首選方法之一，它主要是以線性狀態(tài)模塊和測量模塊為基礎(chǔ)，但實際中許多的物理系統(tǒng)常常是用非線性系統(tǒng)方程式來描述。為了提高這種情況下的定位精度。為了提高這種情況下的定位精度，本論文提出一種基于多平臺下傳感器融合的移動機器人精確定位技術(shù)。將多個傳感器得到的距離參數(shù)，借助于卡爾曼濾波來進行定位。既可以采用多個狀態(tài)變量的融合策略，又可以采用觀測變量融合的策略。數(shù)值模擬結(jié)果顯示，相比融合前，該方法能夠獲得更加精確的定位。

1 卡爾曼濾波定位

自從R.E.Kalman 在1960年提出卡爾曼濾波(Kalman Filter, KF)以來，卡爾曼濾波器已經(jīng)成功的應用在許多工程問題上，例如：信號處理和目標追蹤等問題?？柭鼮V波器之所以能被應用在工程問題上，主要是因為其他運動估測方法必須儲存觀測系統(tǒng)的運算和濾波器的系數(shù)，這樣將增加系統(tǒng)運算的復雜性。然而，卡爾曼濾波器采取的是遞歸的計算方式，并且使用狀態(tài)空間表示法可以獲得目標在不同時間的系統(tǒng)狀態(tài)，使得計算更為簡單有效。因此卡爾曼濾波定位在移動機器人自主定位中得到成功的應用[2～4]。

移動機器人的定位問題本質(zhì)上是一個隨機統(tǒng)計系統(tǒng)中的狀態(tài)估計問題。如果該系統(tǒng)中的觀測噪聲滿足高斯白噪聲分布，卡爾曼濾波能夠?qū)θ诤系臄?shù)據(jù)給出最大似然意義上的最佳估計。令X(t)=[x(k), y(k), θ(k)]表示機器人的站姿，其中θ(k)表示第k個時刻全局坐標系x軸方向與移動機器人的x軸方向之間的夾角，x(k), y(k)表示全局坐標系中第k個時刻移動機器人的坐標位置。假設k時刻機器人的位置可以利用下面的線性模型來表示：

其中，Φ(k k-1)為狀態(tài)轉(zhuǎn)移矩陣，U(k)為輸入的控制向量，W(k)為影響U(k)的噪聲向量。接下來將機器人的位置作為觀測變量，建立卡爾曼濾波算法中的測量方程：其中，H(k)是觀測矩陣，V(k)是觀測噪聲。一個基本的卡爾曼濾波過程包括如下步驟。

步驟1：首先按照式(3)執(zhí)行系統(tǒng)狀態(tài)的一步預測估計：

步驟2：系統(tǒng)狀態(tài)估計：

Q(k)為系統(tǒng)噪聲協(xié)方差陣。一步預測均方誤差陣P(k k-1)是從估計均方誤差陣P(k-1)轉(zhuǎn)移過來的，并加上了系統(tǒng)噪聲方差的影響。步驟4：濾波增益方程：

2 多傳感器平臺數(shù)據(jù)融合

由上述分析可知，卡爾曼濾波采用的是一個最佳回歸數(shù)據(jù)處理準則，具有以下兩個特點。1）優(yōu)化：利用所有的測量數(shù)據(jù)去求出最佳的狀態(tài)估測。2）回歸：不需保存過去測量數(shù)據(jù)，只處理當時的測量的最新數(shù)據(jù)。然而，卡爾曼濾波主要是以線性狀態(tài)模塊和測量模塊為基礎(chǔ)，但實際中許多的物理系統(tǒng)常常是用非線性系統(tǒng)方程式來描述，所以必須將修正卡爾曼濾波來作非線性系統(tǒng)方程式。在這種情況下，可以采用多傳感器信息融合(Multisensor Information Fusion)來克服。

多傳感器信息融合技術(shù)首先是從軍事領(lǐng)域發(fā)展起來的技術(shù)，主要優(yōu)點是在于觀測信息的重復性和互補性上可提供更好的性能，而且在工程上也廣泛地應用在機器人學（robotics）、監(jiān)視系統(tǒng)（surveillance）和自動化制造技術(shù)（manufacturing automation）上。一般來說，多傳感器信息融合技術(shù)在測試應用上會分為兩種不同的情況：同種類型傳感器信息融合和不同種類型傳感器融合。在本文的研究中采用的同類型的傳感器信息融合。

在本系統(tǒng)中，采用等間隔分布的5個聲納來測定機器人周圍障礙物的距離和合作機器人的距離。超聲波測距的原理是利用控制電路發(fā)出一個脈波驅(qū)動超聲波，使發(fā)射器發(fā)射一小段時間的超聲波信號，直到信號遇到障礙物反射回來后，通過接收器將反射回來的超聲波接收。因此只要計算發(fā)射到接收超聲波訊號的時間，即可算出超聲波模塊與障礙物的距離。用戶可以根據(jù)多傳感器的數(shù)據(jù)建立機器人的導航圖譜，進行多傳感器融合。

將上述卡爾曼濾波定位技術(shù)與多傳感器平臺相結(jié)合，提出兩種融合方案：1）狀態(tài)變量融合。該方法主要是針對個別傳感器分別用卡爾曼濾波器的定位估計技術(shù)而得到的狀態(tài)向量值，這些值再經(jīng)由數(shù)據(jù)融合技術(shù)而得到最佳的狀態(tài)向量。2）觀測向量融合。該方法主要是針對個別傳感器直接經(jīng)由數(shù)據(jù)融合技術(shù)而得到一個權(quán)重值，再利用卡爾曼濾波器的估計技術(shù)來獲得最佳的狀態(tài)向量估計值。如圖1(a)、(b)所示。本文采用的是第二種方案，權(quán)值采用相同值0.2。

圖1 基于信息融合和卡爾曼濾波的移動機器人定位

3 仿真實驗

首先用如圖2所示的運動軌跡方程生成一組全局坐標系下真實的移動機器人坐標值。比較卡爾曼濾波器算法和本文所提出的多傳感器融合卡爾曼濾波算法，從結(jié)果可以看出，多傳感器融合卡爾曼濾波算法與真實的軌跡基本一致，得到的定位結(jié)果非常準確。

圖2 定位估計結(jié)果對比

4 結(jié)束語

爾曼濾波是移動機器人進行自主定位的首選方法之一，然而，它主要是以線性狀態(tài)模塊和測量模塊為基礎(chǔ)，但實際中許多的物理系統(tǒng)常常是用非線性系統(tǒng)方程式來描述。為了提高這種情況下的定位精度。為了提高這種情況下的定位精度，本論文提出一種基于多平臺下傳感器融合的移動機器人精確定位技術(shù)。將多個傳感器得到的距離參數(shù)，借助于卡爾曼濾波來進行定位。既可以采用多個狀態(tài)變量的融合策略，又可以采用觀測變量融合的策略。數(shù)值模擬結(jié)果顯示，相比融合前，該方法能夠獲得更加精確的定位。

[1]R.Simmons, S.Koening, Probabilistic navigation in partially observable environments[C], in: Proceedings of the International Joint Conference on Artifi cial Intelligence,Montreal, Que., Vol.2, 1995, pp.1660-1667.

[2]Mohinder S.Grewal, Angus P.Andrews, Kalman Filtering:Theory and Application[M], John Wiley& Sons, Inc.Canada 2001.

[3]R.G.Brown and P.Y.C Hwang, Introduction to Random Signals and Applied Kalman Filter[M], Second Edition,Wiley, New york, 1992.

[4]Grewal, Mohinder S., Andrews, Angus P., Kalman Filtering: Theory and Practice Using MATLAB Second Edition[M], John Wiley&Sns Inc., January 2001.