专利名称：一种高动态捷联惯性导航并行计算装置的制作方法
技术领域：
本发明涉及捷联惯性导航领域，具体地说是一种适用于高动态环境下的基于单 FPGA的光纤陀螺捷联惯性导航并行计算装置。
背景技术：
近年来以FPGA为代表的可编程逻辑器件技术取得了快速发展，Xilinx公司最新 推出的FPGA器件不仅集成有丰富的可配置逻辑块资源，还包含大量的面向计算密集应用 的DSP48(E)单元，其中DSP48(E)核可用于实现高效的浮点数运算，而丰富的逻辑块资源可 用于实现大规模的并行运算。由此可见，在硬件上，FPGA在并行计算领域具有很大优势。在高动态环境下，捷联惯性导航系统(Strapdown Inertial NavigationSystem, SINS)需采用快速且高精度的导航解算算法以保证系统精度，而高精度的导航解算算法复 杂且包含大量的矩阵及向量间的乘法运算。目前，在SINS中导航解算普遍采用DSP作为 主处理芯片，在DSP芯片中导航解算是按照速度更新、位置更新、姿态更新的流程顺序进行 的，并且运算指令是串行执行的。DSP的这种串行计算特性使其较难提高导航解算的计算速 率，因此在高动态环境下难以保证系统的实时性。可编程逻辑器件技术的引入则为克服SINS导航解算串行计算的性能限制、满足 SINS高性能需求提供了很好的解决方案。应用FPGA的并行计算特性，将导航解算过程进行 并行化处理，并使导航解算算法按多个流程同时进行，可大大加快SINS导航解算的计算速 率，对SINS导航精度的提高具有重大价值。
发明内容
为了解决高动态环境下SINS导航解算的实时性问题，并进一步提高其导航精度， 本发明提供一种用于捷联惯性导航的并行计算装置，该并行计算装置基于单个FPGA设计 而成，大大加快了 SINS导航解算算法的计算速率，提高了 SINS的导航精度。本发明解决其技术问题所采用的技术方案是一种高动态捷联惯性导航(High dynamic SINS，简称HD-SINS)并行计算装置，由 FPGA芯片、电源电路、配置电路、信号采集电路和信号输出电路组成，所述的FPGA芯片集成 了数据采集模块、初始对准模块、并行导航解算模块、通信模块；所述的配置电路当FPGA芯片上电后，自动将片外非易失性存储器中的配置比特 流读入FPGA的静态内存SRAM中，实现内部结构映射；所述的信号采集电路采集来自光纤陀螺、石英挠性加速度计及GPS接收机的输出 信号并输入到FPGA芯片的数据采集模块；所述的数据采集模块将对信号预处理并发送至初始对准模块与并行导航解算模 块，初始对准模块将计算得到的导航参数初始值发送至并行导航解算模块并在并行导航解 算模块内对导航参数进行更新计算，解算后将导航信息送至通信模块，最后由通信模块经 信号输出电路将信号发送至其他设备。
3
本发明采用Xilinx公司型号为)(C5VSX95T的Virtex5系列FPGA，其Slice容量为 14720个，并带有640个DSP48E内核。利用其丰富的Slice及DSP内核资源在)(C5VSX95T 内部集成了数据采集模块、初始对准模块、并行导航解算模块、通信模块。其中，数据采集模 块包含陀螺仪数据接收、AD控制、GPS数据接收和数据缓冲4个子模块。前3个子模块实现 并行采样，其中，陀螺数据接收模块负责读取光纤陀螺输出的串行数字信号，并将信号发送 至数据缓冲模块，获取采样周期内的角增量信息；AD控制模块负责石英挠性加速度计输出 电流信号的模数转换及结果读取，并将信号发送至数据缓冲模块，获取采样周期内的速度 增量信息；GPS接收模块负责接收GPS接收机RS232接口输出的串行数据，并将信号发送至 数据缓冲模块，获得SINS的位置信息。然后数据缓冲模块将信息发送至初始对准模块及并 行导航解算模块，初始对准模块利用此信息对SINS进行粗对准和精对准，将所得的导航参 数初始值发送至并行导航解算模块并启动模块运行。所述的FPGA芯片内的并行导航解算模块基于游动方位坐标系统的单一更新速率 的高精度SINS导航解算算法设计。普通场合应用的多速率算法一般在多个传感器采样周 期后对系统姿态和速度更新一次，计算公式简化且解算流程按照速度更新、位置更新、姿态 更新的顺序串行执行，该单一更新速率算法对多速率SINS导航解算算法作出改进，使用精 确形式的计算式并提高计算速率，每一采样周期内对SINS的姿态、速度和位置更新计算一 次，并且该并行导航解算模块对单一更新速率的SINS导航解算算法在算法级和功能级进 行了并行化处理。所述的并行导航解算模块包含高频运动效应补偿模块、姿态计算模块、速度计算 模块、位置计算模块和地球参数计算模块共5个子模块。高频运动效应补偿模块负责接收 当前采样周期内的角增量信号和速度增量信号，在初始对准模块的启动信号发出后，开始 对角增量和速度增量信号进行快速迭代运算，其中包括圆锥效应补偿计算、速度旋转效应 补偿计算、划桨效应补偿计算、位置旋转效应补偿计算和涡卷效应补偿计算，各部分计算并 行执行。当计算结束时通过准备好(ready)信号同时启动姿态计算、速度计算、位置计算 和地球参数计算4个模块，使其同时并行运行，实现了姿态、速度和位置信息的并行更新计 算。此4个模块间亦相互引用数据，其引用对方参数值均为对方前一采样周期结束时的计 算值，而当前计算得到的是当前采样周期内的导航参数。在任一采样周期内，导航解算结束 时并行导航解算模块将计算所得姿态、速度和位置数据发送至通信模块。所述的通信模块由串口通信模块、155 通信模块、SPI通信模块构成。其中，串口 通信模块可采用RS232、RS422或RS485串行通信IP核，经电平转换电路将信号发送至其他 串口设备；155 通信模块采用MIL-STD-155;3B总线控制IP核，经模拟收发器及耦合电路 将信号发送至1553B总线上的其他总线设备；SPI通信模块采用Xilinx开发套件自带SPI 接口 IP核，通过SPI接线方式与大容量的SD (Secure Digital Memory Card)卡直接相连， 负责将实时导航信息储存在SD卡。本发明的高动态捷联惯性导航并行计算装置工作时，电源模块为FPGA芯片和配 置电路提供电源，光纤陀螺输出信号直接输入到FPGA的陀螺仪数据接收模块；石英挠性加 速度计输出信号经过采样及AD转换后输入到FPGA的AD控制模块；GPS接收机串口输出的 信号经过电平转换后输入到FPGA的GPS数据接收模块。数据采集模块的数据缓冲子模块 对并行输入的传感器信号预处理并送至初始对准模块与并行导航解算模块，初始对准结束后将导航参数的初始值发送至并行导航解算模块，并行导航解算模块对SINS导航解算 算法进行了并行化处理，在并行导航解算模块内对传感器增量信号进行并行的迭代计算， 每采样周期内更新计算一次，解算得到的导航信息分别发送至通信模块的串口通信模块、 155 通信模块和SPI通信模块。其中，串口通信模块经电平转换电路将信号发送至其他串 口设备；155 通信模块经模拟收发器及耦合电路将信号传输到155 总线的其他总线设 备；SPI通信模块负责将实时导航信息储存在大容量的SD卡。本发明将导航计算机各功能模块集成于单片FPGA内，并在FPGA内将单一更新速 率高精度SINS导航解算算法进行了并行化处理，其优点在于大大加快了导航解算的计算 速率，提高了高动态环境下SINS的导航精度，并且增强了导航系统的灵活性，降低了成本， 具有很高的应用价值。
图1是本发明的总原理图。图2是本发明的电源电路原理图。图3是本发明的配置电路原理图。图4是本发明的信号采集原理图。图5是本发明的并行导航解算原理图。图6是本发明的高频运动效应补偿模块并行计算原理图。图7是本发明的并行导航解算运行时序示意图。图8是本发明的信号输出原理图。
具体实施例方式本发明的捷联惯性导航并行计算装置总原理图如图1所示，该并行计算装置由 FPGA芯片、电源电路、配置电路、信号采集电路和信号输出电路组成，其中FPGA芯片采用 Xilinx公司型号为)(C5VSX95T的Virtex5系列FPGA。该并行计算装置工作的总流程为电 源电路为FPGA芯片和配置电路提供工作电源，配置电路采用主串行模式，当FPGA上电后， 配置电路自动将片外非易失性存储器Platform Flash PROM中的配置比特流读入静态内 存SRAM中，实现内部结构映射。光纤陀螺、石英挠性加速度计和GPS接收机通过图示信号 采集电路将传感器信号并行发送至FPGA，FPGA通过内部的数据采集模块读取信号。在FPGA 内部，数据采集模块对传感器信号预处理得到SINS的位置信息和传感器增量信息，并分别 发送至初始对准模块和并行导航解算模块。首先由初始对准模块对SINS进行粗对准和精 对准，并将导航参数初始值发送至并行导航解算模块，然后并行导航解算模块开始对数据 采集模块发送过来的增量信息进行并行导航解算。解算结束得到SINS的姿态、速度和位置 信息，并将其发送至通信模块，然后经图示信号输出电路发送至其他设备或储存在SD卡。本发明所述的电源电路原理图如图2所示，电源电路输入端为5V直流电源VCC5V， 输出端为 VCCINT、VCC0、VCCAUX、VCC3. 3 和 VCC1. 8。VCC5V 直接接入模块 Xl 的 VI 与 TRACK 输入端，并通过去耦电容Cl与C2接地，模块Xl采用TI公司型号为PTV05010W的非隔离 DC-DC转换器，模块Xl的INHIBIT使能端通过上拉电阻Rl与VCC5V相连，ADJUST端口通 过电阻R2与接地端GND和GND2相连并接地，电压输出端VO与V02通过去耦电容C3和C4
5接地，产生)(C5VSX95T所需要的1. OV核心电压VCCINT ；VCC5V直接接入模块X2的VIN输入 端，并通过去耦电容C5接地，模块X2采用TI公司型号为PTH05000WAD的非隔离DC-DC转 换器，模块X2的INHIBIT使能端通过上拉电阻R3与VCC5V相连，V0_ADJ端口通过电阻R4 接地，电压输出端VOUT通过去耦电容C6和C7接地，产生XC5VSX95T所需要的2. 5V的I/O 电压VCCO ；VCC5V直接接入芯片X3的VIN输入端，并通过去耦电容C8接地，芯片X3采用LT 公司型号为LT1764AE-2. 5V的稳压芯片，芯片X3的SHDN端口通过上拉电阻R5与VCC5V相 连，TAB与GND端口接地，SENSE端口通过电阻R6与电压输出端VOUT相连，并由电阻R7接 地，VOUT通过去耦电容C9和ClO接地，产生)(C5VSX95T所需要的2. 5V的辅助电压VCCAUX ； VCC5V直接接入芯片X4的VIN输入端，并通过去耦电容Cl 1接地，芯片X4采用LT公司型号 为LT1764AE-3. 3V的稳压芯片，芯片X4的SHDN端口通过上拉电阻R8与VCC5V相连，TAB与 GND端口接地，SENSE端口通过电阻R9与电压输出端VOUT相连，并由电阻RlO接地，VOUT 通过去耦电容C12和C13接地，产生配置芯片XCF32P所需要的3. 3V电压VCC3. 3，如图3所 示；VCC3. 3直接接入芯片)(5的VIN输入端，并通过去耦电容C14接地，芯片)(5采用LT公 司型号为LT1764AE-2. 5V的稳压芯片，芯片X5的SHDN端口通过上拉电阻Rll与VCC3. 3相 连，TAB与GND端口接地，SENSE端口通过电阻R12与电压输出端VOUT相连，并由电阻R13接 地，VOUT通过去耦电容C15和C16接地，产生配置芯片XCF32P所需要的1. 8V电压VCC1. 8， 如图3所示。本发明所述的配置电路原理图如图3所示，配置电路采用主串行配置方式，其中 芯片X7为XC5VSX95T，配置芯片)(6采用Xilinx公司的型号为XCF32P的Platform Flash PROM非易失存储芯片，其容量为32M，如图所示，芯片)(6的核心电压VCCINT由VCC1. 8提 供，引脚VCCO与VCCJ均接至VCC3. 3电源端。主串接口 JPl的TDI引脚连接)(6的TDI引 脚，TMS引脚分别连接X6与X7的TMS引脚，TCK引脚分别连接X6与X7的TCK引脚，TDO引 脚连接X6的TDO引脚，)(6的TDO引脚连接X7的TDI引脚；X7的CCLK引脚连接X6的CLK 引脚，为X6提供时钟信号；X7的DONE引脚连接)(6的CE引脚，并通过上拉电阻R16接至 VCC3. 3，为X6提供使能信号；X7的INIT_B连接1的RESET引脚，并通过上拉电阻R15接至 VCC3. 3，为1提供初始化信号；X7的PR0G_B引脚连接1的CF引脚，并通过上拉电阻R14 接至VCC3. 3 ；X7的DIN引脚连接)(6的DO引脚，接收从)(6输出的串行配置数据。本发明的信号采集原理图如图4所示，外部传感器有三轴垂直正交的光纤陀螺仪 X、Y和Z，三轴垂直正交的石英挠性加速度计X、Y和Z以及GPS接收机，其信号最终传输至 FPGA数据采集模块，数据采集模块由陀螺仪数据接收模块、AD控制模块、GPS数据接收模块 和数据缓冲模块组成。单轴的光纤陀螺仪存在两路输出信号，以陀螺仪X为例，一路为每 帧对位的陀螺串行数字信号Gyro_X data，另一路为同步时钟信号Gyro_X clk，两路信号 直接与数据采集模块的陀螺仪数据接收子模块相连，在陀螺仪数据接收模块内信号Gyro_X elk与FPGA内部时钟信号产生单稳态触发电路，根据单稳态触发器信号启动和结束Gyro_ X data每帧信息的读取，读取结束将信息发送至数据缓冲模块，陀螺仪Y和Z的信号同步 发出，其信号采集原理与陀螺仪X相同；3轴的石英挠性加速度计输出电流形式的速度信 号ACC_X、ACC_Y和Acc_Z，电流信号分别经过精密电阻进行采样后输入至INAl 18运算放大 器，经放大的信号并行输入至模数转换器，这里采用4通道的高速22位模数转换器AD7716， 同时接收3路加速度计信号，并由数据采集模块的AD控制子模块控制，对3路信号同时进行模数转换并进行数据读取，读取结束将数据发送至数据缓冲模块；GPS接收机接口为 RS232串行接口，其输出端通过MAX232接至数据采集模块的GPS数据接收子模块的输入端， MAX232负责RS232电平到FPGA的TTL电平的转换，GPS数据接收模块接收数据后将其发送 至数据缓冲模块。数据缓冲模块接收各路传感器数字信号并对其进行提取和预处理，得到 SINS系统位置信息、当前采样周期内的角增量信息和当前采样周期内的速度增量信息，处 理结束后将信息分别发送至FPGA的初始对准模块和并行导航解算模块。本发明的并行导航解算的原理图如图5所示，并行导航解算模块(Ml)基于游动 方位坐标系统的单一更新速率的高精度SINS导航解算算法设计。该单一更新速率算法对 多速率SINS导航解算算法作出改进，使用精确形式的计算式并提高计算速率，每一采样周 期内对SINS的姿态、速度和位置更新计算一次，并且模块(Ml)对单一更新速率的SINS导 航解算算法在算法级和功能级进行了并行化处理。模块(Ml)由高频运动效应补偿模块 (Mil)、姿态计算模块(M12)、速度计算模块(M13)、位置计算模块(M14)和地球参数计算模 块(M15)5个子模块组成，各模块间通过数据流相互连接，并由补偿模块(Mil)控制姿态模 块(M12)、速度模块(M13)、位置模块(M14)和地球参数模块(M15)同时并行运行。在并行 导航解算模块(Ml)开始工作之前，首先接收初始对准模块的发送过来的导航参数初始值 initial，姿态模块(M12)、速度模块(M13)、位置模块(M14)和地球参数模块(MM)完成初 始化后等待各模块启动信号start。每当一个采样周期过后，数据采集模块将角增量信号 da和速度增量信号dv发送至补偿模块(Mil)，并通过信号start启动补偿模块(Mil)。补 偿模块(Mil)启动后对载体高频运动引起的效应进行并行计算(其原理图如图6所示)，由 内部数据线将Φ发送至姿态模块(Μ12)，dVsf发送至速度模块(M13)，dRsf发送至位置模 块(M14)，同时将ready (准备好)作为启动信号发送至姿态模块(M12)、速度模块(M13)、 位置模块(M14)和地球参数模块(M15)的start端口。模块(M12)-(M15)启动后并行运 行，其中，姿态模块(MU)通过当前采样周期内的等效旋转矢量Φ、模块(MU)在前一采样 周期计算得到的导航坐标系下的速度Vru模块(MK)在前一采样周期计算得到的地球旋转 角速率wien和曲率矩阵Fcn，计算得到当前采样时刻的姿态方向余弦矩阵Cbl及姿态角 Eular ；速度模块(MU)首先通过模块(14)在前一采样周期计算得到的游动方位角a计算 得到前一采样时刻地理坐标系下的速度Vgeo，然后通过当前采样周期内的比力引起的速度 增量dVsf、模块(M12)在前一采样周期计算得到的姿态方向余弦矩阵Cbl、模块(M15)在前 一采样周期计算得到的地球旋转角速率wien和曲率矩阵Fcn及重力加速度gn，计算得到当 前采样时刻的重力/科里奥利加速度修正量dVg/cor及导航坐标系下的速度Vn ；位置模块 (M14)通过当前采样周期内的比力引起的位置增量dRsf、模块(MU)在前一采样周期计算 得到的姿态方向余弦矩阵Cbl、模块(MU)在前一采样周期计算得到的重力/科里奥利加速 度修正量dVg/cor及导航坐标系下的速度Vru模块(MM)在前一采样周期计算得到的曲率 矩阵Fcn，计算得到当前采样时刻的位置矩阵Cne及经纬度Pos和高度h。当前采样周期导 航解算结束后，并行导航解算模块(Ml)将得到的导航信息发送至FPGA内的通信模块。本发明的高频运动效应补偿模块并行计算原理图如图6所示，高频运动效应补偿 模块(Mil)包含姿态补偿模块(Mill)、速度补偿模块(MlU)和位置补偿模块(Mli;3)3个子 模块。每当一个采样周期过后，数据采集模块将角增量信号da和速度增量信号dv发送至 模块(Mil)，并通过信号start同时启动模块(Mill) -(Ml 13)，模块(Mill)-(Ml 13)并行执行。其中，姿态补偿模块(Mill)接收角增量信号da，负责圆锥效应补偿及等效旋转矢量计 算，输出旋转矢量Φ及准备好信号ready ；速度补偿模块(MlU)接收角增量信号da和速 度增量信号dv，负责速度旋转效应补偿、划桨效应补偿及比力引起速度增量计算，输出比力 引起速度增量dVsf及准备好信号ready ；位置补偿模块(MlU)接收角增量信号da和速度 增量信号dv，负责位置旋转效应补偿、润卷效应补偿及比力引起位置增量计算，输出比力引 起位置增量dRsf及准备好信号ready。同时由模块(Ml 11)-(Ml 13)的ready信号经3输 入与门汇总为模块(Mil)的ready信号发出。由图示可知，各补偿操作并行执行，大大减少 了更新计算的时间。本发明的并行导航解算运行时序示意图如图7所示，记Ts为当前采样周期，Ts^1, Ts+1分别为前一采样周期。并行导航解算模块启动后，首先并行执行高频运动效应补偿模块 的姿态补偿、速度补偿、位置补偿模块，模块计算结束后再并行执行姿态计算、速度计算、位 置计算、地球参数计算模块。如图所示，并行导航解算过程并行程度很高，且在每一采样周 期内执行一次，可大幅提高SINS导航解算的计算速率。本发明的信号输出原理图如图8所示，FPGA的通信模块由串口通信模块、155 通 信模块和SPI通信模块组成，接收来自FPGA并行导航解算模块输出的导航信息，并将其分 别发送至其他串口设备、其他1553B总线航电设备和SD卡。其中，串口通信模块采用串口 通信IP核，将导航信息通过FPGA的T)(D端发送至电平转换芯片MAX232或MAX485，电平转 换芯片负责将TTL电平的数字信号转换为对应串口通信格式的电平信号，并发送至其他设 备的T)(D端口 ；155 通信模块采用MIL-STD-155;3B总线协议控制IP核，通过模拟收发器 芯片H0LT-1570将数据发送至155 总线耦合器，此处总线耦合采用隔离变压器耦合方式， 由H0LT-1570输出的信号先经过隔离变压器，再经过耦合变压器和隔离电阻输送到155 的双绞总线上，最终完成FPGA的TTL电平信号到1553B总线信号的转换；SPI接口模块直 接采用SPI接口 IP核，模块的SPI MOSI, SPI MISO和SPI CLK端口直接连接SD卡的DI、 DO和CLK引脚，由SPI接口 IP核控制总线，将实时的导航信息储存在SD卡内。
权利要求
1.一种高动态捷联惯性导航并行计算装置，由FPGA芯片、电源电路、配置电路、信号采 集电路和信号输出电路组成，其特征在于所述的FPGA芯片集成了数据采集模块、初始对准模块、并行导航解算模块、通信模块；所述的配置电路为当FPGA芯片上电后，自动将片外非易失性存储器中的配置比特流 读入FPGA的静态内存中，实现内部结构映射；所述的信号采集电路采集来自光纤陀螺、石英挠性加速度计及GPS接收机的输出信号 并输入到FPGA芯片的数据采集模块；所述的数据采集模块将对信号预处理并发送至初始对准模块与并行导航解算模块，初 始对准模块将计算得到的导航参数初始值发送至并行导航解算模块并在并行导航解算模 块内对导航参数进行更新计算，解算后将导航信息送至通信模块，最后由通信模块经信号 输出电路将信号发送至其他设备。
2.根据权利要求1所述的高动态捷联惯性导航并行计算装置，其特征在于所述的数 据采集模块包含陀螺仪数据接收模块、AD控制模块、GPS数据接收模块、数据缓冲模块，其 中所述的陀螺仪数据接收模块负责采集光纤陀螺输出信号，AD控制模块控制石英挠性加速 度计信号的模数转换并接收数据，GPS数据接收模块负责接收GPS接收机输出的数据，数 据缓冲模块负责对传感器数据缓冲预处理，并将其发送至初始对准模块与并行导航解算模 块。
3.根据权利要求1所述的高动态捷联惯性导航并行计算装置，其特征在于所述的 FPGA芯片内的并行导航解算模块在每一采样周期内对导航参数中的姿态、速度和位置信息 进行并行更新计算。
4.根据权利要求3所述的高动态捷联惯性导航并行计算装置，其特征在于所述的并 行导航解算模块包含高频运动效应补偿模块、姿态计算模块、速度计算模块、位置计算模块 和地球参数计算模块5个子模块，其中所述的高频运动效应补偿模块负责接收当前采样周 期内的角增量信号和速度增量信号，在初始对准模块的启动信号发出后，开始对角增量和 速度增量信号进行快速迭代运算，其中包括圆锥效应补偿计算、速度旋转效应补偿计算、划 桨效应补偿计算、位置旋转效应补偿计算和涡卷效应补偿计算，各部分计算并行执行，当计 算结束时通过指令信号同时启动姿态计算、速度计算、位置计算和地球参数计算4个模块， 使其同时并行运行，实现了姿态、速度和位置信息的并行更新计算。
5.根据权利要求1所述的高动态捷联惯性导航并行计算装置，其特征在于所述的通 信模块由串口通信模块、155 通信模块、SPI通信模块构成，并分别采用对应通信格式IP 核，负责与其他串口设备、155 总线设备、SD卡的通信任务。
全文摘要
本发明公开了一种高动态捷联惯性导航的并行计算装置，由FPGA芯片、电源电路、配置电路、信号采集电路和信号输出电路组成，FPGA芯片集成了数据采集模块、初始对准模块、并行导航解算模块、通信模块；光纤陀螺、石英挠性加速度计及GPS接收机的输出信号经信号采集电路输入到FPGA芯片的数据采集模块，对信号预处理并发送至初始对准模块和并行导航解算模块，初始对准模块将计算得到的导航参数初始值发送至并行导航解算模块并在并行导航解算模块内对导航参数进行更新计算，解算后将导航信息送至通信模块，经信号输出电路将信号发送至其他设备。本发明装置大大加快了SINS导航解算算法的计算速率，提高了SINS的导航精度。
文档编号G01C21/16GK102128624SQ20101060852
公开日2011年7月20日 申请日期2010年12月28日 优先权日2010年12月28日
发明者吴铁军, 孙国栋, 马龙华 申请人:浙江大学