CN115586782B - 一种自主式水下机器人运动控制方法及装置 - Google Patents

Abstract

本发明公开了一种自主式水下机器人运动控制方法及装置，该方法包括：构建自主式水下机器人运动控制中的动力学与运动学模型、状态变量、控制变量、效用函数和性能指标函数；设置双启发式动态规划的水下机器人的模型网络；构建双启发式动态规划的动作网络、评价网络和扰动网络并初始化网络；进行基于双启发式动态规划的策略提升过程，包括训练动作网络；进行基于启发式动态规划的策略评估过程，包括训练评价网络；训练启发式动态规划的扰动网络用于拟合外界扰动，包括训练扰动网络；重复动作网络、评价网络和扰动网络的训练，直至满足预设条件，最终得到的动作网络的输出公式用于控制自主式水下机器人的运动。

Description

一种自主式水下机器人运动控制方法及装置

技术领域

本发明涉及自主式水下机器人技术领域，特别是关于一种自主式水下机器人运动控制方法及装置。

背景技术

自主式水下机器人作为水域作业的重要工具，应用十分广泛，其在深海探测、安全搜救、水下电缆铺设等民用与军用场景中发挥着重要作用。由于水下环境干扰复杂多变，精准的水下机器人动力学模型难以确定，同时水下机器人系统又具有强耦合性和强非线性，传统的运动控制技术基于不精准的动力学模型很难达到期望的控制要求。此外，自主式水下机器人的执行器，例如螺旋桨、舵等，产生的推进力或舵角的范围是有限的，因此执行器输入饱和问题也是设计控制器系统时需要重点考虑的。同时，水下机器人在工作环境中易受到风、浪、流、涌等复杂环境干扰，这将对系统的控制性能造成不利影响，进而导致水下机器人在作业时难以保持稳定状态。

因此，亟待提供一种新的自主式水下机器人运动控制方法及装置。

发明内容

本发明的目的在于提供一种自主式水下机器人运动控制方法及装置来克服或至少减轻现有技术的上述缺陷中的至少一个。

为实现上述目的，本发明提供一种自主式水下机器人运动控制方法，包括：

步骤1，构建自主式水下机器人运动控制中的动力学与运动学模型、状态变量、控制变量、效用函数和性能指标函数；

其中，定义遵循右手定则的地面坐标系E-ξηζ和自主式水下机器人的运动坐标系O-xyz，地面坐标系原点E为水面上的点，Eζ轴的正向指向地心，取自主式水下机器人的主航向为Eξ轴的正向；运动坐标系的原点O置于自主式水下机器人上，Ox轴的正向为自主式水下机器人主对称轴指向船艏的方向，Oy轴平行于基线面并垂直于Ox轴，指向右舷方向为正；

其中，动力学与运动学模型包括三种类型：纵向速度运动模型、航向运动模型与深度控制运动模型；

对于纵向速度运动模型，状态变量表示为x＝[u]，其中u为自主式水下机器人在运动坐标系下的纵向x轴上的速度，控制变量表示为a＝[τ₁]，τ₁为螺旋桨沿x轴方向的推力；

对于航向运动模型，状态变量表示为x＝[v,r,ψ]^T，其中v为自主式水下机器人在运动坐标系下的侧向y轴上的速度，r为自主式水下机器人在运动坐标系下绕z轴的航向角速度，ψ为自主式水下机器人在地面坐标系下的航向角，控制变量表示为a＝[δ_r,τ₂]^T，δ_r为垂直舵角，τ₂为螺旋桨绕z轴方向的扭矩；

对于深度控制模型，状态变量表示为x＝[w,q,θ,z]^T，其中w为自主式水下机器人在运动坐标系下的垂向z轴上的速度，q为自主式水下机器人在运动坐标系下绕y轴的纵倾角速度，θ为自主式水下机器人在地面坐标系下的纵倾角，z为自主式水下机器人在地面坐标系下的垂向深度，控制变量a＝[δ_s,τ₁,τ₃]^T，其中δ_s为水平舵角，τ₁为螺旋桨沿x轴方向的推力，τ₃为螺旋桨绕y轴方向的扭矩；

效用函数U(x(k),a(k),ω(k))满足：

其中，x(k)为k时刻的状态变量；a(k)＝[a₁(k),a₂(k),…,a_m(k)]^T为k时刻的控制变量，m为控制器的数量；ω(k)为k时刻的外界干扰；e_x(k)为k时刻状态变量x(k)与目标状态x_d(k)的差值，Q、R、P为预设权重矩阵，为有界的单调递增奇函数，/>表示控制器控制变量的最大值，s表示积分中的被积变量；γ为预设正实数常量；

性能指标函数J(x(j))满足：

J(x(j))＝U(x(k),a(k),ω(k))+j(x(k+1))，

其中，定义协状态函数为性能指标函数J(x(k))关于状态变量x(k)的梯度；

步骤2，设置双启发式动态规划的水下机器人的模型网络；包括设置状态变量满足下式：

其中，x(k+1)为k+1时刻的状态变量，ω_m为模型网络的权值，下标m表示模型网络，φ_m(·)为预设激活函数，h(x(k))为预设扰动矩阵；

步骤3，构建双启发式动态规划的动作网络、评价网络和扰动网络并初始化网络；

其中，动作网络、评价网络和扰动网络分别包括1层输入层、1层隐藏层和1层输出层，动作网络的输入为k时刻下的状态变量x(k)，输出为与状态变量x(k)对应的控制变量a(k)；评价网络的输入为k时刻下的状态变量x(k)，输出为当前状态对应的协状态函数λ(x(k))的近似值；扰动网络的输入为k时刻下的状态变量x(k)，输出为与状态变量x(k)对应的外界干扰ω(k)；

其中，设置动作网络的输出计算公式为评价网络的输出计算公式为/>扰动网络的输出计算公式为/> 其中ω_a、ω_c、ω_ω分别为动作网络、评价网络与扰动网络各神经层之间的权值，φ_a(·)、φ_c(·)、φ_ω(·)为动作网络、评价网络与扰动网络的预设激活函数；

其中，对动作网络、评价网络和扰动网络进行初始化，令ω_a和ω_ω为[-0.1，0.1]之间的任意值，ω_c＝0；

步骤4，进行基于双启发式动态规划的策略提升过程，包括训练动作网络；

其中，随机选择一组数量为p的预设状态集{x⁽¹)(k),x⁽²)(k),…,x^(p)(k)}，将状态集作为动作网络的输入得到对应的动作集{a₀(x⁽¹⁾(k)),a₀(x⁽²⁾(k)),…,a₀(x^(p)(k))}，再将状态集与动作集作为模型网络的输入，得到对应k+1时刻的状态集{x⁽¹⁾(k+1),x⁽²)(k+1),…,x^(p)(k+1)}，利用k+1时刻的状态集作为评价网络的输入得到对应的λ(x(k+1))；

在动作网络的训练过程中，令x(k)对应的动作网络的训练输出为即公式/>计算得到的值作为/>并利用下式计算训练动作网络的目标值a_i(x(k))：

采用梯度下降法进行训练，设置权值更新公式为 其中，i表示策略提升的次数；ω_ai表示第i次策略提升时动作网络的权重；j^a为训练动作网络时的迭代步数，/>表示在第i次策略提升时，经历第j^a次训练后动作网络的权重；α_a为动作网络的预设学习率，损失函数

当j^a达到预设值或损失函数E_ai(k)小于阈值时训练终止，以得到的ω_ai(j^a+1)作为动作网络的输出计算公式中的ω_a的值；

否则，以得到的ω_ai(j^a+1)代入动作网络的输出计算公式，以预设状态集作为输入计算得到新的动作集，进行下一次计算；

步骤5，进行基于启发式动态规划的策略评估过程，包括训练评价网络；

其中，在评价网络的训练过程中，令x(k)对应的评价网络的训练输出为即公式/>计算得到的值作为/>

对J(x(k))＝U(x(k),a(k),ω(k))+J(x(k+1))等式两边分别求关于x(k)的梯度，得到下式计算训练评价网络的目标值λ_i+1(x(k))：

采用梯度下降法进行训练，权值更新公式为 其中j^c为训练评价网络的迭代步数，α_c为评价网络的预设学习率，损失函数当j^c达到预设值或损失函数E_ci(k)小于阈值时训练终止，以得到的ω_ci(j^c+1)作为评价网络的输出计算公式中的ω_c的值；否则，以ω_ci(j^c+1)代入评价网络的输出计算公式中计算得到新的λ_i(x(k))，进行下一次迭代；

步骤6，训练启发式动态规划的扰动网络用于拟合外界扰动，包括训练扰动网络；

其中，在训练扰动网络的过程中，令x(k)对应的扰动网络的输出为即公式/>计算得到的值作为/>利用下式计算训练扰动网络的目标值ω_i(x(k))：

采用梯度下降法进行训练，权值更新公式为 其中j^ω为训练扰动网络的迭代步数，α_ω为扰动网络的预设学习率，损失函数当j^ω达到预设值或损失函数E_ωi(k)小于阈值时训练终止，以得到的ω_ωi(j^ω+1)作为扰动网络的输出计算公式中的ω_ω的值；否则，将ω_ωi(j^ω+1)代入扰动网络的输出公式中计算得到新的ω_i(x(k))，进行下一次迭代；

步骤7，重复步骤4-6直至||λ_i+1(x(k))-λ_i(x(k))||²<ε或i>i_max，其中，ε为迭代容许误差，i_max为i的预设最大值，最终得到的动作网络的输出公式用于控制自主式水下机器人的运动。

其中，步骤2中包括：

构建模型网络；设置模型网络的输入为k时刻的状态变量x(k)和控制变量a(k)，输出为k+1时刻的状态变量x(k+1)；

训练模型网络；包括：训练过程中将模型网络的输出表示为 权值更新公式为/>其中j^m为训练模型网络的迭代步数，损失函数/> 当j^m或E_m(k)满足预设条件时训练终止，以得到的ω_m(j^m+1)作为模型网络中ω_m的值。

其中，j^a、j^c和j^ω的预设值均为2000，E_ai(k)、E_ci(k)和E_ωi(k)的阈值均为10^-6。

本发明实施例还提供一种自主式水下机器人运动控制装置，包括处理器和驱动设备，其中：

处理器用于：执行根据权利要求1所述的自主式水下机器人运动控制方法；

驱动设备用于：根据处理器得到的动作网络公式控制自主式水下机器人的运动。

其中，处理器用于：

构建模型网络；设置模型网络的输入为k时刻的状态变量x(k)和控制变量a(k)，输出为k+1时刻的状态变量x(k+1)；

训练模型网络；包括：训练过程中将模型网络的输出表示为 权值更新公式为/>其中j^m为训练模型网络的迭代步数，损失函数/> 当j^m或E_m(k)满足预设条件时训练终止，以得到的ω_m(j^m+1)作为模型网络中ω_m的值。

其中，j^a、j^c和j^ω的预设值均为2000，E_ai(k)、E_ci(k)和E_ωi(k)的阈值均为10^-6。

本发明由于采取以上技术方案，其具有以下优点：

本发明基于采集数据利用模型网络拟合的方式得到与实际环境更相符的水下机器人动力学与运动学模型，使得基于模型的控制方法可充分发挥其控制效果稳定、控制精度高的特性，从而提升了水下机器人运动控制精度。另外，本发明考虑水下机器人运动控制执行器输入饱和的物理特性，能有效防止过大的冲击破坏执行器的物理结构，从而避免出现故障，有利于保持水系机器人运行的稳定性。本发明还考虑了水下环境复杂多变、外界扰动不可忽略的环境特征，进行抗扰动控制器设计，使得水下机器人在面对洋流、浪等外界扰动时依旧能保持较高的控制精度。本发明使用评价网络拟合协状态量，不仅可以加快策略提升时的计算速度还可以很好的处理扰动。

附图说明

图1为本发明实施例提供的自主式水下机器人运动控制方法的流程示意图。

图2为本发明实施例提供的地面坐标系和运动坐标系的示意图。

图3为本发明实施例提供的模型网络、执行网络、评价网络与扰动网络的结构示意图。

图4为本发明实施例提供的启发式动态规划算法结构示意图。

图5为本发明实施例提供的自主式水下机器人运动控制装置的结构示意图。

具体实施方式

在附图中，使用相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面结合附图对本发明的实施例进行详细说明。

在本发明的描述中，术语“中心”、“纵向”、“横向”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明保护范围的限制。

在不冲突的情况下，本发明各实施例及各实施方式中的技术特征可以相互组合，并不局限于该技术特征所在的实施例或实施方式中。

下面结合附图以及具体实施例对本发明做进一步的说明，需要指出的是，下面仅以一种最优化的技术方案对本发明的技术方案以及设计原理进行详细阐述，但本发明的保护范围并不仅限于此。

本文涉及下列术语，为便于理解，对其含义说明如下。本领域技术人员应当理解，下列术语也可能有其它名称，但在不脱离其含义的情形下，其它任何名称都应当被认为与本文所列术语一致。

本发明实施例提供一种自主式水下机器人运动控制方法，如图1所示，包括：

步骤1，构建自主式水下机器人运动控制中的状态变量、控制变量、动力学与运动学模型、效用函数和性能指标函数。

首先定义两个参考坐标系：地面坐标系和运动坐标系。参考坐标系示意图如图2所示，两坐标系都满足右手法则，箭头方向为正向，其中E-ξηζ表示固定于地面的地面坐标系(例如原点E为水面上的点)，O-xyz表示固定于自主式水下机器人的运动坐标系(例如原点O置于自主式水下机器人的重心或其他位置)，地面坐标系与运动坐标系之间的转换关系可由自主式水下机器人的运动学方程反映。在下文中，地面坐标系原点E为水面上某一点，Eζ轴的正向指向地心，取自主式水下机器人的主航向为Eξ轴的正向，拇指指向Eξ轴的正向，中指向外延伸的方向则为Eζ轴的正向，则食指伸直的方向对应Eη轴的正向；运动坐标系原点O置于自主式水下机器人的重心，Ox轴为自主式水下机器人主对称轴，指向船艏为正向，拇指指向Ox轴的正向，食指伸直的方向对应于Oy轴的正向，则中指向外延伸的方向则为Oz轴的正向。Oy轴平行于基线面并垂直于Ox轴，指向右舷为正。

然后将自主式水下机器人系统的动力学模型与运动学模型分解为以下三种类型：纵向速度运动模型、航向运动模型与深度控制运动模型。在该三种模型中均用x表示状态变量，a表示控制变量。

状态变量的选取用于描述自主式水下机器人在运动控制问题中的状态信息。对于纵向速度运动模型，状态变量表示为x＝[u]，其中u为自主式水下机器人在运动坐标系下的纵向速度，控制变量表示为a＝[τ₁]，τ₁为螺旋桨沿x轴方向的推力；对于航向运动模型，状态变量表示为x＝[v,r,ψ]^T，其中v为自主式水下机器人在运动坐标系下的侧向速度，r为自主式水下机器人在运动坐标系下绕z轴的航向角速度，ψ为自主式水下机器人在地面坐标系下的航向角，控制变量表示为a＝[δ_r,τ₂]^T，δ_r为垂直舵角，τ₂为螺旋桨绕z轴方向的扭矩；对于深度控制运动模型，状态变量表示为x＝[w,q,θ,z]^T，其中w为自主式水下机器人在运动坐标系下的垂向速度，q为自主式水下机器人在运动坐标系下绕y轴的纵倾角速度，θ为自主式水下机器人在地面坐标系下纵倾角，z为自主式水下机器人在地面坐标系下的垂向深度，控制变量a＝[δ_s,τ₁,τ₃]^T，其中δ_s为水平舵角，τ₁为螺旋桨沿x轴方向的推力，τ₃为螺旋桨绕y轴方向的扭矩。

应注意的是，由于自主式水下机器人系统的运动控制执行器多种多样，控制变量应根据自主式水下机器人的具体形式确定。

定义效用函数 其中，x(k)为k时刻的状态变量，例如，在纵向速度运动控制中，x(k)＝[u(k)]，其中，u为自主式水下机器人在运动坐标系下的纵向速度；a(k)为k时刻的控制变量；ω(k)为k时刻受外界风浪、洋流等影响对状态变量产生的干扰；Q、R、P为预设权重矩阵，γ为预设正实数常量，e_x(k)为k时刻状态x(k)与目标状态x_d(k)的差值，a(k)＝[a₁(k),a₂(k),…,a_m(k)]^T为k时刻的控制变量，m为控制器的数量，φ(·)为有界的单调递增奇函数，用于将控制变量约束在预设的许可范围内，在本文中φ(·)＝tanh(·)，即双曲正切函数。/>表示控制器控制变量的最大值，由于执行器具有输入饱和特性，每个控制器的控制量具有最大值， 是第i个控制器控制量的最大值，/>s表示积分中的被积变量，在本文为控制变量。

根据贝尔曼最优性原理，定义性能指标函数J(x(k))满足：J(x(k))＝U(x(k),a(k),ω(k))+J(x(k+1))，定义协状态函数为性能指标函数J(x(k))关于当前状态x(k)的梯度。

步骤2，设置双启发式动态规划的水下机器人的模型网络；包括设置状态变量满足下式：

其中，x(k+1)为k+1时刻的状态变量，ω_m为模型网络的权值，下标m表示模型网络，φ_m(·)为激活函数，常见的激活函数有：sigmoid激活函数、tanh激活函数、relu激活函数、ELU激活函数等。本专利使用Tanh激活函数，表达式为tanh(z)＝(e^z-e^-z)/e^z+e^-z。引入激活函数是为了增加神经网络模型的非线性，h(x(k))为预设扰动矩阵，例如在航向运动控制中h(x(k))＝[1,1,1]。

本步骤中，构建双启发式动态规划的模型网络，用于拟合上文提到的三种运动模型。在无水流波动或波动较小的静止水域收集足量自主式水下机器人运动数据，包含自主式水下机器人在相应模型下的k时刻状态变量x(k)及其对应的控制变量a(k)。

模型网络的输入为k时刻的状态变量x(k)、控制变量a(k)，输出为k+1时刻的状态变量x(k+1)。训练模型网络时，模型网络的输出可以表示为 其中φ_m为模型网络各神经层之间的权值，下标m表示model network，即模型网络，φ_m(·)为激活函数。模型网络的训练采用梯度下降法，权值更新公式为/>其中j^m为训练模型网络的迭代步数，下标m表示model network，即模型网络，损失函数为在本发明中当j^m达到预设值例如2000或损失函数E_m(k)<阈值例如10^-6时训练终止。

最终，水下机器人系统模型可描述为 h(x(k))为扰动矩阵。

步骤3，构建双启发式动态规划的动作网络、评价网络和扰动网络并初始化网络。

其中，其中，动作网络、评价网络和扰动网络分别包括1层输入层、1层隐藏层和1层输出层，输入层的节点数量为p个，隐藏层的节点数量为q个，输出层的节点数量为n个。这三种类型的网络输入层节点和输出层节点的数目与网络的输入与输出维度是相对应的，例如动作网络在航向运动控制中输入层节点的数量对应状态变量x(k)的维数为3，输出层节点对应控制变量a(k)的维数为2。本文中，p、q和n均为根据实际情况灵活调整的预设经验值。

动作网络的输入为k时刻下的状态变量x(k)，输出为与状态变量x(k)对应的控制变量a(k)；

评价网络的输入为k时刻下的状态变量x(k)，输出为当前状态对应的协状态函数λ(x(k))的近似值；

扰动网络的输入为k时刻下的状态变量x(k)，输出为与状态变量x(k)对应的外界扰动ω(k)。

其中，动作网络的输出可以表示为评价网络的输出可以表示为/>扰动网络的输出可以表示为/> 其中ω_a、ω_c、ω_ω为动作网络、评价网络与扰动网络各神经层之间的权值，φ_a(·)、φ_c(·)、φ_ω(·)为动作网络、评价网络与扰动网络的预设激活函数。

将三个神经网络的权值进行初始化，执行网络与扰动网络的权值设置为[-0.1,0.1]之间的任意值，评价网络的权值全部设置为0，使λ₀(·)＝0。

模型网络、执行网络、评价网络与扰动网络的结构图如图3所示，其包括1层输入层、1层隐藏层和1层输出层，输入层的节点数量为p个，各隐藏层的节点数量为q个，输出层的节点数量为n个。启发式动态规划算法结构图如图4所示，其中，自主式水下机器人系统(以下简称系统)在k时刻的状态变量x(k)分别经过扰动网络与动作网络，分别得到当前状态系统所能处理的最差的扰动变量ω(k)与控制变量a(k)，x(k)与这两个变量一起作为模型网络的输入得到下一时刻系统的状态变量x(k+1)，x(k+1)输入评价网络1得到对应的协状态变量λ_i(k+1)，下标i表示启发式动态规划算法当前的迭代次数，λ_i(k+1)可用于策略提升和扰动网络的训练，还可以在策略评估中经过运算得到λ_i+1(k)，作为评价网络2训练的目标值。

步骤4，进行基于双启发式动态规划的策略提升过程。

设置当前迭代步数i＝1。随机选择一组数量为p的状态集{x⁽¹)(k),x⁽²)(k),…,x^(p)(k)}，将状态集作为动作网络的输入，得到对应的动作集{a₀(x⁽¹⁾(k)),a₀(x⁽²⁾(k)),…,a₀(x^(p)(k))}，再将状态集与动作集作为已经训练好的模型网络的输入，得到对应k+1时刻的状态集{x⁽¹⁾(k+1),x⁽²⁾(k+1),…,x^(p)(k+1)}，利用k+1时刻的状态集作为评价网络的输入得到对应的λ(k+1)，即λ(x(k+1))，本文为便于表达，将λ(x(k+1))简写为λ(k+1)，类似的，a(k)与a(x(k))含义相同。

训练动作网络时，令x(k)对应的动作网络的输出为以公式计算得到的值作为/>并利用下式计算目标值a_i(x(k))：

动作网络的训练采用梯度下降法，权值更新公式为 其中，i表示策略提升的次数；ω_ai表示第i次策略提升时动作网络的权重；j^a为训练动作网络时的迭代步数，/>表示在第i次策略提升时，经历第j^a次训练后动作网络的权重；α_a为动作网络的预设学习率，损失函数/>在本发明中当j^a达到预设值例如2000或损失函数E_ai(k)<阈值例如10^-6时训练终止，以得到的ω_ai(j^a+1)作为动作网络的输出计算公式/>中的ω_a的值；否则，以得到的ω_ai(j^a+1)代入动作网络的输出计算公式，以预设状态集作为输入计算得到新的动作集，进行下一次计算。

步骤5，进行基于启发式动态规划的策略评估过程。

训练评价网络时，x(k)对应的评价网络的输出为即以公式计算得到的值作为/>

对J(x(k))＝U(x(k),a(k),ω(k))+J(x(k+1))等式两边分别求关于x(k)的梯度，得到下式计算训练评价网络的目标值λ_i+1(x(k))：

评价网络的训练采用梯度下降法，权值更新公式为 其中j^c为训练评价网络的迭代步数，α_c为评价网络的学习率，损失函数在本发明中当j^c达到预设值例如2000或损失函数E_ci(k)<阈值例如10^-6时训练终止，以得到的ω_ci(j^c+1)作为评价网络的输出计算公式/>中的ω_c的值；否则，以ω_ci(j^c+1)代入评价网络的公式中计算得到新的λ_i(x(k))，进行下一次迭代。

步骤6，训练启发式动态规划的扰动网络用于拟合外界扰动ω(k)。

该外界扰动为控制器所能处理的最恶劣的外界扰动，即当外界真实扰动不超过扰动网络的输出ω(k)时，控制器能保证水下机器人系统的稳定性和控制精度。

其中，在训练扰动网络过程中，令x(k)对应的扰动网络的输出为即以公式/>计算得到的值作为/>利用下式计算训练扰动网络的目标值ω_i(x(k))：

扰动网络的训练采用梯度下降法，权值更新公式为 其中j^ω为训练扰动网络的迭代步数，α_ω为扰动网络的学习率，损失函数/>在本发明中当j^ω达到预设值例如2000或损失函数F_ωi(k)<阈值例如10^-6时训练终止，以得到的ω_ωi(j^ω+1)作为扰动网络的输出计算公式/>中的ω_ω的值；否则，将ω_ωi(j^ω+1)代入扰动网络的输出计算公式中计算得到新的ω_i(x(k))，进行下一次迭代。

步骤7，重复步骤4-6直至||λ_i+1(x(k))-λ_i(x(k))||²<ε或i>i_max，其中，ε为迭代容许误差，i_max为i的预设最大值，根据此时得到的动作网络公式控制自主式水下机器人的运动。例如，最大迭代次数i_max＝100，迭代容许误差ε＝10^-10。

本发明实施例提供一种自主式水下机器人运动控制装置，如图5所示，该装置包括处理器51和驱动设备52，其中，处理器51用于：执行上述实施例中的自主式水下机器人运动控制方法，驱动设备52用于：根据处理器得到的动作网络公式控制自主式水下机器人的运动。

该处理器执行的操作与上述方法实施例中的各步骤对应，本文不再一一赘述。

例如，处理器51可以用于：

构建模型网络；设置模型网络的输入为k时刻的状态变量x(k)和控制变量a(k)，输出为k+1时刻的状态变量x(k+1)；

训练模型网络；包括：在训练模型网络的过程中，将模型网络的输出表示为权值更新公式为/> 其中j^m为训练模型网络的迭代步数，损失函数/> 当j^m或E_m(k)满足预设条件时训练终止，以得到的ω_m(j^m+1)作为模型网络中ω_m的值。

其中，j^a、j^c和j^ω的预设值均为2000，E_ai(k)、E_ci(k)和E_ωi(k)的阈值均为10^-6。

本发明实施例提供一种基于双启发式动态规划的自主式水下机器人运动控制系统，其可以包括深度传感器、惯性导航传感器、计算单元、执行单元、数据采集单元和推进器驱动单元等，其中深度计通过计算水压从而得到自主式水下机器人距离水面的高度，惯性导航传感器采集自主式水下机器人的姿态信息等数据并进行均值滤波得到更加准确的姿态数据，计算单元可用以运行上述控制方法，推进器驱动单元可将控制输入转换为PWM(脉冲宽度调制)波进而控制电机转速。

本发明基于采集数据利用模型网络拟合的方式得到与实际环境更相符的水下机器人动力学与运动学模型，使得基于模型的控制方法可充分发挥其控制效果稳定、控制精度高的特性，从而提升了水下机器人运动控制精度。另外，本发明考虑水下机器人运动控制执行器输入饱和的物理特性，能有效防止过大的冲击破坏执行器的物理结构，从而避免出现故障，有利于保持水系机器人运行的稳定性。本发明还考虑了水下环境复杂多变、外界扰动不可忽略的环境特征，进行抗扰动控制器设计，使得水下机器人在面对洋流、浪等外界扰动时依旧能保持较高的控制精度。本发明使用评价网络拟合协状态量，不仅可以加快策略提升时的计算速度还可以很好的处理扰动。

最后需要指出的是：以上实施例仅用以说明本发明的技术方案，而非对其限制。本领域的普通技术人员应当理解：可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (6)

1.一种自主式水下机器人运动控制方法，其特征在于，包括：

步骤1，构建自主式水下机器人运动控制中的动力学与运动学模型、状态变量、控制变量、效用函数和性能指标函数；

其中，定义遵循右手定则的地面坐标系E-ξηζ和自主式水下机器人的运动坐标系O-xyz，地面坐标系原点E为水面上的点，Eζ轴的正向指向地心，取自主式水下机器人的主航向为Eξ轴的正向；运动坐标系的原点O置于自主式水下机器人上，Ox轴的正向为自主式水下机器人主对称轴指向船艏的方向，Oy轴平行于基线面并垂直于Ox轴，指向右舷方向为正；

其中，动力学与运动学模型包括三种类型：纵向速度运动模型、航向运动模型与深度控制运动模型；

对于纵向速度运动模型，状态变量表示为x＝[u]，其中u为自主式水下机器人在运动坐标系下的纵向x轴上的速度，控制变量表示为a＝[τ₁]，τ₁为螺旋桨沿x轴方向的推力；

对于航向运动模型，状态变量表示为x＝[v,r,ψ]^T，其中v为自主式水下机器人在运动坐标系下的侧向y轴上的速度，r为自主式水下机器人在运动坐标系下绕z轴的航向角速度，ψ为自主式水下机器人在地面坐标系下的航向角，控制变量表示为a＝[δ_r,τ₂]^T，δ_r为垂直舵角，τ₂为螺旋桨绕z轴方向的扭矩；

对于深度控制模型，状态变量表示为x＝[w,q,θ,z]^T，其中w为自主式水下机器人在运动坐标系下的垂向z轴上的速度，q为自主式水下机器人在运动坐标系下绕y轴的纵倾角速度，θ为自主式水下机器人在地面坐标系下的纵倾角，z为自主式水下机器人在地面坐标系下的垂向深度，控制变量a＝[δ_s,τ₁,τ₃]^T，其中δ_s为水平舵角，τ₁为螺旋桨沿x轴方向的推力，τ₃为螺旋桨绕y轴方向的扭矩；

效用函数U(x(k),a(k),ω(k))满足：

其中，x(k)为k时刻的状态变量；a(k)＝[a₁(k),a₂(k),…,a_m(k)]^T为k时刻的控制变量，m为控制器的数量；ω(k)为k时刻的外界干扰；e_x(k)为k时刻状态变量x(k)与目标状态x_d(k)的差值，Q、R、P为预设权重矩阵，为有界的单调递增奇函数，/>表示控制器控制变量的最大值，s表示积分中的被积变量；γ为预设正实数常量；

性能指标函数J(x(k))满足：

J(x(k))＝U(x(k),a(k),ω(k))+J(x(k+1))，

其中，定义协状态函数为性能指标函数J(x(k))关于状态变量x(k)的梯度；

步骤2，设置双启发式动态规划的水下机器人的模型网络；包括设置状态变量满足下式：

其中，x(k+1)为k+1时刻的状态变量，ω_m为模型网络的权值，下标m表示模型网络，φ_m(·)为预设激活函数，h(x(k))为预设扰动矩阵；

步骤3，构建双启发式动态规划的动作网络、评价网络和扰动网络并初始化网络；

其中，动作网络、评价网络和扰动网络分别包括1层输入层、1层隐藏层和1层输出层，动作网络的输入为k时刻下的状态变量x(k)，输出为与状态变量x(k)对应的控制变量a(k)；评价网络的输入为k时刻下的状态变量x(k)，输出为当前状态对应的协状态函数λ(x(k))的近似值；扰动网络的输入为k时刻下的状态变量x(k)，输出为与状态变量x(k)对应的外界干扰ω(k)；

其中，设置动作网络的输出计算公式为评价网络的输出计算公式为/>扰动网络的输出计算公式为/> 其中ω_a、ω_c、ω_ω分别为动作网络、评价网络与扰动网络各神经层之间的权值，φ_a(·)、φ_c(·)、φ_ω(·)为动作网络、评价网络与扰动网络的预设激活函数；

其中，对动作网络、评价网络和扰动网络进行初始化，令ω_a和ω_ω为[-0.1，0.1]之间的任意值，ω_c＝0；

步骤4，进行基于双启发式动态规划的策略提升过程，包括训练动作网络；

其中，随机选择一组数量为p的预设状态集{x⁽¹⁾(k),x⁽²⁾(k),…,x^(p)(k)}，将状态集作为动作网络的输入得到对应的动作集{a₀(x⁽¹⁾(k)),a₀(x⁽²⁾(k)),…,a₀(x^(p)(k))}，再将状态集与动作集作为模型网络的输入，得到对应k+1时刻的状态集{x⁽¹⁾(k+1),x⁽²⁾(k+1),…,x^(p)(k+1)}，利用k+1时刻的状态集作为评价网络的输入得到对应的λ(x(k+1))；

在动作网络的训练过程中，令x(k)对应的动作网络的训练输出为即公式计算得到的值作为/>并利用下式计算训练动作网络的目标值a_i(x(k))：

采用梯度下降法进行训练，设置权值更新公式为 其中，i表示策略提升的次数；ω_ai表示第i次策略提升时动作网络的权重；j^a为训练动作网络时的迭代步数，/>表示在第i次策略提升时，经历第j^a次训练后动作网络的权重；α_a为动作网络的预设学习率，损失函数/>当j^a达到预设值或损失函数E_ai(k)小于阈值时训练终止，以得到的ω_ai(j^a+1)作为动作网络的输出计算公式/>中的ω_a的值；

否则，以得到的ω_ai(j^a+1)代入动作网络的输出计算公式，以预设状态集作为输入计算得到新的动作集，进行下一次计算；

步骤5，进行基于启发式动态规划的策略评估过程，包括训练评价网络；

其中，在评价网络的训练过程中，令x(k)对应的评价网络的训练输出为即公式/>计算得到的值作为/>

对J(x(k))＝U(x(k),a(k),ω(k))+J(x(k+1))等式两边分别求关于x(k)的梯度，得到下式计算训练评价网络的目标值λ_i+1(x(k))：

采用梯度下降法进行训练，权值更新公式为 其中j^c为训练评价网络的迭代步数，α_c为评价网络的预设学习率，损失函数当j^c达到预设值或损失函数E_ci(k)小于阈值时训练终止，以得到的ω_ci(j^c+1)作为评价网络的输出计算公式中的ω_c的值；否则，以ω_ci(j^c+1)代入评价网络的输出计算公式中计算得到新的λ_i(x(k))，进行下一次迭代；

步骤6，训练启发式动态规划的扰动网络用于拟合外界扰动，包括训练扰动网络；

其中，在训练扰动网络的过程中，令x(k)对应的扰动网络的输出为即公式计算得到的值作为/>利用下式计算训练扰动网络的目标值ω_i(x(k))：

采用梯度下降法进行训练，权值更新公式为 其中j^ω为训练扰动网络的迭代步数，α_ω为扰动网络的预设学习率，损失函数当j^ω达到预设值或损失函数E_ωi(k)小于阈值时训练终止，以得到的ω_ωi(j^ω+1)作为扰动网络的输出计算公式/>中的ω_ω的值；否则，将ω_ωi(j^ω+1)代入扰动网络的输出公式中计算得到新的ω_i(x(k))，进行下一次迭代；

步骤7，重复步骤4-6直至||λ_i+1(x(k))-λ_i(x(k))||²<ε或i>i_max，其中，ε为迭代容许误差，i_max为i的预设最大值，最终得到的动作网络的输出公式用于控制自主式水下机器人的运动。

2.根据权利要求1所述的方法，其特征在于，步骤2中包括：

构建模型网络；设置模型网络的输入为k时刻的状态变量x(k)和控制变量a(k)，输出为k+1时刻的状态变量x(k+1)；

训练模型网络；包括：训练过程中将模型网络的输出表示为 权值更新公式为/>其中j^m为训练模型网络的迭代步数，损失函数/> 当j^m或E_m(k)满足预设条件时训练终止，以得到的ω_m(j^m+1)作为模型网络中ω_m的值。

3.根据权利要求1所述的方法，其特征在于，j^a、j^c和j^ω的预设值均为2000，E_ai(k)、E_ci(k)和E_ωi(k)的阈值均为10^-6。

4.一种自主式水下机器人运动控制装置，其特征在于，包括处理器和驱动设备，其中：

处理器用于：执行根据权利要求1所述的自主式水下机器人运动控制方法；

驱动设备用于：根据处理器得到的动作网络公式控制自主式水下机器人的运动。

5.根据权利要求4所述的装置，其特征在于，处理器用于：

构建模型网络；设置模型网络的输入为k时刻的状态变量x(k)和控制变量a(k)，输出为k+1时刻的状态变量x(k+1)；

训练模型网络；包括：训练过程中将模型网络的输出表示为 权值更新公式为/>其中j^m为训练模型网络的迭代步数，损失函数/> 当j^m或E_m(k)满足预设条件时训练终止，以得到的ω_m(j^m+1)作为模型网络中ω_m的值。

6.根据权利要求4所述的装置，其特征在于，j^a、j^c和j^ω的预设值均为2000，E_ai(k)、E_ci(k)和E_ωi(k)的阈值均为10^-6。