基于MEMS器件的姿態(tài)航向系統(tǒng)實(shí)現(xiàn)

摘要:針對(duì)目前捷聯(lián)式姿態(tài)航向系統(tǒng)低成本、低功耗、高精度、微型化的發(fā)展趨勢(shì)，提出一種基于MEMS器件的姿態(tài)航向系統(tǒng)的實(shí)現(xiàn)方案，介紹了該系統(tǒng)的方案設(shè)計(jì)與算法原理，實(shí)驗(yàn)證明該系統(tǒng)可以很好的測(cè)量出載體的姿態(tài)航向，可用于小型無人直升機(jī)的導(dǎo)航控制中。

關(guān)鍵詞:姿態(tài)航向系統(tǒng);MEMS器件;姿態(tài)算法;圓錐運(yùn)動(dòng)

中圖分類號(hào):U666文獻(xiàn)標(biāo)識(shí)碼:A文章編號(hào):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

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

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

1 系統(tǒng)方案設(shè)計(jì)

1.1 硬件設(shè)計(jì)

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

該文處理器模塊由STM32F103RBT6及其外圍電路構(gòu)成，72MHz主頻、32位數(shù)據(jù)寬度能夠?qū)崿F(xiàn)高速、高性能，同時(shí)可以方便的與慣性傳感器進(jìn)行數(shù)據(jù)通信，滿足姿態(tài)航向系統(tǒng)對(duì)運(yùn)算速度和精度的要求。

慣性數(shù)據(jù)采集模塊由ADIS16355單芯片實(shí)現(xiàn)，其六自由度、14位數(shù)字輸出的高集成度特性，省去了安裝和設(shè)計(jì)數(shù)據(jù)采集電路的麻煩，標(biāo)準(zhǔn)的SPI接口簡(jiǎn)化了傳感器與處理器的數(shù)據(jù)通信。使系統(tǒng)無需額外的解碼電路和邏輯控制電路便可以實(shí)現(xiàn)導(dǎo)航解算。RS-485模塊實(shí)現(xiàn)LVTTL/LVCMOS電平到RS-485電平之間的轉(zhuǎn)換，使導(dǎo)航信息通過RS-485總線傳輸?shù)絇C。實(shí)際應(yīng)用中，需要用到RS232/RS485轉(zhuǎn)換器將RS-485信號(hào)轉(zhuǎn)換成RS-232信號(hào)，然后與計(jì)算機(jī)通信。這里用RS-485傳輸而不用RS-232直接傳輸，主要是考慮到RS-485受外界干擾小，傳輸距離遠(yuǎn)，適合環(huán)境惡劣的場(chǎng)合。

1.2 軟件設(shè)計(jì)

本系統(tǒng)軟件設(shè)計(jì)主要包括傳感器數(shù)據(jù)采集、角度對(duì)準(zhǔn)和解算以及整個(gè)系統(tǒng)的任務(wù)調(diào)度。軟件流程圖如圖2所示。

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

2 姿態(tài)航向系統(tǒng)算法

2.1 坐標(biāo)系參數(shù)說明

本系統(tǒng)選取導(dǎo)航坐標(biāo)系為東北天地理坐標(biāo)系Oxtytzt，載體坐標(biāo)系為機(jī)體坐標(biāo)系Oxbybzb，其中xb指向載體的右側(cè)，yb指向載體的縱軸方向，zb指向載體的豎軸方向。載體坐標(biāo)系相對(duì)地理坐標(biāo)系的方位為載體的姿態(tài)角，包括俯仰角θ、橫滾角γ與航向角ψ。

2.2 捷聯(lián)姿態(tài)算法

載體具有靜止?fàn)顟B(tài)、低速運(yùn)動(dòng)狀態(tài)和高速旋轉(zhuǎn)狀態(tài)三種工作狀態(tài)，針對(duì)這三種狀態(tài)采用不同的姿態(tài)解算方法，以達(dá)到解算精度與計(jì)算量的平衡。

1) 加速度計(jì)確定姿態(tài)算法

在載體處于靜止或勻速直線運(yùn)動(dòng)時(shí)，加速度計(jì)可以單獨(dú)確定載體的橫滾和俯仰角[2]:

(1)

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

(2)

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

2) 四元數(shù)姿態(tài)算法

四元數(shù)算法適用于低速運(yùn)動(dòng)下的載體，姿態(tài)矩陣可以表示為姿態(tài)角θ，γ，ψ的關(guān)系:

(3)

設(shè)載體坐標(biāo)系相對(duì)地理坐標(biāo)系的轉(zhuǎn)動(dòng)四元數(shù)為:

(4)

四元數(shù)與姿態(tài)矩陣間的關(guān)系為:

(5)

四元數(shù)進(jìn)行歸一化，通過公式(6)即可求出姿態(tài)角:

(6)

3) 等效旋轉(zhuǎn)矢量算法

當(dāng)運(yùn)載體姿態(tài)變化劇烈時(shí)，四元數(shù)解算將會(huì)出現(xiàn)嚴(yán)重的不可交換性誤差，1971年JohnE.Bortz提出了等效旋轉(zhuǎn)矢量概念[3]，將運(yùn)載體的姿態(tài)四元數(shù)更新轉(zhuǎn)化為姿態(tài)變化四元數(shù)的更新，對(duì)不可交換誤差作了適當(dāng)補(bǔ)償，正好彌補(bǔ)了四元數(shù)法的不足。由于本系統(tǒng)采用的陀螺儀輸出為角速率，因此考慮角速率輸出情況下的等效旋轉(zhuǎn)矢量算法。

① 計(jì)算陀螺儀在一個(gè)姿態(tài)更新周期h內(nèi)的四次采樣值ω1，ω2，ω3，ω4;

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

③ 計(jì)算等效旋轉(zhuǎn)矢量 (7)

其中

錐運(yùn)動(dòng)環(huán)境下進(jìn)行優(yōu)化

(8)

(9)

④ 利用Φ=[Φx Φy， Φz(mì)]構(gòu)造一個(gè)變換四元數(shù)，即:

(10)

其中

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

2.3 姿態(tài)融合算法

對(duì)載體運(yùn)動(dòng)狀態(tài)的實(shí)時(shí)判斷是進(jìn)行姿態(tài)算法融合的依據(jù)，本系統(tǒng)提取兩方面信息判斷載體運(yùn)動(dòng)狀態(tài)。

1) 勻速運(yùn)動(dòng)門限值εa

在靜止或勻速運(yùn)動(dòng)的情況下，加速度計(jì)三軸的測(cè)量值應(yīng)滿足下式:

(11)

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

2) 旋轉(zhuǎn)運(yùn)動(dòng)門限值εω

旋轉(zhuǎn)運(yùn)動(dòng)可通過陀螺儀的角速度輸出加以判斷:

(12)

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

本系統(tǒng)將載體做高動(dòng)態(tài)運(yùn)動(dòng)的角速度門限值設(shè)為10°/s。

姿態(tài)融合算法是指載體在勻速運(yùn)動(dòng)或靜止時(shí)，利用加速度計(jì)和電子羅盤獲取姿態(tài)信息，同時(shí)每周期檢測(cè)系統(tǒng)的運(yùn)動(dòng)狀態(tài)，當(dāng)系統(tǒng)檢測(cè)到載體在進(jìn)行低速非旋轉(zhuǎn)運(yùn)動(dòng)時(shí)，就在上周期加速度計(jì)解算值的基礎(chǔ)上利用四元數(shù)算法繼續(xù)解算，若系統(tǒng)恢復(fù)勻速運(yùn)動(dòng)，就重新利用加速度計(jì)解算，當(dāng)系統(tǒng)檢測(cè)到載體做旋轉(zhuǎn)運(yùn)動(dòng)時(shí)，則直接用等效旋轉(zhuǎn)矢量算法進(jìn)行解算。

3 系統(tǒng)實(shí)驗(yàn)與仿真

3.1 低動(dòng)態(tài)環(huán)境下實(shí)驗(yàn)

在Keil μVision3集成開發(fā)環(huán)境下對(duì)系統(tǒng)進(jìn)行調(diào)試與實(shí)現(xiàn)，姿態(tài)算法部分采用C語言編寫。通過實(shí)時(shí)采集低動(dòng)態(tài)環(huán)境下傳感器輸出數(shù)據(jù)，并分別采用四元數(shù)算法和融合算法兩種解算模式進(jìn)行姿態(tài)解算，將解算結(jié)果輸入到matlab中進(jìn)行評(píng)估與分析。

系統(tǒng)解算初始條件設(shè)為:經(jīng)度38.8859°，緯度121.5243°，高度100m，俯仰角0.0°，橫滾角0.0°，方位角0.0°，姿態(tài)解算周期為0.01s，解算時(shí)間為40s。

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

由表1可以看出， 單純使用四元數(shù)解算，由于陀螺漂移的存在，算法誤差會(huì)隨時(shí)間增長(zhǎng)而累積，而通過實(shí)時(shí)判斷載體運(yùn)動(dòng)狀態(tài)，利用加速度計(jì)對(duì)陀螺漂移進(jìn)行重調(diào)補(bǔ)償，航向角的誤差漂移減小了一個(gè)數(shù)量級(jí)。在低動(dòng)態(tài)環(huán)境下，系統(tǒng)解算精度俯仰角與橫滾角誤差<0.8°/s，航向角誤差<0.2°/s，滿足系統(tǒng)設(shè)計(jì)要求。

3.2 高動(dòng)態(tài)環(huán)境下仿真

由于實(shí)驗(yàn)條件有限，無法確定載體的高動(dòng)態(tài)運(yùn)動(dòng)狀態(tài)，因此對(duì)于旋轉(zhuǎn)運(yùn)動(dòng)狀態(tài)下的解算采用MATLAB進(jìn)行仿真，以確定旋轉(zhuǎn)矢量算法的精確性。

以含誤差的圓錐運(yùn)動(dòng)作為仿真環(huán)境，仿真條件設(shè)置如下:

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

=ε+wg

其中ε為系統(tǒng)隨機(jī)常值漂移，wg為白噪聲，陀螺儀實(shí)際輸出為:

(14)

錐運(yùn)動(dòng)的半錐角α=1°、錐頻率為f=15Hz，系統(tǒng)陀螺儀隨機(jī)常值漂移為0.0015°/s，白噪聲密度為0.07°/s，姿態(tài)更新周期h=12ms、仿真時(shí)間tmax=60s。

仿真結(jié)果如圖5、6所示。

由仿真結(jié)果可以看出，當(dāng)載體做高速旋轉(zhuǎn)運(yùn)動(dòng)時(shí)，姿態(tài)角誤差增大，旋轉(zhuǎn)矢量算法精度高于四元數(shù)解算精度。

4 結(jié)論

該文以STM32F103RBT6為核心處理器，以ADIS16355為慣性檢測(cè)傳感器，設(shè)計(jì)實(shí)現(xiàn)了一個(gè)小型姿態(tài)航向系統(tǒng)，實(shí)驗(yàn)表明采用嵌入式處理器及基于MEMS的慣性傳感器，經(jīng)過合理的軟、硬件設(shè)計(jì)，可以很好的完成導(dǎo)航任務(wù)，實(shí)現(xiàn)系統(tǒng)的微型化和低成本，而根據(jù)載體運(yùn)動(dòng)狀態(tài)采用不同的姿態(tài)解算算法，則可以提高不同環(huán)境下導(dǎo)航系統(tǒng)可靠性，滿足工程中對(duì)系統(tǒng)精度的要求，實(shí)現(xiàn)預(yù)期的設(shè)計(jì)目標(biāo)。

參考文獻(xiàn):

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

[2] 沈曉蓉，張海，范耀祖.加速度計(jì)輔助模糊混合捷聯(lián)姿態(tài)確定算法研究[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的系統(tǒng)仿真技術(shù)與應(yīng)用[M].北京:清華大學(xué)出版社，2002:281-296

[5] 黃旭，王常虹.基于微機(jī)械慣性器件和磁強(qiáng)計(jì)的姿態(tài)航向系統(tǒng)[J].傳感器技術(shù)，2005(8):54-56.