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

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

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

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

1 系统结构

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

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)所示。

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

2.3 位置信息并行解算的FPGA设计

通过传感器获得的加速度以及上述模块解算的姿态角可以解算飞机三轴速度，飞机速度解算方程如式(2)所示。

依据上述并行计算结构进行硬件描述语言的编程和编译，导航解算系统所占用的FPCA硬件资源如表1所示。

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

3 实验结果与误差分析

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

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

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

4 结论

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