[发明专利]基于DSPTMS320F28335的全自主捷联惯性导航系统

摘要

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

主权项

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：终止程序，关闭系统。