沈繼睿，鄭永安，史忠科

（西北工業(yè)大學 自動化學院，陜西 西安 710129）

基于FPGA的高速導航解算硬件實現(xiàn)

沈繼睿，鄭永安，史忠科

（西北工業(yè)大學 自動化學院，陜西 西安 710129）

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

并行計算；FPGA；姿態(tài)解算；導航解算

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

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

1 系統(tǒng)結構

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

圖1 系統(tǒng)結構圖Fig.1 Structure diagram of system

2 導航解算模塊的FPGA設計

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

慣性測量單元發(fā)出的數(shù)據(jù)是有符號位整型的十六位數(shù)據(jù)包，分頻一個十六倍于波特率的采樣時鐘對串口數(shù)據(jù)進行采樣。由于50 MHz的系統(tǒng)時鐘不能分頻得到正好十六倍于57 600 Hz，需要實時進行相位差的同步。定義一個case結構的語句，第七個采樣時鐘周期對串口數(shù)據(jù)進行采樣，同時定義一個寄存器，檢測到串口數(shù)據(jù)的上升沿或者下降沿時產(chǎn)生時鐘同步標志位，這樣就解決了數(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ù)預處理模塊對需要其進行單精度浮點型的格式轉換，然后進行單位標定。其中16位有符號整型數(shù)據(jù)向單精度浮點型數(shù)據(jù)格式轉換的步驟如下：

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

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

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

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

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

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

.

v

■ ■■ ■■

■ ■■ ■■

■ ■■ ■■ψ = ·

（r■.

.

.

.θ ψ

φ= =

p

（

■ ■■ ■■

.

φ ψ = =

p

■ ■■ ■■.·

..

..導航解算系統(tǒng)所占用的FPGA硬件資源如表1所示。

表1 硬件資源消耗Tab.1 Hardware resource consumption

表3 功能仿真與MATLAB計算結果列表Tab.3 Result of functional simulation and MATLAB calculation

圖2 導航解算仿真時序圖Fig.2 Sequence diagram of navigation system

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

3 實驗結果與誤差分析

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

圖3 車輛導航試驗平臺Fig.3 Vehicle navigation test platform

圖4 陀螺儀傳感器數(shù)據(jù)Fig.4 Data of gyros

圖5 加速度計傳感器數(shù)據(jù)Fig.5 Data of accelerometers

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

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

圖6 運動車輛導航解算結果Fig.6 Result of Vehicle navigation

4 結 論

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

[1] Wang B，Deng Z，Liu C，et al.Estimation of Information Sharing Error by Dynamic Deformation Between Inertial Navigation Systems[J].IEEE Transactions on Industrial Electroics，2014，61（4）：2015-2023.

[2] Hasan A M，Samsudin K，Ramli A R，et al.A review of navigation systems（integration and algorithms）[J].Australian journal of basic and applied sciences，2009，3（2）：943-959.

[3] Agarwal V，Arya H，Bhaktavatsala S.Design and development of a real-time DSP and FPGA-based integrated GPS-INS system for compact and low power applications[J].IEEE Transactions on，Aerospace and Electronic Systems，2009，45（2）：443-454.

[4] Monmasson E，Cirstea M N.FPGA design methodology for industrial control systems—a review[J].IEEE Transactions on Industrial Electronics，2007，54（4）：1824-1842.

[5] Li T H S，Yeh Y C，Wu J D，et al.Multifunctional intelligent autonomous parking controllers for carlike mobile robots[J].IEEE Transactions on Industrial Electronics，2010，57（5）：1687-1700.

[6] Allaire F C J，Tarbouchi M，Labonté G，et al.FPGA implementation of genetic algorithm for UAV real-time path planning[C]//Unmanned Aircraft Systems.Springer Netherlands，2009：495-510.

[7] Bhatt D，Aggarwal P，Bhattacharya P，et al.An enhanced mems error modeling approach based on nu-support vector regression[J].Sensors，2012，12（7）：9448-9466.

[8] Leclère J，Botteron C，F(xiàn)arine P A.Acquisition of modern GNSS signals using a modified parallel code-phase search architecture[J].Signal Processing，2014（95）：177-191.

The hardware implementation of a navigation solution system based on FPGA

SHEN Ji-rui，ZHENG Yong-an，SHI Zhong-ke
（College of Automation，Northwestern Polytechnical University，Xi'an 710129，China）

In view of the defect of bloat hardware，poor operational reliability，and low process speed of existing embedded navigation system，this paper proposes a navigation solution system based on a single FPGA chip.The vehicle motion equations and the navigation equations are parallel decomposed by the system and parallel computation was carried out on the independent variables.Six times of floating-point calculation are made in a cycle which makes the calculating speed increased while the hardware resources consumption does not occupied too much.The simulations and experiments indicate that the system works well and it has important value in the field of navigation of small UAV.Key words：parallel computation；FPGA；attitude computation；navigation solution

TN06

A

1674-6236（2014）11-0103-04

2014-04-11 稿件編號：201404120

國家自然科學基金重點項目（61134004）

沈繼睿（1987—），男，湖北咸寧人，碩士研究生。研究方向：無人機姿態(tài)估計與導航系統(tǒng)硬件設計。