基于MPU-6050和UKF的姿態(tài)測(cè)量系統(tǒng)設(shè)計(jì)

陶子涵 高國偉 徐萬芝

北京信息科技大學(xué) 信息獲取與檢測(cè)實(shí)驗(yàn)室 北京 100101

一、引言

姿態(tài)信息是導(dǎo)航系統(tǒng)中最為關(guān)鍵的參數(shù)之一[1]，姿態(tài)測(cè)量系統(tǒng)在航空航天[2]、車輛船舶、機(jī)器人、人體姿態(tài)捕捉等諸多領(lǐng)域中被廣泛使用。而在姿態(tài)測(cè)量系統(tǒng)中，人們最關(guān)心的就是測(cè)量精度。

對(duì)姿態(tài)測(cè)量系統(tǒng)精度造成影響的因素主要來源于兩個(gè)方面：

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

其次，姿態(tài)估計(jì)所使用的算法也對(duì)姿態(tài)測(cè)量的精度有很大影響，只有選取的算法適用于系統(tǒng)的實(shí)際情況才能取得良好的效果，例如Hajiyev C及趙琳等人將EKF算法應(yīng)用于姿態(tài)估計(jì)[3-4]取得了良好的估計(jì)效果。

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

二、系統(tǒng)總體設(shè)計(jì)

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

系統(tǒng)的整體結(jié)構(gòu)框圖如圖1所示，主要由MPU-6050姿態(tài)檢測(cè)電路及其外圍電路、微控制器電路、串口通信接口電路、電源電路、等組成。通過姿態(tài)檢測(cè)電路獲取載體的姿態(tài)信息，經(jīng)過A/D轉(zhuǎn)換電路將數(shù)據(jù)傳輸至微處理器對(duì)姿態(tài)進(jìn)行解算和更新，最后采用串口輸出姿態(tài)解算結(jié)果在PC機(jī)中顯示。

MPU-6050慣性測(cè)量模塊是美國InvenSense公司于2009年推出的一款低成本的MEMS傳感器，是全球第一款整合型六軸運(yùn)動(dòng)傳感器，其數(shù)據(jù)采集能力具有高速、精確的特點(diǎn)，在本系統(tǒng)中用于獲取姿態(tài)信息原始數(shù)據(jù)。

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

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

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

本系統(tǒng)的軟件設(shè)計(jì)整體流程圖如圖2所示。首先對(duì)系統(tǒng)進(jìn)行初始化。初始對(duì)準(zhǔn)即將姿態(tài)測(cè)量系統(tǒng)對(duì)準(zhǔn)到運(yùn)動(dòng)體的初始姿態(tài)，初始對(duì)準(zhǔn)之后，采集運(yùn)動(dòng)體的姿態(tài)信息傳輸至STM32微處理器，作為姿態(tài)估計(jì)算法的輸入量，然后進(jìn)行姿態(tài)解算和更新，并將四元數(shù)表示的姿態(tài)信息轉(zhuǎn)化為俯仰角、橫滾角和偏航角經(jīng)由串口輸出，在上位機(jī)中顯示。

三、姿態(tài)估計(jì)算法

1、狀態(tài)方程

不同的姿態(tài)參數(shù)表示的動(dòng)力學(xué)模型表達(dá)形式不同，基于四元數(shù)推導(dǎo)的姿態(tài)動(dòng)力學(xué)模型[7]為：

其中，Ωb—載體坐標(biāo)系相對(duì)于地理坐標(biāo)系上的角速度分量，可由MPU-6050的陀螺儀部分測(cè)得，其矩陣表達(dá)式為：

其中，ωx—滾轉(zhuǎn)角速率；

ωy—俯仰角速率；

ωz—偏航角速率。

即四元數(shù)動(dòng)力學(xué)微分方程為：

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

ω—載體真實(shí)角速度；

ωd—陀螺儀真實(shí)漂移；

W1—陀螺儀輸出噪聲；

—載體速度估計(jì)值；

—陀螺漂移估計(jì)值。

設(shè)Δωd為陀螺漂移估計(jì)誤差，則：

其中，W1，W2設(shè)為零均值白噪聲。

設(shè)過程噪聲Wk為可加性噪聲，其統(tǒng)計(jì)特性為：Wk～N(0,Qk)，由（2）（5）組成的狀態(tài)方程為：

將陀螺漂移與姿態(tài)四元數(shù)一起選為系統(tǒng)狀態(tài)向量，可以對(duì)陀螺漂移進(jìn)行實(shí)時(shí)補(bǔ)償，解決了MEMS陀螺漂移較大引起的姿態(tài)估計(jì)精度下降的問題。

2、量測(cè)方程

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

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

V—傳感器輸出噪聲。

設(shè)量測(cè)噪聲也為可加性噪聲，其統(tǒng)計(jì)特性為Vk～N(0,Rk)，即量測(cè)方程為：

綜上，狀態(tài)方程和量測(cè)方程滿足：

3、UKF算法[6]

微小型無人機(jī)姿態(tài)測(cè)量系統(tǒng)是一種典型的非線性系統(tǒng)，若采用EKF算法進(jìn)行濾波，必須對(duì)原系統(tǒng)和量測(cè)作泰勒級(jí)數(shù)展開并且只保留線性項(xiàng)，在系統(tǒng)非線性度較高的情況下，EKF的濾波精度會(huì)大大降低，因此只適用于弱非線性對(duì)象的估計(jì)問題。

UKF是基于一種UT變換的濾波算法，它首先根據(jù)初始狀態(tài)均值和方差矩陣產(chǎn)生k時(shí)刻的sigma點(diǎn)，計(jì)算sigma點(diǎn)時(shí)所給的方差矩陣必須是正定矩陣。將sigma點(diǎn)帶入到狀態(tài)方程中計(jì)算出變換的樣本點(diǎn)并確定相應(yīng)的權(quán)值，完成狀態(tài)預(yù)測(cè)。然后計(jì)算k時(shí)刻的增廣樣本點(diǎn)，帶入到觀測(cè)方程計(jì)算出預(yù)測(cè)觀測(cè)模型的樣本點(diǎn)，完成觀測(cè)模型預(yù)測(cè)。最后根據(jù)上面計(jì)算出的狀態(tài)預(yù)測(cè)方程和觀測(cè)預(yù)測(cè)方程，計(jì)算出變換后的均值和方差矩陣及增益矩陣，計(jì)算出濾波值，完成狀態(tài)估計(jì)和更新從而達(dá)到濾波的目的。

UKF算法步驟如下：

（1）選定濾波初值：

（2）計(jì)算sigma樣本點(diǎn)

（3）時(shí)間更新

（4）量測(cè)更新

4、實(shí)驗(yàn)測(cè)試

選用SGT320E型多功能三軸轉(zhuǎn)臺(tái)對(duì)本文所設(shè)計(jì)的姿態(tài)測(cè)量系統(tǒng)進(jìn)行測(cè)試，將測(cè)量系統(tǒng)固定在轉(zhuǎn)臺(tái)上，控制轉(zhuǎn)臺(tái)位置使得測(cè)量系統(tǒng)初始角均為零度，采樣頻率設(shè)置為500Hz，轉(zhuǎn)臺(tái)轉(zhuǎn)速設(shè)置為2°/s，采樣點(diǎn)為600個(gè)，測(cè)量出系統(tǒng)輸出的角度誤差以驗(yàn)證系統(tǒng)對(duì)運(yùn)動(dòng)體各姿態(tài)角度的檢測(cè)效果。

通過實(shí)驗(yàn)對(duì)比了采用EKF和采用UKF兩種姿態(tài)估計(jì)算法下系統(tǒng)的俯仰角、滾轉(zhuǎn)角和偏航角的輸出角度誤差，如圖3、4、5所示，EKF和UKF濾波誤差對(duì)比如表1所示。

由表1統(tǒng)計(jì)結(jié)果表明，UKF用于姿態(tài)估計(jì)的誤差均值、方差及最大誤差都明顯低于EKF，說明UKF具有更高的濾波精度。

四、結(jié)論

表1 EKF和UKF濾波誤差對(duì)比

本文設(shè)計(jì)了一種基于MPU-6050六軸姿態(tài)傳感器的微型姿態(tài)測(cè)量系統(tǒng)，針對(duì)現(xiàn)有低成本MEMS傳感器所存在的測(cè)量精度較低、漂移大等問題，提出了基于UKF算法的姿態(tài)估計(jì)方法應(yīng)用于所設(shè)計(jì)的姿態(tài)測(cè)量系統(tǒng)。文章首先簡(jiǎn)要的介紹了姿態(tài)測(cè)量系統(tǒng)的整體硬件結(jié)構(gòu)設(shè)計(jì)和軟件設(shè)計(jì)流程，然后對(duì)系統(tǒng)的狀態(tài)方程和量測(cè)方程進(jìn)行了分析并給出了UKF算法，最后通過實(shí)驗(yàn)及實(shí)驗(yàn)數(shù)據(jù)對(duì)所設(shè)計(jì)的系統(tǒng)進(jìn)行了分析和驗(yàn)證。結(jié)果表明，UKF算法應(yīng)用于基于MPU-6050姿態(tài)測(cè)量系統(tǒng)的姿態(tài)估計(jì)精度較EKF算法有很大提高，驗(yàn)證了UKF算法的優(yōu)越性以及系統(tǒng)的可用性。