無跡信息濾波耦合交互式多模型的多傳感器機器人軌跡控制

盧 晨,劉 正

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

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

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

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

1 UIF

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

(1)

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

利用當前估計狀態(tài)(xk|k)和協(xié)方差(Pk|k)計算一系列Sigma點,以預測下一幀的狀態(tài)和協(xié)方差，即

(2)

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

對Sigma點的狀態(tài)進行傳播,利用無跡變換計算出下一時刻狀態(tài)和協(xié)方差為

(3)

(4)

(5)

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

(6)

(7)

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

(8)

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

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

(9)

2 UIF-IMM

將UIF方法和IMM相結(jié)合,提出UIF-IMM算法.主要思想是使用IMM每一動作模式的估計進行數(shù)據(jù)融合,而不是將IMM的估計簡單結(jié)合在一起.此外,該算法共享每個動作模式的模型似然函數(shù),以提高多傳感器系統(tǒng)中動作模式檢測的性能.關于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)

預測狀態(tài)和誤差協(xié)方差計算為

(13)

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

(14)

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

(15)

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

(16)

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

(17)

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

(18)

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

(19)

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

(20)

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

(21)

3 仿真結(jié)果

3.1 仿真條件

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

圖2 機器人運動軌跡Fig.2 Robot motion track表1 軌跡規(guī)劃目標Tab.1 Trajectory planning target

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

3.2 模型和參數(shù)

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

CV模型被定義為

(22)

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

CT模型被定義為

(23)

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

3.3 仿真結(jié)果

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

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

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

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

分析圖3和圖4可知：采用多傳感器對機器人跟蹤具有更高的精度,因為其比單傳感器跟蹤系統(tǒng)可分析處理更多的測量結(jié)果,一定程度上避免噪聲及外部干擾引起的誤差.此外,在分布式傳感器節(jié)點(UIF-IMM 2)中,采用模型似然函數(shù)組合的多傳感器融合算法,其位置精度和角度精度均優(yōu)于不進行組合的多傳感器融合算法(UIF-IMM 1).在4種目標跟蹤算法中,本文提出的算法定位誤差最小,跟蹤效果最好.

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

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

圖5 模型概率Fig.5 Model probability

4 結(jié)語

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