呂金銳

摘? 要： 為了實(shí)現(xiàn)對(duì)某彈載系統(tǒng)的姿態(tài)測(cè)量，提出一種基于FPGA+DSP的姿態(tài)測(cè)量系統(tǒng)的設(shè)計(jì)。系統(tǒng)采用FPGA作為邏輯控制核心，完成對(duì)MEMS傳感器輸出的陀螺加表數(shù)據(jù)的采集，并通過(guò)EMIF接口完成與DSP的數(shù)據(jù)通信。DSP完成對(duì)姿態(tài)信息的解算，并由FPGA通過(guò)RS422接口將更新后的姿態(tài)信息上傳至上位機(jī)。系統(tǒng)選用地理坐標(biāo)系作為導(dǎo)航坐標(biāo)系，利用四元數(shù)法及Kalman信息融合算法對(duì)采集到的姿態(tài)信息進(jìn)行解算。測(cè)試結(jié)果表明，該系統(tǒng)輸出的姿態(tài)信息精度較好，總體角誤差在0.5°以內(nèi)。

關(guān)鍵詞： MEMS傳感器; 姿態(tài)測(cè)量; FPGA+DSP; 四元數(shù); Kalman信息融合; 姿態(tài)解算

中圖分類號(hào)：TN702? ? ? ? ? 文獻(xiàn)標(biāo)識(shí)碼：A? ? ? ? ? ? 文章編號(hào)：1006-8228（2021）05-29-04

Design and implementation of FPGA and DSP for attitude measurement system

LV Jinrui

（Department of Information Engineering， Taiyuan City Vocational college， Taiyuan， Shanxi 044000， China）

Abstract： A kind of attitude measurement system based on FPGA and DSP is proposed to realize the attitude measurement of a projectile-borne system. The system uses FPGA as the core of logic control to collect the gyro table data output by MEMS sensor and to communicate data with DSP through EMIF interface; The DSP completes the resolution of the attitude information， while the FPGA uploads the updated attitude information to the upper computer through the RS422 interface. Geographic coordinate system is chosen as navigation coordinate system， and the attitude information collected is solved by Quaternion method and Kalman information fusion algorithm. The test results show that the accuracy of attitude information output by the system is good， and the overall angle error is within 0.5.

Key words： MEMS sensor; attitude measurement; FPGA and DSP; Quaternion; Kalman information fusion; attitude resolution

0 引言

姿態(tài)解算作為姿態(tài)測(cè)量系統(tǒng)中的核心部分，正朝著高精度和高速度的方向發(fā)展[1]。為實(shí)現(xiàn)更加準(zhǔn)確的對(duì)彈載系統(tǒng)的姿態(tài)測(cè)量，不僅對(duì)數(shù)據(jù)的采集及處理算法提出更高的要求，同時(shí)對(duì)于處理器的選擇也越來(lái)越重視。在測(cè)量系統(tǒng)中，確定姿態(tài)主要運(yùn)用的是陀螺，但由于陀螺儀長(zhǎng)期工作存在精度測(cè)量漂移從而無(wú)法保證系統(tǒng)精度，對(duì)姿態(tài)的確定會(huì)產(chǎn)生很大的偏差[2]。針對(duì)以上問(wèn)題，系統(tǒng)采用ADIS16488傳感器為核心，結(jié)合FPGA的并行處理方式以及DSP強(qiáng)大的數(shù)據(jù)處理能力，構(gòu)建了一個(gè)姿態(tài)測(cè)量系統(tǒng)。

1 系統(tǒng)總體設(shè)計(jì)

系統(tǒng)采用美國(guó)AD公司產(chǎn)品ADIS16488 MEMS慣性傳感器作為核心，其內(nèi)部集成了三軸微陀螺儀，三軸微加速度計(jì)，三軸微磁強(qiáng)計(jì)以及壓力傳感器[3]。當(dāng)MEMS慣性傳感器處于工作模式時(shí)，能夠同時(shí)采集三個(gè)軸向上的加速度、角速度及磁強(qiáng)度。

系統(tǒng)總體結(jié)構(gòu)圖如圖1所示。系統(tǒng)工作原理：FPGA作為主控芯片，首先通過(guò)SPI接口對(duì)傳感器進(jìn)行初始化配置，然后根據(jù)上位機(jī)指令采集傳感器輸出的陀螺加表等數(shù)據(jù)至數(shù)據(jù)緩沖模塊，通過(guò)EMIF接口將數(shù)據(jù)傳輸至DSP進(jìn)行數(shù)據(jù)處理，最后將解算好的姿態(tài)信息通過(guò)RS422發(fā)送模塊上傳至上位機(jī)進(jìn)行顯示。

2 硬件電路設(shè)計(jì)

2.1 傳感器數(shù)據(jù)采集模塊

慣性測(cè)量單元正常工作時(shí)，需加載3.3V電壓至10、11、12引腳[4]。上電后，系統(tǒng)首先通過(guò)FPGA完成對(duì)傳感器的初始化配置，然后SPI接口來(lái)讀取更新后的傳感器數(shù)據(jù)。傳感器內(nèi)部寄存器采用分頁(yè)結(jié)構(gòu)，每一頁(yè)都對(duì)應(yīng)其相應(yīng)的功能[5]。FPGA對(duì)MEMS傳感器進(jìn)行讀寫(xiě)時(shí)需要連續(xù)兩個(gè)8bit地址進(jìn)行操作，其中低地址在前，高地址在后。高8bit地址表示傳感器內(nèi)部相應(yīng)寄存器的地址，低8bit地址表示寫(xiě)入該寄存器的數(shù)據(jù)內(nèi)容。如圖2所示，對(duì)傳感器進(jìn)行讀操作時(shí)需要一個(gè)16bit的寄存器地址，且相應(yīng)寄存器地址上的數(shù)據(jù)輸出將在下一個(gè)寄存器地址輸入時(shí)才會(huì)輸出。

2.2 FPGA與DSP通信接口設(shè)計(jì)

FPGA與DSP通過(guò)EMIF接口來(lái)實(shí)現(xiàn)通信，如圖3所示，EMA_DATA[15：0]為數(shù)據(jù)總線，EMA_WE為FIFO讀使能信號(hào)，GPIO4_15為啟動(dòng)數(shù)據(jù)發(fā)送信號(hào)，EMA_CS為EMIF使能信號(hào)。為匹配數(shù)據(jù)傳輸速率，采用雙端口RAM來(lái)對(duì)讀取的DSP數(shù)據(jù)進(jìn)行緩存，在雙端口RAM模塊和DSP模塊之間串聯(lián)33Ω電阻來(lái)減少信號(hào)失真[6]。雙口RAM發(fā)送模塊不僅實(shí)現(xiàn)了數(shù)據(jù)的讀寫(xiě)?yīng)毩?，也滿足了數(shù)據(jù)在發(fā)送時(shí)間上的要求。

3 姿態(tài)解算算法

3.1 初始姿態(tài)對(duì)準(zhǔn)

在求解載體姿態(tài)前對(duì)系統(tǒng)進(jìn)行初始對(duì)準(zhǔn)，對(duì)于提高姿態(tài)信息精度有很大的影響。首先利用載體在地理坐標(biāo)系的投影[00gT]和在載體坐標(biāo)系的投影[fxbfybfzbT]，得出載體橫滾角和俯仰角，然后利用求得的初始橫滾角和俯仰角計(jì)算初始航向角[7]。算法推導(dǎo)過(guò)程如下：

[fxbfybfzb=Cnb00g]=[cosγcosγsinθ-sinγcosθ0cosθsinθsinγ-sinγsinθcosγcosθ][ 00g]

求解得初始橫滾角和俯仰角：

[θ=sin-1][fyb/g]

[γ=tan-1-fxb/fzb]

結(jié)合磁強(qiáng)計(jì)由載體坐標(biāo)系映射到磁場(chǎng)x、y方向上的值，求解得：

[Hx=mxcosθ+mysinγsinθ+mzcosθsinγ]

[Hy=mycosγ-mzsinγ]

然后求得初始航向角：

[ψ=arctanHy/Hx]

3.2 四元數(shù)法

根據(jù)歐拉定理可知，載體坐標(biāo)系b相對(duì)于地理坐標(biāo)系n的方位相當(dāng)于b系繞定軸轉(zhuǎn)動(dòng)3次方位角，四元數(shù)包含了轉(zhuǎn)動(dòng)的信息[8]。[un]為旋轉(zhuǎn)軸和旋轉(zhuǎn)方向，θ為轉(zhuǎn)過(guò)的角度。

[Q=cosθ2+unsinθ2]

利用哥式等式推導(dǎo)得出四元數(shù)的微分方程為：

[Q=12Q?ωbnb]

矩陣形式為：

[T0T1T2T3=0-ωx-ωy-ωzωx0ωz-ωyωyωz-ωzωy0ωx-ωx0]

然后利用四階龍格庫(kù)塔法求得四元數(shù)更新表達(dá)式：

[k1=T2ωbt?qt]

[k2=T2ωbt+T2?qt+k12]

[k3=T2ωbt+T2?qt+k22]

[k4=T2ωbt+T2?qt+k3]

[qt+T=q（t）+][T6][k1+2k2+2k3+k4]

四元數(shù)可確定出載體坐標(biāo)系到地理坐標(biāo)系的坐標(biāo)轉(zhuǎn)換公式：

[Cnb=q20+q21-q22-q232q1q2-q0q32q1q3+q0q22q1q2+q0q3q20-q21+q22-q232q2q3-q0q12q1q3-q0q22q2q3+q0q1q20-q21-q22+q23]

3.3 卡爾曼濾波

卡爾曼濾波的主要作用在于最大限度的減小噪聲目標(biāo)信號(hào)的影響，利用不同時(shí)刻的系統(tǒng)狀態(tài)信息，對(duì)信息預(yù)測(cè)和估計(jì)，最終得到一個(gè)較為精確的數(shù)值。它相當(dāng)于一個(gè)動(dòng)態(tài)遞歸的過(guò)程：

[Xk=AkXk-1+Bkuk-1+wk-1]

[Zk=HkXk+vk]

其中，Xk為k時(shí)刻的系統(tǒng)狀態(tài)，Zk表示k時(shí)刻的觀測(cè)變量。uk為K時(shí)刻對(duì)系統(tǒng)的控制量。vk，wk分別表示狀態(tài)方程的觀測(cè)噪聲和過(guò)程噪聲。Hk是觀測(cè)噪聲誤差狀態(tài)增益矩陣，Ak和Bk是系統(tǒng)參數(shù)[9]。

卡爾曼濾波的時(shí)間更新公式為：

[X-k=AXk-1+Buk-1]

[P-k=APk-1AT+Q]

由k-1時(shí)刻的最優(yōu)值[Xk-1]去預(yù)測(cè)k時(shí)刻系統(tǒng)的狀態(tài)值[X-k]。然后由上一次的誤差協(xié)方差[Pk-1]和過(guò)程噪聲Q預(yù)測(cè)新的誤差[10]。

卡爾曼濾波的狀態(tài)更新公式為：

[Kk=P-kHTHP-kHT+R-1]

[Xk]=[X-k+KkZk-HX-k]

[Pk=I-KkHP-k]

計(jì)算卡爾曼增益Kk，并進(jìn)行校正更新，得到的[Xk]即為k時(shí)刻的最優(yōu)估計(jì)值。為下一步估計(jì)k+1時(shí)刻的最優(yōu)估計(jì)值的迭代進(jìn)行更新操作，即更新Pk。

3.4 小波閾值降噪

由于ADIS16488內(nèi)部的MEMS陀螺儀的抗干擾能力較弱，產(chǎn)生的噪聲相對(duì)于其他兩個(gè)傳感器而言更大。因此系統(tǒng)結(jié)合陀螺儀輸出信號(hào)不具備線性的特點(diǎn)，選用小波閾值降噪算法來(lái)對(duì)進(jìn)行降噪處理。小波閥值消噪原理如圖4所示。

系統(tǒng)進(jìn)行了對(duì)靜置的傳感器進(jìn)行了降噪實(shí)驗(yàn)，系統(tǒng)上電后對(duì)陀螺儀y軸數(shù)據(jù)進(jìn)行采集，采樣頻率為200Hz，采樣時(shí)間為100s，得到陀螺儀的實(shí)時(shí)數(shù)據(jù)如圖5所示。

由圖5可以看出，當(dāng)陀螺儀靜置時(shí)，其y軸輸出的角速率誤差值較大，范圍在[-0.04°，-0.04°]。對(duì)該陀螺儀采用小波閾值降噪處理后，得到如圖6所示的誤差波形圖。由圖6可以看出，陀螺儀靜置時(shí)y軸輸出的波形有了明顯的改善，角速率誤差約束到了[-0.02°，-0.02°]。

4 系統(tǒng)測(cè)試

4.1 傳感器數(shù)據(jù)采集模塊仿真

傳感器輸出的數(shù)據(jù)對(duì)于整個(gè)測(cè)量系統(tǒng)來(lái)說(shuō)有著十分重要的作用，它關(guān)乎著數(shù)據(jù)的正確性和完整性。FPGA負(fù)責(zé)接收傳感器輸出的數(shù)據(jù)并將其緩存至內(nèi)部RAM中，同時(shí)也負(fù)責(zé)讀出DSP解算后緩存至RAM中的數(shù)據(jù)，并將其上傳至上位機(jī)。為了驗(yàn)證FPGA與DSP之間通信的正確性，將在FPGA RAM中寫(xiě)入數(shù)據(jù)0-256，DSP通過(guò)EMIF接口讀取RAM中的數(shù)據(jù)，在開(kāi)發(fā)環(huán)境ISE下顯示DSP讀取的數(shù)據(jù)以及其他信號(hào)的一些變化，它讀取的數(shù)據(jù)地址空間是從0xc0000000開(kāi)始。如圖7所示，DSP與FPGA之間通信正確。

4.2 誤差分析

為了對(duì)系統(tǒng)性能進(jìn)行測(cè)試與驗(yàn)證，采用AHRS（商業(yè)級(jí)姿態(tài)參考系統(tǒng)）作為參考。驗(yàn)證過(guò)程中，將該參考系統(tǒng)與電路板固定，并置于三軸無(wú)磁轉(zhuǎn)臺(tái)中。未上電之前調(diào)整軸向，使得系統(tǒng)坐標(biāo)系和轉(zhuǎn)臺(tái)的軸向一致。以z軸為定軸進(jìn)行5°/s的轉(zhuǎn)動(dòng)，采集時(shí)間為360s，數(shù)據(jù)采集完成后通過(guò)串口上傳至上位機(jī)。上位機(jī)對(duì)接收到的數(shù)據(jù)進(jìn)行分析計(jì)算，采用Matlab對(duì)接收到的數(shù)據(jù)進(jìn)行曲線顯示，如圖8、圖9、圖10所示。

由于系統(tǒng)在進(jìn)行測(cè)試時(shí)選用z軸為定軸進(jìn)行轉(zhuǎn)動(dòng)，從得到的誤差曲線圖中可以看出，橫滾角和俯仰角的誤差較小，均在0.4°以內(nèi)，姿態(tài)解算精度較好。即驗(yàn)證了系統(tǒng)在轉(zhuǎn)動(dòng)過(guò)程中，x軸和y軸方向的姿態(tài)沒(méi)有發(fā)散。

將系統(tǒng)姿態(tài)解算的結(jié)果與AHRS測(cè)量的數(shù)據(jù)進(jìn)行比較，得出其誤差的標(biāo)準(zhǔn)差，如表1所示。驗(yàn)證系統(tǒng)解算的結(jié)果較好，誤差值均在0.5°以內(nèi)。

4.3 上位機(jī)數(shù)據(jù)顯示

系統(tǒng)通過(guò)RS422接口將解算好的姿態(tài)信息發(fā)送至上位機(jī)進(jìn)行顯示，如圖11所示。圖11為傳感器繞z軸轉(zhuǎn)動(dòng)時(shí)，三個(gè)軸向角度的變化曲線圖。如圖12所示，可以直觀的看到傳感器的全姿態(tài)信息。

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

系統(tǒng)采用FPGA+DSP雙處理器的方案來(lái)實(shí)現(xiàn)對(duì)某彈載控制器的姿態(tài)解算，通過(guò)系統(tǒng)測(cè)試，驗(yàn)證該姿態(tài)測(cè)量系統(tǒng)的輸出精度較好，均在0.5°以內(nèi)，能夠滿足技術(shù)要求。這為后續(xù)測(cè)量系統(tǒng)的進(jìn)一步優(yōu)化提供了可靠的實(shí)驗(yàn)數(shù)據(jù)。

參考文獻(xiàn)（References）：

[1] 沈懷營(yíng)，吳伯衣.基于DSP/FPGA的高速旋轉(zhuǎn)體姿態(tài)角及轉(zhuǎn)速的解算系統(tǒng)設(shè)計(jì)[J].山西電子技術(shù)，2012.3（1）：41-43

[2] 李炳炳.基于MEMS慣性傳感器的人體姿態(tài)檢測(cè)系統(tǒng)的研究[D].南京理工大學(xué)，2017.

[3] 楊陽(yáng).基于ADIS16488 MEMS慣性組合姿態(tài)測(cè)量設(shè)計(jì)與研究[D].中北大學(xué)，2015.

[4] 廖文平.基于MEMS傳感器的姿態(tài)解算的FPGA實(shí)現(xiàn)[D].長(zhǎng)沙理工大學(xué)，2015.

[5] 李越.四旋翼無(wú)人機(jī)姿態(tài)解算及控制研究[D].昆明理工大學(xué)，2017.

[6] 張銳.基于FPGA的小型捷聯(lián)式慣性導(dǎo)航系統(tǒng)研究[D].中國(guó)民航大學(xué)，2014.

[7] 楊兆靜.基于FPGA的載體姿態(tài)角解算算法的研究與設(shè)計(jì)[D].燕山大學(xué)，2013.

[8] 楊龍，李范鳴.DSP EMIF與FPGA雙口RAM高速通信實(shí)現(xiàn)[J].現(xiàn)代電子技術(shù)，2016.37（13）：10-13

[9] 文雨辰.基于FPGA+DSP架構(gòu)的GPS接收機(jī)設(shè)計(jì)和實(shí)現(xiàn)[D].電子科技大學(xué)，2010.

[10] 張榮輝，賈宏光.基于四元數(shù)法的捷聯(lián)式慣性導(dǎo)航系統(tǒng)的姿態(tài)解算[J].光學(xué)精密工程，2008.16（10）：1963-1970