基于STM32的六自由度平臺的姿態(tài)測量系統(tǒng)設計

胡 雄, 汪 超, 唐 剛

(上海海事大學 物流工程學院, 上海 201306 )

基于STM32的六自由度平臺的姿態(tài)測量系統(tǒng)設計

胡 雄, 汪 超, 唐 剛

(上海海事大學 物流工程學院, 上海 201306 )

提出一種新型的六自由度平臺的姿態(tài)測量系統(tǒng)設計方案, 該方案是以STM32為主控制器實現對平臺的數據采集和姿態(tài)計算. 由傳感器得到平臺的加速度、角速度和磁阻數據, 采用四元數法作為六自由度平臺的姿態(tài)更新算法, 經互補濾波后得到六自由度平臺的姿態(tài)角, 通過無線傳輸到上位機顯示, 并將數據存于外接存儲器中. 上位機顯示結果和試驗數據表明該六自由度平臺姿態(tài)測量系統(tǒng)是穩(wěn)定可靠的.

六自由度平臺; STM32; 姿態(tài)測量; 無線傳輸

六自由度平臺是由運動平臺通過鉸鏈連接6條伸縮桿的一端, 而伸縮桿的另一端通過鉸鏈連接下方固定平臺而構成的空間并聯(lián)機構[1]. 六自由度平臺在各種訓練模擬器如艦艇模擬器、飛行模擬器中有著非常廣泛的應用, 并在飛行器的空間對接模擬試驗中發(fā)揮了重要的作用[2]. 對于試驗設備的研究與其性能的檢測, 六自由度平臺提供了較為真實的試驗環(huán)境.

隨著船舶科技的不斷進步, 船舶的安全性越來越受到人們的重視. 船舶在行駛過程中因為各種因素的影響會發(fā)生晃動, 如果船舶的姿態(tài)不能保證在一定的范圍內, 就會對船舶的安全造成威脅. 船舶上電子設備的正常運行、船上人員的正常生活和船舶上手術的操作等問題都需要船舶保持其平穩(wěn)性. 因此, 船舶在行駛中, 其姿態(tài)參數和穩(wěn)定性有著至關重要的作用. 然而, 船舶的研制是一項浩大而復雜的工程, 直接用實體船進行試驗, 盡管能獲得最真實、最具說服力的姿態(tài)響應, 但試驗成本過高, 并有著一定的危險性. 若利用計算機對船舶性能模擬仿真, 所建立的數學模型采用人為的方式推導而來, 得到的姿態(tài)響應數據很難有說服力. 六自由度平臺的模擬仿真, 在實驗室中將船舶在海洋中搖擺的姿態(tài)和運動模擬出來, 既能很好地接近實際情況, 又能避免大量的人力、物力資源的浪費. 這種半物理仿真的方法也是目前研究船舶姿態(tài)最為有效的方法.

六自由度平臺的姿態(tài)測量是平臺運行的關鍵技術之一, 實時得到六自由度平臺的姿態(tài)數據并反饋給操作臺, 從而更好地控制六自由度平臺的模擬運轉操作. 本文采用STM32F103RBT6為主控芯片, 由MPU6050六軸傳感器和HMC5883L三軸數字羅盤完成六自由度平臺的數據采集, 并經過數據處理和整合后得到其姿態(tài)角數據, 并將角速度、加速度等數據存儲于外接存儲器K9K8G08U0M中, 通過無線傳輸到上位機, 在PC端實現六自由度平臺的姿態(tài)信息顯示.

1 六自由度平臺的工作原理

六自由度平臺工作時, 它通過6條伸縮桿的伸縮運動來改變伸縮桿的桿長, 從而使得上平臺呈現不同的空間姿態(tài), 實現上平臺在空間中六自由度的運動, 分別為升降、橫移、縱擺、俯仰、滾轉、偏航, 即為x、y、z軸方向的平移和繞x、y、z軸的旋轉運動, 或者是這些運動的復合運動[1-2]. 六自由度平臺的三維模型如圖1所示.

圖1 六自由度平臺的三維模型Fig.1 Three-dimensional model of six degrees of freedom platform

2 六自由度平臺的姿態(tài)測量系統(tǒng)設計

2.1 姿態(tài)測量系統(tǒng)框架設計

姿態(tài)測量的方法是六自由度平臺姿態(tài)測量系統(tǒng)的核心, 常見的測量方法有加速度計和陀螺儀組合進行測量的慣性組合測量法、電子羅盤測姿方法、GPS姿態(tài)算法、圖像視覺處理法等[3]. GPS姿態(tài)算法主要應用于視野開闊、障礙物較少的室外場所, 如飛機和船舶的姿態(tài)測量, 不適合于室內測量[4]. 圖像視覺處理法在醫(yī)療和工業(yè)上的應用較為廣泛, 但是其測量系統(tǒng)體積較大, 成本昂貴, 姿態(tài)解算方法較為復雜. 加速度計和磁場力計的測量方法有著體積小、測量速度快、成本低等優(yōu)點, 但在動態(tài)測量時數據準確度不夠. 陀螺儀單獨測量姿態(tài)時, 在測量角度時存在漂移問題[5].

結合上述各測量方法的優(yōu)缺點, 本文采用三軸加速度計、三軸陀螺儀、三軸磁阻傳感器作為傳感器單元, 其中, 三軸加速度計和三軸陀螺儀采用集成的MPU6050六軸傳感器, 三軸磁阻傳感器采用HMC5883L數字羅盤. 姿態(tài)測量裝置實物如圖2所示. 由四元數姿態(tài)算法得到六自由度平臺的姿態(tài)角數據, 為了解決姿態(tài)測量系統(tǒng)中加速度計和磁阻傳感器在使用中受線性加速度干擾和磁場干擾問題, 以及陀螺儀在四元數法中的累積誤差問題, 系統(tǒng)設計相應的互補濾波算法對傳感器信息進行融合, 從而解出六自由度平臺的姿態(tài), 最后將姿態(tài)數據和傳感器數據通過無線傳輸, 在PC端上位機上進行顯示, 同時將傳感器信息和姿態(tài)信息存儲于存儲單元里. 姿態(tài)測量系統(tǒng)的設計方案如圖3所示.

2.2 姿態(tài)更新算法

姿態(tài)更新算法是姿態(tài)測量系統(tǒng)的核心, 只有確定姿態(tài)更新算法, 才能設計相應的濾波算法來估計六自由度平臺的全姿態(tài)選擇的姿態(tài)更新算法是否合適, 將會直接影響六自由度平臺姿態(tài)的測量結果. 歐拉角法、方向余弦法和四元數法是目前較為典型的姿態(tài)更新算法[6].

歐拉角法求解姿態(tài)角時, 不僅求解速度較為緩慢, 姿態(tài)求解方程中會出現“奇點”, 而且其最大的缺點在于并不能進行全姿態(tài)解算. 方向余弦法則是在求解過程中計算量過大, 不能快速地得到姿態(tài)結果, 因此很難達到實際工作中的目標. 四元數法不僅求解方程無奇性, 而且線性程度高, 方程解算的時間大大減少, 因此，本文姿態(tài)測量系統(tǒng)中選用四元數法作為姿態(tài)更新算法[7].

圖2 姿態(tài)測量裝置Fig.2 Attitude measurement device

圖3 姿態(tài)測量系統(tǒng)設計方案Fig.3 Design program of attitude measurement system

四元數法求解姿態(tài)時所用的定位參數只有4個, 并且4個定位參數之間只有一個約束方程, 可以表示為一個1個標量部分和3個矢量部分組成的形式, 同樣可以看作是由1個標量和1個矢量之和的形式. 實際工程中多采用特征四元數, 其標量部分可以看做一個坐標系相對于另一坐標系轉角的一半的余弦值, 矢量部分可以看做某一時刻轉動軸n的方向. 如式(1)中的R為一矢量, 相對于某一坐標系旋轉了θ后得到新矢量R′, 在這里采用Rodrigue-Hamilton參數作為四元數實元,λ為其標量部分,p1、p2、p3為其矢量部分[7].

R′=qRq-1

(1)

從上述方程中可以看出, 四元數法求解載體的姿態(tài)角時, 只需要求解4個一階微分方程即可, 相比于方向余弦法, 其大大地減少了計算量, 能夠滿足工程中的實時性要求, 被廣泛應用于捷聯(lián)式慣性導航技術和空間技術中[8].

四元數法求解六自由度平臺的姿態(tài)過程如下:

(1) 輸入四元數方程在求解過程中初始姿態(tài)角, 如式(2)所示.

(2)

(3)

(4)

故姿態(tài)矩陣可以表示為

(5)

(4) 解算六自由度平臺的姿態(tài)角, 得到θ、ψ、γ. 輸入分別為T11(n)、T12(n)、T13(n)、T23(n)、T33(n), 利用方向余弦矩陣中對應項相等即可得到姿態(tài)角, 計算式如式(6)所示.

(6)

2.3 互補濾波算法

在實際使用中, 加速度傳感器和磁阻傳感器解算得到的姿態(tài)數據不會產生累積誤差, 但是在運動加速度干擾和磁場干擾等情況下, 在短時間內姿態(tài)數據的精度較差. 陀螺儀的動態(tài)響應較好, 但是在計算姿態(tài)過程中會出現數據累積誤差, 導致得到的結果沒用使用價值. 因此，六自由度平臺姿態(tài)測量系統(tǒng)中采用相應的互補濾波算法對傳感器信息進行融合[9].

(7)

其中:A(s)+B(s)=1,A(s)=K/(S+K),B(s)=S/(S+K),S、K分別為磁阻傳感器和加速度傳感

器解算得到的姿態(tài)角度和陀螺儀解算得到姿態(tài)角度的權重值.A(s)具有低通濾波性,B(s)具有高通濾波特性, 從而分別將θjc中的高頻噪聲和ωm中的低頻噪聲濾除, 代入式(7)中可得:

(8)

式(8)的時域形式為

(9)

互補濾波算法中僅需要調節(jié)相應的K值即可, 系統(tǒng)的截止頻率fc與K的關系滿足:

(10)

其中:ts為系統(tǒng)的采樣周期.

2.4 姿態(tài)測量系統(tǒng)的硬件設計

本文六自由度平臺姿態(tài)測量系統(tǒng)的主控芯片選用STM32F103RBT6, 它是一款成本低、功耗低、性能高的cortex-M3架構內核的32位處理器產品, 滿足該系統(tǒng)的性能需求. 加速度和角速度測量采用MPU6050六軸傳感器, 相對于傳統(tǒng)的多組件測量方法, 該傳感器消除了加速度計與陀螺儀之間的軸差問題, 其不僅體積小, 且有自帶的數字運動處理器, 較為方便地實現姿態(tài)解算, MPU6050傳感器與主控芯片的接口方式如圖4所示[10].

三軸數字羅盤選用HMC5883L, 用來測量六自由度平臺的外界干擾磁場強度和地球磁場強度, 其采用了AMR(anisotropic magneto-resistance)技術, 有著軸向的高靈敏性和線性高精度性, 體積小且成本低, 滿足本文姿態(tài)測量系統(tǒng)的性能需求, 數字羅盤與主控芯片的接口圖如圖5所示[11]. 數據存儲模塊采用三星公司的K9K8G08U0M, 該存儲器有著1 GByte的存儲空間, 它具有尋址簡單、可靠性強、功耗低等特點, 由于采用NAND(Not AND)結構, 同時具有較強的抗震能力, 而且能保證10 a的數據存儲時間.

圖5 數字羅盤與主控芯片接口圖Fig.5 Interface figure of digital compass and main control chip

無線傳輸模塊選用nRF24L01+芯片, 該芯片工作頻率為2.4～2.5 GHz, 具備自動重發(fā)功能和6個數據傳輸通道, 最大無線傳輸速率為2 Mbit/s, 它在通信率較高的設備互聯(lián)間有著很好的效果, 具有功耗低、成本低、體積小等優(yōu)點[12]. 六自由度平臺的姿態(tài)測量系統(tǒng)的主控芯片STM32作為無線傳輸的發(fā)送端, 接收端也同樣采用STM32芯片. 接收端通過無線傳輸得到傳感器數據和姿態(tài)角數據, 通過USB將數據傳輸給PC端的上位機, 并在上位機上顯示. 無線傳輸的示意如圖6所示.

圖6 無線傳輸示意圖

Fig.6 Schematic diagram of wireless transmission

2.5 姿態(tài)測量系統(tǒng)軟件設計

六自由度平臺姿態(tài)測量系統(tǒng)的軟件設計是整個系統(tǒng)中的重要組成部分, 它主要是下位機軟件和上位機軟件兩個部分. 其中: 下位機主要包括系統(tǒng)的微處理器的初始化、傳感器數據的采集和融合、姿態(tài)解算, 并將傳感器信息和姿態(tài)解算結果發(fā)送給上位機; 上位機則是對下位機傳輸信息的解析, 并在顯示界面上顯示六自由度平臺的姿態(tài)信息. 六自由度平臺姿態(tài)測量系統(tǒng)的軟件結構框圖如圖7所示.

圖7 系統(tǒng)軟件結構框圖Fig.7 Block diagram of the system software

2.6 上位機數據顯示

PC端上位機通過USB傳輸得到傳感器數據和姿態(tài)角信息, 并編寫程序實現姿態(tài)測量模塊運動姿態(tài)的模擬和顯示. 圖8和9分別為上位機顯示的姿態(tài)變化時的三維模型和姿態(tài)歐拉角以及傳感器原始數據的波形圖, 可以很直接明了地看出平臺的姿態(tài), 在上位機中可以用相應的控件分析其角速度、加速度、磁通量波形圖, 可直接放大波形圖和改變其時間柵格. 上位機接收數據的格式為“＄×1×2×3×4×5×6×7×8×9×10×11×12＄”, 總共傳輸12個數據, 分別為三軸角速度、三軸加速度、三軸磁通量、三個姿態(tài)角.

圖8 三維模型和姿態(tài)角顯示Fig.8 Three-dimensional model and attitude angle display

圖9 傳感器原始數據波形圖Fig.9 Original data waveform figure of sensor

2.7 系統(tǒng)測試

為了測試六自由度平臺姿態(tài)測量系統(tǒng)的性能, 進行了系統(tǒng)姿態(tài)測量精度試驗. 將本文設計的姿態(tài)測量裝置與標準的姿態(tài)測量裝置進行姿態(tài)測量對比, 保持試驗臺上兩者的重心在同一位置, 以標準的姿態(tài)測量裝置為準, 記錄所設計的姿態(tài)測量系統(tǒng)的數據, 取其3次平均值來代表姿態(tài)測量系統(tǒng)的精度.

試驗中保證兩個姿態(tài)測量裝置的三軸平行, 將本文設計的姿態(tài)測量系統(tǒng)的實際數據與標準姿態(tài)測量裝置得到的數值進行對比, 評價姿態(tài)角的測量精度, 俯仰角、滾轉角、偏航角的測量結果如表1～3所示. 由表1～3可以看到, 俯仰角的絕對誤差在0.10°以內, 滾轉角的絕對誤差在0.15°以內, 偏航角的絕對誤差在0.18°以內. 由此可知, 本文所設計的系統(tǒng)能夠很好地應用于六自由度平臺的姿態(tài)測量.

表1 俯仰角測量結果

表2 滾轉角測量結果

表3 偏航角測量結果

3 結 語

本文所設計的一種基于STM32的六自由度平臺的測量系統(tǒng), 采用了MPU6050六軸傳感器與三軸數字羅盤的姿態(tài)測量模塊, 四元數更新姿態(tài), 互補濾波法提高姿態(tài)測量系統(tǒng)的精度. 該姿態(tài)測量裝置體積小、易攜帶、成本低, 并且有著較高的精度. 試驗測量數據基本穩(wěn)定, 表明該姿態(tài)測量系統(tǒng)有著較好的性能和穩(wěn)定性, 可以用于六自由度平臺進行船舶模擬試驗時的姿態(tài)測量, 為船舶的安全性提供了一定的保障.

[1] STEWART D A. Platform with six degrees of freedom[J]. Proceedings of the Institution of Mechanical, 1965, 180: 371-386.

[2] 楊世祥, 楊濤, 徐悅桐. 大型數字式六自由度運動平臺的開發(fā)[J]. 液壓與氣動, 2003(8): 46-47.

[3] 朱榮, 周兆英. 基于MEMS的姿態(tài)測量系統(tǒng)[J]. 測控技術, 2002, 21(10): 6-9.

[4] 趙鑫爐. 基于MARG傳感器的車輛姿態(tài)測量系統(tǒng)設計[D]. 太原: 中北大學儀器與電子學院, 2014.

[5] 朱文杰, 王廣龍, 高鳳岐, 等. 基于MIMU和磁強計的在線實時定姿方法[J]. 傳感技術學報, 2013，26(4): 536-540.

[6] 劉建業(yè), 曾慶化, 趙偉, 等. 導航系統(tǒng)理論與應用[M]. 西安: 西北工業(yè)大學出版社, 2009: 297-310.

[7] 張榮輝, 賈宏光, 陳濤, 等. 基于四元數法的捷聯(lián)式慣性導航系統(tǒng)的姿態(tài)解算[J]. 光學精密工程, 2008, 16(10): 1964-1968.

[8] 胡淑娥. 關于四元數矩陣若干問題的結果與應用[D]. 長沙: 國防科學技術大學理學院, 2005.

[9] 郭曉鴻, 楊忠, 陳酷, 等. EKF和互補濾波器在飛行姿態(tài)確定中的應用[J]. 傳感器與微系統(tǒng), 2011, 30(11): 149-152.

[10] 傅忠云, 朱海霞, 孫金秋, 等. 基于慣性傳感器MPU6050的濾波算法研究[J]. 壓電與聲光, 2015, 37(5): 822-825.

[11] 朱文杰, 王廣龍, 高鳳岐, 等. 基于MIMU和磁強計的在線實時定姿方法[J]. 傳感技術學報, 2013, 26(4): 536-540.

[12] 謝輝程, 郭莉. 基于STM32的無線環(huán)境監(jiān)控系統(tǒng)設計[J]. 軟件, 2014, 35(12): 5-7.

A Six Degrees of Freedom Platform Attitude Measurement System Design Based on STM32

HUXiong,WANGChao,TANGGang

(School of Logistics Engineering, Shanghai Maritime University, Shanghai 201306, China)

A new type of attitude measuring system design scheme of six degrees of freedom platform is proposed, realizing data collection and attitude calculation for the platform using STM32 as the main controller. The acceleration, angular velocity and magnetic resistance data are obtained by the sensor, the quaternion method is adopted as the attitude updating algorithm of the six degrees of freedom platform, and the attitude angle of the six degrees of freedom platform can be got after the complementary filter, through the wireless transmission , the attitude angle is displayed on PC, also, the data is stored in external memory. The results and experimental data of the PC display show that the proposed six degrees of freedom platform attitude measuring system is stable and reliable.

six degrees of freedom platform; STM32; attitude measurement; wireless transmission

1671-0444 (2016)04-0597-07

2015-12-30

胡 雄(1962—)，男，上海人，教授，博士，研究方向為機械工程.E-mail:huxiong@shmtu.edu.cn

V 19

A