陶子涵 高國偉 徐萬芝

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

一、引言

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

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

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

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

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

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

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

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

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

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

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

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

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

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

1、狀態(tài)方程

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

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

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

ωy—俯仰角速率；

ωz—偏航角速率。

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

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

ω—載體真實角速度；

ωd—陀螺儀真實漂移；

W1—陀螺儀輸出噪聲；

—載體速度估計值；

—陀螺漂移估計值。

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

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

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

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

2、量測方程

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

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

V—傳感器輸出噪聲。

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

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

3、UKF算法[6]

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

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

UKF算法步驟如下：

（1）選定濾波初值：

（2）計算sigma樣本點

（3）時間更新

（4）量測更新

4、實驗測試

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

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

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

四、結(jié)論

表1 EKF和UKF濾波誤差對比

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