基于卡尔曼滤波的潜器运动状态观测器＊

刘常波 刘 培 丁风雷

（海军潜艇学院作战指挥系1） 青岛 266071）（海军潜艇学院研究生队2） 青岛 266071）

1 引言

潜器的自动控制，多是采用基于反馈构成的闭环结构，这种结构对内部参数变动和外部环境影响都有良好的抑制作用。反馈的基本类型包括“状态反馈”和“输出反馈”，状态反馈可以完整的反馈系统的结构信息，在性能上也远优于输出反馈。

但某些状态变量在系统中不可测量，需要引入附加的状态观测器。潜器的运动方程是非线性微分方程，经典的卡尔曼滤波只能处理线性方程，通过对潜器运动方程线性化，建立相应的状态观测器，给出状态变量的估计值。

在弱机动时潜器的运动可以分解成垂直面运动和水平面运动，本文以航速稳定的垂直面运动方程为例，设计其状态观测器，并根据某一给出水动力系数的深潜器，建立模型，给出仿真结果。

2 潜器垂直面运动方程及其线性化

考虑潜器的垂直面运动，假定航速近似稳定，对潜器六自由度方程简化，建立运动方程如式（1）所示，取潜器的重心为坐标原点。

方程组（1）是简化的垂直面潜器运动方程，将其看作一个系统的状态方程，状态变量为垂向速度w，纵倾角速度q，纵倾角θ，深度ζ。纵倾角θ和深度ζ可由仪器直接测量，为系统输出。在一定航速u下，系统的输入是首升降舵角δb和尾升降舵角δs。状态w和q，不能直接测量，是需要估计的量。理论上对输出的纵倾角θ和深度ζ求导可以得出w和q，但潜器上的深度计和纵倾仪精度有限，对其求导不能得到较为精确的W和q，可以用卡尔曼滤波建立相应的状态观测器估计w和q。经典的卡尔曼滤波只能处理线性系统，状态方程（1）是一组非线性方程，对其线性化得到垂直面线性运动方程（2）。

整理成标准形式如式（3）所示，由卡尔曼滤波，即可给出潜器垂直面运动的状态观测器

3 潜器垂直面运动的状态观测器

采用连续系统的卡尔曼滤波，潜器垂直面运动非线性系统的数学模型如下：

w（t）为随机性的干扰输入，ν（t）为量测噪声，其噪声统计特性如下：

其中，R1（t），R2（t）分别是w（t）及ν（t）的谱密度，δ（t）是置于t＝0处的一阶脉冲函数。

图1 潜器垂直面运动状态观测器框图

潜器在水中的运动，可分成水平面运动和垂直面运动，以垂直面运动方程为例，建立状态观测器，框图如图1所示。

状态估计方程如式（7）所示，估计误差协方差阵P（t）的传播方程如式（8）所示，卡尔曼增益阵K（t）如式（9）所示，初始条件如式（10）所示：

式中A，B，C，如式（4）所示，过程噪声w（t）的统计特性R1（t）由潜器航行时的内外环境所定，量测噪声ν（t）的统计特性R2（t）由纵倾角θ深度ζ的精度确定。由框图可知，状态观测器通过简化的线性模型预测状态值，然后通过量测信息更新预测值，得到估计的状态值。以上采用的线性模型比较简单，由此得出的预测值不够精确，主要靠量测信息来修正。量测信息（深度和纵倾）的精度高，其中包含的信息就多，由此得到的状态估计值就准确。

以Fossen在“Guidance and Control of Ocean Vehicles”书中给出的水下潜体为例，建立模型仿真。建立非线性运动方程，描述潜体的真实运动，用上述状态观测器，给出系统状态的估计值。

4 仿真结果

编程仿真，分析比较非线性模型计算出的系统状态量和观测器给出的状态估计量，状态量纵倾角θ和深度ζ可由仪器量测，只比较垂向速度w和纵倾角速度q的估计值与真实值。

运行程序，给出量测输出θ（纵倾角）和ζ（深度）的精度分别为1、0.1、0.001、0.001（度或米）的仿真结果，潜器速度4节，艏升降舵0°，艉升降舵打上浮舵5°。

潜器速度4节，艏升降舵0度，艉升降舵打上浮舵5度时，不同量测精度下，状态观测器的估计误差，如表1所示。

表1 不同精度下状态观测器的估计误差

图2 纵倾角速度q的真实值与估计值比较

图3 垂向速度w的真实值与估计值比较

5 结语

根据实际的仿真结果可以看出，观测器的估计误差与量测输出的精度有关，量测输出的精度越高，观测器的估计误差就越小。因此采用高精度的深度计和纵倾仪，可以降低观测器的估计误差。卡尔曼滤波主要靠模型预测和新的信息修正来估计状态，优化预测模型，采用扩展卡尔曼滤波，能得到更好的效果。由仿真结果还可以看出，当潜器的运动状态变化缓慢时，观测器能更好地跟踪潜器的真实运动状态。

[1]施生达.潜艇操纵性[M].北京：国防工业出版社，1995

[2]胡坤，吴超.潜艇垂直面运动仿真及分析[J].青岛大学学报，2003，16（4）

[3]张晶.潜艇运动建模及简化技术研究[D].哈尔滨工程大学硕士研究生学位论文，2009

[4]贾欣乐，杨盐生.船舶运动数学模型[M].大连：大连海事出版社，1999

[5]Thor I.Fossen.Guidance and Control of Ocean Vehicles[M].University of Trondheim，1994