基于dsptms320f28335的全自主捷联惯性导航系统的制作方法

基于dsptms320f28335的全自主捷联惯性导航系统的制作方法
【专利摘要】基于DSPTMS320F28335的全自主捷联惯性导航系统，本系统以DSPTMS320F28335作为控制器，将全自主捷联惯性导航算法嵌入到浮点型DSP内的软件实现，实时采集、分析惯性测量单元IMU中光纤陀螺和加速度计的输出信号并控制算法运行，以保证全自主捷联惯性导航系统的高速处理能力、高实时性、高精度、高性能、低成本、低功耗。本发明适合应用于不提供精确定位信息、受外界干扰载体姿态时刻都在发生变化的场合下载体的全自主实时捷联惯性系统，诸如直接不提供纬度信息，或者纬度信息不可测的隧道深处、深山密林、深海海底情况下的载体全自主实时捷联惯性导航。
【专利说明】
基于DSPTMS320F28335的全自主捷联惯性导航系统
技术领域
[0001 ]本发明提供的是一种基于DSPTMS320F28335的全自主捷联惯性导航系统，精确度 高，数据处理效率高，涉及算法嵌入浮点型DSPTMS320F28335内的软件实现、晃动基座下载 体所在位置炜度信息计算、动态干扰情况下捷联姿态矩阵的解算，自对准完成后的捷联惯 性自主导航。本发明适合应用于不提供精确定位信息、受外界干扰载体姿态时刻都在发生 变化的场合下载体的全自主实时捷联惯性系统，诸如直接不提供炜度信息，或者炜度信息 不可测的隧道深处、深山密林、深海海底情况下的载体全自主实时捷联惯性导航。
【背景技术】
[0002]导航就是以一定的精度、在规定的时间内将载体沿着预定的路线引导至目的地的 过程。惯性导航是一种自主式导航技术，它不依靠外界信息，也不向外界辐射任何能量，仅 利用陀螺仪和加速度计等惯性测量器件以及初始的导航信息来确定载体运行期间的各项 导航参数，隐蔽性好、抗干扰性强，能够全天时、全天候为载体提供完备的运动信息。
[0003]惯性导航中的捷联式惯性导航技术将陀螺仪和加速度计直接安装在载体上，得到 载体系下的加速度和角速度，通过导航计算机将测得的数据转换至导航坐标系完成导航， 它不需要实体的稳定平台，成本低、体积小、重量轻、可靠性高。
[0004]惯性导航系统在进入导航工作之前，需要获得初始姿态以及位置信息，即进行初 始对准过程。初始对准通常分为两个阶段:粗对准阶段和精对准阶段。诸如发动机处于高频 振动的汽车、浪涌下的舰船等受干扰较大的载体，角振动和线振动会导致基座晃动，捷联惯 导系统在自对准过程中的姿态时刻都在发生变化，初始对准的时间和精度会受到影响。因 此，在运载体晃动干扰环境下完成初始对准过程，就必须屏蔽这些无法消除的扰动影响。
[0005] 无论是导航过程还是对准过程，无论是静基座自对准方法还是动基座自对准方 法，无论粗对准还是精对准，都需要外界辅助信息提供当地准确的地理炜度信息，如果地理 炜度存在误差，将可能影响到初始对准的精度。这种依赖辅助信息的捷联惯性导航是不自 主导航，然而在深山荒野，海底等环境中，想要获得当地的地理位置信息并不是一件很容易 的事情，或者某些应用场合，如通信卫星跟踪或者武器平台的稳定，有较高的定向要求，而 没有必要提供精确的定位要求，或者在某些应急场合，如军事战争中，受到敌方攻击，需要 迅速做出反击，而定位系统来不及工作，却急需定向。在上述炜度未知的条件下，想要完成 导航工作，就必须利用系统自身的惯性测量单元输出解算出载体初始时刻所在位置的炜度 信息，利用自身解算炜度信息值完成导航过程，实现载体的全自主捷联惯性导航。
[0006] 另外，随着捷联式惯性导航技术的不断应用，对系统的精度、算法、数据处理能力、 实时性、稳定性都有了相应的进一步的要求，且大多相关应用场合都提出了系统小型化、低 功耗等要求。从处理器硬件平台选择上考虑，在大量的浮点数运算或者复杂算法运算时，对 于一般的单片机或定点型处理器，程序执行效率较低，数据更新较慢，执行周期较长，不仅 不能满足对运载体姿态矩阵实时高效地解算，同时数据处理精度低，运算能力也不能完全 满足。
[0007] 针对上述问题，本发明应用TI公司的TMS320F28335浮点型DSP控制器作为硬件平 台，来控制数据采集和算法解算，完成载体的实时快速捷联惯性导航系统。在本发明中， TMS320F28335型数字信号处理器的高速处理能力、高精度、高性能、低成本、低功耗、外设集 成度高、数据以及程序存储量大、32位浮点运算单元等性能在解算方面得到了充分发挥，从 而满足数据处理效率高，实时性好，精确度高，有效在动态干扰情况下解算炜度信息值，有 效隔离晃动对载体初始对准的影响，有效实现全自主捷联惯性导航系统的实时高精度快速 自对准，进入自主导航状态。

【发明内容】

[0008] 在本发明方法中，全自主捷联惯性导航算法嵌入到浮点型DSPTMS320F28335内的 软件实现。本发明的目的在于，针对需要外界辅助信息提供当地地理炜度信息的导航环境， 以及对导航系统数据处理能力、实时性、稳定性、小型化、低功耗的进一步高要求，提出一种 基于DSPTMS320F28335的全自主捷联惯性导航系统，不依外界赖辅助信息，仅利用系统自身 的惯性测量单元输出，高精确度、高数据处理效率地在动态干扰情况计算炜度信息，有效隔 离晃动对载体初始对准的影响，有效实现实时快速高精度全自主式导航过程。
[0009] 实现本发明目的的技术解决方案为：
[0010] 本发明提出一种基于DSPTMS320F28335的全自主捷联惯性导航系统，包括以下方 面：
[0011] (1)上位导航计算机开启，CCS5.3集成开发环境打开，建立工程并编写编译算法程 序，链接SEED-XDS510PLUS仿真器，将算法嵌入到DSPTMS320F28335内的软件，DSP控制算法 运行；
[0012] (2)32位浮点型DSPTMS320F28335控制惯性全自主导航系统，向导航系统发送命令 协议，利用SCI通信，通过RS232通信串口，采集惯性测量单元頂U中光纤陀螺和加速度计的 输出信号；
[0013] ⑶不依外界赖辅助信息，仅利用系统自身的惯性测量单元输出，以惯性空间为参 考基准，利用不同时刻重力投影之间的夹角，确定晃动基座下运载体初始时刻所在位置的 炜度信息；
[0014] (4)不依外界赖辅助信息，带入计算所得炜度信息，利用系统自身的惯性测量单元 输出，基于双矢量定姿原理，利用重力加速度在惯性空间方位的改变包含地球北向信息这 一特性，求解惯性系下粗略的初始姿态阵，接着采用基于新息的自适应滤波方法获取载体 的失准角，用失准角修正捷联姿态矩阵，得到三轴姿态信息，实时记录数据，完成初始对准， 进入全自主导航状态。
[0015] 本发明使用基于DSPTMS320F28335的全自主捷联惯性导航系统，装置总体结构框 图如图1，装置总体实物图如图2。系统装置包括：浮点型DSPTMS320F28335、SEED-XDS510PLUS仿真器、光纤陀螺、加速度计、SCI通信电路、RS232及RS422转USB通信串口、上位 导航计算机、数据电缆部分、电源。以DSPTMS320F28335作为控制器，将全自主捷联惯性导航 算法嵌入到浮点型DSP内的软件实现，实时采集、分析惯性测量单元頂U中光纤陀螺和加速 度计的输出信号并控制算法运行，以保证全自主捷联惯性导航系统的高速处理能力、高实 时性、高精度、高性能、低成本、低功耗。惯性测量IMU单元中，光纤陀螺仪的零点偏置稳定性 是0.5度/h，零点偏置重复性是0.5度/h，加速度计的零点偏置小于lmg，零点偏置稳定性小 于lmg，数据更新速率100HZ，波特率115200bps;数据电缆部分，供电电压VDC9至36VDC可选， 输出方式RS-232、USB或者RS-422，可通过具体的命令协议，在上位机或者DSP软件部分，得 到MU信息。5V2A电源与浮点型DSP模块连接，18VDC电源与导航系统连接。数字信号处理器 选择TI公司的TMS320F28335浮点DSP控制器作为硬件平台，实时性好，精确度高，数据处理 效率高，用来控制数据采集和算法解算；仿真器使用TI公司的SEED-XDS510PLUS，用于在 CCS5.3集成开发环境内，将算法嵌入到DSP内，；DSP模块与导航系统通过RS232通信串口 SCI 通信方式连接，上位导航计算机与导航系统通过RS422转USB通信串口连接。
[0016] 本发明的优点在于：
[0017] (1)针对需要外界辅助信息提供当地地理炜度信息的导航环境，本发明提出全自 主捷联惯性导航算法。不需要外界辅助信息，仅利用系统自身的惯性测量单元输出，首先以 惯性空间为参考基准，利用重力投影，确定晃动基座下载体初始时刻所在位置的炜度信息， 然后带入计算所得炜度信息，基于双矢量定姿原理，利用重力加速度在惯性空间方位的改 变包含地球北向信息这一特性，求解惯性系下粗略的初始姿态阵，最后在惯性系下粗略的 初始姿态阵已求得的基础上，建立了炜度未知动态干扰条件下的精对准误差模型，采用基 于新息的自适应滤波方法解算载体的失准角，用失准角修正捷联姿态矩阵，有效隔离晃动 对载体初始对准的影响，完成初始对准，进入自主导航状态。系统不依赖外界赖辅助信息， 仅利用系统自身的陀螺仪和加速度计等惯性测量器件输出完成导航解算过程，不向外界辐 射任何能量，隐蔽性好，抗干扰性强，能够全天时、全天候为载体提供完备的运动信息，完成 全自主导航。
[0018] (2)针对捷联惯性导航系统对数据处理能力、实时性、稳定性、小型化、低功耗的进 一步高要求，从处理器硬件平台选择上考虑，本发明应用TI公司的TMS320F28335浮点型DSP 控制器作为硬件平台，来控制数据采集和算法解算，完成载体的实时快速捷联惯性导航。系 统在大量的浮点运算时有很大优势，对复杂的算法程序执行效率高，TMS320F28335型数字 信号处理器的高速处理能力、高精度、高性能、低成本、低功耗、外设集成度高、数据以及程 序存储量大、32位浮点运算单元等性能在解算方面得到了充分发挥，实现高精确度、高数据 处理效率地在动态干扰情况计算炜度信息，有效隔离晃动对载体初始对准的影响，实现捷 联惯导系统的实时高精度快速导航。
[0019] (3)在本发明方法的最后，算法解算姿态信息与实际姿态信息进行比较，增加了对 算法和系统的验证部分，可以实时监控并证明本系统的可行性和有效性，实现全自主捷联 惯性导航系统的实时高精度快速解算。
【附图说明】
[0020] 图1为本发明基于DSPTMS320F28335的全自主捷联惯性导航系统装置总体结构框 图；
[0021] 图2为本发明基于DSPTMS320F28335的全自主捷联惯性导航系统装置总体实物图； [0022]图3为本发明系统的流程图；
[0023] 图4为惯性坐标系下炜度估计示意图；
[0024] 图5为基于新息的自适应滤波方程计算流程示意图；
[0025] 图6为炜度信息仿真结果示意图；
[0026] 图7为炜度信息仿真结果分析示意图；
[0027] 图8为惯性系粗对准仿真结果示意图；
[0028] 图9为惯性系粗对准仿真结果分析示意图；
[0029] 图10为精对准仿真结果分析示意图；
[0030] 图11为上位机采集真实姿态信息页面示意图；
[0031]图12为实验结果示意图。
[0032]图13为dsp算法解算流程图。
【具体实施方式】
[0033]本发明是基于32位浮点型DSPTMS320F28335控制数据采集和算法解算的实时快全 自主捷联惯性导航系统，下面结合本发明系统流程图3对本发明的具体实施步骤进行详细 的描述：
[0034]在下文实施步骤的详细描述中坐标系定义如下:地球坐标系e系，原点选取地球中 心，X轴位于赤道平面内，从地心指向载体所在点经线，Z轴沿地球自转轴方向，随地球自转 而转动，X轴、Y轴和Z轴构成右手坐标系，随地球自转而转动;地心惯性坐标系i系，是在粗对 准起始时刻将地球坐标系e系惯性凝固后形成的坐标系；导航坐标系n系，即导航基准的坐 标系，导航相关运算都在该坐标系下进行，原点位于舰载机重心，X轴指向东向（E)，Y轴指向 北向(N)，Z轴指向天向(U);载体坐标系b系，原点位于舰载机重心，X轴、Y轴、Z轴分别沿舰载 机机体横轴指向右、沿纵轴指向前、沿立轴指向上;载体惯性坐标系ibO系，是在对准初始时 刻将载体坐标系经惯性凝固后的坐标系。
[0035]步骤1:系统准备阶段，硬件模块连接，DSP上电，导航系统预热；
[0036]步骤2:上位导航计算机开启，CCS5.3集成开发环境打开，建立工程并编写编译算 法程序，链接SEED-XDS510PLUS仿真器，将算法嵌入到DSPTMS320F28335内的软件，DSP控制 全自主捷联惯性导航算法运行；
[0037]步骤3: DSP初始化，完成系统及各个模块的初始化工作，正确配置状态寄存器、中 断、引脚、看门狗等整个系统；
[0038]步骤4:导航算法初始化，正确设置相关参数；
[0039] 步骤5:DSPTMS320F28335控制导航系统，利用SCI通信，通过RS232通信串口，向导 航系统发送命令协议"$GCCMD，OUTPUT，COMA，GTIMU，0.01*FF"，以100HZ的数据更新速率， 115200bps的波特率，采集惯性测量单元MU数据，分析并解包得到光纤陀螺仪的三轴测量 值gyout_b和加速度计的三轴测量值acout_b。其SCI通信电路原理图如图6;
[0040]步骤6:不依外界赖辅助信息，仅利用系统自身的惯性测量单元输出，在得到光纤 陀螺仪的三轴测量值gy〇ut_b和加速度计的三轴测量值&〇〇此_13后，以惯性空间为参考基 准，采用重力投影，利用两个不同时刻惯性坐标系下重力加速度之间的夹角求取载体所在 位置的炜度信息，隔离晃动干扰对炜度求解的影响，完成晃动基座下运载体初始时刻所在 位置炜度信息的求解过程，其具体步骤如下：
[0041 ]设起始时刻t惯性坐标系i系与载体坐标系b系重合，载体坐标系随地球一起转 动，转动至时间t2，惯性坐标系没有转动保持不变。由于地球的转动，重力加速度g在惯性空 间i系内的方向投影由时刻的gUtd变成了 t2时刻的gi(t2)，设gUtO与gi(t2)之间的夹角 为0。如图4所示，0与炜度L之间存在几何关系，因此通过求取0可以间接地求取炜度信息L， 下面叙述具体过程。
[0042] 确定起始时刻tjP炜度估计时间t2，《 为地球坐标系e系相对于地心惯性坐标系 i系的地球自转角速度大小，在t、t2时间差内地球转过的角度a可求得：
[0043] a= o ie(t2~ti) (1)
[0044]在。时刻，i系与b系重合，重力加速度g在b系内的方向投影为ght)，重力加速度g 在惯性空间内的方向gi ( )可求得：
[0045] g1 (ti) = gb (ti) = -acout_b (ti) (2)
[0046]在t2时刻，重力加速度g在b系内的方向投影为gb(t2)，重力加速度g在惯性空间内 的方向gi(t2)由下式求取：
[0047] g' (/：) =(q- (/： ))* ? gh {>：) ? q (>z) = -(q (/： )V ? ac〇m _ h{t2) ? 〇;; u2) ⑴
[0048] 式中，《疼)为〇时刻b系相对于i系的关系四元数值，(q丨(yf是的共辄， q'Q >可由下式微分方程式求得：
[0050] 式中，< 为光纤陀螺输出值，表示b系相对于i系的载体运动角速率在b系内的投 影。
[0051] 由⑵式求解的矢量gUti)和⑶式求解的矢量gUt2)，可以求得两矢量gUt)与g1 (t2)之间的夹角0:
[0053]式中，?为内积符号，| IgUtOl |表示求取gUtO的模值，| |gUt2)| |表示求取g1 (t2)的模值。
[0054] 0与炜度L之间存在几何关系，下面通过求取0来间接地求取炜度信息L:
[0055] 由图4可知几何关系如下：
[0056] | Ao，| = | Bo，| = | Ao | cos L (6)
[0059]将(6)式和(7)式代入(8)式，可求得晃动基座下炜度信息L的计算公式：
[0061]将（1)式所求得角度a和(5)式所求得夹角0代入(9)式，即可求得炜度信息L，隔离 了晃动干扰的影响，完成了晃动基座下炜度值的估计过程。
[0062] 步骤7:不依外界赖辅助信息，带入计算所得炜度信息，利用系统自身的惯性测量 单元输出，基于双矢量定姿原理，利用重力加速度在惯性空间方位的改变包含地球北向信 息这一特性，求解惯性系下粗略的初始姿态阵，姿态误差角可视为小角，接着采用基于新息 的自适应滤波方法获取载体的失准角，用失准角修正捷联姿态矩阵，得到三轴姿态信息航 向角L俯仰角9、横滚角Y，实时记录数据，完成初始对准，进入全自主导航状态。其具体步 骤如下：
[0063] 1)载体所在位置的炜度信息计算完成后，利用计算所得炜度值，进入晃动基座初 始对准粗对准过程，以惯性空间为参考基准，利用双矢量定姿算法计算出的初始姿态阵粗 略值，完成粗对准步骤。
[0064] 在惯性系下捷联惯导系统的自对准粗对准算法中，以惯性空间为参考基准，导航 坐标系n系与载体坐标系b系之间的初始姿态矩阵的实现分解成和常 值矩阵三部分的求解过程，姿态矩阵的分解表达式为：
[_] q(，) <(0(〇(，） （動
[0066] 其中，导航坐标系n系与地心惯性坐标系i系之间的转换矩阵由所 求得的运载体所在对准点的地理位置炜度信息和初始对准时间确定：
[0067] 导航坐标系n系与地球坐标系e系之间的转换矩阵：
[0069]地球坐标系e系与地心惯性坐标系i系之间的转换矩阵：
[0071] 载体坐标系b系与载体惯性坐标系为ibO系之间的转换矩阵⑷的初值 = 其更新利用陀螺测量输出角速度信息<(〇跟踪求解，且其更新求解过程隐含了 对角运动的隔离：
[0072] =C^[(0l(t)x\ (|3)
[0073] 式中，[4(加]为向量<(〇的叉乘反对称矩阵。
[0074] 地心惯性坐标系i系与载体惯性坐标系ibO系之间的转动关系Ct是不随时间变化 的常值矩阵，
[0075] 其确定过程是姿态矩阵 估算问题的关键，是初始对准的核心。
[0076] 因为地心惯性坐标系i系内的速度投影V1与载体惯性坐标系ibO系内的速度投影 r-有关系式^，因此cf9可根据速度在两惯性坐标系中的投影v1和构造矢量 实现计算。在两个不同时刻，根据i系下的重力加速度的积分V1和ibO系下的重力加速度的 积分rM1分别构建矢量，采用双矢量定姿法求解矩阵求逆得到C:。具体求解过程如下：
[0077]设n系下的重力加速度值为gn，i系下的重力加速度值g1为：
[0079] 设A tk = tk-to,积分得i系下的速度投影VS
[0081] 在粗对准过程中忽略加速度计的刻度系数误差和安装误差角，如3财_6包含重力 加速度gb、载体震荡纵荡横荡引起的干扰加速度杆臂干扰加速度^4如下式所示： 、 ?
[0084]则ibQ坐标系下陀螺输出积分所得速度的投影为：
[0086]由于为线振动干扰速度，与重力加速度积分的速度相比较小，粗对准过程中 可以将其忽略。同理，由炜度误差引起的重力加速度误差引起的误差可以忽略，故求解ib〇系 下的速度的公式可化简为：
[0088]在两个不同时刻，根据式（15)求得i系下的速度投影V1，根据式（17)和式（18)求得 ib〇系下的速度"。后，分别构建矢量，采用双矢量定姿法求解gQ。
[0089] 首先，在tkl、tk2时刻（切〈七1^1〈七1^2)有：
[0090] =
[0091] r卞2) = c;f (，,f2) :(19):
[0092] 构造矩阵：
[0094] 由此，采用双矢量定姿法求解过程完成。将式(11)式(12)所得C(t^式(13)所 得々u(t\式(20)所得C|fl代入式（10)计算，即可计算出G，完成惯性系下捷联惯导系统的 粗对准对准过程，得到粗略的初始姿态阵。
[0095] 2)通过上述粗对准方法计算出姿态阵的粗略值，姿态误差角可视为小角，在此基 础上利用基于新息的自适应滤波获取载体的失准角，用失准角修正捷联姿态矩阵，对姿态 阵做精确估计以进一步提高对准精度，结束自对准，进入自主导航状态。
[0096] 在精对准阶段，姿态矩阵C((〇分为3个矩阵求取，如下所示：
[0097] ci{t)=c：q{twb{t) m)
[0098] 式中，C：和按照上述粗对准方案中的计算方法进行求取。因此的确定 关键在于地心惯性坐标系i系与载体坐标系b系之间的转换矩阵C;,(/)的确定。
[0099] 设i'系是由陀螺输出计算得到的存在失准角的惯性坐标系，i系是真实的惯性坐 标系。由于惯性器件存在各种误差，i'系和i系并不重合，设i'系与i系之间的失准角为#， 其三轴值分别表示为<和?6i'系与i系之间的转换关系为<：,:(〇，1'系与13系之间的转 换矩阵为G 0) 〇?
[0102] 其中，Ct'(0)是通过粗对准过程建立的载体坐标系与计算惯性坐标系之间的转换 矩阵，作为精对准开始时的转换矩阵。ACi根据陀螺和加速度计输出通过四元数算法求得。 因此，的确定关键在于从i系到i'系的转换矩阵的求取，也就是说关键在于i'系与i系 之间的失准角#的求取。
[0103] 针对上述所需过程，在这一步骤中，首先建立系统状态方程和量测方程，创建系统 状态空间模型，利用基于新息的自适应滤波器精确估计载体的失准角#，然后用失准角以修 正捷联姿态矩阵，计算准确地姿态矩阵，完成自对准的精对准过程，进入自主导航状态。
[0104] ①建立系统状态方程
[0105] 在精对准过程中，加速度计的刻度系数误差和安装误差不可忽略。设加表常值偏 置向量= + Vb为加速度计的常值偏置误差，其三轴值分别为\^和Vfc， 为高斯白噪声，其三轴值分别为％?、Vw>1和D加表刻度系数误差矩阵为SKA = diag(S KAx，SKAy，SKAz)，加表安装误差矩阵.
，则加速度计输出的比力 在V系中的投影为：
[0106] Qacoul_b = Cf[(/ +^,)(/ + 4, +?0) +Vb] (24)
[0107] 忽略二阶小项之后，可得：
[0109]设& = 4 + <4 + 44，为等效干扰加速度。将上式整理可 得：
[0111]对上式积分，可得：
[0118] 设惯性系i系下的速度误差为SVSSV1的三轴值分别为#；⑴、和^^⑴，由 式(32)得到惯性系下的速度误差方程：
[0119] sf:(t)=-ig; it )x]^ (/)+q (0 v;, -t- (; (n v w (33)
[0120] 设陀螺的常值漂移和量测高斯白噪声分别为一和co b，eb的三轴值分别表示为 44和<且矿=〇,《 b的三轴值分别表示为<、<和<。由姿态转换阵Ci和C丨' 的微分方 程，得到惯性系下的失准角方程：
[0121] m=-Q{t)sh-Q(t)G/ (34)
[0122] 根据惯性系下的速度误差方程和失准角方程，系统的状态方程是如下形式：
[0123] X{() = A(t)X(n + B(t)lV (35)
[0132] ②建立系统量测方程
[0133] 设量测矢量表示为：
[0134] Z = P F』-% =5F!+Fw+d>= (4〇;
[0135] 根据式(27)至式(32)，得到系统的量测方程：
[0136] Z⑴：-/认(/) +匕十汾(41)
[0137] 式中，量测矩阵为H=[I3X3 03x9];#g为不确定性量测干扰，主要由垂荡、纵荡、横 荡产生的干扰速度构成;Vw为量测高斯白噪声。
[0138] ③基于新息的自适应滤波算法
[0139] 系统状态方程和量测方程完成求解，由（35)时和(41)式，系统的状态空间模型也 就建立了。系统空间模型离散化后的形式如下：
[0141 ]式中，Xk为k时刻的状态矢量;Zk为k时刻的量测矢量;FKK-Ak-1时刻到k时刻的一 步转移矩阵；r^Sk-i时刻的系统噪声矩阵;Hk为k时刻的量测矩阵;Wk-dPVk为随机干扰的 白噪声;Uk为不确定性量测干扰。
[0142] 经典卡尔曼滤波采用新息的理论协方差计算滤波增益矩阵Kk，但是新息的理论协 方差无法直接反映出外部量测噪声的变化，所以该算法使用新息的实际协方差代替理论协 方差来计算滤波增益矩阵Kk。
[0143] 对新息％ 进行加权求和：
[0145] 由极大似然估计可知系统噪声协方差矩阵Rk的估计值为：
[0146] 4 = "/"I (My
[0147] 用此处Rk的估计值代替经典卡尔曼滤波公式中的Rk，可得到：
[0148] Kk = Pk\i.-iHlCh
[0149] 由于新息的不稳定性，进一步对其采用自适应的方式进行计算：
[0151] 式中a为调节因子，用于调节新的新息在新息加权求和过程中所占的比例，可根据 经验选择默认值。当新的新息比新息边界大的时候，认为此次的新息误差过大，降低其比例 降低a，反之a采用默认值，以此抑制滤波器发散。
[0152] a的计算方法如下：
[0154] 其中G为允许的误差边界。从公式可以看出，超过误差允许边界，距离越远，a越 小，则新的新息在计算过程中所占比例越小。
[0155] 结合图5可知，系统的基于新息的自适应滤波方程如下：
[0163]新息的计算方程:匕=石(48h)
[0164] 利用基于新息的自适应滤波可估计出系统状态矢量，系统状态矢量的第四项、第 五项、第六项即为载体的失准角<(/)、0)、<(十
[0165] ④用失准角修正捷联姿态矩阵
[0166] 从i系到i'系的转换矩阵：
[0168] Cf代入(22)式求得C；U)，€：、€(〇与代入(21)式求得精确的<=(〇,完成 了用失准角修正捷联姿态矩阵，对姿态矩阵做了精确估计，结束了自对准，进入自主导航状 〇
[0169] 步骤8:终止程序，关闭系统。
[0170] 本发明的有益效果如下：
[0171] 本发明首先在仿真环境下对全自主捷联惯性导航算法进行仿真实验，证明该算法 的有效性。然后通过真实实验对本发明提出的基于DSPTMS320F28335的全自主捷联惯性导 航系统进行验证，实验结果与实际载体姿态信息对比，证明本系统的可行性和有效性，证明 本发明实时性好，精确度高，数据处理效率高，能有效在动态干扰情况下计算炜度信息，能 有效隔离晃动对载体初始对准的影响，有效实现捷联惯性导航系统的实时快速全自主导 航。
[0172] (1)在仿真环境下对该算法进行仿真实验，验证算法。
[0173]首先进行仿真环境设置：
[0174]①设载体受到外界扰动，其航向角iK俯仰角0、横滚角Y作周期性变化，如下：
[0176]②存在横荡、纵荡和垂荡引起的线速度：
[0178] Adx = 0 ? 02m，ADy = 0 ? 03m，ADz = 0? 3m; ?Di = 2Ji/TDi，且Tdx = 7s，TDy = 6s，Tdz = 8s ;供a 为[0,23i]上服从均匀分布的随机相位；
[0179]③假设陀螺三个方向轴上的随机常值漂移为0.01°/h，陀螺的随机游走系数为 o.oor/V^.加速度计三个方向轴上的常值偏差为1 x io-4g，加速度计量测白噪声标准差为 1 / 1()〇*'^：
[0180]④常数设置
[0181] 赤道半径为Re = 6378165.0m，椭球扁率为e = 3.352e-3，由万有引力可得地球表面 重力加速度为g〇 = 9 ? 78049，地球自转角速度是单位是7 ? 2921158e-5;常数31 = 3 ? 1415926
[0182] 算法仿真结果如下：
[0183] 炜度信息真值为北炜39.78度。维度信息仿真估计进行了十次，每次炜度估计仿真 时间为ll〇s，且以取4〇8，^取11〇 8。仿真结果如图6所示。统计出十次炜度估计仿真实验对应 炜度误差的均值、最大值、最小值，如图7所示。可以清楚地看出，该炜度估计结果满足误差 要求，可为后续自对准提供初始炜度信息。
[0184] 粗对准仿真采用惯性系粗对准算法，进行了 50次，每次粗对准仿真时间为80s，且 tkl取40s，tk2取80s。仿真结果如图8所示。以粗对准结束时刻的姿态误差角<(0、<的、 作为衡量粗对准精度的指标，统计出五十次粗对准仿真实验对应的姿态误差角的均值、最 大值和最小值，如图9所示。可以清楚地看出，航向姿态误差小于0.76度，俯仰姿态误差小于 0.23度，横滚姿态误差小于0.1度，该粗对准的结果满足了精对准的要求，可为后续精对准 提供初始姿态。
[0185] 进行了 840s精对准仿真，精对准仿真结果如图10所示。以姿态误差角的估计误差 <(〇、<(〇、&⑴作为衡量精对准的指标。可以看得出，俯仰姿态在60s内完成精对准，稳 定在0.07度;横滚姿态在60s内完成精对准，稳定在0.045度;航向姿态在210s内完成精对 准，精度为〇.〇9度以内。
[0186] 由仿真结果可知，在动态干扰情况下，不依赖任何外界辅助信息，本算法可以自主 估计出载体所在位置的炜度信息值，自主解算精度要求范围内的粗略初始姿态阵，快速完 成精对准过程，完成全自主自对准，精度达到了要求，进入自主导航状态。
[0187] (2)通过真实实验对本发明提出的基于DSPTMS320F28335的全自主捷联惯性导航 系统进行验证。真实试验中，不提任何供外界辅助信息，系统装置放置在车上，有人员上下 车、开关车门、对车进行晃动等干扰。实验历时980s，试验地点在北京工业大学羽毛球馆南 广场位置。上位导航计算机控制导航系统，通过RS422转USB串口，向导航系统发送命令协议 "$GCCMD，OUTPUT，COMC，GPFPD，0.01*FF"，以 100HZ的数据更新速率，115200bps的波特率，采 集航向精度达〇. 1度、姿态精度达〇. 05度的实际三轴姿态信息，其上位机采集页面示意图如 图11。解算获得的载体姿态信息与本步骤中得到的高精度真实载体姿态信息做比较，证明 本算法和系统的可行性和有效性。
[0188] 系统实验结果如下：
[0189] 炜度信息真值为北炜39.8709度。维度信息估计在120s内完成，以取4〇8，t2取120s。 仿真估计值为北炜39.706790度，仿真估计误差为0.164110度，在误差允许范围内，可为后 续自对准提供初始炜度信息。
[0190] 粗对准过程采用惯性系粗对准算法，历时80s，tkl取40s，tk2取80s。粗对准实验结 果为航向姿态180.122500度、俯仰姿态0.513390度、横滚姿态0.082630度，此时刻真实姿态 信息输出值为航向姿态179.87度、俯仰姿态0.40度、横滚姿态0.21度，以姿态误差角的估计 误差作为衡量精对准的指标，航向姿态误差小于0.252500度，俯仰姿态误差小于0.113390 度，横滚姿态误差小于0.12737度，该粗对准的结果满足了精对准的要求，可为后续精对准 提供初始姿态。
[0191] 精对准过程历时780s，结果如图12。以姿态误差角的估计误差作为衡量精对准的 指标。可以看得出，俯仰姿态、横滚姿态和航向姿态均快速完成对准工作，俯仰姿态精度达 到〇. 06度，横滚姿态精度达到0.07度，航向姿态精度达到0.15度，收敛速度、稳态误差均符 合要求，完成自对准过程，进入自主导航状态。
[0192] 实验结果表明：不依赖任何外界辅助信息的情况下，针对导航系统对数据处理能 力、实时性、稳定性、小型化、低功耗的进一步高要求，本发明基于DSPTMS320F28335的全自 主捷联惯性导航系统仅利用自身的惯性测量单元输出，能自主有效在动态干扰情况下计算 炜度信息，能自主有效隔离晃动对载体初始对准的影响，能自主有效实现捷联惯性导航系 统的实时高精度快速自对准，进入全自主导航状态。
【主权项】
1.基于DSPTMS320F28335的全自主捷联惯性导航系统，其特征在于： 本发明使用基于DSPTMS320F28335的全自主捷联惯性导航系统，系统装置包括:浮点型 DSPTMS320F28335、SEED-XDS510PLUS仿真器、光纤陀螺、加速度计、SCI通信电路、RS232及 RS422转USB通信串口、上位导航计算机、数据电缆部分、电源；以DSPTMS320F28335作为控制 器，将全自主捷联惯性导航算法嵌入到浮点型DSP内的软件实现，实时采集、分析惯性测量 单元IMU中光纤陀螺和加速度计的输出信号并控制算法运行，以保证全自主捷联惯性导航 系统的高速处理能力、高实时性、高精度、高性能、低成本、低功耗;惯性测量MU单元中，光 纤陀螺仪的零点偏置稳定性是〇. 5度/h，零点偏置重复性是0.5度/h，加速度计的零点偏置 小于lmg，零点偏置稳定性小于lmg，数据更新速率100HZ，波特率115200bps;数据电缆部分， 供电电压VDC9至36VDC可选，输出方式RS-232、USB或者RS-422，可通过具体的命令协议，在 上位机或者DSP软件部分，得到MU信息；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轴指向东向(E)，Y轴指向北向（N)，Z轴指向天向（U);载体坐 标系b系，原点位于舰载机重心，X轴、Y轴、Z轴分别沿舰载机机体横轴指向右、沿纵轴指向 前、沿立轴指向上;载体惯性坐标系ibO系，是在对准初始时刻将载体坐标系经惯性凝固后 的坐标系； 步骤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的波特率，采集惯性测量单元MU数据，分析并解包得到光纤陀螺仪的三轴测量 值gyout_b和加速度计的三轴测量值acout_b; 步骤6:不依外界赖辅助信息，仅利用系统自身的惯性测量单元输出，在得到光纤陀螺 仪的三轴测量值gyout_b和加速度计的三轴测量值8(3〇此_13后，以惯性空间为参考基准，采 用重力投影，利用两个不同时刻惯性坐标系下重力加速度之间的夹角求取载体所在位置的 炜度信息，隔离晃动干扰对炜度求解的影响，完成晃动基座下运载体初始时刻所在位置炜 度信息的求解过程，其具体步骤如下： 设起始时刻ti惯性坐标系i系与载体坐标系b系重合，载体坐标系随地球一起转动，转动 至时间t2，惯性坐标系没有转动保持不变；由于地球的转动，重力加速度g在惯性空间i系内 的方向投影由乜时刻的gi(t〇变成了 t2时刻的gi(t2)，设gi(t〇与gi(t2)之间的夹角为0;0与 炜度L之间存在几何关系，因此通过求取0可以间接地求取炜度信息L，下面叙述具体过程； 确定起始时刻以和炜度估计时间t 2，CO i(5为地球坐标系e系相对于地心惯性坐标系i系的 地球自转角速度大小，在ti、t 2时间差内地球转过的角度a可求得： a=?ie(t2_tl) (1) 在。时刻，i系与b系重合，重力加速度g在b系内的方向投影为ghti)，重力加速度g在惯 性空间内的方向gi ( ti )可求得： g1 (ti) = gb (ti) = -acout_b (ti) (2) 在t2时刻，重力加速度g在b系内的方向投影为gb(t2)，重力加速度g在惯性空间内的方 向gVts)由下式求取：式中，q>2)St2时刻b系相对于i系的关系四元数值，(q〖(r2)f是q冰)的共辄，q> 2)可由 下式微分方程式求得：式中，< 为光纤陀螺输出值，表示b系相对于i系的载体运动角速率在b系内的投影； 由（2)式求解的矢量gUtd和(3)式求解的矢量gUt2)，求得两矢量gUtd与gUt2)之间 的夹角9:式中，?为内积符号，I IgUtdl |表示求取gUtO的模值，| |gUt2)| |表示求取gVts)的 模值； 9与炜度L之间存在几何关系，下面通过求取9来间接地求取炜度信息L: 几何关系如下： |Ao '| = |Bo '| = |Ao|cosL(6)将(6)式和(7)式代入(8)式，可求得晃动基座下炜度信息L的计算公式：将（1)式所求得角度a和（5)式所求得夹角0代入(9)式，即可求得炜度信息L，隔离了晃 动干扰的影响，完成了晃动基座下炜度值的估计过程； 步骤7:不依外界赖辅助信息，带入计算所得炜度信息，利用系统自身的惯性测量单元 输出，基于双矢量定姿原理，利用重力加速度在惯性空间方位的改变包含地球北向信息这 一特性，求解惯性系下粗略的初始姿态阵，姿态误差角可视为小角，接着采用基于新息的自 适应滤波方法获取载体的失准角，用失准角修正捷联姿态矩阵，得到三轴姿态信息航向角 也、俯仰角9、横滚角Y，实时记录数据，完成初始对准，进入全自主导航状态;其具体步骤如 下： 1)载体所在位置的炜度信息计算完成后，利用计算所得炜度值，进入晃动基座初始对 准粗对准过程，以惯性空间为参考基准，利用双矢量定姿算法计算出的初始姿态阵粗略值， 完成粗对准步骤； 在惯性系下捷联惯导系统的自对准粗对准算法中，以惯性空间为参考基准，导航坐标 系n系与载体坐标系b系之间的初始姿态矩阵的实现分解成⑴和常值矩 阵三部分的求解过程，姿态矩阵的分解表达式为：其中，导航坐标系n系与地心惯性坐标系i系之间的转换矩阵G _由所求得 的运载体所在对准点的地理位置炜度信息和初始对准时间确定： 导航坐标系n系与地球坐标系e系之间的转换矩阵：地球坐标系e系与地心惯性坐标系i系之间的转换矩阵：载体坐标系b系与载体惯性坐标系为ibO系之间的转换矩阵的初值= 其 更新利用陀螺测量输出角速度信息⑴跟踪求解，且其更新求解过程隐含了对角运动的隔 离：式中，[4(〇x]为向量的叉乘反对称矩阵； 地心惯性坐标系i系与载体惯性坐标系ib〇系之间的转动关系是不随时间变化的常 值矩阵，其确定过程是姿态矩阵:C|估算问题的关键，是初始对准的核心； 因为地心惯性坐标系i系内的速度投影V1与载体惯性坐标系ibO系内的速度投。有 关系式，因此Cf°可根据速度在两惯性坐标系中的投影V1和F~构造矢量实现计 算;在两个不同时刻，根据i系下的重力加速度的积分V1和ibO系下的重力加速度的积分 分别构建矢量，采用双矢量定姿法求解C,ib°矩阵求逆得到^^ .具体求解过程如下： 设n系下的重力加速度值为gn，i系下的重力加速度值g1为：设A tk = tk-to，积分得i系下的速度投影V1:在粗对准过程中忽略加速度计的刻度系数误差和安装误差角包含重力加速 度813、载体震荡纵荡横荡引起的干扰加速度、杆臂干扰加速度^4，如下式所示：则ib〇坐标系下陀螺输出积分所得速度的投影为：由于为线振动干扰速度，与重力加速度积分的速度相比较小，粗对准过程中可以 将其忽略；同理，由炜度误差引起的重力加速度误差引起的误差可以忽略，故求解ib〇系下的 速度的公式可化简为：在两个不同时刻，根据式（15)求得i系下的速度投影V1，根据式（17)和式（18)求得ib0系 下的速度后，分别构建矢量，采用双矢量定姿法求解 首先，在 tkl、tk2 时刻（t0〈tkl〈tk2)有： r-{tkl)=crv,-{tkl) vi-(til)=crvi{ti2) 值) 构造矩阵：由此，采用双矢量定姿法求解clM过程完成；将式（li)式（12)所得式（13)所得 式(20)所得代入式（10)计算，即可计算出G，完成惯性系下捷联惯导系统的粗 对准对准过程，得到粗略的初始姿态阵； 2)通过上述粗对准方法计算出姿态阵的粗略值，姿态误差角可视为小角，在此基础上 利用基于新息的自适应滤波获取载体的失准角，用失准角修正捷联姿态矩阵，对姿态阵做 精确估计以进一步提高对准精度，结束自对准，进入自主导航状态； 在精对准阶段，姿态矩阵C/1 /)分为3个矩阵求取，如下所示： Cl(t) = C：C：{t)Ch^ 式中，和CTW按照上述粗对准方案中的计算方法进行求取；因此，C7⑴的确定关键 在于地心惯性坐标系i系与载体坐标系b系之间的转换矩阵q⑴的确定； 设i '系是由陀螺输出计算得到的存在失准角的惯性坐标系，i系是真实的惯性坐标系； 由于惯性器件存在各种误差，i'系和i系并不重合，设i'系与i系之间的失准角为￥，其三轴 值分别表示为成、<和€，i'系与i系之间的转换关系为Cf的，i'系与b系之间的转换矩阵 为(:⑴：其中，ct'(〇)是通过粗对准过程建立的载体坐标系与计算惯性坐标系之间的转换矩阵， 作为 精对准开始时的转换矩阵；AC【根据陀螺和加速度计输出通过四元数算法求得；因此， <：,;(/)的确定关键在于从i系到i'系的转换矩阵的求取，也就是说关键在于i'系与i系之间的 失准角#的求取； 针对上述所需过程，在这一步骤中，首先建立系统状态方程和量测方程，创建系统状态 空间模型，利用基于新息的自适应滤波器精确估计载体的失准角#，然后用失准角￥修正 捷联姿态矩阵，计算准确地姿态矩阵，完成自对准的精对准过程，进入自主导航状态； ①建立系统状态方程 在精对准过程中，加速度计的刻度系数误差和安装误差不可忽略；设加表常值偏置向 量v6=vb+vw，vb为加速度计的常值偏置误差，其三轴值分别为I、，和，▽: w为高 斯白噪声，其三轴值分别为I、和;加表刻度系数误差矩阵为SKA=diag(SK Ax，S KAy，SKAz)，加表安装误差矩阵为则加速度计输出的比力在V系中的投影为：忽略二阶小项之后，可得：设dV +心!)(-y + + 为等效干扰加速度;将上式整理可得：对上式积分，可得：设惯性系i系下的速度误差为SVSSV1的三轴值分别为和&<(〇,由式 (32)得到惯性系下的速度误差方程：设陀螺的常值漂移和量测高斯白噪声分别为^和^13，^的三轴值分别表示为4、<和 <且矿=0、《b的三轴值分别表示为4 <和烤.由姿态转换阵C;和C丨:的微分方程，得到 惯性系下的失准角方程： <p'{t) = -Ch{t)sb -Cl(t)a>h ('4) 根据惯性系下的速度误差方程和失准角方程，系统的状态方程是如下形式： 式中，状态矢量设为： 系统噪声是：② 建立系统量测方程 设量测矢量表示为：根据式(27)至式(32 )，得到系统的量测方程： Z(t) = IIX(l) + V, +SV：, (4]) 式中，量测矩阵为H= [ 13X3 〇3X9] ; 为不确定性量测干扰，主要由垂荡、纵荡、横荡产 生的干扰速度构成;Vw为量测高斯白噪声； ③ 基于新息的自适应滤波算法 系统状态方程和量测方程完成求解，由（35)时和(41)式，系统的状态空间模型也就建 立了；系统空间模型离散化后的形式如下：式中，Xk为k时刻的状态矢量;Zk为k时刻的量测矢量;FKK-Ak-1时刻到k时刻的一步转 移矩阵；r k-Ak-l时刻的系统噪声矩阵;Hk为k时刻的量测矩阵;Wk-jPVk为随机干扰的白噪 声;Uk为不确定性量测干扰； 经典卡尔曼滤波采用新息的理论协方差计算滤波增益矩阵Kk，但是新息的理论协方差 无法直接反映出外部量测噪声的变化，所以该算法使用新息的实际协方差代替理论协方差 来计算滤波增益矩阵Kk; 对新息K =4 -巧兄M进行加权求和：由极大似然估计可知系统噪声协方差矩阵Rk的估计值为： 鳥=凡-/( 44 ) 用此处Rk的估计值代替经典卡尔曼滤波公式中的Rk，可得到： K^P^HlC^ (45) 由于新息的不稳定性，进一步对其采用自适应的方式进行计算：式中a为调节因子，用于调节新的新息在新息加权求和过程中所占的比例，可根据经验 选择默认值;当新的新息比新息边界大的时候，认为此次的新息误差过大，降低其比例降低 a，反之a采用默认值，以此抑制滤波器发散； a的计算方法如下：其中G为允许的误差边界;从公式可以看出，超过误差允许边界，距离越远，a越小，则 新的新息在计算过程中所占比例越小； 系统的基于新息的自适应滤波方程如下： 状态一步预测方程：^ kxjn (48 a) 均方误差一步预测方程+込-！ (48b) 滤波增益方程：A = Ax-1凡-(I. (48c) 状态估计方程：尤:--1 (48d) 均方误差估计方程:Pk = (I-KkHk ) PKK-i (48e)新息的计算方程:K = Za--(48b)： 利用基于新息的自适应滤波可估计出系统状态矢量，系统状态矢量的第四项、第五项、 第六项即为载体的失准角#(〇、<(〇、穴(乂 ④用失准角修正捷联姿态矩阵 从i系到i'系的转换矩阵：Cf代入(22)式求得4(〇, C：、与(7丨(〇代入(21)式求得精确的€：(〇,完成了用失 准角修正捷联姿态矩阵，对姿态矩阵做了精确估计，结束了自对准，进入自主导航状态； 步骤8:终止程序，关闭系统。
【文档编号】G01C21/16GK106052686SQ201610540172
【公开日】2016年10月26日
【申请日】2016年7月10日
【发明人】裴福俊, 智岩, 梁青琳, 魏晓丽
【申请人】北京工业大学