基于MPU-6050及藍牙技術的姿態檢測系統設計

白禮卓 鮮鵬飛 尹雪 馬勇

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

關鍵詞：姿態檢測;卡爾曼濾波算法;四元數;ARM;藍牙技術

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

1? ? 理論基礎

1.1? 四元數法

四元數法在三維空間旋轉變換中能有效避開萬向節死鎖問題且計算量小。四元數用英文字母Q表示，它由單位為1的實部q和以i、j、k表示基本單位的3個相互正交虛部共同構成[3]。載體姿態表達式可用這4個參數線性組合表示。Q的表達式通常寫成：

載體的姿態更新，實際上就是要獲得下一時刻載體的四元數。四元數包含了載體姿態的全部信息，因此也可以通過四元數的微分方程來獲得新的四元數。

1.2? 卡爾曼濾波算法

卡爾曼濾波算法（Kalman Filter Algorithm，KFA）實際上是求下一時刻數據最優解算法。在姿態解算過程中陀螺儀的積分誤差不斷地積累，計算出的姿態角會產生明顯漂移，要修正姿態角最好的方法是融合加速度計和磁力計的觀測值[4]。

假設某線性系統的狀態方程為：

當前時刻t的系統真實值是Xt，Ft為狀態轉移矩陣，Ut-1是t-1時刻外部的控制量，Bt-1是控制矩陣，Wt-1為系統噪聲[5]。系統的狀態預測公式為：

卡爾曼濾波每個時刻系統的不確定性都用P來表示，為了表明觀測的不確定性在t和t-1時刻間的傳遞，引入預測估計協方差矩陣，其中Q為過程噪聲：

Pt-1是t-1時刻系統誤差協方差，P-t是預測的t時刻系統誤差協方差。

卡爾曼濾波不斷地重復“預測值→實測值→最優估計值”的數學運算思路構建起最優的估計。

2? ? 系統硬件設計

系統硬件總體設計方案如圖1所示。

采用MPU-6050六軸傳感器芯片，里面集成了加速度計和陀螺儀，將其與STM32單片機通過I2C接口連接。再將藍牙模塊HC05通過杜邦線與STM32單片機相連接，姿態數據通過藍牙模塊無線傳輸到手機APP。

3? ? 系統軟件設計

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

4? ? 測試結果

本論文設計的手機APP主要實現顯示MPU-6050原始數據，以及解算出來的姿態角AngelX，AngelY，AngelZ數據。手機APP顯示結果如圖3所示。

5? ? 結語

在Matlab靜態測試中，橫滾角AngleROLL變化范圍為-2.510 0°～-2.400 0°，俯仰角AnglePIT變化范圍為0.870 0°～ 0.910 0°，偏航角AngleYAW變化范圍為2.110 0°～2.140 0°。在檢測偏航角時，當載體在靜止狀態時，可增大磁力計數據的權重，當載體在運動狀態時，增大陀螺儀權重，互補了數據的誤差，獲得了更準確的姿態數據。此設計具有成本低、實時性的特點，在研究姿態數據回傳上具有良好的可視性，在提高小型無人機飛行穩定性、提高智能駕駛準確性、更精確的動作捕捉（可穿戴設備）等方面有一定的意義。

[參考文獻]

[1]胡茂曉.慣性動作捕捉前端設備與數據傳輸研究[D].濟南：山東大學，2015.

[2]蔣碩碩.傳感器技術的發展現狀與應用前景探討[J].電子技術與軟件工程，2013（9）：23.

[3]文亮.四元數矩陣[M].長沙：國防科技大學出版社，2002.

[4]喬會敏，張嘉易，郝永平，等.一種微機械陀螺儀誤差的高精度補償方法[J].國外電子測量技術，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]羅瑋.一種新興的藍牙技術—超低功耗藍牙技術[J].現代電信科技，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