張國(guó)麗

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

0 引言

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

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

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

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

1 卡爾曼濾波定位

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

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

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

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

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

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

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

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

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

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

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

圖1 基于信息融合和卡爾曼濾波的移動(dòng)機(jī)器人定位

3 仿真實(shí)驗(yàn)

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

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

4 結(jié)束語(yǔ)

爾曼濾波是移動(dòng)機(jī)器人進(jìn)行自主定位的首選方法之一，然而，它主要是以線性狀態(tài)模塊和測(cè)量模塊為基礎(chǔ)，但實(shí)際中許多的物理系統(tǒng)常常是用非線性系統(tǒng)方程式來(lái)描述。為了提高這種情況下的定位精度。為了提高這種情況下的定位精度，本論文提出一種基于多平臺(tái)下傳感器融合的移動(dòng)機(jī)器人精確定位技術(shù)。將多個(gè)傳感器得到的距離參數(shù)，借助于卡爾曼濾波來(lái)進(jìn)行定位。既可以采用多個(gè)狀態(tài)變量的融合策略，又可以采用觀測(cè)變量融合的策略。數(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.