三軸矢量數(shù)據(jù)融合的視覺(jué)跟蹤慣性穩(wěn)定系統(tǒng)設(shè)計(jì)

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

（92724部隊(duì)，青島266108）

引 言

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

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

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

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

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

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

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

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

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

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

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

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

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

2.2 觀測(cè)方程更新

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

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

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

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

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

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

式中：α為磁傾角。

2.3 定姿算法

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

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

狀態(tài)估計(jì)：

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

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

濾波增益：

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

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

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

3 程序設(shè)計(jì)

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

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

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

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

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

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

程序流程如圖2所示。

圖2 程序流程圖

4 對(duì)比實(shí)驗(yàn)

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

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

圖3 三軸搖擺臺(tái)

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

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

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

結(jié) 語(yǔ)

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

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

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

[2]呂漫麗，孫靈芳.多傳感器信息融合技術(shù)[J].自動(dòng)化技術(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)角測(cè)量系統(tǒng)設(shè)計(jì)[J].電子測(cè)量技術(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.