基于最大一致容積Kalman濾波器的 多機(jī)器人協(xié)同定位

于鎮(zhèn)滔，王忠慶，劉 鵬,2

(1.中北大學(xué)電氣與控制工程學(xué)院，太原 030051；2.中北大學(xué)前沿交叉科學(xué)研究院，太原 030051)

0 引言

近年來(lái)，作為一項(xiàng)融合了測(cè)量、通信和計(jì)算等技術(shù)的新型智能測(cè)量單元群，無(wú)線傳感器網(wǎng)絡(luò)(Wi-reless Sensor Networks，WSN)成為了多個(gè)學(xué)科的研究熱點(diǎn)，并且被應(yīng)用于多個(gè)領(lǐng)域。根據(jù)WSN信息的不同傳輸方式，可以分為集中式和分布式。集中式通信在組網(wǎng)結(jié)構(gòu)中通常包含一個(gè)中央融合節(jié)點(diǎn)，在融合計(jì)算時(shí)，該節(jié)點(diǎn)匯集了所有感知節(jié)點(diǎn)采集到的狀態(tài)信息，其缺點(diǎn)是當(dāng)該節(jié)點(diǎn)損壞或出現(xiàn)故障時(shí)，整個(gè)系統(tǒng)將停止運(yùn)行。在分布式通信中，某一節(jié)點(diǎn)僅與其相鄰節(jié)點(diǎn)進(jìn)行信息互換，這種并行計(jì)算方式極大地緩解了單個(gè)傳感器節(jié)點(diǎn)計(jì)算能力不足的問(wèn)題，并提高了WSN系統(tǒng)面對(duì)故障時(shí)的魯棒性。機(jī)器人系統(tǒng)擁有大量傳感器，可視為一種可移動(dòng)的無(wú)線傳感器網(wǎng)絡(luò)。通常所討論的機(jī)器人定位是指通過(guò)所搭載的傳感器來(lái)感知所處環(huán)境的相對(duì)位置、絕對(duì)位置信息以及環(huán)境信息，通過(guò)計(jì)算或信號(hào)處理將環(huán)境信息轉(zhuǎn)換為位置信息，常見(jiàn)如溫度、濕度和光照強(qiáng)度等。機(jī)器人定位系統(tǒng)從包含個(gè)體數(shù)量上可劃分為由單個(gè)機(jī)器人構(gòu)成的定位系統(tǒng)和由多個(gè)機(jī)器人構(gòu)成的多機(jī)器人定位系統(tǒng)。在多機(jī)器人系統(tǒng)中，為降低傳感器數(shù)量在融合計(jì)算中造成的經(jīng)濟(jì)負(fù)擔(dān)，研究人員可通過(guò)減少測(cè)量所需的傳感器種類(lèi)來(lái)實(shí)現(xiàn)機(jī)器人群中個(gè)體間的差異化。所構(gòu)成的多機(jī)器人系統(tǒng)通過(guò)任務(wù)分配和協(xié)同策略并行處理各自問(wèn)題，進(jìn)而提高工作效率。因?yàn)槊總€(gè)機(jī)器人所搭載的傳感器種類(lèi)不同，為傳感器的選型留有較大空間。

單機(jī)器人系統(tǒng)定位的主要方法有全球定位系統(tǒng)、航跡推算法、路標(biāo)定位等。航跡推算是利用機(jī)器人自身攜帶的里程計(jì)和慣性測(cè)量組合，利用上一時(shí)刻速度和航向角的狀態(tài)信息，結(jié)合當(dāng)前時(shí)刻速度和航向角測(cè)量值，估算出下一時(shí)刻的位置信息和航向角信息，但受累積誤差的影響，定位精度會(huì)隨距離和運(yùn)行時(shí)間的增加而不斷下降。為了解決誤差累積問(wèn)題，可以利用路標(biāo)定位或者分布式估計(jì)算法給予其誤差修正。相比單個(gè)機(jī)器人系統(tǒng)，多機(jī)器人系統(tǒng)可以修正已有定位方法，通過(guò)每個(gè)個(gè)體采集動(dòng)態(tài)信息并分享數(shù)據(jù)給其余個(gè)體實(shí)現(xiàn)協(xié)同定位。這種協(xié)作方式極大地減少了單個(gè)機(jī)器人的能耗和對(duì)存儲(chǔ)空間的消耗，并且當(dāng)組內(nèi)個(gè)別機(jī)器人發(fā)生故障時(shí)，整個(gè)系統(tǒng)仍可以繼續(xù)完成任務(wù)，提高了系統(tǒng)的魯棒性。

在多機(jī)器人系統(tǒng)數(shù)據(jù)傳輸和數(shù)據(jù)融合方面，將Kalman濾波算法與一致性思想相結(jié)合的分布式Kalman濾波算法已經(jīng)得到了廣泛的研究。W. Li等利用分布式Kalman濾波實(shí)現(xiàn)了基于絕對(duì)測(cè)量和相對(duì)測(cè)量的協(xié)同定位。之后不同研究者又相繼提出了分布式擴(kuò)展Kalman濾波和分布式無(wú)跡Kalman濾波。隨著芯片級(jí)計(jì)算能力的提升，研究人員將注意力轉(zhuǎn)向了傳感器網(wǎng)絡(luò)環(huán)境下分布式容積Kalman濾波的設(shè)計(jì)。J. Liu等利用五階容積規(guī)則開(kāi)發(fā)了一種分布式的容積Kalman濾波器來(lái)處理高階估計(jì)問(wèn)題，相比三階規(guī)則，五階規(guī)則更加精確和穩(wěn)定。基于Kullback-Leibler散度，C. Hu等提出了一種分布式容積Kalman濾波器來(lái)解決協(xié)同跟蹤空間物體問(wèn)題，實(shí)驗(yàn)結(jié)果表明，分布式容積Kalman濾波器比分布式容積信息濾波器更為準(zhǔn)確，但未給出具體原因。以上所研究的分布式算法多基于平均一致性思想，雖然其極大地減少了集中式算法的計(jì)算量，但無(wú)法在有限時(shí)間內(nèi)實(shí)現(xiàn)精確一致性。為了解決此問(wèn)題，本文將采用最大一致思想設(shè)計(jì)分布式容積Kalman濾波算法。在對(duì)最大一致問(wèn)題的研究中，文獻(xiàn)[12]提出了一種解決方案,減小了在選擇最大狀態(tài)估計(jì)值過(guò)程中加性噪聲對(duì)結(jié)果的影響，提高了算法可靠性。文獻(xiàn)[13]提出了一種用于在有限時(shí)間內(nèi)實(shí)現(xiàn)平均一致性的算法，該算法提高了計(jì)算效率并且不需要在線內(nèi)存。

機(jī)器人系統(tǒng)是一個(gè)典型非線性系統(tǒng)，分析并結(jié)合前面描述的不同定位算法，本文采用最大一致容積Kalman濾波器實(shí)現(xiàn)多機(jī)器人系統(tǒng)的協(xié)同定位。通過(guò)圖論來(lái)刻畫(huà)機(jī)器人節(jié)點(diǎn)間的數(shù)據(jù)傳輸方式，將光電傳感器和偏振光傳感器采集的數(shù)據(jù)進(jìn)行數(shù)據(jù)融合，以減少累積誤差帶來(lái)的影響。

1 問(wèn)題的描述

本節(jié)首先對(duì)單個(gè)機(jī)器人運(yùn)動(dòng)系統(tǒng)進(jìn)行運(yùn)動(dòng)學(xué)建模，然后描述多個(gè)機(jī)器人節(jié)點(diǎn)之間的通信拓?fù)鋱D。

1.1 單機(jī)器人運(yùn)動(dòng)學(xué)模型

本文選擇麥克納姆輪機(jī)器人作為兩類(lèi)傳感器的載體。機(jī)器人在正常運(yùn)行時(shí)，載體系的原點(diǎn)與機(jī)器人的質(zhì)心相重合，并且令機(jī)器人運(yùn)動(dòng)方向?yàn)檩S正方向，平行于地面沿軸正方向順時(shí)針旋轉(zhuǎn)90°得到軸正方向，以軸正方向和軸正方向的叉乘結(jié)果作為軸正方向。使用初始時(shí)刻的載體系作為機(jī)器人在整個(gè)運(yùn)動(dòng)過(guò)程中的全局系。在移動(dòng)過(guò)程中，機(jī)器人所搭載的傳感器對(duì)應(yīng)的測(cè)量坐標(biāo)系與載體系重合。

多機(jī)器人系統(tǒng)中每個(gè)個(gè)體在全局坐標(biāo)系中的位置為=[,,]和姿態(tài)四元數(shù)為=[,,,]，其中為標(biāo)量值，假設(shè)、、分別為三維空間坐標(biāo)系中3個(gè)坐標(biāo)軸正方向上的單位矢量，則、、分別為對(duì)應(yīng)的標(biāo)量值，即=+*+*+*；速度向量可表示為=[,,]，則機(jī)器人的狀態(tài)信息可以表示為=[,,]。時(shí)刻機(jī)器人運(yùn)動(dòng)學(xué)方程如式(1)所示

=(-1)+-1

(1)

其中，函數(shù)(·)如式(2)所示

(2)

其中，-1表示均值為0、方差為-1的過(guò)程噪聲。

在移動(dòng)過(guò)程中，機(jī)器人系統(tǒng)可以通過(guò)傳感器采集自身狀態(tài)信息或環(huán)境信息。但是受設(shè)備精度的影響，測(cè)量值在真實(shí)值上下波動(dòng)。偏振光傳感器可仿照昆蟲(chóng)，利用天空偏振光導(dǎo)航的原理進(jìn)行光學(xué)輔助導(dǎo)航。時(shí)刻偏振光傳感器的測(cè)量方程如式(3)所示

,=+

(3)

其中，為時(shí)刻的航向角；為服從均值為0、方差為的測(cè)量噪聲。光流傳感器的測(cè)量方程如式(4)所示

,=V+

(4)

其中，矩陣表示全局系到載體系的坐標(biāo)變換矩陣；表示均值為0、方差為的測(cè)量噪聲。通常復(fù)雜機(jī)器人系統(tǒng)將式(3)和式(4)的測(cè)量以集中式的方式疊放在一個(gè)觀測(cè)矩陣中。本文將兩種傳感器分別配置于不同的機(jī)器人，這不僅可以降低機(jī)器人的硬件成本，而且有利于減少計(jì)算過(guò)程所需的存儲(chǔ)量。

1.2 多機(jī)器人系統(tǒng)通信描述

假設(shè)多機(jī)器人系統(tǒng)由個(gè)機(jī)器人構(gòu)成，其可以定義為圖=(,)。將系統(tǒng)中的單個(gè)機(jī)器人視為一個(gè)節(jié)點(diǎn)。定義={,,…,}為節(jié)點(diǎn)集合，?×是信息在個(gè)體間傳遞的通道集合，并且定義個(gè)體的鄰居由={|(,)∈}表示。在其鄰居內(nèi)，個(gè)體的入鄰居和個(gè)體的出鄰居如圖1所示。個(gè)體的入鄰居數(shù)量由入度=||決定，而其出鄰居數(shù)量由出度=||決定，其中={|(,)∈}，={|(,)∈}。

圖1 入鄰居和出鄰居定義Fig.1 The definitions of in-neighborhood and out-neighborhood

在多機(jī)器人系統(tǒng)中，傳感器采集的本地信息與入鄰居所傳遞的數(shù)據(jù)進(jìn)行融合，再將融合后的信息傳遞給出鄰居。本文將采用一致性算法實(shí)現(xiàn)此數(shù)據(jù)融合過(guò)程。一致性算法可以大致分為平均一致性和最大一致性。平均一致性是求取數(shù)據(jù)集的平均值，具體形式如式(5)所示

(5)

(6)

其中，(0)表示第個(gè)傳感器的初始值；代表個(gè)體集合。

2 容積Kalman濾波器及分布式形式

為解決多機(jī)器人系統(tǒng)在移動(dòng)過(guò)程中的狀態(tài)估計(jì)問(wèn)題，本文采用基于最大一致的容積Kalman濾波算法予以解決。本節(jié)第一部分簡(jiǎn)單介紹容積Kalman濾波算法，并以其為框架得到容積信息濾波算法；第二部分介紹基于最大一致的容積Kalman濾波算法。

2.1 容積Kalman濾波器

在非線性Kalman濾波的測(cè)量更新時(shí)，狀態(tài)值、誤差協(xié)方差、自相關(guān)協(xié)方差和互相關(guān)協(xié)方差都具有“非線性函數(shù)×高斯密度函數(shù)”形式的高斯積分計(jì)算，具體如下

(7)

其中，函數(shù)()為被積函數(shù)與部分高斯分布函數(shù)的乘積；R為積分域；(·)表示矩陣的轉(zhuǎn)置。

為了解決以上問(wèn)題，I.Arasaratnam和S.Haykin使用三階球面容積法則來(lái)近似高斯積分。具體計(jì)算過(guò)程見(jiàn)參考文獻(xiàn)[15]，利用球面—徑向容積準(zhǔn)則得到如下結(jié)果

(8)

以容積Kalman濾波器為基本框架，采用統(tǒng)計(jì)線性誤差傳播方法，并以近似的方式表示擴(kuò)展信息濾波器中的雅可比矩陣，從而得到容積信息濾波器。

當(dāng)初始化和時(shí)間更新部分不變時(shí)，測(cè)量更新中的誤差協(xié)方差矩陣|-1、自協(xié)方差矩陣,|-1和互協(xié)方差矩陣,|-1可改寫(xiě)如下

(9)

(10)

(11)

其中

=[(|-1),|-1]

=(,|-1)(|-1)

(12)

從而由擴(kuò)展信息濾波器算法可知,容積信息濾波器的信息矩陣和信息向量表示如下

(13)

(14)

(15)

進(jìn)而得到信息矩陣和信息狀態(tài)矢量的更新方程為

(16)

2.2 最大一致容積Kalman濾波算法

由個(gè)異構(gòu)機(jī)器人構(gòu)造的多機(jī)器人系統(tǒng)，每個(gè)個(gè)體的觀測(cè)方程不同，具體表述如下

(17)

由式(17)可以得到等式(13)和式(14)中相關(guān)向量的表達(dá)形式，具體如下：

(18)

表1 最大一致容積Kalman濾波

3 仿真

這一節(jié)通過(guò)麥克納姆輪機(jī)器人驗(yàn)證文中提出算法(記為DMKF)的效果，與其進(jìn)行比較的是基于平均一致容積Kalman濾波器(簡(jiǎn)稱(chēng)DAKF)和集中式容積Kalman濾波器(簡(jiǎn)稱(chēng)JCKF)。

3.1 參數(shù)設(shè)置

對(duì)于集中式容積Kalman濾波算法，其觀測(cè)方程如下

(19)

對(duì)于分布式容積Kalman濾波器，第一組機(jī)器人S1和S2的觀測(cè)方程為

,=+

(20)

其中，=006,=1,2。第二組機(jī)器人S3和S4的觀測(cè)方程為

,=+

(21)

其中，=01,=1,2。令表示狀態(tài)值,初始值為=[0,0,10,0,45°]，初始的協(xié)方差矩陣=diag([1,2,1,2,05°])。機(jī)器人間的信息傳輸關(guān)系如圖2所示。

圖2 異構(gòu)機(jī)器人系統(tǒng)信息傳輸關(guān)系圖Fig.2 Information transmission diagram of heterogeneous robot systems

3.2 結(jié)果與討論

通過(guò)仿真，得到了基于JCKF、DAKF和DMKF算法的多機(jī)器人協(xié)同定位結(jié)果。令機(jī)器人S1的數(shù)據(jù)代表整個(gè)系統(tǒng)的狀態(tài)，其沿預(yù)先設(shè)計(jì)的軌跡運(yùn)動(dòng)。位置狀態(tài)信息估計(jì)的結(jié)果如圖3所示。與真實(shí)軌跡結(jié)果進(jìn)行對(duì)比，軌跡的均方根誤差如圖4所示。本文將方向角的估計(jì)值與真實(shí)值之差作為方向角誤差，如圖5所示。

圖3 定位結(jié)果 Fig.3 Location results

圖4 軌跡誤差對(duì)比Fig.4 Comparison of trajectory errors

圖5 航向角誤差對(duì)比Fig.5 Comparison of heading angle error

由圖3可知，基于DMKF協(xié)同定位所產(chǎn)生的折線與真實(shí)軌跡重合度較高，誤差基本穩(wěn)定在0～12.3533m之間。而對(duì)于DAKF所代表的折線，相較于DMKF所代表的折線其定位效果較差，每段路程中都有較大偏離。在圖4中，前200次定位誤差波動(dòng)劇烈，最大誤差為19.4265m；后200次定位誤差波動(dòng)減小，但沒(méi)有收斂的趨勢(shì)。對(duì)于JCKF所代表的折線，全程僅有幾段定位估計(jì)與真實(shí)軌跡重合，是三組定位中最差的一條。由圖4可知，其定位誤差在前300次迭代中保持上升趨勢(shì)，最大誤差為16.2072m，之后逐漸下降，但誤差依然保持在高位。

從圖5可得，JCKF算法的方向角在整個(gè)定位過(guò)程中誤差較小，誤差范圍基本保持在[-6.9887，5.2885]，但在轉(zhuǎn)彎時(shí)波動(dòng)較大。與此相比，DAKF算法的方向角誤差范圍為[-3.2815，12.2235]，DMKF算法的方向角誤差范圍為[-5.3972，6.5912]，與JCKF算法的方向角誤差范圍相近，但無(wú)較大波動(dòng)。

4 實(shí)物仿真

為了驗(yàn)證算法1在協(xié)同定位中的可行性，本文通過(guò)構(gòu)建機(jī)器人系統(tǒng)進(jìn)行驗(yàn)證。采用麥克納姆輪機(jī)器人作為平臺(tái)，通過(guò)上位機(jī)對(duì)機(jī)器人系統(tǒng)進(jìn)行控制，如圖6所示。運(yùn)行結(jié)束后，取其中一個(gè)機(jī)器人的運(yùn)行數(shù)據(jù)進(jìn)行曲線擬合，如圖7所示。通過(guò)多次測(cè)試，驗(yàn)證了攜帶單一傳感器的多個(gè)機(jī)器人可采用該算法實(shí)現(xiàn)協(xié)同定位。

圖6 麥克納姆輪機(jī)器人和上位機(jī)界面Fig.6 Mecanum wheel robot and PC interface

圖7 實(shí)物仿真軌跡圖Fig.7 Physical simulation trajectory

5 結(jié)論

本文提出了一種配置單一的多機(jī)器人協(xié)同定位策略，該策略采用容積信息濾波算法分別將本地觀測(cè)信息和鄰居觀測(cè)信息進(jìn)行融合。通過(guò)最大一致性思想得到全部個(gè)體的最大信息矩陣和信息向量，經(jīng)過(guò)狀態(tài)更新獲得下個(gè)時(shí)刻的狀態(tài)值。通過(guò)比較集中式容積Kalman濾波算法和平均一致容積Kalman濾波算法，得到以下結(jié)論：

1)基于平均一致容積Kalman濾波算法無(wú)法在有限時(shí)間內(nèi)達(dá)到一致性，導(dǎo)致系統(tǒng)定位誤差較大，而基于最大一致的容積Kalman濾波器正好彌補(bǔ)了這一點(diǎn)。相比集中式容積Kalman濾波算法的軌跡誤差和方向角誤差，基于最大一致的容積Kalman濾波算法在濾波效果和穩(wěn)定性上都有較好的表現(xiàn)，并通過(guò)多次實(shí)物測(cè)試驗(yàn)證了最大一致容積Kalman濾波算法可實(shí)現(xiàn)多機(jī)器人協(xié)同定位。

2)將搭載多個(gè)傳感器的單個(gè)機(jī)器人拆解為多個(gè)攜帶單個(gè)傳感器的異構(gòu)機(jī)器人。通過(guò)任務(wù)分配和數(shù)據(jù)共享，可以降低單個(gè)機(jī)器人的計(jì)算量，并且不會(huì)降低整個(gè)系統(tǒng)的定位性能，這可以降低因?yàn)樽非蟾呔榷ㄎ欢x購(gòu)昂貴傳感器所造成的高額費(fèi)用。