基于DSP/FPGA的高速旋轉(zhuǎn)體姿態(tài)角及轉(zhuǎn)速的解算系統(tǒng)設(shè)計(jì)

沈懷營(yíng)，吳伯農(nóng)，王帥輝

(北方工業(yè)大學(xué) 機(jī)械與材料工程學(xué)院，北京 100144)

?

基于DSP/FPGA的高速旋轉(zhuǎn)體姿態(tài)角及轉(zhuǎn)速的解算系統(tǒng)設(shè)計(jì)

沈懷營(yíng)，吳伯農(nóng)，王帥輝

(北方工業(yè)大學(xué) 機(jī)械與材料工程學(xué)院，北京 100144)

摘要：傳統(tǒng)的陀螺儀動(dòng)態(tài)范圍小、精度低、抗沖擊能力差，很難達(dá)到慣性導(dǎo)航的目的。因此，設(shè)計(jì)了一種基于線性加速度計(jì)(ADXL193)及磁阻傳感器(HMC1043)為慣性測(cè)量單元的轉(zhuǎn)速及姿態(tài)角解算系統(tǒng)。該系統(tǒng)運(yùn)用FPGA協(xié)處理器控制ADC芯片完成12路模擬信號(hào)的同步轉(zhuǎn)換，并將轉(zhuǎn)換后的數(shù)據(jù)緩存；最終由DSP芯片TMS320C6713主處理器完成姿態(tài)角及轉(zhuǎn)速的實(shí)時(shí)解算。

關(guān)鍵詞：慣性導(dǎo)航；轉(zhuǎn)速及姿態(tài)角解算；ADXL193；TMS320C6713

在傳統(tǒng)的慣性測(cè)量中常選用陀螺儀測(cè)量轉(zhuǎn)速及姿態(tài)角，但是其動(dòng)態(tài)范圍小、精度低、抗沖擊能力差，很難達(dá)到慣性導(dǎo)航的目的。而MEMS加速度計(jì)相比與陀螺儀具有量程大、能耗小、動(dòng)態(tài)范圍大、壽命長(zhǎng)、成本低，而且體積小、安裝方便，其應(yīng)用前景非常好，是今后捷聯(lián)慣性導(dǎo)航系統(tǒng)的主要發(fā)展方向。

目前，在無(wú)陀螺慣性導(dǎo)航領(lǐng)域，國(guó)內(nèi)學(xué)者在最近幾年也做了不少研究工作，并提出了各種加速度計(jì)安裝方案，主要以六加速度計(jì)及九加速度計(jì)安裝方案為主。以六加速度計(jì)解算姿態(tài)角，只能解算出載體的轉(zhuǎn)速平方項(xiàng)，開(kāi)方后無(wú)法確定其方向。本系統(tǒng)設(shè)計(jì)了一種由九加速度計(jì)與磁強(qiáng)計(jì)作為慣性測(cè)量單元進(jìn)行轉(zhuǎn)速及姿態(tài)角的解算，該方案解算精度高，不必把加速度計(jì)放在質(zhì)心處，雖然有漂移誤差，但是由于載體的飛行時(shí)間較短，誤差符合預(yù)設(shè)精度要求。

采用FPGA作為協(xié)處理器完成微慣性測(cè)量單元中多路傳感器信號(hào)的同步采集及緩存，可以使DSP專注于解算而免于被頻繁的數(shù)據(jù)采集中斷而打擾，從而提高了姿態(tài)角測(cè)量的精度和實(shí)時(shí)性。

1加速度計(jì)配置方案及仿真分析

九加速度計(jì)及磁強(qiáng)計(jì)的安裝方式如圖1所示，編號(hào)1-9為加速度計(jì)，編號(hào)10-12為磁強(qiáng)計(jì)，箭頭方向?yàn)榧铀俣扔?jì)或磁強(qiáng)計(jì)的敏感方向。

圖1　九加速度計(jì)及磁強(qiáng)計(jì)的安裝示意圖

對(duì)于飛行體的轉(zhuǎn)速，在這里不進(jìn)行詳細(xì)解算，參考文獻(xiàn)[1]可知，飛行體的角加速度為：

(1)

利用積分法對(duì)上式積分解得鐵餅的轉(zhuǎn)速，對(duì)于飛行體的姿態(tài)角，這里采用四元數(shù)法，參考文獻(xiàn)[2]可知，飛行體的姿態(tài)角為：

(2)

對(duì)所設(shè)計(jì)的慣性測(cè)量單元中加速度計(jì)的構(gòu)型方案進(jìn)行仿真分析，論述該方案的可行性，本次運(yùn)用MATLAB中的Simulink模塊實(shí)現(xiàn)對(duì)捷聯(lián)慣性導(dǎo)航系統(tǒng)仿真。由于條件有限，我們不能得到實(shí)際數(shù)據(jù)，因此我們先采用數(shù)學(xué)方法模擬出鐵餅飛行過(guò)程中的軌跡，然后計(jì)算出陀螺儀和加速度計(jì)的輸出，把其作為SINS解算器的輸入，進(jìn)行SINS解算，把解算的數(shù)據(jù)與模擬的軌跡數(shù)據(jù)(角速度、姿態(tài)、位置)進(jìn)行對(duì)比，得到誤差曲線，進(jìn)行誤差分析。

軌跡生成器的數(shù)據(jù)[3]如式(3)，(4)所示：

(3)

(4)

由于鐵餅投擲過(guò)程長(zhǎng)其飛行時(shí)間大約為16 s內(nèi)，因此將本次仿真的時(shí)間設(shè)置為16 s，采樣時(shí)間間隔為0.01 s，本次仿真中，忽略加速度計(jì)的常值漂移及隨機(jī)誤差，進(jìn)行仿真，轉(zhuǎn)速及俯仰角的仿真誤差如圖2，圖3所示。

圖2　轉(zhuǎn)速仿真誤差

圖3　姿態(tài)角仿真誤差

本次設(shè)計(jì)的轉(zhuǎn)速傳感器量程[3]為30 r/s(轉(zhuǎn)每秒)，而從圖2可以看出在16 s內(nèi)其轉(zhuǎn)速誤差低于30°/s(度每秒)，符合預(yù)設(shè)要求；從圖3可以看出由四元數(shù)法解得的俯仰角誤差在0.5°內(nèi)，誤差特別小，因此此處可以選用這種方法解算。

2系統(tǒng)硬件設(shè)計(jì)

該解算系統(tǒng)包括電源模塊、微慣性測(cè)量單元電路、信號(hào)調(diào)理電路、信號(hào)采集電路、FPGA及外圍電路、DSP及外圍電路。微慣性測(cè)量單元輸出的模擬信號(hào)經(jīng)信號(hào)調(diào)理電路濾波及放大，進(jìn)入AD轉(zhuǎn)換，在FPGA中設(shè)計(jì)了兩個(gè)模塊，其中ADC控制模塊負(fù)責(zé)對(duì)A/D轉(zhuǎn)換模塊控制，F(xiàn)IFO存儲(chǔ)模塊負(fù)責(zé)對(duì)轉(zhuǎn)換后的數(shù)據(jù)緩存，DSP負(fù)責(zé)把FIFO緩存的數(shù)據(jù)進(jìn)行解算，從而把DSP從繁瑣的數(shù)據(jù)采集及緩存中斷中解脫出來(lái)。

2.1微慣性測(cè)量單元電路設(shè)計(jì)

2.1.1ADXL193電路設(shè)計(jì)

考慮到鐵餅飛行過(guò)程中產(chǎn)生的加速度較大，因此，加速度計(jì)的量程必須滿足要求，本文采用的是美國(guó)ADI公司生產(chǎn)的ADXL193硅微變電容式加速度計(jì)。ADXL193是單軸加速度計(jì)，其測(cè)量范圍可以達(dá)到±250 g，其靈敏度為8 g/mV，輸出信號(hào)帶寬為0.4 kHz，能抵抗達(dá)4 000 g的沖擊強(qiáng)度，元器件尺寸大小為5 mm×5 mm×5 mm，采用LCC封裝。加速度計(jì)ADXL193的電路圖如圖4所示。

圖4　加速度計(jì)ADXL193的電路圖

2.1.2HMC1043復(fù)位電路設(shè)計(jì)

由于地磁場(chǎng)磁場(chǎng)強(qiáng)度很弱，因此我們要選用分辨率高的傳感器；考慮到鐵餅的內(nèi)部空間有限，因此選擇的磁傳感器的體積要盡量小。這里選用HMC1043磁傳感器，其分辨率高達(dá)120 uguass，量程為±6 guass，封裝尺寸僅為3 mm×3 mm×3 mm，符合要求。由于該傳感器長(zhǎng)期暴漏在外部磁場(chǎng)中，內(nèi)部單元的磁敏感方向雜亂無(wú)章，因而會(huì)造成靈敏度的下降，因此必須為磁傳感器進(jìn)行置位/復(fù)位。磁傳感器HMC1043的置位/復(fù)位電路圖如圖5所示。

圖5　磁傳感器HMC1043的復(fù)位/置位電路圖

2.2AD7656模數(shù)轉(zhuǎn)換器

模數(shù)轉(zhuǎn)換芯片采用美國(guó)模擬器件(ADI)公司推出的AD7656芯片[6]，該芯片分辨率為16位，吞吐率高達(dá)250 Ksps，可處理的最高輸入頻率高達(dá)12 MHz；轉(zhuǎn)換時(shí)間僅為3.1 μs；可允許6個(gè)ADC獨(dú)立的進(jìn)行同步采樣。

為了同時(shí)完成12路傳感器輸出信號(hào)的模數(shù)轉(zhuǎn)換，這里選用2片AD7656芯片，同時(shí)CONVSTA、CONVSTB、CONVSTC引腳要全部接在一起，即可完成12通道的同步轉(zhuǎn)換；AD7656轉(zhuǎn)換時(shí)，為了提高效率，簡(jiǎn)化設(shè)計(jì)，AD7656將采用內(nèi)部基準(zhǔn)電壓，因此可將H/SSEL引腳拉低，同時(shí)將REFEN/DIS拉高即可；由于各傳感器的輸出電壓小于5 V，因此將AD7656的參考電壓定為2.5 V，輸入方式選為±2REF，因此將RANGE拉高即可。為了提高采集數(shù)據(jù)的效率，可將轉(zhuǎn)換后的數(shù)據(jù)輸出設(shè)置為并行傳輸方式；另外，將A/D轉(zhuǎn)換芯片的16根輸出引腳與FPGA的16個(gè)I/O引腳相連接；本次設(shè)計(jì)選用XILINX公司的Spartan3A系列FPGA，該芯片最大可用I/O口311個(gè)，可滿足設(shè)計(jì)要求。

2.3DSP與FPGA的接口電路設(shè)計(jì)

FPGA與DSP采用DSP的EMIF接口進(jìn)行彼此通信[5]，即將FPGA芯片作為DSP的異步外設(shè)，只需設(shè)置EMIF控制的寄存器為SRAM類型即可。由于在FPGA中設(shè)計(jì)兩個(gè)模塊，因此需要選擇兩個(gè)片選信號(hào)，即CE2和CE3，其地址范圍為0XA00000000-0XB00000000。FPGA與DSP的接口示意圖如圖6所示。在圖6中，ED[31:0]為數(shù)據(jù)總線，EA[21:2]為地址總線；CE為選通線，AOE為讀控制線，AWE為寫(xiě)控制線，它們的時(shí)序可以通過(guò)設(shè)置相關(guān)的控制寄存器來(lái)實(shí)現(xiàn)。另外，為了保證DSP與FPGA在信號(hào)傳輸中的完整性，因此在它們之間串聯(lián)了33 Ω的電阻來(lái)減小信號(hào)失真。

圖6　FPGA與DSP的接口示意圖

3系統(tǒng)軟件設(shè)計(jì)

軟件設(shè)計(jì)主要是通過(guò)程序?qū)崿F(xiàn)鐵餅導(dǎo)航參數(shù)的解算[4]，其工作流程圖如圖7所示，由圖可知，首先系統(tǒng)進(jìn)行復(fù)位，然后進(jìn)行系統(tǒng)初始化，包括PLL時(shí)鐘模塊、外部存儲(chǔ)器接口(EMIF)、中斷寄存器、A/D等初始化設(shè)置；在執(zhí)行完初始化后，啟動(dòng)定時(shí)器，等待FPGA中斷，當(dāng)中斷結(jié)束后，DSP讀取FIFO中的數(shù)據(jù)，進(jìn)行導(dǎo)航參數(shù)的解算，并將解算的結(jié)果存在指定的存儲(chǔ)位置。

圖7　系統(tǒng)程序流程圖

4結(jié)束語(yǔ)

本文設(shè)計(jì)的轉(zhuǎn)速傳感器的量程為30 r/s，通過(guò)轉(zhuǎn)速的解算及仿真可以看出解算誤差在量程范圍內(nèi)，符合預(yù)設(shè)要求；同時(shí)，加速度計(jì)與磁傳感器的選用解決了陀螺儀量程不足、抗沖擊性不強(qiáng)的劣勢(shì)；陀螺在設(shè)計(jì)系統(tǒng)時(shí)，采用FPGA作為協(xié)處理器，實(shí)現(xiàn)了數(shù)據(jù)的高速實(shí)時(shí)采集及緩存，提高了DSP的解算工作效率。

參考文獻(xiàn)

[1]李海濤.加速度計(jì)/磁強(qiáng)計(jì)捷聯(lián)慣導(dǎo)系統(tǒng)姿態(tài)解算方法研究[D].太原：中北大學(xué),2007.5.

[2]史震，于秀萍，馬澍田.無(wú)陀螺捷聯(lián)式慣性導(dǎo)航系統(tǒng)[M].哈爾濱：哈爾濱工程大學(xué)出版社，2005.

[3]羅林燕.基于DSP的無(wú)陀螺角速度傳感器的研究[D].北京： 北方工業(yè)大學(xué)，2013.7.

[4]楊敏.小型捷聯(lián)慣性導(dǎo)航系統(tǒng)研究[D].長(zhǎng)沙：中南大學(xué)，2010.3.

[5]劉國(guó)志.基于DSP-FPGA的捷聯(lián)慣性導(dǎo)航系統(tǒng)設(shè)計(jì)[D].大連：大連理工大中學(xué)，2009.11.

[6]美國(guó)ADI公司.AD7656數(shù)據(jù)手冊(cè)[Z].Norwood:ADI,2006.

The Solution System Design of High Speed Rotating Body Attitude Angle and Rotational Speed Based on DSP/FPGA

Shen Huaiying, Wu Bonong, Wang Shuaihui

(NorthChinaUniversityofTechnology,Beijing100144,China)

Abstract:The traditional gyroscope is difficult to achieve the goal of inertial navigation due to its small dynamic range, low precision and poor impact resistance. A mini attitude angle resolving system is designed to detect attitude angle. It takes accelerometer ADXL193 and geomagnetic sensor HMC1043 as MIMU and adopts FPGA as core coprocessor to control the ADC chip to make the 12-way sensor signals converted synchronously, then sampling data cached. Finally, the real-time resolving for attitude angle is achieved by DSP chip which as the core processor.

Key words:inertial navigation; speed and attitude angle resolving; ADXL193; TMS320C6713

收稿日期：2016-03-29

作者簡(jiǎn)介：沈懷營(yíng)(1988- )，男，山東滕州市人，碩士研究生，研究方向：慣性導(dǎo)航。

文章編號(hào)：1674- 4578(2016)03- 0041- 03

中圖分類號(hào)：TP 212.13

文獻(xiàn)標(biāo)識(shí)碼：A