一种捷联惯导设备多核DSP并行解算方法与流程

本发明属导航定位授时技术领域，涉及捷联惯导设备，尤其是一种捷联惯导设备多核dsp并行解算方法。

背景技术：

目前，捷联惯性导航设备在高动态环境下，受惯性元件采样频率和dsp解算频率限制，特别是冲击、震动及里程计组合定位定向试验，受采集和解算频率限制，设备定向精度和定位精度会出现比正常情况下发散快的问题，严重影响了设备在高动态环境下的正常使用。有文献显示，国内有人尝试使用双dsp方式来提高惯导解算频率，但都是以串行处理的方式实现，其解算速度提升空间有限，也有学者提出可以使用单片fpga实现多处理器并行计算，但并未深入研究。普通的捷联惯导解算算法通常按顺序串行执解算流程。在不改变硬件的条件下，提高设备采样和解算频率一般已接近上限，并且难度较大，而使用多核dsp可以提供并行计算能力，但涉及到并行计算的规划，特别是软件并行计算和捷联惯导解算方案的并行运行。

通过专利公开文献的检索，虽然有多篇涉及多核dsp并行处理的技术，但与本发明申请所涉及的高动态环境下抑制三维姿态角发散快问题的技术方案，尚未发现。

技术实现要素：

本发明的目的在于克服现有技术的不足之处，提供一种捷联惯导设备多核dsp并行解算方法，该方法引入多核dsp进行并行计算，重新规划捷联惯导方案解算步骤由串行变为并行思路，以达到提高捷联惯导解算频率的目的，进而提高设备在高动态环境下的定位定向精度。

本发明实现目的的技术方案是：

一种捷联惯导设备多核dsp并行解算方法，步骤是：

步骤1：惯导设备上电cpu初始化：

解算板dsp上电，完成双核心初始化，配置中断向量表及中断号、硬件中断服务(hwi)程序绑定；创建错误块，为多任务创建做准备；

步骤2：分配及创建多任务：

根据计算量大小和逻辑先后顺序，分配独立任务模块多个来完成捷联惯导解算及数据通讯功能；

步骤3：根据多核并行解算捷联惯导方案，采用tms320c6657型号dsp为双核dsp进行并行解算及通讯：

步骤4：仿真验证。

而且，所述步骤1中多核dsp的初始化包括：外设使能配置、gpio管脚复用配置、gpio管脚初始化、hwi初始化及错误块初始化。

而且，所述步骤2中的独立任务模块分别为：陀螺脉冲采集、加速度计脉冲采集、里程计脉冲采集、北斗同步信号采集、地球参数计算、速度更新、姿态更新、位置更新、卡尔曼滤波预测、卡尔曼滤波更新、以太网通讯收发、can通讯收发、串口通讯收发、里程计数据采集、北斗数据接收。

而且，所述步骤2的具体实施过程为：

严格按照如下过程周期性实施，上一步骤完成是下一步骤开始的前题条件，并且均为独立运行任务模块：一是陀螺脉冲采集、加速度计脉冲采集、里程计脉冲采集、北斗同步信号采集为基础数据采集，作为后续解算数据源；二是计算地球参数计算；三是捷联惯导速度更新、姿态角更新和位置更新计算；四是卡尔曼滤波预和滤波更新计算；五是进行以太网、can总线以及串口的数据收发、北斗数据和高度计数据以及调试串口的数据收发。

而且，所述步骤3的具体实施过程为

选用实时操作系统的好处是当外界事件产生时，能够接受并以足够快的速度予以处理，其处理的结果又能在规定的时间之内来控制生产过程或对处理系统做出快速响应，调度一切可利用的资源完成实时任务，并控制所有实时任务协调一致运行；选用ti公司发布的tms320c6657型dsp为双核dsp，满足多核并行计算需求；为平衡dsp双cpu使用性能，以cpu1为主核cpu2为副核，其中设备自调试串口预留在主核上，主核和副核共享内存和数据资源；采用ti公司发布的sys/bios实时操作系统进行实现，实时操作系统的本质就是在多个cpu上同一时间进行不同任务的计算，从而达到并行计算的目的，而sys/bios操作系统是基于多任务多优先级抢占式的实时操作系统；具体集成开发套件使用的sys/bios组件涉及到时钟、硬中断和软件中断，任务和信号量组件。

本发明的优点和积极效果是：

1、本发明充分利用多核dsp并行计算的性能，有效提高dsp解算频率，达到在高动态环境下抑制三维姿态角发散快的目的。

2、本发明能够提高捷联惯导解算频率，提高设备响应速度，在一定程度上能够提高惯导/里程计组合导航定位精度，同时抑制在冲击和震动试验时设备的三维姿态误差发散快的作用，在实际工程应用中具有较好的使用效果。

附图说明

图1为本发明多核dsp6657初始化流程图；

图2为本发明并行解算捷联惯导方案多任务分配方案图；

图3为本发明集成开发套件使用的sys/bios组件图。

图4为卡尔曼滤波预测和更新过程，其中：xk-1和pk-1为初始给定经验值。

具体实施方式

以下结合附图对本发明的实施例做进一步详述；本实施例是描述性的，不是限定性的，不能由此限定本发明的保护范围。

一种捷联惯导设备多核dsp并行解算方法，步骤是：

步骤1：惯导设备上电cpu初始化：

解算板dsp上电，完成双核心初始化，配置中断向量表及中断号、硬件中断服务(hwi)程序绑定；创建错误块，为多任务创建做准备。

图1所示为具体步骤，多核dsp的初始化包括：

外设使能配置、gpio管脚复用配置、gpio管脚初始化、hwi初始化及错误块初始化。

步骤2：分配及创建多任务：

根据计算量大小和逻辑先后顺序，共分配独立任务模块15个来完成捷联惯导解算及数据通讯功能。任务模块为：1陀螺脉冲采集,2加速度计脉冲采集，3里程计脉冲采集，4北斗同步信号采集，5地球参数计算，6速度更新，7姿态更新，8位置更新，9卡尔曼滤波预测，10卡尔曼滤波更新，11以太网通讯收发，12can通讯收发，13串口通讯收发,14里程计数据采集，15北斗数据接收。

本步骤中，所述陀螺脉冲采集、加速度计脉冲采集、里程计脉冲以及北斗同步信号采集任务，是捷联惯导设备的基础数据源，其采用dsp外部高精度晶振进行计时产生中断，从而进行驱动dsp进行周期性(如1000hz)读取惯性元器件的原始脉冲数据。

地球参数计算，包括地球重力加速度、地球子午圈半径和卯酉圈半径。按如下公式计算：

g＝g0[1+0.00527094sin²l+0.0000232718sin⁴l]-0.000003086h

速度、姿态、位置的三类更新采用公式如下：

捷联惯导方向余弦矩阵为：

则：姿态为：

θ＝sin^-1(t32)

速度为：

位置为：

卡尔曼滤波预测和更新过程参见图4

can总线以及串口和以太网接口的收发过程，均采用fpga扩展的内核作为dsp与外部接收的中间转换件，其过程与读取惯性元件的原始数据同样处理，即周期性读取指定地址空间，如果有缓冲数据，则根据缓冲数据空间大小进行全部数据的读取，而不是采用中断的方式。根据工程经验采用中间转换件的方法要比采用dsp直接与外部接口进行通讯的方式更具稳定性和可靠性。

步骤2中创建的模块均创建一个任务，其在操作系统运行时，根据优先级实施过程进行自动调度。图2中横向为同时启动的任务模块，其在主核心cpu和副核心cpu核心上同时进行并行计算；图2中纵向为按时间先后顺序创建并运行在不同cpu核心上的任务模块，上一任务模块计算完成后才开始下一任务模块的计算。主核心cpu为按图2左侧任务顺序执行，与此同时副核心cpu按图2右侧任务顺序执行，从而达到双核cpu任务的分配，达到并行解算的目的。

步骤3：根据多核并行解算捷联惯导方案，进行并行解算及通讯：

选用实时操作系统的好处是当外界事件产生时，能够接受并以足够快的速度予以处理，其处理的结果又能在规定的时间之内来控制生产过程或对处理系统做出快速响应，调度一切可利用的资源完成实时任务，并控制所有实时任务协调一致运行。ti公司发布的tms320c6657型dsp为双核dsp，满足多核并行计算需求，可以利用适当的策略控制多任务的执行，提高资源的利用效率。可以使cpu的利用率得到极大的发挥，把复杂的应用程序层次化，使dsp软件激光器容易设计和维护。为平衡dsp双cpu使用性能，以cpu1为主核cpu2为副核，其中设备自调试串口预留在主核上，主核和副核共享内存和数据资源，具体实施过程见图2。该步骤的具体实施过程为：

本发明采用ti公司发布的sys/bios实时操作系统进行实现，实时操作系统的本质就是在多个cpu上同一时间进行不同任务的计算，从而达到并行计算的目的，而sys/bios操作系统是基于多任务多优先级抢占式的实时操作系统。具体集成开发套件使用的sys/bios组件如图3所示，涉及到时钟、硬中断和软件中断，任务和信号量组件。

步骤4：试验验证：

按照上述步骤一、二、三实施后，经试验验证，捷联惯导设备解算时间由1.25ms缩减到0.57ms，提升效率约为45％，其中约5％是双核dsp进行资源共享及核间通讯和操作系统开销占用，通过使用并行解算方法，合理分配捷联惯导设备并行解算步骤，达到了提升解算频率的目的。