[发明专利]基于模糊逻辑和扩张状态观测器的低成本AUV速度估计方法

权利要求书

1.基于模糊逻辑和扩张状态观测器的低成本AUV速度估计方法，其特征在于，包括以下步骤：

步骤A、基于AUV运动模型得到螺旋桨转速与AUV航速的关系，获得在静水环境当前螺旋桨转速下对应的电机电流和AUV航速；

步骤B、基于模糊逻辑控制，根据AUV电机电流和转速信息估计出当前环境下海流作用在AUV前进速度上的影响，以修正根据螺旋桨转速计算得到的AUV航速，进而得到AUV前进速度的粗略估计值：

步骤B1、将步骤A中估计的电机电流与测量得到的电机电流的偏差以及电机转速输入模糊逻辑中；根据电流偏差值将偏差度分为负大NB，负中NM，负小NS，零ZO，正小PS，正中PM，正大PB七部分；将电机转速分为低L，中低ML，中高MH，高H四部分；

步骤B2、输入的电机转速和电流偏差值对应的根据电机转速隶属度函数和电流偏差隶属度函数划分为不同的模糊量；

电流偏差隶属度函数及范围为：NB采用梯形函数范围是-4A--2.5A，NM采用三角函数范围是-3A--1.5A，NS采用三角函数范围是-2A--0.5A，ZO采用三角函数范围是-1A-1A，PS采用三角函数范围是0.5A-2.5A，PM采用三角函数范围是1.5A-3A，PB采用梯形函数范围是2.5A-4A；

电机转速隶属度函数及范围为：L区域采用梯形函数范围是0-1500RPM，ML区域采用三角函数范围是1000RPM-2500RPM，MH采用三角函数范围是2000RPM-3500RPM，H采用梯形函数范围3500RPM-4500RPM；

步骤B3、根据模糊逻辑规则生成对应的模糊输出，并将模糊输出输入至输出隶属度函数得到当前环境下海流对AUV前进速度的影响，进而得到AUV前进速度的粗略估计值U’；

输出隶属度函数及范围为：NB采用梯形函数范围是-0.5m/s--0.35m/s，NM采用三角函数范围是-0.4m/s--0.2m/s，NS采用三角函数范围是-0.25m/s--0.05m/s，ZO采用三角函数范围是-0.1m/s-0.1m/s，PS采用三角函数范围是0.05m/s-0.25m/s，PM采用三角函数范围是0.2m/s-0.4m/s，PB采用梯形函数范围是0.35m/s-0.5m/s；

步骤C、基于电机运动方程构建扩张状态观测器，并将AUV前进速度的粗略估计值、电机电流作为输入，得到外部扰动和模糊控制造成的速度误差，得到准确的速度估计值。

2.根据权利要求1所述的基于模糊逻辑和扩张状态观测器的低成本AUV速度估计方法，其特征在于：所述步骤A具体通过以下方式实现：

首先获得电机当前转速n，根据前向速度估计模型U＝F(n)，利用电机转速估计出在当前电机转速下在无海流的水域中AUV的航速U，同时根据电机电流估计模型i＝G(n)估计出当前电机转速下在无海流水域中电机的电流值i。

3.根据权利要求2所述的基于模糊逻辑和扩张状态观测器的低成本AUV速度估计方法，其特征在于：所述步骤C中得到准确的速度估计值具体采用以下方式：

步骤C1、构建扩张状态观测器

(1)确定电机的运动方程并将其写为：

其中，T_e是电机电磁转矩，Q_M是电机的负载转矩，J_M是电机转动惯量，B_v是电机摩擦系数，n是电机转速，Q是在电机转速n，AUV前进速U'下螺旋桨产生转矩，Q₀是由其他损耗造成的转矩损失；

Q表示为：

其中D是螺旋桨直径，ρ是海水密度；

Q₀表示为：

Q₀＝0.02ΔUρD⁴n (4)

其中ΔU为因为外部因素造成的速度损失；

(2)结合式(2),(3),(4)得到扩张状态方程：

其中C＝1，ξ＝T_e-Q；

(3)根据扩张状态方程(5)得到扩张状态观测器：

其中是转速n的观测值，是W的观测值，e₁是观测输出和输出y的差，ζ₁,ζ₂是观测增益；

步骤C2、将U’、测量电机电流和电机转速输入至扩张状态观测器，得到速度误差ΔU，与U’做差得到当前环境下AUV前进速度的准确值，完成对AUV速度的估计。