本发明提供一种欠驱动水下航行器的双层抗扰深度跟踪控制方法及系统，将欠驱动水下航行器的深度跟踪分为运动学层制导和动力学层纵倾跟踪。在运动学层使用自适应视线角制导将深度误差转化为期望纵倾角，并估计和补偿攻角以抵抗真实攻角不可测引入的干扰。在此基础上，在动力学层使用自抗扰‑滑模纵倾跟踪方法，使用自抗扰框架观测包括未知动力学模型和环境干扰的复合干扰，将模型补偿为统一的积分串联型，最后设计滑模控制律抵抗观测误差，计算升降舵控制舵角。上述控制方法部署于航行器艇载计算机上，读取传感器反馈的状态数据，控制升降舵执行机构，构成双层抗扰深度跟踪控制系统，实现航行器鲁棒深度跟踪航行。
欠驱动水下航行器的双层抗扰深度跟踪控制方法及系统
技术领域
[0001] 本发明属于航行器自动控制领域，更具体地，涉及一种欠驱动水下航行器的双层抗扰深度跟踪控制方法及系统。
背景技术
[0002] 自主水下航行器在海洋调查和水下作业方面发挥着至关重要的作用，如资源勘探、海底测绘、水下打捞和海洋结构维护等。特别是欠驱动自主水下航行器，由于其结构简单和水动力性能好的优势，在勘察和搜索类任务中更是占领着重要地位。自主水下航行器的运动控制直接关系到上述任务执行的高效性和任务质量。特别是深度跟踪能力，更为必需和关键的运动能力之一。欠驱动自主水下航行器的深度跟踪控制面临着欠驱动、模型非线性、运动强耦合，外界环境干扰等困难，使得深度跟踪精度难以保证。因此，欠驱动水下自主航行器深度跟踪控制的抗扰能力显得尤为关键。
发明内容
[0003] 针对现有技术的缺陷，本发明的目的在于提供一种欠驱动水下航行器的双层抗扰深度跟踪控制方法及系统，旨在解决现有水下航行器在有干扰情况下的深度跟踪精度难以保证的问题。
[0004] 为实现上述目的，第一方面，本发明提供了一种欠驱动水下航行器的双层抗扰深度跟踪控制方法，包括如下步骤：
[0005] 确定水下航行器的航行信息；所述航行信息包括：期望深度、当前深度、当前航行速度及当前纵倾角；
[0006] 根据期望深度和当前深度确定深度跟踪误差，基于自适应视线角制导律将深度跟踪误差转化为期望纵倾角；所述自适应视线角制导律包括自适应参数，以对水下航行器的真实攻角进行估计，减少忽略攻角引入的运动干扰；所述自适应参数根据当前航行速度和深度跟踪误差确定；
[0007] 确定自抗扰‑滑模纵倾跟踪控制律，其包括：扩展状态观测器和滑模反馈控制律；所述扩展状态观测器用于观测水下航行器的复合干扰；所述复合干扰为由航行器内部结构和外部环境引入的动力干扰总和；所述滑模反馈控制律的设计过程考虑所述扩展状态观测器的观测误差，用于结合所述期望纵倾角计算水下航行器升降舵的舵角，以克服所述观测误差；
[0008] 结合所述自抗扰‑滑模纵倾跟踪控制律确定复合干扰的观测量和滑模反馈控制律计算的舵角，并基于所述复合干扰观测量和滑模反馈控制律计算的舵角计算升降舵的最终舵角，使得升降舵按照所述最终舵角航行，以使水下航行器按照所述期望深度稳定航行；按照所述自适应视线角制导律和自抗扰‑滑模纵倾跟踪控制律计算升降舵最终舵角使得水下航行器抵抗运动干扰和动力干扰的能力提升，提高了水下航行器深度跟踪的精度。
[0009] 在一个可选的示例中，所述根据期望深度和当前深度确定深度跟踪误差，基于自适应视线角制导律将深度跟踪误差转化为期望纵倾角，具体为：
[0010] 将期望深度zd与当前深度z相减得到所述深度跟踪误差ez；
[0011] 基于自适应视线角制导律将深度跟踪误差ez转化为期望纵倾角θd，具体包括如下公式：
[0012]
[0013] 式中，Δ为前视距离，为自适应参数；
[0014] 按照如下公式自适应变化：
[0015]
[0016] 式中，U为航行器的航行速度，γ为自适应增益。
[0017] 在一个可选的示例中，所述扩展状态观测器用于观测水下航行器的复合干扰，具体为：
[0018] 水下航行器的纵倾运动模型为：
[0019]
[0020] 式中，表示纵倾角加速率； 表示由航行器内部结构引起的力矩对应的加速率，θ表示航行器纵倾角、表示航行器纵倾角速度、U表示航行速度、Iyy表示航行器惯性矩；dθ为外部环境干扰；b0为舵效系数，δ为最终舵角；
[0021] 将 考虑为复合干扰并使用如下扩展状态观测器将复合干扰迭代计算出来：
[0022]
[0023] 式中，z1，z2和z3分别是对航行器纵倾角θ，纵倾角速度 和复合干扰的观测量，[t]表示第t步迭代计算，h为积分步长，l1，l2和l3为扩展状态观测器的观测增益。
[0024] 在一个可选的示例中，所述自抗扰‑滑模纵倾跟踪控制律还包括：跟踪微分器，其用于确定所述期望纵倾角的数值微分v2和期望纵倾角的跟踪信号v1；
[0025] 所述滑模反馈控制律的设计过程考虑所述扩展状态观测器的观测误差，用于结合所述期望纵倾角计算水下航行器升降舵的舵角，具体为：
[0026] 定义滑模面s＝ce1+e2，其中，c为权重系数，e1为纵倾角跟踪误差，e1＝v1‑z1，e2为纵倾角速度跟踪误差，e2＝v2‑z2；
[0027] 根据指数趋近律设计方法得出滑模反馈控制律：
[0028]
[0029] 式中，δ0为滑模反馈控制律计算的舵角，k参数控制着误差以指数收敛到0的速率，η为鲁棒项系数，用以克服观测误差；
[0030] 通过如下公式计算v1和v2；
[0031]
[0032] 式中，r为跟踪加速度系数，h为计算步长，[t]表示第t步迭代计算，fhan(x1,x2,r,h)为最速控制综合函数，x1和x2为中间变量，分别表示为x1＝v1[t]‑θd，x2＝v2[t]，θd为期望纵倾角；
[0033] 其中，fhan(x1,x2,r,h)通过如下公式计算：
[0034]
[0035] 式中，d、d0、y、a0以及a均为中间变量。
[0036] 在一个可选的示例中，基于所述复合干扰观测量和滑模反馈控制律计算的舵角计算升降舵的最终舵角δ，具体为：
[0037] 第二方面，本发明提供了一种欠驱动水下航行器的双层抗扰深度跟踪控制系统，包括：
[0038] 航行信息确定单元，用于确定水下航行器的航行信息；所述航行信息包括：期望深度、当前深度、当前航行速度及当前纵倾角；
[0039] 期望纵倾角确定单元，用于根据期望深度和当前深度确定深度跟踪误差，基于自适应视线角制导律将深度跟踪误差转化为期望纵倾角；所述自适应视线角制导律包括自适应参数，以对水下航行器的真实攻角进行估计，减少忽略攻角引入的运动干扰；所述自适应参数根据当前航行速度和深度跟踪误差确定；
[0040] 纵倾跟踪控制律确定单元，用于确定自抗扰‑滑模纵倾跟踪控制律，其包括：扩展状态观测器和滑模反馈控制律；所述扩展状态观测器用于观测水下航行器的复合干扰；所述复合干扰为由航行器内部结构和外部环境引入的动力干扰总和；所述滑模反馈控制律的设计过程考虑所述扩展状态观测器的观测误差，用于结合所述期望纵倾角计算水下航行器升降舵的舵角，以克服所述观测误差；
[0041] 升降舵舵角确定单元，用于结合所述自抗扰‑滑模纵倾跟踪控制律确定复合干扰的观测量和滑模反馈控制律计算的舵角，并基于所述复合干扰观测量和滑模反馈控制律计算的舵角计算升降舵的最终舵角，使得升降舵按照所述最终舵角航行，以使水下航行器按照所述期望深度稳定航行；按照所述自适应视线角制导律和自抗扰‑滑模纵倾跟踪控制律计算升降舵最终舵角使得水下航行器抵抗运动干扰和动力干扰的能力提升，提高了水下航行器深度跟踪的精度。
[0042] 在一个可选的示例中，所述期望纵倾角确定单元根据期望深度和当前深度确定深度跟踪误差，基于自适应视线角制导律将深度跟踪误差转化为期望纵倾角，具体为：将期望深度zd与当前深度z相减得到所述深度跟踪误差ez；基于自适应视线角制导律将深度跟踪误差ez转化为期望纵倾角θd，具体包括如下公式： 式中，Δ为前视距离，为自适应参数；按照如下公式自适应变化： 式中，U为航行器的航行速度，γ
为自适应增益。
[0043] 在一个可选的示例中，所述扩展状态观测器用于观测水下航行器的复合干扰，具体为：
[0044] 水下航行器的纵倾运动模型为：
[0045]
[0046] 式中，表示纵倾角加速率； 表示由航行器内部结构引起的力矩对应的加速率，θ表示航行器纵倾角、表示航行器纵倾角速度、U表示航行速度、Iyy表示航行器惯性矩；dθ为外部环境干扰；b0为舵效系数，δ为最终舵角；
[0047] 将 考虑为复合干扰并使用如下扩展状态观测器将复合干扰迭代计算出来：
[0048]
[0049] 式中，z1，z2和z3分别是对航行器纵倾角θ，纵倾角速度 和复合干扰的观测量，[t]表示第t步迭代计算，h为积分步长，l1，l2和l3为扩展状态观测器的观测增益。
[0050] 在一个可选的示例中，所述自抗扰‑滑模纵倾跟踪控制律还包括：跟踪微分器，其用于确定所述期望纵倾角的数值微分v2和期望纵倾角的跟踪信号v1；
[0051] 所述滑模反馈控制律的设计过程考虑所述扩展状态观测器的观测误差，用于结合所述期望纵倾角计算水下航行器升降舵的舵角，具体为：
[0052] 定义滑模面s＝ce1+e2，其中，c为权重系数，e1为纵倾角跟踪误差，e1＝v1‑z1，e2为纵倾角速度跟踪误差，e2＝v2‑z2；
[0053] 根据指数趋近律设计方法得出滑模反馈控制律：
[0054]
[0055] 式中，δ0为滑模反馈控制律计算的舵角，k参数控制着误差以指数收敛到0的速率，η为鲁棒项系数，用以克服观测误差；
[0056] 通过如下公式计算v1和v2；
[0057]
[0058] 式中，r为跟踪加速度系数，h为计算步长，[t]表示第t步迭代计算，fhan(x1,x2,r,h)为最速控制综合函数，x1和x2为中间变量，分别表示为x1＝v1[t]‑θd，x2＝v2[t]，θd为期望纵倾角；
[0059] 其中，fhan(x1,x2,r,h)通过如下公式计算：
[0060]
[0061] 式中，d、d0、y、a0以及a均为中间变量。
[0062] 在一个可选的示例中，所述升降舵舵角确定单元基于所述复合干扰观测量和滑模反馈控制律计算的舵角计算升降舵的最终舵角δ，具体为：
[0063] 总体而言，通过本发明所构思的以上技术方案与现有技术相比，具有以下有益效果：
[0064] 本发明提供一种欠驱动水下航行器的双层抗扰深度跟踪控制方法及系统，将欠驱动水下航行器的深度跟踪分为运动学层制导和动力学层纵倾跟踪。在运动学层使用自适应视线角制导将深度误差转化为期望纵倾角，并估计和补偿攻角以抵抗真实攻角不可测引入的干扰。在此基础上，在动力学层使用自抗扰‑滑模纵倾跟踪方法，使用自抗扰框架观测包括未知动力学模型和环境干扰的复合干扰，将模型补偿为统一的积分串联型，最后设计滑模控制律抵抗观测误差，计算升降舵控制舵角。上述控制方法部署于航行器艇载计算机上，读取传感器反馈的深度和纵倾角数据，控制升降舵执行机构，构成双层抗扰深度跟踪控制系统，实现航行器鲁棒深度跟踪航行。
附图说明
[0065] 图1为本发明实施例提供的欠驱动水下航行器的双层抗扰深度跟踪控制方法流程图；
[0066] 图2为本发明实施例提供的欠驱动自主水下航行器的双层抗扰深度跟踪控制方法框图；
[0067] 图3为本发明实施例提供的典型欠驱动自主水下航行器示意图；
[0068] 图4a为本发明实施例提供的实艇试验的深度跟踪曲线图；
[0069] 图4b为本发明实施例提供的实艇试验的纵倾跟踪和观测纵倾曲线图；
[0070] 图4c为本发明实施例提供的实艇试验的纵倾角速度跟踪和观测角速度曲线图；
[0071] 图4d为本发明实施例提供的实艇试验的复合干扰观测值曲线图；
[0072] 图4e为本发明实施例提供的实艇试验的攻角估计值曲线图；
[0073] 图5为本发明实施例提供的欠驱动水下航行器的双层抗扰深度跟踪控制系统架构图。
具体实施方式
[0074] 为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。
[0075] 本发明公开了欠驱动水下航行器的双层抗扰深度跟踪控制方法及系统，属于自动控制领域。本发明将欠驱动水下航行器的深度跟踪分为运动学层制导和动力学层纵倾跟踪。在运动学层使用自适应视线角制导将深度误差转化为期望纵倾角，并估计和补偿攻角以抵抗真实攻角不可测引入的干扰。在此基础上，在运动学层使用自抗扰‑滑模纵倾跟踪方法，使用自抗扰框架观测包括未知动力学模型和环境干扰的复合干扰，将模型补偿为统一的积分串联型，最后设计滑模控制律抵抗观测误差，计算升降舵控制舵角。上述控制方法部署于航行器艇载计算机上，读取传感器反馈的数据，控制升降舵执行机构，构成双层抗扰深度跟踪控制系统，实现航行器鲁棒深度跟踪航行。
[0076] 本发明设计了一种针对欠驱动自主水下航行器的双层抗扰深度跟踪控制方法，该方法将航行器的深度跟踪控制分为运动学层自适应视线角制导律和动力学层自抗扰‑滑模纵倾跟踪控制律，两者分别抵抗来自运动学层和动力学层的干扰，以提高干扰下的深度跟踪精度。
[0077] 可以理解的是，从本发明实际技术手段出发，显而易见地，本发明提供的水下航行器双层抗扰深度跟踪控制方法不仅适用于欠驱动自主水下航行器，还可拓展到其他类型的水下航行器控制中，本发明提供的技术手段的目的旨在提高水下航行器的深度跟踪精度，具体水下航行器的类型并不能从实质上对本发明技术方案的保护范围起任何限定作用。
[0078] 图1为本发明实施例提供的欠驱动水下航行器的双层抗扰深度跟踪控制方法流程图，如图1所示，包括如下步骤：
[0079] S101，确定水下航行器的航行信息；所述航行信息包括：期望深度、当前深度、当前航行速度及当前纵倾角；
[0080] S102，根据期望深度和当前深度确定深度跟踪误差，基于自适应视线角制导律将深度跟踪误差转化为期望纵倾角；所述自适应视线角制导律包括自适应参数，以对水下航行器的真实攻角进行估计，减少忽略攻角引入的运动干扰；所述自适应参数根据当前航行速度和深度跟踪误差确定；
[0081] S103，确定自抗扰‑滑模纵倾跟踪控制律，其包括：扩展状态观测器和滑模反馈控制律；所述扩展状态观测器用于观测水下航行器的复合干扰；所述复合干扰为由航行器内部结构和外部环境引入的动力干扰总和；所述滑模反馈控制律的设计过程考虑所述扩展状态观测器的观测误差，用于结合所述期望纵倾角计算水下航行器升降舵的舵角，以克服所述观测误差；
[0082] S104，结合所述自抗扰‑滑模纵倾跟踪控制律确定复合干扰的观测量和滑模反馈控制律计算的舵角，并基于所述复合干扰观测量和滑模反馈控制律计算的舵角计算升降舵的最终舵角，使得升降舵按照所述最终舵角航行，以使水下航行器按照所述期望深度稳定航行；按照所述自适应视线角制导律和自抗扰‑滑模纵倾跟踪控制律计算升降舵最终舵角使得水下航行器抵抗运动干扰和动力干扰的能力提升，提高了水下航行器深度跟踪的精度。
[0083] 如图2所示为本发明提供的双层抗扰深度跟踪控制方法框图，如图2所示：
[0084] 在运动学层，自适应视线角制导律根据式(1)将深度跟踪误差ez转化为期望纵倾θd输出到纵倾跟踪控制律中。
[0085]
[0086] 式中，深度跟踪误差定义为期望深度zd与实际深度z之差ez＝zd‑z。Δ为前视距离，为人工可调参数，一般取3‑5倍的航行器长度。制导律引入自适应参数 对航行器真实攻角进行估计。由于真实精确攻角无法轻易获得，故现有航行器控制方法一般会忽略攻角，但忽略攻角会引起运动学层干扰，正是由于 的引入使得制导律拥有抵抗运动学层干扰的能力。按照式(2)公式自适应变化。
[0087]
[0088] 式中，U为航行器的航行速度，γ为人工可调的自适应增益，自适应参数 对应估计出的攻角。
[0089] 在动力学层，使用自抗扰‑滑模控制方法构建式(3)的纵倾跟踪控制律计算升降舵舵角δ。自抗扰‑滑模纵倾跟踪控制律由扩展状态观测器，滑模反馈控制律和跟踪微分器组成。使用扩展状态观测器将包括内部未知模型和外部环境干扰在内的复合干扰观测出来，然后根据式(3)带入复合干扰的观测值z3将复杂的纵倾运动模型补偿为积分串联型，实现纵倾强抗扰跟踪。
[0090]
[0091] 式中，δ0为滑模反馈控制律计算的舵角，可进一步抵抗自抗扰的观测误差。b0为舵效系数，与航行器运动模型有关。
[0092] 水下航行器的纵倾运动模型可以表示为式(4)
[0093]
[0094] 式中，表示纵倾角加速率， 表示由航行器内部属性引起的力矩对应的加速率，是一个随航行器纵倾θ、纵倾角速度 航速U、惯性矩Iyy等复杂未知因素变化的时变量，dθ为外部未知环境干扰。
[0095] 使用扩张状态观测器(5)，可以将式(4)中复杂且未知的内外总未知量考虑为复合干扰并迭代计算出来，用z3表示。
[0096]
[0097] 式中，z1，z2和z3分别是对航行器纵倾角θ，纵倾角速度 和内外总未知复合干扰的估计，[t]表示第t步迭代计算，h为积分步长，l1，l2和l3为观测器的观测增益。
[0098] 将观测出的复合干扰的估计值z3带入的纵倾控制律(3)中，并将(3)带入到复杂的纵倾运动模型(4)中，原复杂的被控对象将转为简单的积分串联型 因此，无论内部动力学和外部环境干扰如何变化，通过使用扩张状态观测器(5)和纵倾控制律(3)，原被控对象都会转化为统一的积分串联型，因此纵倾跟踪控制器具备抵抗动力学层干扰的能力。
[0099] 由于扩展 状态观测器无法保 证观测值z3能 完美收敛至真实 值在反馈控制律设计的过程中要考虑到观测误差的存在。反馈控制律采
用滑模控制的设计思想来进一步克服观测误差。
[0100] 首先定义滑模面s＝ce1+e2，其中，c为权重系数，权衡了e1和e2的贡献量，e1为纵倾角跟踪误差，e2为纵倾角速度跟踪误差。根据指数趋近律设计方法 得出滑模反馈控制律：
[0101]
[0102] 式中，k参数控制着误差以指数收敛到0的速率，η为鲁棒项系数，用以克服观测误差。
[0103] 使用跟踪微分器(7)求取期望纵倾角的数值微分v2，并得到期望纵倾角的跟踪信号v1。最后构成式(6)中误差的计算公式e1＝v1‑z1，e2＝v2‑z2。
[0104]
[0105] 式中，r为跟踪加速度系数，h为计算步长，[t]表示第t步迭代计算。fhan(x1,x2,r,h)为最速控制综合函数，x1和x2为中间变量，分别表示为x1＝v1[t]‑θd，x2＝v2[t]；
[0106]
[0107] 为了实现上述方法，本发明设计了一种针对欠驱动水下航行器的双层抗扰深度跟踪控制系统。将公式(1)‑(3)和(5)‑(8)转化程序部署于航行器艇载计算机上，计算机调用该程序，自动读取深度传感器的深度数据z和纵倾传感器反馈的纵倾角数据θ，运行深度跟踪控制算法程序，将计算后的升降舵指令δ用以控制执行舵机，构成航行器的双层抗扰深度跟踪控制系统，实现水下自主航行器干扰下的高精度鲁棒深度跟踪航行。
[0108] 为验证本发明方法的深度跟踪效果和抗扰能力，使用如图3所示的一种典型的欠驱动自主水下航行器作为试验平台。该水下自主航行器在深度面运动只有推进器提供推力，升降舵提供纵倾控制力矩，控制力数量小于自由度数量，具有欠驱动特性。该平台具有深度和纵倾角反馈的能力，艉部有升降舵可以用来控制纵倾角，适合构成双层抗扰深度跟踪控制系统用以验证双层抗扰深度跟踪控制方法。在该自主水下航行器的艇载计算机中编写闭环控制代码，实现自主采集传感器数据和控制升降舵功能，代码具体实现了式(1)‑(3)和(5)‑(8)双层抗扰深度跟踪控制方法。
[0109] 期望深度首先渐变至2米，然后维持2米持续60秒，航行器运行本方法跟踪期望深度实现下潜动作，然后进行定深航行，任务结束后，航行器跟踪期望深度自主上浮。实艇试验结果见图4a‑图4e。
[0110] 图4a显示了航行器的深度曲线，航行器实际深度最终以无偏差收敛至期望深度2米。高精度的深度跟踪性能是由纵倾角的优异跟踪和制导律估计并补偿攻角保证的。图4b中显示了航行器的纵倾跟踪性能，可以看出航行器纵倾角可以完美地跟踪期望纵倾角。该性能是由扩张状态观测器对复合干扰的观测和补偿，滑模反馈控制律的观测误差补偿同时保证的。50秒后，航行器进行稳定航行阶段，纵倾角稳定在‑1.4°。
[0111] 图4b和图4c中分别显示出观测纵倾角z1收敛至实际纵倾角θ，观测角速度z2收敛至实际角速度 因此可以推测，图4d显示的观测复合干扰z3也收敛至真实复合干扰正是由于扩张状态观测器能有效地对复合干扰进行估计，将复杂的纵倾运动模型补偿为积分串联型，使得实际纵倾角能无偏跟踪期望纵倾角。由于水下自主航行器一般配置为微正浮力，航行器在深度稳定后，需要一定的攻角来平衡剩余浮力。如果使用视线角制导而忽略了攻角的影响，会在运动学层引入干扰，导致深度存在稳态误差。自适应视线角制导可实时估计并补偿攻角，实时估计出的攻角见图4e，结合图4a所示，50秒后，航行器进入深度稳定航行阶段，在深度稳定航行阶段，航行器的深度保持不变，此时潜浮角为0°。65秒后的估计攻角为‑1.4°，近似等于图4b所示的纵倾角，符合航行器的运动特性，从而验证了自适应视线角制导可有效地估计攻角。
[0112] 图5为本发明实施例提供的欠驱动水下航行器的双层抗扰深度跟踪控制系统架构图，如图5所示，包括：
[0113] 航行信息确定单元510，用于确定水下航行器的航行信息；所述航行信息包括：期望深度、当前深度、当前航行速度及当前纵倾角；
[0114] 期望纵倾角确定单元520，用于根据期望深度和当前深度确定深度跟踪误差，基于自适应视线角制导律将深度跟踪误差转化为期望纵倾角；所述自适应视线角制导律包括自适应参数，以对水下航行器的真实攻角进行估计，减少忽略攻角引入的运动干扰；所述自适应参数根据当前航行速度和深度跟踪误差确定；
[0115] 纵倾跟踪控制律确定单元530，用于确定自抗扰‑滑模纵倾跟踪控制律，其包括：扩展状态观测器和滑模反馈控制律；所述扩展状态观测器用于观测水下航行器的复合干扰；所述复合干扰为由航行器内部结构和外部环境引入的动力干扰总和；所述滑模反馈控制律的设计过程考虑所述扩展状态观测器的观测误差，用于结合所述期望纵倾角计算水下航行器升降舵的舵角，以克服所述观测误差；
[0116] 升降舵舵角确定单元540，用于结合所述自抗扰‑滑模纵倾跟踪控制律确定复合干扰的观测量和滑模反馈控制律计算的舵角，并基于所述复合干扰观测量和滑模反馈控制律计算的舵角计算升降舵的最终舵角，使得升降舵按照所述最终舵角航行，以使水下航行器按照所述期望深度稳定航行；按照所述自适应视线角制导律和自抗扰‑滑模纵倾跟踪控制律计算升降舵最终舵角使得水下航行器抵抗运动干扰和动力干扰的能力提升，提高了水下航行器深度跟踪的精度。
[0117] 可以理解的是，图5中各个单元的详细功能实现可参见前述方法实施例中的介绍，在此不做赘述。
[0118] 本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。
