一種姿態(tài)測量系統(tǒng)的FPGA和DSP設計與實現(xiàn)

1、    一種姿態(tài)測量系統(tǒng)的fpga和dsp設計與實現(xiàn)    呂金銳摘  要： 為了實現(xiàn)對某彈載系統(tǒng)的姿態(tài)測量，提出一種基于fpga+dsp的姿態(tài)測量系統(tǒng)的設計。系統(tǒng)采用fpga作為邏輯控制核心，完成對mems傳感器輸出的陀螺加表數(shù)據(jù)的采集，并通過emif接口完成與dsp的數(shù)據(jù)通信。dsp完成對姿態(tài)信息的解算，并由fpga通過rs422接口將更新后的姿態(tài)信息上傳至上位機。系統(tǒng)選用地理坐標系作為導航坐標系，利用四元數(shù)法及kalman信息融合算法對采集到的姿態(tài)信息進行解算。測試結果表明，該系統(tǒng)輸出的姿態(tài)信息精度較好，總體角誤差在0.5°以內(nèi)

2、。關鍵詞： mems傳感器; 姿態(tài)測量; fpga+dsp; 四元數(shù); kalman信息融合; 姿態(tài)解算：tn702          ：a            ：1006-8228（2021）05-29-04design and implementation of fpga and dsp for attitude measurement systemlv jinrui（department of information engineering， taiyuan city voca

3、tional 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

4、 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 t

5、he 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

6、; quaternion; kalman information fusion; attitude resolution0 引言姿態(tài)解算作為姿態(tài)測量系統(tǒng)中的核心部分，正朝著高精度和高速度的方向發(fā)展1。為實現(xiàn)更加準確的對彈載系統(tǒng)的姿態(tài)測量，不僅對數(shù)據(jù)的采集及處理算法提出更高的要求，同時對于處理器的選擇也越來越重視。在測量系統(tǒng)中，確定姿態(tài)主要運用的是陀螺，但由于陀螺儀長期工作存在精度測量漂移從而無法保證系統(tǒng)精度，對姿態(tài)的確定會產(chǎn)生很大的偏差2。針對以上問題，系統(tǒng)采用adis16488傳感器為核心，結合fpga的并行處理方式以及dsp強大的數(shù)據(jù)處理能力，構建了一個姿態(tài)測量系統(tǒng)。1 系統(tǒng)總體設計系統(tǒng)采

7、用美國ad公司產(chǎn)品adis16488 mems慣性傳感器作為核心，其內(nèi)部集成了三軸微陀螺儀，三軸微加速度計，三軸微磁強計以及壓力傳感器3。當mems慣性傳感器處于工作模式時，能夠同時采集三個軸向上的加速度、角速度及磁強度。系統(tǒng)總體結構圖如圖1所示。系統(tǒng)工作原理：fpga作為主控芯片，首先通過spi接口對傳感器進行初始化配置，然后根據(jù)上位機指令采集傳感器輸出的陀螺加表等數(shù)據(jù)至數(shù)據(jù)緩沖模塊，通過emif接口將數(shù)據(jù)傳輸至dsp進行數(shù)據(jù)處理，最后將解算好的姿態(tài)信息通過rs422發(fā)送模塊上傳至上位機進行顯示。2 硬件電路設計2.1 傳感器數(shù)據(jù)采集模塊慣性測量單元正常工作時，需加載3.3v電壓至10、11

8、、12引腳4。上電后，系統(tǒng)首先通過fpga完成對傳感器的初始化配置，然后spi接口來讀取更新后的傳感器數(shù)據(jù)。傳感器內(nèi)部寄存器采用分頁結構，每一頁都對應其相應的功能5。fpga對mems傳感器進行讀寫時需要連續(xù)兩個8bit地址進行操作，其中低地址在前，高地址在后。高8bit地址表示傳感器內(nèi)部相應寄存器的地址，低8bit地址表示寫入該寄存器的數(shù)據(jù)內(nèi)容。如圖2所示，對傳感器進行讀操作時需要一個16bit的寄存器地址，且相應寄存器地址上的數(shù)據(jù)輸出將在下一個寄存器地址輸入時才會輸出。2.2 fpga與dsp通信接口設計fpga與dsp通過emif接口來實現(xiàn)通信，如圖3所示，ema_data15：0為數(shù)據(jù)

9、總線，ema_we為fifo讀使能信號，gpio4_15為啟動數(shù)據(jù)發(fā)送信號，ema_cs為emif使能信號。為匹配數(shù)據(jù)傳輸速率，采用雙端口ram來對讀取的dsp數(shù)據(jù)進行緩存，在雙端口ram模塊和dsp模塊之間串聯(lián)33電阻來減少信號失真6。雙口ram發(fā)送模塊不僅實現(xiàn)了數(shù)據(jù)的讀寫獨立，也滿足了數(shù)據(jù)在發(fā)送時間上的要求。3 姿態(tài)解算算法3.1 初始姿態(tài)對準在求解載體姿態(tài)前對系統(tǒng)進行初始對準，對于提高姿態(tài)信息精度有很大的影響。首先利用載體在地理坐標系的投影00gt和在載體坐標系的投影fxbfybfzbt，得出載體橫滾角和俯仰角，然后利用求得的初始橫滾角和俯仰角計算初始航向角7。算法推導過程如下：fxbf

10、ybfzb=cnb00g=coscossin-sincos0cossinsin-sinsincoscos 00g求解得初始橫滾角和俯仰角：=sin-1fyb/g=tan-1-fxb/fzb結合磁強計由載體坐標系映射到磁場x、y方向上的值，求解得：hx=mxcos+mysinsin+mzcossinhy=mycos-mzsin然后求得初始航向角：=arctanhy/hx3.2 四元數(shù)法根據(jù)歐拉定理可知，載體坐標系b相對于地理坐標系n的方位相當于b系繞定軸轉動3次方位角，四元數(shù)包含了轉動的信息8。un為旋轉軸和旋轉方向，為轉過的角度。q=cos2+unsin2利用哥式等式推導得出四元數(shù)的微分方程為

11、：q=12q?bnb矩陣形式為：t0t1t2t3=0-x-y-zx0z-yyz-zy0x-x0然后利用四階龍格庫塔法求得四元數(shù)更新表達式：k1=t2bt?qtk2=t2bt+t2?qt+k12k3=t2bt+t2?qt+k22k4=t2bt+t2?qt+k3qt+t=q（t）+t6k1+2k2+2k3+k4四元數(shù)可確定出載體坐標系到地理坐標系的坐標轉換公式：cnb=q20+q21-q22-q232q1q2-q0q32q1q3+q0q22q1q2+q0q3q20-q21+q22-q232q2q3-q0q12q1q3-q0q22q2q3+q0q1q20-q21-q22+q233.3 卡爾曼濾波卡爾

12、曼濾波的主要作用在于最大限度的減小噪聲目標信號的影響，利用不同時刻的系統(tǒng)狀態(tài)信息，對信息預測和估計，最終得到一個較為精確的數(shù)值。它相當于一個動態(tài)遞歸的過程：xk=akxk-1+bkuk-1+wk-1zk=hkxk+vk其中，xk為k時刻的系統(tǒng)狀態(tài)，zk表示k時刻的觀測變量。uk為k時刻對系統(tǒng)的控制量。vk，wk分別表示狀態(tài)方程的觀測噪聲和過程噪聲。hk是觀測噪聲誤差狀態(tài)增益矩陣，ak和bk是系統(tǒng)參數(shù)9?？柭鼮V波的時間更新公式為：x-k=axk-1+buk-1p-k=apk-1at+q由k-1時刻的最優(yōu)值xk-1去預測k時刻系統(tǒng)的狀態(tài)值x-k。然后由上一次的誤差協(xié)方差pk-1和過程噪聲q預測新

13、的誤差10。卡爾曼濾波的狀態(tài)更新公式為：kk=p-khthp-kht+r-1xk=x-k+kkzk-hx-kpk=i-kkhp-k計算卡爾曼增益kk，并進行校正更新，得到的xk即為k時刻的最優(yōu)估計值。為下一步估計k+1時刻的最優(yōu)估計值的迭代進行更新操作，即更新pk。3.4 小波閾值降噪由于adis16488內(nèi)部的mems陀螺儀的抗干擾能力較弱，產(chǎn)生的噪聲相對于其他兩個傳感器而言更大。因此系統(tǒng)結合陀螺儀輸出信號不具備線性的特點，選用小波閾值降噪算法來對進行降噪處理。小波閥值消噪原理如圖4所示。系統(tǒng)進行了對靜置的傳感器進行了降噪實驗，系統(tǒng)上電后對陀螺儀y軸數(shù)據(jù)進行采集，采樣頻率為200hz，采樣時

14、間為100s，得到陀螺儀的實時數(shù)據(jù)如圖5所示。由圖5可以看出，當陀螺儀靜置時，其y軸輸出的角速率誤差值較大，范圍在-0.04°，-0.04°。對該陀螺儀采用小波閾值降噪處理后，得到如圖6所示的誤差波形圖。由圖6可以看出，陀螺儀靜置時y軸輸出的波形有了明顯的改善，角速率誤差約束到了-0.02°，-0.02°。4 系統(tǒng)測試4.1 傳感器數(shù)據(jù)采集模塊仿真?zhèn)鞲衅鬏敵龅臄?shù)據(jù)對于整個測量系統(tǒng)來說有著十分重要的作用，它關乎著數(shù)據(jù)的正確性和完整性。fpga負責接收傳感器輸出的數(shù)據(jù)并將其緩存至內(nèi)部ram中，同時也負責讀出dsp解算后緩存至ram中的數(shù)據(jù)，并將其上傳至上位機

15、。為了驗證fpga與dsp之間通信的正確性，將在fpga ram中寫入數(shù)據(jù)0-256，dsp通過emif接口讀取ram中的數(shù)據(jù)，在開發(fā)環(huán)境ise下顯示dsp讀取的數(shù)據(jù)以及其他信號的一些變化，它讀取的數(shù)據(jù)地址空間是從0xc0000000開始。如圖7所示，dsp與fpga之間通信正確。4.2 誤差分析為了對系統(tǒng)性能進行測試與驗證，采用ahrs（商業(yè)級姿態(tài)參考系統(tǒng)）作為參考。驗證過程中，將該參考系統(tǒng)與電路板固定，并置于三軸無磁轉臺中。未上電之前調(diào)整軸向，使得系統(tǒng)坐標系和轉臺的軸向一致。以z軸為定軸進行5°/s的轉動，采集時間為360s，數(shù)據(jù)采集完成后通過串口上傳至上位機。上位機對接收到的數(shù)

16、據(jù)進行分析計算，采用matlab對接收到的數(shù)據(jù)進行曲線顯示，如圖8、圖9、圖10所示。由于系統(tǒng)在進行測試時選用z軸為定軸進行轉動，從得到的誤差曲線圖中可以看出，橫滾角和俯仰角的誤差較小，均在0.4°以內(nèi)，姿態(tài)解算精度較好。即驗證了系統(tǒng)在轉動過程中，x軸和y軸方向的姿態(tài)沒有發(fā)散。將系統(tǒng)姿態(tài)解算的結果與ahrs測量的數(shù)據(jù)進行比較，得出其誤差的標準差，如表1所示。驗證系統(tǒng)解算的結果較好，誤差值均在0.5°以內(nèi)。4.3 上位機數(shù)據(jù)顯示系統(tǒng)通過rs422接口將解算好的姿態(tài)信息發(fā)送至上位機進行顯示，如圖11所示。圖11為傳感器繞z軸轉動時，三個軸向角度的變化曲線圖。如圖12所示，可以直

17、觀的看到傳感器的全姿態(tài)信息。5 結束語系統(tǒng)采用fpga+dsp雙處理器的方案來實現(xiàn)對某彈載控制器的姿態(tài)解算，通過系統(tǒng)測試，驗證該姿態(tài)測量系統(tǒng)的輸出精度較好，均在0.5°以內(nèi)，能夠滿足技術要求。這為后續(xù)測量系統(tǒng)的進一步優(yōu)化提供了可靠的實驗數(shù)據(jù)。參考文獻（references）：1 沈懷營，吳伯衣.基于dsp/fpga的高速旋轉體姿態(tài)角及轉速的解算系統(tǒng)設計j.山西電子技術，2012.3（1）：41-432 李炳炳.基于mems慣性傳感器的人體姿態(tài)檢測系統(tǒng)的研究d.南京理工大學，2017.3 楊陽.基于adis16488 mems慣性組合姿態(tài)測量設計與研究d.中北大學，2015.4 廖文平.基于mems傳感器的姿態(tài)解算的fpga實現(xiàn)d.長沙理工大學，2015.5 李越.四旋翼無人機姿態(tài)解算及控制研究d.昆明理工大學，2017.6 張銳.基于fpga的小型捷聯(lián)式慣性導航系統(tǒng)研究d.中國民航大學，2014.7 楊兆靜.基于fpga的載體姿態(tài)角解算算法的研究與設計d.燕山大學，2013.8 楊龍，李范鳴.dsp emif與fpga雙口ram高速通信實現(xiàn)j.現(xiàn)代電