江瀟，何波賢，鄭鐵山，岳明

（92724部隊，青島266108）

引 言

在三軸視覺跟蹤慣性穩(wěn)定控制系統(tǒng)中，保持視覺穩(wěn)定的關(guān)鍵技術(shù)在于隔離因外界因素引起的抖動。慣性穩(wěn)定系統(tǒng)的核心是檢測角運動敏感器件三軸陀螺，對轉(zhuǎn)臺姿態(tài)擾動十分敏感，一旦發(fā)生擾動立刻反饋到控制系統(tǒng)[1]。在實際系統(tǒng)的測量中，由于三軸陀螺本身存在零點漂移誤差以及隨機誤差，采用多傳感器信息融合技術(shù)處理慣性穩(wěn)定姿態(tài)信號，把三軸陀螺、加速度計、磁極三個不相干的三維矢量進行互補融合，致力提高慣性穩(wěn)定系統(tǒng)測量精度[2]。

1 系統(tǒng)框架設(shè)計

三軸視覺跟蹤穩(wěn)定平臺的姿態(tài)測量系統(tǒng)主要由三軸陀螺、三軸加速度計、三軸磁計、溫度傳感器和STM32系列微處理器組成。MPU-6050集成了三軸陀螺、三軸加速度計、溫度傳感器，HMC5883是三軸磁阻傳感器。姿態(tài)測量系統(tǒng)框架結(jié)構(gòu)如圖1所示。

2 基于四元數(shù)卡爾曼濾波器設(shè)計

圖1 姿態(tài)測量系統(tǒng)框架結(jié)構(gòu)

三軸陀螺的主要測量載體是三維角速度，三軸加速度計的主要測量載體三維重力加速度，三軸磁計用來測量三維地磁強度。其中三軸加速計和三軸磁阻傳感器提供了兩個不相關(guān)的三維矢量，并且這些矢量可以作為觀測矩陣來校正因為三軸陀螺引起的誤差[3]。四元數(shù)卡爾曼濾波器結(jié)合這三種傳感器的特點，進行數(shù)據(jù)互補融合，以三軸陀螺輸出的角速度為狀態(tài)更新，以重力加速度和磁場強度作為觀測方程的更新，估計出精度高、穩(wěn)定性強的姿態(tài)角[4]。

2.1 狀態(tài)量隨時間更新矩陣

基于四元數(shù)的卡爾曼濾波器是以四元數(shù)為狀態(tài)量，所以設(shè)狀態(tài)參數(shù)為：

卡爾曼濾波器狀態(tài)方程是基于三軸陀螺四元數(shù)輸出進行設(shè)計，建立四元數(shù)的狀態(tài)方程為四元數(shù)的微分方程：

采用基于定時采樣的畢卡三階逼近法將此四元數(shù)微分方程展開，已知四元數(shù)的畢卡三階逼近法形式為：

Δθy、Δθz同理可求，其中：Δθx、Δθy、Δθz為 三 軸 陀 螺 在[k，k＋1]采樣時間內(nèi)的角增量。

基于四元數(shù)的微分方程矩陣形式為：

將式（3）代入四元數(shù)的微分方程矩陣，得到狀態(tài)變量隨時間更新的矩陣為：

2.2 觀測方程更新

把三軸加速度計以及三軸磁計作為卡爾曼濾波器觀測方程的輸入，那么觀測方程的更新就是基于加速度計和磁計數(shù)據(jù)的更新，也就是把三軸加速度計和三軸磁計的本體坐標(biāo)系上的重力加速度和地磁場的參考矢量，通過四元數(shù)轉(zhuǎn)移矩陣轉(zhuǎn)移到當(dāng)前坐標(biāo)系上進行處理。其中四元數(shù)轉(zhuǎn)移矩陣為[5]：

假設(shè)三維參考向量V＝[VxVyVz]轉(zhuǎn)移到旋轉(zhuǎn)當(dāng)前坐標(biāo)系中可由觀測方程表示：

由于上式是非線性方程，所以對h進行求取雅可比矩陣，也就是h對X偏導(dǎo)的雅可比矩陣：

當(dāng)重力加速度（相對加速度靜止水平放置時）觀測更新時有：

當(dāng)磁場觀測（相對磁阻傳感器靜止水平放置并航向指向正北時）更新時有：

把式（8）、式（9）代入式（7），得到基于三軸加速度計以及三軸磁計的觀測方程：

式中：α為磁傾角。

2.3 定姿算法

觀測方程進行了離散線性化處理后，得到了狀態(tài)更新的方程，并且系統(tǒng)的過程噪音Wk-1和系統(tǒng)的量測噪聲Vk滿足上述的要求，則卡爾曼濾波方程滿足以下求解過程。

預(yù)測狀態(tài)：

狀態(tài)估計：

由于式（12）并不是離散的，必須采用以下離散連續(xù)的公式進行代替：

用陀螺或者加速度計以及磁計來計算姿態(tài)角，陀螺的特性就是動態(tài)性能比較好，但是較長時間內(nèi)存在零點漂移，使誤差結(jié)果放大；而加速度計以及磁計靜態(tài)性能較好，但是運動時就會引起較大的噪音，既不能全依靠陀螺計算的值，也不可以完全依靠加速度計和磁計計算出來的數(shù)值，只能利用傳感器的互補信息進行相互補償，用陀螺計算出來的姿態(tài)角，以及加速度計和磁計計算出來的姿態(tài)角進行作差，所以f（X（k））就是加速計和磁計計算出來的角度，也就是三軸加速度計和磁計測量出來的數(shù)值。HkX（k）就是陀螺在上一次誤差四元數(shù)計算出來的歐拉角。

濾波增益：

預(yù)測誤差方差陣：

估計誤差協(xié)方差：

卡爾曼濾波是一種遞推的過程，該算法充分利用陀螺輸出的角速度信號以及加速度計、磁計信號互補特性[6]。當(dāng)運動時，以陀螺的動態(tài)特性為主；靜止時，以加速度計、磁計的靜態(tài)特性為主。權(quán)衡參數(shù)主要由增益矩陣決定，得到對狀態(tài)輸出的最優(yōu)估計，增益矩陣的特性主要依據(jù)當(dāng)前更新估計以及前次狀態(tài)系統(tǒng)估計的統(tǒng)計特性得到，減小了系統(tǒng)計算量。

3 程序設(shè)計

基于四元數(shù)卡爾曼濾波器設(shè)計的三軸視覺跟蹤慣性穩(wěn)定系統(tǒng)程序流程。主要的處理步驟如下：

①系統(tǒng)的初始化主要包括主控STM32、傳感器MPU6050、HMC5883初始化。

②首先在三軸慣性系統(tǒng)保持靜止時，通過采集3 200組數(shù)據(jù)進行零位自動標(biāo)定，同時完成卡爾曼濾波器誤差協(xié)方差矩陣P、噪聲協(xié)方差矩陣Q、方差強度矩陣R的初始化；然后通過采集三軸加速度以及三軸磁場強度計算出系統(tǒng)的初始姿態(tài)；再把東—北—天坐標(biāo)系下的磁場強度初始化；最后，把歐拉角形式的初始化姿態(tài)轉(zhuǎn)化成四元數(shù)形式，實現(xiàn)四元數(shù)卡爾曼濾波器的初始化。

③采集三軸陀螺10個數(shù)據(jù)，取平均值，然后對四元數(shù)的卡爾曼濾波器隨時間預(yù)測更新；然后采集三軸加速度以及三軸磁場強度的數(shù)值，對卡爾曼濾波器的觀測矩陣進行更新。

④當(dāng)加速度過大時，不是進行數(shù)據(jù)融合，而是進行誤差協(xié)方差矩陣P更新；當(dāng)判斷加速度在合適的范圍時，進行卡爾曼數(shù)據(jù)融合。

⑤完成卡爾曼更新后，通過UD分解確保誤差協(xié)方差P不發(fā)散，最后把四元數(shù)轉(zhuǎn)換成歐拉角形式。

程序流程如圖2所示。

圖2 程序流程圖

4 對比實驗

為了檢測這種算法輸出的姿態(tài)精度，利用一款工業(yè)級的微垂直陀螺系統(tǒng)進行對比實驗。微垂直陀螺系統(tǒng)是一款高性能陀螺系統(tǒng)，基于三軸陀螺儀和三軸加速度計設(shè)計的，采用自適應(yīng)卡爾曼數(shù)據(jù)融合算法，以100 Hz更新速率輸出載體的慣性運動信息（三軸角速度、三軸加速度）、最優(yōu)姿態(tài)角（橫滾角和俯仰角）和溫度等參數(shù)，并對傳感器的安裝誤差、軸間正交誤差和溫度誤差進行有效補償。

實驗主要采用三軸搖擺臺進行測試，首先把陀螺系統(tǒng)安裝在三軸搖擺臺的內(nèi)框上，如圖3所示，然后對轉(zhuǎn)臺進行轉(zhuǎn)動，通過轉(zhuǎn)臺上的1024光柵碼盤計算出轉(zhuǎn)臺轉(zhuǎn)過的角度并傳遞到上位機，上位機同時采集陀螺和碼盤的數(shù)據(jù)進行對比。

圖3 三軸搖擺臺

本實驗主要采用俯仰角的數(shù)據(jù)進行對比。圖4為微垂直陀螺系統(tǒng)輸出的俯仰角以及碼盤輸出的角度，它的優(yōu)點就是跟蹤性能比較好，但是波峰相差比較大，最大的幅度差在20°左右，主要原因就是自適應(yīng)卡爾曼濾波器的參數(shù)沒有進行優(yōu)化，把波形的高通部分當(dāng)作噪音來處理，所以該類型的陀螺系統(tǒng)性能并不是很好，存在很多改進的可能。

圖4 微垂直陀螺數(shù)據(jù)

由圖5可知，基于四元數(shù)卡爾曼濾波器設(shè)計的三軸陀螺系統(tǒng)的性能更具有優(yōu)勢，角度誤差在±2°范圍，還增加了三軸磁計融合陀螺方位角信息，可滿足三軸跟蹤轉(zhuǎn)臺全方位測量要求。

結(jié) 語

采用四元數(shù)擴展卡爾曼濾波算法把三軸陀螺、加速度、磁阻傳感器的誤差特性進行相互擬合、相互補償，不僅提高了姿態(tài)解算的精度，而且更新速率快、實時性強。目前，該測試系統(tǒng)已用于視覺目標(biāo)跟蹤系統(tǒng)進行測試，滿足了三軸視覺跟蹤穩(wěn)定系統(tǒng)對姿態(tài)控制測量的需求。

圖5 陀螺數(shù)據(jù)

[1]辛哲奎，方勇純，張雪波.小型無人機地面目標(biāo)跟蹤系統(tǒng)機載云臺自適應(yīng)跟蹤控制[J].控制理論與應(yīng)用，2010，27（8）：1002-1003.

[2]呂漫麗，孫靈芳.多傳感器信息融合技術(shù)[J].自動化技術(shù)與應(yīng)用，2008，2（2）：79-80.

[3]秦永元.慣性導(dǎo)航[M].2版.北京：科學(xué)出版社，2014.

[4]秦永元，張洪鉞，汪叔華.卡爾曼濾波與組合導(dǎo)航原理[M].2版.西安：西北工業(yè)大學(xué)出版社，2012.

[5]徐梓皓，周召發(fā)，孫立江.基于卡爾曼濾波器的姿態(tài)角測量系統(tǒng)設(shè)計[J].電子測量技術(shù)，2014，37（1）：1-2.

[6]RuanJianS-guo，LinJiaS-jun.Analysis on stability of digital protectional gorithms[J].Proceedings of 2005 IEEE International conference on information acquisition，2005：313-317.