蒲曉湘1，周世紀(jì)

(1．重慶電力高等?？茖W(xué)校，重慶 400053；2.重慶大學(xué) 電氣工程學(xué)院，重慶 400044)

姿態(tài)儀主要用于飛機、導(dǎo)彈和火箭系統(tǒng)等飛行器航行參數(shù)的監(jiān)控，如幾何角度測量、速度、加速度等慣性量的測量，也廣泛應(yīng)用于航模無人機、機器人、人體運動分析等需要低成本、高動態(tài)三維姿態(tài)測量的產(chǎn)品設(shè)備中。

本文在傳統(tǒng)姿態(tài)儀基礎(chǔ)上，引入了卡爾曼濾波算法，設(shè)計了一種新型航姿系統(tǒng)，大大提高了測量和控制精度。它可實時提供載體運動的姿態(tài)角(航向、俯仰角和滾動角)、角速率、加速度等信息，其還具有體積小、質(zhì)量輕、抗沖擊振動、性價比高和供電方便等特點，可滿足航空、航天車載等的測量及控制要求。該姿態(tài)儀在標(biāo)準(zhǔn)轉(zhuǎn)臺上進(jìn)行了測試，并用MATLAB仿真驗證了本文所用算法的正確性和有效性。

1 硬件設(shè)計及分析

1.1 硬件組成

如圖1所示，航姿儀的硬件部分由慣性測量單元(IMU)、輸入接口、輸出接口、導(dǎo)航計算機和二次電源等部分組成。

圖1 航姿儀的硬件組成框圖

1.2 IMU選擇

1.2.1 加速度計的設(shè)計

動態(tài)姿態(tài)組合儀技術(shù)指標(biāo)中，姿態(tài)精度要求小于0.1°。因此，根據(jù)系統(tǒng)姿態(tài)初始對準(zhǔn)原理，可得到加速度計的精度估算公式。

(1)

式中：δθ為初始姿態(tài)誤差；Δ為加速度計誤差；g為重力加速度。根據(jù)式(1)可得到系統(tǒng)對加速度計的精度要求為

Δ<1.72 mg

(2)

對誤差進(jìn)行軟件補償，可以使加速度的精度達(dá)到1 mg，滿足系統(tǒng)對加速度計的精度要求。

1.2.2 陀螺儀的設(shè)計

姿態(tài)更新算法中，陀螺儀的測量精度直接影響系統(tǒng)的姿態(tài)精度。加速度計的精度若達(dá)到1 mg，可估算出姿態(tài)初始對準(zhǔn)誤差為

(3)

由于系統(tǒng)對姿態(tài)精度要求為0.1°，可以估算出陀螺的精度要求為

δθ=εt<0.04°

(4)

式中：ε為陀螺儀的精度；t為卡爾曼濾波修正周期。該動態(tài)姿態(tài)組合儀中，卡爾曼濾波周期為0.05 s，考慮到修正后的誤差，初步要求陀螺的精度為

ε<0.08°/s

(5)

1.3 導(dǎo)航計算機的選擇

姿態(tài)儀更新算法和各種濾波算法都由導(dǎo)航計算機完成。導(dǎo)航計算機是制導(dǎo)裝置的中心控制器，它控制A/D轉(zhuǎn)換、攝取數(shù)據(jù)，以及進(jìn)行數(shù)據(jù)預(yù)處理和姿態(tài)計算。它的工作量相當(dāng)繁重，且工作周期短，要在一個周期內(nèi)完成各項控制任務(wù)，并進(jìn)行十分復(fù)雜的卡爾曼濾波解算，這就要求計算機運行速度快。根據(jù)這些要求，僅單獨使用現(xiàn)有的單片機和DSP難以滿足上述復(fù)雜的控制和運算，實驗證明，在此情況下采用基于DSP和C8051單片機的主從式雙CPU導(dǎo)航計算機可以勝任該項工作。

1.4 輸入/輸出接口

輸入接口主要是將IMU輸出的原始信息轉(zhuǎn)換成數(shù)字信號和外部的輔助信息送入導(dǎo)航計算機，輸出接口主要是將導(dǎo)航計算機計算出的導(dǎo)航信息傳送給上位機，此接口按技術(shù)協(xié)議采用RS422方式與上位機通信，具體通信協(xié)議按用戶要求進(jìn)行。

加速度計和陀螺的信號要輸入到導(dǎo)航計算機，就需要把模擬量轉(zhuǎn)換為數(shù)字量，考慮系統(tǒng)對信號轉(zhuǎn)換的精度要求較高，本文選用了16位高速A/D轉(zhuǎn)換器。

2 導(dǎo)航解算原理基本原理

姿態(tài)儀的IMU部分由3個速率陀螺和3個加速度計組成。陀螺測得的載體角速度信息和加速度計測得的載體加速度信息由數(shù)據(jù)采集器進(jìn)行模/數(shù)轉(zhuǎn)換后被送入導(dǎo)航計算機。導(dǎo)航計算機對采樣數(shù)據(jù)進(jìn)行預(yù)處理，且利用這些數(shù)據(jù)進(jìn)行載體姿態(tài)更新，然后通過融合機載主慣導(dǎo)信息不斷修正純慣導(dǎo)時間累積的誤差，滿足長時間高精度測量需要，制導(dǎo)解算原理及過程如圖2所示。

圖2 導(dǎo)航解算原理框圖

3 軟件設(shè)計及分析

3.1 陀螺誤差補償

姿態(tài)儀的軟件采用模塊化設(shè)計，包括輸入模塊、輸出模塊、誤差補償模塊、姿態(tài)更新計算模塊和初始對準(zhǔn)模塊。最核心的就是誤差補償模塊、姿態(tài)更新計算模塊，本文主要對誤差補償模塊作詳細(xì)闡述。

誤差補償模塊輸入主要來自陀螺信號和加速度信號。

對陀螺的誤差補償主要分為兩大部分：陀螺信號的初始補償和卡爾曼濾波動態(tài)補償。其補償原理如圖3所示。

陀螺信號的初始補償主要補償陀螺的零位誤差、比例系數(shù)和安裝誤差引起的交叉耦合。其補償?shù)臄?shù)學(xué)模型為

(6)

陀螺信號的動態(tài)補償，主要是通過卡爾曼濾波技術(shù)對陀螺的漂移進(jìn)行實時的估計，并且實時地進(jìn)行補償。在卡爾曼濾波中，建立的陀螺的誤差模型為

ε=εc+εg+εr

(7)

式中：εc為隨機常值漂移；εg為隨機白噪聲漂移；εr為隨機一階馬爾柯夫過程漂移。

3.2 加速度計誤差補償

對加速度計的誤差補償同樣分為兩大部分：加速度計信號的初始補償和卡爾曼濾波動態(tài)補償。其補償原理如圖4所示。

加速度計信號的初始補償主要補償加速度計的零位誤差、比例系數(shù)和安裝誤差引起的交叉耦合。其補償?shù)臄?shù)學(xué)模型為

(8)

加速度計信號的動態(tài)補償，主要是通過卡爾曼濾波技術(shù)對加速度計的漂移進(jìn)行實時的估計，并且實時地進(jìn)行補償。與陀螺的模型不同，在卡爾曼濾波中，加速度計誤差認(rèn)為由兩部分組成，隨機白噪聲誤差和隨機一階馬爾柯夫過程誤差。

(9)

3.3 卡爾曼濾波算法

首先，根據(jù)位置誤差方程、速度誤差方程、姿態(tài)誤差方程和陀螺、加速度計的隨機誤差方程，建立系統(tǒng)的狀態(tài)方程為

(10)

在深入分析和研究該數(shù)學(xué)模型的基礎(chǔ)上，建立離散的卡爾曼濾波系統(tǒng)模型為

狀態(tài)一步預(yù)測方程為

狀態(tài)估計方程為

濾波增益方程為

Kk+1=Pk+1/kHTk+1(Hk+1Pk+1/kHTk+1+Rk+1)-1

一步預(yù)測均方誤差方程為

估計均方誤差方程為

Pk+1=(I-Kk+1Hk+1)Pk+1/k

4 系統(tǒng)仿真

在標(biāo)準(zhǔn)測試臺上，利用MATLAB軟件對本文設(shè)計的姿態(tài)儀進(jìn)行仿真。

水平零位位置時，x，y，z軸姿態(tài)數(shù)據(jù)如圖5～7所示。從圖中可以看出，在零位位置時，3個軸向數(shù)據(jù)性能指標(biāo)都能達(dá)到0.1°以內(nèi)，滿足要求。

圖5 水平零位位置時，x軸姿態(tài)數(shù)據(jù)

圖6 水平零位位置時，y軸姿態(tài)數(shù)據(jù)

圖7 水平零位位置時，z軸姿態(tài)數(shù)據(jù)

x，y，z軸傾斜15°時，姿態(tài)數(shù)據(jù)如圖8～10所示。從圖中可以看出，通電后，姿態(tài)數(shù)據(jù)經(jīng)過短暫的震蕩后收斂在-15°附近，精度穩(wěn)定在0.1°以內(nèi)，符合設(shè)計要求。

圖8 x軸傾斜15°時姿態(tài)數(shù)據(jù)

圖9 y軸傾斜15°時姿態(tài)數(shù)據(jù)

圖10 z軸傾斜-15°時姿態(tài)數(shù)據(jù)

5 結(jié)語

相對于傳統(tǒng)的航姿儀，本文設(shè)計姿態(tài)儀，在軟件部分的誤差補償模塊中引入卡爾曼濾波，對誤差進(jìn)行實時補償，控制運算精度高，更好地滿足測量及控制要求。該系統(tǒng)硬件部分在標(biāo)準(zhǔn)轉(zhuǎn)臺上進(jìn)行了測試，對本文所采用的算法，利用MATLAB仿真軟件，驗證了算法的正確性和有效性。

參考文獻(xiàn)：

[1] CUYPERS W，VAN GESTELB N，VOET A，et a1． Optical measurement techniques for mobile an d large scale Dimensional metrology [J]．Optics and Lasers in Engineering，2009，47(3-4)：292-300．

[2] 高青，袁亮，吳金強．基于新型LQR的四旋翼無人機姿態(tài)控制人機姿態(tài)控制[J]．制造業(yè)自動化，2014(10)：13-16．

[3] 秦勇，臧希酷，王曉宇，等．基于MEMS慣性傳感器的機器人姿態(tài)檢測系統(tǒng)的研究[J]．傳感技術(shù)學(xué)報，2007，20(2)：298-301．

[4] 秦永元．慣性導(dǎo)航[M]．北京：科學(xué)出版社，2006：297．

[5] 劉興川，張盛，李麗哲，等．基于四元數(shù)的MARG傳感器姿態(tài)測量算法[J]．清華大學(xué)學(xué)報(自然科學(xué)版)，2012，52(5)：627-631．

[6] SUH Y S．Orientation estimation using a quaternion-based indirect Kalman filter with adaptive estimation of external acceleration[J]．IEEE Transactions on Instrumentation and Measurement，2010，59(12)：3 296-3 305.

[7] 徐劍，段哲民.卡爾曼預(yù)測器在目標(biāo)相關(guān)跟蹤中的應(yīng)用[J].計算機仿真，2005，22(11)：120-122.

[8] 宋宇.基于四元數(shù)EKF算法的小型無人機姿態(tài)估計[J].吉林大學(xué)學(xué)報(理學(xué)版)，2015，53(3)：511-518.

[9] 冀亮.基于四元數(shù)的四軸無人機姿態(tài)的估計和控制[J].現(xiàn)代電子技術(shù)，2015，38(11)：112-115.

[10] 楊蒲，李奇.三軸陀螺穩(wěn)定平臺控制系統(tǒng)設(shè)計與實現(xiàn)[J].中國慣性技術(shù)學(xué)報，2007，15(2)：171-176.

[11] LEE B J, PARK J B, JOO Y H, et a1．Intelligent Kalman filter for tracking a manoeuvfing target[J]．Iee Proceedings Radar Sonar & Navigation，2004，151(6)：344-350.

[12] LEE J H，PARK K，YOU K H，et a1.Kalman filter applications to laser interferometer measurements in position-servo systems[J]．Electronics letters，2006，42(17):969-970.