基于多維尺度法和卡爾曼濾波的機器人傳感器網(wǎng)絡跟蹤定位

嚴爾軍,張 強

(1.青海交通職業(yè)技術學院 信息工程學院,西寧 810003; 2.西安理工大學 機械工程學院,西安 710048)

隨著人工智能的不斷發(fā)展,機器人系統(tǒng)已經(jīng)被廣泛應用于各種領域,如分布式傳感、目標搜索、采礦、繪制地圖等[1-3].無論在任何環(huán)境中,必須首先掌握機器人自身以及其在周圍環(huán)境中的位置關系才可有效地執(zhí)行任務.在多數(shù)場合下,執(zhí)行任務的機器人數(shù)量往往可達數(shù)十個之多.目前,對于機器人定位常用的方法是航位推測法,這種方法在近距離定位的情況下精度高,但在長距離運動時誤差不斷累積,具有發(fā)散性,且存在隨機誤差[4-5],因此,需要絕對定位手段進行校正補償.GPS作為常用的定位方法,由于其設備太大,無法安裝在微型傳感器上,GPS不能在室內(nèi)實現(xiàn)定位,且需要消耗大量的能量.學者們針對GPS定位在室內(nèi)機器人上的應用開展了大量研究,并提出“到達角”(AOA)技術[6-7],可估計機器人到達信號方向以指出位置,然而這些技術需要天線系統(tǒng),導致硬件要求非常復雜.同時其缺點是由多徑和非視線現(xiàn)象引起的誤差[8-9].多維尺度(Multidimensional Scaling,MDS)是指用于多維數(shù)據(jù)探索性分析的一系列方法.MDS方法最初由Torgerson[10]提出,該算法基于度量分析和余弦定律,將物體放置在歐氏坐標系中,以反映測量的距離或彼此之間的不同.Agrawal等[11]利用MDS算法對由大量節(jié)點組成的無線傳感器網(wǎng)絡進行定位.該方法進行了大量的模擬分析,以解決由各向異性的網(wǎng)絡拓撲和復雜地形所造成的缺點,但缺乏對通信成本、消息復雜性和功耗的分析.針對大規(guī)模網(wǎng)絡中節(jié)點距離的傳感器功率限制問題,文獻[8]提出了用節(jié)點間最短路徑逼近歐氏距離的MDS-MAP算法,在已知局部連通性的基礎上,構造了二維地圖,利用Dijkstra算法求出了最短路徑,此外,MDS還適用于通過距離測量進行三維定位[12-14].以前,濾波算法是用來減小雷達和生物傳感器等跟蹤設備造成的測量誤差的,然而在距離測量可用的情況下,對跟蹤節(jié)點的研究還不多見.主要難點在于估計節(jié)點間距離的非線性方程.Kalman(KL)濾波器為定位問題構造了一個線性濾波器,MDS定位算法為節(jié)點間距離定位提供了有效的模型[15].因此,線性KL濾波器結合MDS跟蹤可以將跟蹤與獨立節(jié)點的跟蹤擴展到一個考慮節(jié)點距離的模型.此外,線性濾波器的有效實現(xiàn)使其適合于更廣泛的實際應用.

基于上述原因,本文提出了一個線性卡爾曼濾波器.該濾波器與MDS相結合來跟蹤傳感器網(wǎng)絡節(jié)點的位置,給出了一種在測量對應的成對距離時尋找節(jié)點變化位置的有效方法,并據(jù)此設計了KL濾波器,以實現(xiàn)對室內(nèi)機器人在傳感器網(wǎng)絡下的精確定位.

1 定位算法

1.1 MDS算法

經(jīng)典MDS通常利用相異性矩陣計算低維空間中(通常是二維或三維)對應的節(jié)點信息.假設網(wǎng)絡中布置有N個需要定位的機器人,每個機器人上設置有傳感器網(wǎng)絡節(jié)點,分別由x1,x2,…,xn表示,D=[dij]N×N為距離矩陣,dij表示節(jié)點i與節(jié)點j之間的實際距離，即

(1)

式中:m為坐標維度.

MDS方法的目標是在m維空間中找到網(wǎng)絡節(jié)點[x1,x2,…,xN],xi=[xi1,xi2,…,xim]T使目標函數(shù)F(y)最小,得

(2)

式中:yi=[yi1,yi2,…,yim]T為m維空間中的向量.

定義矩陣B=-(1/2)CND2CN,表示X元素的雙中心距離平方矩陣,其中CN是中心算子,CN=IN-(1/N)IIT,I是N階列向量.對矩陣B進行奇異分解(SVD),B=UVUT,獲得雙中心距離矩陣平方的所有特征向量U和特征值V,進一步可求得N個節(jié)點的坐標矩陣為

(3)

由于矩陣B的奇異值求解需要用正交三角(QR)分解來估計所有特征向量和特征值,運算量大,耗時較長.本文利用快速不動點算法來估計B的最大特征向量,將特征向量估計表示為優(yōu)化問題，即

(4)

在每次迭代時,都采用歸一化算法w←w/‖w‖,利用施密特正交化過程,使第p個特征向量與其他特征向量不相關，即

(5)

因此,可獲得每個特征向量對應的特征值為

(6)

式(5)和式(6)中提出的增量更新需要多次迭代才能收斂.

1.2 卡爾曼濾波

卡爾曼濾波器根據(jù)其測量結果對系統(tǒng)隱含狀態(tài)進行估計,該濾波器用于處理狀態(tài)轉換和測量過程中出現(xiàn)的高斯噪聲.將N個機器人群作為一個離散時間系統(tǒng)，得

(7)

式中:θk為系統(tǒng)在時間指數(shù)k時的狀態(tài)；Zk為Hkθk的測量值和矩陣；Hk為測量結果矩陣，F(xiàn)k為預測矩陣，其作用是預測下一個取樣步驟時的測量值；ωk為過程協(xié)方差;vk為測量協(xié)方差.

假設wk-1，vk的協(xié)方差矩陣Qk和Rk為零均值高斯噪聲,則系統(tǒng)狀態(tài)可以用卡爾曼濾波進行重建，即

(8)

系統(tǒng)狀態(tài)的更新方程為

(9)

式中:I為單位矩陣;δZk為預測觀測值與測量值之間的差值;xk|k-1為系統(tǒng)狀態(tài)在第k步的先驗估計;θk|k-1為系統(tǒng)狀態(tài)在第k步的先驗估計；θk-1為k-1時刻實際測量值；矩陣Fk為預測矩陣，用于預測下一個取樣步驟時的測量值；Pk|k-1為狀態(tài)誤差協(xié)方差矩陣在第k步的先驗估計；Pk為狀態(tài)誤差的協(xié)方差矩陣;Kk為卡爾曼增益.

1.3 基于卡爾曼濾波的MDS算法

基于MDS方法獲得的傳感器網(wǎng)絡節(jié)點距離定位是非線性的,因此，建立直接模型來反映節(jié)點位置的成對距離誤差是非常復雜的.在本文中,從給定的測量距離矩陣D中跟蹤雙中心距離平方矩陣Bk的變化,跟蹤系統(tǒng)的狀態(tài)為矢量，即

(10)

測量矢量Zk是實際距離測量D的雙中心距離平方矩陣為

(11)

反映測量誤差Zk的噪聲模型不再遵循高斯噪聲,因此不能使用卡爾曼濾波器.兩個傳感器節(jié)點i和j之間距離測量可表示為

(12)

(13)

(14)

(15)

式中:?為Kronecker積,令Λ=CN?CN;估計值Zk的協(xié)方差矩陣為

E{vect(Bk)vect(Bk)T}=ΛψkΞkψkΛ

(16)

建立了基于MDS跟蹤傳感器位置的狀態(tài)方程和測量誤差.設系統(tǒng)狀態(tài)定義為N2×1,xk=vect(Bk)的向量,相應的狀態(tài)方程可以表示為

(17)

式中:F=IN2.

(18)

式中:H為單位矩陣IN2.

vk的協(xié)方差矩陣計算式為

(19)

在定義了所有矩陣F,H,Q和R之后,按照式(8)和式(9)方法進行了機器人位置的預測、更新.由于Bk描述的系統(tǒng)狀態(tài)在每次迭代時都被跟蹤,因此,可實時獲得矩陣Bk的特征向量和特征值,并根據(jù)式(3)獲得機器人的位置信息.

2 實驗結果

在二維平面內(nèi)布置N=30個機器人傳感器節(jié)點的全連通網(wǎng)絡用于位置定位,節(jié)點最初分布在100 m×100 m的區(qū)域內(nèi),坐標(10,10),(10,90),(90,10)和(90,90)處固定4個信標用于節(jié)點的基準定位.每次迭代獲取機器人節(jié)點位置時,節(jié)點在垂直方向和水平方向上都遠離信標,移動步長控制在(0,2)范圍內(nèi)均勻分配.由于噪聲隨節(jié)點間距離的增加而增大,定位估計的精度會隨著時間的推移而衰減.為了對比多維尺度法、卡爾曼濾波聯(lián)合定位方法和其他定位方法的性能,定義第k次迭代處的平均誤差距離(MED)為

(20)

圖1 不同噪聲水平下的平均誤差距離Fig.1 Mean error distance under different noise levels

由圖1可知：隨著傳感器網(wǎng)絡節(jié)點逐漸遠離原點,噪聲干擾對距離測量誤差的影響逐漸增大.基于MDS法的機器人傳感器網(wǎng)絡節(jié)點定位誤差隨距離的增大逐漸發(fā)散,存在較大誤差.而本文提出的MDS和KL聯(lián)合定位方法測量的平均誤差距離MED逐漸收斂,遠低于傳統(tǒng)的MDS定位方法.在γ=10噪聲水平下,MDS法定位誤差高達100多m,而本文提出的MDS和KL聯(lián)合定位誤差小于20 m;同樣,在γ=20,定位誤差小于5 m,在γ=30,定位誤差在1 m左右.相比于MDS法,誤差減小到20%以下，證明了本文提出的MDS和KL聯(lián)合定位方法對提高定位跟蹤性能的有效性.

在時間為150 s及γ=30噪聲水平下,對機器人傳感器網(wǎng)絡節(jié)點位置信息進行了測量,如圖2所示.通過顯示與原始節(jié)點之間的實際位置、估計位置,對MDS結合KL定位方法進行了定性評價.由于噪聲的影響,估計的位置與節(jié)點的實際坐標間存在較大的距離誤差.采用MDS和KL聯(lián)合定位方法,誤差的波動遠小于傳統(tǒng)的MDS定位方法.

圖2 不同定位方法下機器人群位置信息Fig.2 Robots position information under different positioning methods

取其中一節(jié)點,獲得t=200 s時節(jié)點的運動軌跡,如圖3所示.在連續(xù)時間內(nèi),基于MDS的節(jié)點移動軌跡呈現(xiàn)出快速變化,而采用MDS結合KL法得到的節(jié)點軌跡更為光滑,更適合節(jié)點實際運動的估計.

圖3 不同定位方法下單個機器人節(jié)點運動軌跡Fig.3 Single robot node trajectory under different positioning methods

進一步研究在不同節(jié)點數(shù)量二維傳感器網(wǎng)絡下,傳統(tǒng)MDS法、擴展卡爾曼濾波方法及MDS和KL聯(lián)合定位方法的定位性能,機器人節(jié)點數(shù)量分別為N=20和N=30,測量時間為t=100 s,如圖4所示.對比不同定位方法下的誤差變化如表1所示.

圖4 不同節(jié)點數(shù)量下各定位方法的平均誤差距離Fig.4 Mean error distance of different positioning methods under different robot numbers表1 不同定位方法下的誤差變化Tab.1 Error under different positioning methods

噪聲水平γMDS結合KLEKLMDSN=20百分比/%N=30百分比/%N=20百分比/%N=30百分比/%N=20百分比/%N=30百分比/%109.337.7810.238.5316.3613.6316.0213.3521.5717.9823.0219.18118.527.109.557.9615.7013.0815.4412.8720.9217.4322.2118.51127.856.548.807.3415.0412.5314.7812.3220.1816.8221.5017.92137.416.188.026.6814.5512.1314.1611.8019.6616.3820.3716.97147.025.857.576.3113.5711.3113.8411.5317.8814.9019.6216.35156.525.437.055.8712.8010.6713.3511.1316.6713.8918.6715.56165.444.536.015.0012.6710.5611.989.9815.7013.0817.3614.46175.004.175.904.9211.759.7910.919.0913.3911.1617.6214.69185.644.705.324.4311.519.5910.478.7216.1213.4316.4613.72194.413.685.354.4610.058.389.127.6013.1010.9215.0912.58204.293.583.603.009.768.138.967.4611.099.2414.5512.13

分析可知：在不同節(jié)點數(shù)量和不同噪聲水平下,EKL相比傳統(tǒng)MDS法在一定程度上降低了節(jié)點的定位誤差,但與傳統(tǒng)MDS和EKL相比,本文提出的MDS結合KL法的傳感器網(wǎng)絡定位方法顯著降低了定位誤差.不同噪聲水平及不同傳感器網(wǎng)絡節(jié)點數(shù)量下,基于MDS法的傳感器網(wǎng)絡節(jié)點定位誤差最高接近20%,基于EKL法的定位誤差在7.46%～13.13%范圍內(nèi)變化,而本文提出的MDS和KL聯(lián)合定位方法誤差始終控制在8%以內(nèi),表明此方法可有效應用于傳感器網(wǎng)絡下機器人的定位.

3 結語

本文提出了一種基于MDS和KL濾波器相結合的機器人傳感器網(wǎng)絡節(jié)點有效定位跟蹤算法.使用4個已知位置的信標,可實現(xiàn)對任意位置處節(jié)點的定位.利用MDS提供了所有傳感器節(jié)點的成對距離矩陣,針對MDS法定位中受到噪聲的影響,特別是當傳感器節(jié)點遠離其他傳感器節(jié)點時,噪聲水平會增加,引入KL濾波器有效降低了噪聲引起的定位干擾.通過仿真實驗驗證了該算法的有效性,表明該算法在定位精度上優(yōu)于傳統(tǒng)的MDS和EKL跟蹤算法，同時，該算法有效地降低了實際環(huán)境中的噪聲影響,具有很好解決移動傳感器定位問題的潛力,滿足了高精度定位的實際要求.