基于六軸MEMS器件姿態測量系統

2021-06-30 12:44:30高國偉

計算機測量與控制 2021年6期

景希,高國偉,2

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

0 引言

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

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

1 系統結構及原理

1.1 MEMS陀螺儀工作原理

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

1.2 姿態檢測優點及發展趨勢

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

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

1.3 系統框架

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

通信模塊將接收到的數據傳輸到上位機，完成系統之間的通信，通信模塊選用RS232串口作為系統與上位機的連接。

系統原理如圖1所示。

圖1 系統原理框圖

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

2 系統硬件設計

2.1 ADXL355加速度器

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

圖2 ADXL355原理圖

2.2 STM32F103RBT6介紹

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

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

圖3 MCU供電模塊

2.3 陀螺儀介紹

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

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

圖4 陀螺儀電路

3 系統軟件設計

3.1 坐標變換及姿態角

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

(1)

(2)

(3)

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

(4)

即實現導航坐標系轉換為載體坐標系：

(5)

Cbn=CnbT=

(6)

即實現載體坐標系轉換為導航坐標系：

(7)

(8)

(9)

θ=acrsin(R32)

(10)

(11)

由此可以計算出姿態角數據。

3.2 姿態測量

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

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

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

圖5 姿態解算系統工作流程圖

3.3 濾波算法

卡爾曼濾波算法是一種利用遞推的方法估算系統運動過程的狀態，狀態估計是卡爾曼濾波的重要組成部分。一般來說，根據觀測數據對隨機量進行定量推斷就是估計問題，特別是對動態行為的狀態估計，能夠實現實時運行狀態的估計和預測功能。

系統狀態空間模型：

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

(12)

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

(13)

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

卡爾曼濾波算法：

時間更新方程：

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)

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

RC濾波算法：

Xout0=Xout1+(Xin-Xout1)/num

(19)

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

4 實驗結果與分析

4.1 實驗方案

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

4.2 實驗結果

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

圖6 原始數據

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

圖7 卡爾曼濾波

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

圖8 RC濾波num=100

圖9 RC濾波num=1 000

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

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

4.3 誤差分析

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

5 結束語

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