沈翔+郭善鏡

摘 要： 為了改善現(xiàn)有全機(jī)疲勞試驗(yàn)中姿態(tài)監(jiān)控的缺點(diǎn)，采用加速度傳感器、陀螺儀、磁力計(jì)和DSP處理器，通過四元數(shù)法和二階龍格?庫塔法實(shí)時解算出飛機(jī)的姿態(tài)數(shù)據(jù)，然后經(jīng)過擴(kuò)展卡爾曼濾波器對多傳感器數(shù)據(jù)進(jìn)行融合濾波后得到準(zhǔn)確的姿態(tài)數(shù)據(jù)，并通過無線傳輸模塊發(fā)送至上位機(jī)，姿態(tài)數(shù)據(jù)超差后能夠發(fā)出聲光報警提醒試驗(yàn)控制人員。經(jīng)試驗(yàn)，系統(tǒng)誤差在0.58°以內(nèi)。通過在某型號全機(jī)疲勞試驗(yàn)中的實(shí)際應(yīng)用，該系統(tǒng)工作可靠、準(zhǔn)確，滿足試驗(yàn)要求。

關(guān)鍵詞： 疲勞試驗(yàn)； 姿態(tài)解算； 四元數(shù)法； 擴(kuò)展卡爾曼濾波

中圖分類號： TN919?34； TP23 文獻(xiàn)標(biāo)識碼： A 文章編號： 1004?373X（2014）22?0060?03

Attitude monitoring system of whole plan fatigue test

SHEN Xiang1， GUO Shan?jing2

（1. China Aircraft Strength Research Institute， Xian 710065， China； 2. The 213th Research Institute of China Ordnance Industry， Xian 710061， China）

Abstract： In order to improve the performance of the existing attitude monitoring system， the attitude data of plane was obtained with acceleration sensor， gyroscope， magnetometer and DSP processor through real?time calculation of quaternion algorithm and Runge?Kutta algorithm. The accurate attitude data was got by means of fusion and filtering of the extended Kalman filter with the data from multiple sensors. The data is send to upper computer through wireless transmission module. The system will reminding the test controller through sound and light alarm when the attitude error is greater than the threshold. In the test， the system error is within 0.58°. With the application in the fatigue test， the system is proved to be reliable and accurate， and meets the fatigue test requirements.

Keywords： fatigue test； attitude calculation； quaternion algorithm； extended Kalman filtering

0 引 言

在疲勞實(shí)驗(yàn)中需要將飛機(jī)的姿態(tài)保持在合理的范圍內(nèi)，否則會造成加載力與實(shí)際飛行受力不符的情況。姿態(tài)監(jiān)控即監(jiān)視飛機(jī)的俯仰角[θ]、航向角[φ]和橫滾角[γ]，并實(shí)時顯示，超出設(shè)定的閾值范圍后報警。蔡明等采用電阻式位移傳感器和數(shù)據(jù)采集卡，在Windows平臺上實(shí)現(xiàn)了姿態(tài)測量[1]。目前的飛機(jī)姿態(tài)測量是通過在飛機(jī)起落架上安裝力學(xué)傳感器，通過傳感器的受力偏差來估計(jì)飛機(jī)姿態(tài)的變化。

這兩種方法采用的均為接觸式傳感器，調(diào)試計(jì)算復(fù)雜，尤其是在大型飛機(jī)上安裝、調(diào)試更加困難。本系統(tǒng)采用加速度傳感器、陀螺儀和磁力計(jì)，通過四元數(shù)法解算出飛機(jī)姿態(tài)，然后經(jīng)擴(kuò)展卡爾曼濾波器進(jìn)行數(shù)據(jù)融合濾波后得到準(zhǔn)確的姿態(tài)數(shù)據(jù)。

1 系統(tǒng)結(jié)構(gòu)

姿態(tài)監(jiān)控系統(tǒng)系統(tǒng)結(jié)構(gòu)如圖1所示，主要包括三軸加速度傳感器、三軸陀螺儀、三軸磁力計(jì)、數(shù)據(jù)處理單元、無線通信模塊以及上位機(jī)。

圖1 姿態(tài)監(jiān)控系統(tǒng)結(jié)構(gòu)

數(shù)據(jù)處理單元采用浮點(diǎn)DSP處理器TMS320F28335，具有處理速度快實(shí)時性好的優(yōu)點(diǎn)。三軸加速度傳感器和磁力計(jì)采用六軸數(shù)字傳感器FXOS8700CQ。該傳感器集成了14位三軸加速度傳感器和16位三軸磁力計(jì)，加速度測量范圍[±2g/±4g/±8g]可選，磁力測量范圍[±1 200 μT]，數(shù)據(jù)輸出頻率最高為[800 Hz]，其典型分辨率為0.1°。三軸陀螺儀采用ITG3400A，測量范圍[±250 （°）/s/±500 （°）/s/±1 000 （°）/s]可選，片內(nèi)集成16位ADC，數(shù)據(jù)輸出頻率[8 000 Hz]。

2 四元數(shù)法姿態(tài)解算[2?6]

姿態(tài)解算就是將載體上慣性傳感器的輸出實(shí)時轉(zhuǎn)換成載體的姿態(tài)。

假設(shè)向量在世界坐標(biāo)系[n]中的坐標(biāo)為[xn]、[yn]、[zn]，在機(jī)體坐標(biāo)系[b]中的坐標(biāo)為[xb]、[yb]、[zb]。坐標(biāo)系[n]經(jīng)過[z→x→y]旋轉(zhuǎn)后到坐標(biāo)系[b]。

因此坐標(biāo)變換公式為：

[xbybzb=Cbnxnynzn] （1）

四元數(shù)是一種超復(fù)數(shù)，可以描述一個坐標(biāo)系或一個向量相對于某一個坐標(biāo)系的旋轉(zhuǎn)。四元數(shù)[Q]定義為：

[Q=q0+q1i+q2j+q3k] （3）

式中：[q0，q1，q2，q3∈R]，[i2=j2=k2=-1，ij=k=-ji]。[Q]的標(biāo)量向量之和形式為：

[Q=q0+q] （4）

式中：[q=q1i+q2j+q3k]。

四元數(shù)的乘法表示為：

[Q1?Q2=q01q02-q1?q2+q01q2+q02q1+q1×q2] （5）

坐標(biāo)系變換如圖2所示。

圖2 坐標(biāo)系變換圖

四元數(shù)[Q]的共軛：

[Q*=q0-q1i-q2j-q3k] （6）

向量[p]在坐標(biāo)系[b]和[n]中的投影四元形式分別為：

[pb=0+xbi+ybj+zbkpn=0+xni+ynj+znk] （7）

坐標(biāo)系[n]按照四元數(shù)[Q]旋轉(zhuǎn)后得到一個新的坐標(biāo)系[b]，其投影關(guān)系可以表示為：

[pb=Q*?pn?Q] （8）

將式（7）代入式（8）中得：

[xbi+ybj+zbk=q0-q1i-q2j-q3k? xni+ynj+znk?q0+q1i+q2j+q3k] （9）

將式（9）按四元數(shù)展開，寫成矩陣形式為：

[xbybzb=q21+q20-q23-q222q1q2+q0q32q1q3-q0q22q1q2-q0q3q22-q23+q20-q212q2q3+q0q12q1q3+q0q22q2q3-q0q1q23-q22-q21+q20xnynzn=Cbnxnynzn] （10）

飛機(jī)姿態(tài)角[θ]，[γ]，[φ]分別為：

[θ=arcsin2q2q3+q0q1γ=arctan-2q1q3-q0q2q23+q20-q22-q21φ=arctan-2q1q2-q0q3q22-q23+q20-q21] （11）

由于全機(jī)疲勞試驗(yàn)海拔低，可以忽略地球自轉(zhuǎn)因素的影響，那么四元數(shù)[Q]具有如下微分方程[4]：

[Q?=12ΩbnbQ] （12）

[Ωbnb=0-ωbx-ωby-ωbzωbx0ωbz-ωbyωby-ωbz0ωbxωbzωby-ωbx0] （13）

式中：[ωbx]，[ωby]，[ωbz]分別為機(jī)體坐標(biāo)系下x、y和z軸的角速度。設(shè)T為采樣周期，那么四元數(shù)微分方程的二階龍格?庫塔法計(jì)算公式為：

[K1=ΩbnbtQtY=Qt+TΩbnbtQtK2=Ωbnbtt+TYQt+T=Qt+T2*K1+K2] （14）

3 擴(kuò)展卡爾曼濾波數(shù)據(jù)融合

陀螺儀測量精度低，存在累計(jì)漂移誤差，但是動態(tài)性能良好；加速度傳感器和磁力計(jì)沒有漂移問題，靜態(tài)特性好，但是容易受機(jī)體振動和外部磁場等因素干擾，所以其動態(tài)可信度降低[7]，因此采用擴(kuò)展卡爾曼濾波[8?11]進(jìn)行數(shù)據(jù)融合。取系統(tǒng)的狀態(tài)向量為：

[X=q0q1q2q3T]

根據(jù)式（11）可得狀態(tài)方程：

[Xk=I4×4+12TΩbnbk-1Xk-1+Γk-1Wk-1] （15）

狀態(tài)轉(zhuǎn)移矩陣[Φk，k-1=I4×4+12TΩnnbk-1]，其中[T]為采樣周期。[Γ]為[4×4]維的系統(tǒng)噪聲矩陣；[W]為零均值的高斯白噪聲，其方差陣為[Μ]。

根據(jù)加速度傳感器和磁力計(jì)可以計(jì)算出飛機(jī)姿態(tài)角[θ]，[γ]，[φ]，根據(jù)式（2）計(jì)算出余弦矩陣[Cbn]，根據(jù)式（9），采用高斯?牛頓迭代法求解四元數(shù)[8]。以求得的四元數(shù)[qk=q0q1q2q3T]，作為擴(kuò)展卡爾曼濾波器的觀測量，則系統(tǒng)的量測方程為：

[Zk=HkXk+Vk] （16）

觀測矩陣[Hk=I4×4]。[V]是與[W]不相關(guān)的觀測噪聲，其方差陣為[R]。根據(jù)擴(kuò)展卡爾曼濾波器，[xk]是[xk]的無偏估計(jì)和最小方差估計(jì)。完整的濾波算法為：

[xk，k-1=Φk，k-1xkxk=xk，k-1+Kkzk-zk，k-1zk，k-1=Hkxk，k-1Kk=Pk，k-1HTkHkPk，k-1HTk+VkRkVTk-1Pk，k-1=Φk，k-1Pk-1ΦTk，k-1+Γk，k-1Mk-1ΓTk，k-1Pk=I4×4-KkHkPk，k-1] （17）

經(jīng)過擴(kuò)展卡爾曼濾波器后得到的四元數(shù)，代入到[Cbn]，由式（11），即可得出姿態(tài)角。

圖3 僅利用陀螺儀時輸出角度

4 試驗(yàn)結(jié)果

為了檢測姿態(tài)監(jiān)控系統(tǒng)的準(zhǔn)確性，將其安裝在單軸轉(zhuǎn)動臺上，轉(zhuǎn)臺調(diào)水平。由于飛機(jī)在試驗(yàn)過程中姿態(tài)變化是一個緩慢變化的過程，因此以一個相對緩慢的速度來旋轉(zhuǎn)單軸轉(zhuǎn)臺，靜止時采集的數(shù)據(jù)用來計(jì)算噪聲針[M]和[R]。由于單軸轉(zhuǎn)臺水平，因此橫滾角和俯仰角應(yīng)該一直為[0°]，因此只利用偏航角來驗(yàn)證姿態(tài)監(jiān)控系統(tǒng)的準(zhǔn)確性。圖3是僅利用陀螺儀輸出的角速度采用四元數(shù)法解算的偏航角，可以看出隨著時間的增長，偏航角開始發(fā)散。圖4是采用擴(kuò)展卡爾曼濾波后解算的偏航角，最大誤差為[0.58°]。

圖4 擴(kuò)展卡爾曼濾波后輸出角度

5 結(jié) 論

通過在某型號全機(jī)疲勞試驗(yàn)中的應(yīng)用，姿態(tài)監(jiān)控系統(tǒng)安裝簡便、校準(zhǔn)方便，運(yùn)行可靠，能夠及時、準(zhǔn)確、直觀地反應(yīng)飛機(jī)姿態(tài)變化，滿足試驗(yàn)要求。

參考文獻(xiàn)

[1] 蔡明，楊叢青，趙翔.直升機(jī)全機(jī)靜力試驗(yàn)姿態(tài)測量系統(tǒng)軟件設(shè)計(jì)分析[J].直升機(jī)技術(shù)，2005（4）：20?22.

[2] 袁信，鄭諤.捷聯(lián)式慣性導(dǎo)航原理[M].北京：航空專業(yè)教材編審組，1985.

[3] 李文亮.四元數(shù)矩陣[M].長沙：國防科技大學(xué)出版社，2002.

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

[5] 張榮輝，賈宏光，陳濤，等.基于四元數(shù)法的捷聯(lián)式慣性導(dǎo)航系統(tǒng)的姿態(tài)解算[J].光學(xué)精密工程，2008，16（10）：1963?1970.

[6] 杜海龍，張榮輝，劉平，等.捷聯(lián)慣導(dǎo)系統(tǒng)姿態(tài)解算模塊的實(shí)現(xiàn)[J].光學(xué)精密工程，2008，16（10）：1956?1961.

[7] 劉星.多維MEMS慣性傳感器的姿態(tài)解算算法研究[D].哈爾濱：哈爾濱工程大學(xué)，2013.

[8] 黃旭，王常虹，伊國興，等.利用磁強(qiáng)計(jì)及微機(jī)械加速度計(jì)和陀螺的姿態(tài)估計(jì)擴(kuò)展卡爾曼濾波器[J].中國慣性學(xué)報，2005，13（2）：27?34.

[9] 楊淑潔，曾慶雙，伊國興.低成本無人機(jī)姿態(tài)測量系統(tǒng)研究[J].傳感器與微系統(tǒng)，2012，31（2）：15?22.

[10] 楊峻巍.水下航行器導(dǎo)航及數(shù)據(jù)融合技術(shù)研究[D].哈爾濱：哈爾濱工程大學(xué)，2013.

[11] 喬相偉.基于四元數(shù)非線性濾波的飛行器姿態(tài)確定算法研究[D].哈爾濱：哈爾濱工程大學(xué)，2011.

式中：[q0，q1，q2，q3∈R]，[i2=j2=k2=-1，ij=k=-ji]。[Q]的標(biāo)量向量之和形式為：

[Q=q0+q] （4）

式中：[q=q1i+q2j+q3k]。

四元數(shù)的乘法表示為：

[Q1?Q2=q01q02-q1?q2+q01q2+q02q1+q1×q2] （5）

坐標(biāo)系變換如圖2所示。

圖2 坐標(biāo)系變換圖

四元數(shù)[Q]的共軛：

[Q*=q0-q1i-q2j-q3k] （6）

向量[p]在坐標(biāo)系[b]和[n]中的投影四元形式分別為：

[pb=0+xbi+ybj+zbkpn=0+xni+ynj+znk] （7）

坐標(biāo)系[n]按照四元數(shù)[Q]旋轉(zhuǎn)后得到一個新的坐標(biāo)系[b]，其投影關(guān)系可以表示為：

[pb=Q*?pn?Q] （8）

將式（7）代入式（8）中得：

[xbi+ybj+zbk=q0-q1i-q2j-q3k? xni+ynj+znk?q0+q1i+q2j+q3k] （9）

將式（9）按四元數(shù)展開，寫成矩陣形式為：

[xbybzb=q21+q20-q23-q222q1q2+q0q32q1q3-q0q22q1q2-q0q3q22-q23+q20-q212q2q3+q0q12q1q3+q0q22q2q3-q0q1q23-q22-q21+q20xnynzn=Cbnxnynzn] （10）

飛機(jī)姿態(tài)角[θ]，[γ]，[φ]分別為：

[θ=arcsin2q2q3+q0q1γ=arctan-2q1q3-q0q2q23+q20-q22-q21φ=arctan-2q1q2-q0q3q22-q23+q20-q21] （11）

由于全機(jī)疲勞試驗(yàn)海拔低，可以忽略地球自轉(zhuǎn)因素的影響，那么四元數(shù)[Q]具有如下微分方程[4]：

[Q?=12ΩbnbQ] （12）

[Ωbnb=0-ωbx-ωby-ωbzωbx0ωbz-ωbyωby-ωbz0ωbxωbzωby-ωbx0] （13）

式中：[ωbx]，[ωby]，[ωbz]分別為機(jī)體坐標(biāo)系下x、y和z軸的角速度。設(shè)T為采樣周期，那么四元數(shù)微分方程的二階龍格?庫塔法計(jì)算公式為：

[K1=ΩbnbtQtY=Qt+TΩbnbtQtK2=Ωbnbtt+TYQt+T=Qt+T2*K1+K2] （14）

3 擴(kuò)展卡爾曼濾波數(shù)據(jù)融合

陀螺儀測量精度低，存在累計(jì)漂移誤差，但是動態(tài)性能良好；加速度傳感器和磁力計(jì)沒有漂移問題，靜態(tài)特性好，但是容易受機(jī)體振動和外部磁場等因素干擾，所以其動態(tài)可信度降低[7]，因此采用擴(kuò)展卡爾曼濾波[8?11]進(jìn)行數(shù)據(jù)融合。取系統(tǒng)的狀態(tài)向量為：

[X=q0q1q2q3T]

根據(jù)式（11）可得狀態(tài)方程：

[Xk=I4×4+12TΩbnbk-1Xk-1+Γk-1Wk-1] （15）

狀態(tài)轉(zhuǎn)移矩陣[Φk，k-1=I4×4+12TΩnnbk-1]，其中[T]為采樣周期。[Γ]為[4×4]維的系統(tǒng)噪聲矩陣；[W]為零均值的高斯白噪聲，其方差陣為[Μ]。

根據(jù)加速度傳感器和磁力計(jì)可以計(jì)算出飛機(jī)姿態(tài)角[θ]，[γ]，[φ]，根據(jù)式（2）計(jì)算出余弦矩陣[Cbn]，根據(jù)式（9），采用高斯?牛頓迭代法求解四元數(shù)[8]。以求得的四元數(shù)[qk=q0q1q2q3T]，作為擴(kuò)展卡爾曼濾波器的觀測量，則系統(tǒng)的量測方程為：

[Zk=HkXk+Vk] （16）

觀測矩陣[Hk=I4×4]。[V]是與[W]不相關(guān)的觀測噪聲，其方差陣為[R]。根據(jù)擴(kuò)展卡爾曼濾波器，[xk]是[xk]的無偏估計(jì)和最小方差估計(jì)。完整的濾波算法為：

[xk，k-1=Φk，k-1xkxk=xk，k-1+Kkzk-zk，k-1zk，k-1=Hkxk，k-1Kk=Pk，k-1HTkHkPk，k-1HTk+VkRkVTk-1Pk，k-1=Φk，k-1Pk-1ΦTk，k-1+Γk，k-1Mk-1ΓTk，k-1Pk=I4×4-KkHkPk，k-1] （17）

經(jīng)過擴(kuò)展卡爾曼濾波器后得到的四元數(shù)，代入到[Cbn]，由式（11），即可得出姿態(tài)角。

圖3 僅利用陀螺儀時輸出角度

4 試驗(yàn)結(jié)果

為了檢測姿態(tài)監(jiān)控系統(tǒng)的準(zhǔn)確性，將其安裝在單軸轉(zhuǎn)動臺上，轉(zhuǎn)臺調(diào)水平。由于飛機(jī)在試驗(yàn)過程中姿態(tài)變化是一個緩慢變化的過程，因此以一個相對緩慢的速度來旋轉(zhuǎn)單軸轉(zhuǎn)臺，靜止時采集的數(shù)據(jù)用來計(jì)算噪聲針[M]和[R]。由于單軸轉(zhuǎn)臺水平，因此橫滾角和俯仰角應(yīng)該一直為[0°]，因此只利用偏航角來驗(yàn)證姿態(tài)監(jiān)控系統(tǒng)的準(zhǔn)確性。圖3是僅利用陀螺儀輸出的角速度采用四元數(shù)法解算的偏航角，可以看出隨著時間的增長，偏航角開始發(fā)散。圖4是采用擴(kuò)展卡爾曼濾波后解算的偏航角，最大誤差為[0.58°]。

圖4 擴(kuò)展卡爾曼濾波后輸出角度

5 結(jié) 論

通過在某型號全機(jī)疲勞試驗(yàn)中的應(yīng)用，姿態(tài)監(jiān)控系統(tǒng)安裝簡便、校準(zhǔn)方便，運(yùn)行可靠，能夠及時、準(zhǔn)確、直觀地反應(yīng)飛機(jī)姿態(tài)變化，滿足試驗(yàn)要求。

參考文獻(xiàn)

[1] 蔡明，楊叢青，趙翔.直升機(jī)全機(jī)靜力試驗(yàn)姿態(tài)測量系統(tǒng)軟件設(shè)計(jì)分析[J].直升機(jī)技術(shù)，2005（4）：20?22.

[2] 袁信，鄭諤.捷聯(lián)式慣性導(dǎo)航原理[M].北京：航空專業(yè)教材編審組，1985.

[3] 李文亮.四元數(shù)矩陣[M].長沙：國防科技大學(xué)出版社，2002.

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

[5] 張榮輝，賈宏光，陳濤，等.基于四元數(shù)法的捷聯(lián)式慣性導(dǎo)航系統(tǒng)的姿態(tài)解算[J].光學(xué)精密工程，2008，16（10）：1963?1970.

[6] 杜海龍，張榮輝，劉平，等.捷聯(lián)慣導(dǎo)系統(tǒng)姿態(tài)解算模塊的實(shí)現(xiàn)[J].光學(xué)精密工程，2008，16（10）：1956?1961.

[7] 劉星.多維MEMS慣性傳感器的姿態(tài)解算算法研究[D].哈爾濱：哈爾濱工程大學(xué)，2013.

[8] 黃旭，王常虹，伊國興，等.利用磁強(qiáng)計(jì)及微機(jī)械加速度計(jì)和陀螺的姿態(tài)估計(jì)擴(kuò)展卡爾曼濾波器[J].中國慣性學(xué)報，2005，13（2）：27?34.

[9] 楊淑潔，曾慶雙，伊國興.低成本無人機(jī)姿態(tài)測量系統(tǒng)研究[J].傳感器與微系統(tǒng)，2012，31（2）：15?22.

[10] 楊峻巍.水下航行器導(dǎo)航及數(shù)據(jù)融合技術(shù)研究[D].哈爾濱：哈爾濱工程大學(xué)，2013.

[11] 喬相偉.基于四元數(shù)非線性濾波的飛行器姿態(tài)確定算法研究[D].哈爾濱：哈爾濱工程大學(xué)，2011.

式中：[q0，q1，q2，q3∈R]，[i2=j2=k2=-1，ij=k=-ji]。[Q]的標(biāo)量向量之和形式為：

[Q=q0+q] （4）

式中：[q=q1i+q2j+q3k]。

四元數(shù)的乘法表示為：

[Q1?Q2=q01q02-q1?q2+q01q2+q02q1+q1×q2] （5）

坐標(biāo)系變換如圖2所示。

圖2 坐標(biāo)系變換圖

四元數(shù)[Q]的共軛：

[Q*=q0-q1i-q2j-q3k] （6）

向量[p]在坐標(biāo)系[b]和[n]中的投影四元形式分別為：

[pb=0+xbi+ybj+zbkpn=0+xni+ynj+znk] （7）

坐標(biāo)系[n]按照四元數(shù)[Q]旋轉(zhuǎn)后得到一個新的坐標(biāo)系[b]，其投影關(guān)系可以表示為：

[pb=Q*?pn?Q] （8）

將式（7）代入式（8）中得：

[xbi+ybj+zbk=q0-q1i-q2j-q3k? xni+ynj+znk?q0+q1i+q2j+q3k] （9）

將式（9）按四元數(shù)展開，寫成矩陣形式為：

[xbybzb=q21+q20-q23-q222q1q2+q0q32q1q3-q0q22q1q2-q0q3q22-q23+q20-q212q2q3+q0q12q1q3+q0q22q2q3-q0q1q23-q22-q21+q20xnynzn=Cbnxnynzn] （10）

飛機(jī)姿態(tài)角[θ]，[γ]，[φ]分別為：

[θ=arcsin2q2q3+q0q1γ=arctan-2q1q3-q0q2q23+q20-q22-q21φ=arctan-2q1q2-q0q3q22-q23+q20-q21] （11）

由于全機(jī)疲勞試驗(yàn)海拔低，可以忽略地球自轉(zhuǎn)因素的影響，那么四元數(shù)[Q]具有如下微分方程[4]：

[Q?=12ΩbnbQ] （12）

[Ωbnb=0-ωbx-ωby-ωbzωbx0ωbz-ωbyωby-ωbz0ωbxωbzωby-ωbx0] （13）

式中：[ωbx]，[ωby]，[ωbz]分別為機(jī)體坐標(biāo)系下x、y和z軸的角速度。設(shè)T為采樣周期，那么四元數(shù)微分方程的二階龍格?庫塔法計(jì)算公式為：

[K1=ΩbnbtQtY=Qt+TΩbnbtQtK2=Ωbnbtt+TYQt+T=Qt+T2*K1+K2] （14）

3 擴(kuò)展卡爾曼濾波數(shù)據(jù)融合

陀螺儀測量精度低，存在累計(jì)漂移誤差，但是動態(tài)性能良好；加速度傳感器和磁力計(jì)沒有漂移問題，靜態(tài)特性好，但是容易受機(jī)體振動和外部磁場等因素干擾，所以其動態(tài)可信度降低[7]，因此采用擴(kuò)展卡爾曼濾波[8?11]進(jìn)行數(shù)據(jù)融合。取系統(tǒng)的狀態(tài)向量為：

[X=q0q1q2q3T]

根據(jù)式（11）可得狀態(tài)方程：

[Xk=I4×4+12TΩbnbk-1Xk-1+Γk-1Wk-1] （15）

狀態(tài)轉(zhuǎn)移矩陣[Φk，k-1=I4×4+12TΩnnbk-1]，其中[T]為采樣周期。[Γ]為[4×4]維的系統(tǒng)噪聲矩陣；[W]為零均值的高斯白噪聲，其方差陣為[Μ]。

根據(jù)加速度傳感器和磁力計(jì)可以計(jì)算出飛機(jī)姿態(tài)角[θ]，[γ]，[φ]，根據(jù)式（2）計(jì)算出余弦矩陣[Cbn]，根據(jù)式（9），采用高斯?牛頓迭代法求解四元數(shù)[8]。以求得的四元數(shù)[qk=q0q1q2q3T]，作為擴(kuò)展卡爾曼濾波器的觀測量，則系統(tǒng)的量測方程為：

[Zk=HkXk+Vk] （16）

觀測矩陣[Hk=I4×4]。[V]是與[W]不相關(guān)的觀測噪聲，其方差陣為[R]。根據(jù)擴(kuò)展卡爾曼濾波器，[xk]是[xk]的無偏估計(jì)和最小方差估計(jì)。完整的濾波算法為：

[xk，k-1=Φk，k-1xkxk=xk，k-1+Kkzk-zk，k-1zk，k-1=Hkxk，k-1Kk=Pk，k-1HTkHkPk，k-1HTk+VkRkVTk-1Pk，k-1=Φk，k-1Pk-1ΦTk，k-1+Γk，k-1Mk-1ΓTk，k-1Pk=I4×4-KkHkPk，k-1] （17）

經(jīng)過擴(kuò)展卡爾曼濾波器后得到的四元數(shù)，代入到[Cbn]，由式（11），即可得出姿態(tài)角。

圖3 僅利用陀螺儀時輸出角度

4 試驗(yàn)結(jié)果

為了檢測姿態(tài)監(jiān)控系統(tǒng)的準(zhǔn)確性，將其安裝在單軸轉(zhuǎn)動臺上，轉(zhuǎn)臺調(diào)水平。由于飛機(jī)在試驗(yàn)過程中姿態(tài)變化是一個緩慢變化的過程，因此以一個相對緩慢的速度來旋轉(zhuǎn)單軸轉(zhuǎn)臺，靜止時采集的數(shù)據(jù)用來計(jì)算噪聲針[M]和[R]。由于單軸轉(zhuǎn)臺水平，因此橫滾角和俯仰角應(yīng)該一直為[0°]，因此只利用偏航角來驗(yàn)證姿態(tài)監(jiān)控系統(tǒng)的準(zhǔn)確性。圖3是僅利用陀螺儀輸出的角速度采用四元數(shù)法解算的偏航角，可以看出隨著時間的增長，偏航角開始發(fā)散。圖4是采用擴(kuò)展卡爾曼濾波后解算的偏航角，最大誤差為[0.58°]。

圖4 擴(kuò)展卡爾曼濾波后輸出角度

5 結(jié) 論

通過在某型號全機(jī)疲勞試驗(yàn)中的應(yīng)用，姿態(tài)監(jiān)控系統(tǒng)安裝簡便、校準(zhǔn)方便，運(yùn)行可靠，能夠及時、準(zhǔn)確、直觀地反應(yīng)飛機(jī)姿態(tài)變化，滿足試驗(yàn)要求。

參考文獻(xiàn)

[1] 蔡明，楊叢青，趙翔.直升機(jī)全機(jī)靜力試驗(yàn)姿態(tài)測量系統(tǒng)軟件設(shè)計(jì)分析[J].直升機(jī)技術(shù)，2005（4）：20?22.

[2] 袁信，鄭諤.捷聯(lián)式慣性導(dǎo)航原理[M].北京：航空專業(yè)教材編審組，1985.

[3] 李文亮.四元數(shù)矩陣[M].長沙：國防科技大學(xué)出版社，2002.

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

[5] 張榮輝，賈宏光，陳濤，等.基于四元數(shù)法的捷聯(lián)式慣性導(dǎo)航系統(tǒng)的姿態(tài)解算[J].光學(xué)精密工程，2008，16（10）：1963?1970.

[6] 杜海龍，張榮輝，劉平，等.捷聯(lián)慣導(dǎo)系統(tǒng)姿態(tài)解算模塊的實(shí)現(xiàn)[J].光學(xué)精密工程，2008，16（10）：1956?1961.

[7] 劉星.多維MEMS慣性傳感器的姿態(tài)解算算法研究[D].哈爾濱：哈爾濱工程大學(xué)，2013.

[8] 黃旭，王常虹，伊國興，等.利用磁強(qiáng)計(jì)及微機(jī)械加速度計(jì)和陀螺的姿態(tài)估計(jì)擴(kuò)展卡爾曼濾波器[J].中國慣性學(xué)報，2005，13（2）：27?34.

[9] 楊淑潔，曾慶雙，伊國興.低成本無人機(jī)姿態(tài)測量系統(tǒng)研究[J].傳感器與微系統(tǒng)，2012，31（2）：15?22.

[10] 楊峻巍.水下航行器導(dǎo)航及數(shù)據(jù)融合技術(shù)研究[D].哈爾濱：哈爾濱工程大學(xué)，2013.

[11] 喬相偉.基于四元數(shù)非線性濾波的飛行器姿態(tài)確定算法研究[D].哈爾濱：哈爾濱工程大學(xué)，2011.