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

張國麗

（河北軟件職業技術學院，保定 071000）

0 引言

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

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

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

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

1 卡爾曼濾波定位

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

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

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

步驟1：首先按照式(3)執行系統狀態的一步預測估計：

步驟2：系統狀態估計：

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

2 多傳感器平臺數據融合

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

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

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

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

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

3 仿真實驗

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

圖2 定位估計結果對比

4 結束語

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

[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.