基于模糊逻辑和扩张状态观测器的低成本AUV速度估计方法.pdf

《基于模糊逻辑和扩张状态观测器的低成本AUV速度估计方法.pdf》由会员分享，可在线阅读，更多相关《基于模糊逻辑和扩张状态观测器的低成本AUV速度估计方法.pdf（13页完成版）》请在专利查询网上搜索。

1、(19)中华人民共和国国家知识产权局 (12)发明专利申请 (10)申请公布号 (43)申请公布日 (21)申请号 202010159727.X (22)申请日 2020.03.09 (71)申请人 中国海洋大学 地址 266000 山东省青岛市崂山区松岭路 238号 (72)发明人 何波高爽沈钺严天宏 (74)专利代理机构 青岛中天汇智知识产权代理 有限公司 37241 代理人 王丹丹雷斐 (51)Int.Cl. H02P 21/18(2016.01) H02P 21/13(2006.01) H02P 21/00(2016.01) (54)发明名称 基于模糊逻辑和扩张状态观测器的低成本 AU。

2、V速度估计方法 (57)摘要 本发明涉及一种基于模糊逻辑和扩张状态 观测器的低成本AUV速度估计方法， 首先基于AUV 运动模型得到螺旋桨转速与AUV航速的关系， 获 得在静水环境当前螺旋桨转速下对应的电机电 流和AUV航速； 然后基于模糊逻辑控制， 根据AUV 电机电流和转速信息估计出当前环境下海流作 用在AUV前进速度上的影响， 得到AUV前进速度的 粗略估计值； 最后基于电机运动方程构建扩张状 态观测器， 并将AUV前进速度的粗略估计值、 电机 电流作为输入， 得到外部扰动和模糊控制造成的 速度误差， 得到准确的速度估计值。 本方案为不 携带测速设备的低成本AUV中提供必要的准确的 速度。

3、信息， 通过该技术实时估计出当前AUV的前 进速度， 对于AUV的定速、 导航、 路径规划等具有 重要意义。 权利要求书2页 说明书8页 附图2页 CN 111446898 A 2020.07.24 CN 111446898 A 1.基于模糊逻辑和扩张状态观测器的低成本AUV速度估计方法， 其特征在于， 包括以下 步骤： 步骤A、 基于AUV运动模型得到螺旋桨转速与AUV航速的关系， 获得在静水环境当前螺旋 桨转速下对应的电机电流和AUV航速； 步骤B、 基于模糊逻辑控制， 根据AUV电机电流和转速信息估计出当前环境下海流作用 在AUV前进速度上的影响， 以修正根据螺旋桨转速计算得到的AUV航。

4、速， 进而得到AUV前进速 度的粗略估计值； 步骤C、 基于电机运动方程构建扩张状态观测器， 并将AUV前进速度的粗略估计值、 电机 电流作为输入， 得到外部扰动和模糊控制造成的速度误差， 得到准确的速度估计值。 2.根据权利要求1所述的基于模糊逻辑和扩张状态观测器的低成本AUV速度估计方法， 其特征在于： 所述步骤A具体通过以下方式实现： 首先获得电机当前转速n， 根据前向速度估计模型UF(n)， 利用电机转速估计出在当 前电机转速下在无海流的水域中AUV的航速U， 同时根据电机电流估计模型iG(n)估计出 当前电机转速下在无海流水域中电机的电流值i。 3.根据权利要求2所述的基于模糊逻辑和。

5、扩张状态观测器的低成本AUV速度估计方法， 其特征在于： 所述步骤B具体通过以下方式实现： 步骤B1、 将步骤A中估计的电机电流与测量得到的电机电流的偏差以及电机转速输入 模糊逻辑中； 根据电流偏差值将偏差度分为负大NB， 负中NM， 负小NS， 零ZO， 正小PS， 正中PM， 正大PB 七部分； 将电机转速分为低L， 中低ML， 中高MH， 高H四部分； 步骤B2、 输入的电机转速和电流偏差值对应的根据电机转速隶属度函数和电流偏差隶 属度函数划分为不同的模糊量； 电流偏差隶属度函数及范围为： NB采用梯形函数范围是-4A- -2.5A， NM采用三角函数 范围是-3A- -1.5A， NS。

6、采用三角函数范围是-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前。

7、进速度的粗略估计值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。 4.根据权利要求3所述的基于模糊逻辑和扩张状态观测器的低成本AUV速度估计方法， 其特征在于： 所述步骤C中得到准确的速度估计值具体采用以下方式：。

8、 步骤C1、 构建扩张状态观测器 权利要求书 1/2 页 2 CN 111446898 A 2 (1)确定电机的运动方程并将其写为： 其中， Te是电机电磁转矩， QM是电机的负载转矩， JM是电机转动惯量， Bv是电机摩擦系数， n是电机转速， Q是在电机转速n， AUV前进速U 下螺旋桨产生转矩， Q0是由其他损耗造成的转 矩损失； Q表示为： 其中D是螺旋桨直径， 是海水密度； Q0表示为： Q00.02U D4n (4) 其中U为因为外部因素造成的速度损失； (2)结合式(2),(3),(4)得到扩张状态方程： 其中C1， Te-Q； (3)根据扩张状态方程(5)得到扩张状态观测器： 。

9、其中 是转速n的观测值， 是W的观测值， e1是观测输出和输出y的差， 1, 2是观测增 益； 步骤C2、 将U 、 测量电机电流和电机转速输入至扩张状态观测器， 得到速度误差U， 与 U 做差得到当前环境下AUV前进速度的准确值， 完成对AUV速度的估计。 权利要求书 2/2 页 3 CN 111446898 A 3 基于模糊逻辑和扩张状态观测器的低成本AUV速度估计方法 技术领域 0001 本发明涉及AUV速度估计领域， 具体涉及一种基于模糊逻辑和扩张状态观测器的 低成本AUV速度估计方法。 背景技术 0002 自主式水下航行器(AUV)在国防、 海洋调查和工业应用方面的广泛应用。 随着A。

10、UV 应用领域的不断扩大， 低成本化已逐渐成为AUV的一种发展趋势， 它也是衡量性价比的重要 指标。 但是， 无论是标准型AUV还是低成本AUV， 没有精确的导航子系统和有效的控制子系统 就无法完成诸多任务。 0003 自1970年代以来， AUV的导航子系统和控制子系统已得到逐步完善。 这意味着随着 导航与控制算法的改进， 对速度， 姿态和位置信息的依赖性变得越来越强。 而与标准型AUV 不同， 低成本AUV缺乏用于改善导航和控制性能的关键传感器， 例如多普勒测速仪(DVL)或 高端惯性导航系统(INS)。 低成本AUV的导航和控制子系统主要依靠姿态和航向参考系统 (AHRS)， 水流量计，。

11、 全球定位系统(GPS)和电子罗盘。 然而， 事实上并不是所有的低成本AUV 都配备水流计， 因此在复杂的海洋条件下， 未配备水流计的低成本AUV的速度信息受到海流 影响。 在这种情况下， AUV导航与控制的主要挑战是在没有DVL、 INS或水流计的情况下估计 AUV巡航速度， 从而实现控制和导航任务。 0004 对于AUV， 由于不能直接测量速度， 现有技术中有许多关于速度估计的相关方案， 比如，“Estimating water current velocities by using a model-based high-gain observer for an autonomous un。

12、derwater vehicle” 设计了一种基于高增益观测器和AUV 流体动力学模型的方法利用惯性导航系统获取AUV的位置、 加速度、 角速度等信息估计出 AUV附近的水柱速度来估计AUV的速度；“A Novel Hybrid Approach to Deal with DVL Malfunctions for Underwater Integrated Navigation Systems” 运用偏最小二乘回归 和支持向量回归建立AUV速度估计器；“Extended State Observer-Based Integral Sliding Mode Control for an Unde。

13、rwater Robot With Unknown Disturbances and Uncertain Nonlinearities” 基于多输入和多输出扩展状态观测器和外部视觉定位系统， 开发了一种新型的ROV集成滑模控制器； 利用外部视觉定位系统为ROV提供位置信息以估计 相应的速度信息；“Implementation of a Hydrodynamic Model-Based Navigation System for a Low-Cost AUV Fleet” 利用姿态和航向参考系统提供的AUV前进速度作为参 考， 并利用AUV水动力方程对其进行修正得到AUV的速度信息。 0005 。

14、但是， 对于现有的速度估计方法仍然存在以下问题： 0006 1、 通常情况下需要额外搭载水流计、 INS、 声纳、 ADCP(多普勒流速剖面仪)、 摄像 头、 USBL(超短基线)等能够测量出AUV当前的位置信息或者海流信息的测量设备， 这些设备 考虑到价格成本以及AUV体积结构等因素并不适用于低成本AUV； 0007 2、 在有海流干扰等情况下， 搭建的模型以及所用的算法无法实时反映出当前环境 对AUV速度的影响、 且计算量大， 无法准确实时估计低成本AUV的当前速度； 而且在不使用 说明书 1/8 页 4 CN 111446898 A 4 INS或者测量水流设备的基础上可能会存在较大误差，。

15、 降低控制效果或者导航精度。 发明内容 0008 本发明针对现有AUV速度估计方法存在的缺陷， 提出一种基于模糊逻辑和扩张状 态观测器的低成本AUV速度估计方法， 能够为低成本AUV实时提供前进速度， 不需要额外的 流速测量设备或者定位装置， 成本低且精度高。 0009 本发明是采用以下的技术方案实现的： 一种基于模糊逻辑和扩张状态观测器的低 成本AUV速度估计方法， 包括以下步骤： 0010 步骤A、 基于AUV运动模型得到螺旋桨转速与AUV航速的关系， 获得在静水环境当前 螺旋桨转速下对应的电机电流和AUV航速； 0011 步骤B、 基于模糊逻辑控制， 根据AUV电机电流和转速信息估计出当。

16、前环境下海流 作用在AUV前进速度上的影响， 以修正根据螺旋桨转速计算得到的AUV航速， 进而得到AUV前 进速度的粗略估计值； 0012 步骤C、 基于电机运动方程构建扩张状态观测器， 并将AUV前进速度的粗略估计值、 电机电流作为输入， 得到外部扰动和模糊控制造成的速度误差， 得到准确的速度估计值。 0013 进一步的， 所述步骤A具体通过以下方式实现： 0014 首先获得电机当前转速n， 根据前向速度估计模型UF(n)， 利用电机转速估计出 在当前电机转速下在无海流的水域中AUV的航速U， 同时根据电机电流估计模型iG(n)估 计出当前电机转速下在无海流水域中电机的电流值i。 0015 。

17、进一步的， 所述步骤B具体通过以下方式实现： 0016 步骤B1、 将步骤A中估计的电机电流与测量得到的电机电流的偏差以及电机转速 输入模糊逻辑中； 0017 根据电流偏差值将偏差度分为负大(NB)， 负中(NM)， 负小(NS)， 零(ZO)， 正小(PS)， 正中(PM)， 正大(PB)七部分； 0018 将电机转速分为低(L)， 中低(ML)， 中高(MH)， 高(H)四部分； 0019 步骤B2、 输入的电机转速和电流偏差值对应的根据电机转速隶属度函数和电流偏 差隶属度函数划分为不同的模糊量； 0020 电流偏差隶属度函数及范围为： NB采用梯形函数范围是-4A-2.5A， NM采用三。

18、角函 数范围是-3A-1.5A， NS采用三角函数范围是-2A-0.5A， ZO采用三角函数范围是-1A-1A， PS 采用三角函数范围是0.5A-2.5A， PM采用三角函数范围是1.5A-3A， PB采用梯形函数范围是 2.5A-4A； 0021 电机转速隶属度函数及范围为： L区域采用梯形函数范围是0-1500RPM， ML区域采 用三角函数范围是1000RPM-2500RPM， MH采用三角函数范围是2000RPM-3500RPM， H采用梯形 函数范围3500RPM-4500RPM； 0022 步骤B3、 根据模糊逻辑规则生成对应的模糊输出， 并将模糊输出输入至输出隶属 度函数得到当。

19、前环境下海流对AUV前进速度的影响， 进而得到AUV前进速度的粗略估计值 U ； 0023 输出隶属度函数及范围为： NB采用梯形函数范围是-0.5m/s-0.35m/s， NM采用三 角函数范围是-0.4m/s-0.2m/s， NS采用三角函数范围是-0.25m/s-0.05m/s， ZO采用三角 说明书 2/8 页 5 CN 111446898 A 5 函数范围是-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。 0024 进一步的， 所述步骤C中。

20、得到准确的速度估计值具体采用以下方式： 0025 步骤C1、 构建扩张状态观测器 0026 (1)确定电机的运动方程并将其写为： 0027 0028 其中， Te是电机电磁转矩， QM是电机的负载转矩， JM是电机转动惯量， Bv是电机摩擦 系数， n是电机转速， Q是在电机转速n， AUV前进速U 下螺旋桨产生转矩， Q0是由其他损耗造 成的转矩损失； 0029 Q表示为： 0030 0031 其中D是螺旋桨直径， 是海水密度； 0032 Q0表示为： 0033 Q00.02U D4n (4) 0034 其中U为因为外部因素造成的速度损失； 0035 (2)结合式(2),(3),(4)得到扩。

21、张状态方程： 0036 0037其中C1， Te-Q； 0038 (3)根据扩张状态方程(5)得到扩张状态观测器： 0039 0040其中 是转速n的观测值， 是W的观测值， e1是观测输出 和输出y的差， 1, 2是观 测增益； 0041 步骤C2、 将U 、 测量电机电流和电机转速输入至扩张状态观测器， 得到速度误差 U， 与U 做差得到当前环境下AUV前进速度的准确值， 完成对AUV速度的估计。 0042 与现有技术相比， 本发明的优点和积极效果在于： 0043 本发明所提出的方案利用AUV内主推电机的数据作为数据源， 利用模糊逻辑估计 海流对AUV速度的影响程度， 不需要额外的流速测量。

22、设备或者定位装置； 并根据电机运动方 程构建扩张状态观测器， 利用实时测量的电机转速和电机电流来修正由模糊逻辑带来的估 说明书 3/8 页 6 CN 111446898 A 6 计误差， 提高前进速度的估计精度； 在有海流干扰的环境下， 能够为低成本AUV实时提供前 进速度， 对于AUV的定速、 导航、 路径规划等具有重要意义。 附图说明 0044 图1为本发明实施例所述AUV速度估计方法原理示意图； 0045 图2为本发明实施例转速隶属度函数示意图； 0046 图3为本发明实施例电流偏差隶属度函数示意图； 0047 图4为本发明实施例输出隶属度函数示意图； 0048 图5为本发明实施例模糊规。

23、则示意图。 具体实施方式 0049 为了能够更加清楚地理解本发明的上述目的、 特征和优点， 下面结合附图及实施 例对本发明做进一步说明。 在下面的描述中阐述了很多具体细节以便于充分理解本发明， 但是， 本发明还可以采用不同于在此描述的其他方式来实施， 因此， 本发明并不限于下面公 开的具体实施例。 0050 本发明提出一种基于模糊逻辑和扩张状态观测器的低成本AUV速度估计方法， 其 基本原理为： 0051 1、 首先基于AUV运动模型得到螺旋桨转速与AUV航速的关系， 获得在静水环境当前 螺旋桨转速下对应的电机电流和AUV航速(前进速度)； 0052 2、 基于模糊逻辑控制， 根据AUV电机电。

24、流和转速信息估计出当前环境下海流作用 在AUV前进速度上的影响， 以修正根据螺旋桨转速计算得到的AUV航速， 得到AUV前进速度的 粗略估计值； 0053 3、 基于电机运动方程构建扩张状态观测器， 并将AUV前进速度的粗略估计值、 电机 电流作为输入， 得到外部扰动和模糊控制造成的速度误差， 得到准确的速度估计值。 0054 具体的， 如图1所示： 0055 1、 基于AUV运动模型得到螺旋桨转速与AUV航速的关系， 获得在静水环境当前螺旋 桨转速下对应的电机电流和AUV航速； 0056 首先获得电机当前转速n， 根据前向速度估计模型UF(n)， 利用电机转速估计出 在当前电机转速下在无海流。

25、的水域中AUV的航速U， 同时根据电机电流估计模型iG(n)估 计出当前电机转速下在无海流水域中电机的电流值i。 0057 关于前向速度估计模型和电机电流估计模型等为比较常用的形式， 比如所述前向 速度估计模型具体可基于AUV水平运动方程， 忽略横滚、 俯仰和垂荡简化得到的AUV轴向运 动模型来获得的； 所述电机电流估计模型根据所选择的电机参数以及电机运动方程、 结合 AUV运动方程和螺旋桨方程化简得到的， 在此不做详细阐述。 0058 2、 基于模糊逻辑控制， 根据AUV电机电流和转速信息估计出当前环境下海流作用 在AUV前进速度上的影响， 得到AUV前进速度的粗略估计值； 0059 考虑到。

26、海流信息的不确定性， 使用海流模型计算不仅缺乏必要的信息， 还带来繁 杂的计算过程， 无法实现速度的实时估计。 事实上除了测速传感器以外， 与AUV速度关系最 为密切的是AUV内的主推电机， 因此使用电机的反馈信息来估计海流对速度的影响成为了 说明书 4/8 页 7 CN 111446898 A 7 一种可能。 0060 利用电机信息来估计AUV速度关键在于如何将电机信息与AUV的速度联系起来。 常 规AUV使用的主推电机往往采用转速-电流双闭环控制， 即在转速不变的情况下内电流随着 负载增加而增加。 众所周知， 对于潜艇、 船舶等海洋航行器当其顺流时， 电机的负载小； 当逆 流时， 电机负载。

27、大， 这就为估计海流对航速的影响的估计提供了一定依据。 本实施例中， 为 快速准确的获取海流对AUV速度的影响， 引入模糊逻辑对速度影响进行估计。 0061 (1)将第1步中估计的电机电流与测量得到的电机电流的偏差以及电机转速输入 模糊逻辑中； 0062 估计的电机电流与测量得到的电机电流的差送入图3所示的电流偏差隶属度函数 中。 该电流偏差是用来衡量AUV当前所处的环境与在无海流环境的偏差。 电流偏差越大说明 当前环境与无海流环境偏差越大， 即偏差越大海流对AUV速度的影响越大， 考虑主推电机性 能， 根据电流偏差值(比如以1.25A为间隔)将偏差度分为负大(NB)， 负中(NM)， 负小(。

28、NS)， 零 (ZO)， 正小(PS)， 正中(PM)， 正大(PB)七部分， 以针对所有可能存在的偏差进行估计。 在设置 隶属度函数时， 为便于计算和增加所属的范围， 在边缘部分(PB， NB)选择梯形函数， 在其他 部分选择三角形隶属度函数。 0063 如图2所示， 将输入至模糊逻辑的电机转速分为低(L)， 中低(ML)， 中高(MH)， 高(H) 四部分。 转速的划分可依据AUV海试实验数据得到， 在边缘部分(L,H)采用梯形函数， 其他部 分选择三角形函数。 0064 (2)输入的电机转速和电流偏差值根据图2、 图3对应的隶属度函数划分为不同的 模糊量； 0065 图2所示为电机转速隶。

29、属度函数及范围， 本实施例中， L区域采用梯形函数范围是 0-1500RPM， ML区域采用三角函数范围是1000RPM-2500RPM， MH采用三角函数范围是 2000RPM-3500RPM， H采用梯形函数范围3500RPM-4500RPM； 上述三角函数的中点均由上下限 值求平均值得到。 0066 图3中所示为电流偏差隶属度函数及范围， 本实施例中NB采用梯形函数范围是- 4A- 0067 -2.5A， NM采用三角函数范围是-3A-1.5A， NS采用三角函数范围是-2A-0.5A， ZO 采用三角函数范围是-1A-1A， PS采用三角函数范围是0.5A-2.5A， PM采用三角函数。

30、范围是 1.5A-3A， PB采用梯形函数范围是2.5A-4A， 上述三角函数的中点均由上下限值求平均值得 到。 0068 根据图5所示的模糊规则生成对应的模糊输出， 规则库如下： 0069 IF nL AND iNB THEN uPB。 IF nML AND iNB THEN uPM。 IF nMH AND iNB THEN uPM。 0070 IF nH AND iNB THEN uPS。 IF nL AND iNM THEN uPM。 IF nML AND iNM THEN uPM。 0071 IF nMH AND iNM THEN uPS。 IF nH AND iNM THEN uPS。

31、。 IF nL AND iNS THEN uPS。 0072 IF nML AND iNS THEN uPS。 IF nMH AND iNS THEN uPS。 IF nH AND iNS THEN uZO。 说明书 5/8 页 8 CN 111446898 A 8 0073 IF nL AND iZO THEN uZO。 IF nML AND iZO THEN uZO。 IF nMH AND iZO THEN uZO。 0074 IF nH AND iZO THEN uZO。 IF nL AND iPS THEN uNS。 IF nML AND iPS THEN uNS。 0075 IF n。

32、MH AND iPS THEN uNS。 IF nH AND iPS THEN uZO。 IF nL AND iPM THEN uNM。 0076 IF nML AND iPM THEN uNM。 IF nMH AND iPM THEN uNS。 IF nH AND iPM THEN uNS。 0077 IF nL AND iPB THEN uNB。 IF nML AND iPB THEN uNM。 IF nMH AND iPB THEN uNM。 0078 IF nH AND iPB THEN uNS。 0079 所述模糊规则是依据AUV的航行特点构建而成的， 即在相同转速下海流对AUV速度。

33、 影响越大， 电机的电流越大， 电流的偏差度越大； 相同电流偏差下， 转速越大， 海流对速度的 影响越小。 当本实施例出的方法所搭载的AUV在中国沿海地区执行任务时， 其前进速度偏差 范围设定在-0.5m/s-0.5m/s(中国沿海海流速度大多在0.5m/s以下)； 所述输出隶属度函数 以0.1m/s为间隔， 边缘部分(PB， NB)为梯形函数， 其他部分设定为三角函数。 0080 (3)将模糊输出量输入至图4所示的输出隶属度函数得到当前环境下海流对AUV前 进速度的影响， 图4所示为输出隶属度函数及范围， 即NB采用梯形函数范围是-0.5m/s- 0.35m/s， NM采用三角函数范围是-0。

34、.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。 上述三 角函数的中点均由上下限值求平均值得到， 将所得值与U做差得到当前环境下AUV前向速度 的粗略估计值U 。 0081 在不使用测量海量设备的基础上估计出当前的航速， 在不安装位置和水流测量设 备的基础上得到粗略的前进速度信息， 可以在降低成本的基础上得到考虑到海流影响的速 度， 同时也避。

35、免了由于构建复杂模型造成的估计滞后性。 0082 3、 基于电机运动方程构建扩张状态观测器， 并将AUV前进速度的粗略估计值、 电机 电流作为输入， 得到外部扰动和模糊控制造成的速度误差， 进而得到准确的速度估计值 0083 本实施例中， 第2步所设计的模糊逻辑算法存在一定的局限性， 必然会对估计结果 造成误差， 该误差由模糊逻辑、 忽略的变量以及数据采集等等因素共同造成， 因此对模糊逻 辑以及其他因素造成的影响进行修正是十分必要的。 目前常用的方法包括使用粒子群、 遗 传、 退火、 蚁群等优化算法对模糊逻辑进行优化， 但在优化过程中由于计算量大， 无法考虑 AUV的实时状态， 仅能进行离线优。

36、化。 0084 本实施例中引入扩展状态观测器， 利用电机信息， 如反馈转速和反馈电流通过电 机运动方程， 将螺旋桨、 AUV估计速度、 电机电流、 外界扰动及模糊逻辑对前进速度的影响结 合起来， 以反馈转速为参照估计速度影响值对U 进行修正获取准确速度估计值， 过程如下 所述。 0085 (1) 构建扩张状态观测器 0086 电机的运动方程如公式(1)所示： 说明书 6/8 页 9 CN 111446898 A 9 0087 0088 其中， Te是电机电磁转矩， QM是电机的负载转矩， JM是电机转动惯量， Bv是电机摩擦 系数， n是电机转速。 当电机工作状态稳定时， 其中电机电磁转矩可由。

37、电流表示， 负载转矩可 等效为螺旋桨， 电机加速度为0， 电机转速为常数。 而在AUV中电机显然不能维持稳定， 因此 将式(1)改写成式(2)： 0089 0090 其中Q是在电机转速n， AUV前进速U 下螺旋桨产生转矩， Q0是由其他损耗造成的转 矩损失； 0091 式(2)中Q表示为： 0092 0093 其中D是螺旋桨直径， 是海水密度； 0094 式(2)中Q0表示为： 0095 Q00.02U D4n (4) 0096 其中U是因为外部因素(如公式化简、 模型计算、 侧向海流、 系统误差等因素)造 成的速度损失。 0097 结合式(2),(3),(4)得到扩张状态方程(5)。 00。

38、98 0099其中C1， Te-Q。 0100 根据扩张状态方程(5)得到扩张状态观测器(6) 0101 0102其中是转速n的观测值，是W的观测值， e1是观测输出和输出y的差， 1, 2是 观测增益。 0103 (2)将U 、 测量电机电流和电机转速输入至扩张状态观测器 0104 根据公式(6)可以求得速度误差U， 与U 做差得到当前环境下AUV前进速度的准 确值， 完成对AUV速度的估计。 0105 以上所述， 仅是本发明的较佳实施例而已， 并非是对本发明作其它形式的限制， 任 何熟悉本专业的技术人员可能利用上述揭示的技术内容加以变更或改型为等同变化的等 说明书 7/8 页 10 CN 。

39、111446898 A 10 效实施例应用于其它领域， 比如利用电机的测量电流和模型估计电流得到的电流偏差和电 机当前速度用来估计当前环境对AUV前进速度的影响的方法可以采用神经网络， 线性、 非线 性逼近算法等； 扩张状态观测器也可进过改进设计替换为其他观测器， 如自适应观测， 高增 益观测器等等， 但是凡是未脱离本发明技术方案内容， 依据本发明的技术实质对以上实施 例所作的任何简单修改、 等同变化与改型， 仍属于本发明技术方案的保护范围。 说明书 8/8 页 11 CN 111446898 A 11 图1 图2 图3 说明书附图 1/2 页 12 CN 111446898 A 12 图4 图5 说明书附图 2/2 页 13 CN 111446898 A 13 。