基于DSP和STM32的航姿參考系統(tǒng)設(shè)計

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

一、引言

姿態(tài)測量能夠為導(dǎo)航和控制提供精確的參考信息，近年來隨著MEMS慣性傳感器的快速發(fā)展，民用領(lǐng)域?qū)ψ藨B(tài)測量的使用也越來越廣泛，如機(jī)器人、無人機(jī)、船舶導(dǎo)航、無人駕駛、三維游戲以及跳傘訓(xùn)練的監(jiān)測等領(lǐng)域。而在姿態(tài)的測量中，航姿參考系統(tǒng)（Attitude and Heading Reference System, AHRS）便顯得尤為重要[1]。

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

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

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

二、硬件結(jié)構(gòu)

基于MEMS技術(shù)的傳感器和姿態(tài)解算技術(shù)極大促進(jìn)了低成本小型化AHRS技術(shù)的發(fā)展。硬件結(jié)構(gòu)中，在基于STM32為處理器的同時，以DSP處理器為核心，采用低成本、體積小、低功耗和重量輕的MPU6050慣性傳感器和HMC588L磁強(qiáng)計構(gòu)成測量單元，構(gòu)建航姿參考系統(tǒng)的硬件平臺。

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

1、慣性測量單元MPU6050

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

2、磁傳感器HMC5883L

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

3、DSP數(shù)字信號處理器TMS320F28335

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

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

4、STM32處理器的選擇

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

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

三、基于四元數(shù)的姿態(tài)解算法

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

1、慣導(dǎo)坐標(biāo)系

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

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

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

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

2、基于四元數(shù)的姿態(tài)解算算法

四元數(shù)是由四個元構(gòu)成的數(shù)，其形式為：

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

四元數(shù)可用于描述剛體相對于某個坐標(biāo)系的定點轉(zhuǎn)動，標(biāo)量部分表示為轉(zhuǎn)角一半的余弦值，矢量部分保存旋轉(zhuǎn)順軸、旋轉(zhuǎn)方向和轉(zhuǎn)動角度信息?？杀硎緸椋?/p>

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

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

四元數(shù)可確定出載體坐標(biāo)系系到導(dǎo)航坐標(biāo)系的坐標(biāo)變換矩陣：

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

求解的姿態(tài)四元數(shù)代入到坐標(biāo)變換矩陣的四元數(shù)表達(dá)式中，即可解算出姿態(tài)角：

四、軟件設(shè)計

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

工作的整個軟件流程可以按圖4來進(jìn)行，外設(shè)初始化單元、傳感器初始化、原始數(shù)據(jù)處理、姿態(tài)角輸出。

五、航姿參考系統(tǒng)的驗證

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

為了滿足實際應(yīng)用需求，本文選用SGT320E型三軸多功能轉(zhuǎn)臺（如圖5所示）進(jìn)行航姿參考系統(tǒng)的誤差標(biāo)定、誤差補(bǔ)償以及性能測試等。

1、航姿系統(tǒng)的靜態(tài)實驗

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

我們對姿態(tài)角和航向角數(shù)據(jù)進(jìn)行誤差分析，采集100個航姿系統(tǒng)輸出的姿態(tài)數(shù)據(jù)并和理論值進(jìn)行對比，得到俯仰角、橫滾角和航向角的均值誤差和均方差誤差，如表1所示。

表1 航姿系統(tǒng)的靜態(tài)誤差分析

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

2、航姿系統(tǒng)的動態(tài)實驗

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

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

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

表2 航姿系統(tǒng)的動態(tài)誤差分析

由以上圖標(biāo)數(shù)據(jù)可知，在動態(tài)環(huán)境下姿態(tài)角的輸出能夠?qū)崟r地反映姿態(tài)角的信息。故實驗數(shù)據(jù)表明：航姿參考系統(tǒng)整體動態(tài)性能良好。

綜上所述，該航姿參考系統(tǒng)能夠滿足對姿態(tài)的實時精確測量，實現(xiàn)了設(shè)計小型化、低功耗和低成本的設(shè)計要求，可應(yīng)用于民用等領(lǐng)域，滿足了本文的設(shè)計要求。

六、結(jié)語

航姿參考系統(tǒng)能夠?qū)崟r精確的獲得姿態(tài)信息是系統(tǒng)運(yùn)行的根本保障。本文首先介紹了選取硬件器材的依據(jù)，采用了精度較高的MEMS器件與STM32單片機(jī)，并且結(jié)合了DSP高速處理數(shù)據(jù)的能力，構(gòu)建了航姿參考系統(tǒng)的硬件平臺。其次，提出了硬件軟件的設(shè)計流程，將得到的原始數(shù)據(jù)經(jīng)過卡爾曼濾波處理，輸出了更為準(zhǔn)確的姿態(tài)信息。最后，通過三軸轉(zhuǎn)臺進(jìn)行了數(shù)據(jù)的分析和驗證。結(jié)果表明，本文設(shè)計的航姿參考系統(tǒng)能夠?qū)崟r準(zhǔn)確的輸出穩(wěn)定的姿態(tài)數(shù)據(jù)，滿足了系統(tǒng)對姿態(tài)控制的所需要求。因此，可以廣泛地運(yùn)用于生活民用領(lǐng)域。