基于DSP和STM32的航姿參考系統設計

北京信息科技大學 信息獲取與檢測實驗室，北京 100101

一、引言

姿態測量能夠為導航和控制提供精確的參考信息，近年來隨著MEMS慣性傳感器的快速發展，民用領域對姿態測量的使用也越來越廣泛，如機器人、無人機、船舶導航、無人駕駛、三維游戲以及跳傘訓練的監測等領域。而在姿態的測量中，航姿參考系統（Attitude and Heading Reference System, AHRS）便顯得尤為重要[1]。

AHRS包括多個軸向傳感器，能夠為飛行器提供航向、俯仰和橫滾信息，這類系統用來為飛行器提供準確可靠的姿態與航行信息。航姿參考系統包括MEMS三軸陀螺儀、加速度計和磁強計。而AHRS與慣性測量單元（Inertial Measurement Unit, IMU）的區別在于，AHRS包含了嵌入式的姿態數據解算單元與航向信息，IMU僅僅提供傳感器數據，并不具有提供準確可靠的姿態數據的功能。故航姿參考系統具有重要的研究價值和廣泛的應用前景。

低成本的AHRS是綜合利用了微處理器、MEMS傳感器和各種姿態解算算法，在相對較小的成本、功耗和體積內計算并提供三軸姿態以及校正的三軸加速度、角速率和地磁場等信息。與基于機械、激光、光纖等陀螺的AHRS相比，低成本AHRS的MEMS傳感器有更大的正交誤差、測量誤差或噪聲，因此其姿態解算精度更加依賴于多傳感器融合算法。

本文依托實驗室在研項目——“基于先驗約束信息的航姿參考系統濾波算法研究”，在基于DSP和STM32單片機的基礎上，采用多傳感器數據融合進行了卡爾曼濾波作為航姿解算單元，設計并完成了航姿參考系統的核心部分。實現其小型化、低功耗和低成本的設計要求，并可應用于對精度要求不是很高的民用領域，如工業和商業無人機、可穿戴設備對姿態的實時監測等。

二、硬件結構

基于MEMS技術的傳感器和姿態解算技術極大促進了低成本小型化AHRS技術的發展。硬件結構中，在基于STM32為處理器的同時，以DSP處理器為核心，采用低成本、體積小、低功耗和重量輕的MPU6050慣性傳感器和HMC588L磁強計構成測量單元，構建航姿參考系統的硬件平臺。

MEMS器件的AHRS硬件基本組成如下：三軸角速度陀螺儀和三軸加速度計MPU6050、三軸磁阻傳感器HMC5883L、DSP處理器TMS320C6713和STM32系列處理器STM32F103T8。航姿參考系統的硬件結構框圖如圖1所示。

1、慣性測量單元MPU6050

慣性測量單元MPU6050具有低成本、功耗低、體積小、重量輕等優點，由于MPU6050單元能夠以數字輸出6軸或9軸的旋轉矩陣、四元數、歐拉角格式的融合演算數據，對姿態的解算更為方便，大大簡化航姿系統的硬件設計，同時減小安裝、調試的難度和工作量。MPU6050一個片上1024 byte的FIFO有助于降低系統功耗，角速度全格感測范圍為±250o/s、±500o/s、±1000o/s與±2000o/s，可準確追蹤快速與慢速動作，并且用戶可程式控制的加速器全格感測范圍為±2g、±4g、±8g與 ±16g[2]。

2、磁傳感器HMC5883L

霍尼韋爾HMC5883L是一種表面貼裝高集成弱磁傳感器芯片，帶有數字接口，應用于低成本羅盤和磁場檢測領域。HMC5883L包括先進的高分辨率HMC118X系列磁阻傳感器，附帶霍尼韋爾專利集成電路包括放大器、自動消磁驅動器、偏差校準、能使羅盤精度控制在1o～2o的12位ADC。可在2.16V～3.6V低電壓工作，超低功耗僅為100μA，此外，還有著簡易的I2C系列總線接口。綜上所述，我們選用HMC5883L作為測量航姿信息的輔助模塊。

3、DSP數字信號處理器TMS320F28335

TMS320F28335是TI公司的一款浮點DSP控制器。與以往定點DSP相比，該器件具有精度高、成本低、功耗小、性能高、外設集成度高、數據以及程序存儲量大，A/D轉換更精確快速等優點。

TMS320F28335具有150MHz的高速處理能力，具備32位浮點處理單元，6個DMA通道支持ADC、McBSP和EMIF，多達18路的PWM輸出，其中6路為TI特有的更高精度的PWM輸出（HRPWM），12位16通道ADC。得益于其浮點運算單元，用戶可快速編寫控制算法而無需在處理小數操作上耗費過多的時間和精力，與前代DSP相比，平均性能提高50%，并與定點C28x控制器軟件兼容，從而簡化軟件開發，縮短開發周期，降低開發成本。并且，TMS320F28335具有低功耗和省電模式，支持IDLE（空閑）、STANDBY（待機）、HALT（暫停）模式。以上優點，滿足了姿態解算速度、較高精度和低成本的要求。

4、STM32處理器的選擇

主控制芯片STM32F系列屬于中低端的32位ARM微控制器，該系列芯片是意法半導體（ST）公司出品，其內核是Cortex-M3。該系列芯片按片內Flash的大小可分為三大類：小容量（16K和32K）、中容量（64K和128K）、大容量（256K、384K和512K）。我們這里選用了STM32F103T8處理器，該處理器以ARM32位Cortex-M3為內核，具有64kb的閃存存儲器，20kb的運行內存，有著睡眠、停機和待機三種省電模式。對于姿態解算更加快速便捷，從而能可以完成更多的解算優化。

綜上所述，選用了最為合理的器件來構建航姿參考系統的硬件平臺，從而使系統更加低功耗、小型化，其電路連接設計如圖2所示。

三、基于四元數的姿態解算法

姿態解算算法是捷聯慣導系統算法的核心，通過陀螺儀和加速度計的測量值實時地計算姿態矩陣，從而可以根據姿態矩陣獲得運動載體的姿態和航向信息。目前常用的姿態解算方法有歐拉角法、方向余弦法、四元數法和等效旋轉矢量法，本文對四元數法加以解釋[3-4]。

1、慣導坐標系

航姿系統為了得到比較精準的位置信息，需要提供載體相對于某一坐標系的位置、姿態信息和速度等信息。載體的姿態往往是利用載體坐標系和地理坐標系的位置關系來反映，慣性導航通常用航向角ψ、俯仰角θ、橫滾角γ來表示無人機的三個姿態角，如圖3所示。

航向角ψ：航向角定義為飛行器繞Z軸轉動的角度，定義繞Z軸按右手定則轉動為正方向，定義域為-180o～+180o；

俯仰角θ：俯仰角定義為飛行器繞X軸旋轉的角度，定義繞X軸按右手定則轉動為正方向，定義域為 -90o ～ +90o；

橫滾角γ：橫滾角定義為飛行器繞Y軸旋轉的角度，定義繞Y軸按右手定則轉動為正方向，定義域為 -180o ～ +180o。

2、基于四元數的姿態解算算法

四元數是由四個元構成的數，其形式為：

其中，q0、q1、q2、q3是實數；i、j、k表示 3個虛數單位。

四元數可用于描述剛體相對于某個坐標系的定點轉動，標量部分表示為轉角一半的余弦值，矢量部分保存旋轉順軸、旋轉方向和轉動角度信息。可表示為：

其中，uR—表示旋轉順軸和旋轉方向；θ—表示轉動的角度。

當只關心載體坐標系相對導航坐標系的角位置關系時，可認為載體坐標系是由導航坐標系一次等效旋轉形成，并無中間多次基本旋轉，而四元數包含了等效旋轉的全部信息，因此可用來描述剛體的定點轉動。

四元數可確定出載體坐標系系到導航坐標系的坐標變換矩陣：

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

求解的姿態四元數代入到坐標變換矩陣的四元數表達式中，即可解算出姿態角：

四、軟件設計

TMS320F28335處理器獲得原始傳感器數據后，首先對初始位置的航姿信息進行姿態解算。根據得到的姿態四元數的值，利用卡爾曼濾波進行數據融合，計算出最終姿態四元數并反推出姿態角。并且由于卡爾曼濾波數據融合對陀螺進行了補償，抑制了其漂移，進而得到了輸出穩定的航姿信息。

工作的整個軟件流程可以按圖4來進行，外設初始化單元、傳感器初始化、原始數據處理、姿態角輸出。

五、航姿參考系統的驗證

完成了航姿參考系統的硬件和軟件的設計，將進行其性能的精度及穩定性的測試。航姿參考系統的試驗包括：靜態實驗和動態試驗，主要是用來測試系統的性能，即在相應條件下，得出航向角ψ、俯仰角θ、橫滾角γ與理論值的誤差，并且驗證其是否在合理的范圍之內。

為了滿足實際應用需求，本文選用SGT320E型三軸多功能轉臺（如圖5所示）進行航姿參考系統的誤差標定、誤差補償以及性能測試等。

1、航姿系統的靜態實驗

測試系統的靜態性能，就是驗證靜態條件下，測試航向角、俯仰角和橫滾角與理論值的誤差。將航姿參考系統固定于轉臺X軸平面上，并且調平，此時能保證系統的Z軸與X、Y軸垂直，也就是保證系統的俯仰角和橫滾角的理論值為0。穩定后，通過上位機輸出的姿態信息，生成如圖6所示的姿態角。

我們對姿態角和航向角數據進行誤差分析，采集100個航姿系統輸出的姿態數據并和理論值進行對比，得到俯仰角、橫滾角和航向角的均值誤差和均方差誤差，如表1所示。

表1 航姿系統的靜態誤差分析

由圖6及表1所示可知，橫滾角和俯仰角的數據在0.2o以內變化，航向角的數據在0.3o以內變化，數值的波動性不大，數據表明，航姿參考系統的靜態性能良好。

2、航姿系統的動態實驗

測試系統的動態性能，就是利用三軸轉臺模擬飛行器在平飛、轉彎、升降等動態環境下，航姿參考系統輸出的信息，我們把輸出的姿態角信息與理論值進行對比，它們之間的差值即為誤差，從而驗證系統的動態性能。

將航姿參考系統固定于轉臺X軸平面上，并且調節初始航向角ψ=0o、俯仰角θ=30o、橫滾角γ=0o，并且使轉臺繞Z軸以1o/s的速度轉動，X軸和Y軸保持初始值。通過上位機輸出的姿態信息，在此動態環境下，通過上位機輸出姿態信息，圖7(a)為俯仰角的姿態角。同理，得出在此動態環境下的橫滾角和航向角，在此就不再贅述。

在動態條件下，取1000個俯仰角采樣點，與理論值進行誤差仿真生成如圖7(b)所示。由圖可知，動態情況下，俯仰角的角度誤差控制在0.5o以內，同理我們得出橫滾角和航向角的誤差控制在0.4o和0.6o。我們對姿態角和航向角數據進行誤差分析，采集100個航姿系統輸出的姿態數據并和理論值進行對比，得到俯仰角、橫滾角和航向角的均值誤差和均方差誤差，如表2所示。

表2 航姿系統的動態誤差分析

由以上圖標數據可知，在動態環境下姿態角的輸出能夠實時地反映姿態角的信息。故實驗數據表明：航姿參考系統整體動態性能良好。

綜上所述，該航姿參考系統能夠滿足對姿態的實時精確測量，實現了設計小型化、低功耗和低成本的設計要求，可應用于民用等領域，滿足了本文的設計要求。

六、結語

航姿參考系統能夠實時精確的獲得姿態信息是系統運行的根本保障。本文首先介紹了選取硬件器材的依據，采用了精度較高的MEMS器件與STM32單片機，并且結合了DSP高速處理數據的能力，構建了航姿參考系統的硬件平臺。其次，提出了硬件軟件的設計流程，將得到的原始數據經過卡爾曼濾波處理，輸出了更為準確的姿態信息。最后，通過三軸轉臺進行了數據的分析和驗證。結果表明，本文設計的航姿參考系統能夠實時準確的輸出穩定的姿態數據，滿足了系統對姿態控制的所需要求。因此，可以廣泛地運用于生活民用領域。