CN105005679B - 一种基于粒子滤波的船舶参数辨识方法 - Google Patents

Abstract

本发明属于船舶参数辨识领域，具体的说是一种能够用于船舶时域状态空间模型中未知参数的估计的基于粒子滤波的船舶参数辨识方法。本发明利用船舶自身的推进器、舵等执行机构对船舶施加力和力矩，使船舶做特定运动；利用测量系统测出地坐标系下，船舶在每个离散采样时刻的位移和姿态角信息，其为船舶的输出观测值；利用船舶参数估计器，根据船舶的状态估计值及输出观测值辨识船舶得到每个时刻的船舶参数估计值；利用粒子滤波器，根据每个时刻的受力信息和前一时刻辨识的参数值，估计船舶的状态值，即船舶在体坐标系下的角速度和线速度值。本发明无需对观测数据进行批处理，无需进行大量迭代，具有很高的滤波精度；计算简单，执行效率高。

Description

一种基于粒子滤波的船舶参数辨识方法

技术领域

本发明属于船舶参数辨识领域，具体的说是一种能够用于船舶时域状态空间模型中未知参数的估计的基于粒子滤波的船舶参数辨识方法。

背景技术

在航行过程中船舶的姿态控制是一个复杂的问题。船舶有6个自由度，船舶的运动方程存在高度的非线性，水动力系数之间相互耦合；同时，船舶在航行过程中受到不确定的洋流、海浪、风等的干扰，执行机构存在滞后性，这样就对船舶的控制造成了很大困难。为了对船舶进行准确的控制，一般需要建立准确的船舶模型，进一步需要对包括水动力系数在内的船舶参数进行辨识，减小控制的不确定性。船舶参数可以通过实测或理论估计得到，前者往往要借助于特殊的仪器，成本较高，而且时间消耗较大，获取麻烦；后者采用经验或切片法等理论，往往不够准确。通过辨识方法确定船舶参数综合了二者的优点，有较高的研究价值。参数辨识还可以用于解决其它领域的一些测试方法中要求的物理量难以直接获取的情况。

粒子滤波采用蒙特卡罗方法进行随机采样，粒子滤波基本思想是利用一个随机样本，而不是状态空间方程中的函数来近似指定的概率密度函数。粒子滤波的计算精度较高，而且能够用于高度非线性、噪声非高斯分布的系统。

发明内容

本发明的目的在于针对船舶非线性状态空间模型的参数辨识问题，提供一种适合在线运行的参数辨识方法。

本发明的目的是这样实现的：

基于粒子滤波的船舶参数辨识方法，包括以下几个步骤：

(1)采集船舶的位移和姿态角信息作为输出观测量η，通过推进器、舵等机构对船舶施加的可测量的力及力矩控制输入，根据控制输入τ，输出观测量建立船舶模型，得到船舶模型中待辨识的参数θ；

船舶6自由度模型为：

其中，M＝M_RB+M_A为惯性矩阵，M_RB是船体的刚体质量矩阵，M_A是附加质量矩阵，C(ξ)＝C_RB(ξ)+C_A(ξ)为由地球自转引起的科氏力和向心力矩阵，C_RB(ξ)是刚体科氏力和向心力矩阵，C_A(ξ)是附加质量科氏力和向心力矩阵，D(ξ)为流体阻尼矩阵，g(η)为重力及浮力同时作用引起的恢复力及力矩，τ为控制输入，观测变量η表示船体在地坐标系中的位置与姿态角度信息，用体坐标系坐标原点在地坐标系中的坐标(x,y,z)表示船体在地坐标系中的位置，同时用地坐标系与体坐标系之间的欧拉角(φ,θ,ψ)表示船体的姿态，状态变量ξ表示船体的体坐标系线速度和角速度，其中(u,v,w)表示船体的线速度，(p,q,r)表示船体绕各轴旋转的角速度；

w＝p＝q＝0，同时考虑水下船体的工作情况和对称性，建立水下船体的平面3自由度模型，关于xz平面对称，则I_xy＝I_yz＝0；浮心和重心重合，

向量g(η)表示重力和浮力及相应力矩，取g(η)＝[0 0 0]^T，

向量τ表示外力及外力矩作用，不包括环境干扰，τ＝[τ_X τ_Y τ_Z]^T，欧拉变换方程将体坐标系中观察到的物理量：前进线速度、侧移线速度、艏摇角速度变换到地坐标系中，得到x轴上速度y轴上速度及艏摇角速度

则船舶模型进一步表示为

采样时间间隔为Δ，采用一阶欧拉法进行离散化，得到船舶的非线性离散模型

ξ_k＝ξ_k-1+ΔM^-1[τ_k-1-C(ξ_k-1)ξ_k-1-D(ξ_k-1)ξ_k-1-g(η_k-1)],

η_k＝η_k-1+ΔJ(η_k-1)ξ_k,

船舶模型的输出观测值指每个时刻的η_k；

(2)根据船舶当前的状态估计值，初始时刻的状态估计值按正态分布随机产生的，输出观测量η，利用参数估计器，辨识船舶模型的参数估计值；

(3)根据辨识出的船舶模型的参数估计值，利用粒子滤波器，更新当前的状态估计值；

(4)重复步骤(2)-步骤(3)结束条件为k＝M，M为总的观测值的数量，得到每个采样时刻的参数估计值。

所述的根据船舶当前的状态值及输出观测量η，利用参数估计器，辨识船舶模型的参数估计值的方法为：

(1.1)令k＝1，初始化θ₀；

(1.2)计算和N为所用参数值θ_k的粒子数，其中θ_k-1表示k-1时刻得到的参数值，θ_k表示k时刻得到的参数新值，

θ_k＝θ_k-1+ε,

其中，为k时刻得到的参数θ_k的最优的估计值；

(1.3)令k＝k+1，并从第(2)步迭代计算，直至k＝M+1。

所述的根据辨识出的船舶模型前一时刻的参数估计值，利用粒子滤波器，更新当前的状态估计值的方法为：

(2.1)利用船舶的受力τ、输出值η及待辨识参数的前一时刻的估计值，由粒子滤波器产生粒子及其权重具体过程为：

(2.1.2)计算粒子权重并且归一化

For i＝1:N，

End,

(2.1.3)重采样

For j＝1:N，

End

(2.1.4)计算k+1时刻的状态粒子

(2.1.5)令k＝k+1，从步骤(2.1.2)迭代计算，直至k＝M+1停止计算；

(2.2)由粒子滤波器估计出当前时刻k的状态估计值

本发明的有益效果在于：

无需对观测数据进行批处理，无需进行大量迭代，在每个采样时刻都能取得一个以极大似然概率保证的参数估计值；采用了粒子滤波算法进行参数辨识，能够用于复杂的非线性状态空间模型，具有很高的滤波精度；计算简单，执行效率高，辨识结果准确。

附图说明

图1是整个船舶参数辨识方法原理图。

图2是ODIN作绕垂线旋转运动时艏摇角速度r的仿真结果。

图3是ODIN作绕垂线旋转运动时艏摇角ψ的仿真结果。

图4是ODIN作绕垂线旋转运动时参数辨识结果。

图5是ODIN作绕垂线旋转运动时粒子滤波结果。

图6是ODIN在水平面上作直线运动时需要施加的控制力矩τ_p。

图7是ODIN在水平面上作直线运动时的系统仿真模型。

图8是ODIN在水平面上作直线运动时在u、v两个方向的速度变化曲线。

图9是ODIN在水平面上作直线运动时位移x、y随时间变化曲线。

图10是ODIN在水平面上作直线运动时位置变化曲线。

图11是ODIN在水平面上作直线运动时艏摇角ψ变化曲线。

图12是ODIN在水平面上作直线运动时参数θ＝X_u|u|＝Y_v|v|的辨识结果。

图13是ODIN在水平面上作直线运动时状态u的滤波结果。

具体实施方式

下面结合附图对本发明做进一步描述。

本发明提供的是一种基于粒子滤波的船舶参数辨识方法。

首先根据机理法建立船舶的待辨识时域状态空间模型，通过离散化得到对应的离散化的数学模型；

在一定的通过推进器、舵等机构施加的可测量的力及力矩的作用下，船舶进行特定的航行，通过GPS定位器、陀螺仪等测量与控制输入、状态变量对应的每个观测时刻的模型输出结果，即船舶的位置和姿态信息；

根据模型输出，利用系统模型和噪声统计信息进行粒子滤波，得到船舶离散模型中状态变量的估计值，即船舶自身的位置和姿态信息的估计值；

根据模型输出和状态估计值，利用船舶参数估计器进行船舶参数辨识，可以得到每个采样时刻的船舶参数的估计值。

基于粒子滤波用于船舶非线性状态空间模型的参数辨识，方法可行并且效果较好。

本发明的优点在于能够应用于非线性程度很高的系统参数辨识，计算方便，能够用于多参数辨识；本方法不需进行多次迭代，算法收敛，计算速度快，有在线运行的前景。

该方法的步骤如下：

(1)利用船舶自身的推进器、舵等执行机构对船舶施加力和力矩，使船舶做特定运动；

(2)利用测量系统测出地坐标系下，船舶在每个离散采样时刻的位移和姿态角信息，其为船舶的输出观测值；

(3)利用船舶参数估计器，根据船舶的状态估计值及输出观测值辨识船舶得到每个时刻的船舶参数估计值；

(4)利用粒子滤波器，根据每个时刻的受力信息和前一时刻辨识的参数值，估计船舶的状态值，即船舶在体坐标系下的角速度和线速度值；

(5)在观测数据结束前，不断执行(3)和(4)两步。

本发明描述的是一种新的船舶参数辨识方法，利用了粒子滤波方法，方案图如图1所示。

具体步骤如下：

(1)利用船舶自身的推进器、舵等执行机构对船舶施加力和力矩，使船舶做特定运动，具体为：

通过控制器对船舶的螺旋桨、舵等执行机构进行控制，记录下每个采样时刻的推进力及力矩；船舶的整体运动方式应该严格按照船舶模型确定；为了使船舶完成特定的运动方式，可以采用额外的机械结构对船舶施加外部力和力矩，并且所施加的外部力及力矩均要求可测并且进行记录。

(2)利用测量系统测出地坐标系下，船舶在每个离散采样时刻的位移和姿态角信息，其为船舶模型的输出观测值，具体为：

这里所说的船舶模型为：

根据牛顿定理和拉格朗日公式，船体的6自由度非线性动态方程可以描述为

以上动态方程同时适用于其它自由度。其中，M＝M_RB+M_A为惯性矩阵，M_RB是船体的刚体质量矩阵，M_A是附加质量矩阵；C(ξ)＝C_RB(ξ)+C_A(ξ)为由地球自转引起的科氏力和向心力矩阵，C_RB(ξ)是刚体科氏力和向心力矩阵，C_A(ξ)是附加质量科氏力和向心力矩阵；D(ξ)为流体阻尼矩阵，包括船体震动引起的阻尼、摩擦阻尼、洋流阻尼、涡流阻尼等；g(η)为重力及浮力同时作用引起的恢复力及力矩；τ为控制输入，这里忽略了环境干扰力等外部作用。观测变量η表示船体在地坐标系中的位置与姿态，用体坐标系坐标原点在地坐标系中的坐标(x,y,z)表示船体在地坐标系中的位置，同时用地坐标系与体坐标系之间的欧拉角(φ,θ,ψ)表示船体的姿态；状态变量ξ表示船体的体坐标系线速度和角速度，其中(u,v,w)表示船体的线速度，(p,q,r)表示船体绕各轴旋转的角速度。

通常船体的6自由度模型非常复杂，在其基础上忽略垂荡、纵摇、横滚运动，即认为w＝p＝q＝0，同时考虑水下船体的工作情况和对称性，建立水下船体的平面3自由度模型。假设船体具有均匀质量分布，关于xz平面对称，则I_xy＝I_yz＝0；假设浮心和重心重合，这样浮力和重力作用于一点。

向量g(η)表示重力和浮力及相应力矩，这里认为船体能够稳定在某一水深位置，取g(η)＝[0 0 0]^T。

向量τ表示外力及外力矩作用，不包括环境干扰，τ＝[τ_X τ_Y τ_Z]^T。为了评估参数辨识的效果，这里认为所有施加的外力及外力矩均是可测的。

欧拉变换方程将体坐标系中观察到的物理量：前进线速度、侧移线速度、艏摇角速度变换到地坐标系中，得到x轴上速度y轴上速度及艏摇角速度

参数：

将式(1)和式(2)进一步表示为

设采样时间间隔为Δ，采用一阶欧拉法进行离散化，得到船舶的非线性离散模型

ξ_k＝ξ_k-1+ΔM^-1[τ_k-1-C(ξ_k-1)ξ_k-1-D(ξ_k-1)ξ_k-1-g(η_k-1)] (5)

η_k＝η_k-1+ΔJ(η_k-1)ξ_k (6)

船舶模型的输出观测值指每个时刻的η_k。

(3)利用船舶参数估计器，根据船舶的状态估计值及输出观测值辨识船舶得到每个时刻的船舶参数估计值，具体为：

这里的所说的船舶参数估计器，指的是：

将式(5)和式(6)写成含参非线性系统通用形式如下

其中，{ξ_k}是一阶马尔科夫过程；η_k的条件概率分布仅仅与ξ_k有关；θ为未知参数；w_k-1和v_k分别为过程噪声和输出噪声，且统计特性已知，这里假设w_k-1和v_k均服从均值为0的高斯分布，即w_k-1～N(0,W)，v_k～N(0,V)。已知输出方程得到的η_k的观测序列η_1:M＝{η_k,k＝1,2,...,M}，其中M为总的观测值的数量。这里需要解决两个问题，即参数θ的辨识问题和k时刻状态ξ_k的估计问题。

非线性参数辨识问题描述为参数θ下η_1:M的极大似然条件概率问题，即

利用贝叶斯公式，可以得到

式(9)认为η_k之间相互独立。当所有M个观测时刻都取得似然函数极大值时，就可以取得p(η_1:M/θ)的极大值，所研究的问题等价转化为

利用Chapman-Kolmogorov方程，对p(η_k|θ)给出的数值解法，需要进行积分计算，不易实现。本发明利用蒙特卡罗方法，从p(ξ_k|θ)中抽取可以得到

式(11)由大数定理保证成立。

注意到式(11)并不是关于θ的显式表达式，直接求梯度进行p(η_k/θ)的极大值计算是不能实现的。设参数存在于一定范围内，θ∈[-B,B],B∈R⁺。对于时刻k，生成一组支持点在这N个支持点确定的可行解中找到最优值，将极值问题转化为有界集合内的参数寻优问题。

利用先验知识可以更准确地估计θ_k，为此考虑利用前一步参数的极大似然估计结果θ_k-1。考虑如下在θ_k-1的基础上更新θ_k的一阶自回归模型

θ_k＝θ_k-1+ε (12)

其中ε服从方差收敛的高斯分布ε～N(0,b_k)，或ε服从边界收敛的均匀分布ε～U[-b_k,b_k]，b_k＝k^-αb_k-1，b₀＝b，α(0＜α≤1)为衰减因子，b∈R⁺，经过证明θ_k收敛。

在时刻k，根据式(12)生成一组支持点对于每个按照式(11)计算得到则取其中的最大值，即

这样即可完成θ_k的收敛的迭代更新和极大似然函数的求取。

满足式(13)的θ_k为k时刻得到的参数θ_k的最优的估计值即

定义在含M个观测值的情况下，只要满足似然概率就将得到的θ_k作为第k步迭代的参数估计值，其中θ_k-1表示k-1时刻得到的参数值，θ_k表示k时刻得到的参数新值。具体算法如下：

1)令k＝1，初始化θ₀

2)期望阶段：

计算

计算N为所用θ_k的粒子数

3)最大化阶段：

4)令k＝k+1，并从第(2)步迭代计算，直至k＝M+1

参数估计器的思想是利用期望-最大化算法，使用搜索算法，在参数的可行解空间中确定出参数值。每个参数产生N个支持点，用于单参数辨识时，搜索空间是1×N维的；用于n个参数辨识时，搜索空间是Nⁿ维。

船舶参数估计器根据当前时刻k的状态估计值输出观测值η_k辨识得到当前时刻船舶参数的估计值

(4)利用粒子滤波器，根据每个时刻的受力信息、船舶的输出观测值和前一时刻辨识的参数值，估计船舶的状态值，具体为：

这里所说的粒子滤波器，指的是：

对于式(5)式(6)表示的非线性系统，由步骤(3)辨识出参数前一时刻值后，可以表示成下式

其中，

滤波器就是根据η_1:k和船舶的受力τ得到ξ_k的估计值。如果得到概率密度函数p(ξ_k|η_1:k,τ)，ξ_k的估计值将很容易得到。设p(ξ₀|η₀,τ)＝p(ξ₀)已知，即ξ₀的先验分布已知。

粒子滤波器离散实现的算法为：

2)计算粒子权重并且归一化

For i＝1:N

End

3)重采样

For j＝1:N

End

4)计算k+1时刻的状态粒子

5)令k＝k+1，从步骤(2)迭代计算，直至k＝M+1停止计算

利用船舶的受力τ、输出值η及待辨识参数的前一时刻的估计值，由粒子滤波器估计出当前时刻k的状态估计值

算法的整个过程的计算是收敛的、稳定的，参数估计值将不断趋于正确值。

实施例：

通过一个水下无人器ODIN进行实验，ODIN(Omni-directional IntelligentNavigator,全方位智能导航器)是由Hawaii大学研制的具有6个自由度的无人水下航行器。

该潜器具有对称性，即船体具有均匀质量分布。

1)ODIN作绕垂线旋转运动时参数辨识

通过推进器，使ODIN在水平面上作绕垂线的旋转运动，即可忽略垂荡、纵摇、横滚运动。同时考虑由于流体动力学引入的附加质量，考虑海浪干扰影响。在上述条件下，推导出ODIN的离散化模型。

ψ_k＝ψ_k-1+Δr_k+n_o,k (17)

其中：N----关于z轴旋转(艏摇)力矩，r---绕z轴旋转的(艏摇)角速度。

N_r及N_r|r|为待辨识参数。N_r和N_r|r|的真实值分别为-30和-80。

取r和ψ的初始值为零，Δ＝0.05s，n_s,k-1～N(0,0.0001)，n_o,k～N(0,0.01)，固定力矩τ_p＝30N，进行5s仿真，得到101个采样时刻。艏摇角速度r和艏摇角ψ的仿真结果分别如图2和图3所示。

利用参数估计器和粒子滤波器，对水下机器人进行参数辨识及滤波。取初始状态方差P₀＝0.1，N_r和N_r|r|的初值分别设为-45和-60，期望-最大化算法中取α＝0.015，利用方差收敛的高斯分布，其初始方差设为5。参数辨识和滤波所用支持点均取100。参数辨识结果如图4所示，粒子滤波结果如图5所示。从图4可以看出，参数辨识过程收敛，N_r和N_r|r|的参数辨识结果分别为-31.3和-71.6，接近真实值。

2)ODIN在水平面上作直线运动时参数辨识

通过推进器对ODIN施加力及力矩，使ODIN能够沿着水平面上的直线航行，此时r＝0。

在上述条件下，推导出ODIN的离散化模型。

为了得到观测数据，建立系统的Simulink仿真模型，如图7所示。仿真时取X_u|u|＝Y_v|v|＝-48。

仿真100s，共有M＝501个采样时刻，记录下每个采样时刻u,v,x,y的数据。

ODIN的仿真航行数据如图8–图11所示。

从图9和图10可以看出，仿真模型很好地完成了预定的沿直线航行的任务。图8得到的船舶在体坐标系中的状态真值，是为了与滤波算法估计的状态值作比较。从图11可以看出，艏摇角的观测结果受到噪声影响较大。

根据式(18)及(19)确定的离散含参非线性模型，利用参数估计器和粒子滤波器进行参数辨识和状态滤波，采用MATLAB进行编程实现。实验时取初始状态方差P₀＝0.01，参数初值为-60。粒子滤波中粒子数为300；期望-最大化算法中取α＝0.005，利用方差收敛的高斯分布，其初始方差设为1.5，结果如图12和图13所示。

X_u|u|＝Y_v|v|的真实值为-48，由图12可以看出辨识过程收敛，结果为-50，辨识结果很好。由图13可以看出，根据得到的辨识参数进行粒子滤波，得到的滤波后的状态值与真实的状态值接近，具有较高的精度。从图13中可以看出，虽然施加了u方向上的控制力τ_x以及控制力矩τ_p，u最后稳定在某一数值附近。这是因为方程(18)中的非线性因素导致的阻力随着速度u和v的增大逐渐增大，最后与控制力和力矩平衡。

Claims (1)

1.一种基于粒子滤波的船舶参数辨识方法，其特征在于，包括以下几个步骤：

(1)采集船舶的位移和姿态角信息作为输出观测量η，通过推进器、舵等机构对船舶施加的可测量的力及力矩控制输入，根据控制输入τ，输出观测量建立船舶模型，得到船舶模型中待辨识的参数θ；

船舶6自由度模型为：

其中，M＝M_RB+M_A为惯性矩阵，M_RB是船体的刚体质量矩阵，M_A是附加质量矩阵，C(ξ)＝C_RB(ξ)+C_A(ξ)为由地球自转引起的科氏力和向心力矩阵，C_RB(ξ)是刚体科氏力和向心力矩阵，C_A(ξ)是附加质量科氏力和向心力矩阵，D(ξ)为流体阻尼矩阵，g(η)为重力及浮力同时作用引起的恢复力及力矩，τ为控制输入，观测变量η表示船体在地坐标系中的位置与姿态角度信息，用体坐标系坐标原点在地坐标系中的坐标(x,y,z)表示船体在地坐标系中的位置，同时用地坐标系与体坐标系之间的欧拉角(φ,θ,ψ)表示船体的姿态，状态变量ξ表示船体的体坐标系线速度和角速度，其中(u,v,w)表示船体的线速度，(p,q,r)表示船体绕各轴旋转的角速度；

w＝p＝q＝0，同时考虑水下船体的工作情况和对称性，建立水下船体的平面3自由度模型，关于xz平面对称，则I_xy＝I_yz＝0；浮心和重心重合，

重新定义状态变量ξ＝[u v r]^T，状态变量通过粒子滤波器估计出来，η＝[x yψ]^TM_RB是一个对称阵，m为船体质量，包含自由流动空间的水质量，x_G和y_G为水平面上的船体的重心位置,I_z为转动惯量，矩阵中A_x，表示A对下标x方向上的附加质量，定义为

矩阵C(ξ)是一个反对称阵，其中

矩阵矩阵中带下标的元素与矩阵M_A中元素的意义相同，

向量g(η)表示重力和浮力及相应力矩，取g(η)＝[0 0 0]^T，

向量τ表示外力及外力矩作用，不包括环境干扰，τ＝[τ_X τ_Y τ_Z]^T，欧拉变换方程将体坐标系中观察到的物理量：前进线速度、侧移线速度、艏摇角速度变换到地坐标系中，得到x轴上速度y轴上速度及艏摇角速度

参数：

则船舶模型进一步表示为

采样时间间隔为Δ，采用一阶欧拉法进行离散化，得到船舶的非线性离散模型

ξ_k＝ξ_k-1+ΔM^-1[τ_k-1-C(ξ_k-1)ξ_k-1-D(ξ_k-1)ξ_k-1-g(η_k-1)],

η_k＝η_k-1+ΔJ(η_k-1)ξ_k,

船舶模型的输出观测值指每个时刻的η_k；

(2)根据船舶当前的状态估计值，初始时刻的状态估计值按正态分布随机产生的，输出观测量η，利用参数估计器，辨识船舶模型的参数估计值；

(3)根据辨识出的船舶模型的参数估计值，利用粒子滤波器，更新当前的状态估计值；

(4)重复步骤(2)-步骤(3)结束条件为k＝M，M为总的观测值的数量，得到每个采样时刻的参数估计值；

所述的根据船舶当前的状态值及输出观测量η，利用参数估计器，辨识船舶模型的参数估计值的方法为：

(1.1)令k＝1，初始化θ₀；

(1.2)计算和N为所用参数值θ_k的粒子数，其中θ_k-1表示k-1时刻得到的参数值，θ_k表示k时刻得到的参数新值，

θ_k＝θ_k-1+ε,

在时刻k，生成一组支持点对于每个计算得到进而得到相应的参数估计值

其中，为k时刻得到的参数θ_k的最优的估计值；

(1.3)令k＝k+1，并从第(2)步迭代计算，直至k＝M+1；

所述的根据辨识出的船舶模型前一时刻的参数估计值，利用粒子滤波器，更新当前的状态估计值的方法为：

(2.1)利用船舶的受力τ、输出值η及待辨识参数的前一时刻的估计值，由粒子滤波器产生粒子及其权重具体过程为：

(2.1.1)初始化粒子，

(2.1.2)计算粒子权重并且归一化

For i＝1:N，

End,

(2.1.3)重采样

For j＝1:N，

End

(2.1.4)计算k+1时刻的状态粒子

(2.1.5)令k＝k+1，从步骤(2.1.2)迭代计算，直至k＝M+1停止计算；

(2.2)由粒子滤波器估计出当前时刻k的状态估计值

1)ODIN作绕垂线旋转运动时参数辨识

通过推进器，使ODIN在水平面上作绕垂线的旋转运动，即可忽略垂荡、纵摇、横滚运动；在上述条件下，推导出ODIN的离散化模型；

ψ_k＝ψ_k-1+Δr_k+n_o,k

N_x，x＝r,r|r|，表示N对下标x方向上的附加质量，定义为

其中：N为关于z轴旋转力矩，r为绕z轴旋转的角速度；

N_r及N_r|r|为待辨识参数；N_r和N_r|r|的真实值分别为-30和-80；

取r和ψ的初始值为零，Δ＝0.05s，n_s,k-1～N(0,0.0001)，n_o,k～N(0,0.01)，固定力矩τ_p＝30N，进行5s仿真，得到101个采样时刻；

利用参数估计器和粒子滤波器，对水下机器人进行参数辨识及滤波；取初始状态方差P₀＝0.1，N_r和N_r|r|的初值分别设为-45和-60，期望-最大化算法中取α＝0.015，利用方差收敛的高斯分布，其初始方差设为5；参数辨识和滤波所用支持点均取100；参数辨识过程收敛，N_r和N_r|r|的参数辨识结果分别为-31.3和-71.6，接近真实值；

2)ODIN在水平面上作直线运动时参数辨识

通过推进器对ODIN施加力及力矩，使ODIN能够沿着水平面上的直线航行，此时r＝0；

在上述条件下，推导出ODIN的离散化模型；

η_k＝η_k-1+Δ[u_kcosψ_k-1-v_ksinψ_k-1 v_kcosψ_k-1+u_ksinψ_k-1]^T+n_o,k

ξ_k＝[u_k v_k]^T，η_k＝[x_k y_k]^T，n_s和no分别为过程噪声和观测噪声，并且取Δ＝0.2s，其中X_u|u|＝Y_v|v|是待辨识参数；利用MATLAB/Simulink建立ODIN的时域模型，使ODIN的初始艏摇角为30°，并且沿直线运动，u方向施加固定力τ_x＝173N，v方向控制力τ_y＝0N；由于ODIN自身的非线性及变量间的耦合，为使ODIN保持直线航行，需要施加控制力矩τ_p。