一种用于定位导航授时微终端的SoC系统的设计与实现

摘要：为了解决遮挡情况下的实时定位问题，美国提出了Micro-PNT方案，我国也提出了定位导航授时微终端（Micro Positioning Navigation and Timing Terminal，MPNTT）方案。定位导航授时微终端集成了卫星导航系统、微惯性测量单元、微型原子钟及处理器系统，可为终端用户提供精确可用、完好及时、连续安全的定位导航服务。介绍了一种用于定位导航授时微终端的SoC系统设计，其包括了基于SoC FPGA的硬件设计和基于GNSS/MIMU的组合导航滤波算法。SoC系统集成了Flash、SSRAM等存储芯片，通过RS422、RS232、CAN等通信接口接收GNSS、MIMU及外源传感器信息，并在ARM核中完成组合导航算法，以得到导航结果。SoC芯片单片实现了ARM与FPGA的功能，系统集成面积满足小型化需求，为后续移植为ASIC芯片提供了基础。对组合导航滤波算法进行嵌入式软件移植并测试，结果表明：SoC系统单次惯导解算时间为7ms，实测与仿真输出的导航位置差距在0.05m以内，俯仰角差和横滚角差在0.005°以内，航向角差在0.05°以内。本文设计的SoC系统高精度、集成化、可扩展，满足了微终端的要求。

0 引言

随着信息技术的发展，人们对实时位置信息获取的要求越来越高。目前，最常见的定位方式依赖于卫星定位导航系统（如GPS、北斗等）。然而，在城市、丛林等有遮挡、信号弱的地方，卫星无法给出定位信息。为了解决遮挡情况下的实时定位问题，不依赖于卫星的自主导航成为了近年来各国的研究热点。美国DARPA提出了微型定位导航授时（Micro-PNT）方案，其目标是研发尺寸小、质量轻、功耗低的惯性器件和时钟芯片，以作为GNSS混淆环境的增强、校验、延续的后援PNT解决方案；国内一些单位也提出了相关的定位导航授时微终端（Micro Positioning Navigation and Timing Terminal，MPNTT）方案。

MPNTT方案拟采用微系统集成技术，将高性能微惯性测量单元、卫星导航系统、时钟、辅助传感器接口、信息融合处理器等集成于一个小型化模块，经过多源信息的融合，为终端用户提供精确可用、完好及时、连续安全的定位导航服务。该方案的重点在于两部分：其一是高精度惯性传感器的研制，其二是微集成技术与多源信息融合算法。微半球谐振陀螺精度高、对称性好、可靠性高，是未来高精度传感器的代表，也是微终端的研究重点，其校准、补偿对微终端处理器提出了高处理能力的要求。微集成技术与多源信息融合算法需要将多种传感器集成在小尺寸内，并将各种信息融合滤波进行导航，这也对处理器系统提出了集成化、高精度、可扩展的要求。

目前，用于导航系统的处理器方案主要有DSP、DSP+FPGA、DSP+ARM等，这些方案在芯片级间存在数据传输速度的限制，无法支持大量传感器信息的接入与传输，且占用面积较大。为解决微终端方案对处理器系统提出的高精度、集成化、可扩展的要求，本文选择了片上系统（System on Chip，SoC）解决方案，该方案兼有FPGA的灵活性和ARM编程的易用性。芯片内部的ARM与FPGA之间使用了AXI总线通信，传输速度高且占用面积小。此种方案在验证后还可根据外设情况制作数字专用集成威廉希尔官方网站 （Application Specific Integrated Circuit，ASIC），以进一步降低功耗和威廉希尔官方网站 集成面积。

本文在第1部分介绍了定位导航微终端总体方案；在第2部分介绍了SoC系统设计，其中包含了硬件设计方案和算法流程；在第3部分介绍了系统的验证与分析，包含硬件计算速度测试和嵌入式软件与仿真对比测试。最后，根据结果给出了分析和展望。

1 定位导航微终端总体方案

如图1（a）所示，定位导航授时微终端包含了4个部分，即：卫星导航系统、微惯性测量单元微型原子钟和SoC信息处理器。其中，微惯性测量单元（Micro Inertial Measurement Unit，MIMU）由3个MEMS陀螺与3个MEMS加速度计组成，采用旋转调制方案，可通过旋转积分消除随机误差，提高了初始对准寻北精度，其具体集成如图1（b）所示。旋转调制MIMU与卫星导航系统通过RS422和RS232接口与处理器进行通信。当接收到惯性测量信息与GNSS信息时，处理器将采用多源信息融合算法进行导航解算，并将结果通过RS422对外接口发出。此系统还集成了CAN总线接口，处理器内部预置了多种传感器驱动，支持相同协议外源传感器即插即用，可融合磁强计、气压计、激光雷达、里程计等多种传感器信息，增强定位精度与可靠性。而微型原子钟授时精度高，产生的秒脉冲信号可长时间支持卫星导航系统的无信号状态运行，也为MIMU和卫星导航系统提供了统一时间标。定位导航授时微终端的整体集成如图1（c）所示。

图１ 定位导航微终端总体方案与部件集成

2 微终端处理器SoC系统设计

2.1 硬件设计

SoC系统的整体设计方案如图2所示，ARM处理器通过AXI总线与外接存储芯片控制器、通信接口相连。ARM处理器为SoC系统提供了优秀的处理能力，外接存储芯片为大规模信息融合算法提供了存储与运行空间，对外通信接口接收的传感器信息可用于数据融合。下面将介绍关键部分的详细设计。

2.1.1 外设地址与中断分配

处理器芯片采用Alera公司的Cyclone V SoC芯片，其最高主频为925MHz。该系列芯片含有2个Cortex-A9核，通过2条宽度为64bit的AXI总线及1条宽度为32bit的轻量级AXI总线与FPGA端的IP核相连。AXI总线将地址、读数据、写数据、握手信号在不同通道中进行单向传输，具有性能高、延迟低的特点，较AHB等总线提高了数据传输效率。

图２ SoC系统总体设计方案

图3为外设地址配置。本系统将大容量、快速的外设如RAM、SSRM、Flash等，挂载到正常的AXI总线，采用了100MHz时钟；将低速的外设如UART、TIMER（定时器）等，挂载到轻量级的AXI总线，采用了25MHz时钟。

图3 外设地址配置

系统的中断捕获模块（Interrupt Capture Module）根据表1的中断优先级将JTAG、串行Flash、UART、TIMER等外设的中断信号反馈给ARM进行处理，并保证程序存储芯片的中断优先级最高，JTAG调试器第二，定时器最低。

表1 外设中断优先级

2.1.2 存储芯片读写时序控制

外接存储芯片包含SSRAM、并行Flash和串行Flash。其中，SRAM、并行Flash由三态控制器（Tri-State Controller）进行控制，串行Flash由串行Flash控制器（Serial Flash Controller）进行控制。上述3个控制器均通过地址范围扩展器（Address Span Extender）挂载到64bit AXI总线上。地址范围扩展器创建了一个窗口桥，可以允许ARM通过总线访问更大的地址映射。SSRAM的大小为1M×32bit，用作系统运行内存；并行Flash的大小为1M×16bit，用于掉电时的数据存储；串行Flash的总容量为8MB，用于存储系统程序，其中FPGA程序占用了4.5MB左右，剩余为ARM软件程序所用。本系统设置为从FPGA启动，当系统上电时将运行FPGA端ROM中的preloader程序，将硬件在FPGA中进行部署，再将软件程序及数据从串行Flash中拷贝到SSRAM中运行，上述过程即为整个系统的启动。

串行Flash的控制接口为SPI接口，其读写控制方法简单。并行Flash与SSRAM的控制方式根据地址与片选、读写使能信号进行，此处以SSRAM为例说明存储芯片的读写控制。

图4为SSRAM的读写控制时序。CK为时钟信号，ADSC_________为地址选通信号，A0-An为并行地址，BW______为字节写入使能（低写入高读取），Ba____-Bd____为写入a-d的4个区域的字节写入使能，E1____为片选信号，G___为输出使能，DQa_______-DQd________为a-d的4个区域并行数据输入输出（Q为读出，D为写入）。控制信号带上划线表示低有效。图4的前半部分为读取时序，当读取完成时，先拉高E1____取消选择，再拉低E1____选通芯片进行下一步操作。图4的后半部分为写入时序，系统依靠BW______控制读写方向，依靠G___控制数据输入输出方向，依靠地址A0-An控制读写位置。

图4 SSRAM的读写时序控制

外接存储芯片为大规模组合导航算法提供了程序存储与运行空间，为实现更高精度的融合算法提供了平台。

2.1.3 通信接口工作流程

SoC系统的通信接口分为两部分，一部分为2个UART接口（RS232）和1个CAN总线接口。这些接口被直接挂载在HPS端（即ARM处理器），便于捕捉数据状态的变化。其中，UART负责与卫星导航系统进行通信，接收GNSS信号，返回定位导航结果；CAN总线接口用于接收其他外源传感器的信号，并在接收到数据时根据数据帧头判断传感器的类型，支持即插即用。另一部分为2个UART接口（RS422），接口挂载在FPGA端，通过AXI总线与ARM核相连，负责与IMU进行通信及输出导航结果。多种通信接口为实现传感器的信息融合提供了硬件支持，接口与软件配合可实现传感器的即插即用，完成导航解算。

通信接口数据的接收流程如图5所示。数据要经过有效性检验，若数据无丢包、错包现象，则进行数据编码。解析数据与变量一一对应，并可存储到相应的buffer，将接收状态设置为1，等待处理器调用。

图5 接口数据接收流程

2.2 算法简介

算法目前融合了MIMU信息与GNSS信息，主要计算过程有初始对准、基于GNSS/MIMU的组合导航Kalman滤波算法。

初始对准可分为粗对准与精对准。在载体静止时，粗对准利用载体三轴加速度计的输出等于重力加速度在载体坐标系的投影的原理，可求出水平方向的俯仰角与横滚角。完成粗对准后进行精对准，需要卫星导航系统提供载体运动信息。根据位置、速度等信息列出误差方程，可求解出航向角，即完成了导航初始化。

组合导航算法主要可分为状态方程、观测方程建立与滤波计算两部分。

（1）状态方程建立

在ECEF坐标系下，组合导航算法中的Kalman滤波误差状态向量主要包括姿态误差δψeeb、速度误差δveeb、位置误差δreeb、三轴加速度计零偏ba、三轴陀螺零偏bg，其具体表述如下

对其列状态方程如下

式中，FeINS为状态转移矩阵，G为噪声转移矩阵，W为状态噪声。

（2）观测方程建立

系统的观测量为IMU解算的位置和速度与GNSS获取的位置和速度的差值，据此建立观测方程

式中，Z为观测量矩阵，H为量测矩阵，V为量测噪声。

（3）滤波计算过程

主要采用标准Kalman滤波进行算法求解，其具体过程如下

式中，k=1，2，3，…，Φ为一步转移阵，P为预测协方差阵，Q为系统噪声协方差阵，K为Kalman增益，H为观测矩阵，R为测量噪声协方差阵。

导航算法的工作流程如图6所示，IMU信息GNSS信息分别由两个串口中断接收。当收到GNSS信息时，整个算法进行初始对准。初始化完成后，在接收到IMU信息时进行机械编排，得到惯性导航系统（Inertial Navigation System，INS）的推算结果。将此推算结果与GNSS信号进行时空基准统一，将相同时间数据转换到同一坐标系。随后对传感器数据做出有效性判断。若数据无效则去掉数据，若数据有效则执行最优融合滤波算法，修正INS机械编排结果，并输出导航结果。

图6 融合算法工作流程

3 结果验证与分析

图7为SoC系统集成实物图及其测试环境。将系统集成在一块53mm×59mm大小的PCB板上，各信号通过一块外接板引出。用上位机将IMU数据和GNSS数据按照时序发送给SoC处理器进行测试，测试结果如下。

图7 系统集成实物图及测试环境

3.1 算法运行时间测试

在算法函数入口及出口处设置计数器，并在周期为1ms的定时器中断函数中进行累加，对算法在SoC系统中的运行时间进行测试。图8展现了SoC系统在启动后一段时间内的测试结果。当未接收到运动的GNSS信息时，算法的运行时间小于1ms；当接收到运动的GNSS信息时，进行初始对准，算法单次运行时间约为12ms；在初始化完成后，惯性导航的单次解算时间约为7ms，可支持100Hz的惯性导航信息融合计算。

图8 算法运行时间测试

3.2 系统实测与仿真对比

将IMU数据与GNSS数据按照时序发送给SoC系统，进行40min导航解算，将返回的解算结果与仿真结果对应点做差，结果如图9~图11所示。

由图9可知，测试结果与仿真结果的经纬度差在5×10-7°以下，转换为距离约在0.05m以下，高程差小于0.01m。

图9 实测与仿真的经纬度差和高程差

由图10可知，测试结果与仿真结果在东向、北向的速度差在0.01m/s以下，在天向的速度差在0.001m/s以下。

图10 实测与仿真各方向速度差

由图11可知，测试结果与仿真结果的俯仰角差和横滚角差在0.005°以下，航向角差在0.05°以下。ARM核为单精度浮点型运算，仿真为双精度浮点型运算，实测与仿真计算结果偏差基本在10-7数量级，符合预期，可满足微终端的需求。SoC系统可添加双精度浮点型DSP协处理器，提高计算速度与精度。

图11 实测与仿真的姿态角差

4 结论

本文介绍了一种用于定位导航授时微终端的SoC系统的设计与实现，基于SoC FPGA的硬件设计，满足了微终端对于集成化、高精度、可扩展的要求。基于GNSS/MIMU的组合导航滤波算法具有可扩展性，可融合更多种类的传感器信息，提高系统导航精度。测试结果显示：SoC系统的单次惯导解算时间为7ms，速度较慢，可增加DSP协处理器，增强计算能力，提高算法运行速度。实测与仿真的位置差在0.05m以内，俯仰角差和横滚角差在0.005°以内，航向角差在0.05°以内，满足了微终端的要求。未来，可根据此SoC系统设计ASIC芯片，进一步降低功耗与集成面积。