本发明涉及的是一种气垫船船位推算方法,具体地说是一种气垫船在GPS信号丢失情况下基于IMM算法的船位推算控制方法。
背景技术:
由于气垫船是高速船,在高速航行过程中对突发情况的处理能力较弱,对其进行船位推算是很有必要的。
国内外对船位推算方法的研究,主要集中在对历史船舶位置数据的处理,常见处理方法为灰色预测,自适应卡尔曼等;关于AUV的航迹推算,彭树萍在2011年的哈尔滨工程大学硕士论文中,提出一种在AUV速度传感器DVL失效的情况下用AUV运动学模型解算的速度信息代替DVL给出的速度信息;而对于气垫船的船位推算方法的研究目前还是空白状态。
技术实现要素:
本发明的目的在于提供一种在气垫船GPS信号丢失的情况下能使气垫船能安全地航行,安全地到达目的地的气垫船船位推算控制方法。
本发明的目的是这样实现的:
(1)在气垫船GPS数据丢失的情况下,基于气垫船的速度及航行信息,用传统船位推算的方法计算出船的位置;
(2)根据气垫船的运动参数,建立匀速模型、匀加速模型、相关噪声模型模型,建立基于匀速匀速模型、匀加速模型、相关噪声模型模型的互式多模型模型,用互式多模型模型来描述气垫船的机动运动;
(3)根据先验信息确定马尔可夫转移概率矩阵,模型间的转换由转移概率矩阵决定;根据互式多模型算法进行循环运算,一个循环周期内进行:状态估计输入交互、模型滤波处理、模型和状态的概率更新、滤波结果加权输出,输出结果即为推算的船位;
(4)将推算的船位作为GPS测量数据反馈给航迹控制器,实现安全航行。
本发明的气垫船船位推算控制方法,在气垫船GPS信号丢失的情况下,通过船位推算得到的推算位置代替GPS位置信息,保证了气垫船能安全地航行,安全地到达目的地。本发明的技术方案的要点在于:
1、根据气垫船的航速,艏向角及历史位置数据,通过传统推算算法得到实时位置估计数据;
2、结合气垫船特性建立基于气垫船的匀速(CV)模型、匀加速(CA)模型及Singer模型;
3、建立交互式多模型(IMM),设计相应的强跟踪卡尔曼滤波器;
4、推算数据反馈给航迹控制器,实现安全航行。
本发明基于IMM算法结合气垫船模型,能实现高精度的位置推算。船位推算是通过全垫升气垫船之前的历史航行数据参数(实船的航行方向、航行速度、航行位置以及航行时间等)来进行船体位置推算,将推算出来的结果当成信号丢失时刻GPS信号所对应的航行运动参数,这样就可以使船舶正常行驶。IMM算法是使用不同模型匹配目标不同运动状态,代表各个模型的滤波器并行工作,并且假设各模型之间的切换服从Markov过程,最终的估计结果为各模型估计的加权和。本发明采用IMM算法,建立基于气垫船的匀速(CV)模型、匀加速(CA)模型及Singer模型,利用强跟踪卡尔曼滤波算法对推算结果进行了次最优估计,提高了推算精度。
附图说明
图1 传统船位推算方法;
图2 MMI算法原理图;
图3 基于IMM算法的气垫船船位推算的详细流程图;
图4 本发明的原理框图。
具体实施方式
下面结合附图举例对本发明作进一步描述。
1.基于IMM算法的气垫船船位推算的详细流程图如图3所示,具体步骤如下:
(1)在气垫船GPS数据丢失的情况下,基于气垫船的速度及航行信息,用传统船位推算的方法计算出船的位置;
(2)根据气垫船的运动参数,建立相应的CV模型,CA模型,Singer模型;建立基于CV,CA,Singer模型的IMM模型,用IMM模型来描述气垫船的机动运动;
(3)根据先验信息确定马尔可夫转移概率矩阵,模型间的转换由转移概率矩阵决定;根据IMM算法进行循环运算,一个循环周期内进行:状态估计输入交互、模型滤波处理、模型和状态的概率更新、滤波结果加权输出,输出结果即为推算的船位;
(4)将推算数据作为GPS测量数据反馈给航迹控制器,实现安全航行。
2.传统推算算法过程如下:
起始位置用(x0,y0)表示,通过GPS可以得到在tn时刻的位置,该时刻的位置用(xn,yn)来表示,通过下式得到:
在式1中,Δx,Δy分别表示在时刻ti-1和ti单位时间内全垫升气垫船分别在X轴和Y轴方向上的位移,T是船位推算系统的时间间隔,Vi和ψi分别表示在ti-1时刻下的总速度和对应的大地坐标系下的航向角。
3.根据气垫船运动参数建立匀速(CV)模型、匀加速(CA)模型及Singer模型
根据气垫船的航速及艏向角,由式(1)简单推算出气垫船的船位,基于以上参数建立气垫船的匀速模型、匀加速模型及Singer模型。
(1)匀速(CV)模型、匀加速(CA)模型的建立
CV模型将气垫船的运动简单地定义为匀速运动,机动被视为随机干扰情况,
当气垫船作匀加速直线运动时,则采用三阶CA模型
(2)气垫船运动的相关噪声模型(Singer模型)的建立
设气垫船的加速度为a(t),a(t)是满足均值为0且指数相关的随机过程,即
其中,为运动目标加速度的方差,a为机动时间常数的倒数,称为机动频率。
其中amax最大机动加速度,p0为零加速度时的发生概率,pmax为最大加速度时的发生概率。
因此,机动加速度可用白噪声驱动的一阶时间相关模型表示为
式中W'(t)是方差为的零均值高斯白噪声。于是Singer模型为
将上式进行离散化,可得相应的离散时间模型为
采样周期h=0.5s,因此,可以略去阶次为h3及以上的项,于是可得系统方程为
式中W'(t)是方差为的零均值高斯白噪声。
W'(t)的方差矩阵满足
Q(k)=E[W(k)WT(k)]=[qij]3×3 (9)
4.建立交互式多模型(IMM),设计相应的强跟踪卡尔曼滤波器
建立基于CV,CA,Singer模型的IMM模型,用IMM模型来描述气垫船的机动运动;根据先验信息确定马尔可夫转移概率矩阵,模型间的转换由转移概率矩阵决定。在每一时刻,基于上诉三个模型的各个滤波器的初始输入值是实际模型转移概率修正后的交互值。
交互式多模型算法有3个模型,其离散系统的状态方程满足:
X(k+1)=Φj(k)X(k)+Wj(k),j=1,2,3 (10)
其中,Φj(k)是第j个模型的过程矩阵;Wj(k)是均值为零、协方差矩阵为Qj(k)的白噪声序列。并设M={m1,m1,m3}为离散系统的3个模型的集合,假设k时刻所处的模型为M(k),k时刻处在mj模型时用mj(k)表示,则模型的初始状态概率
P(mj(0))=P(M(0)=mj)=μj(0) (11)
模型的一步转移概率为
第j个模型的观测方程为
Z(k)=Hj(k)X(k)+Vj(k) (13)
其中Hj(k)是第j个模型的观测矩阵;Vj(k)是均值为零,协方差矩阵为Rj(k)的白噪声序列。
马尔科夫链的转移概率矩阵为
其中,pij是从第i个模型到第j个模型的转移概率。
IMM算法的一次循环可由四个步骤组成,如图2所示,分别为:状态估计输入交互、模型滤波处理、模型和状态的概率更新、滤波结果加权输出。
(1)状态估计输入交互
设μj(k/k-1)为第j个模型的模型预测概率,有
μij(k/k-1)为输入混合概率
μij(k/k-1)=P(mj(k)|Z(k-1))=πijμi(k-1) (16)
从而,交互运算后,k时刻滤波器的输入为
交互协方差为
(2)模型滤波处理
对模型进行强跟踪卡尔曼滤波处理,则状态预测为
协方差预测为
残差为
残差协方差为
Sj(k)=Hj(k)Pj(k|k-1)(Hj(k))T+Rj(k) (22)
增益为
Kj(k)=Pj(k|k-1)(Hj(k))T(Sj(k))-1 (23)
β(k+1)是引入的自适应渐消因子,利用渐消因子限制卡尔曼滤波器的记忆长度,调整状态预报误差以及相应的增益矩阵,改善滤波器的动态性能。次渐消因子β(k+1)的计算如下:
其中β0为:
(3)模型和状态的概率更新
滤波后的状态更新为
滤波后的状态协方差更新为
Pj(k|k)=Pj(k|k-1)-kj(k)Sj(k)(kj(k))T (27)
模型概率更新为
其中,Aj(k)为似然函数,且uj(k|k-1)是从第i个模型到第j个模型的转移概率。
(4)输出交互
状态交互
协方差交互
5.推算数据反馈给航迹控制器,实现安全航行。
将推算数据作为GPS测量数据反馈给航迹控制器,实现安全航行。
本气垫船船位推算控制方法主要包括传统船位推算方法和交互式多模型(IMM)算法。在气垫船GPS位置信息丢失的情况下,由气垫船的航速,艏向角,及位置历史数据,根据气垫船特性建立的MMI模型,推算出高精度的位置信息,获得气垫船的实时位置,从而实现安全航行。
推算过程分为两个部分:第一部分为传统船位推算方法部分,根据气垫船的航速,艏向角及历史位置数据,通过推算算法得到实时位置估计数据;第二部分为结合气垫船特性建立基于气垫船的匀速(CV)模型、匀加速(CA)模型及Singer模型的交互式多模型(IMM),利用强跟踪卡尔曼滤波算法对推算结果进行次最优估计。
基于气垫船安全航行的考虑,在航行过程中突然GPS发生故障的情况下,可以这船位推算的位置信息代替实船位置。
利用推算的位置信息,反馈给航迹控制器,控制器根据规划航迹给出舵角指令,从而实现安全航行。
传统船位推算方法,根据船体航行方向以及航行速度的运动参数预判下一个位置的坐标的导航过程,可以给出船体当前以及未来的位置坐标的估计值。
IMM算法模型间的切换是通过马尔科夫链来完成的。IMM算法的一次循环可由四个步骤组成:状态估计输入交互、模型滤波处理、模型和状态的概率更新、滤波结果加权输出。
基于IMM算法建立强强跟踪卡尔曼滤波模型,对推算结果进行次最优估计,实现高精度的推算船位推算。
船舶航迹控制算法的研究与仿真_杨树仁(航迹控制)。