基于FPGA的高速導(dǎo)航解算硬件實(shí)現(xiàn)

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

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

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

1 系統(tǒng)結(jié)構(gòu)

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

2 導(dǎo)航解算模塊的FPGA設(shè)計(jì)

2.1 數(shù)據(jù)的接收和預(yù)處理

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

步驟1：判斷整型數(shù)據(jù)的最高位即符號位，記錄符號位并轉(zhuǎn)換成補(bǔ)碼形式;

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

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

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

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

相互不影響的中間變量可以同時計(jì)算，依此對式(Ⅱ)進(jìn)行并行化分解。分析其計(jì)算過程，一次加減法或者乘除法通常是兩個三角函數(shù)值之間的運(yùn)算，乘法運(yùn)算較多，除法運(yùn)算只有一次，而每兩次乘除法運(yùn)算才進(jìn)行一次加減法運(yùn)算。基于上述分析和硬件資源消耗的考慮，通過3個乘法運(yùn)算模塊、1個除法運(yùn)算模塊、2個加減法運(yùn)算模塊和2個正余弦函數(shù)運(yùn)算模塊對姿態(tài)角進(jìn)行解算。FPGA每一個計(jì)算周期最多同時調(diào)用6個運(yùn)算模塊對數(shù)據(jù)進(jìn)行并行處理，不同計(jì)算周期所計(jì)算的算子安排如下所示：

2.3 位置信息并行解算的FPGA設(shè)計(jì)

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

依據(jù)上述并行計(jì)算結(jié)構(gòu)進(jìn)行硬件描述語言的編程和編譯，導(dǎo)航解算系統(tǒng)所占用的FPCA硬件資源如表1所示。

圖2為導(dǎo)航解算FPGA功能仿真時序圖，以此估算模塊計(jì)算所消耗的時間。一次姿態(tài)解算需要230個時鐘周期，一次導(dǎo)航解算需要980個時鐘周期，那么在50 MHz的系統(tǒng)時鐘下，姿態(tài)解算需時4.7微秒，導(dǎo)航解算需時20微秒。導(dǎo)航解算系統(tǒng)功能仿真結(jié)果與計(jì)算機(jī)計(jì)算結(jié)果進(jìn)行對比，仿真步長為0.1秒，仿真輸入?yún)?shù)如表2所示，計(jì)算結(jié)果如表3所示，通過比對可以發(fā)現(xiàn)，F(xiàn)PGA的計(jì)算結(jié)果與MATLAB計(jì)算結(jié)果沒有偏差，說明導(dǎo)航解算系統(tǒng)能夠正確地進(jìn)行導(dǎo)航信息的解算。

3 實(shí)驗(yàn)結(jié)果與誤差分析

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

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

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

4 結(jié)論

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