基于FPGA的高速导航解算硬件实现

沈继睿，郑永安，史忠科

（西北工业大学 自动化学院，陕西 西安 710129）

基于FPGA的高速导航解算硬件实现

沈继睿，郑永安，史忠科

（西北工业大学 自动化学院，陕西 西安 710129）

针对现有小型无人机导航系统的解算速度慢、多处理器核心臃肿可靠性差的缺点，实现了一种仅使用单一FPGA作为数据处理核心的小型高速导航解算系统。该系统对飞机运动方程组和导航方程组进行并行化分解，对相互独立的中间变量进行并行计算，使得单个运算周期能够同时进行6次浮点运算，在不盲目增加硬件消耗的条件下有效提高了解算速度。仿真和实验结果表明系统能够高效地进行导航信息解算，在小型无人机的导航控制领域有重要的工程应用价值。

并行计算；FPGA；姿态解算；导航解算

导航解算是小型无人机导航控制的基础，小型无人机机动性强，为了完成自主导航任务，必须快速获得姿态和位置信息，如果导航信息无法得到高速解算，导航控制系统会因为不能及时得到载体正确位置信息而发出错误指令，会对运载体以及人员造成极大危险[1]。平台式惯导系统虽然精度高、实时性好，但是庞大的体积和昂贵的造价不适用于小型无人机的发展，GPS等卫星导航设备虽然价格低廉、体积小巧，但是其卫星信号会受到建筑物和天气等因素的干扰[2]。目前国内外应用于无人机上的低成本小型化的导航解算系统研究方面大多使用基于DSP、ARM为主处理器的嵌入式系统，或者另外添加一颗协处理器帮助进行传感器数据的采集[3]，这样的系统要么解算速度慢，通信效率低，要么系统臃肿，可靠性差。现场可编程门阵列（FPGA）直接使用硬件描述语言进行编程，与ARM和DSP器件相比，可以获得更有效率的数据处理速度，可以兼容各种格式和长度的数据，同时集成了常用IP核，使之可以灵活的用来进行系统设计[4-6]。在单片FPGA芯片上实现导航信息的高速解算，将会有广阔的发展空间。

针对现有小型无人机导航解算系统解算速度慢、多处理器臃肿可靠性差的缺点，文中设计了一种在单片FPGA芯片上实现数据传输、姿态解算和位置解算等功能的导航解算系统，节省了小型无人机宝贵的空间和成本，提出了一种导航信息的FPGA并行解算方法，充分发挥FPGA的并行数据处理能力提高解算速度，一次导航解算过程只需20微秒。

1 系统结构

系统结构如图1所示，由单片FPGA芯片作为数据处理的核心，型号为EP2C35F484C6N，其与一个型号为NAV440的惯性测量单元（IMU）进行串口通信接收所需的三轴加速度、三轴角加速度等信息，FPGA依次由数据接收模块、数据预处理模块、姿态解算模块、位置解算模块对数据进行处理，最后将数据封包发出，上位机保存数据。

图1 系统结构图Fig.1 Structure diagram of system

2 导航解算模块的FPGA设计

2.1 数据的接收和预处理

惯性测量单元发出的数据是有符号位整型的十六位数据包，分频一个十六倍于波特率的采样时钟对串口数据进行采样。由于50 MHz的系统时钟不能分频得到正好十六倍于57 600 Hz，需要实时进行相位差的同步。定义一个case结构的语句，第七个采样时钟周期对串口数据进行采样，同时定义一个寄存器，检测到串口数据的上升沿或者下降沿时产生时钟同步标志位，这样就解决了数据稳态和时钟相位同步的问题。接收到的串行数据从低到高位按位依次存放到8位缓冲寄存器的第0到第7位，这样就完成了串行数据接收。FPGA的据接收模块对两个8位数据拼接后得到的数据是16位有符号整型数据，数据预处理模块对需要其进行单精度浮点型的格式转换，然后进行单位标定。其中16位有符号整型数据向单精度浮点型数据格式转换的步骤如下：

步骤1：判断整型数据的最高位即符号位，记录符号位并转换成补码形式；

步骤2：接着将上述补码形式左移位，直到第14位为1，并记录下左移位数，阶码即等于14减去左移的位数；

步骤3：将上述移位后的16位数据再左移2位即浮点数的尾数的整数部分，直接赋值给浮点数的第7到22位，由于整型数据小数点右边全是零，所以浮点数的第0到6位也是0，浮点数的第23到30位即阶码加上127的偏移量，第31位为符号位与整型数据的最高位相同。

2.2 欧拉法姿态并行解算模块的FPGA设计

FPGA芯片拥有良好的并行运算能力，不同程序块可以相对独立的进行运算，只要对算式进行合理的并行化分解，就能够提高运算速度。并行计算的程序块越多，数据处理的速度越快，消耗的硬件资源也越多。飞机运动方程如式（1）所示。

相互不影响的中间变量可以同时计算，依此对式（1）进行并行化分解。分析其计算过程，一次加减法或者乘除法通常是两个三角函数值之间的运算，乘法运算较多，除法运算只有一次，而每两次乘除法运算才进行一次加减法运算。基于上述分析和硬件资源消耗的考虑，通过3个乘法运算模块、1个除法运算模块、2个加减法运算模块和2个正余弦函数运算模块对姿态角进行解算。FPGA每一个计算周期最多同时调用6个运算模块对数据进行并行处理，不同计算周期所计算的算子安排如下所示：φ ）c

.

v

■ ■■ ■■

■ ■■ ■■

■ ■■ ■■ψ = ·

（r■.

.

.

.θ ψ

φ= =

p

（

■ ■■ ■■

.

φ ψ = =

p

■ ■■ ■■.·

..

..导航解算系统所占用的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功能仿真时序图，以此估算模块计算所消耗的时间。一次姿态解算需要230个时钟周期，一次导航解算需要980个时钟周期，那么在50 MHz的系统时钟下，姿态解算需时4.7微秒，导航解算需时20微秒。导航解算系统功能仿真结果与计算机计算结果进行对比，仿真步长为0.1秒，仿真输入参数如表2所示，计算结果如表3所示，通过比对可以发现，FPGA的计算结果与MATLAB计算结果没有偏差，说明导航解算系统能够正确地进行导航信息的解算。

3 实验结果与误差分析

在一辆普通轿车上进行导航实验，系统的搭建如图1和图3所示。系统的核心是一块承担数据处理任务的FPGA，在芯片外接合适的IMU，IMU的功耗和体积基本决定了导航系统的功耗和体积。所以整个导航系统的结构是简单和小巧的而且节能的。

图3 车辆导航试验平台Fig.3 Vehicle navigation test platform

图4 陀螺仪传感器数据Fig.4 Data of gyros

图5 加速度计传感器数据Fig.5 Data of accelerometers

实验进行了340秒，如图4和图5所示，依次是三轴陀螺仪传感器数据和三轴加速度计传感器数据。位置曲线如图6所示，实线是导航解算系统解算的位置信息，虚线是GPS获得的实际位置信息。可以发现导航解算系统良好地跟踪了实际位置变化趋势，但是随着时间的推移，导航解算系统解算出的位置信息与实际位置信息偏差越来越大。

导航解算系统的误差引入主要因为基于MEMS的惯性传感器的误差较大，使用单一传感器进行姿态和位置解算会在姿态计算和速度计算环节两次引入积累误差[7-8]。在实际使用中，载体使用的战术级高精度IMU，在一定的使用时间内，导航系统不会产生很大的积累误差。除此之外，发挥本系统动态特性好、更新速率快的优势，借助最优估计的方法，通过进行多种传感器的信息融合也可以收敛误差。

图6 运动车辆导航解算结果Fig.6 Result of Vehicle navigation

4 结 论

针对现有小型无人机导航解算系统解算速度慢、多处理器臃肿可靠性差的缺点，文中提出了一种并行化的导航解算方法，并搭建了一种仅使用单一FPGA芯片为数据处理核心的小型高速导航解算系统，功能仿真验证了导航解算的高速性和准确性。车载实验验证了系统可以在实际中完成导航信息的解算工作。根据一次结算消耗时间可知系统理论拥有50 000 Hz的导航解算能力，在实际使用中，辅以足够精度的高速IMU，系统将会发挥小型化、高速率和低功耗的优势，在相关的小型无人机导航系统设计领域有重要借鉴意义。

[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，Farine 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—），男，湖北咸宁人，硕士研究生。研究方向：无人机姿态估计与导航系统硬件设计。