景 希,高國偉,2

(1.北京信息科技大學 傳感器北京市重點實驗室，北京 100101;2.北京信息科技大學 現(xiàn)代測控技術(shù)教育部重點實驗室，北京 100101)

0 引言

隨著MEMS技術(shù)的日益發(fā)展，MEMS(micro electromechanical system)的加工工藝應用在一些慣性器件，陀螺儀是一種能夠敏感載體角度或角速度的慣性器件[1]。傳統(tǒng)的陀螺儀是利用高速轉(zhuǎn)動的物體具有保持其角動量的特性來測量角速度的，傳統(tǒng)機械陀螺儀由于在制作上成本較高，而且體積較大，壽命短，不適合用于批量生產(chǎn)，限制了其應用，一般僅用于導航方面，而MEMS陀螺儀利用科里奧利力[2]，可以感知載體的轉(zhuǎn)動角速率，有更好的性能，微細的加工工藝大幅度地減小了體積，降低了生產(chǎn)成本，功耗低，質(zhì)量輕，拓展了應用領(lǐng)域，除了復雜的導航、軍工領(lǐng)域，在攝像機、運動機械、汽車等領(lǐng)域也發(fā)揮了較大的作用[3]。

照相機拍攝時需要穩(wěn)定的拍攝環(huán)境才能有清晰的拍攝效果，目前除了照相機內(nèi)部有防抖動模式和動態(tài)模式，還有很多工具能夠幫助穩(wěn)定相機，比如豆袋、吸盤支架、三腳架等。三腳架是穩(wěn)定相機最好的裝置，性能較高的三腳架價格也比較昂貴，一般用于專業(yè)影視劇拍攝，比如美國歐克諾品牌一款云臺三腳套裝，金額高達十六萬。本文討論捷聯(lián)式測量系統(tǒng)，將慣性器件陀螺儀和加速度計模塊固定在三腳架上，可以測量三腳架靜態(tài)姿態(tài)，保持其穩(wěn)定拍攝。

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

1.1 MEMS陀螺儀工作原理

MEMS陀螺儀的主要原理是通過科里奧利加速度，在驅(qū)動模式和檢測模式之間轉(zhuǎn)移能量。陀螺儀通常有徑向和橫向兩個方向的電容板，徑向電容板使物體做徑向運動，橫向的電容板測量由于橫向科里奧利運動帶來的電容變化，從而得到科里奧利力。因為科里奧利力正比于角速度，所以由電容的變化可以計算出角速度[4]。

1.2 姿態(tài)檢測優(yōu)點及發(fā)展趨勢

姿態(tài)檢測系統(tǒng)不依賴外部信息，并且具有良好的隱蔽性，不向外輻射能量，也不受電磁干擾影響，它能夠提供各種不同的數(shù)據(jù)，比如位置、航向、速度和姿態(tài)角，提供良好的連續(xù)性和低噪聲的導航信息，數(shù)據(jù)更新率高，穩(wěn)定性好[5]。

陀螺儀和加速度計是慣性制導系統(tǒng)中必要的兩個核心裝置[6]。隨著需求精度的提高，對陀螺儀和加速度計提出了更高的要求。陀螺儀的漂移誤差和加速度計的零偏差影響著整個系統(tǒng)精度[7]。陀螺儀和加速度計測量精度的提高和性能的提高一直備受關(guān)注。我國慣導技術(shù)已經(jīng)逐漸發(fā)展，有了飛躍性的進步，利用捷聯(lián)慣導技術(shù)對采煤機進行定位，液體浮動陀螺平臺系統(tǒng)和動態(tài)調(diào)優(yōu)陀螺四軸平臺系統(tǒng)，已應用于世界著名的長征系列運載火箭。

1.3 系統(tǒng)框架

根據(jù)系統(tǒng)的功能需求，姿態(tài)測量又陀螺儀模塊和加速度計輸出模塊，系統(tǒng)由兩個部分組成：上位機和下位機。下位機是一個硬件結(jié)構(gòu)。它主要由MEMS傳感器、STM32微控制器和通信模塊組成，負責數(shù)據(jù)的收集、處理和傳輸。MEMS陀螺儀和加速度計形成精度高、抗振動能力強的姿態(tài)模塊，并能實時準確地獲得物體運動中的角速度和加速度數(shù)據(jù)。

通信模塊將接收到的數(shù)據(jù)傳輸?shù)缴衔粰C，完成系統(tǒng)之間的通信，通信模塊選用RS232串口作為系統(tǒng)與上位機的連接。

系統(tǒng)原理如圖1所示。

圖1 系統(tǒng)原理框圖

系統(tǒng)采集到三軸加速度計和三軸陀螺儀的信號，通過SPI串口接受加速度計和陀螺儀的信號，通過時鐘頻率設(shè)置計數(shù)頻率以及采樣頻率，控制器內(nèi)部對讀取到的數(shù)據(jù)信息進行數(shù)據(jù)濾波[8]、解算處理，然后將控制器與上位機以RS232串口相連接，在上位機中顯示輸出數(shù)據(jù)，輸出為處理后的加速度計數(shù)據(jù)和三個陀螺儀數(shù)據(jù)。

2 系統(tǒng)硬件設(shè)計

2.1 ADXL355加速度器

選用ADXL355型號的加速度器，其封裝能夠保證長期的穩(wěn)定性，低功耗，噪聲低，使獲得的數(shù)據(jù)信息比較穩(wěn)定，工作溫度范圍：-40～125 ℃，分辨率高(256 000/g)，內(nèi)部集成20位ADC,具有數(shù)字串行外設(shè)接口SPI接口，便于加速度器與其他傳感器和主控制器相連接，更好地進行通信，提高可靠性。加速度計的SPI通信接口引腳與控制器STM32引腳PB0連接，SPI接口中的MOSI、MISO、SCK、分別接STM32的SPI總線。ADXL355原理如圖2所示。

圖2 ADXL355原理圖

2.2 STM32F103RBT6介紹

姿態(tài)測量以STM32F103RB為核心，是一種功耗低性能高的微控制器，控制外圍電路以及完成對數(shù)據(jù)的處理。供電電壓為2.0 ～3 V,以高達72 MHz的CPU頻率工作，能夠高速處理數(shù)據(jù)，快速解算姿態(tài)角，運用了串行外設(shè)接口(SPI)，支持三線全雙工同步傳輸，內(nèi)置的I2C、USART 模塊可以簡化程序設(shè)計、減輕控制器的工作強度。STM32具有12位模數(shù)轉(zhuǎn)換器，將敏感器件模擬信號轉(zhuǎn)化為數(shù)字信號，輸入時鐘經(jīng)過PCLK2分頻產(chǎn)生8 MHz。

單邊機正常供電需要3.3 V電壓，所以我們選用ASM1117-3.3 V的穩(wěn)壓芯片穩(wěn)壓，為確保AMS1117的穩(wěn)定性，輸出需要連接一個10 μF的鉭電容。5 V電壓輸入AMS1117進行轉(zhuǎn)換為3.3 V，如圖3所示。

圖3 MCU供電模塊

2.3 陀螺儀介紹

ADXR450芯片系統(tǒng)是一款角速率陀螺儀，由陀螺芯片、BOOST升壓電路和濾波電容構(gòu)成，分辨率高，噪聲低，0.015°/(sec/g)，采用差分式傳感器設(shè)計，可以抑制線性加速度的影響，數(shù)字串行外設(shè)接口SPI接口，便于陀螺儀和主控制器相連接，更好地實現(xiàn)通信，通信接口與控制器具有SPI串口功能的引腳連接，提高可靠性，每個傳感結(jié)構(gòu)都包含了一個抖動框架，該框架通過經(jīng)典驅(qū)動實現(xiàn)共振，這就產(chǎn)生了必要的速度元素，在經(jīng)歷角速度時產(chǎn)生科里奧利力，感知科里奧利力的電容式傳感器結(jié)構(gòu)將所得信號饋送到一系列的增益和解調(diào)階段，這些階段產(chǎn)生電速率信號輸出。

本次使用的是一款單軸陀螺儀，因此需要用到三個陀螺儀分別測量X、Y、Z三個軸方向的角速率。在電路板中放置時，應確保使三個陀螺儀敏感軸相互垂直，有利于提高敏感軸輸出值的準確度。將測量X軸陀螺儀芯片10號引腳與控制器PC4相連接，測量Y軸陀螺儀芯片10號引腳與控制器PB1相連接，測量Z軸陀螺儀芯片10號引腳與控制器PB11相連接，每個陀螺儀的3、5、12引腳分別與控制器PB13、PB14、PB15引腳連接，完成信號的通信。該陀螺儀電路如圖4所示。

圖4 陀螺儀電路

3 系統(tǒng)軟件設(shè)計

3.1 坐標變換及姿態(tài)角

慣性測量單元固定安裝在運動物體上，將運動物體作為研究對象，確定運動物體的姿態(tài)的坐標系稱為載體坐標系b，導航坐標系n是不變的，將導航坐標系n分別繞Z軸X軸Y軸旋轉(zhuǎn)Ψ、θ、γ就得到載體坐標系，姿態(tài)角指兩個坐標系各軸之間夾角。探究載體坐標系和導航坐標系的方位關(guān)系就可以確定研究對象的姿態(tài)，將導航坐標系分別繞Z軸X軸Y軸旋轉(zhuǎn)Ψ、θ、γ，每旋轉(zhuǎn)一次對應的矩陣變換可以表示為：

(1)

(2)

(3)

三次旋轉(zhuǎn)后載體坐標系和導航坐標系的位置可以由方向余弦矩陣表示:

(4)

即實現(xiàn)導航坐標系轉(zhuǎn)換為載體坐標系：

(5)

Cbn=CnbT=

(6)

即實現(xiàn)載體坐標系轉(zhuǎn)換為導航坐標系：

(7)

(8)

(9)

θ=acrsin(R32)

(10)

(11)

由此可以計算出姿態(tài)角數(shù)據(jù)。

3.2 姿態(tài)測量

姿態(tài)檢測系統(tǒng)是指量測各傳感器的信息,通過數(shù)據(jù)處理解析出載體的姿態(tài)、速度和位置等信息,提供給檢測控制系統(tǒng),達到對載體的感知[9-10]。使用捷聯(lián)式安裝，測量技術(shù)的原理是將陀螺儀直接固定到需要測量的物體上，陀螺儀測量在慣性空間中3個方向軸旋轉(zhuǎn)的角速度[11]，對速度進行積分以獲得姿態(tài)信息，然后根據(jù)坐標變換計算得出數(shù)據(jù)，將數(shù)據(jù)轉(zhuǎn)換為系統(tǒng)坐標，獲得新的坐標系統(tǒng)中的姿態(tài)信息，從而可以判斷平臺的穩(wěn)定性[12]。

將陀螺儀固定在照相機三腳架上，先測量靜態(tài)時由于重力所得角速度值，然后再測量動態(tài)時由于照相抖動或外界因素造成的抖動大小。

姿態(tài)測量系統(tǒng)用模塊化方式，包含了系統(tǒng)初始化、采集傳感器信號模塊和姿態(tài)角解算模塊。數(shù)據(jù)采集模塊包含了加速度計陀螺儀和控制器的模數(shù)轉(zhuǎn)換依據(jù)數(shù)據(jù)讀取模塊。姿態(tài)解算系統(tǒng)工作流程如圖5所示。

圖5 姿態(tài)解算系統(tǒng)工作流程圖

3.3 濾波算法

卡爾曼濾波算法是一種利用遞推的方法估算系統(tǒng)運動過程的狀態(tài)，狀態(tài)估計是卡爾曼濾波的重要組成部分。一般來說，根據(jù)觀測數(shù)據(jù)對隨機量進行定量推斷就是估計問題，特別是對動態(tài)行為的狀態(tài)估計，能夠?qū)崿F(xiàn)實時運行狀態(tài)的估計和預測功能。

系統(tǒng)狀態(tài)空間模型：

X(k)=AX(k-1)+BU(k-1)+W(k-1)

(12)

Z(k)=HX(k)+V(k)

(13)

X(k)是k時刻的系統(tǒng)狀態(tài)，U(k)是k時刻對系統(tǒng)的控制量。A和B是系統(tǒng)參數(shù)矩陣。Z(k)是k時刻的測量值，H是測量系統(tǒng)的參數(shù)矩陣。W(k)和V(k)分別表示過程和測量的噪聲，并假設(shè)它們是相互獨立并滿足正態(tài)分布。

卡爾曼濾波算法：

時間更新方程：

X(k|k-1)=AX(k-1|k-1)+BU(k)

(14)

P(k│k-1)=AP(k-1|k-1)A′+Q

(15)

校正：

(16)

X(k|k)=X(k|k-1)+K(Z(k)-HX(k|k-1))

(17)

P(k|k)=(I-KH)P(k|k-1)

(18)

X(k|k)是k時刻的系統(tǒng)狀態(tài)，U(k)是k時刻對系統(tǒng)的控制量(如果沒有控制量，可以為零)，A是狀態(tài)轉(zhuǎn)移矩陣，B是系統(tǒng)參數(shù)；X(k-1|k-1)是上一狀態(tài)的最優(yōu)結(jié)果值，X(k|k-1)通過利用上一狀態(tài)預測后的值，這樣可以同步更新系統(tǒng)的結(jié)果。P(k-1|k-1)為上一狀態(tài)X(k-1|k-1)s的協(xié)方差，P(k|k-1)是X(k|k-1)對應的協(xié)方差，Q是系統(tǒng)過程噪聲(高斯白噪聲)的協(xié)方差矩陣，用于對系統(tǒng)結(jié)果對應的協(xié)方差進行更新；K為卡爾曼增益，H為觀測矩陣。

陀螺儀噪聲的存在對結(jié)果會有較大的影響，如果信號不加處理直接輸出，會有較大的誤差，不能準確地獲得精準的角度，因此需要對數(shù)據(jù)進行卡爾曼濾波，獲取在靜止時刻器件信號輸出的數(shù)據(jù)。

RC濾波算法：

Xout0=Xout1+(Xin-Xout1)/num

(19)

Xin是輸入值，Xout1是上次濾波后的輸出值，Xout0是本次濾波結(jié)果，num是和RC值有關(guān)的一個數(shù)值，稱之為濾波系數(shù)，num的值決定新的采樣值在本次濾波結(jié)果中所占的比例的大小。當前采樣的輸入值對濾波起到修正作用，濾波結(jié)果主要依據(jù)上次濾波后的輸出值。濾波系數(shù)num越小，響應快，靈敏度較高，但濾波結(jié)果越不穩(wěn)定，濾波系數(shù)num越大，響應慢，濾波結(jié)果平穩(wěn)，但是靈敏度低。

4 實驗結(jié)果與分析

4.1 實驗方案

為了測試傳感器準確性，將傳感器穩(wěn)定置于三軸轉(zhuǎn)臺上，上電預熱十分鐘，控制三周專挑，首先令內(nèi)軸、中軸、外軸均為0°，控制內(nèi)軸中軸位置方式，速率方式和加速度速率，令速率方式為10°/s，加速度速率為10°/s2，將內(nèi)軸角度依次設(shè)置為-20～20°，每10°旋轉(zhuǎn)一個角度，中軸設(shè)置為0°，然后中軸角度依次設(shè)置為-20～20°，每10°旋轉(zhuǎn)一個角度，內(nèi)軸設(shè)置為0°。記錄每個角度下傳感器輸出角度，利用誤差模型計算傳感器數(shù)據(jù)，然后代入公式分別解算X軸和Y軸的姿態(tài)角。

4.2 實驗結(jié)果

測試靜態(tài)下傳感器輸出角度，將三軸轉(zhuǎn)臺設(shè)定內(nèi)軸為43.5°，中軸設(shè)定為0°。分別用3種方式進行數(shù)據(jù)處理。圖6為加速度計X軸原始數(shù)據(jù)輸出結(jié)果，該曲線表示在靜態(tài)環(huán)境下傳感器濾波降噪前的數(shù)據(jù)輸出，分析曲線可知，傳感器輸出最大值為69.9°，最小值為23.8°，和設(shè)定值43.5°相差甚遠，角度偏差過大的原因可以考慮為在某一時刻轉(zhuǎn)臺因人為因素受震動導致的數(shù)據(jù)偏差，除了人為因素導致的偏差數(shù)據(jù)段，其他時刻的數(shù)據(jù)輸出約在38～50°，最大偏差角達到了6.3°，最大偏差達到了14.94%，數(shù)據(jù)輸出不穩(wěn)定，波動幅度大。原始數(shù)據(jù)如圖6所示。

圖6 原始數(shù)據(jù)

圖7是對加速度計X軸原始數(shù)據(jù)進行卡爾曼濾波處理后數(shù)據(jù)輸出，令過程噪聲Q=0.018，分析曲線可知，傳感器輸出最大值為54.9°，最小值為32.7°，經(jīng)過卡爾曼濾波后，因人為因素導致的數(shù)據(jù)波動較原始輸出數(shù)據(jù)波動減少了23.9°，可見卡爾曼濾波效果顯著，體現(xiàn)該濾波算法實時運行狀態(tài)的估計和預測功能。其他時刻的數(shù)據(jù)輸出約在41°至46°，最大偏差角達到了1.4°，最大偏差達到了3.2%，數(shù)據(jù)輸出仍有波動，較原始波動性小，受波動影響較大，但數(shù)據(jù)響應較快。

圖7 卡爾曼濾波

圖8是對加速度計X軸原始數(shù)據(jù)進行一階RC濾波的結(jié)果，其中系數(shù)num為100，圖9是經(jīng)過一階RC濾波的結(jié)果，系數(shù)num為1 000。RC濾波對數(shù)據(jù)處理的速率較緩，num為100時數(shù)據(jù)穩(wěn)定需要11 s,而num為1 000時數(shù)據(jù)穩(wěn)定需要72 s，其穩(wěn)定速率受系數(shù)num影響，num越大，數(shù)據(jù)達穩(wěn)定較慢，圖8中，震動情況下，傳感器輸出最大值43.774°，最小值42.562°，經(jīng)過一階RC濾波后，因人為因素導致的數(shù)據(jù)波動較原始輸出數(shù)據(jù)波動減少了44.888°，震動時刻對整體濾波結(jié)果影響微乎其微。其他時刻的數(shù)據(jù)輸出約在43.604～43.510°，最大偏差角達到了0.104°，最大偏差達到了0.24%，數(shù)據(jù)波動幅度小，穩(wěn)定度較高。經(jīng)過一階RC濾波，系統(tǒng)響應快，靈敏度較高，濾波結(jié)果越穩(wěn)定，選取系數(shù)num應適合于系統(tǒng)實際應用情況。

圖8 RC濾波num=100

圖9 RC濾波num=1 000

綜合分析得卡爾曼濾波性能動態(tài)響應快，實時性能較好，但濾波效果不理想，這是由于選取濾波參數(shù)量測噪聲和過程噪聲系數(shù)而造成的，在時間更新方程中，由于對過程噪聲的不確定，對狀態(tài)預測時，將量測噪聲設(shè)置成0。

實驗中，將狀態(tài)轉(zhuǎn)移矩陣設(shè)定為單位向量，過程噪聲參數(shù)Q設(shè)置為0.018，參數(shù)的選取取決于硬件設(shè)施和外部環(huán)境等因素，需要結(jié)合經(jīng)驗合適的選取，難于合適的選取，卡爾曼濾波具有比較好的實時性，而實驗中對實時性要求不高，所以考慮了一階RC濾波。經(jīng)過對比發(fā)現(xiàn)，當系數(shù)為100時，一階RC動態(tài)響應較快，而且濾波效果也能達到滿意的效果，當系數(shù)為1 000時，濾波效果更加平穩(wěn)，經(jīng)過濾波后的數(shù)據(jù)和原始輸出數(shù)據(jù)相比較，由于系統(tǒng)輸出響應太慢，缺失了很大一部分數(shù)據(jù)，不能夠快速的對數(shù)據(jù)進行處理，反映出經(jīng)過RC濾波降噪后的輸出噪聲數(shù)據(jù)受到了極大的抑制，該系統(tǒng)選取num=100。

4.3 誤差分析

在本文中討論的MEMS陀螺儀采用的是ADXR450型號，該陀螺儀用作檢測加速度和加速度的敏感性元器件，MEMS陀螺儀的精度隨環(huán)境的變化而產(chǎn)生誤差，誤差來源主要是兩個方面：第一個方面是由于室外溫度的變化是陀螺儀產(chǎn)生干擾力矩；第二個方面是由于照相機拍攝時，地球的自傳和路面的平緩度以及拍照瞬時造成的抖動，對陀螺儀產(chǎn)生干擾。隨機漂移造成的誤差，由零偏和速度隨機游走造成，零偏是傳感器在零輸入時系統(tǒng)由自身原因而產(chǎn)生的輸出值。系統(tǒng)漂移，由預熱偏差和溫度敏感造成，還有交叉耦合誤差和軸失準誤差。在環(huán)境變化較大時測量勢必會造成檢測值的降低，所以首先需要對誤差進行分析，了解誤差來源，然后對數(shù)據(jù)進行測量分析，建立誤差模型，進行校準，最后盡可能得到實際測量的角度。

5 結(jié)束語

基于STM32單片機的姿態(tài)測量系統(tǒng)，以STM32微控制器核心芯片，搭配加速度計模塊和陀螺儀模塊和通信模塊，文中介紹了MEMS器件的工作原理以及系統(tǒng)的硬件內(nèi)容分析和軟件部分，對測量數(shù)據(jù)的誤差進行了分析。實驗采用的單軸陀螺儀測量角度，加速度計測量加速度值，分別比較兩種不同的數(shù)據(jù)處理方式，得到了較理想的結(jié)果。但是誤差標定實驗是在實驗室常溫環(huán)境下完成，忽略了溫度對敏感元器件的影響，在實際運用中由于溫度對敏感元器造成的漂移情況不能忽視，因此可以加入溫度補償，標定不同溫度下的溫度影響因子，這是一個可以改進傳感器精度的方面。隨著微電子技術(shù)和集成電路的發(fā)展，除了軟件溫度補償方式以外，可以在傳感器敏感元器件外圍增加恒溫控制電路模塊，從硬件電路角度改進溫度漂移產(chǎn)生的誤差。