盧 晨,劉 正

(蘇州工業(yè)園區(qū)服務(wù)外包職業(yè)學(xué)院 信息工程學(xué)院,江蘇 蘇州 215123)

隨著工業(yè)自動(dòng)化的快速發(fā)展,越來(lái)越多的機(jī)器人參與到智能制造中,特別是在搬運(yùn)和傳送等工作簡(jiǎn)單繁瑣需要投入大量人力的崗位上,機(jī)器人發(fā)揮著重要的作用[1-2].機(jī)器人在各生產(chǎn)線上一旦投入使用,且需要調(diào)整新的生產(chǎn)線時(shí),其原有的控制程序?qū)⑿枰罅康木S護(hù)和編程投入,此外,在多機(jī)器人協(xié)同作業(yè)方面,對(duì)機(jī)器人位置的定位是實(shí)現(xiàn)協(xié)同工作的基礎(chǔ),因此,對(duì)機(jī)器人軌跡跟蹤是機(jī)械制造行業(yè)發(fā)展的必然需求[3].

多傳感器數(shù)據(jù)融合由于能從多種傳感器中提取優(yōu)勢(shì)互補(bǔ)的數(shù)據(jù),具有更高的精度和更好的魯棒性,已廣泛應(yīng)用于車輛定位、組合導(dǎo)航、目標(biāo)跟蹤等工程實(shí)踐中[4-5].在多傳感器數(shù)據(jù)融合算法中,由于卡爾曼濾波結(jié)合信息濾波實(shí)現(xiàn)簡(jiǎn)單,廣泛應(yīng)用于處理多傳感器數(shù)據(jù).該算法能很好地處理線性系統(tǒng)的數(shù)據(jù)融合,但對(duì)于非線性系統(tǒng)效果欠佳[6-7].針對(duì)非線性系統(tǒng)的融合,Lee等[8]提出了無(wú)跡信息過(guò)濾器(Unscented Information Filtering，UIF),該濾波算法基于Sigma點(diǎn)濾波器的無(wú)跡變換方法從擴(kuò)展信息濾波結(jié)構(gòu)中獲得,可直接對(duì)非線性系統(tǒng)進(jìn)行處理,避免了線性化誤差,能夠?qū)崿F(xiàn)多傳感器估計(jì)的信息濾波器的有效性.無(wú)跡信息濾波被大量應(yīng)用于非線性系統(tǒng)中[9-10],然而,隨著傳感器數(shù)量的增多,如何對(duì)多傳感器之間的數(shù)據(jù)進(jìn)行有效融合,提高多傳感器的測(cè)量精度,成為需要研究的問(wèn)題.交互多模型(Interacting Multiple Model，IMM)是用于機(jī)器人跟蹤最常用的多模型算法之一[11],IMM濾波器由多個(gè)并行運(yùn)行的濾波器組成,利用模型概率將每個(gè)濾波器的狀態(tài)估計(jì)和協(xié)方差結(jié)合起來(lái).數(shù)據(jù)融合與IMM濾波器的一個(gè)缺點(diǎn)是它不使用以前的組合估計(jì)來(lái)預(yù)測(cè)下一個(gè)狀態(tài)和協(xié)方差,但這是常規(guī)信息過(guò)濾器計(jì)算信息所需要的[12].

本文利用UIF和IMM各自的優(yōu)點(diǎn),提出了一種基于分布式多傳感器融合結(jié)構(gòu)下的信息融合算法(Unscented Information Filtering and Interacting Multiple Models，UIF-IMM).該算法融合了每個(gè)由IMM濾波器組成的UIF信息,而不是IMM濾波器簡(jiǎn)單的組合估計(jì),融合的信息不僅是信息狀態(tài)貢獻(xiàn)和信息矩陣,而且是模式似然函數(shù),在實(shí)現(xiàn)機(jī)器人定位方面具有較好的特性.

1 UIF

采用UIF代替線性信息濾波器,UIF在擴(kuò)展信息濾波體系結(jié)構(gòu)中，嵌入了一種源于Sigma點(diǎn)濾波器的無(wú)跡變換方法,其非線性離散時(shí)間狀態(tài)空間模型定義如下:

(1)

式中:xk為狀態(tài)向量;zk為測(cè)量向量；wk，vk分別為過(guò)程噪聲和零均值高斯序列的測(cè)量噪聲,相應(yīng)的協(xié)方差矩陣分別為Qk和Rk.

利用當(dāng)前估計(jì)狀態(tài)(xk|k)和協(xié)方差(Pk|k)計(jì)算一系列Sigma點(diǎn),以預(yù)測(cè)下一幀的狀態(tài)和協(xié)方差，即

(2)

式中:χ，W分別為2n+1個(gè)Sigma點(diǎn)和權(quán)重,通過(guò)縮放參數(shù)κ計(jì)算.

對(duì)Sigma點(diǎn)的狀態(tài)進(jìn)行傳播,利用無(wú)跡變換計(jì)算出下一時(shí)刻狀態(tài)和協(xié)方差為

(3)

(4)

(5)

信息狀態(tài)貢獻(xiàn)ik+1和關(guān)聯(lián)信息矩陣Ik+1計(jì)算公式為

(6)

(7)

如果多個(gè)傳感器獲得的測(cè)量結(jié)果是同步的,則狀態(tài)將與所有測(cè)量的疊加向量同時(shí)更新.在這種情況下,信息貢獻(xiàn)項(xiàng)在回波和測(cè)量矩陣方面具有組對(duì)角線結(jié)構(gòu).因此,多傳感器估計(jì)和數(shù)據(jù)融合的更新方程被表示為局部信息貢獻(xiàn)項(xiàng)的線性組合，即

(8)

式中:m為傳感器數(shù)量.

狀態(tài)估計(jì)和誤差協(xié)方差估計(jì)值被計(jì)算為

(9)

2 UIF-IMM

將UIF方法和IMM相結(jié)合,提出UIF-IMM算法.主要思想是使用IMM每一動(dòng)作模式的估計(jì)進(jìn)行數(shù)據(jù)融合,而不是將IMM的估計(jì)簡(jiǎn)單結(jié)合在一起.此外,該算法共享每個(gè)動(dòng)作模式的模型似然函數(shù),以提高多傳感器系統(tǒng)中動(dòng)作模式檢測(cè)的性能.關(guān)于UIF-IMM算法的結(jié)構(gòu)如圖1所示,主要分為交互、濾波和融合3部分.

圖1 UIF-IMM算法結(jié)構(gòu)框圖Fig.1 Block diagram of UIF-IMM algorithm structure

(10)

(11)

(12)

預(yù)測(cè)狀態(tài)和誤差協(xié)方差計(jì)算為

(13)

利用預(yù)測(cè)狀態(tài)和誤差協(xié)方差計(jì)算出信息狀態(tài)向量和Fisher信息矩陣為

(14)

預(yù)測(cè)觀測(cè)向量、回波協(xié)方差和交叉協(xié)方差分別被預(yù)測(cè)為

(15)

假設(shè)測(cè)量是同步得到的,則將IMM濾波器的每個(gè)動(dòng)作模式的信息狀態(tài)向量和信息矩陣更新為局部信息貢獻(xiàn)項(xiàng)的線性組合，即

(16)

各傳感器動(dòng)作模式的狀態(tài)估計(jì)和誤差協(xié)方差為

(17)

對(duì)每個(gè)局部傳感器節(jié)點(diǎn)的狀態(tài)和狀態(tài)協(xié)方差進(jìn)行上述估計(jì)后,利用模型概率對(duì)狀態(tài)和狀態(tài)協(xié)方差進(jìn)行組合,同時(shí)，利用IMM濾波器中的模型似然函數(shù)計(jì)算模型概率,IMM中每個(gè)動(dòng)作模式的似然函數(shù)為

(18)

由于式(18)中的模型似然函數(shù)是通過(guò)局部測(cè)量和局部測(cè)量單元的估算計(jì)算的,因此，如果有多個(gè)傳感器測(cè)量可用,可以通過(guò)融合其他局部模型似然函數(shù)來(lái)更新.每個(gè)動(dòng)作模式的更新局部似然函數(shù)表示為累積似然函數(shù)，即

(19)

使用更新后的模型似然函數(shù)計(jì)算模型概率為

(20)

利用模型概率組合了每一種動(dòng)作模式的狀態(tài)估計(jì)和協(xié)方差估計(jì)，即

(21)

3 仿真結(jié)果

3.1 仿真條件

將UIF-IMM算法應(yīng)用于機(jī)器人定位系統(tǒng)中.機(jī)器人移動(dòng)速度為0.3 m/s,其運(yùn)行軌跡如圖1所示,軌跡規(guī)劃目標(biāo)如表1所示,采樣時(shí)間為0.1 s.

圖2 機(jī)器人運(yùn)動(dòng)軌跡Fig.2 Robot motion track表1 軌跡規(guī)劃目標(biāo)Tab.1 Trajectory planning target

階段開始時(shí)間/s持續(xù)時(shí)間/s轉(zhuǎn)角速率/(rad·s-1)10700.0270404.53110700.0418060-3.05240600.0

3.2 模型和參數(shù)

假定傳感器站點(diǎn)的每個(gè)局部過(guò)濾器都使用由恒速(Constant Velocity,CV)模型和常數(shù)旋轉(zhuǎn)(Constant Turn,CT)模型組成的IMM.

CV模型被定義為

(22)

式中:ξ，η為水平平面上的正交坐標(biāo);vk為零均值高斯白噪聲的過(guò)程噪聲,用于以標(biāo)準(zhǔn)偏差σv=3 mm/s2模擬加速度.

CT模型被定義為

(23)

式中:ω為轉(zhuǎn)角速度;vk為具有零均值高斯白噪聲的過(guò)程噪聲,其標(biāo)準(zhǔn)偏差分別為σva=1.6 mm/s2σvω=0.2 rad·s-2.

3.3 仿真結(jié)果

從具有100次蒙特卡羅仿真實(shí)驗(yàn)中獲得機(jī)器人跟蹤估計(jì)誤差.為了觀測(cè)模型似然函數(shù)的影響,本文選擇了兩種UIF-IMM設(shè)計(jì)方案,UIF-IMM 1表示除式(20)外的IMM濾波器,而UIF-IMM 2則完全將UIM和IMM融合.為了驗(yàn)證該融合算法的有效性,本文對(duì)單傳感器系統(tǒng)和集中式融合系統(tǒng)的跟蹤結(jié)果進(jìn)行了對(duì)比分析.機(jī)器人位置估計(jì)誤差、角度跟蹤誤差分別如圖3和圖4所示,其統(tǒng)計(jì)結(jié)果如表2所示.

圖3 位置跟蹤誤差Fig.3 Position tracking error

圖4 角度跟蹤誤差Fig.4 Angle tracking error表2 估計(jì)誤差Tab.2 Error of estimation

誤差單傳感器多傳感器集中式UIF-IMM 1UIF-IMM 2RMS位置誤差0.1580.0590.0680.047RMS角度誤差2.11.21.20.9

分析圖3和圖4可知：采用多傳感器對(duì)機(jī)器人跟蹤具有更高的精度,因?yàn)槠浔葐蝹鞲衅鞲櫹到y(tǒng)可分析處理更多的測(cè)量結(jié)果,一定程度上避免噪聲及外部干擾引起的誤差.此外,在分布式傳感器節(jié)點(diǎn)(UIF-IMM 2)中,采用模型似然函數(shù)組合的多傳感器融合算法,其位置精度和角度精度均優(yōu)于不進(jìn)行組合的多傳感器融合算法(UIF-IMM 1).在4種目標(biāo)跟蹤算法中,本文提出的算法定位誤差最小,跟蹤效果最好.

由表2可知：基于UIF-IMM 2的均方根位置誤差和角度誤差均最小,分別為0.047和0.9.而UIF-IMM 1的均方根誤差與集中式融合系統(tǒng)的均方根誤差幾乎是重疊的,表明除了狀態(tài)和狀態(tài)協(xié)方差外,還應(yīng)該采用模型似然函數(shù)的融合,以防止在分布式系統(tǒng)融合過(guò)程中的信息丟失.

進(jìn)一步對(duì)模型概率估計(jì)結(jié)果進(jìn)行了研究,以對(duì)比UIF-IMM 2與UIF-IMM 1兩種方法,找出提高位置和角度精度的原因.如圖5所示,UIF-IMM 2考慮了基于各種傳感器的累積似然函數(shù),使濾波器能夠更精確地估計(jì)模型概率,從而對(duì)狀態(tài)和協(xié)方差進(jìn)行更精確的融合和估計(jì).

圖5 模型概率Fig.5 Model probability

4 結(jié)語(yǔ)

本文提出了一種新的機(jī)器人多傳感器數(shù)據(jù)融合算法,該算法將UIF算法與IMM算法相結(jié)合,實(shí)現(xiàn)了分布式系統(tǒng)中的多傳感器數(shù)據(jù)融合(UIF-IMM).為了提高動(dòng)作模式檢測(cè)和變化的性能以及跟蹤精度,在UIF-IMM中提出了一種融合IMM濾波器模型似然函數(shù)的方法.對(duì)比不同算法下的機(jī)器人跟蹤結(jié)果表明：本文提出的濾波方法可以很好地解決分布式多傳感器環(huán)境下機(jī)器人的跟蹤問(wèn)題.