CN111446898A - 基于模糊逻辑和扩张状态观测器的低成本auv速度估计方法 - Google Patents

Abstract

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

Description

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

技术领域

本发明涉及AUV速度估计领域，具体涉及一种基于模糊逻辑和扩张状态观测器的低成本AUV速度估计方法。

背景技术

自主式水下航行器(AUV)在国防、海洋调查和工业应用方面的广泛应用。随着AUV应用领域的不断扩大，低成本化已逐渐成为AUV的一种发展趋势，它也是衡量性价比的重要指标。但是，无论是标准型AUV还是低成本AUV，没有精确的导航子系统和有效的控制子系统就无法完成诸多任务。

自1970年代以来，AUV的导航子系统和控制子系统已得到逐步完善。这意味着随着导航与控制算法的改进，对速度，姿态和位置信息的依赖性变得越来越强。而与标准型AUV不同，低成本AUV缺乏用于改善导航和控制性能的关键传感器，例如多普勒测速仪(DVL)或高端惯性导航系统(INS)。低成本AUV的导航和控制子系统主要依靠姿态和航向参考系统(AHRS)，水流量计，全球定位系统(GPS)和电子罗盘。然而，事实上并不是所有的低成本AUV都配备水流计，因此在复杂的海洋条件下，未配备水流计的低成本AUV的速度信息受到海流影响。在这种情况下，AUV导航与控制的主要挑战是在没有DVL、INS或水流计的情况下估计AUV巡航速度，从而实现控制和导航任务。

对于AUV，由于不能直接测量速度，现有技术中有许多关于速度估计的相关方案，比如，“Estimating water current velocities by using a model-based high-gainobserver for an autonomous underwater vehicle”设计了一种基于高增益观测器和AUV流体动力学模型的方法利用惯性导航系统获取AUV的位置、加速度、角速度等信息估计出AUV附近的水柱速度来估计AUV的速度；“A Novel Hybrid Approach to Deal with DVLMalfunctions for Underwater Integrated Navigation Systems”运用偏最小二乘回归和支持向量回归建立AUV速度估计器；“Extended State Observer-Based IntegralSliding Mode Control for an Underwater Robot With Unknown Disturbances andUncertain Nonlinearities”基于多输入和多输出扩展状态观测器和外部视觉定位系统，开发了一种新型的ROV集成滑模控制器；利用外部视觉定位系统为ROV提供位置信息以估计相应的速度信息；“Implementation of a Hydrodynamic Model-Based NavigationSystem for a Low-Cost AUV Fleet”利用姿态和航向参考系统提供的AUV前进速度作为参考，并利用AUV水动力方程对其进行修正得到AUV的速度信息。

但是，对于现有的速度估计方法仍然存在以下问题：

1、通常情况下需要额外搭载水流计、INS、声纳、ADCP(多普勒流速剖面仪)、摄像头、USBL(超短基线)等能够测量出AUV当前的位置信息或者海流信息的测量设备，这些设备考虑到价格成本以及AUV体积结构等因素并不适用于低成本AUV；

2、在有海流干扰等情况下，搭建的模型以及所用的算法无法实时反映出当前环境对AUV速度的影响、且计算量大，无法准确实时估计低成本AUV的当前速度；而且在不使用INS或者测量水流设备的基础上可能会存在较大误差，降低控制效果或者导航精度。

发明内容

本发明针对现有AUV速度估计方法存在的缺陷，提出一种基于模糊逻辑和扩张状态观测器的低成本AUV速度估计方法，能够为低成本AUV实时提供前进速度，不需要额外的流速测量设备或者定位装置，成本低且精度高。

本发明是采用以下的技术方案实现的：一种基于模糊逻辑和扩张状态观测器的低成本AUV速度估计方法，包括以下步骤：

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

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

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

进一步的，所述步骤A具体通过以下方式实现：

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

进一步的，所述步骤B具体通过以下方式实现：

步骤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中得到准确的速度估计值具体采用以下方式：

步骤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速度的估计。

与现有技术相比，本发明的优点和积极效果在于：

本发明所提出的方案利用AUV内主推电机的数据作为数据源，利用模糊逻辑估计海流对AUV速度的影响程度，不需要额外的流速测量设备或者定位装置；并根据电机运动方程构建扩张状态观测器，利用实时测量的电机转速和电机电流来修正由模糊逻辑带来的估计误差，提高前进速度的估计精度；在有海流干扰的环境下，能够为低成本AUV实时提供前进速度，对于AUV的定速、导航、路径规划等具有重要意义。

附图说明

图1为本发明实施例所述AUV速度估计方法原理示意图；

图2为本发明实施例转速隶属度函数示意图；

图3为本发明实施例电流偏差隶属度函数示意图；

图4为本发明实施例输出隶属度函数示意图；

图5为本发明实施例模糊规则示意图。

具体实施方式

为了能够更加清楚地理解本发明的上述目的、特征和优点，下面结合附图及实施例对本发明做进一步说明。在下面的描述中阐述了很多具体细节以便于充分理解本发明，但是，本发明还可以采用不同于在此描述的其他方式来实施，因此，本发明并不限于下面公开的具体实施例。

本发明提出一种基于模糊逻辑和扩张状态观测器的低成本AUV速度估计方法，其基本原理为：

1、首先基于AUV运动模型得到螺旋桨转速与AUV航速的关系，获得在静水环境当前螺旋桨转速下对应的电机电流和AUV航速(前进速度)；

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

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

具体的，如图1所示：

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

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

关于前向速度估计模型和电机电流估计模型等为比较常用的形式，比如所述前向速度估计模型具体可基于AUV水平运动方程，忽略横滚、俯仰和垂荡简化得到的AUV轴向运动模型来获得的；所述电机电流估计模型根据所选择的电机参数以及电机运动方程、结合AUV运动方程和螺旋桨方程化简得到的，在此不做详细阐述。

2、基于模糊逻辑控制，根据AUV电机电流和转速信息估计出当前环境下海流作用在AUV前进速度上的影响，得到AUV前进速度的粗略估计值；

考虑到海流信息的不确定性，使用海流模型计算不仅缺乏必要的信息，还带来繁杂的计算过程，无法实现速度的实时估计。事实上除了测速传感器以外，与AUV速度关系最为密切的是AUV内的主推电机，因此使用电机的反馈信息来估计海流对速度的影响成为了一种可能。

利用电机信息来估计AUV速度关键在于如何将电机信息与AUV的速度联系起来。常规AUV使用的主推电机往往采用转速-电流双闭环控制，即在转速不变的情况下内电流随着负载增加而增加。众所周知，对于潜艇、船舶等海洋航行器当其顺流时，电机的负载小；当逆流时，电机负载大，这就为估计海流对航速的影响的估计提供了一定依据。本实施例中，为快速准确的获取海流对AUV速度的影响，引入模糊逻辑对速度影响进行估计。

(1)将第1步中估计的电机电流与测量得到的电机电流的偏差以及电机转速输入模糊逻辑中；

估计的电机电流与测量得到的电机电流的差送入图3所示的电流偏差隶属度函数中。该电流偏差是用来衡量AUV当前所处的环境与在无海流环境的偏差。电流偏差越大说明当前环境与无海流环境偏差越大，即偏差越大海流对AUV速度的影响越大，考虑主推电机性能，根据电流偏差值(比如以1.25A为间隔)将偏差度分为负大(NB)，负中(NM)，负小(NS)，零(ZO)，正小(PS)，正中(PM)，正大(PB)七部分，以针对所有可能存在的偏差进行估计。在设置隶属度函数时，为便于计算和增加所属的范围，在边缘部分(PB，NB)选择梯形函数，在其他部分选择三角形隶属度函数。

如图2所示，将输入至模糊逻辑的电机转速分为低(L)，中低(ML)，中高(MH)，高(H)四部分。转速的划分可依据AUV海试实验数据得到，在边缘部分(L,H)采用梯形函数，其他部分选择三角形函数。

(2)输入的电机转速和电流偏差值根据图2、图3对应的隶属度函数划分为不同的模糊量；

图2所示为电机转速隶属度函数及范围，本实施例中，L区域采用梯形函数范围是0-1500RPM，ML区域采用三角函数范围是1000RPM-2500RPM，MH采用三角函数范围是2000RPM-3500RPM，H采用梯形函数范围3500RPM-4500RPM；上述三角函数的中点均由上下限值求平均值得到。

图3中所示为电流偏差隶属度函数及范围，本实施例中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，上述三角函数的中点均由上下限值求平均值得到。

根据图5所示的模糊规则生成对应的模糊输出，规则库如下：

IF n＝L AND Δi＝NB THEN Δu＝PB。IF n＝ML AND Δi＝NB THEN Δu＝PM。IFn＝MH AND Δi＝NB THEN Δu＝PM。

IF n＝H AND Δi＝NB THEN Δu＝PS。IF n＝L AND Δi＝NM THEN Δu＝PM。IFn＝ML AND Δi＝NM THEN Δu＝PM。

IF n＝MH AND Δi＝NM THEN Δu＝PS。IF n＝H AND Δi＝NM THEN Δu＝PS。IFn＝L AND Δi＝NS THEN Δu＝PS。

IF n＝ML AND Δi＝NS THEN Δu＝PS。IF n＝MH AND Δi＝NS THEN Δu＝PS。IF n＝H AND Δi＝NS THEN Δu＝ZO。

IF n＝L AND Δi＝ZO THEN Δu＝ZO。IF n＝ML AND Δi＝ZO THEN Δu＝ZO。IFn＝MH AND Δi＝ZO THEN Δu＝ZO。

IF n＝H AND Δi＝ZO THEN Δu＝ZO。IF n＝L AND Δi＝PS THEN Δu＝NS。IFn＝ML AND Δi＝PS THEN Δu＝NS。

IF n＝MH AND Δi＝PS THEN Δu＝NS。IF n＝H AND Δi＝PS THEN Δu＝ZO。IFn＝L AND Δi＝PM THEN Δu＝NM。

IF n＝ML AND Δi＝PM THEN Δu＝NM。IF n＝MH AND Δi＝PM THEN Δu＝NS。IF n＝H AND Δi＝PM THEN Δu＝NS。

IF n＝L AND Δi＝PB THEN Δu＝NB。IF n＝ML AND Δi＝PB THEN Δu＝NM。IFn＝MH AND Δi＝PB THEN Δu＝NM。

IF n＝H AND Δi＝PB THEN Δu＝NS。

所述模糊规则是依据AUV的航行特点构建而成的，即在相同转速下海流对AUV速度影响越大，电机的电流越大，电流的偏差度越大；相同电流偏差下，转速越大，海流对速度的影响越小。当本实施例出的方法所搭载的AUV在中国沿海地区执行任务时，其前进速度偏差范围设定在-0.5m/s-0.5m/s(中国沿海海流速度大多在0.5m/s以下)；所述输出隶属度函数以0.1m/s为间隔，边缘部分(PB，NB)为梯形函数，其他部分设定为三角函数。

(3)将模糊输出量输入至图4所示的输出隶属度函数得到当前环境下海流对AUV前进速度的影响，图4所示为输出隶属度函数及范围，即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。上述三角函数的中点均由上下限值求平均值得到，将所得值与U做差得到当前环境下AUV前向速度的粗略估计值U’。

在不使用测量海量设备的基础上估计出当前的航速，在不安装位置和水流测量设备的基础上得到粗略的前进速度信息，可以在降低成本的基础上得到考虑到海流影响的速度，同时也避免了由于构建复杂模型造成的估计滞后性。

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

本实施例中，第2步所设计的模糊逻辑算法存在一定的局限性，必然会对估计结果造成误差，该误差由模糊逻辑、忽略的变量以及数据采集等等因素共同造成，因此对模糊逻辑以及其他因素造成的影响进行修正是十分必要的。目前常用的方法包括使用粒子群、遗传、退火、蚁群等优化算法对模糊逻辑进行优化，但在优化过程中由于计算量大，无法考虑AUV的实时状态，仅能进行离线优化。

本实施例中引入扩展状态观测器，利用电机信息，如反馈转速和反馈电流通过电机运动方程，将螺旋桨、AUV估计速度、电机电流、外界扰动及模糊逻辑对前进速度的影响结合起来，以反馈转速为参照估计速度影响值对U’进行修正获取准确速度估计值，过程如下所述。

(1)‘构建扩张状态观测器

电机的运动方程如公式(1)所示：

其中，T_e是电机电磁转矩，Q_M是电机的负载转矩，J_M是电机转动惯量，B_v是电机摩擦系数，n是电机转速。当电机工作状态稳定时，其中电机电磁转矩可由电流表示，负载转矩可等效为螺旋桨，电机加速度为0，电机转速为常数。而在AUV中电机显然不能维持稳定，因此将式(1)改写成式(2)：

其中Q是在电机转速n，AUV前进速U′下螺旋桨产生转矩，Q₀是由其他损耗造成的转矩损失；

式(2)中Q表示为：

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

式(2)中Q₀表示为：

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

其中ΔU是因为外部因素(如公式化简、模型计算、侧向海流、系统误差等因素)造成的速度损失。

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

其中

C＝1，

ξ＝T_e-Q。

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

其中

是转速n的观测值，

是W的观测值，e₁是观测输出

和输出y的差，ζ₁,ζ₂是观测增益。

(2)将U’、测量电机电流和电机转速输入至扩张状态观测器

根据公式(6)可以求得速度误差ΔU，与U’做差得到当前环境下AUV前进速度的准确值，完成对AUV速度的估计。

以上所述，仅是本发明的较佳实施例而已，并非是对本发明作其它形式的限制，任何熟悉本专业的技术人员可能利用上述揭示的技术内容加以变更或改型为等同变化的等效实施例应用于其它领域，比如利用电机的测量电流和模型估计电流得到的电流偏差和电机当前速度用来估计当前环境对AUV前进速度的影响的方法可以采用神经网络，线性、非线性逼近算法等；扩张状态观测器也可进过改进设计替换为其他观测器，如自适应观测，高增益观测器等等，但是凡是未脱离本发明技术方案内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化与改型，仍属于本发明技术方案的保护范围。

Claims (4)

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

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

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

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

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

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

3.根据权利要求2所述的基于模糊逻辑和扩张状态观测器的低成本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采用三角函数范围是-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。

4.根据权利要求3所述的基于模糊逻辑和扩张状态观测器的低成本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速度的估计。

Images