白禮卓 鮮鵬飛 尹雪 馬勇

摘? ?要：文章對(duì)姿態(tài)傳感器國(guó)內(nèi)外現(xiàn)狀進(jìn)行了分析，對(duì)其技術(shù)發(fā)展和存在的問(wèn)題進(jìn)行了簡(jiǎn)單介紹。提出了一種基于MPU-6050六軸姿態(tài)傳感器（三軸陀螺儀、三軸加速度計(jì)）及藍(lán)牙技術(shù)的設(shè)計(jì)方案，采用ARM和MPU-6050姿態(tài)傳感器模塊設(shè)計(jì)了一套姿態(tài)檢測(cè)系統(tǒng)，手機(jī)可連接藍(lán)牙模塊并通過(guò)安卓手機(jī)APP實(shí)時(shí)顯示姿態(tài)角數(shù)據(jù)。此外，研究了四元數(shù)及卡爾曼濾波等方法，對(duì)六軸傳感器數(shù)據(jù)進(jìn)行了靜態(tài)修正，得到了更高精度的姿態(tài)角數(shù)據(jù)。

關(guān)鍵詞：姿態(tài)檢測(cè);卡爾曼濾波算法;四元數(shù);ARM;藍(lán)牙技術(shù)

最近十年來(lái)，人機(jī)交互領(lǐng)域得到了快速的發(fā)展，其中包括智能駕駛汽車(chē)、動(dòng)作捕捉（可穿戴設(shè)備）、無(wú)人機(jī)等。但有很多問(wèn)題需要進(jìn)一步解決，其中關(guān)鍵的問(wèn)題有兩個(gè)[1-2]：（1）姿態(tài)傳感器的數(shù)據(jù)采集與數(shù)據(jù)融合。（2）實(shí)時(shí)進(jìn)行數(shù)據(jù)的回傳與分析。其中，姿態(tài)解算以及姿態(tài)控制、姿態(tài)角回傳等是非常值得研究的關(guān)鍵技術(shù)，研究基于ARM的姿態(tài)解算算法及藍(lán)牙通信技術(shù)，不僅具有廣泛的理論意義，而且對(duì)實(shí)際的應(yīng)用也非常重要。

1? ? 理論基礎(chǔ)

1.1? 四元數(shù)法

四元數(shù)法在三維空間旋轉(zhuǎn)變換中能有效避開(kāi)萬(wàn)向節(jié)死鎖問(wèn)題且計(jì)算量小。四元數(shù)用英文字母Q表示，它由單位為1的實(shí)部q和以i、j、k表示基本單位的3個(gè)相互正交虛部共同構(gòu)成[3]。載體姿態(tài)表達(dá)式可用這4個(gè)參數(shù)線(xiàn)性組合表示。Q的表達(dá)式通常寫(xiě)成：

載體的姿態(tài)更新，實(shí)際上就是要獲得下一時(shí)刻載體的四元數(shù)。四元數(shù)包含了載體姿態(tài)的全部信息，因此也可以通過(guò)四元數(shù)的微分方程來(lái)獲得新的四元數(shù)。

1.2? 卡爾曼濾波算法

卡爾曼濾波算法（Kalman Filter Algorithm，KFA）實(shí)際上是求下一時(shí)刻數(shù)據(jù)最優(yōu)解算法。在姿態(tài)解算過(guò)程中陀螺儀的積分誤差不斷地積累，計(jì)算出的姿態(tài)角會(huì)產(chǎn)生明顯漂移，要修正姿態(tài)角最好的方法是融合加速度計(jì)和磁力計(jì)的觀測(cè)值[4]。

假設(shè)某線(xiàn)性系統(tǒng)的狀態(tài)方程為：

當(dāng)前時(shí)刻t的系統(tǒng)真實(shí)值是Xt，F(xiàn)t為狀態(tài)轉(zhuǎn)移矩陣，Ut-1是t-1時(shí)刻外部的控制量，Bt-1是控制矩陣，Wt-1為系統(tǒng)噪聲[5]。系統(tǒng)的狀態(tài)預(yù)測(cè)公式為：

卡爾曼濾波每個(gè)時(shí)刻系統(tǒng)的不確定性都用P來(lái)表示，為了表明觀測(cè)的不確定性在t和t-1時(shí)刻間的傳遞，引入預(yù)測(cè)估計(jì)協(xié)方差矩陣，其中Q為過(guò)程噪聲：

Pt-1是t-1時(shí)刻系統(tǒng)誤差協(xié)方差，P-t是預(yù)測(cè)的t時(shí)刻系統(tǒng)誤差協(xié)方差。

卡爾曼濾波不斷地重復(fù)“預(yù)測(cè)值→實(shí)測(cè)值→最優(yōu)估計(jì)值”的數(shù)學(xué)運(yùn)算思路構(gòu)建起最優(yōu)的估計(jì)。

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

系統(tǒng)硬件總體設(shè)計(jì)方案如圖1所示。

采用MPU-6050六軸傳感器芯片，里面集成了加速度計(jì)和陀螺儀，將其與STM32單片機(jī)通過(guò)I2C接口連接。再將藍(lán)牙模塊HC05通過(guò)杜邦線(xiàn)與STM32單片機(jī)相連接，姿態(tài)數(shù)據(jù)通過(guò)藍(lán)牙模塊無(wú)線(xiàn)傳輸?shù)绞謾C(jī)APP。

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

從圖2中可知，最先開(kāi)始配置時(shí)鐘，再初始化MPU-6050傳感器。再將MPU-6050中讀取到的陀螺儀、加速度計(jì)數(shù)據(jù)存儲(chǔ)到寄存器，準(zhǔn)備讓STM32內(nèi)核處理器進(jìn)行四元數(shù)歸一化處理和卡爾曼濾波。同時(shí)進(jìn)行藍(lán)牙模塊的初始化操作，成功則操作藍(lán)牙模塊進(jìn)入MARSTER主機(jī)模式，嘗試進(jìn)行藍(lán)牙匹配。最后手機(jī)端匹配藍(lán)牙模塊，成功則進(jìn)行姿態(tài)數(shù)據(jù)傳輸，失敗則返回INIT指令重啟藍(lán)牙模塊。STM32主要負(fù)責(zé)完成姿態(tài)傳感器數(shù)據(jù)采集、姿態(tài)解算和姿態(tài)輸出，藍(lán)牙模塊負(fù)責(zé)讀取STM32的姿態(tài)輸出數(shù)據(jù)并且發(fā)送至手機(jī)，上位機(jī)軟件負(fù)責(zé)PC端解析數(shù)據(jù)和3D航模實(shí)時(shí)顯示[6]。

4? ? 測(cè)試結(jié)果

本論文設(shè)計(jì)的手機(jī)APP主要實(shí)現(xiàn)顯示MPU-6050原始數(shù)據(jù)，以及解算出來(lái)的姿態(tài)角AngelX，AngelY，AngelZ數(shù)據(jù)。手機(jī)APP顯示結(jié)果如圖3所示。

5? ? 結(jié)語(yǔ)

在Matlab靜態(tài)測(cè)試中，橫滾角AngleROLL變化范圍為-2.510 0°～-2.400 0°，俯仰角AnglePIT變化范圍為0.870 0°～ 0.910 0°，偏航角AngleYAW變化范圍為2.110 0°～2.140 0°。在檢測(cè)偏航角時(shí)，當(dāng)載體在靜止?fàn)顟B(tài)時(shí)，可增大磁力計(jì)數(shù)據(jù)的權(quán)重，當(dāng)載體在運(yùn)動(dòng)狀態(tài)時(shí)，增大陀螺儀權(quán)重，互補(bǔ)了數(shù)據(jù)的誤差，獲得了更準(zhǔn)確的姿態(tài)數(shù)據(jù)。此設(shè)計(jì)具有成本低、實(shí)時(shí)性的特點(diǎn)，在研究姿態(tài)數(shù)據(jù)回傳上具有良好的可視性，在提高小型無(wú)人機(jī)飛行穩(wěn)定性、提高智能駕駛準(zhǔn)確性、更精確的動(dòng)作捕捉（可穿戴設(shè)備）等方面有一定的意義。

[參考文獻(xiàn)]

[1]胡茂曉.慣性動(dòng)作捕捉前端設(shè)備與數(shù)據(jù)傳輸研究[D].濟(jì)南：山東大學(xué)，2015.

[2]蔣碩碩.傳感器技術(shù)的發(fā)展現(xiàn)狀與應(yīng)用前景探討[J].電子技術(shù)與軟件工程，2013（9）：23.

[3]文亮.四元數(shù)矩陣[M].長(zhǎng)沙：國(guó)防科技大學(xué)出版社，2002.

[4]喬會(huì)敏，張嘉易，郝永平，等.一種微機(jī)械陀螺儀誤差的高精度補(bǔ)償方法[J].國(guó)外電子測(cè)量技術(shù)，2012（8）：18-20，30.

[5]DE RUITER A H J.Extended kalman filtering and nonlinear predictive filtering for spacecraft attitude determination[J].Aeronautical Journa，2015（7）：22-32.

[6]羅瑋.一種新興的藍(lán)牙技術(shù)—超低功耗藍(lán)牙技術(shù)[J].現(xiàn)代電信科技，2010（10）：31-34，38.

Abstract：In this paper， the status quo of attitude sensor at home and abroad is analyzed， and its technical development and existing problems are briefly introduced. A design scheme based on MPU-6050 six-axis attitude sensor（three-axis gyroscope， three-axis accelerometer）and Bluetooth technology is proposed. A set of attitude detection system is designed by ARM and MPU-6050 attitude sensor module. The mobile phone can be connected to the Bluetooth module. And the attitude angle data is displayed in real time through the Android mobile phone APP. In addition， methods such as quaternion and Kalman filtering are studied， and the six-axis sensor data is statically corrected to obtain higher-precision attitude angle data.

Key words：attitude detection; Kalman filtering algorithm; quaternion; ARM; Bluetooth technology