基于MPU-6050和UKF的姿態測量系統設計

陶子涵 高國偉 徐萬芝

北京信息科技大學 信息獲取與檢測實驗室 北京 100101

一、引言

姿態信息是導航系統中最為關鍵的參數之一[1]，姿態測量系統在航空航天[2]、車輛船舶、機器人、人體姿態捕捉等諸多領域中被廣泛使用。而在姿態測量系統中，人們最關心的就是測量精度。

對姿態測量系統精度造成影響的因素主要來源于兩個方面：

一是系統的硬件配置，主要包括獲取原始姿態信息的傳感器原件和所選擇的處理器。現今陀螺儀測姿定姿技術已經相當成熟，在航空航天、空間或軍事等重要領域，采用高精度光學陀螺能進行準確的導航定姿，但因其成本高而使應用范圍受到限制。而基于MEMS慣性測量元件設計的姿態測量系統價格低廉、穩定可靠，目前已被廣泛的應用于民用微小型飛行器、老人防跌倒報警器、智能家居等許多方面。

其次，姿態估計所使用的算法也對姿態測量的精度有很大影響，只有選取的算法適用于系統的實際情況才能取得良好的效果，例如Hajiyev C及趙琳等人將EKF算法應用于姿態估計[3-4]取得了良好的估計效果。

綜合考慮微小型無人機的應用背景[5]，設計了一款性價比較高的姿態測量系統。系統選用MPU-6050傳感器[6-7]作為其數據采集模塊的核心元件，考慮到UPF算法的實時性和有效性均能滿足系統需求，所以采用UPF算法[8-10]完成系統姿態估計。針對MEMS傳感器精度低、漂移大等特點[11]，設計了合理可行的軟件解決方案。

二、系統總體設計

1、系統硬件設計

系統的整體結構框圖如圖1所示，主要由MPU-6050姿態檢測電路及其外圍電路、微控制器電路、串口通信接口電路、電源電路、等組成。通過姿態檢測電路獲取載體的姿態信息，經過A/D轉換電路將數據傳輸至微處理器對姿態進行解算和更新，最后采用串口輸出姿態解算結果在PC機中顯示。

MPU-6050慣性測量模塊是美國InvenSense公司于2009年推出的一款低成本的MEMS傳感器，是全球第一款整合型六軸運動傳感器，其數據采集能力具有高速、精確的特點，在本系統中用于獲取姿態信息原始數據。

控制核心選用STM32F103C8T6微處理器。由于姿態估計過程要進行大量的矩陣運算和高階運算，以完成狀態預測、狀態估計和更新、量測更新、均值和均方差等諸多運算，因此對系統的運算精度和速度要求較高，STM32F103C8T6是ST公司的ARM32位MCU，屬于STM32系列增強型芯片之一，處理頻率為72Hz，能夠滿足姿態解算的要求。

系統采用RS232接口與PC機進行通信，傳輸方式選擇為非平衡傳輸，工作為電壓±12V，由于STM32F103C8T6最高工作電壓為3.6V，不符合RS232接口的電壓要求，因此采用MAX3232芯片來進行電平轉換。

2、系統軟件設計

本系統的軟件設計整體流程圖如圖2所示。首先對系統進行初始化。初始對準即將姿態測量系統對準到運動體的初始姿態，初始對準之后，采集運動體的姿態信息傳輸至STM32微處理器，作為姿態估計算法的輸入量，然后進行姿態解算和更新，并將四元數表示的姿態信息轉化為俯仰角、橫滾角和偏航角經由串口輸出，在上位機中顯示。

三、姿態估計算法

1、狀態方程

不同的姿態參數表示的動力學模型表達形式不同，基于四元數推導的姿態動力學模型[7]為：

其中，Ωb—載體坐標系相對于地理坐標系上的角速度分量，可由MPU-6050的陀螺儀部分測得，其矩陣表達式為：

其中，ωx—滾轉角速率；

ωy—俯仰角速率；

ωz—偏航角速率。

即四元數動力學微分方程為：

假設陀螺儀的偏差僅受高斯白噪聲影響，陀螺儀部分的輸出模型為：

ω—載體真實角速度；

ωd—陀螺儀真實漂移；

W1—陀螺儀輸出噪聲；

—載體速度估計值；

—陀螺漂移估計值。

設Δωd為陀螺漂移估計誤差，則：

其中，W1，W2設為零均值白噪聲。

設過程噪聲Wk為可加性噪聲，其統計特性為：Wk～N(0,Qk)，由（2）（5）組成的狀態方程為：

將陀螺漂移與姿態四元數一起選為系統狀態向量，可以對陀螺漂移進行實時補償，解決了MEMS陀螺漂移較大引起的姿態估計精度下降的問題。

2、量測方程

MPU-6050傳感器的加速度輸出模型可按照下式建立：

an—地理系下的重力向量，an=[0 0g]T；

V—傳感器輸出噪聲。

設量測噪聲也為可加性噪聲，其統計特性為Vk～N(0,Rk)，即量測方程為：

綜上，狀態方程和量測方程滿足：

3、UKF算法[6]

微小型無人機姿態測量系統是一種典型的非線性系統，若采用EKF算法進行濾波，必須對原系統和量測作泰勒級數展開并且只保留線性項，在系統非線性度較高的情況下，EKF的濾波精度會大大降低，因此只適用于弱非線性對象的估計問題。

UKF是基于一種UT變換的濾波算法，它首先根據初始狀態均值和方差矩陣產生k時刻的sigma點，計算sigma點時所給的方差矩陣必須是正定矩陣。將sigma點帶入到狀態方程中計算出變換的樣本點并確定相應的權值，完成狀態預測。然后計算k時刻的增廣樣本點，帶入到觀測方程計算出預測觀測模型的樣本點，完成觀測模型預測。最后根據上面計算出的狀態預測方程和觀測預測方程，計算出變換后的均值和方差矩陣及增益矩陣，計算出濾波值，完成狀態估計和更新從而達到濾波的目的。

UKF算法步驟如下：

（1）選定濾波初值：

（2）計算sigma樣本點

（3）時間更新

（4）量測更新

4、實驗測試

選用SGT320E型多功能三軸轉臺對本文所設計的姿態測量系統進行測試，將測量系統固定在轉臺上，控制轉臺位置使得測量系統初始角均為零度，采樣頻率設置為500Hz，轉臺轉速設置為2°/s，采樣點為600個，測量出系統輸出的角度誤差以驗證系統對運動體各姿態角度的檢測效果。

通過實驗對比了采用EKF和采用UKF兩種姿態估計算法下系統的俯仰角、滾轉角和偏航角的輸出角度誤差，如圖3、4、5所示，EKF和UKF濾波誤差對比如表1所示。

由表1統計結果表明，UKF用于姿態估計的誤差均值、方差及最大誤差都明顯低于EKF，說明UKF具有更高的濾波精度。

四、結論

表1 EKF和UKF濾波誤差對比

本文設計了一種基于MPU-6050六軸姿態傳感器的微型姿態測量系統，針對現有低成本MEMS傳感器所存在的測量精度較低、漂移大等問題，提出了基于UKF算法的姿態估計方法應用于所設計的姿態測量系統。文章首先簡要的介紹了姿態測量系統的整體硬件結構設計和軟件設計流程，然后對系統的狀態方程和量測方程進行了分析并給出了UKF算法，最后通過實驗及實驗數據對所設計的系統進行了分析和驗證。結果表明，UKF算法應用于基于MPU-6050姿態測量系統的姿態估計精度較EKF算法有很大提高，驗證了UKF算法的優越性以及系統的可用性。