虛擬陀螺改進卡爾曼濾波設計

呂琦煒，鮑其蓮

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

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

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

1 誤差模型及卡爾曼濾波算法簡介

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

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

對陀螺陣列的各陀螺建立其隨機誤差模型如下：

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

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

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

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

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

其中，

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

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

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

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

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

ρ=0表示兩個陀螺完全不相關；

ρ=±1表示兩個陀螺最大正（負）相關；

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

虛擬陀螺的重要特點即為陀螺陣列中各個陀螺之間存在的相關性。如式（12）和式（13）所示。

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

在實際虛擬陀螺設計時，通過統(tǒng)計其相關性、推導其相關函數(shù)，測量其相關時間，進而得到陀螺間的相關系數(shù)。

2 仿真結果

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

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

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

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

3 結 論

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

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

5 結 論

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

[1]烏蘭巴根，胡繼忠，徐元銘.無人直升機在移動目標上的自主降落[J].計算機工程與應用，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].計算機應用研究，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].航空學報，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結合 Kalman預測的實時性視覺跟蹤算法研究[J].計算機應用，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.