基于MEMS器件的姿態航向系統實現

摘要:針對目前捷聯式姿態航向系統低成本、低功耗、高精度、微型化的發展趨勢，提出一種基于MEMS器件的姿態航向系統的實現方案，介紹了該系統的方案設計與算法原理，實驗證明該系統可以很好的測量出載體的姿態航向，可用于小型無人直升機的導航控制中。

關鍵詞:姿態航向系統;MEMS器件;姿態算法;圓錐運動

中圖分類號:U666文獻標識碼:A文章編號:1009-3044(2009)33-9527-03

Implementation of Attitude Heading System based on MEMS Units

CUI Lu-lu， HAO Ying-guang

(School of Electronic and Information Engineering， Dalian University of Technology， Dalian 116024， China)

Abstract: In view of the current strapdown attitude and heading system's development trend in low cost， low power， high accuracy and miniaturization， an implementation of attitude and heading system based on MEMS devices is proposed. The system's design and algorithm theory are introduced. The experiment proves that the system can get a good result on measuring attitude，which can be embedded in navigation of small unmanned helicopter control.

Key words: attitude and heading system; MEMS units; attitude algorithm; coning motion

捷聯慣導系統是慣性傳感器(主要指陀螺和加速度計)直接固連在運載體上的慣導系統，捷聯式姿態航向系統(AHS，Attitude and Heading System)是基于捷聯慣導系統原理，通過陀螺儀和加速度計測量載體的角運動和加速度，得到載體姿態和航向信息的系統，目前已成為慣性技術發展的一個重要方向。然而無論是國內還是國外較先進的AHS系統由于傳感器因素導致體積和重量都較大，成本昂貴，這在一定程度上限制了航向姿態測量系統的應用。

隨著微電子技術的發展而出現的微電子機械系統(MEMS，Micro Electro Mechanical System)由于具有尺寸小、重量輕、成本低、功耗小和動態范圍寬等優點[1]，被廣泛應用于測量領域。本文從工程實踐出發，介紹了一種基于微機械慣性傳感器的姿態航向系統，該系統以ST公司的32位單片機STM32F103RBT6為核心，以ADIS16355為基本測量元件，同時采用電子羅盤作為輔助測量元件以保證測量精度。

1 系統方案設計

1.1 硬件設計

姿態航向系統的主要任務是:采集各類傳感器信號，主要指陀螺儀和加速度計信號，將采集到的角速度和加速度信息進行導航解算，最后，將解算后的姿態信息送到顯示終端顯示。系統主要由慣性數據采集模塊、處理器模塊、RS-485電平轉換模塊以及外圍輔助模塊四部分組成，圖1為系統硬件原理框圖。

該文處理器模塊由STM32F103RBT6及其外圍電路構成，72MHz主頻、32位數據寬度能夠實現高速、高性能，同時可以方便的與慣性傳感器進行數據通信，滿足姿態航向系統對運算速度和精度的要求。

慣性數據采集模塊由ADIS16355單芯片實現，其六自由度、14位數字輸出的高集成度特性，省去了安裝和設計數據采集電路的麻煩，標準的SPI接口簡化了傳感器與處理器的數據通信。使系統無需額外的解碼電路和邏輯控制電路便可以實現導航解算。RS-485模塊實現LVTTL/LVCMOS電平到RS-485電平之間的轉換，使導航信息通過RS-485總線傳輸到PC。實際應用中，需要用到RS232/RS485轉換器將RS-485信號轉換成RS-232信號，然后與計算機通信。這里用RS-485傳輸而不用RS-232直接傳輸，主要是考慮到RS-485受外界干擾小，傳輸距離遠，適合環境惡劣的場合。

1.2 軟件設計

本系統軟件設計主要包括傳感器數據采集、角度對準和解算以及整個系統的任務調度。軟件流程圖如圖2所示。

系統首先執行初始化操作，包括STM32內核和外設的初始化以及系統任務和變量的初始化。之后通過檢查讀取數據標志是否置位來確定起止數據讀取操作，其中讀取數據標志的設置通過微控制器的高級定時器TIM1實現。TIM1每10ms產生一次中斷設置讀取數據標志，實現微控制器每10ms從ADIS16355中讀取一組數據。采樣5組數據后，經過平滑濾波取均值用于姿態解算。最后，系統判斷數據采樣是否完成，進入姿態解算狀態。需要注意的是，傳感器的片選信號是通過微控制器的通用定時器TIM2控制的。開始讀取數據時，系統將傳感器的片選信號拉低，使能傳感器，同時使能定時器TIM2。當定時時間到產生中斷時，系統將片選信號拉低，失能傳感器。這樣，不僅可以高效的利用傳感器，還可以進一步降低功耗。

2 姿態航向系統算法

2.1 坐標系參數說明

本系統選取導航坐標系為東北天地理坐標系Oxtytzt，載體坐標系為機體坐標系Oxbybzb，其中xb指向載體的右側，yb指向載體的縱軸方向，zb指向載體的豎軸方向。載體坐標系相對地理坐標系的方位為載體的姿態角，包括俯仰角θ、橫滾角γ與航向角ψ。

2.2 捷聯姿態算法

載體具有靜止狀態、低速運動狀態和高速旋轉狀態三種工作狀態，針對這三種狀態采用不同的姿態解算方法，以達到解算精度與計算量的平衡。

1) 加速度計確定姿態算法

在載體處于靜止或勻速直線運動時，加速度計可以單獨確定載體的橫滾和俯仰角[2]:

(1)

航向角ψ可以由電子羅盤得到初始值ψ0，并通過陀螺儀 軸測量值的積分獲得，即:

(2)

其中ψk為陀螺儀z軸第k個周期的采樣值，dt為采樣周期。

2) 四元數姿態算法

四元數算法適用于低速運動下的載體，姿態矩陣可以表示為姿態角θ，γ，ψ的關系:

(3)

設載體坐標系相對地理坐標系的轉動四元數為:

(4)

四元數與姿態矩陣間的關系為:

(5)

四元數進行歸一化，通過公式(6)即可求出姿態角:

(6)

3) 等效旋轉矢量算法

當運載體姿態變化劇烈時，四元數解算將會出現嚴重的不可交換性誤差，1971年JohnE.Bortz提出了等效旋轉矢量概念[3]，將運載體的姿態四元數更新轉化為姿態變化四元數的更新，對不可交換誤差作了適當補償，正好彌補了四元數法的不足。由于本系統采用的陀螺儀輸出為角速率，因此考慮角速率輸出情況下的等效旋轉矢量算法。

① 計算陀螺儀在一個姿態更新周期h內的四次采樣值ω1，ω2，ω3，ω4;

② 將陀螺儀一個周期內的輸出以三項式擬合，即:ω(T+t)=a+2bt+3ct2+4dt3

③ 計算等效旋轉矢量 (7)

其中

錐運動環境下進行優化

(8)

(9)

④ 利用Φ=[Φx Φy， Φz]構造一個變換四元數，即:

(10)

其中

⑤ 利用Q(t+h)= Q(t) ?茚q(h)更新四元數從而得到姿態角。

2.3 姿態融合算法

對載體運動狀態的實時判斷是進行姿態算法融合的依據，本系統提取兩方面信息判斷載體運動狀態。

1) 勻速運動門限值εa

在靜止或勻速運動的情況下，加速度計三軸的測量值應滿足下式:

(11)

實際系統中由于加速度計精度與噪聲的問題，在判斷時不可能完全滿足上式，而需要設置一個門限值εa，只要加速度計三軸分量的測量值的平方和減去重力加速度的平方的差小于此門限值，即可近似的認為載體處于靜止或勻速狀態。

2) 旋轉運動門限值εω

旋轉運動可通過陀螺儀的角速度輸出加以判斷:

(12)

式中:ωibbx、ωibby、ωibbz為載體坐標系x、y、z軸的角速度輸出。

本系統將載體做高動態運動的角速度門限值設為10°/s。

姿態融合算法是指載體在勻速運動或靜止時，利用加速度計和電子羅盤獲取姿態信息，同時每周期檢測系統的運動狀態，當系統檢測到載體在進行低速非旋轉運動時，就在上周期加速度計解算值的基礎上利用四元數算法繼續解算，若系統恢復勻速運動，就重新利用加速度計解算，當系統檢測到載體做旋轉運動時，則直接用等效旋轉矢量算法進行解算。

3 系統實驗與仿真

3.1 低動態環境下實驗

在Keil μVision3集成開發環境下對系統進行調試與實現，姿態算法部分采用C語言編寫。通過實時采集低動態環境下傳感器輸出數據，并分別采用四元數算法和融合算法兩種解算模式進行姿態解算，將解算結果輸入到matlab中進行評估與分析。

系統解算初始條件設為:經度38.8859°，緯度121.5243°，高度100m，俯仰角0.0°，橫滾角0.0°，方位角0.0°，姿態解算周期為0.01s，解算時間為40s。

解算結果如圖3、圖4所示，誤差結果如表1所示。

由表1可以看出， 單純使用四元數解算，由于陀螺漂移的存在，算法誤差會隨時間增長而累積，而通過實時判斷載體運動狀態，利用加速度計對陀螺漂移進行重調補償，航向角的誤差漂移減小了一個數量級。在低動態環境下，系統解算精度俯仰角與橫滾角誤差<0.8°/s，航向角誤差<0.2°/s，滿足系統設計要求。

3.2 高動態環境下仿真

由于實驗條件有限，無法確定載體的高動態運動狀態，因此對于旋轉運動狀態下的解算采用MATLAB進行仿真，以確定旋轉矢量算法的精確性。

以含誤差的圓錐運動作為仿真環境，仿真條件設置如下:

考慮陀螺儀的誤差為白噪聲，其測量誤差模型為:

=ε+wg

其中ε為系統隨機常值漂移，wg為白噪聲，陀螺儀實際輸出為:

(14)

錐運動的半錐角α=1°、錐頻率為f=15Hz，系統陀螺儀隨機常值漂移為0.0015°/s，白噪聲密度為0.07°/s，姿態更新周期h=12ms、仿真時間tmax=60s。

仿真結果如圖5、6所示。

由仿真結果可以看出，當載體做高速旋轉運動時，姿態角誤差增大，旋轉矢量算法精度高于四元數解算精度。

4 結論

該文以STM32F103RBT6為核心處理器，以ADIS16355為慣性檢測傳感器，設計實現了一個小型姿態航向系統，實驗表明采用嵌入式處理器及基于MEMS的慣性傳感器，經過合理的軟、硬件設計，可以很好的完成導航任務，實現系統的微型化和低成本，而根據載體運動狀態采用不同的姿態解算算法，則可以提高不同環境下導航系統可靠性，滿足工程中對系統精度的要求，實現預期的設計目標。

參考文獻:

[1] Bernstein J.An overview of MEMS inertial sensing technology[EB/OL].http://www.sensorsmag.com/articles/0203/14/.

[2] 沈曉蓉，張海，范耀祖.加速度計輔助模糊混合捷聯姿態確定算法研究[J].壓電與聲光，2008(2):106-108.

[3] Bortz J E. A New Mathematical Formulation for Strapdown Inertial Navigation[J].IEEE Transactions on Aerospace and Electronic Systems，1971，7(1):61-66.

[4] 薛定宇，陳陽泉.基于MATLAB/Simulink的系統仿真技術與應用[M].北京:清華大學出版社，2002:281-296

[5] 黃旭，王常虹.基于微機械慣性器件和磁強計的姿態航向系統[J].傳感器技術，2005(8):54-56.