景 希,高國偉,2

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

0 引言

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

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

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

1.1 MEMS陀螺儀工作原理

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

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

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

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

1.3 系統(tǒng)框架

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

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

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

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

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

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

2.1 ADXL355加速度器

選用ADXL355型號的加速度器，其封裝能夠保證長期的穩(wěn)定性，低功耗，噪聲低，使獲得的數(shù)據(jù)信息比較穩(wěn)定，工作溫度范圍：-40～125 ℃，分辨率高(256 000/g)，內(nèi)部集成20位ADC,具有數(shù)字串行外設(shè)接口SPI接口，便于加速度器與其他傳感器和主控制器相連接，更好地進(jìn)行通信，提高可靠性。加速度計(jì)的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,以高達(dá)72 MHz的CPU頻率工作，能夠高速處理數(shù)據(jù)，快速解算姿態(tài)角，運(yùn)用了串行外設(shè)接口(SPI)，支持三線全雙工同步傳輸，內(nèi)置的I2C、USART 模塊可以簡化程序設(shè)計(jì)、減輕控制器的工作強(qiáng)度。STM32具有12位模數(shù)轉(zhuǎn)換器，將敏感器件模擬信號轉(zhuǎn)化為數(shù)字信號，輸入時鐘經(jīng)過PCLK2分頻產(chǎn)生8 MHz。

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

圖3 MCU供電模塊

2.3 陀螺儀介紹

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

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

圖4 陀螺儀電路

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

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

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

(1)

(2)

(3)

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

(4)

即實(shí)現(xiàn)導(dǎo)航坐標(biāo)系轉(zhuǎn)換為載體坐標(biāo)系：

(5)

Cbn=CnbT=

(6)

即實(shí)現(xiàn)載體坐標(biāo)系轉(zhuǎn)換為導(dǎo)航坐標(biāo)系：

(7)

(8)

(9)

θ=acrsin(R32)

(10)

(11)

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

3.2 姿態(tài)測量

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

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

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

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

3.3 濾波算法

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

系統(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è)它們是相互獨(dú)立并滿足正態(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)預(yù)測后的值，這樣可以同步更新系統(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)對應(yīng)的協(xié)方差，Q是系統(tǒng)過程噪聲(高斯白噪聲)的協(xié)方差矩陣，用于對系統(tǒng)結(jié)果對應(yīng)的協(xié)方差進(jìn)行更新；K為卡爾曼增益，H為觀測矩陣。

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

RC濾波算法：

Xout0=Xout1+(Xin-Xout1)/num

(19)

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

4 實(shí)驗(yàn)結(jié)果與分析

4.1 實(shí)驗(yàn)方案

為了測試傳感器準(zhǔn)確性，將傳感器穩(wěn)定置于三軸轉(zhuǎn)臺上，上電預(yù)熱十分鐘，控制三周專挑，首先令內(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°。記錄每個角度下傳感器輸出角度，利用誤差模型計(jì)算傳感器數(shù)據(jù)，然后代入公式分別解算X軸和Y軸的姿態(tài)角。

4.2 實(shí)驗(yàn)結(jié)果

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

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

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

圖7 卡爾曼濾波

圖8是對加速度計(jì)X軸原始數(shù)據(jù)進(jìn)行一階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ù)達(dá)穩(wěn)定較慢，圖8中，震動情況下，傳感器輸出最大值43.774°，最小值42.562°，經(jīng)過一階RC濾波后，因人為因素導(dǎo)致的數(shù)據(jù)波動較原始輸出數(shù)據(jù)波動減少了44.888°，震動時刻對整體濾波結(jié)果影響微乎其微。其他時刻的數(shù)據(jù)輸出約在43.604～43.510°，最大偏差角達(dá)到了0.104°，最大偏差達(dá)到了0.24%，數(shù)據(jù)波動幅度小，穩(wěn)定度較高。經(jīng)過一階RC濾波，系統(tǒng)響應(yīng)快，靈敏度較高，濾波結(jié)果越穩(wěn)定，選取系數(shù)num應(yīng)適合于系統(tǒng)實(shí)際應(yīng)用情況。

圖8 RC濾波num=100

圖9 RC濾波num=1 000

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

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

4.3 誤差分析

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

5 結(jié)束語

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