基于MSP430單片機的姿態測量系統設計

，，

(1.海軍工程大學 a.兵器工程系；b.導航工程系，武漢 430033；2.海軍海洋測繪研究所，天津 300061)

隨著MEMS技術的成熟，以及對姿態測量中低功耗、低成本的需求，微機械陀螺、加速度計和磁傳感器在無人機、智能機器人等領域得到廣泛的應用[1]。針對單一傳感器測量姿態存在零漂，受環境因素影響大等問題，多傳感器融合測姿已成為一種發展趨勢，并且隨著MEMS傳感器的迅速發展以及向各個學科領域的滲透，其各個方面性能如精度、魯棒性、動態響應等都得到了很大的提高，因此，如何在多MEMS傳感器系統中實現數據融合進而進行姿態測量成為主要研究方向。Marins基于四元數擴展的卡爾曼濾波，研究使用MARG傳感器進行姿態檢測[2]，然而該算法計算量較大，難以在硬件上實現。宋東等人[3]提出了一種基于MEMS加速度計、磁阻傳感器的方法，該方法簡化了計算量，但無法抑制動態環境下數據中的噪聲。Mahony等人[4]利用MEMS器件及互補濾波器建立姿態估計方法，得出互補濾波器截止頻率設計方法，但該算法PI參數只能通過試湊，無法實時調整PI參數。針對以上姿態測量方法的缺陷，基于GY-85九軸傳感器模塊，設計一種以單片機MSP430F5438為核心的姿態檢測系統，利用梯度下降法對傳感器進行數據融合，彌補單一傳感器測姿的不足。

1 硬件組成

設計的系統以MSP430F5438為核心，通過與集成了三軸加速度計ADXL345、三軸地磁傳感器HMC5883L以及陀螺儀ITG3205的九軸傳感器GY-85進行I2C通信以采集各個傳感器的輸出數據，并將解算出的各個姿態角通過無線串口發送給上位機。系統設計的原理框圖如圖1所示。

圖1 系統設計原理框圖

MSP430F5438是德州儀器近兩年推出的MSP43054XX系列單片機中的最高型號，其資源充足，功能強大，非常適用于多傳感器的姿態檢測。ADXL345是一款超低功耗的三軸加速度計，分辨率高(13位)，測量范圍達±16 g，能測量到小于1°的傾斜角度變化[5]。HMC5883L磁場傳感器分辨率達0.002 G，最大輸出頻率達160 Hz。陀螺儀ITG3205內部具有3個整合16位模數轉換器，提供陀螺儀同步取樣，并帶有可選擇的數字低通濾波器以及快速模式(400 kHz)的I2C接口。

2 軟件流程

系統的主流程圖如圖2所示。為了便于移植，對所設計程序進行模塊化處理，通過調用各個子函數完成所需的功能。程序首先完成MSP430F5438初始化，并對各個傳感器的工作模式進行初始化設定，接著對四元數初始化及各個傳感器的初始偏移進行校正。隨后在中斷函數程序中完成數據采集與濾波，并解算出姿態角發送給上位機。

圖2 系統流程

3 系統校正

加速度計校正的方法為將傳感器水平放置，使z軸在1 g的重力場，x、y軸在0 g的重力場，取一系列樣本的平均值，記為x0、y0、z1。這些值即為相對于各軸的偏移量，通過減去這些值以獲得實際加速度的值。

(1)

式中：Xacc、Yacc、Zacc為加速度計實際值，Xmea、Ymea、Zmea為加速度計測量值，SZ為加速度計z軸在1 g的重力場下的理想靈敏度。

陀螺儀校正時需采集初始時的一組數據求平均值后作為零位偏移，此后將測量值減去偏移即可。

ωacc=ωmea-ωbias

(2)

式中：ωacc為陀螺儀實際值；ωmea為陀螺儀測量值；ωbias為零位偏移值。

在理想狀態下，地磁傳感器三軸的輸出磁場矢量應構成一個圓心在原點的正球體，而在實驗的過程中，地磁傳感器受到周圍硬磁干擾和軟磁干擾，導致該球體變成了一個偏離圓心的橢球體，所以需對地磁傳感器進行校正。圖3和圖4為采用AntMag軟件對地磁傳感器校正前后的對比示意圖。校正后的地磁傳感器各軸的零位偏移和均方誤差有了明顯的改善，如表1所示。

圖3 校正前地磁傳感器輸出

圖4 校正后地磁傳感器輸出

校正X零位偏移Y零位偏移Z零位偏移均方誤差校正前0.08660.09380.11930.059校正后0.00080.00180.00330.029

4 姿態解算

4.1 坐標系建立、四元數與歐拉角之間的轉換

分別建立導航坐標系N和載體坐標系B，N系采用東北天坐標系，B系固連在載體上，傳感器的三軸分別安裝在B系的x、y、z軸上。設α、β、γ分別為俯仰角，滾轉角、航向角。則由N系到B系的旋轉矩陣可表示為

(3)

用四元數Q=q0+q1i+q2j+q3k(|Q|=1)表示該旋轉矩陣為

(4)

通過上述2種表示形式，將歐拉角用四元數表示為

(5)

4.2 四元數微分方程求解

根據四元數微分方程定義，對應的矩陣[6]為

采用一階龍格庫塔求解四元數微分方程。

Q(t+h)=Q(t)+hk1

(7)

得到四元數更新方程為

式中：ωx、ωy、ωz為△軸輸出角速度；h為采樣周期。

4.3 單傳感器解算姿態角

ADXL345加速度計在靜態或者低動態的情況下利用重力在各軸上的投影分量求解姿態角[7]。

(9)

式中：α、β、γ分別為俯仰角、翻滾角、z軸與重力加速度的夾角。

ITG3205陀螺儀測得繞各個軸的角速度，通過積分便可以得到繞各個軸旋轉的角度[8]:

θi=θi-1+ωdt

(10)

式中：θi為i時刻的角度值；θi-1為i-1時刻的角度值；ω為修正后的角速度；dt為積分時間。

HMC5883L地磁傳感器通過測得地磁場在其各軸上的投影分量解算出航向角。當地磁傳感器處于水平狀態時，可通過式(11)對航向角進行檢測，而當其處于傾斜狀態時，則需通過加速度計所測俯仰角α和翻滾角β代入式(12)，對其進行傾斜補償后解算出航向角。

γ=arctan(Hy/Hx)

(11)

(12)

式中：Hx、Hy、Hz為地磁傳感器各軸所測數據。

4.4 梯度下降法融合解算姿態

通過陀螺儀積分解算姿態，由于受到溫度和零漂的影響，長時間使用誤差較大[9]。而加速度計不僅對重力，同時對外力產生的加速度十分敏感，動態下噪聲明顯，且其不能對陀螺儀所測的航向角進行修正，故無法使用單傳感器對姿態進行測量。本文基于Madgwick所提出的梯度下降法，將九軸傳感器中的加速度計、陀螺儀、地磁傳感器數據進行融合修正旋轉矩陣，從而得到姿態的近似最優解。

(13)

構造目標函數為

F(Q)=(egx)2+(egy)2+(egz)2+

(emx)2+(emy)2+(emz)2

將四元數旋轉矩陣的優化轉化為求F(Q)的最小值。

由梯度下降法得到其迭代公式為[10]

(14)

式中：m為迭代步長。

將其與由四元數微分方程得到的四元數Qω(t)相結合，得到最終的梯度下降法的姿態融合公式：

(15)

式中：n為迭代步長；Δt為采樣時間；Q(t)為所求姿態四元數。

通過加速度計對地磁傳感器進行傾斜補償后求得的歐拉角，帶入式(16)轉化為初始四元數，以解決初始對準時梯度下降法收斂過慢的問題。當四元數初始化后，便可利用上述方法解算姿態的近似值。

(16)

5 實驗與分析

5.1 靜態實驗

利用角度分度裝置將九軸傳感器靜止放置水平，同時采集通過加速度計、陀螺儀、梯度下降法解算出的俯仰角，實驗結果如圖5所示。通過對比可以看出：加速度計存在明顯的噪聲，陀螺儀存在較明顯的偏移，隨著時間的增加，累積誤差逐漸增大。梯度下降法能有效的抑制加速度計的噪聲，俯仰角測量誤差大約在±0.3°以內。

圖5 俯仰角靜止實驗

5.2 動態實驗

將傳感器初始水平，此后繞x軸旋轉30°得到的圖像如圖6所示。由圖像看出，梯度下降法能及時跟蹤姿態的變化，同時一定程度上抑制了加速度計的噪聲和陀螺儀的漂移，所測的俯仰角誤差在±1°以內。

圖6 俯仰角動態實驗

5.3 四元數初始化對比試驗

初始時將傳感器x軸指向東方，然后旋轉90°使其指向北方，分別將前文所提出的對四元數初始化的方法與一般采用的將初始四元數設為(1，0，0，0)所測的航向角進行比較，實驗效果如圖7、8所示。由實驗結果看出，進行初始對準后，梯度下降法初始收斂時間由原來的5 s降到了1 s以內，改善了其姿態角初始時刻不穩的問題，采用該系統測的航向角誤差在±2°以內。

圖7 未初始對準航向角實驗結果

圖8 初始對準航向角實驗結果

6 結論

基于MSP430F5438與九軸傳感器GY- 85搭建姿態測量系統，實現了姿態角的檢測。針對地磁傳感器的所測數據受周圍磁場干擾比較嚴重的問題，使用AntMag軟件對其進行了校正，實驗結果表明該軟件簡單可行，正確有效。另外為解決加速度計測量噪聲、陀螺儀漂移誤差及加速度計無法對航向角進行修正等問題，引入梯度下降法，在角度分度裝置上進行了動靜態實驗。結果表明，該系統克服了航向角初始難以對準的問題，具有響應快、實時性好的特點，并可應用于平衡小車、無人機姿態檢測、慣性導航等領域。

[1] 夏圣，許勇.基于MEMS組合模塊的姿態檢測系統設計[J].單片機與嵌入式系統應用,2011(4):52- 55.

[2] MARINS J L,YUN X,BACHAMARM E,et al. An extended Kalman filter for quaternion based orientation rst- imation using MARG sensors[C]∥Proceedings of the 2001 IEEE/RSJ I- nternational Conference on Intel- ligent Robots and systems.Maui,Hawaii:IEEE,2001(8):2003- 2011.

[3] 宋東,章侃,王彥文,等.基于MEMS傳感器的飛行姿態指示系統[J].測控技術,2009,28(3):1- 4.

[4] MAHONY R,HAMEL T,PFILMLIN J M,et al.Nonlinear complementary filte- rs on the special orthogonal gro- up[J].IEEE trans.on autom atic control,2008,53(5):1203- 1217.

[5] 陳新雋，劉曉平，戴若犁，等.多MEMS傳感器的嵌入式姿態測量系統設計[A].單片機與嵌入式系統：應用天地,2011(10):58- 61.

[6] 秦永元.慣性導航[M].北京:科學出版社,2014.

[7] KIMBERLY T.Tilt sensing using linear Accelerometers[J].Freescale Semiconductor Application Note,2007(6):1- 7.

[8] 李偉，何鵬舉，高社生.多傳感器加權信息融合算法研究[J].西北工業大學學報，2010,28(5):674 - 678.

[9] 畢盛.閔華清.李淳，等.姿態傳感器采集測試系統的設計與實現[J].計算機測量與控制，2011,19(7):1562- 1564.

[10] MADGWICK S.An efficient orientation filter for intertial and inertial/magnetic sensor arrays [R].Report xio and University of Bristol(UK),2010.