渠慎征，万德钧
（东南大学仪器科学与工程系，南京，210096）
摘 要：介绍了在捷联式航姿系统中采用的简化Sage－Husa算法的自适应滤波原理，建立了基于该自适应滤波算法的导航系统状态方程和观测方程，给出了基于TMS320C32浮点数字信号处理器（DSP）的半物理仿真硬件原理图和软件运行流程，并进行了仿真计算。仿真分析结果表明，基于浮点DSP的航姿系统在采用自适应卡尔曼滤波算法时不仅能有效抑制滤波发散，还使系统具有优越的实时性能和较高的精度。
关键词：自适应卡尔曼滤波；数字信号处理器；导航系统；仿真
引 言
随着数字信号处理技术及大规模集成电路的发展，运算量很大的卡尔曼导航滤波程序已可以在基于数字信号处理器（DSP）的系统中实现。但是，因计算位数和相关支持软件的局限，其滤波精度仍低于基于个人计算机（PC）架构的导航系统。同时，常规卡尔曼滤波器需要准确地知道系统的数学模型和系统噪声与观测噪声的统计特性。而实际应用中很难对陀螺漂移和加速度计零偏的随机特性准确描述，观测噪声的随机特性取决于测量设备以及系统工作环境。对其建模更为困难，这使得常规卡尔曼滤波器在基于DSP的导航系统中更易于发散。因此，在基于DSP的导航系统中，应采用更稳定的自适应滤波算法［1］。本文主要研究在捷联航姿系统中，利用TMS320C32实现自适应卡尔曼导航滤波的方法，给出了相应的算法，并进行了仿真。结果表明，基于DSP的自适应卡尔曼导航滤波算法能够快速稳定，并满足导航系统对实时性和精度的要求，而且与基于个人计算机架构的航姿系统相比，使用灵活，体积小，功耗和成本低。
1 自适应滤波算法
自适应滤波器不完全依靠验前确定的系统模型，而是以测量信息为反馈，不断地在线估计系统噪声和观测噪声的统计特性，以得到系统状态变量的精确估计值，从而使滤波更稳定，更适合于运算位数低于PC的DSP系统。
导航系统数学模型的离散形式为
式中：Xk为状态向量；Φk，k－1为状态转移矩阵；Zk为观测向量；Hk为观测矩阵；Γk－1为系统噪声矩阵；Wk－1为系统噪声向量；Vk为观测噪声向量。Wk－1，Vk是不相关高斯白噪声序列，其均值、方差分别为
由极大后验估计可以得到估计q，Q，r，R的算式，并参加卡尔曼滤波迭代，此即为Sage－Husa自适应滤波算法［2］。由于在系统噪声方差阵Q和观测噪声方差阵R均未知时，得到的Q，R的估值与真实值之间有较大误差，故根据系统的特点和工程应用实际，设只有R未知，r＝0，q＝0，Q为常数，就得到简化的Sage－Husa自适应滤波算法，其方程为：
上述算法中估计观测噪声的协方差阵R时，引入遗忘因子b（0＜b＜1）的目的是注重当前观测数据，对过去的观测信息逐渐遗忘，b依照噪声统计变化情况而定，以便调整噪声继承值与更新值的加权系数，从而适应实际观测噪声的变化。
2 导航系统状态方程与观测方程的建立［3～5］
陀螺漂移的数学模型：可认为陀螺漂移由白噪声wg和随机常数εb组成，即ε＝εb＋wg。εb的各次启动值是随机变化的，但单次启动后是不变的，故εb＝0。设三个方向的陀螺模型相同，分别为εE，εN，εU。
加速度计数学模型：也可认为是由白噪声wa和随机常数▽b组成，即▽＝▽b＋wa。▽b的各次启动值是随机变化的，但单次启动后是不变的，故  ＝0。设三个方向的加速度计模型相同，分别为▽E，▽N，▽U。
上述误差方程中：ωie为地球自转角速率；RN，RE分别为载体所在位置的地球子午圈和卯酉圈的曲率半径；λ，L为载体所在位置的经度、纬度；▽E，▽N，▽U为平台坐标系相对导航坐标系的三个误差角；vE，vN，vU分别为载体的东北天向速度。
由于本系统为捷联舰载航姿系统，故可根据上述方程进行适当简化，选取状态向量为
可建立系统状态方程：X（t）＝F（t）X（t）＋W（t），其中F（t）为系统系数矩阵，W（t）为系统噪声矩阵。
采用速度航向误差校正模型，将导航系统输出的速度和航向角分别与计程仪的速度和平台罗经输出的航向角之差作为观测量，可得观测方程
Z（t）＝H（t）X（t）＋V（t）
将系统的状态方程和测量方程离散化，就可以得到系统的线性离散数学模型。
3 仿真软硬件环境
本文实现滤波算法的硬件环境是一种主从式紧耦合双处理器结构的导航计算机，以浮点DSPTMS320C32作导航算法处理机，用于导航计算，单片微控制器89C51则用作I／O接口处理机。美国TI（德州仪器）公司生产的TMS320C32浮点DSP，其处理能力达到40位浮点精度，浮点数操作能力达到150MFLOPS，指令速度接近60 MIPS，运算能力非常强大，可满足实时处理信号的要求，而单片机则拓展了系统的I／O控制功能。二者通过双口RAM以互中断高速并行接口相联，计算出的姿态结果数据与预设的理论值之差经串口传输到计算机上保存，以便仿真计算完成后进行数据处理分析。硬件框图［6］见图1所示。
软件设计采用主从机制，以中断方式执行。每个周期开始时，先由89C51产生10 ms定时中断，使DSP开始计算，而89C51则把DSP上一周期放在双口RAM中的结果数据取出并经异步串口传输到PC，在仿真分析时，由于惯性仪表的输出和滤波观测值都由软件模拟，DSP不需要从外部获取数据，而只需要将计算的结果发送至双口RAM。DSP中设立计数器，每完成100个计算周期，就进行一次卡尔曼滤波处理。整个软件系统采用C语言和汇编语言混合编程，捷联和滤波计算程序采用C语言，89C51和DSP的相关硬件操作采用汇编语言编程，使得系统中双机紧密配合，能以流水线方式顺利执行。
4 仿真结果
根据实际系统中各仪表的精度，假定传感器的误差如下：
陀螺仪：随机漂移0．05°／h，常值漂移0．05°／h；
加速度计：随机漂移50μg，常值漂移50μg；
初始失准角：航向角0．15°，纵摇角0．15°，横摇角0．15°。
为了滤波器稳定，选取滤波协方差阵P0时一般把滤波模型的先验协方差设计得比实际值大一些，约为实际值的2～10倍。故可选取的滤波器的初始值如下：
由于系统的可观测性较高，状态估计值对初始状态向量的初值不敏感，故可将初始状态值取为零，即：X0＝｛0，0，0，0，0，0，0，0，0，0，0，0｝；
遗忘因子b取为0．99，以加重新近观测的数据对现时估计计算的作用。
假定载体以航速为15 m／s前进，同时三轴按正弦函数进行摇摆运动，幅值分别为纵摇角（▽N）9°、横摇角（▽U）12°、航向角（▽E）14°，对应的周期为8，12，6 s，初始航向角为45°，纵摇角和横摇角均为0°。仿真8 h后结果如图2～4所示，图中波动较大的曲线是没有滤波时的原始误差曲线，此时，系统处于纯惯性状态，观测信息不返回校正系统参数；较直的线为滤波校正后得到的误差曲线。
图5～7是三个姿态误差角的均方根值曲线，可以看出其均方误差值在程序执行后很快就收敛为一个稳定的小值，系统收敛快，说明滤波器有很好的稳定性，而接近于零的稳定值，则是由于惯性仪表的输出存在着误差。
5 结 论
由上面的仿真可以看出，在基于DSP的导航系统中采用自适应卡尔曼滤波后能够取得明显的效果，原始误差中的舒拉周期振荡、地球周期振荡和傅科周期振荡基本消除，滤波具有稳定的收敛性能，这保证了系统在较长时间运行后仍具有较高的输出精度。同时，与基于PC架构的航姿系统相比，基于DSP的航姿系统因其具有灵活、体积小、功耗低、成本低及可靠性高等诸多优点，对于扩展捷联航姿系统的应用领域（如小型廉价的姿态基准与制导系统等）仍然具有很大的积极作用。
参考文献：
［1］ 陆 恺．最优估计理论及其在导航中应用［M］．上海：上海交通大学出版社，1990．
［2］ 储 静．SINS／GPS组合导航系统在线校正技术研究［D］．南京：东南大学，2000．35～40．
［3］ 袁 信．捷联式惯性导航原理［M］．北京：航空专业教材编审组，1985．
［4］ 张树侠，孙 静．捷联式惯性导航系统［M］．北京：国防工业出版社，1992．
［5］ 俞济祥．卡尔曼滤波及其在惯性导航中的应用［M］．北京：航空专业教材编审组，1984．
［6］ Texas Instruments．TMS320C3x User′s Guide［Z］．Owensvilie：Custom Printing Company，1997．
|
