CN106052686B - 基于dsptms320f28335的全自主捷联惯性导航系统 - Google Patents

Abstract

基于DSPTMS320F28335的全自主捷联惯性导航系统，本系统以DSPTMS320F28335作为控制器，将全自主捷联惯性导航算法嵌入到浮点型DSP内的软件实现，实时采集、分析惯性测量单元IMU中光纤陀螺和加速度计的输出信号并控制算法运行，以保证全自主捷联惯性导航系统的高速处理能力、高实时性、高精度、高性能、低成本、低功耗。本发明适合应用于不提供精确定位信息、受外界干扰载体姿态时刻都在发生变化的场合下载体的全自主实时捷联惯性系统，诸如直接不提供纬度信息，或者纬度信息不可测的隧道深处、深山密林、深海海底情况下的载体全自主实时捷联惯性导航。

Description

基于DSPTMS320F28335的全自主捷联惯性导航系统

技术领域

本发明提供的是一种基于DSPTMS320F28335的全自主捷联惯性导航系统，精确度高，数据处理效率高，涉及算法嵌入浮点型DSPTMS320F28335内的软件实现、晃动基座下载体所在位置纬度信息计算、动态干扰情况下捷联姿态矩阵的解算，自对准完成后的捷联惯性自主导航。本发明适合应用于不提供精确定位信息、受外界干扰载体姿态时刻都在发生变化的场合下载体的全自主实时捷联惯性系统，诸如直接不提供纬度信息，或者纬度信息不可测的隧道深处、深山密林、深海海底情况下的载体全自主实时捷联惯性导航。

背景技术

导航就是以一定的精度、在规定的时间内将载体沿着预定的路线引导至目的地的过程。惯性导航是一种自主式导航技术，它不依靠外界信息，也不向外界辐射任何能量，仅利用陀螺仪和加速度计等惯性测量器件以及初始的导航信息来确定载体运行期间的各项导航参数，隐蔽性好、抗干扰性强，能够全天时、全天候为载体提供完备的运动信息。

惯性导航中的捷联式惯性导航技术将陀螺仪和加速度计直接安装在载体上，得到载体系下的加速度和角速度，通过导航计算机将测得的数据转换至导航坐标系完成导航，它不需要实体的稳定平台，成本低、体积小、重量轻、可靠性高。

惯性导航系统在进入导航工作之前，需要获得初始姿态以及位置信息，即进行初始对准过程。初始对准通常分为两个阶段：粗对准阶段和精对准阶段。诸如发动机处于高频振动的汽车、浪涌下的舰船等受干扰较大的载体，角振动和线振动会导致基座晃动，捷联惯导系统在自对准过程中的姿态时刻都在发生变化，初始对准的时间和精度会受到影响。因此，在运载体晃动干扰环境下完成初始对准过程，就必须屏蔽这些无法消除的扰动影响。

无论是导航过程还是对准过程，无论是静基座自对准方法还是动基座自对准方法，无论粗对准还是精对准，都需要外界辅助信息提供当地准确的地理纬度信息，如果地理纬度存在误差，将可能影响到初始对准的精度。这种依赖辅助信息的捷联惯性导航是不自主导航，然而在深山荒野，海底等环境中，想要获得当地的地理位置信息并不是一件很容易的事情，或者某些应用场合，如通信卫星跟踪或者武器平台的稳定，有较高的定向要求，而没有必要提供精确的定位要求，或者在某些应急场合，如军事战争中，受到敌方攻击，需要迅速做出反击，而定位系统来不及工作，却急需定向。在上述纬度未知的条件下，想要完成导航工作，就必须利用系统自身的惯性测量单元输出解算出载体初始时刻所在位置的纬度信息，利用自身解算纬度信息值完成导航过程，实现载体的全自主捷联惯性导航。

另外，随着捷联式惯性导航技术的不断应用，对系统的精度、算法、数据处理能力、实时性、稳定性都有了相应的进一步的要求，且大多相关应用场合都提出了系统小型化、低功耗等要求。从处理器硬件平台选择上考虑，在大量的浮点数运算或者复杂算法运算时，对于一般的单片机或定点型处理器，程序执行效率较低，数据更新较慢，执行周期较长，不仅不能满足对运载体姿态矩阵实时高效地解算，同时数据处理精度低，运算能力也不能完全满足。

针对上述问题，本发明应用TI公司的TMS320F28335浮点型DSP控制器作为硬件平台，来控制数据采集和算法解算，完成载体的实时快速捷联惯性导航系统。在本发明中，TMS320F28335型数字信号处理器的高速处理能力、高精度、高性能、低成本、低功耗、外设集成度高、数据以及程序存储量大、32位浮点运算单元等性能在解算方面得到了充分发挥，从而满足数据处理效率高，实时性好，精确度高，有效在动态干扰情况下解算纬度信息值，有效隔离晃动对载体初始对准的影响，有效实现全自主捷联惯性导航系统的实时高精度快速自对准，进入自主导航状态。

发明内容

在本发明方法中，全自主捷联惯性导航算法嵌入到浮点型DSPTMS320F28335内的软件实现。本发明的目的在于，针对需要外界辅助信息提供当地地理纬度信息的导航环境，以及对导航系统数据处理能力、实时性、稳定性、小型化、低功耗的进一步高要求，提出一种基于DSPTMS320F28335的全自主捷联惯性导航系统，不依外界赖辅助信息，仅利用系统自身的惯性测量单元输出，高精确度、高数据处理效率地在动态干扰情况计算纬度信息，有效隔离晃动对载体初始对准的影响，有效实现实时快速高精度全自主式导航过程。

实现本发明目的的技术解决方案为：

本发明提出一种基于DSPTMS320F28335的全自主捷联惯性导航系统，包括以下方面：

(1)上位导航计算机开启，CCS5.3集成开发环境打开，建立工程并编写编译算法程序，链接SEED-XDS510PLUS仿真器，将算法嵌入到DSPTMS320F28335内的软件，DSP控制算法运行；

(2)32位浮点型DSPTMS320F28335控制惯性全自主导航系统，向导航系统发送命令协议，利用SCI通信，通过RS232通信串口，采集惯性测量单元IMU中光纤陀螺和加速度计的输出信号；

(3)不依外界赖辅助信息，仅利用系统自身的惯性测量单元输出，以惯性空间为参考基准，利用不同时刻重力投影之间的夹角，确定晃动基座下运载体初始时刻所在位置的纬度信息；

(4)不依外界赖辅助信息，带入计算所得纬度信息，利用系统自身的惯性测量单元输出，基于双矢量定姿原理，利用重力加速度在惯性空间方位的改变包含地球北向信息这一特性，求解惯性系下粗略的初始姿态阵，接着采用基于新息的自适应滤波方法获取载体的失准角，用失准角修正捷联姿态矩阵，得到三轴姿态信息，实时记录数据，完成初始对准，进入全自主导航状态。

本发明使用基于DSPTMS320F28335的全自主捷联惯性导航系统，装置总体结构框图如图1，装置总体实物图如图2。系统装置包括：浮点型DSPTMS320F28335、SEED-XDS510PLUS仿真器、光纤陀螺、加速度计、SCI通信电路、RS232及RS422转USB通信串口、上位导航计算机、数据电缆部分、电源。以DSPTMS320F28335作为控制器，将全自主捷联惯性导航算法嵌入到浮点型DSP内的软件实现，实时采集、分析惯性测量单元IMU中光纤陀螺和加速度计的输出信号并控制算法运行，以保证全自主捷联惯性导航系统的高速处理能力、高实时性、高精度、高性能、低成本、低功耗。惯性测量IMU单元中，光纤陀螺仪的零点偏置稳定性是0.5度/h，零点偏置重复性是0.5度/h，加速度计的零点偏置小于1mg，零点偏置稳定性小于1mg，数据更新速率100HZ，波特率115200bps；数据电缆部分，供电电压VDC9至36VDC可选，输出方式RS-232、USB或者RS-422，可通过具体的命令协议，在上位机或者DSP软件部分，得到IMU信息。5V2A电源与浮点型DSP模块连接，18VDC电源与导航系统连接。数字信号处理器选择TI公司的TMS320F28335浮点DSP控制器作为硬件平台，实时性好，精确度高，数据处理效率高，用来控制数据采集和算法解算；仿真器使用TI公司的SEED-XDS510PLUS，用于在CCS5.3集成开发环境内，将算法嵌入到DSP内，；DSP模块与导航系统通过RS232通信串口SCI通信方式连接，上位导航计算机与导航系统通过RS422转USB通信串口连接。

本发明的优点在于：

(1)针对需要外界辅助信息提供当地地理纬度信息的导航环境，本发明提出全自主捷联惯性导航算法。不需要外界辅助信息，仅利用系统自身的惯性测量单元输出，首先以惯性空间为参考基准，利用重力投影，确定晃动基座下载体初始时刻所在位置的纬度信息，然后带入计算所得纬度信息，基于双矢量定姿原理，利用重力加速度在惯性空间方位的改变包含地球北向信息这一特性，求解惯性系下粗略的初始姿态阵，最后在惯性系下粗略的初始姿态阵已求得的基础上，建立了纬度未知动态干扰条件下的精对准误差模型，采用基于新息的自适应滤波方法解算载体的失准角，用失准角修正捷联姿态矩阵，有效隔离晃动对载体初始对准的影响，完成初始对准，进入自主导航状态。系统不依赖外界赖辅助信息，仅利用系统自身的陀螺仪和加速度计等惯性测量器件输出完成导航解算过程，不向外界辐射任何能量，隐蔽性好，抗干扰性强，能够全天时、全天候为载体提供完备的运动信息，完成全自主导航。

(2)针对捷联惯性导航系统对数据处理能力、实时性、稳定性、小型化、低功耗的进一步高要求，从处理器硬件平台选择上考虑，本发明应用TI公司的TMS320F28335浮点型DSP控制器作为硬件平台，来控制数据采集和算法解算，完成载体的实时快速捷联惯性导航。系统在大量的浮点运算时有很大优势，对复杂的算法程序执行效率高，TMS320F28335型数字信号处理器的高速处理能力、高精度、高性能、低成本、低功耗、外设集成度高、数据以及程序存储量大、32位浮点运算单元等性能在解算方面得到了充分发挥，实现高精确度、高数据处理效率地在动态干扰情况计算纬度信息，有效隔离晃动对载体初始对准的影响，实现捷联惯导系统的实时高精度快速导航。

(3)在本发明方法的最后，算法解算姿态信息与实际姿态信息进行比较，增加了对算法和系统的验证部分，可以实时监控并证明本系统的可行性和有效性，实现全自主捷联惯性导航系统的实时高精度快速解算。

附图说明

图1为本发明基于DSPTMS320F28335的全自主捷联惯性导航系统装置总体结构框图；

图2为本发明基于DSPTMS320F28335的全自主捷联惯性导航系统装置总体实物图；

图3为本发明系统的流程图；

图4为惯性坐标系下纬度估计示意图；

图5为基于新息的自适应滤波方程计算流程示意图；

图6为纬度信息仿真结果示意图；

图7为纬度信息仿真结果分析示意图；

图8为惯性系粗对准仿真结果示意图；

图9为惯性系粗对准仿真结果分析示意图；

图10为精对准仿真结果分析示意图；

图11为上位机采集真实姿态信息页面示意图；

图12为实验结果示意图。

图13为dsp算法解算流程图。

具体实施方式

本发明是基于32位浮点型DSPTMS320F28335控制数据采集和算法解算的实时快全自主捷联惯性导航系统，下面结合本发明系统流程图3对本发明的具体实施步骤进行详细的描述：

在下文实施步骤的详细描述中坐标系定义如下：地球坐标系e系，原点选取地球中心，X轴位于赤道平面内，从地心指向载体所在点经线，Z轴沿地球自转轴方向，随地球自转而转动，X轴、Y轴和Z轴构成右手坐标系，随地球自转而转动；地心惯性坐标系i系，是在粗对准起始时刻将地球坐标系e系惯性凝固后形成的坐标系；导航坐标系n系，即导航基准的坐标系，导航相关运算都在该坐标系下进行，原点位于舰载机重心，X轴指向东向(E)，Y轴指向北向(N)，Z轴指向天向(U)；载体坐标系b系，原点位于舰载机重心，X轴、Y轴、Z轴分别沿舰载机机体横轴指向右、沿纵轴指向前、沿立轴指向上；载体惯性坐标系ib0系，是在对准初始时刻将载体坐标系经惯性凝固后的坐标系。

步骤1：系统准备阶段，硬件模块连接，DSP上电，导航系统预热；

步骤2：上位导航计算机开启，CCS5.3集成开发环境打开，建立工程并编写编译算法程序，链接SEED-XDS510PLUS仿真器，将算法嵌入到DSPTMS320F28335内的软件，DSP控制全自主捷联惯性导航算法运行；

步骤3：DSP初始化，完成系统及各个模块的初始化工作，正确配置状态寄存器、中断、引脚、看门狗等整个系统；

步骤4：导航算法初始化，正确设置相关参数；

步骤5：DSPTMS320F28335控制导航系统，利用SCI通信，通过RS232通信串口，向导航系统发送命令协议“$GCCMD,OUTPUT,COMA,GTIMU,0.01*FF”，以100HZ的数据更新速率，115200bps的波特率，采集惯性测量单元IMU数据，分析并解包得到光纤陀螺仪的三轴测量值gyout_b和加速度计的三轴测量值acout_b。其SCI通信电路原理图如图6；

步骤6：不依外界赖辅助信息，仅利用系统自身的惯性测量单元输出，在得到光纤陀螺仪的三轴测量值gyout_b和加速度计的三轴测量值acout_b后，以惯性空间为参考基准，采用重力投影，利用两个不同时刻惯性坐标系下重力加速度之间的夹角求取载体所在位置的纬度信息，隔离晃动干扰对纬度求解的影响，完成晃动基座下运载体初始时刻所在位置纬度信息的求解过程，其具体步骤如下：

设起始时刻t₁惯性坐标系i系与载体坐标系b系重合，载体坐标系随地球一起转动，转动至时间t₂，惯性坐标系没有转动保持不变。由于地球的转动，重力加速度g在惯性空间i系内的方向投影由t₁时刻的gⁱ(t₁)变成了t₂时刻的gⁱ(t₂)，设gⁱ(t₁)与gⁱ(t₂)之间的夹角为θ。如图4所示，θ与纬度L之间存在几何关系，因此通过求取θ可以间接地求取纬度信息L，下面叙述具体过程。

确定起始时刻t₁和纬度估计时间t₂，ω_ie为地球坐标系e系相对于地心惯性坐标系i系的地球自转角速度大小，在t₁、t₂时间差内地球转过的角度α可求得：

α＝ω_ie(t₂-t₁) (1)

在t₁时刻，i系与b系重合，重力加速度g在b系内的方向投影为g^b(t₁)，重力加速度g在惯性空间内的方向gⁱ(t₁)可求得：

gⁱ(t₁)＝g^b(t₁)＝-acout_b(t₁) (2)

在t₂时刻，重力加速度g在b系内的方向投影为g^b(t₂)，重力加速度g在惯性空间内的方向gⁱ(t₂)由下式求取：

式中，为t₂时刻b系相对于i系的关系四元数值，是的共轭，可由下式微分方程式求得：

式中，为光纤陀螺输出值，表示b系相对于i系的载体运动角速率在b系内的投影。

由(2)式求解的矢量gⁱ(t₁)和(3)式求解的矢量gⁱ(t₂)，可以求得两矢量gⁱ(t₁)与gⁱ(t₂)之间的夹角θ：

式中，·为内积符号，||gⁱ(t₁)||表示求取gⁱ(t₁)的模值，||gⁱ(t₂)||表示求取gⁱ(t₂)的模值。

θ与纬度L之间存在几何关系，下面通过求取θ来间接地求取纬度信息L：

由图4可知几何关系如下：

|Ao'|＝|Bo'|＝|Ao|cos L (6)

将(6)式和(7)式代入(8)式，可求得晃动基座下纬度信息L的计算公式：

将(1)式所求得角度α和(5)式所求得夹角θ代入(9)式，即可求得纬度信息L，隔离了晃动干扰的影响，完成了晃动基座下纬度值的估计过程。

步骤7：不依外界赖辅助信息，带入计算所得纬度信息，利用系统自身的惯性测量单元输出，基于双矢量定姿原理，利用重力加速度在惯性空间方位的改变包含地球北向信息这一特性，求解惯性系下粗略的初始姿态阵，姿态误差角可视为小角，接着采用基于新息的自适应滤波方法获取载体的失准角，用失准角修正捷联姿态矩阵，得到三轴姿态信息航向角ψ、俯仰角θ、横滚角γ，实时记录数据，完成初始对准，进入全自主导航状态。其具体步骤如下：

1)载体所在位置的纬度信息计算完成后，利用计算所得纬度值，进入晃动基座初始对准粗对准过程，以惯性空间为参考基准，利用双矢量定姿算法计算出的初始姿态阵粗略值，完成粗对准步骤。

在惯性系下捷联惯导系统的自对准粗对准算法中，以惯性空间为参考基准，导航坐标系n系与载体坐标系b系之间的初始姿态矩阵的实现分解成和常值矩阵三部分的求解过程，姿态矩阵的分解表达式为：

其中，导航坐标系n系与地心惯性坐标系i系之间的转换矩阵由所求得的运载体所在对准点的地理位置纬度信息和初始对准时间确定：

导航坐标系n系与地球坐标系e系之间的转换矩阵：

地球坐标系e系与地心惯性坐标系i系之间的转换矩阵：

载体坐标系b系与载体惯性坐标系为ib0系之间的转换矩阵的初值其更新利用陀螺测量输出角速度信息跟踪求解，且其更新求解过程隐含了对角运动的隔离：

式中，为向量的叉乘反对称矩阵。

地心惯性坐标系i系与载体惯性坐标系ib0系之间的转动关系是不随时间变化的常值矩阵，

其确定过程是姿态矩阵估算问题的关键，是初始对准的核心。

因为地心惯性坐标系i系内的速度投影Vⁱ与载体惯性坐标系ib0系内的速度投影有关系式因此可根据速度在两惯性坐标系中的投影Vⁱ和构造矢量实现计算。在两个不同时刻，根据i系下的重力加速度的积分Vⁱ和ib0系下的重力加速度的积分分别构建矢量，采用双矢量定姿法求解矩阵求逆得到具体求解过程如下：

设n系下的重力加速度值为g_n，i系下的重力加速度值gⁱ为：

设Δt_k＝t_k-t₀，积分得i系下的速度投影Vⁱ：

在粗对准过程中忽略加速度计的刻度系数误差和安装误差角，包含重力加速度g^b、载体震荡纵荡横荡引起的干扰加速度杆臂干扰加速度如下式所示：

设

则i_b0坐标系下陀螺输出积分所得速度的投影为：

由于为线振动干扰速度，与重力加速度积分的速度相比较小，粗对准过程中可以将其忽略。同理，由纬度误差引起的重力加速度误差引起的误差可以忽略，故求解i_b0系下的速度的公式可化简为：

在两个不同时刻，根据式(15)求得i系下的速度投影Vⁱ，根据式(17)和式(18)求得i_b0系下的速度后，分别构建矢量，采用双矢量定姿法求解

首先，在tk1、tk2时刻(t0<tk1<tk2)有：

构造矩阵：

由此，采用双矢量定姿法求解过程完成。将式(11)式(12)所得式(13)所得式(20)所得代入式(10)计算，即可计算出完成惯性系下捷联惯导系统的粗对准对准过程，得到粗略的初始姿态阵。

2)通过上述粗对准方法计算出姿态阵的粗略值，姿态误差角可视为小角，在此基础上利用基于新息的自适应滤波获取载体的失准角，用失准角修正捷联姿态矩阵，对姿态阵做精确估计以进一步提高对准精度，结束自对准，进入自主导航状态。

在精对准阶段，姿态矩阵分为3个矩阵求取，如下所示：

式中，和按照上述粗对准方案中的计算方法进行求取。因此，的确定关键在于地心惯性坐标系i系与载体坐标系b系之间的转换矩阵的确定。

设i'系是由陀螺输出计算得到的存在失准角的惯性坐标系，i系是真实的惯性坐标系。由于惯性器件存在各种误差，i'系和i系并不重合，设i'系与i系之间的失准角为其三轴值分别表示为和i'系与i系之间的转换关系为i'系与b系之间的转换矩阵为

其中，是通过粗对准过程建立的载体坐标系与计算惯性坐标系之间的转换矩阵，作为精对准开始时的转换矩阵。根据陀螺和加速度计输出通过四元数算法求得。因此，的确定关键在于从i系到i'系的转换矩阵的求取，也就是说关键在于i'系与i系之间的失准角的求取。

针对上述所需过程，在这一步骤中，首先建立系统状态方程和量测方程，创建系统状态空间模型，利用基于新息的自适应滤波器精确估计载体的失准角然后用失准角修正捷联姿态矩阵，计算准确地姿态矩阵，完成自对准的精对准过程，进入自主导航状态。

①建立系统状态方程

在精对准过程中，加速度计的刻度系数误差和安装误差不可忽略。设加表常值偏置向量为加速度计的常值偏置误差，其三轴值分别为和为高斯白噪声，其三轴值分别为和加表刻度系数误差矩阵为δK_A＝diag(δK_Ax,δK_Ay,δK_Az)，加表安装误差矩阵为则加速度计输出的比力在i′系中的投影为：

忽略二阶小项之后，可得：

设为等效干扰加速度。将上式整理可得：

对上式积分，可得：

设

设惯性系i系下的速度误差为δVⁱ，δVⁱ的三轴值分别为和由式(32)得到惯性系下的速度误差方程：

设陀螺的常值漂移和量测高斯白噪声分别为ε^b和ω^b，ε^b的三轴值分别表示为和且ω^b的三轴值分别表示为和由姿态转换阵和的微分方程，得到惯性系下的失准角方程：

根据惯性系下的速度误差方程和失准角方程，系统的状态方程是如下形式：

式中，状态矢量设为：

系统噪声是：

状态转移矩阵：

系统噪声矩阵：

②建立系统量测方程

设量测矢量表示为：

根据式(27)至式(32)，得到系统的量测方程：

式中，量测矩阵为H＝[I_3×3 0_3×9]；为不确定性量测干扰，主要由垂荡、纵荡、横荡产生的干扰速度构成；V_w为量测高斯白噪声。

③基于新息的自适应滤波算法

系统状态方程和量测方程完成求解，由(35)时和(41)式，系统的状态空间模型也就建立了。系统空间模型离散化后的形式如下：

式中，X_k为k时刻的状态矢量；Z_k为k时刻的量测矢量；F_KK-1为k-1时刻到k时刻的一步转移矩阵；Γ_k-1为k-1时刻的系统噪声矩阵；H_k为k时刻的量测矩阵；W_k-1和V_k为随机干扰的白噪声；U_k为不确定性量测干扰。

经典卡尔曼滤波采用新息的理论协方差计算滤波增益矩阵K_k，但是新息的理论协方差无法直接反映出外部量测噪声的变化，所以该算法使用新息的实际协方差代替理论协方差来计算滤波增益矩阵K_k。

对新息进行加权求和：

由极大似然估计可知系统噪声协方差矩阵R_k的估计值为：

用此处R_k的估计值代替经典卡尔曼滤波公式中的R_k,可得到：

由于新息的不稳定性，进一步对其采用自适应的方式进行计算：

式中α为调节因子，用于调节新的新息在新息加权求和过程中所占的比例，可根据经验选择默认值。当新的新息比新息边界大的时候，认为此次的新息误差过大，降低其比例降低α，反之α采用默认值，以此抑制滤波器发散。

α的计算方法如下：

其中为允许的误差边界。从公式可以看出，超过误差允许边界，距离越远，α越小，则新的新息在计算过程中所占比例越小。

结合图5可知，系统的基于新息的自适应滤波方程如下：

状态一步预测方程：

均方误差一步预测方程：

滤波增益方程：

状态估计方程：

均方误差估计方程：P_K＝(I-K_KH_K)P_KK-1 (48e)

的计算方程：

α的计算方程：

新息的计算方程：

利用基于新息的自适应滤波可估计出系统状态矢量，系统状态矢量的第四项、第五项、第六项即为载体的失准角

④用失准角修正捷联姿态矩阵

从i系到i'系的转换矩阵：

代入(22)式求得与代入(21)式求得精确的完成了用失准角修正捷联姿态矩阵，对姿态矩阵做了精确估计，结束了自对准，进入自主导航状态。

步骤8：终止程序，关闭系统。

本发明的有益效果如下：

本发明首先在仿真环境下对全自主捷联惯性导航算法进行仿真实验，证明该算法的有效性。然后通过真实实验对本发明提出的基于DSPTMS320F28335的全自主捷联惯性导航系统进行验证，实验结果与实际载体姿态信息对比，证明本系统的可行性和有效性，证明本发明实时性好，精确度高，数据处理效率高，能有效在动态干扰情况下计算纬度信息，能有效隔离晃动对载体初始对准的影响，有效实现捷联惯性导航系统的实时快速全自主导航。

(1)在仿真环境下对该算法进行仿真实验，验证算法。

首先进行仿真环境设置：

①设载体受到外界扰动，其航向角ψ、俯仰角θ、横滚角γ作周期性变化，如下：

②存在横荡、纵荡和垂荡引起的线速度：

A_Dx＝0.02m，A_Dy＝0.03m，A_Dz＝0.3m；ω_Di＝2π/TDi,且T_Dx＝7s，T_Dy＝6s，T_Dz＝8s；为[0,2π]上服从均匀分布的随机相位；

③假设陀螺三个方向轴上的随机常值漂移为0.01°/h，陀螺的随机游走系数为加速度计三个方向轴上的常值偏差为1×10-4g，加速度计量测白噪声标准差为

④常数设置

赤道半径为R_e＝6378165.0m，椭球扁率为e＝3.352e-3，由万有引力可得地球表面重力加速度为g₀＝9.78049，地球自转角速度是单位是7.2921158e-5；常数π＝3.1415926

算法仿真结果如下：

纬度信息真值为北纬39.78度。维度信息仿真估计进行了十次，每次纬度估计仿真时间为110s，且t₁取40s，t₂取110s。仿真结果如图6所示。统计出十次纬度估计仿真实验对应纬度误差的均值、最大值、最小值，如图7所示。可以清楚地看出，该纬度估计结果满足误差要求，可为后续自对准提供初始纬度信息。

粗对准仿真采用惯性系粗对准算法，进行了50次，每次粗对准仿真时间为80s，且tk1取40s，tk2取80s。仿真结果如图8所示。以粗对准结束时刻的姿态误差角作为衡量粗对准精度的指标，统计出五十次粗对准仿真实验对应的姿态误差角的均值、最大值和最小值，如图9所示。可以清楚地看出，航向姿态误差小于0.76度，俯仰姿态误差小于0.23度，横滚姿态误差小于0.1度，该粗对准的结果满足了精对准的要求，可为后续精对准提供初始姿态。

进行了840s精对准仿真，精对准仿真结果如图10所示。以姿态误差角的估计误差作为衡量精对准的指标。可以看得出，俯仰姿态在60s内完成精对准，稳定在0.07度；横滚姿态在60s内完成精对准，稳定在0.045度；航向姿态在210s内完成精对准，精度为0.09度以内。

由仿真结果可知，在动态干扰情况下，不依赖任何外界辅助信息，本算法可以自主估计出载体所在位置的纬度信息值，自主解算精度要求范围内的粗略初始姿态阵，快速完成精对准过程，完成全自主自对准，精度达到了要求，进入自主导航状态。

(2)通过真实实验对本发明提出的基于DSPTMS320F28335的全自主捷联惯性导航系统进行验证。真实试验中，不提任何供外界辅助信息，系统装置放置在车上，有人员上下车、开关车门、对车进行晃动等干扰。实验历时980s，试验地点在北京工业大学羽毛球馆南广场位置。上位导航计算机控制导航系统，通过RS422转USB串口，向导航系统发送命令协议“$GCCMD,OUTPUT,COMC,GPFPD,0.01*FF”，以100HZ的数据更新速率，115200bps的波特率，采集航向精度达0.1度、姿态精度达0.05度的实际三轴姿态信息，其上位机采集页面示意图如图11。解算获得的载体姿态信息与本步骤中得到的高精度真实载体姿态信息做比较，证明本算法和系统的可行性和有效性。

系统实验结果如下：

纬度信息真值为北纬39.8709度。维度信息估计在120s内完成，t₁取40s，t₂取120s。仿真估计值为北纬39.706790度，仿真估计误差为0.164110度，在误差允许范围内，可为后续自对准提供初始纬度信息。

粗对准过程采用惯性系粗对准算法，历时80s，tk1取40s，tk2取80s。粗对准实验结果为航向姿态180.122500度、俯仰姿态0.513390度、横滚姿态0.082630度，此时刻真实姿态信息输出值为航向姿态179.87度、俯仰姿态0.40度、横滚姿态0.21度，以姿态误差角的估计误差作为衡量精对准的指标，航向姿态误差小于0.252500度，俯仰姿态误差小于0.113390度，横滚姿态误差小于0.12737度，该粗对准的结果满足了精对准的要求，可为后续精对准提供初始姿态。

精对准过程历时780s，结果如图12。以姿态误差角的估计误差作为衡量精对准的指标。可以看得出，俯仰姿态、横滚姿态和航向姿态均快速完成对准工作，俯仰姿态精度达到0.06度，横滚姿态精度达到0.07度，航向姿态精度达到0.15度，收敛速度、稳态误差均符合要求，完成自对准过程，进入自主导航状态。

实验结果表明：不依赖任何外界辅助信息的情况下，针对导航系统对数据处理能力、实时性、稳定性、小型化、低功耗的进一步高要求，本发明基于DSPTMS320F28335的全自主捷联惯性导航系统仅利用自身的惯性测量单元输出，能自主有效在动态干扰情况下计算纬度信息，能自主有效隔离晃动对载体初始对准的影响，能自主有效实现捷联惯性导航系统的实时高精度快速自对准，进入全自主导航状态。

Claims (1)

1.基于DSPTMS320F28335的全自主捷联惯性导航系统，其特征在于：该系统包括：浮点型DSPTMS320F28335、SEED-XDS510PLUS仿真器、光纤陀螺、加速度计、SCI通信电路、RS232及RS422转USB通信串口、上位导航计算机、数据电缆部分、电源；以DSPTMS320F28335作为控制器，将全自主捷联惯性导航算法嵌入到浮点型DSP内的软件实现，实时采集、分析惯性测量单元IMU中光纤陀螺和加速度计的输出信号并控制算法运行，以保证全自主捷联惯性导航系统的高速处理能力、高实时性、高精度、高性能、低成本、低功耗；惯性测量IMU单元中，光纤陀螺仪的零点偏置稳定性是0.5度/h，零点偏置重复性是0.5度/h，加速度计的零点偏置小于1mg，零点偏置稳定性小于1mg，数据更新速率100HZ，波特率115200bps；数据电缆部分，供电电压VDC9至36VDC可选，输出方式RS-232、USB或者RS-422，可通过具体的命令协议，在上位机或者DSP软件部分，得到IMU信息；5V2A电源与浮点型DSP模块连接，18VDC电源与导航系统连接；数字信号处理器选择TI公司的TMS320F28335浮点DSP控制器作为硬件平台，实时性好，精确度高，数据处理效率高，用来控制数据采集和算法解算；仿真器使用TI公司的SEED-XDS510PLUS，用于在CCS5.3集成开发环境内，将算法嵌入到DSP内；DSP模块与导航系统通过RS232通信串口SCI通信方式连接，上位导航计算机与导航系统通过RS422转USB通信串口连接；

坐标系定义如下：地球坐标系e系，原点选取地球中心，X轴位于赤道平面内，从地心指向载体所在点经线，Z轴沿地球自转轴方向，随地球自转而转动，X轴、Y轴和Z轴构成右手坐标系，随地球自转而转动；地心惯性坐标系i系，是在粗对准起始时刻将地球坐标系e系惯性凝固后形成的坐标系；导航坐标系n系，即导航基准的坐标系，导航相关运算都在该坐标系下进行，原点位于舰载机重心，X轴指向东向，Y轴指向北向，Z轴指向天向；载体坐标系b系，原点位于舰载机重心，X轴、Y轴、Z轴分别沿舰载机机体横轴指向右、沿纵轴指向前、沿立轴指向上；载体惯性坐标系ib0系，是在对准初始时刻将载体坐标系经惯性凝固后的坐标系；

步骤1：系统准备阶段，硬件模块连接，DSP上电，导航系统预热；

步骤2：上位导航计算机开启，CCS5.3集成开发环境打开，建立工程并编写编译算法程序，链接SEED-XDS510PLUS仿真器，将算法嵌入到DSPTMS320F28335内的软件，DSP控制全自主捷联惯性导航算法运行；

步骤3：DSP初始化，完成系统及各个模块的初始化工作，正确配置状态寄存器、中断、引脚与看门狗；

步骤4：导航算法初始化，正确设置相关参数；

步骤5：DSPTMS320F28335控制导航系统，利用SCI通信，通过RS232通信串口，向导航系统发送命令协议“$GCCMD,OUTPUT,COMA,GTIMU,0.01*FF”，以100HZ的数据更新速率，115200bps的波特率，采集惯性测量单元IMU数据，分析并解包得到光纤陀螺仪的三轴测量值gyout_b和加速度计的三轴测量值acout_b；

步骤6：不依赖外界辅助信息，仅利用系统自身的惯性测量单元输出，在得到光纤陀螺仪的三轴测量值gyout_b和加速度计的三轴测量值acout_b后，以惯性空间为参考基准，采用重力投影，利用两个不同时刻惯性坐标系下重力加速度之间的夹角求取载体所在位置的纬度信息，隔离晃动干扰对纬度求解的影响，完成晃动基座下运载体初始时刻所在位置纬度信息的求解过程，其具体步骤如下：

设起始时刻t₁惯性坐标系i系与载体坐标系b系重合，载体坐标系随地球一起转动，转动至时间t₂，惯性坐标系没有转动保持不变；由于地球的转动，重力加速度g在惯性空间i系内的方向投影由t₁时刻的gⁱ(t₁)变成了t₂时刻的gⁱ(t₂)，设gⁱ(t₁)与gⁱ(t₂)之间的夹角为θ；θ与纬度L之间存在几何关系，因此通过求取θ可以间接地求取纬度信息L，下面叙述具体过程；

确定起始时刻t₁和纬度估计时间t₂，ω_ie为地球坐标系e系相对于地心惯性坐标系i系的地球自转角速度大小，在t₁、t₂时间差内地球转过的角度α可求得：

α＝ω_ie(t₂-t₁) (1)

在t₁时刻，i系与b系重合，重力加速度g在b系内的方向投影为g^b(t₁)，重力加速度g在惯性空间内的方向gⁱ(t₁)可求得：

gⁱ(t₁)＝g^b(t₁)＝-acout_b(t₁) (2)

在t₂时刻，重力加速度g在b系内的方向投影为g^b(t₂)，重力加速度g在惯性空间内的方向gⁱ(t₂)由下式求取：

式中，为t₂时刻b系相对于i系的关系四元数值，是的共轭，可由下式微分方程式求得：

式中，为光纤陀螺输出值，表示b系相对于i系的载体运动角速率在b系内的投影；

由(2)式求解的矢量gⁱ(t₁)和(3)式求解的矢量gⁱ(t₂)，求得两矢量gⁱ(t₁)与gⁱ(t₂)之间的夹角θ：

式中，·为内积符号，||gⁱ(t₁)||表示求取gⁱ(t₁)的模值，||gⁱ(t₂)||表示求取gⁱ(t₂)的模值；

θ与纬度L之间存在几何关系，下面通过求取θ来间接地求取纬度信息L：

几何关系如下：

|Ao′|＝|Bo′|＝|Ao|cos L (6)

将(6)式和(7)式代入(8)式，可求得晃动基座下纬度信息L的计算公式：

将(1)式所求得角度α和(5)式所求得夹角θ代入(9)式，即可求得纬度信息L，隔离了晃动干扰的影响，完成了晃动基座下纬度值的估计过程；

步骤7：不依赖外界辅助信息，带入计算所得纬度信息，利用系统自身的惯性测量单元输出，基于双矢量定姿原理，利用重力加速度在惯性空间方位的改变包含地球北向信息这一特性，求解惯性系下粗略的初始姿态阵，姿态误差角可视为小角，接着采用基于新息的自适应滤波方法获取载体的失准角，用失准角修正捷联姿态矩阵，得到三轴姿态信息航向角ψ、俯仰角θ、横滚角γ，实时记录数据，完成初始对准，进入全自主导航状态；其具体步骤如下：

1)载体所在位置的纬度信息计算完成后，利用计算所得纬度值，进入晃动基座初始对准粗对准过程，以惯性空间为参考基准，利用双矢量定姿算法计算出的初始姿态阵粗略值，完成粗对准步骤；

在惯性系下捷联惯导系统的自对准粗对准算法中，以惯性空间为参考基准，导航坐标系n系与载体坐标系b系之间的初始姿态矩阵的实现分解成和常值矩阵三部分的求解过程，姿态矩阵的分解表达式为：

其中，导航坐标系n系与地心惯性坐标系i系之间的转换矩阵由所求得的运载体所在对准点的地理位置纬度信息和初始对准时间确定：

导航坐标系n系与地球坐标系e系之间的转换矩阵：

地球坐标系e系与地心惯性坐标系i系之间的转换矩阵：

载体坐标系b系与载体惯性坐标系为ib0系之间的转换矩阵的初值其更新利用陀螺测量输出角速度信息跟踪求解，且其更新求解过程隐含了对角运动的隔离：

式中，为向量的叉乘反对称矩阵；

地心惯性坐标系i系与载体惯性坐标系ib0系之间的转动关系是不随时间变化的常值矩阵，其确定过程是姿态矩阵估算问题的关键，是初始对准的核心；

因为地心惯性坐标系i系内的速度投影Vⁱ与载体惯性坐标系ib0系内的速度投影有关系式因此可根据速度在两惯性坐标系中的投影Vⁱ和构造矢量实现计算；在两个不同时刻，根据i系下的重力加速度的积分Vⁱ和ib0系下的重力加速度的积分分别构建矢量，采用双矢量定姿法求解矩阵求逆得到具体求解过程如下：

设n系下的重力加速度值为gⁿ，i系下的重力加速度值gⁱ为：

设Δt_k＝t_k-t₀，积分得i系下的速度投影Vⁱ：

在粗对准过程中忽略加速度计的刻度系数误差和安装误差角，包含重力加速度g^b、载体震荡纵荡横荡引起的干扰加速度杆臂干扰加速度如下式所示：

设

则i_b0坐标系下陀螺输出积分所得速度的投影为：

由于为线振动干扰速度，与重力加速度积分的速度相比较小，粗对准过程中可以将其忽略；同理，由纬度误差引起的重力加速度误差引起的误差可以忽略，故求解i_b0系下的速度的公式可化简为：

在两个不同时刻，根据式(15)求得i系下的速度投影Vⁱ，根据式(17)和式(18)求得i_b0系下的速度后，分别构建矢量，采用双矢量定姿法求解

首先，在tk1、tk2时刻，其中t0＜tk1＜tk2，有：

构造矩阵：

由此，采用双矢量定姿法求解过程完成；将式(11)式(12)所得式(13)所得式(20)所得代入式(10)计算，即可计算出完成惯性系下捷联惯导系统的粗对准对准过程，得到粗略的初始姿态阵；

2)通过上述粗对准方法计算出姿态阵的粗略值，姿态误差角可视为小角，在此基础上利用基于新息的自适应滤波获取载体的失准角，用失准角修正捷联姿态矩阵，对姿态阵做精确估计以进一步提高对准精度，结束自对准，进入自主导航状态；

在精对准阶段，姿态矩阵分为3个矩阵求取，如下所示：

式中，和按照上述粗对准方案中的计算方法进行求取；因此，的确定关键在于地心惯性坐标系i系与载体坐标系b系之间的转换矩阵的确定；

设i′系是由陀螺输出计算得到的存在失准角的惯性坐标系，i系是真实的惯性坐标系；由于惯性器件存在各种误差，i′系和i系并不重合，设i′系与i系之间的失准角为其三轴值分别表示为和i′系与i系之间的转换关系为i′系与b系之间的转换矩阵为

其中，是通过粗对准过程建立的载体坐标系与计算惯性坐标系之间的转换矩阵，作为精对准开始时的转换矩阵；根据陀螺和加速度计输出通过四元数算法求得；因此，的确定关键在于从i系到i′系的转换矩阵的求取，也就是说关键在于i′系与i系之间的失准角的求取；

根据所需过程，在这一步骤中，首先建立系统状态方程和量测方程，创建系统状态空间模型，利用基于新息的自适应滤波器精确估计载体的失准角然后用失准角修正捷联姿态矩阵，计算准确地姿态矩阵，完成自对准的精对准过程，进入自主导航状态；

①建立系统状态方程

在精对准过程中，加速度计的刻度系数误差和安装误差不可忽略；设加表常值偏置向量 为加速度计的常值偏置误差，其三轴值分别为和为高斯白噪声，其三轴值分别为和加表刻度系数误差矩阵为δK_A＝diag(δK_Ax，δK_Ay，δK_Az)，加表安装误差矩阵为则加速度计输出的比力在i′系中的投影为：

忽略二阶小项之后，可得：

设为等效干扰加速度；将上式整理可得：

对上式积分，可得：

设

设惯性系i系下的速度误差为δVⁱ，δVⁱ的三轴值分别为和由式(32)得到惯性系下的速度误差方程：

设陀螺的常值漂移和量测高斯白噪声分别为ε^b和ω^b，ε^b的三轴值分别表示为和且ω^b的三轴值分别表示为和由姿态转换阵和的微分方程，得到惯性系下的失准角方程：

根据惯性系下的速度误差方程和失准角方程，系统的状态方程是如下形式：

式中，状态矢量设为：

系统噪声是：

状态转移矩阵：

系统噪声矩阵：

②建立系统量测方程

设量测矢量表示为：

根据式(27)至式(32)，得到系统的量测方程：

式中，量测矩阵为H＝[I_3×3 0_3×9]；为不确定性量测干扰，主要由垂荡、纵荡、横荡产生的干扰速度构成；V_w为量测高斯白噪声；

③基于新息的自适应滤波算法

系统状态方程和量测方程完成求解，由(35)时和(41)式，系统的状态空间模型也就建立了；系统空间模型离散化后的形式如下：

式中，z_l为k时刻的状态矢量；z_b为k时刻的量测矢量；F_k|k-1为k-1时刻到k时刻的一步转移矩阵；ω₁＝3ω₂＝ω为k-1时刻的系统噪声矩阵；l为k时刻的量测矩阵；x_l和ω₁为随机干扰的白噪声；l为不确定性量测干扰；

经典卡尔曼滤波采用新息的理论协方差计算滤波增益矩阵K_k，但是新息的理论协方差无法直接反映出外部量测噪声的变化，所以该算法使用新息的实际协方差代替理论协方差来计算滤波增益矩阵K_k；

对新息进行加权求和：

由极大似然估计可知系统噪声协方差矩阵R_k的估计值为：

用此处R_k的估计值代替经典卡尔曼滤波公式中的R_k，可得到：

由于新息的不稳定性，进一步对其采用自适应的方式进行计算：

式中α为调节因子，用于调节新的新息在新息加权求和过程中所占的比例，可根据经验选择默认值；当新的新息比新息边界大的时候，认为此次的新息误差过大，降低其比例降低α，反之α采用默认值，以此抑制滤波器发散；

α的计算方法如下：

其中为允许的误差边界；从公式可以看出，超过误差允许边界，距离越远，α越小，则新的新息在计算过程中所占比例越小；

系统的基于新息的自适应滤波方程如下：

状态一步预测方程：

均方误差一步预测方程：

滤波增益方程：

状态估计方程：

均方误差估计方程：P_k＝(I-K_kH_k)P_k|_k-1 (48e)

的计算方程：

α的计算方程：

新息的计算方程：

利用基于新息的自适应滤波可估计出系统状态矢量，系统状态矢量的第四项、第五项、第六项即为载体的失准角

④用失准角修正捷联姿态矩阵

从i系到i’系的转换矩阵：

代入(22)式求得与代入(21)式求得精确的完成了用失准角修正捷联姿态矩阵，对姿态矩阵做了精确估计，结束了自对准，进入自主导航状态；

步骤8：终止程序，关闭系统。