基于DSP的捷联惯导系统组合初始对准算法设计∗

（海军工程大学电气工程学院 武汉 430033）

1 引言

惯性导航作为当今各种导航方式之一，具有抗干扰能力强，不需要外界提供信息等诸多优势，具有不可替代作用。而光纤陀螺捷联惯导系统通过数学平台替代物理平台，具有结构简单，成本低，重量轻等特点，是惯性导航发展的必然趋势［1］。

在平台惯导系统中，运载体的姿态角以及航向角可通过平台框架上得到，计算量小。而在捷联惯导系统中由于没有平台惯导类似的物理平台隔离角运动，载体的晃动干扰直接作用于惯性元件，并采用数学平台得到载体坐标系和导航坐标系之间的方向余弦矩阵，姿态角和航向角都需要经过计算获得，这导致捷联惯导的导航解算部分计算量庞大［2］。而高精度捷联惯导系统又对系统处理数据的实时性要求很高，这就需要选取合适处理器来完成精度高、计算量大的导航解算任务。另外惯性导航的初始对准关系到导航的精度和使用前的准备时间，初始对准误差会随着导航解算过程进行累积，制约着导航的精度［2～6］。基于此本文通过TMS320F28335 高速 DSP［7～8］完成导航系统初始对准工作，采用惯性系粗对准和卡尔曼精对准完成初始对准，通过设计软件程序设计等步骤流程验证算法的可行性。

2 捷联惯导组合初始对准技术

捷联惯性导航系统的初始对准是系统正常进入导航解算阶段的必要前提。初始对准分为粗对准和精对准两个阶段，对准过程中首先利用粗对准在短时间内得到载体坐标系到导航坐标系的姿态矩阵的估计粗略值，再通过精对准计算得到精确的姿态矩阵［5］。而初始位置信息一般通过外界输入得到（如通过GPS获取［9］）。对于初始速度信息，由于一般是在静止状态下进行初始对准，初始速度为零。本文所用的组合对准是将对准速度快的惯性系对准和精度高的卡尔曼滤波对准有机结合，达到将二者算法优势结合的目的。

2.1 惯性系快速粗对准

传统解析式粗对准是基于地理系，惯性系粗对准是基于惯性系，但本质上都是双矢量定姿［3］。惯性系对准是通过重力加速度信息在惯性坐标系内进行相应的解算得到姿态矩阵。惯性系对准较传统的对准方法可以在短时间内得到导航前初始姿态角，并可以较好地适应晃动基座环境下的初始对准，具有较好地抗干扰能力，能够适用于系泊晃动的摇摆基座环境。解决了传统解析粗对准仅能够适用于载体静态或轻微晃动环境下实现对准的局限性［10～12］。

在惯性坐标系下将姿态矩阵分裂成三个矩阵［3］分别求解：

式中，i系为地心惯性坐标系（初始对准时刻的地球坐标系）；ib0系为基座惯性坐标系（初始时刻的载体坐标系，相对惯性空间无转动）。

为载体相对基座惯性坐标系的姿态阵，其初值是单位矩阵，即，可以体现出晃动基座的姿态变化，而可由陀螺输出利用旋转矢量法实时更新得到［2，10］。

可以根据载体所在位置的纬度信息L及时间t计算得到：

对于求解阵，由于i和ib0均为惯性坐标系，可选取惯性坐标系下的两个不共线的参考矢量加以确定。

由于在晃动基座下加计输出因载体晃动会产生干扰线加速度，将这种大致呈现周期性变化的干扰通过积分可在一定程度上降低此干扰产生的速度误差。因此对式（3）和式（4）进行积分得到式（5）和（6）：

构建两组速度适量v(tk1)、v(tk2)以及两组速度矢量的叉乘矢量v(tk1)×v(tk2)，以此建立ib0和i的关系：

考虑到在对重力加速度在ib0系内进行积分运算时没有对垂荡、纵荡和横荡的扰动作补偿处理，在晃动基座环境下这会导致的求取产生较大误差，因此只用惯性系对准作为粗对准的方法。

2.2 卡尔曼滤波精对准

卡尔曼精对准解算过程是在粗对准得到的粗略姿态信息的基础上，采用卡尔曼滤波器精确估计出失准角［13～14］，再对粗对准得到的姿态矩阵作修正，进而得到更精确的姿态信息。

经典卡尔曼滤波器包含两个回路：状态估计回路（主回路）和误差方差回路（引导回路），通过卡尔曼增益，将两个回路连接。根据是否引入量测量将滤波器的更新方式分为两类：时间更新和量测更新。卡尔曼滤波方程可划分为五个基本公式。

在卡尔曼滤波过程中，考虑到简化计算复杂程度，建立了9维的卡尔曼滤波方程。选取姿态误差、速度误差和位置误差作为状态变量X，将系统解算得到的速度Vn和位置POS与GPS提供的准确的速度vnGPS和位置posGPS作差得到观测量Z：

状态转移矩阵Φ由捷联惯导系统的误差方程经过离散化处理后获得的9×9维矩阵。观测矩阵H为

DSP对IMU的数据采集频率为50Hz，设置GPS发送频率为1Hz，GPS信号通过串口被DSP采集并解码提取速度和位置信息。在GPS传输间隔只进行卡尔曼滤波器的时间更新，在获取到GPS提供的速度和位置信息后进行一次量测更新。每次状态变量更新后，通过得到的9个误差量对当前解算得到的导航结果进行一次反馈修正。

系统噪声Q阵，由实测数据反推得到陀螺的角度随机游走和加速度计的速率随机游走。

观测噪声R阵，根据GPS标称的定位误差设置。

3 基于CCS的组合对准程序设计

DSP通常使用TI公司提供的集成开发环境CCS［7］，该环境为用户提供了环境配置、源程序编辑、源程序链接、源程序调试、运行结果分析等功能，可为用户提供与硬件开发板结合的在线编程调试。在开发过程中，可使用TI为F28335用户提供的利用结构体和联合体定义片内各个寄存器的头文件库，用户可以方便地用C语言对DSP内部的寄存器进行访问。DSP28335程序空间和数据空间是统一编址的，通过配置cmd文件完成对存储空间的划分［8］。

在编写主程序前通常先对系统配置进行初始化，GPIO以及串口初始化要结合硬件的连接进行。本系统采用SCI串口进行数据通信，F28335提供给用户3个可使用的SCI串口。SCI_A和IMU导航原始数据进行通信，SCI_B和上位机进行通信，SCI_C和外部GPS模块进行通信。通过最大可达16级深度的FIFO，设置中断接收函数，减少CPU不必要的开销，提高效率。对准过程中由上位机输出导航参数。图1为惯性系粗对准流程图，图2为卡尔曼精对准流程图，图3为从GPS信号中解码出位置和速度信息流程图。

图1 惯性系粗对准流程

4 实测试验及结果分析

在调试代码通过后，通过F28335自带的FLASH完成程序固化，并上转台试验。基于F28335DSP惯导实验平台如图4所示。值得注意的是由于FLASH中指令运行速度远不如RAM调试中的运行速度，需要将部分对时间较敏感的函数在系统开机后自动复制到F28335的RAM中运行，提高运行速度。

为验证组合对准算法的有效性，以项目组自研光纤陀螺捷联惯导样机为测试平台，完成传统的罗经对准方法和本文采用的组合对准方法的比较验证。

由于实验条件限制，无法使用高精度定向测试转台，只将惯导系统放置于实验台上进行静态初始对准测试，水平姿态角和航向角未知。

图2 卡尔曼精对准流程

图3 提取GPS速度和位置信息流程

首先采用传统的罗经对准方法（下文称作罗经法）对系统进行30min静基座的初始对准测试。由于传统罗经法的水平对准能够在短时间内收敛至较高精度，而方位对准难以在短时间内达到较高精度要求，因此设置水平对准2min，航向对准28min，对准结果如图5和图6所示。

图4 DSP调试平台

图5 传统罗经法水平对准

图6 传统罗经法航向对准

图7 基于卡尔曼滤波的水平对准

然后再采用本文提出的组合对准方法进行对比测试。鉴于惯性系对准具有对准速度快，抗扰动能力强的特性，设置惯性系粗对准时间为2min，卡尔曼滤波精对准时间为18min。其中基于卡尔曼滤波的精对准结果如图7和图8所示。

图8 基于卡尔曼滤波的航向对准

组合对准方法的粗对准和精对准过程分别获得的姿态角见表1。两种对准方法完成后获得的姿态角见表2。

表1 组合法对准结果

表2 两种方法对准结果对比

由于放置惯导系统的实验台水平误差未知，并且北向基准的不确定，因此无法得知理想的具体对准精度，只能相对而言进行评估。由表1可以看到，在静态环境下由于外部干扰小，在惯性系快速对准中阵计算误差小，因而经过粗对准估计出的三个姿态角已经接近了精对准估计出的姿态角，验证了惯性系粗对准的高效性，同时为后续的精对准打下了良好的基础。由表2可以看出，本文使用的组合对准算法和罗经法对准结果在水平姿态角上精度相当。两种方法获得的航向角相差较大，但由于无精确的北向基准，无法判定航向角精度的好坏。

通过对比图6和图8，传统的罗经法航向角在1500s后波动性有所减弱但仍不稳定，短时间对准精度较差。而组合法的航向角在400s后波动性减弱，800s后趋于稳定。可以看出在航向角对准上组合对准方式不管是在速度还是稳定性上都要优于传统罗经对准。通过对比图5和图7，在水平角对准上两种对准方式精度相似，但组合法输出姿态角更加平稳。两种方法在800s后的估计的姿态角标准差见表3。

表3 两种方法800s后估计的姿态角标准差

综上分析，本文采用的惯性系快速粗对准可以在短时间内得到较为精确的姿态角，精度和传统的罗经法对准相当。粗对准后再通过卡尔曼滤波器对失准角进行精确估计。从试验结果看验证了传统的罗经对准方法在航向对准上存在收敛速度较慢、稳定性较差等问题。而组合对准方法可以有效解决传统方式存在的问题，在短时间内航向角标准差比传统法降低两个数量级，水平角标准差比传统法降低一个数量级。另外组合对准在对准速度上比传统方法更快，可以在16min内完成。

5 结语

初始对准作为捷联惯性导航系统进入工作的必要前提，对准误差会在解算过程中进行传递累积，因此对准精度是提高导航精度的前提。通过采用惯性系粗对准和基于卡尔曼滤波精对准的组合方式完成捷联惯导系统的初始对准，可以增强系统对外界的抗扰动能力，同时兼顾对准精度和对准时间。基于DSP28335作为初始对准的处理器，利用CCS软件完成了组合对准程序设计，以光纤陀螺捷联惯导样机以及本文设计程序算法为平台完成比对验证试验，证明了方法的有效性。