虛擬陀螺改進(jìn)卡爾曼濾波設(shè)計(jì)

呂琦煒，鮑其蓮

（上海交通大學(xué) 儀器科學(xué)與工程系，上海200240）

基于MEMS技術(shù)的微機(jī)械陀螺具有廣闊的應(yīng)用前景，但因其精度普遍偏低，從而限制其應(yīng)用，因此提高M(jìn)EMS陀螺精度非常關(guān)鍵[1]。將多個(gè)微陀螺集成在同一個(gè)硅片上構(gòu)成陀螺陣列，利用這些陀螺之間存在的相關(guān)性，通過軟件濾波算法使陣列中各個(gè)陀螺互相進(jìn)行誤差補(bǔ)償，得到最佳估計(jì)值，可有效降低陀螺的漂移誤差。將該陀螺陣列視作一個(gè)虛擬陀螺，其精度相比原單個(gè)陀螺可獲得顯著的提高。

文中提出一種改進(jìn)的卡爾曼濾波算法，將其應(yīng)用于陀螺陣列與虛擬陀螺輸出估計(jì)中，通過多個(gè)微陀螺的信息融合有效降低了虛擬陀螺輸出隨機(jī)漂移誤差，提高了虛擬陀螺的精度。

1 誤差模型及卡爾曼濾波算法簡(jiǎn)介

本文討論的虛擬陀螺系統(tǒng)方案如圖1所示。

圖1 虛擬陀螺系統(tǒng)結(jié)構(gòu)圖Fig.1 Structure of virtual gyroscope system

對(duì)陀螺陣列的各陀螺建立其隨機(jī)誤差模型如下：

其中yi表示陀螺的實(shí)際輸出；nai表示角速率隨機(jī)白噪聲，方差為Qai。ω表示真實(shí)角速率；bi表示角速率隨機(jī)游走噪聲，其激勵(lì)為方差為Qbi的白噪聲nbi。Qa為角度隨機(jī)游走噪聲矢量na的協(xié)方差陣，Qb為速率隨機(jī)游走噪聲矢量nb的協(xié)方差陣。

將b和ω列為狀態(tài)，可得到系統(tǒng)狀態(tài)矢量X=[bω]T，角度隨機(jī)游走nai作為量測(cè)噪聲矢量，將陀螺陣列實(shí)際輸出矢量作為y系統(tǒng)量測(cè)矢量。由此建立虛擬陀螺的系統(tǒng)狀態(tài)方程和量測(cè)方程如下：

其中，w（t），V（t）為相互獨(dú)立白噪聲，方差分別為 q，r。

Qa為角度隨機(jī)游走噪聲矢量na的協(xié)方差陣，Qb為速率隨機(jī)游走噪聲矢量nb的協(xié)方差陣。

將其離散化，得到離散型系統(tǒng)狀態(tài)方程和量測(cè)方程如下：

其中，

T為離散化周期，Wk為系統(tǒng)驅(qū)動(dòng)白噪聲序列，Vk為量測(cè)噪聲序列。且Wk與Vk相互獨(dú)立。則：

在實(shí)際虛擬陀螺設(shè)計(jì)時(shí)，通過統(tǒng)計(jì)其相關(guān)性、推導(dǎo)其相關(guān)函數(shù)，測(cè)量其相關(guān)時(shí)間，進(jìn)而得到陀螺間的相關(guān)系數(shù)。通常單陀螺濾波算法中Q、R值的選取由Allan隨機(jī)誤差建模法[4-5]決定。對(duì)于多陀螺系統(tǒng)，Q、R的選取無法確定。

本文提出一種改進(jìn)的Q、R值選取方法，如下式所示：

其中：qω為真實(shí)角ω速率建模的白噪聲。

微機(jī)械陀螺在設(shè)計(jì)、加工、檢測(cè)等方面都存在一致性，陀螺之間存在相關(guān)性[1]。引入相關(guān)性系數(shù)ρ來表征陀螺之間的相關(guān)程度：

ρ=0表示兩個(gè)陀螺完全不相關(guān)；

ρ=±1表示兩個(gè)陀螺最大正（負(fù)）相關(guān)；

-1<ρ<1且ρ≠0表示兩陀螺之間存在一定的相關(guān)性。

虛擬陀螺的重要特點(diǎn)即為陀螺陣列中各個(gè)陀螺之間存在的相關(guān)性。如式（12）和式（13）所示。

其中：qω為真實(shí)角ω速率建模的白噪聲。

在實(shí)際虛擬陀螺設(shè)計(jì)時(shí)，通過統(tǒng)計(jì)其相關(guān)性、推導(dǎo)其相關(guān)函數(shù)，測(cè)量其相關(guān)時(shí)間，進(jìn)而得到陀螺間的相關(guān)系數(shù)。

2 仿真結(jié)果

本文首先仿真多陀螺輸出數(shù)據(jù)，采樣時(shí)間為120 s，采樣頻率為100 Hz，假設(shè)多陀螺具有類似特性，其誤差隨機(jī)分布在某一均值附近。例如隨機(jī)常值誤差為100°±20°/h，一階馬爾科夫過程為 70°±20°/h，白噪聲為 45°±20°/h。 圖 2 為單陀螺單軸的測(cè)量真值、測(cè)量輸出與測(cè)量誤差曲線。

對(duì)以上的仿真陀螺儀輸出，進(jìn)行卡爾曼濾波。進(jìn)一步，采用多陀螺構(gòu)成虛擬陀螺，其中取ρ=0.5，按照前文介紹的卡爾曼濾波算法，將多陀螺的輸出進(jìn)行融合。圖3為6陀螺構(gòu)成的虛擬陀螺單軸估計(jì)誤差與單陀螺估計(jì)誤差的對(duì)比圖，可以看出，測(cè)量誤差大為減小，經(jīng)計(jì)算，其殘差方差減小了85%左右。

圖2 單陀螺單軸測(cè)量真值、實(shí)際測(cè)量輸出及測(cè)量誤差Fig.2 Measurement and measurement error of single gyroscope

圖3 單陀螺與虛擬陀螺估計(jì)殘差比較Fig.3 Comparison of estimation errors of single gyroscope and virtual gyroscope

3 結(jié) 論

文中針對(duì)基于多個(gè)MEMS陀螺構(gòu)成虛擬陀螺進(jìn)行研究。利用MEMS陀螺間誤差特性具有一定的相似性，構(gòu)造基于多陀螺陣列的虛擬陀螺并建立虛擬陀螺的誤差模型，提出一種方差估計(jì)方法并推導(dǎo)相應(yīng)的卡爾曼濾波算法；仿真實(shí)驗(yàn)驗(yàn)證，多陀螺構(gòu)成的虛擬陀螺輸出估計(jì)精度獲得較大提高，其輸出估計(jì)殘差與單陀螺相比可減小80%以上。

圖8 相對(duì)位置誤差曲線Fig.8 The relative position error curve

5 結(jié) 論

文中實(shí)現(xiàn)了無人直升機(jī)對(duì)移動(dòng)目標(biāo)的跟蹤，從整體方案設(shè)計(jì)到具體實(shí)現(xiàn)給出了較為詳細(xì)的過程。從實(shí)驗(yàn)結(jié)果可以看出，基于計(jì)算機(jī)視覺引導(dǎo)的方法可以很好地應(yīng)用到小型無人直升機(jī)移動(dòng)目標(biāo)跟蹤，并且充分驗(yàn)證了整個(gè)系統(tǒng)的可行性和魯棒性。文中提出的基于視覺引導(dǎo)方式對(duì)小型無人直升機(jī)移動(dòng)目標(biāo)跟蹤的實(shí)現(xiàn)提供了一種有效的解決方案，并且為下一步實(shí)現(xiàn)無人直升機(jī)移動(dòng)平臺(tái)降落打下基礎(chǔ)。

[1]烏蘭巴根，胡繼忠，徐元銘.無人直升機(jī)在移動(dòng)目標(biāo)上的自主降落[J].計(jì)算機(jī)工程與應(yīng)用，2010，46（23）:227-230.WULAN Ba-gen，HU Ji-zhong，XU Yuan-ming.Autonomous helicopter landing onmoving target[J].Computer Engineering and Applications，2010，46（23）:227-230.

[2]范才智，劉云輝，吳亮，等.無人直升機(jī)基于視覺的靜止點(diǎn)目標(biāo)跟蹤[J].計(jì)算機(jī)應(yīng)用研究，2009，29（2）:492-495.FAN Cai-zhi，LIU Yun-hui，WU Liang，etal.Vision-basedstatic point target tracking of unmanned helicopter[J].Application Research of Computers，2009，29（2）:492-495.

[3]刁燦，王英勛，王金提，等.無人機(jī)自動(dòng)著陸中的機(jī)器視覺輔助技術(shù)[J].航空學(xué)報(bào)，2008（29）:79-84.DIAO Can，WANG Ying-xun，WANG Jinti，et al.Computer vision assisted autonomous landing of UAV[J].Journal of Aeronantics，2008（29）:79-84.

[4]Jonathan Courbon，Youcef Mezouar，Nicolas Gue'nard，Philippe Martinet，Vision-based navigation of unmanned aerial vehicles[J].Control Engineering Practice.，2010（18）:789-799.

[5]Laurent Muratet，Stephane Doncieux，Yves Briere，et al.Acontribution to vision-based autonomous helicopter flight in urban environments[J].Robotics and Autonomous Systems，2005（50）:195-209.

[6]劉繼承，孫慧琳，趙洪濤.基于多特征融合的 Camshift結(jié)合 Kalman預(yù)測(cè)的實(shí)時(shí)性視覺跟蹤算法研究[J].計(jì)算機(jī)應(yīng)用，2014，33（5）:54-57.LIU Ji-cheng，SUN Hui-lin，ZHAO Hong-tao.CamShift based on Multi-Feature fusion combines kalman prediction algorithm for real-Time visual Tracking[J].Computer Applications，2014，33（5）:54-57.

[7]Courbon J，Mezouar Y，Martinet P.Autonomous navigation of vehicles from Avisualmemory using Ageneric cameramodel[J].IEEE Transactions on Intelligent Transportation Systems，2009，10（3）:392-402.

[8]Ivain Fernando Mondragon，Pascual Campoy，Juan Fernando Correa，et al.Visual Model Feature Tracking For UAV Control[C].Conference:Intelligent Signal Processing，2007.