[发明专利]一种基于模糊控制和卡尔曼滤波的浮标深度控制算法

权利要求书

1.一种基于模糊控制和卡尔曼滤波的浮标深度控制算法，其特征在于，包括以下步骤：

第一步：构建浮标下潜过程的动力学方程

①仅考虑智能浮标垂直方向的运动过程，忽略其他方向运动的影响；

②不考虑海流对浮标垂向运动的作用；

③智能浮标为直径43cm球体，智能浮标下潜深度是4000m，将浮标设定为质点，浮标质量恒定不变；

④依据实验室内部数据确定浮标下潜的阻力；

⑤考虑温度对浮力的影响F₁；

⑥考虑压力对浮力的影响F₂；

⑦基于①-⑥，分析智能剖面浮标受力，浮标下潜时受到重力G、浮力F_f、阻力R、F₁和F₂五者的共同作用；

浮标浮在水面上的动力方程为：

其中，F_f(0)为剖面浮标初始状态所受浮力，m是浮标的总质量；g是重力加速度，ρ(0)为海平面处的海水密度值，V₀为剖面浮标初始状态下的体积，u(0)为剖面浮标初始速度，a(0)为剖面浮标初始加速度，H(0)剖面浮标初始深度；

浮标下潜的水动力方程为：

其中，u(t)为剖面浮标在t时刻的运行速度，u(t-Δt)为剖面浮标在t-Δt时刻的运行速度，h(t)为剖面浮标在t时刻内的位移，H为剖面浮标的总位移；

代入浮标油囊积分公式后的水动力方程为：

外油囊体积V(τ)表示为液压油的流速对时间t的积分，即浮标处于悬停状态，重力G等于初始浮力F_f(0)；ρ(h)表示深度h处的海水密度值；

公式(3)简化后为：

第二步：利用模糊控制算法控制浮标的加速度

采用的去模糊化方法为重心法，以深度误差e₁(t)及深度误差变化率ce₁(t)作为模糊控制器的输入变量，以剖面浮标的加速度a作为模糊控制器的输出变量u₁(t)，

输入变量深度误差e₁(t)的模糊子集划分为(ZO，N，Z，F，VF)，表示(零，小，中，大，很大)；

深度误差变化率ce₁(t)的模糊子集划分为(ZO，N，Z，F，VF)，表示(零，慢，中，快，很快)；

输出变量u₁(t)的模糊子集划分为(NF，NS，ZO，S，F)，表示(负快，负慢，零，慢，快)；

两个输入变量和输出变量的隶属度函数均采用三角函数，隶属度函数并不均匀分布，而是从左到右越来越宽变化的；

第三步：利用卡尔曼滤波消除测量误差和控制干扰

状态方程为：

s_k＝φ_k,k-1x_k-1+Γ_k-1w_k-1 (5)

在离散卡尔曼滤波的信号模型中，上式称为离散状态方程；其中，φ_k,k-1是系统从k-1时刻到k时刻的M*M一步状态转移矩阵，w_k-1是k-1时刻系统受到的L维扰动噪声矢量，Γ_k-1是k-1时刻反映扰动噪声矢量对系统状态矢量影响程度的M*L控制矩阵，s_k是k时刻的状态向量；

观测方程为：

x_k＝H_ks_k+n_k (6)

选择初始时刻状态矢量的均值作为初始状态滤波值估计值

其中，x_k是k时刻的N维观测信号矢量，H_k是k时刻的N*M观测矩阵，n_k是k时刻的N维观测噪声矢量；

初始状态滤波的均方误差阵为：

其中，E是期望，s₀是初始滤波值，μ_s0是s₀的期望；

离散卡尔曼滤波的时间更新方程为：

其中，是k时刻预测估计值，是上一时刻的状态估计值，u_k-1为噪声估计，A为状态转移矩阵，B为系统控制矩阵，是k时刻预测均误差协方差矩阵，Q是协方差矩阵；

离散卡尔曼滤波的卡尔曼增益为：

其中，K_k是k时刻的卡尔曼增益矩阵，H是观测矩阵，R₁则代表协方差矩阵；

离散卡尔曼滤波的测量更新方程为：

其中，是k时刻估计值，z_k是k时刻测量值，M_k是k时刻误差协方差矩阵。