嚴(yán)爾軍,張 強(qiáng)

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

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

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

1 定位算法

1.1 MDS算法

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

(1)

式中:m為坐標(biāo)維度.

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

(2)

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

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

(3)

由于矩陣B的奇異值求解需要用正交三角(QR)分解來(lái)估計(jì)所有特征向量和特征值,運(yùn)算量大,耗時(shí)較長(zhǎng).本文利用快速不動(dòng)點(diǎn)算法來(lái)估計(jì)B的最大特征向量,將特征向量估計(jì)表示為優(yōu)化問(wèn)題，即

(4)

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

(5)

因此,可獲得每個(gè)特征向量對(duì)應(yīng)的特征值為

(6)

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

1.2 卡爾曼濾波

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

(7)

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

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

(8)

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

(9)

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

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

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

(10)

測(cè)量矢量Zk是實(shí)際距離測(cè)量D的雙中心距離平方矩陣為

(11)

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

(12)

(13)

(14)

(15)

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

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

(16)

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

(17)

式中:F=IN2.

(18)

式中:H為單位矩陣IN2.

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

(19)

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

2 實(shí)驗(yàn)結(jié)果

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

(20)

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

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

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

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

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

圖3 不同定位方法下單個(gè)機(jī)器人節(jié)點(diǎn)運(yùn)動(dòng)軌跡Fig.3 Single robot node trajectory under different positioning methods

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

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

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

3 結(jié)語(yǔ)

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