全機疲勞試驗姿態監控系統

沈翔+郭善鏡

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

關鍵詞： 疲勞試驗； 姿態解算； 四元數法； 擴展卡爾曼濾波

中圖分類號： TN919?34； TP23 文獻標識碼： 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 引 言

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

這兩種方法采用的均為接觸式傳感器，調試計算復雜，尤其是在大型飛機上安裝、調試更加困難。本系統采用加速度傳感器、陀螺儀和磁力計，通過四元數法解算出飛機姿態，然后經擴展卡爾曼濾波器進行數據融合濾波后得到準確的姿態數據。

1 系統結構

姿態監控系統系統結構如圖1所示，主要包括三軸加速度傳感器、三軸陀螺儀、三軸磁力計、數據處理單元、無線通信模塊以及上位機。

圖1 姿態監控系統結構

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

2 四元數法姿態解算[2?6]

姿態解算就是將載體上慣性傳感器的輸出實時轉換成載體的姿態。

假設向量在世界坐標系[n]中的坐標為[xn]、[yn]、[zn]，在機體坐標系[b]中的坐標為[xb]、[yb]、[zb]。坐標系[n]經過[z→x→y]旋轉后到坐標系[b]。

因此坐標變換公式為：

[xbybzb=Cbnxnynzn] （1）

四元數是一種超復數，可以描述一個坐標系或一個向量相對于某一個坐標系的旋轉。四元數[Q]定義為：

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

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

[Q=q0+q] （4）

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

四元數的乘法表示為：

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

坐標系變換如圖2所示。

圖2 坐標系變換圖

四元數[Q]的共軛：

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

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

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

坐標系[n]按照四元數[Q]旋轉后得到一個新的坐標系[b]，其投影關系可以表示為：

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

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

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

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

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

飛機姿態角[θ]，[γ]，[φ]分別為：

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

由于全機疲勞試驗海拔低，可以忽略地球自轉因素的影響，那么四元數[Q]具有如下微分方程[4]：

[Q?=12ΩbnbQ] （12）

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

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

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

3 擴展卡爾曼濾波數據融合

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

[X=q0q1q2q3T]

根據式（11）可得狀態方程：

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

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

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

[Zk=HkXk+Vk] （16）

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

[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）

經過擴展卡爾曼濾波器后得到的四元數，代入到[Cbn]，由式（11），即可得出姿態角。

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

4 試驗結果

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

圖4 擴展卡爾曼濾波后輸出角度

5 結 論

通過在某型號全機疲勞試驗中的應用，姿態監控系統安裝簡便、校準方便，運行可靠，能夠及時、準確、直觀地反應飛機姿態變化，滿足試驗要求。

參考文獻

[1] 蔡明，楊叢青，趙翔.直升機全機靜力試驗姿態測量系統軟件設計分析[J].直升機技術，2005（4）：20?22.

[2] 袁信，鄭諤.捷聯式慣性導航原理[M].北京：航空專業教材編審組，1985.

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

[4] 秦永元.慣性導航[M].北京：科學出版社，2006.

[5] 張榮輝，賈宏光，陳濤，等.基于四元數法的捷聯式慣性導航系統的姿態解算[J].光學精密工程，2008，16（10）：1963?1970.

[6] 杜海龍，張榮輝，劉平，等.捷聯慣導系統姿態解算模塊的實現[J].光學精密工程，2008，16（10）：1956?1961.

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

[8] 黃旭，王常虹，伊國興，等.利用磁強計及微機械加速度計和陀螺的姿態估計擴展卡爾曼濾波器[J].中國慣性學報，2005，13（2）：27?34.

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

[10] 楊峻巍.水下航行器導航及數據融合技術研究[D].哈爾濱：哈爾濱工程大學，2013.

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

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

[Q=q0+q] （4）

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

四元數的乘法表示為：

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

坐標系變換如圖2所示。

圖2 坐標系變換圖

四元數[Q]的共軛：

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

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

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

坐標系[n]按照四元數[Q]旋轉后得到一個新的坐標系[b]，其投影關系可以表示為：

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

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

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

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

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

飛機姿態角[θ]，[γ]，[φ]分別為：

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

由于全機疲勞試驗海拔低，可以忽略地球自轉因素的影響，那么四元數[Q]具有如下微分方程[4]：

[Q?=12ΩbnbQ] （12）

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

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

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

3 擴展卡爾曼濾波數據融合

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

[X=q0q1q2q3T]

根據式（11）可得狀態方程：

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

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

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

[Zk=HkXk+Vk] （16）

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

[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）

經過擴展卡爾曼濾波器后得到的四元數，代入到[Cbn]，由式（11），即可得出姿態角。

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

4 試驗結果

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

圖4 擴展卡爾曼濾波后輸出角度

5 結 論

通過在某型號全機疲勞試驗中的應用，姿態監控系統安裝簡便、校準方便，運行可靠，能夠及時、準確、直觀地反應飛機姿態變化，滿足試驗要求。

參考文獻

[1] 蔡明，楊叢青，趙翔.直升機全機靜力試驗姿態測量系統軟件設計分析[J].直升機技術，2005（4）：20?22.

[2] 袁信，鄭諤.捷聯式慣性導航原理[M].北京：航空專業教材編審組，1985.

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

[4] 秦永元.慣性導航[M].北京：科學出版社，2006.

[5] 張榮輝，賈宏光，陳濤，等.基于四元數法的捷聯式慣性導航系統的姿態解算[J].光學精密工程，2008，16（10）：1963?1970.

[6] 杜海龍，張榮輝，劉平，等.捷聯慣導系統姿態解算模塊的實現[J].光學精密工程，2008，16（10）：1956?1961.

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

[8] 黃旭，王常虹，伊國興，等.利用磁強計及微機械加速度計和陀螺的姿態估計擴展卡爾曼濾波器[J].中國慣性學報，2005，13（2）：27?34.

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

[10] 楊峻巍.水下航行器導航及數據融合技術研究[D].哈爾濱：哈爾濱工程大學，2013.

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

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

[Q=q0+q] （4）

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

四元數的乘法表示為：

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

坐標系變換如圖2所示。

圖2 坐標系變換圖

四元數[Q]的共軛：

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

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

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

坐標系[n]按照四元數[Q]旋轉后得到一個新的坐標系[b]，其投影關系可以表示為：

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

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

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

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

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

飛機姿態角[θ]，[γ]，[φ]分別為：

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

由于全機疲勞試驗海拔低，可以忽略地球自轉因素的影響，那么四元數[Q]具有如下微分方程[4]：

[Q?=12ΩbnbQ] （12）

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

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

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

3 擴展卡爾曼濾波數據融合

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

[X=q0q1q2q3T]

根據式（11）可得狀態方程：

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

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

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

[Zk=HkXk+Vk] （16）

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

[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）

經過擴展卡爾曼濾波器后得到的四元數，代入到[Cbn]，由式（11），即可得出姿態角。

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

4 試驗結果

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

圖4 擴展卡爾曼濾波后輸出角度

5 結 論

通過在某型號全機疲勞試驗中的應用，姿態監控系統安裝簡便、校準方便，運行可靠，能夠及時、準確、直觀地反應飛機姿態變化，滿足試驗要求。

參考文獻

[1] 蔡明，楊叢青，趙翔.直升機全機靜力試驗姿態測量系統軟件設計分析[J].直升機技術，2005（4）：20?22.

[2] 袁信，鄭諤.捷聯式慣性導航原理[M].北京：航空專業教材編審組，1985.

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

[4] 秦永元.慣性導航[M].北京：科學出版社，2006.

[5] 張榮輝，賈宏光，陳濤，等.基于四元數法的捷聯式慣性導航系統的姿態解算[J].光學精密工程，2008，16（10）：1963?1970.

[6] 杜海龍，張榮輝，劉平，等.捷聯慣導系統姿態解算模塊的實現[J].光學精密工程，2008，16（10）：1956?1961.

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

[8] 黃旭，王常虹，伊國興，等.利用磁強計及微機械加速度計和陀螺的姿態估計擴展卡爾曼濾波器[J].中國慣性學報，2005，13（2）：27?34.

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

[10] 楊峻巍.水下航行器導航及數據融合技術研究[D].哈爾濱：哈爾濱工程大學，2013.

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