基于FPGA的高速導航解算硬件實(shí)現

摘要：針對現有小型無(wú)人機導航系統的解算速度慢、多處理器核心臃腫可靠性差的缺點(diǎn)，實(shí)現了一種僅使用單一FPGA作為數據處理核心的小型高速導航解算系統。該系統對飛機運動(dòng)方程組和導航方程組進(jìn)行并行化分解，對相互獨立的中間變量進(jìn)行并行計算，使得單個(gè)運算周期能夠同時(shí)進(jìn)行6次浮點(diǎn)運算，在不盲目增加硬件消耗的條件下有效提高了解算速度。仿真和實(shí)驗結果表明系統能夠高效地進(jìn)行導航信息解算，在小型無(wú)人機的導航控制領(lǐng)域有重要的工程應用價(jià)值。

導航解算是小型無(wú)人機導航控制的基礎，小型無(wú)人機機動(dòng)性強，為了完成自主導航任務(wù)，必須快速獲得姿態(tài)和位置信息，如果導航信息無(wú)法得到高速解算，導航控制系統會(huì )因為不能及時(shí)得到載體正確位置信息而發(fā)出錯誤指令，會(huì )對運載體以及人員造成極大危險。平臺式慣導系統雖然精度高、實(shí)時(shí)性好，但是龐大的體積和昂貴的造價(jià)不適用于小型無(wú)人機的發(fā)展，GPS等衛星導航設備雖然價(jià)格低廉、體積小巧，但是其衛星信號會(huì )受到建筑物和天氣等因素的干擾。目前國內外應用于無(wú)人機上的低成本小型化的導航解算系統研究方面大多使用基于DSP、ARM為主處理器的嵌入式系統，或者另外添加一顆協(xié)處理器幫助進(jìn)行傳感器數據的采集，這樣的系統要么解算速度慢，通信效率低，要么系統臃腫，可靠性差?，F場(chǎng)可編程門(mén)陣列(FPGA)直接使用硬件描述語(yǔ)言進(jìn)行編程，與ARM和DSP器件相比，可以獲得更有效率的數據處理速度，可以兼容各種格式和長(cháng)度的數據，同時(shí)集成了常用IP核，使之可以靈活的用來(lái)進(jìn)行系統設計。在單片FPGA芯片上實(shí)現導航信息的高速解算，將會(huì )有廣闊的發(fā)展空間。

針對現有小型無(wú)人機導航解算系統解算速度慢、多處理器臃腫可靠性差的缺點(diǎn)，文中設計了一種在單片FPGA芯片上實(shí)現數據傳輸、姿態(tài)解算和位置解算等功能的導航解算系統，節省了小型無(wú)人機寶貴的空間和成本，提出了一種導航信息的FPGA并行解算方法，充分發(fā)揮FPGA的并行數據處理能力提高解算速度，一次導航解算過(guò)程只需20微秒。

1 系統結構

系統結構如圖1所示，由單片FPGA芯片作為數據處理的核心，型號為EP2C35F484C6N，其與一個(gè)型號為NAV440的慣性測量單元(IMU)進(jìn)行串口通信接收所需的三軸加速度、三軸角加速度等信息，FPGA依次由數據接收模塊、數據預處理模塊、姿態(tài)解算模塊、位置解算模塊對數據進(jìn)行處理，最后將數據封包發(fā)出，上位機保存數據。

2 導航解算模塊的FPGA設計

2.1 數據的接收和預處理

慣性測量單元發(fā)出的數據是有符號位整型的十六位數據包，分頻一個(gè)十六倍于波特率的采樣時(shí)鐘對串口數據進(jìn)行采樣。由于50 MHz的系統時(shí)鐘不能分頻得到正好十六倍于57 600 Hz，需要實(shí)時(shí)進(jìn)行相位差的同步。定義一個(gè)case結構的語(yǔ)句，第七個(gè)采樣時(shí)鐘周期對串口數據進(jìn)行采樣，同時(shí)定義一個(gè)寄存器，檢測到串口數據的上升沿或者下降沿時(shí)產(chǎn)生時(shí)鐘同步標志位，這樣就解決了數據穩態(tài)和時(shí)鐘相位同步的問(wèn)題。接收到的串行數據從低到高位按位依次存放到8位緩沖寄存器的第0到第7位，這樣就完成了串行數據接收。FPGA的據接收模塊對兩個(gè)8位數據拼接后得到的數據是16位有符號整型數據，數據預處理模塊對需要其進(jìn)行單精度浮點(diǎn)型的格式轉換，然后進(jìn)行單位標定。其中16位有符號整型數據向單精度浮點(diǎn)型數據格式轉換的步驟如下：

步驟1：判斷整型數據的最高位即符號位，記錄符號位并轉換成補碼形式;

步驟2：接著(zhù)將上述補碼形式左移位，直到第14位為1，并記錄下左移位數，階碼即等于14減去左移的位數;

步驟3：將上述移位后的16位數據再左移2位即浮點(diǎn)數的尾數的整數部分，直接賦值給浮點(diǎn)數的第7到22位，由于整型數據小數點(diǎn)右邊全是零，所以浮點(diǎn)數的第0到6位也是0，浮點(diǎn)數的第23到30位即階碼加上127的偏移量，第31位為符號位與整型數據的最高位相同。

2.2 歐拉法姿態(tài)并行解算模塊的FPGA設計

FPGA芯片擁有良好的并行運算能力，不同程序塊可以相對獨立的進(jìn)行運算，只要對算式進(jìn)行合理的并行化分解，就能夠提高運算速度。并行計算的程序塊越多，數據處理的速度越快，消耗的硬件資源也越多。飛機運動(dòng)方程如式(1)所示。

相互不影響的中間變量可以同時(shí)計算，依此對式(Ⅱ)進(jìn)行并行化分解。分析其計算過(guò)程，一次加減法或者乘除法通常是兩個(gè)三角函數值之間的運算，乘法運算較多，除法運算只有一次，而每?jì)纱纬顺ㄟ\算才進(jìn)行一次加減法運算?；谏鲜龇治龊陀布Y源消耗的考慮，通過(guò)3個(gè)乘法運算模塊、1個(gè)除法運算模塊、2個(gè)加減法運算模塊和2個(gè)正余弦函數運算模塊對姿態(tài)角進(jìn)行解算。FPGA每一個(gè)計算周期最多同時(shí)調用6個(gè)運算模塊對數據進(jìn)行并行處理，不同計算周期所計算的算子安排如下所示：

2.3 位置信息并行解算的FPGA設計

通過(guò)傳感器獲得的加速度以及上述模塊解算的姿態(tài)角可以解算飛機三軸速度，飛機速度解算方程如式(2)所示。

依據上述并行計算結構進(jìn)行硬件描述語(yǔ)言的編程和編譯，導航解算系統所占用的FPCA硬件資源如表1所示。

圖2為導航解算FPGA功能仿真時(shí)序圖，以此估算模塊計算所消耗的時(shí)間。一次姿態(tài)解算需要230個(gè)時(shí)鐘周期，一次導航解算需要980個(gè)時(shí)鐘周期，那么在50 MHz的系統時(shí)鐘下，姿態(tài)解算需時(shí)4.7微秒，導航解算需時(shí)20微秒。導航解算系統功能仿真結果與計算機計算結果進(jìn)行對比，仿真步長(cháng)為0.1秒，仿真輸入參數如表2所示，計算結果如表3所示，通過(guò)比對可以發(fā)現，FPGA的計算結果與MATLAB計算結果沒(méi)有偏差，說(shuō)明導航解算系統能夠正確地進(jìn)行導航信息的解算。

3 實(shí)驗結果與誤差分析

在一輛普通轎車(chē)上進(jìn)行導航實(shí)驗，系統的搭建如圖1和圖3所示。系統的核心是一塊承擔數據處理任務(wù)的FPGA，在芯片外接合適的IMU，IMU的功耗和體積基本決定了導航系統的功耗和體積。所以整個(gè)導航系統的結構是簡(jiǎn)單和小巧的而且節能的。

實(shí)驗進(jìn)行了340秒，如圖4和圖5所示，依次是三軸陀螺儀傳感器數據和三軸加速度計傳感器數據。位置曲線(xiàn)如圖6所示，實(shí)線(xiàn)是導航解算系統解算的位置信息，虛線(xiàn)是GPS獲得的實(shí)際位置信息?？梢园l(fā)現導航解算系統良好地跟蹤了實(shí)際位置變化趨勢，但是隨著(zhù)時(shí)間的推移，導航解算系統解算出的位置信息與實(shí)際位置信息偏差越來(lái)越大。

導航解算系統的誤差引入主要因為基于MEMS的慣性傳感器的誤差較大，使用單一傳感器進(jìn)行姿態(tài)和位置解算會(huì )在姿態(tài)計算和速度計算環(huán)節兩次引入積累誤差。在實(shí)際使用中，載體使用的戰術(shù)級高精度IMU，在一定的使用時(shí)間內，導航系統不會(huì )產(chǎn)生很大的積累誤差。除此之外，發(fā)揮本系統動(dòng)態(tài)特性好、更新速率快的優(yōu)勢，借助最優(yōu)估計的方法，通過(guò)進(jìn)行多種傳感器的信息融合也可以收斂誤差。

4 結論

針對現有小型無(wú)人機導航解算系統解算速度慢、多處理器臃腫可靠性差的缺點(diǎn)，文中提出了一種并行化的導航解算方法，并搭建了一種僅使用單一FPGA芯片為數據處理核心的小型高速導航解算系統，功能仿真驗證了導航解算的高速性和準確性。車(chē)載實(shí)驗驗證了系統可以在實(shí)際中完成導航信息的解算工作。根據一次結算消耗時(shí)間可知系統理論擁有50 000 Hz的導航解算能力，在實(shí)際使用中，輔以足夠精度的高速I(mǎi)MU，系統將會(huì )發(fā)揮小型化、高速率和低功耗的優(yōu)勢，在相關(guān)的小型無(wú)人機導航系統設計領(lǐng)域有重要借鑒意義。